The Jacobian, demystified

How joint velocities map to end-effector velocities — both translation and rotation — and why the rotation half needs a special trick. Pitched at your numeric_ik.py / _diff_ik_step in sim_sales.

1. The setup: what are we even mapping?

You have $n$ joint angles $q = (q_1, \dots, q_n)$. Forward kinematics is a function $f$ that turns joints into an end-effector pose:

\[ T_{\text{ee}}(q) \;=\; f(q) \;\in\; SE(3) \]

$T_{\text{ee}}$ is a 4×4 homogeneous matrix — a 3×3 rotation $R(q)$ and a 3-vector position $p(q)$. Differentiating $f$ gives you the Jacobian $J(q)$ — the linear map from joint velocities to end-effector velocity:

\[ \underbrace{\begin{bmatrix} v \\ \omega \end{bmatrix}}_{6 \times 1\ \text{twist}} \;=\; \underbrace{J(q)}_{6 \times n} \;\underbrace{\dot q}_{n \times 1} \]

The top three rows of $J$ are the linear Jacobian $J_v$, the bottom three are the angular Jacobian $J_\omega$. This is exactly the matrix Pinocchio gives you with pin.computeFrameJacobian(...) at robot.py:336.

2. The geometric construction (column by column)

The clean way to build the Jacobian — instead of symbolically differentiating the FK — is to ask: if I rotate only joint $i$ by a tiny amount, what twist does that produce at the end-effector? The answer becomes column $i$ of $J$.

For a revolute joint $i$ with axis unit vector $\hat z_i$ (expressed in the world frame) located at point $p_i$:

\[ J_i \;=\; \begin{bmatrix} J_{v,i} \\[2pt] J_{\omega,i} \end{bmatrix} \;=\; \begin{bmatrix} \hat z_i \times (p_{\text{ee}} - p_i) \\[2pt] \hat z_i \end{bmatrix} \]

For a prismatic joint $i$ sliding along $\hat z_i$:

\[ J_i \;=\; \begin{bmatrix} \hat z_i \\[2pt] 0 \end{bmatrix} \]

That's it. Stack those columns for every unlocked joint and you have a 6×$n$ matrix.

Why does $\hat z_i \times (p_{\text{ee}} - p_i)$ show up?

Rotating about an axis $\hat z_i$ passing through $p_i$ at angular speed $\dot q_i$ produces, at any point $p$, the velocity

\[ v_p \;=\; (\dot q_i\, \hat z_i) \times (p - p_i) \]

This is just the formula for rigid-body rotation: angular velocity cross radius. Set $p = p_{\text{ee}}$, factor out $\dot q_i$, and you've got the linear-Jacobian column. The angular part is dead simple — turning joint $i$ contributes its own axis $\hat z_i$ to the EE's angular velocity. So $J_{\omega,i} = \hat z_i$.

Where do $\hat z_i$ and $p_i$ come from?

Walk the kinematic chain. Each joint $i$ has a transform $T_i(q_i)$ from its parent. Compose them to get the joint's world placement $T^w_i = T_0 \cdot T_1(q_1) \cdots T_i(q_i)$. Then $p_i$ is the translation part of $T^w_i$, and $\hat z_i$ is the third column of its rotation block (the local joint axis, rotated into world coordinates). This is exactly what pin.forwardKinematics + pin.updateFramePlacements computes before you call computeFrameJacobian.

3. The rotation half — why it's harder, and how the log map saves you

Here's the question you asked: how does the rotation of the EE map to joints? The angular part of the Jacobian, $J_\omega$, is easy to build ($\hat z_i$ per revolute joint). The hard part is using it, because orientation error is not a vector.

If positions $p$ live in $\mathbb{R}^3$, you can subtract them: $p_{\text{target}} - p_{\text{current}}$ is a vector you can shove into your loop. But for rotations $R \in SO(3)$, $R_{\text{target}} - R_{\text{current}}$ is meaningless — it's not a rotation, it's some random 3×3 matrix. You can't iterate on it.

The fix: the matrix logarithm on $SO(3)$

