BB.Robot.Kinematics (bb v0.22.0)

Kinematic computations for robot manipulators.

This module provides forward kinematics and related computations for robots defined with the BB DSL.

Forward Kinematics

Forward kinematics computes the position and orientation of any link given the current joint positions:

# Get the transform from base to end-effector
transform = BB.Robot.Kinematics.forward_kinematics(
  robot,
  state,
  :end_effector
)

# Extract position
pos = BB.Math.Transform.get_translation(transform)
{BB.Math.Vec3.x(pos), BB.Math.Vec3.y(pos), BB.Math.Vec3.z(pos)}

Conventions

All positions are in meters
All angles are in radians
Transforms are 4x4 homogeneous matrices (Nx tensors)
The base link is at the identity transform

Summary

Functions

all_link_transforms(robot, state)

Compute transforms for all links in the robot.

compute_joint_transform(robot, positions, joint_name)

Compute the transform for a single joint given its current position.

forward_kinematics(robot, state, target_link)

Compute the forward kinematics transform from base to a target link.

jacobian(robot, state, target_link, joint_names)

Compute the spatial (position and orientation) Jacobian of a link.

link_position(robot, state_or_positions, target_link)

Get the position of a link in the base frame.

position_jacobian(robot, state, target_link, joint_names)

Compute the position Jacobian of a link with respect to the given joints.

Functions

all_link_transforms(robot, state)

@spec all_link_transforms(
  BB.Robot.t(),
  BB.Robot.State.t() | %{required(atom()) => float()}
) :: %{
  required(atom()) => BB.Math.Transform.t()
}

Compute transforms for all links in the robot.

Returns a map from link name to its transform in the base frame.

Examples

transforms = BB.Robot.Kinematics.all_link_transforms(robot, state)
end_effector_transform = transforms[:end_effector]

compute_joint_transform(robot, positions, joint_name)

@spec compute_joint_transform(BB.Robot.t(), %{required(atom()) => float()}, atom()) ::
  BB.Math.Transform.t()

Compute the transform for a single joint given its current position.

This combines the joint's fixed origin transform with the variable transform due to joint motion.

forward_kinematics(robot, state, target_link)

@spec forward_kinematics(
  BB.Robot.t(),
  BB.Robot.State.t() | %{required(atom()) => float()},
  atom()
) ::
  BB.Math.Transform.t()

Compute the forward kinematics transform from base to a target link.

Returns a 4x4 homogeneous transformation matrix representing the position and orientation of the target link in the base frame.

Parameters

robot: The Robot struct
state: The current robot state (or a map of joint positions)
target_link: The name of the link to compute the transform for

Examples

robot = MyRobot.robot()
{:ok, state} = BB.Robot.State.new(robot)
BB.Robot.State.set_joint_position(state, :shoulder, :math.pi() / 4)

transform = BB.Robot.Kinematics.forward_kinematics(robot, state, :forearm)
pos = BB.Math.Transform.get_translation(transform)

jacobian(robot, state, target_link, joint_names)

@spec jacobian(
  BB.Robot.t(),
  BB.Robot.State.t() | %{required(atom()) => float()},
  atom(),
  [atom()]
) ::
  Nx.Tensor.t()

Compute the spatial (position and orientation) Jacobian of a link.

Returns a {6, length(joint_names)} tensor: the top three rows are the position Jacobian (see position_jacobian/4) and the bottom three are the orientation Jacobian — each column the joint's rotation axis in the base frame for revolute joints, zero otherwise. This pairs with an orientation error expressed as a base-frame rotation vector.

Examples

jacobian = BB.Robot.Kinematics.jacobian(robot, positions, :tool0, joint_names)

link_position(robot, state_or_positions, target_link)

@spec link_position(
  BB.Robot.t(),
  BB.Robot.State.t() | %{required(atom()) => float()},
  atom()
) ::
  {float(), float(), float()}

Get the position of a link in the base frame.

This is a convenience function that extracts just the translation from the forward kinematics transform.

Examples

{x, y, z} = BB.Robot.Kinematics.link_position(robot, state, :end_effector)

position_jacobian(robot, state, target_link, joint_names)

@spec position_jacobian(
  BB.Robot.t(),
  BB.Robot.State.t() | %{required(atom()) => float()},
  atom(),
  [
    atom()
  ]
) :: Nx.Tensor.t()

Compute the position Jacobian of a link with respect to the given joints.

Returns a {3, length(joint_names)} tensor where each column is the partial derivative of the link's base-frame position with respect to that joint's position. Joints that do not lie on the chain to target_link (and so do not move it) get a zero column. Columns follow joint_names order.

Computed analytically by differentiating the forward-kinematics defn, rather than by finite differences.

Examples

jacobian = BB.Robot.Kinematics.position_jacobian(robot, positions, :tool0, joint_names)