Forward kinematics expressed as a single composable defn.
BB.Robot.Kinematics packs a chain's static structure (joint origins, axes
and types) into plain tensors once, then calls fk_chain/6 to walk the chain
in one fused computation rather than dozens of eager per-op BB.Math calls.
Keeping the whole chain walk in defn is the point of beam-bots/bb#147: the
computation can be JIT-compiled and, with a leading batch axis on the inputs,
vectorised across many joint configurations or many targets at once.
Tensor layout
For a chain of n joints (root-most first), all inputs are :f64:
positions—{n}joint positions (radians for revolute, metres for prismatic)origin_rpy—{n, 3}per-joint origin orientation as{roll, pitch, yaw}origin_xyz—{n, 3}per-joint origin translationaxes—{n, 3}per-joint motion axis (unit vector)is_revolute—{n}1.0for revolute/continuous joints, else0.0is_prismatic—{n}1.0for prismatic joints, else0.0stored—{n, 4, 4}per-joint multi-DoF motion, identity for single-DoF jointsdeltas—{n, 6}per-joint local perturbation, always passed as zeros
The result is the {4, 4} base-to-tip homogeneous transform. Each joint
contributes
origin · scalar_motion(q) · stored · (I + hat(delta))where origin = Rx · Ry · Rz · T(xyz) reproduces
BB.Math.Transform.from_origin/1, and scalar_motion is a Rodrigues rotation
about axis (revolute) or a translation along axis (prismatic).
Why multi-DoF joints arrive as a matrix and a zero perturbation
A :floating joint's configuration is a BB.Math.Transform and a :planar
joint's is a BB.Math.Transform2D lifted into one. Neither can be handed to
this kernel as scalars without decomposing the rotation into three angles about
three axes — an Euler decomposition, which is lossy, non-unique and
gimbal-locked. So the matrix arrives verbatim in stored, and forward
kinematics is bit-exact.
The Jacobian still needs a differentiable parameter, which is what deltas is.
It is only ever evaluated at zero, and that makes the first-order factor
I + hat(delta) exactly right for both jobs:
- at zero it is the identity, so it contributes nothing to forward kinematics;
exp(delta) = I + hat(delta) + O(delta²), so its derivative at zero matches the true exponential's. The dropped terms are never evaluated.
Differentiating at the identity also means an Euler chart's singularities are
never visited, so there is no gimbal lock to regularise — and unlike a genuine
se(3) exponential there is no (1 - cos θ)/θ² to blow up at theta = 0.
A single-DoF joint carries stored = I and delta = 0, so its motion reduces
to the scalar form. A multi-DoF joint carries both masks at 0.0, so its
scalar_motion is the identity and its motion reduces to stored. One code
path serves both.
Summary
Functions
Walk a kinematic chain, returning the {4, 4} base-to-tip transform.
Compute every link's base-frame transform via a topological prefix-product scan.
Orientation (angular-velocity) Jacobian of the chain tip.
Orientation Jacobian of the chain tip with respect to each joint's local perturbation.
Position Jacobian of the chain tip with respect to each single-DoF joint position.
Position Jacobian of the chain tip with respect to each joint's local perturbation.
Functions
Walk a kinematic chain, returning the {4, 4} base-to-tip transform.
See the module documentation for the tensor layout.
Compute every link's base-frame transform via a topological prefix-product scan.
One row per link, ordered root-first so a link's parent always precedes it
(parent_idx[i] < i for every non-root link). parent_idx indexes into this
same ordering; the root carries its own index and an identity joint transform,
so it resolves to the identity. The per-joint inputs follow the same layout as
fk_chain/6, describing each link's parent joint (identity-valued for the
root). Returns {n, 4, 4}, one transform per link in input order.
Orientation (angular-velocity) Jacobian of the chain tip.
Returns {3, n} where column j is the joint's rotation axis expressed in
the base frame (z_j) for revolute/continuous joints, and zero for prismatic
or fixed joints — the standard geometric angular Jacobian. Stacked beneath the
position Jacobian it forms the {6, n} spatial Jacobian, paired with a
base-frame rotation-vector orientation error.
Inputs follow the fk_chain/6 layout. A prefix-product scan walks the chain
accumulating the transform up to each joint's axis frame; grad is not
involved, so the data-dependent while is fine here.
Orientation Jacobian of the chain tip with respect to each joint's local perturbation.
Returns {3, n, 6}, matching position_jacobian_deltas/8's layout. A
perturbation's three translation components produce no angular velocity, so
those blocks are zero; its three rotation components produce the columns of the
rotation taking the joint's post-motion frame into the base frame.
That frame is the right one because the perturbation is applied after the joint's origin and stored motion — see the module documentation.
Position Jacobian of the chain tip with respect to each single-DoF joint position.
Computed by differentiating fk_chain/8's tip translation via grad — the
composable-defn payoff #147 is after: no finite differences, no extra
forward-kinematics evaluations. Inputs follow the fk_chain/8 layout. Returns
{3, n}: row = spatial axis (x, y, z), column = chain joint in input order.
Multi-DoF joints get a zero column here, since their motion does not depend on
positions. Their columns come from position_jacobian_deltas/8.
Position Jacobian of the chain tip with respect to each joint's local perturbation.
Returns {3, n, 6}: row = spatial axis, then chain joint in input order, then
the six components of that joint's local perturbation — three translations
followed by three rotations, in the frame the joint's motion leaves behind.
A single-DoF joint's block is meaningless and is discarded by the caller; only
:planar and :floating joints draw their columns from here, projected onto
the degrees of freedom they actually have. See the module documentation for why
differentiating at deltas = 0 is exact.