Kinematics · repose-kinematics

Jacobians

The Jacobian connects joint velocities to tool velocity. Column j says how the tool moves when only joint j moves. IK, servoing and singularity checks are all built on it.

Space Jacobian

rust
let mut j = [[0.0_f64; 6]; 6];          // 6 rows × N columns
chain.geometric_jacobian(&q, &mut j)?;
twist = J(q) · q̇
[ v_x v_y v_z  ω_x ω_y ω_z ]ᵀ  =  J(q) · [ q̇_0 … q̇_{N-1} ]ᵀ
  rows 0..3: linear velocity of the tool origin   (m/s)
  rows 3..6: angular velocity of the tool         (rad/s)
  both expressed in the base frame

For a revolute joint with world axis zj through point pj, the column is [ zj × (ptool − pj) ; zj ]. For a prismatic joint it is [ zj ; 0 ]. A fixed joint contributes a zero column. Linear velocity is always taken at the tool origin, so the tool offset matters.

Body Jacobian

rust
chain.body_jacobian(&q, &mut j)?;

Same velocities, same tool origin, but expressed in the tool's own frame. Use it when a command reads naturally from the tool's point of view: "move 1 cm/s along the gripper's approach axis" is a body-frame twist, whatever the arm's pose.

Twists and frames

CuTwist6D is a typed tool velocity with an explicit frame:

rust
let twist = CuTwist6D::new(
    [mps(0.0), mps(0.0), mps(0.02)],    // linear: 2 cm/s
    [rps(0.0); 3],                     // angular
    CuTwistFrame::Body,                // or CuTwistFrame::Base (default)
);

Base pairs with the space Jacobian, Body with the body Jacobian. Differential IK and the servo task pick the right one from the twist's frame, so you never mix them by accident.

Singularities and manipulability

At a singularity the arm loses the ability to move the tool in some direction: the wrist axes line up, the elbow is fully stretched, the wrist sits on the base axis. There, J loses rank and naive IK asks for infinite joint speeds.

rust
let w2 = chain.manipulability_squared(&q)?;   // Yoshikawa: w² = det(J Jᵀ), ≥ 0

w² is zero exactly at a singularity and grows as the arm moves away from one. It has no absolute scale, since it depends on the robot's size, so compare it against values from your own robot. Differential IK uses it to increase damping smoothly as the arm approaches a singularity (Servoing). You can use it yourself to reject IK solutions or planned goals that sit too close to one.

API reference