Your kinematics meets theirs

Point the transform maths you wrote in Module 1 at the SO-101, diff it against MuJoCo's own answer, and find out that the geometry agrees to the last bit while the physics does not.

Where you are. You can load the SO-101, write six numbers into data.ctrl and step the physics. This lesson points the transform code you wrote in Module 1 at that same arm, and measures whether the two of you agree.

Somebody already wrote your library

You spend three weeks implementing a spec. Then you find out there is a reference implementation, and it has been on your machine the whole time.

You do not delete yours. You run both over the same inputs and subtract. If the outputs differ in the fifteenth decimal place, that is floating-point noise and you have learned that you understood the spec. If they differ in the second decimal place, you have a bug with a return address: you know it is in your code, you know which input exposes it, and you can bisect the difference until it confesses.

That diff is worth more than either implementation alone. It is the only test that says you understood the thing, as opposed to the code ran without crashing.

You have a reference implementation of forward kinematics on your laptop right now. It is the engine you spent the last six lessons installing and driving.

The idea in one paragraph

MuJoCo already knows where the arm’s fingertips are; it has to, or it could not draw them. Your Module 1 code also knows, if you point it at the same robot. Running both and subtracting is the sharpest test you will ever run on your own understanding, because there is no partial credit: the answers agree to machine precision or they do not. And once they agree exactly, something more useful becomes possible. Every remaining difference between where you told the arm to go and where the arm actually is has to be physics, because the geometry has been ruled out. Kinematics stops being an approximation you hope is close and becomes a fixed reference you can measure everything else against.

The same five joint angles feed two independent implementations, your transform chain and MuJoCo's forward pass, and both land on the same tool tip pose; the worst disagreement over two thousand random poses is 3.5 times ten to the minus sixteen metres

Wider than the screen; scroll it sideways.

Six multiplications, and none of them are new

The SO-101 is a chain of bodies, each attached to its parent by a fixed offset and one hinge. Reading the arm model walked that tree; here you are going to do arithmetic on it.

One edge of the chain has exactly two parts. The child body sits at a fixed position and orientation relative to its parent, which is machined into the robot and written into the model file. Then the joint turns the child about its own axis by however many radians you commanded.

One step of the chain: the parent frame is carried to the child by a fixed offset read from the model file, then rotated by the joint angle about the child's own z axis; only the second factor changes when the robot moves

Wider than the screen; scroll it sideways.

Written out, with $T_k$ the fixed offset of link $k$ and $R_z(q_k)$ its hinge:

T_{\text{world},\,\text{tip}} \;=\; T_{\text{base}}\left[\prod_{k=1}^{5} T_k \, R_z(q_k)\right] T_{\text{tip}}

In words: start at the world origin, apply each link’s fixed offset and then its joint rotation in order, and finish with one more fixed offset from the last body to the point between the fingertips. That is composing transform chains with the robot’s actual dimensions substituted in.

The last offset lands on a site, and this arm ships the one you want: gripperframe, sitting between the jaw tips. It is a site like any other, massless and collision-free, but it carries an orientation as well as a position, so it is enough to describe a full pose.

The fixed offsets come out of the compiled model: model.body_pos[b] and model.body_quat[b] for each body, plus model.site_pos and model.site_quat for the tool point. Reading those is not cheating. They are the robot’s dimensions, which is what a model file is for; you would otherwise be retyping thirty numbers out of an XML file and introducing typos. What stays yours is the arithmetic: quaternion to rotation matrix, pack into a homogeneous transform, compose in the right order. Nothing in those thirty numbers depends on the joint angles, so none of them can smuggle the answer in.

The diff

mujoco.mj_forward(model, data) runs the engine’s kinematics without integrating anything forward in time: it takes data.qpos and fills in every body and site position. The tool point lands in data.site_xpos[site_id], and its orientation in data.site_xmat[site_id] as a flattened 3x3.

So the test writes itself. Sample joint angles inside the limits, run your chain, run mj_forward, subtract.

Real example

Two thousand random poses, from solutions/fk_so101.py:

2000 random poses inside the joint limits
  worst position disagreement  3.478e-16 m
  worst rotation disagreement  1.332e-15 (matrix element)

About a third of a femtometre. For scale, a proton is roughly $10^{-15}$ m across.

The number matters because of what it rules out. Any disagreement bigger than about $10^{-12}$ would mean a real difference: a wrong multiplication order, a transposed rotation, a quaternion convention mismatch, an offset applied before its rotation instead of after. Those failures do not produce small errors. They produce centimetres. Swap two of the six link offsets in the chain and the tool point moves by a quarter of a metre, which you will do deliberately in the exercise.

The Jacobian, computed twice

