BB.Robot.Kinematics (bb v0.27.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 configurations:

# 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)}

Multi-DoF joints

A joint's configuration is shaped to its type: a bare float for single-DoF joints, a BB.Math.Transform2D for :planar, a BB.Math.Transform for :floating. See BB.Robot.State for the full table. A multi-DoF joint's transform is used verbatim, so forward kinematics through a floating base is bit-exact.

This makes Jacobian width the sum of degrees of freedom along the chain rather than the number of joints in it — a floating joint contributes six columns and a planar one three. jacobian_columns/2 reports which joint and degree of freedom each column belongs to.

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

Types

configuration()

A joint's configuration, shaped to its type.

configurations()

A map of joint configurations, as BB.Robot.State returns.

Functions

all_link_transforms(robot, state)

Compute transforms for all links in the robot.

compute_joint_transform(robot, configurations, 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.

jacobian_columns(robot, joint_names)

Describe the columns a Jacobian over joint_names will have.

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.

Types

configuration()

@type configuration() :: BB.Robot.State.configuration()

A joint's configuration, shaped to its type.

See BB.Robot.State for the table of which shape belongs to which joint type.

configurations()

@type configurations() :: %{required(atom()) => configuration()}

A map of joint configurations, as BB.Robot.State returns.

Functions

all_link_transforms(robot, state)

@spec all_link_transforms(BB.Robot.t(), BB.Robot.State.t() | configurations()) :: %{
  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, configurations, joint_name)

@spec compute_joint_transform(
  BB.Robot.t(),
  %{required(atom()) => configuration()},
  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() | configurations(), 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_configuration(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() | configurations(), atom(), [atom()]) ::
  Nx.Tensor.t()

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

Returns a {6, columns} tensor: the top three rows are the position Jacobian (see position_jacobian/4) and the bottom three are the orientation Jacobian. For a revolute joint the orientation column is its rotation axis in the base frame; for a multi-DoF joint's rotational degrees of freedom it is the corresponding axis of the frame the joint's motion leaves behind; for prismatic and purely translational degrees of freedom it is zero. This pairs with an orientation error expressed as a base-frame rotation vector.

As with position_jacobian/4, width is the sum of degrees of freedom over joint_names — see jacobian_columns/2.

Examples

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

jacobian_columns(robot, joint_names)

@spec jacobian_columns(BB.Robot.t(), [atom()]) :: [{atom(), non_neg_integer()}]

Describe the columns a Jacobian over joint_names will have.

Jacobian width is the sum of degrees of freedom along the chain rather than the number of joints, so a caller applying a solver's delta needs to know which joint and which degree of freedom each column belongs to. Returns one {joint_name, dof_index} per column, in column order. Fixed joints contribute nothing.

A :planar joint's three degrees of freedom are, in order, its two in-plane translations and its rotation about the plane normal — the same order as its BB.Math.Transform2D configuration. A :floating joint's six are three translations then three rotations, in the frame its motion leaves behind.

Examples

BB.Robot.Kinematics.jacobian_columns(robot, [:base, :mast])
#=> [{:base, 0}, {:base, 1}, {:base, 2}, {:mast, 0}]

link_position(robot, state_or_positions, target_link)

@spec link_position(BB.Robot.t(), BB.Robot.State.t() | configurations(), 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() | configurations(), atom(), [
  atom()
]) ::
  Nx.Tensor.t()

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

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

Width is the sum of degrees of freedom over joint_names, not their count — a :floating joint contributes six columns and a :planar one three, while a :fixed joint contributes none. Use jacobian_columns/2 to find out which joint and degree of freedom each column belongs to.

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

Examples

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