Kinematics · repose-kinematics

Forward kinematics

Forward kinematics (FK) answers: given the joint values, where is the tool? It is a single pass over the chain, has exactly one answer, and always succeeds for valid input.

Tool pose

rust
let mut pose = Transform3D::<f64>::identity();
chain.forward_kinematics(&q, &mut pose)?;

let m = pose.to_matrix();               // row-major 4×4 homogeneous matrix
let (x, y, z) = (m[0][3], m[1][3], m[2][3]);   // tool position in metres, base frame

The result is the base-to-tool transform, tool offset included: rotation in the upper-left 3×3, position in the last column. It fails only when a value is non-finite or has the wrong unit for its joint. It does not check joint limits; use is_within_limits for that.

Collision checking, visualisation and debugging need more than the tool. forward_frames writes the frame of every joint in the same pass:

rust
let mut frames = [Transform3D::<f64>::identity(); 6];
let mut tool = Transform3D::<f64>::identity();
chain.forward_frames(&q, &mut frames, &mut tool)?;
// frames[i] = base → joint i, after joint i's motion

frames[i] is the accumulated transform up to and including joint i's motion, without the tool offset. These are exactly the frames collision shapes are attached to: a CuLinkShape with "frame": 2 moves with frames[2], and one without a frame stays on the base. See Robot collision models.

Cost

One FK call is N small transform compositions. It allocates nothing and is cheap enough to run every cycle, inside IK loops and inside planners' collision checks, which call it thousands of times per plan.

As a Copper task

CuForwardKinematicsTask<N> runs FK on every incoming CuJointPositionsPayload<N> and publishes a Transform3D<f64>. A cycle without input publishes nothing. See Copper tasks.

Verify a model

When you bring up a new robot, compare FK against the vendor's own pose readout at a few configurations (home, each joint at ±90°). A wrong axis sign or a swapped origin shows up immediately as a tool pose that's off by a mirror or a link length. Fix the chain before you plan with it.

API reference