The Jacobian is the matrix that answers: if I move each joint a little, which way does the tool point go, and how fast? For a 3D position on a five-joint arm it is 3 by 5. Lesson 1.13 built one for the 2-link arm two ways, analytically and by finite differences, and checked them against each other.

Here you only need the finite-difference version, because writing the analytic one for a 3D chain is a page of algebra and MuJoCo will hand you the exact answer for free. Nudge one joint by a tiny $\varepsilon$ , run your own FK again, divide the change by $\varepsilon$ ; that is one column. mujoco.mj_jacSite fills in the whole thing analytically.

They agree to about $2 \times 10^{-7}$ m per radian, not to $10^{-16}$ , and the gap is the interesting part. A finite difference is a straight line drawn across a curve, so it carries a truncation error that shrinks as $\varepsilon$ shrinks. It also subtracts two nearly equal numbers, so it carries a round-off error that grows as $\varepsilon$ shrinks. Sweep $\varepsilon$ and you can watch the two trade places:

$\varepsilon$	worst disagreement, m/rad
$10^{-2}$	$2.2 \times 10^{-3}$
$10^{-4}$	$2.2 \times 10^{-5}$
$10^{-6}$	$2.2 \times 10^{-7}$
$10^{-8}$	$1.8 \times 10^{-8}$
$10^{-10}$	$1.6 \times 10^{-6}$
$10^{-12}$	$1.7 \times 10^{-4}$

The bottom of the valley sits near $\varepsilon = 10^{-8}$ , which is the square root of double-precision epsilon, exactly where the theory says it should be. Below that, making the step smaller makes the answer worse.

Inverse kinematics, on an arm that is not a toy

Now run it backwards. Same damped-least-squares update as Lesson 1.15, with the error now a 3-vector and the Jacobian 3 by 5:

\Delta \mathbf{q} = J^{\top}\left(J J^{\top} + \lambda^{2} I\right)^{-1} \mathbf{e}

Two things change on a real arm. Joint limits are real, so every step gets clipped into range. And the arm has five joints for three numbers of position, which means a two-dimensional family of joint angles reaches any given point; damped least squares quietly picks the smallest step each iteration, so the wrist settles wherever it happens to settle rather than in an orientation you chose. Controlling orientation as well means adding three more rows to the Jacobian, and Lesson 2.5 already showed why you cannot have all six on this arm.

Measure	Value
targets solved	198
median iterations	16
median residual	0.05 mm
targets that were below the floor	49

n = 200 trials · reach a point sampled from the arm's own workspace · 2026-08-09

That last row is the honest one. Targets were generated by picking random joint angles and running FK, so every one of them is reachable by construction. A quarter of them are still underground, because joint limits describe which configurations the mechanism can hold, not which places are sensible to be. Reachable is a claim about geometry. Usable is a claim about the world.

Where the agreement stops

Take an IK answer, write it into data.ctrl, and let two seconds of physics run. The geometry says the tool tip belongs at the target. Where does it actually end up?

A logarithmic ladder of error: your kinematics and MuJoCo's agree to about ten to the minus sixteen metres, a position servo settles about a fifth of a millimetre from the commanded point in free space, and the same servo stops eighty six millimetres short when the arm is pressed into the floor

Wider than the screen; scroll it sideways.

Group	Poses	Median gap	Worst gap
settled in free space	145	0.22 mm	0.54 mm
settled touching the floor	53	86.3 mm	237.7 mm

n = 198 trials · command each IK solution, then run 2 s of physics · 2026-08-09

The free-space number is the position actuator doing what Lesson 2.7 said it would: a proportional controller with a finite gain holds a small standing error against gravity, forever. Two tenths of a millimetre, which is twelve orders of magnitude above the geometry error.

The contact number is a different animal entirely. Those fifty-three poses put the arm through the floor, and the servo is doing exactly what it was told, pushing toward an angle it can never reach. The gap is not error. It is the world refusing.

That is the payoff for having verified the geometry first. You now have a reference implementation of your own that is provably exact, so when the arm ends up 86 mm from where you asked, you never have to wonder whether your maths was wrong. It was not. Something in the world moved.

Check yourself

1. Your FK reads model.body_pos and model.body_quat straight out of MuJoCo’s compiled model. Does that make the comparison circular?

No, and the reason is that none of those numbers depend on the joint angles. They are the robot’s fixed dimensions, the same information you would otherwise retype from the XML. What the comparison actually tests is the arithmetic you wrote on top of them: quaternion to matrix, transform packing, and above all composition order. Those are exactly the things that go wrong, and none of them are supplied by the model file. If you want to prove it to yourself, hard-code the thirty numbers from the XML and check that you get the same answer.

2. Your FK and MuJoCo’s agree to