Every rotation can be written as “rotate by angle $\theta$ around unit axis $\hat u$”. Pack that into a single 3-vector $\theta \hat u \in \mathbb{R}^3$. That's the axis-angle representation — and it's the log map on $SO(3)$:

\[ \log_{SO(3)} : SO(3) \to \mathbb{R}^3, \qquad \log(R) \;=\; \theta\,\hat u \]

To get an orientation error as a usable 3-vector, you compute the rotation that takes you from current to target, then take its log:

\[ e_\omega \;=\; \log\!\bigl(R_{\text{current}}^{-1}\, R_{\text{target}}\bigr) \]

Now $e_\omega$ has the same shape as $\omega$ in your twist — it's just “the angular velocity you'd need to apply for 1 second to get from current orientation to target”. And that's exactly the thing $J_\omega \dot q$ produces.

Pinocchio's log6 does this for the full pose: it takes $T_{\text{current}}^{-1} T_{\text{target}}$ (a 4×4) and returns a 6-vector $[v;\,\omega]$ — the twist that bridges the two poses. That is line 334 of your robot.py:

error = pin.log6(current_se3.inverse() * desired_se3).vector
J     = pin.computeFrameJacobian(model, data, q_actual, ee_id, pin.LOCAL)

4. From error to joint update: damped least squares

Now we have $J(q)$ and $e = \log_6(T_c^{-1}T_t)$, and we want $\dot q$. The Jacobian equation $e \approx J\,\dot q$ usually has more unknowns than equations (most arms have $n \ge 6$) or fewer (a 5-DOF arm trying to hit a 6-DOF pose). Either way, we use the pseudo-inverse:

\[ \dot q \;=\; J^\dagger\, e \]

Plain $J^\dagger = J^T(JJ^T)^{-1}$ blows up near singularities. The fix in your code is damped least squares: add a tiny regulariser inside the inverse:

\[ \dot q \;=\; J^T\,(J J^T + \lambda^2 I)^{-1}\, e \]

Which is line 343–345 verbatim:

JJt    = J @ J.T + params.damping * np.eye(6)
J_pinv = J.T @ np.linalg.inv(JJt)
dq     = J_pinv @ (gain * error)

Then you step: $q \leftarrow q + \dot q$, recompute everything, repeat until $e$ is small. Each iteration the linearisation is fresh, so you can chase non-trivial pose changes.

The null-space term

For a 7-DOF arm, $J^\dagger J \ne I$ — there's a 1-dimensional null space of joint motions that don't move the EE at all. You exploit that to bias the elbow toward “home” without disturbing the EE:

\[ \dot q \;=\; J^\dagger e \;+\; (I - J^\dagger J)\,k(q_{\text{home}} - q) \]

That's dq_secondary at robot.py:347. The projector $(I - J^\dagger J)$ kills anything in the row-space of $J$, so it can't fight the primary task.

5. What's different in numeric_ik.py?

numeric_ik.py never writes $J$ explicitly. Instead it formulates IK as an optimisation problem:

\[ \min_q \;\; \underbrace{w_p\,\|e_v\|^2 + w_o\,\|e_\omega\|^2}_{\text{tracking}} \;+\; w_r \|q\|^2 \;+\; w_s \|q - q_{\text{prev}}\|^2 \]

with $[e_v;\,e_\omega] = \log_6(T_c^{-1}T_t)$. CasADi differentiates the entire cost symbolically and hands the Jacobian (and Hessian) to IPOPT — so the same maths is happening, just behind the abstraction. The advantage is that joint limits become hard constraints and you can add regularisation/smoothness terms cleanly. The cost is that each opti.solve() is heavier than one damped-LS step.

Rule of thumb in your codebase:

• Big jumps (planner waypoints, place poses) → ArmIK.solve().
• Smooth trajectory following (your slerp-driven carry) → _diff_ik_step with a 6×n Jacobian.

6. TL;DR cheat sheet

Generated for the IK in sim_sales/servo7/utils/numeric_ik.py and sim_sales/servo7/sim/scene_api/robot.py. Open with any browser; MathJax loads from CDN.