3 \times 10^{-16}

m, but your Jacobian and mj_jacSite only agree to

2 \times 10^{-7}

. Why is the second number so much larger, and why does shrinking

\varepsilon

10^{-12}

make it worse rather than better?

The FK comparison is two exact evaluations of the same formula, so the only error is rounding, about $10^{-16}$ relative. The Jacobian comparison involves an approximation: a finite difference is a chord across a curve, and it misses by an amount proportional to $\varepsilon$ . Shrinking $\varepsilon$ shrinks that truncation error but forces you to subtract two numbers that agree in ever more digits, so the difference loses significant figures to cancellation. The two effects cross near $\varepsilon = \sqrt{\text{machine epsilon}} \approx 10^{-8}$ , which is where the measured valley bottoms out. Below it, cancellation wins and the answer degrades.

3. Two hundred IK targets were sampled by running FK on random joint angles, so all of them are reachable. Forty-nine were below the floor. What does that say about the difference between joint limits and a workspace?

Joint limits are a statement about the mechanism in isolation: which configurations the hardware can hold. A workspace is a statement about the mechanism plus its surroundings: which poses are reachable and legal and useful. Nothing in the joint ranges knows there is a table, and kinematics has no concept of solid objects. Any workspace you quote has to be intersected with the scene, which is one reason building a task scene is its own lesson.

4. Commanding an IK solution and running the physics leaves a 0.22 mm gap in free space and an 86 mm gap when the arm is against the floor. Which of those two is a bug?

Neither, and they have completely different causes. The 0.22 mm is the position actuator’s steady-state error: a proportional controller needs a non-zero error to produce the torque that holds the arm against gravity, so it never fully closes. The 86 mm is contact. The commanded angle is inside the floor, so the servo pushes and the floor pushes back, and the arm stops where the forces balance. The first shrinks if you raise the gain. The second does not shrink at all; it is the simulator being right.

5. Your DLS solver has five joints and three equations. What is the extra freedom doing, and why does the wrist end up in an orientation you did not choose?

Five joints for three constraints leaves a two-dimensional set of solutions for any reachable point, and the update $J^{\top}(JJ^{\top} + \lambda^{2}I)^{-1}\mathbf{e}$ picks the smallest joint motion that reduces the position error. Smallest is a choice, but it is a choice about effort, not about orientation, so the wrist drifts to whatever the path of least motion happened to produce. To control orientation you add three more rows to the Jacobian and three more components to the error; on this arm you can afford at most two of them, because the fingertip Jacobian has rank 5.

6. You break your FK by swapping two of the six link offsets. How large an error should you expect, and why is that reassuring?

Centimetres to tens of centimetres; the measured median tip disagreement is about 0.25 m and the worst case about 0.59 m. It is reassuring because it means the test has teeth. A verification whose failures are small is not a verification, since a failure and a pass look alike. Composition-order bugs in transform chains fail loudly, which is why comparing against a reference implementation catches them on the first run instead of three lessons later.

Do this

Finish code/fk_so101.py and code/ik_so101.py, then run four experiments. About forty minutes.

1. Make the diff pass. Fill in quat_to_R and fk in fk_so101.py, then run it. You want the worst position disagreement under $10^{-12}$ m. If it is centimetres, you have a composition-order or quaternion-convention bug, and the fastest way to find it is to print your chain’s answer body by body against data.xpos[b] for the same body; the first row that diverges names the broken edge.

2. Break it on purpose. In chain_geometry, swap two entries of offsets and re-run. Note how big the error becomes. Put it back. You have just measured the sensitivity of the test, which is the only way to know a passing test means anything.

3. Sweep the finite-difference step. Fill in jacobian_fd and ik_step in ik_so101.py, run it, then call jacobian_fd with eps set to each of $10^{-2}$ , $10^{-4}$ , $10^{-6}$ , $10^{-8}$ , $10^{-10}$ and $10^{-12}$ , comparing each against jacobian_mujoco. Reproduce the valley in the table above and write down where the bottom is on your machine.

4. Watch the physics disagree. Run python ik_so101.py --settle. Report both groups: free space and contact. Then raise the servo gain by editing kp in the sts3215 default class of so101.xml, from 998.22 to 4000, and re-run. The free-space gap should shrink; the contact gap should not. Put the number back when you are done, because Lesson 2.5 needs it where it was.

What you can now do

You can write forward kinematics for a real five-joint arm from its model file, verify it against the physics engine to machine precision, build a position Jacobian by finite differences and know which step size to use, and solve inverse kinematics with joint limits and restarts. More importantly, you can now separate the two kinds of error in every experiment that follows: the geometry, which is exact and yours, and the physics, which is not exact and belongs to the world.