A course in articulated-body simulation, taught through a real implementation. Every line of code shown is the actual shipped source of src/robot.zig.
Part I — the vocabulary. What the equation is, and the three types everything
else is written in.
0 The equation of motion ·
1 Scope and layout of robot.zig ·
2 Spatial algebra: three types
Part II — describing a robot. A model as a value, validated by the compiler, and
the two structures the whole engine reads and writes.
3 The spec: what a user writes ·
4 Spec(): the comptime layer ·
5 Model: parallel arrays and the ancestor table ·
6 Data: the per-step scratch space ·
7 Position coordinates: the awkward part
Part III — where everything is. Configuration to Cartesian, and the frame that
makes the next part cheap.
10 Phase 1: forward kinematics ·
11 Two details: quaternion drift and the world body ·
12b comPos: the frame that makes CRB cheap
Part IV — the dynamics. The core of the subject: the mass matrix, its inverse
without inversion, the forces motion creates, and stepping.
14 The mass matrix: composite rigid body ·
15 Inverting M without inverting M ·
16 The forces motion creates ·
17 Stepping and testing the integrator ·
18 Jacobians: the bridge between two spaces ·
22 Integrators: four choices
→ 19 What you can do now — the checkpoint at the end of Part IV.
Part V — touching things. The half that turns a dynamics library into a
simulator.
20 Contact as a constraint ·
21 Two constraint solvers ·
23 Closing loops: constraints a tree cannot express ·
29 Finding the contacts in the first place ·
30 Soft contact and the flesh model ·
27 Where the collision detector lives
Part VI — a real robot. Loading one, reading it, and driving it.
24 Reading a real robot: MJCF ·
25 Sensors ·
26 Control: actuation, servos and inverse kinematics
→ 28 Phase 3: what you can simulate — the checkpoint at the end.
Part VII — planning. Choosing a whole sequence of controls instead of reacting
to the current error. The maths is derived from scratch; no prior optimal-control needed.
31 Planning: choosing the torques ·
32 One step, on paper ·
33 Two steps, and the recursion ·
34 Q-notation ·
35 iLQR: local linearization of a nonlinear system ·
36 The backward pass, in code ·
37 The forward pass and the line search ·
38 Regularization ·
39 Where f32 stops you ·
40 From a plan to a controller ·
41 Example: crane anti-sway ·
42 Example: rocket descent ·
43 Example: tracking a moving target
44 Retargeting: driving a robot from motion capture
Asides. Not part of the through-line.
8 The tests ·
9 What phase 0 leaves out ·
12 Verifying the page against the code ·
13 What the later phases need from this one
Point at a robot arm. It has, say, four joints. Give each one an angle and you have completely specified where every part of it is — four numbers describe the whole machine.
Now ask the question that turns geometry into physics: if I apply this torque at each motor, what does the arm do?
The answer is one line:
Read left to right. M(q)·v̇ is mass times acceleration, in joint coordinates — and M depends on the configuration q, because an extended arm is harder to swing than a folded one. c(q,v) collects everything velocity and gravity conspire to do to you: Coriolis, centrifugal, weight. τ is what your motors apply. Jᵀf is what the world pushes back with when you touch it.
Rearranged, it is a recipe:
Everything in this course computes one of those symbols. Kinematics finds where things are. CRB builds M. RNE builds c. Jacobians build J. A constraint solver finds f. When you have all five you can simulate anything with joints — and, more interestingly, you can invert the question and ask what torques would produce a motion you want.
You could. Give every link a position and orientation — six numbers each — and add constraints that hold the joints together, then let a solver push the parts back into place each step. That is how game physics engines work, and zimr has one (zimrphysics.zig, a Jolt port). For a thousand crates it is the right answer: each body is independent, it parallelises trivially, and a millimetre of joint error nobody will ever see.
For a robot it works poorly, because the error concentrates where it matters most. A solver enforces joints approximately; error accumulates down a chain; a shoulder joint that drifts a millimetre puts the hand centimetres away. And a stiff, heavy base driving a light fingertip is precisely the case iterative solvers converge worst on.
Generalized coordinates make the question disappear. If the arm is four numbers, there is no joint to violate — the freedom to come apart was never represented. That is the trade this course is about: more work per step, in exchange for constraints that hold exactly instead of approximately.
The order is the order the computation actually runs, which is also the order the ideas build:
Where a design could reasonably have gone another way, the alternative is shown and the tradeoff named. The formulas are all standard and can be looked up; choosing between two correct formulations is the part that takes practice, so those choices are made explicit wherever they arise.
The prerequisites are linear algebra and undergraduate mechanics: matrices, cross products, torque, angular momentum. No prior robotics is assumed — screw theory, spatial vectors and the recursive algorithms are introduced here as they are needed.
By the end you can simulate an articulated robot with contact, read a real robot description from MJCF, and compute the torques that drive it along a chosen trajectory. The sections fall into five phases, each leaving the simulator in a usable state:
| phase | sections | what works at the end of it |
|---|---|---|
| 0 | §1–9 | a robot can be described, and its data laid out |
| 1 | §10–13 | forward kinematics: where every part is |
| 2 | §14–19 | forward dynamics: apply torques, get motion |
| 3 | §20–30 | contact, closed loops, MJCF, sensors, control |
| 4 | §31–43 | trajectory optimization, and three worked examples |
zimrphysics.zig simulates in maximal coordinates: bodies carry full poses, joints are constraints, a solver enforces them to a tolerance. robot.zig simulates in generalized coordinates: a four-hinge arm is four numbers, and the joints cannot be violated because the freedom to violate them was never represented.
Everything the file will eventually contain computes one of those symbols. Phase 0 computes none of them; it defines the types they live in.
Two conventions are stated in the header and then relied on everywhere. Both are the kind that produce a three-turn debugging session if they drift:
zimr's linter reserves a vocabulary of maths names: using zm.dot3 qualified inside a function body is an error, because the house rule is to bind it once at the top and then write maths that reads like maths.
const zm = @import("zm");
// zm keywords must be bound at file scope (the linter enforces it), and binding them here
// also keeps the maths below readable as maths.
const Vec = zm.Vec;
pub const Quat = zm.Quat;
const vec = zm.vec;
const vec_zero = zm.vec_zero;
const quat_identity = zm.quat_identity;
const splat = zm.splat;
const dot3 = zm.dot3;
const cross = zm.cross;
const length3 = zm.length3;
const normalize3 = zm.normalize3;
const qmul = zm.qmul;
const rotate = zm.rotate;
const atan2 = zm.atan2;
const maxInt = zm.maxInt;
const pi = zm.pi;
const assertf = zm.assertf;Note what is not here: zimrphysics. The plan has robot.zig consuming it for mass properties and collision, but phase 0 uses neither, and the linter's unused-global rule caught the speculative import immediately. It arrives when it is used.
A rigid body's motion has six components; so does the force on it. Both are 6-vectors, and the whole file's sign convention hangs on their ordering:
// ANGULAR FIRST, then linear. (MuJoCo calls this `rot:lin`.)
//
// We store them as two `Vec` rather than a `[6]f32`. The fourth lane goes unused, which
// costs a little memory and buys the whole zm vocabulary — `cross`, `dot3`, `splat` — and
// code that reads like the vector maths it is.That is a deliberate tradeoff. A [6]f32 is denser; two Vec means every operation below is written in zm's vocabulary instead of index arithmetic.
/// A spatial motion vector: an angular velocity (or acceleration) and a linear one, both
/// expressed about a common reference point. Twists live here.
pub const Motion = struct {
ang: Vec = vec_zero,
lin: Vec = vec_zero,
pub const zero: Motion = .{};
pub fn add(a: Motion, b: Motion) Motion {
return .{ .ang = a.ang + b.ang, .lin = a.lin + b.lin };
}
pub fn sub(a: Motion, b: Motion) Motion {
return .{ .ang = a.ang - b.ang, .lin = a.lin - b.lin };
}
pub fn scale(a: Motion, s: f32) Motion {
const k: Vec = splat(s);
return .{ .ang = a.ang * k, .lin = a.lin * k };
}
};
/// A spatial force vector: a torque and a linear force about the same reference point.
/// Wrenches live here. Structurally identical to `Motion`, deliberately a separate type —
/// adding a velocity to a force is a bug the compiler can catch for free.
pub const Force = struct {
ang: Vec = vec_zero,
lin: Vec = vec_zero,
...
};The two structs are identical on purpose. In MuJoCo both are mjtNum[6] and nothing stops you adding a velocity to a force. Making them distinct types costs a few lines and buys a compile error at every site where the physics would have been silently wrong.
/// The pairing of a motion and a force — the only way the two types legally meet, and the
/// reason they are separate types. `dot6(v, f)` is the rate of work `f` does moving at `v`.
/// Named for MuJoCo's `mju_dot6` rather than `dot`, because it is emphatically not the
/// vector dot product: it contracts a 6-vector against its DUAL.
pub fn dot6(m: Motion, f: Force) f32 {
return dot3(m.ang, f.ang) + dot3(m.lin, f.lin);
}The name is dotSpatial because a spatial pairing contracts a vector against its dual — a different operation from the vector dot product, which merely happens to be computed the same way. Calling it dot would invite exactly the confusion the two separate types exist to prevent.
/// Spatial cross product for MOTION vectors: `v × m`. This is what carries a velocity
/// down a kinematic chain — a child's velocity is its parent's plus its own joint motion,
/// and the coupling term is exactly this.
pub fn crossMotion(v: Motion, m: Motion) Motion {
return .{
.ang = cross(v.ang, m.ang),
.lin = cross(v.ang, m.lin) + cross(v.lin, m.ang),
};
}
/// Spatial cross product for FORCE vectors: `v ×* f`. Not the same operator as
/// `crossMotion` — forces transform by the dual, which is where gyroscopic terms come
/// from. Getting these two confused produces a simulation that looks alive and conserves
/// nothing.
pub fn crossForce(v: Motion, f: Force) Force {
return .{
.ang = cross(v.ang, f.ang) + cross(v.lin, f.lin),
.lin = cross(v.ang, f.lin),
};
}Look closely at the difference: crossMotion puts the two-term sum in lin, crossForce puts it in ang. That asymmetry is the duality. Both will be used by RNE in phase 3 — one to propagate velocities down the tree, one to accumulate forces back up — and swapping them yields a simulator that runs, moves, and gains energy forever.
/// A rigid body's spatial inertia, in the ten-parameter form MuJoCo uses:
///
/// [ I skew(h) ] I = rotational inertia about the reference point
/// [ -skew(h) m·1 ] h = m · (centre of mass − reference point)
///
/// The packing matters. Because all bodies in one kinematic tree share a reference frame
/// (see `comPos` in phase 1), a COMPOSITE inertia is just the sum of its parts — which
/// makes the composite-rigid-body algorithm three vector adds and a scalar add per body.
/// That is the whole reason CRB is cheap, and the reason this is not stored as a 6×6.
pub const Inertia = struct {
/// Ixx, Iyy, Izz.
diag: Vec = vec_zero,
/// Ixy, Ixz, Iyz — the tensor is symmetric, so three numbers cover the rest.
off: Vec = vec_zero,
/// First moment of mass: `mass · com`. Zero when the reference point IS the centre
/// of mass, which is why a body's own inertia is usually stored that way.
h: Vec = vec_zero,
mass: f32 = 0,
pub const zero: Inertia = .{};
pub fn add(a: Inertia, b: Inertia) Inertia {
return .{
.diag = a.diag + b.diag,
.off = a.off + b.off,
.h = a.h + b.h,
.mass = a.mass + b.mass,
};
}A 6×6 spatial inertia has 36 entries, of which 10 are independent. Storing the 10 saves memory, and it also makes add a three-instruction operation, and the composite-rigid-body algorithm is nothing but a backward pass of add. When phase 2 arrives, CRB will be about sixty lines because this type was designed for it.
The property is pinned by a test that checks the algebra rather than a number:
test "spatial algebra: composite inertia is a plain sum" {
// This is the property CRB depends on: two bodies in a shared frame add up.
const a: Inertia = .{ .diag = vec(1, 2, 3), .off = vec(0.1, 0.2, 0.3), .h = vec(1, 0, 0), .mass = 5 };
const b: Inertia = .{ .diag = vec(4, 5, 6), .off = vec(0.4, 0.5, 0.6), .h = vec(0, 2, 0), .mass = 7 };
const sum: Inertia = a.add(b);
const v: Motion = .{ .ang = vec(0.3, -0.2, 0.7), .lin = vec(1.5, 0.25, -2) };
const combined: Force = sum.mul(v);
const separate: Force = a.mul(v).add(b.mul(v));
try expectApproxEqAbs(combined.ang[0], separate.ang[0], 1.0e-5);
...
} /// Apply this inertia to a motion, giving the momentum (or, with an acceleration in,
/// the force out). The expanded form of the 6×6 above.
pub fn mul(self: Inertia, m: Motion) Force {
// Rotational block: the symmetric tensor times the angular part.
const torque: Vec = self.applyTensor(m.ang);
return .{
// torque gains h × v_lin from the off-diagonal block
.ang = torque + cross(self.h, m.lin),
// force is m·v_lin, plus the ω × h coupling when the com is offset
.lin = m.lin * splat(self.mass) + cross(m.ang, self.h),
};
}
/// Translate the BODY by `offset`, keeping the reference point fixed — equivalently,
/// move the reference point by `−offset`. Used to place each body's own inertia into
/// the tree's shared frame.
///
/// (The direction is worth stating twice, because both readings are plausible and the
/// wrong one is a sign error you cannot see. `translate` of a point mass by `+d` puts
/// its centre of mass at `+d`, so `h` grows by `+m·d`.)
///
/// DERIVATION. Write the body's own inertia about its centre of mass as `I_c`, and its
/// centre of mass at `c` relative to the reference. Then the inertia about the
/// reference is the parallel-axis theorem:
///
/// I = I_c + m·(cᵀc·1 − c·cᵀ)
///
/// Translating the body by `d` sends `c → c + d`. Expanding and subtracting the
/// original leaves exactly two groups, which is how the code below is written:
///
/// ΔI = m·(dᵀd·1 − d·dᵀ) ← the pure point-mass term
/// + (2h·d)·1 − (h·dᵀ + d·hᵀ) ← cross terms, using h = m·c
///
/// The cross terms vanish when `h` is zero, which is the common case: a body's own
/// inertia is stored about its own centre of mass. The general form is kept anyway —
/// a COMPOSITE inertia carries a nonzero `h`, and silently being wrong for it in
/// phase 2 would be a nasty trap.
pub fn translate(self: Inertia, offset: Vec) Inertia {
const m: f32 = self.mass;
const d: Vec = offset;
const h: Vec = self.h;
// m·(dᵀd·1 − d·dᵀ), the pure point-mass term.
const dd: f32 = dot3(d, d);
var diag: Vec = vec(
m * (dd - d[0] * d[0]),
m * (dd - d[1] * d[1]),
m * (dd - d[2] * d[2]),
);
var off: Vec = vec(
-m * d[0] * d[1],
-m * d[0] * d[2],
-m * d[1] * d[2],
);
// Cross terms: (h·d)·1 − ½(h·dᵀ + d·hᵀ), symmetric by construction.
const hd: f32 = dot3(h, d);
diag += vec(
2.0 * (hd - h[0] * d[0]),
2.0 * (hd - h[1] * d[1]),
2.0 * (hd - h[2] * d[2]),
);
off += vec(
-(h[0] * d[1] + d[0] * h[1]),
-(h[0] * d[2] + d[0] * h[2]),
-(h[1] * d[2] + d[1] * h[2]),
);
return .{
.diag = self.diag + diag,
.off = self.off + off,
.h = h + d * splat(m),
.mass = m,
};
}Read it against the 2×2 block matrix in the doc comment and every term lines up: the symmetric tensor on the diagonal, skew(h) and its negative off it, m·1 in the corner. Writing the matrix in the comment is what makes the expanded arithmetic checkable by eye.
Moving an inertia is the parallel-axis theorem. Keeping the two contributions apart rather than folded into one expression per component — the folded version is faster to type and impossible to check by reading.
The direction is worth stating explicitly. "Translate by offset" has two plausible readings: move the body, or move the reference point. They differ by a sign, both compile, and both pass a test written against the same misunderstanding. So the doc comment states which one, twice:
/// Translate the BODY by `offset`, keeping the reference point fixed — equivalently,
/// move the reference point by `−offset`. Used to place each body's own inertia into
/// the tree's shared frame.
///
/// (The direction is worth stating twice, because both readings are plausible and the
/// wrong one is a sign error you cannot see. `translate` of a point mass by `+d` puts
/// its centre of mass at `+d`, so `h` grows by `+m·d`.)
///
/// DERIVATION. Write the body's own inertia about its centre of mass as `I_c`, and its
/// centre of mass at `c` relative to the reference. Then the inertia about the
/// reference is the parallel-axis theorem:
///
/// I = I_c + m·(cᵀc·1 − c·cᵀ)
///
/// Translating the body by `d` sends `c → c + d`. Expanding and subtracting the
/// original leaves exactly two groups, which is how the code below is written:
///
/// ΔI = m·(dᵀd·1 − d·dᵀ) ← the pure point-mass term
/// + (2h·d)·1 − (h·dᵀ + d·hᵀ) ← cross terms, using h = m·c
///
/// The cross terms vanish when `h` is zero, which is the common case: a body's own
/// inertia is stored about its own centre of mass. The general form is kept anyway —
/// a COMPOSITE inertia carries a nonzero `h`, and silently being wrong for it in
/// phase 2 would be a nasty trap.
pub fn translate(self: Inertia, offset: Vec) Inertia {
const m: f32 = self.mass;
const d: Vec = offset;
const h: Vec = self.h;
// m·(dᵀd·1 − d·dᵀ), the pure point-mass term.
const dd: f32 = dot3(d, d);
var diag: Vec = vec(
m * (dd - d[0] * d[0]),
m * (dd - d[1] * d[1]),
m * (dd - d[2] * d[2]),
);
var off: Vec = vec(
-m * d[0] * d[1],
-m * d[0] * d[2],
-m * d[1] * d[2],
);
// Cross terms: (h·d)·1 − ½(h·dᵀ + d·hᵀ), symmetric by construction.
const hd: f32 = dot3(h, d);
diag += vec(
2.0 * (hd - h[0] * d[0]),
2.0 * (hd - h[1] * d[1]),
2.0 * (hd - h[2] * d[2]),
);
off += vec(
-(h[0] * d[1] + d[0] * h[1]),
-(h[0] * d[2] + d[0] * h[2]),
-(h[1] * d[2] + d[1] * h[2]),
);
return .{
.diag = self.diag + diag,
.off = self.off + off,
.h = h + d * splat(m),
.mass = m,
};
}The rewrite was prompted by readability rather than by a failing test, which is a sufficient reason on its own for something called from the middle of a tree pass in three later phases.
Then it got pinned twice: once against the textbook answer, once against an invariant that keeps working even if the internals change.
test "spatial algebra: translate is the parallel-axis theorem" {
// A point mass at the origin, moved so the reference point sits 2 m away on X. The
// textbook answer is m·d² about the two axes perpendicular to the offset, and zero
// about the offset axis itself.
const mass: f32 = 3.0;
const point: Inertia = .{ .mass = mass };
const moved: Inertia = point.translate(vec(2, 0, 0));
try expectApproxEqAbs(@as(f32, 0), moved.diag[0], 1.0e-5);
try expectApproxEqAbs(mass * 4.0, moved.diag[1], 1.0e-5);
try expectApproxEqAbs(mass * 4.0, moved.diag[2], 1.0e-5);
try expectApproxEqAbs(mass * 2.0, moved.h[0], 1.0e-5);
// Translating out and back must land exactly where it started, whatever the offset.
const body: Inertia = .{
.diag = vec(1.0, 2.0, 3.0),
.off = vec(0.1, -0.2, 0.05),
.h = vec_zero,
.mass = 5.0,
};
const there: Inertia = body.translate(vec(0.3, -1.2, 0.7));
const back: Inertia = there.translate(vec(-0.3, 1.2, -0.7));
inline for (0..3) |k| {
try expectApproxEqAbs(body.diag[k], back.diag[k], 1.0e-4);
...
}
}A robot is described with designated struct literals, like everything else in zimr. Start with the four kinds of freedom, which are the only four:
/// The four kinds of degree of freedom, and the only four. A body with no joint is welded
/// to its parent, which is free and exact — no constraint needed.
pub const JointType = enum {
/// 7 position coords (xyz + quat), 6 velocity coords. Only on a child of the world.
free,
/// 4 position coords (quat), 3 velocity coords. Orientation relative to the parent.
ball,
/// Translation along a body-fixed axis. 1 and 1.
slide,
/// Rotation about a body-fixed axis. 1 and 1.
hinge,
/// How many `qpos` entries this joint occupies. Larger than `dofCount` for anything
/// carrying a quaternion — the reason `nq != nv` in most models.
pub fn posCount(self: JointType) u32 {
return switch (self) {
.free => 7,
.ball => 4,
.slide, .hinge => 1,
};
}
/// How many `qvel` entries this joint occupies.
pub fn dofCount(self: JointType) u32 {
return switch (self) {
.free => 6,
.ball => 3,
.slide, .hinge => 1,
};
}
};Those two functions are where nq ≠ nv comes from, and every awkward thing about position coordinates in §7 traces back to them.
/// Travel limits `[lo, hi]`, in the joint's own units. `null` means unlimited.
range: ?[2]f32 = null,
/// How the limit pushes back, and how hard. Only consulted when `range` is set.
limit_softness: Softness = .{},
limit_impedance: Impedance = .{},
/// Distance from the limit at which the constraint activates. Zero means "exactly at
/// the limit"; a positive margin engages the constraint early, which lets a soft limit
/// decelerate rather than catch.
limit_margin: f32 = 0,range is stored but not yet enforced — limits are phase 6. It is here anyway because adding a field later means touching every index table, and because otherwise every example model gets rewritten when phase 6 lands.
armature deserves its comment. It is genuinely physical (rotor inertia through a gearbox) and it is also the numerical lever: it adds to the diagonal of the mass matrix, which is exactly what regularizes an ill-conditioned factorization. Defaulting it to zero would be a trap, and defaulting it to a constant would be wrong at every scale but one — so null means "a small fraction of this DOF's own inertia", computed at build.
pub const BodySpec = struct {
name: []const u8,
parent: ?[]const u8 = null,
/// Pose relative to the parent's frame.
pos: Vec = vec_zero,
rot: Quat = quat_identity,
/// Several joints on one body is legal and useful — three hinges give a ball joint
/// with per-axis limits, which real robot models do use.
joints: []const JointSpec = &.{},
geoms: []const GeomSpec = &.{},
sites: []const SiteSpec = &.{},
/// Mass properties stated directly. When `null`, they are derived from the geoms.
inertial: ?InertialSpec = null,
};pos/rot at all — a kinematic tree with no relative transforms — and had joint singular, which would have made a ball-with-limits unrepresentable and cost a rewrite of every index table to fix.
Sites are the other thing that re-read caught. Nothing consumes them until phase 5, but they are declared now for the same reason:
/// A massless, collisionless frame on a body. Sites are where actuators push, where
/// tendons route, where sensors sit, and what Jacobians target. Nothing consumes them
/// until phase 5, but retrofitting them would touch every index table.
pub const SiteSpec = struct {
name: []const u8,
pos: Vec = vec_zero,
rot: Quat = quat_identity,
};This is the double pendulum from the tests — the same model the MuJoCo oracle generates reference values for, and the one phase 3 will draw:
const DoublePendulum = Spec(.{
.bodies = &.{
.{
.name = "upper",
.pos = vec(0, 0, 0),
.joints = &.{.{ .name = "shoulder", .kind = .hinge, .axis = vec(0, 0, 1) }},
.geoms = &.{.{
.shape = .{ .capsule = .{ .half_height = 0.25, .radius = 0.05 } },
.pos = vec(0, -0.25, 0),
}},
},
.{
.name = "lower",
.parent = "upper",
.pos = vec(0, -0.5, 0),
.joints = &.{.{ .name = "elbow", .kind = .hinge, .axis = vec(0, 0, 1) }},
.geoms = &.{.{
.shape = .{ .capsule = .{ .half_height = 0.2, .radius = 0.04 } },
.pos = vec(0, -0.2, 0),
}},
},
},
});Capsules hang along −Y, because zimr is Y-up. The identical model in MuJoCo hangs along −Z. That one difference is the whole Y-up decision, visible.
The plan's decision was a hybrid: Model is an ordinary runtime struct, so a URDF importer can build one; on top sits a comptime layer that validates the spec and generates name enums. Spec() is that layer, and it does exactly three things — validate, name, count. It deliberately does not build the index tables, because those must also be reachable from a runtime-loaded model.
/// Turn a list of comptime names into an exhaustive enum whose tag values are the index
/// into that list. `@backingInt(E.foo)` is therefore the array index, for free.
fn NamesEnum(comptime names: []const []const u8) type {
if (names.len == 0) {
// An enum with no fields is legal but useless; callers guard on the count instead.
return u32;
}
var values: [names.len]u32 = undefined;
for (&values, 0..) |*v, i| {
v.* = @intCast(i);
}
const frozen = values;
return @Enum(u32, .exhaustive, names, &frozen);
}@Enum(TagType, .exhaustive, names, values) is the Zig 1676 spelling, which changes often enough to check rather than assume, since the enum-construction builtin has moved more than once. Because the values are assigned in spec order, @backingInt(Arm.Joint.elbow) is the joint index. Enum lookup costs nothing at runtime; it is the integer, with a name attached and a compiler checking it.
const frozen = values; line is not noise. Returning &values would take the address of a comptime var that is about to go out of scope. Copying to a const first freezes it into the comptime result. The same pattern appears in every name-collecting block below.
This is the payoff for the comptime layer. Each check exists because the runtime symptom would have been much worse than the message:
comptime {
if (spec.bodies.len == 0) {
@compileError("robot: a model needs at least one body");
}
if (spec.options.timestep <= 0.0) {
@compileError("robot: timestep must be positive");
}
var body_names: [spec.bodies.len][]const u8 = undefined;
for (spec.bodies, 0..) |b, i| {
body_names[i] = b.name;
}
assertUniqueNames(&body_names, "body");
for (spec.bodies, 0..) |b, bi| {
// A parent must exist AND be declared earlier — the tree passes rely on it.
if (b.parent) |parent| {
var found: bool = false;
for (spec.bodies[0..bi]) |earlier| {
if (std.mem.eql(u8, earlier.name, parent)) {
found = true;
break;
}
}
if (!found) {
@compileError("robot: body '" ++ b.name ++ "' names parent '" ++ parent ++
"', which is not a body declared before it (bodies must be parent-first)");
}
}Notice the loop bound: spec.bodies[0..bi]. It checks that the parent exists and that it was declared earlier, in one pass, because "declared earlier" is the property the algorithms actually need.
for (b.joints) |j| {
// A free joint IS the six degrees of freedom of a floating base. Putting
// one deeper in the tree, or beside another joint, is always a modelling
// mistake rather than an exotic mechanism.
if (j.kind == .free) {
if (b.parent != null) {
@compileError("robot: free joint '" ++ j.name ++ "' on body '" ++ b.name ++
"' — a free joint only belongs on a direct child of the world");
}
if (b.joints.len != 1) {
@compileError("robot: body '" ++ b.name ++
"' has a free joint alongside other joints; a free joint is already all six DOFs");
}
}
if (j.kind == .slide or j.kind == .hinge) {
const len_sq: f32 = j.axis[0] * j.axis[0] + j.axis[1] * j.axis[1] + j.axis[2] * j.axis[2];
if (len_sq < 1.0e-12) {
@compileError("robot: joint '" ++ j.name ++ "' has a zero-length axis");
}
}
if (j.range) |r| {
if (r[0] > r[1]) {
@compileError("robot: joint '" ++ j.name ++ "' has range lo > hi");
}
// A ball or free joint's limit is a CONE on a rotation, not an interval
// on a scalar. MuJoCo supports it by converting the quaternion to
// axis-angle; we do not, and silently ignoring a limit the model asked
// for is worse than refusing it.
if (j.kind != .hinge and j.kind != .slide) {
@compileError("robot: joint '" ++ j.name ++
"' is a ball or free joint with a range — limits are only " ++
"supported on hinge and slide joints");
}
if (j.limit_margin < 0.0) {
@compileError("robot: joint '" ++ j.name ++ "' has a negative limit margin");
}
// MuJoCo overloads the SIGN of solref to mean "these are K and B
// directly". `Softness` names its fields, so a negative time constant
// is simply nonsense rather than a second meaning.
if (j.limit_softness.time_const_s <= 0.0 or j.limit_softness.damp_ratio <= 0.0) {
@compileError("robot: joint '" ++ j.name ++
"' has a non-positive limit time constant or damping ratio");
}
const imp = j.limit_impedance;
if (imp.min <= 0.0 or imp.max >= 1.0 or imp.min > imp.max) {
@compileError("robot: joint '" ++ j.name ++
"' needs 0 < impedance.min <= impedance.max < 1");
}
if (imp.width < 0.0 or imp.power < 1.0 or
imp.midpoint <= 0.0 or imp.midpoint >= 1.0)
{
@compileError("robot: joint '" ++ j.name ++
"' has an impedance ramp that is not a valid sigmoid " ++
"(need width >= 0, power >= 1, 0 < midpoint < 1)");
}
}
if (j.armature) |a| {
if (a < 0.0) {
@compileError("robot: joint '" ++ j.name ++ "' has negative armature");
}
}
}And the one that matters most:
// A body that moves must have inertia, or the mass matrix is singular and the
// factorization divides by zero. Catch it here with the body's name rather
// than as a NaN three phases downstream.
if (b.inertial) |inertial| {
// ★ A zero mass is legal on a body with NO degrees of freedom, and real
// models rely on it: a KUKA iiwa's `lbr_iiwa_link_0` is the bolted-down
// base and declares `mass="0.0"`. Nothing accelerates it, so nothing
// divides by it.
//
// With joints it is fatal — the mass matrix is singular and `factorM`
// divides by zero — which is the same rule as the geom check below, and
// for the same reason.
if (b.joints.len > 0 and inertial.mass <= 0.0) {
@compileError("robot: body '" ++ b.name ++
"' has joints but a non-positive stated mass — the mass matrix " ++
"would be singular");
}
if (inertial.mass < 0.0) {
@compileError("robot: body '" ++ b.name ++ "' has a negative stated mass");
}
// The diagonal must be positive and satisfy the triangle inequality, or the
// tensor is not the inertia of any real object and `factorM` will fail
// somewhere far from here with a much less helpful message.
const ixx: f32 = inertial.full_inertia[0];
const iyy: f32 = inertial.full_inertia[1];
const izz: f32 = inertial.full_inertia[2];
// A massless jointless body may legitimately state a zero tensor; only a
// body that can actually move needs positive moments.
if (b.joints.len > 0 and (ixx <= 0.0 or iyy <= 0.0 or izz <= 0.0)) {
@compileError("robot: body '" ++ b.name ++
"' has joints and a non-positive principal moment in its stated inertia");
}
if (ixx < 0.0 or iyy < 0.0 or izz < 0.0) {
@compileError("robot: body '" ++ b.name ++ "' has a negative principal moment");
}
if (ixx + iyy < izz or ixx + izz < iyy or iyy + izz < ixx) {
@compileError("robot: body '" ++ b.name ++
"' has a stated inertia violating the triangle inequality — no rigid " ++
"body has those moments");
}
} else if (b.joints.len > 0 and b.geoms.len == 0) {
@compileError("robot: body '" ++ b.name ++
"' has joints but no geoms, so it has no mass — the mass matrix would be singular");
}A massless moving body produces a singular M. Without this check the failure is a NaN appearing during factorization in phase 2, with nothing pointing at the model. With it, the message names the body and the reason.
const Counts = struct { nq: u32, nv: u32, njnt: u32, nsite: u32, ngeom: u32, na: u32 };
const counts: Counts = comptime blk: {
var nq: u32 = 0;
var nv: u32 = 0;
var njnt: u32 = 0;
var nsite: u32 = 0;
var ngeom: u32 = 0;
var na: u32 = 0;
for (spec.actuators) |act| {
if (act.activation != .none) {
na += 1;
}
}
for (spec.bodies) |b| {
for (b.joints) |j| {
nq += j.kind.posCount();
nv += j.kind.dofCount();
njnt += 1;
}
nsite += @intCast(b.sites.len);
ngeom += @intCast(b.geoms.len);
}
break :blk Counts{ .nq = nq, .nv = nv, .njnt = njnt, .nsite = nsite, .ngeom = ngeom, .na = na };
};
const joint_names: [counts.njnt][]const u8 = comptime blk: {
var names: [counts.njnt][]const u8 = undefined;
var n: usize = 0;
for (spec.bodies) |b| {
for (b.joints) |j| {
names[n] = j.name;
n += 1;
}
}
const frozen = names;
break :blk frozen;
};
comptime assertUniqueNames(&joint_names, "joint");
const site_names: [counts.nsite][]const u8 = comptime blk: {
var names: [counts.nsite][]const u8 = undefined;
var n: usize = 0;
for (spec.bodies) |b| {
for (b.sites) |s| {
names[n] = s.name;
n += 1;
}
}
const frozen = names;
break :blk frozen;
};
comptime assertUniqueNames(&site_names, "site");The named Counts struct exists because zimr's untyped-local rule wanted a type annotation on counts, and an anonymous struct literal has no name to write. The rule pushed toward a named type, which reads better than the anonymous version did.
return struct {
/// The spec this was built from, kept so `build` needs no arguments beyond an
/// allocator and so tooling can introspect a model type.
pub const model_spec: ModelSpec = spec;
/// Body 0 is the WORLD, so a spec body's index is its position in the list plus
/// one. These enums number the spec bodies from zero; add `world_body` when
/// indexing `Model`'s arrays.
pub const Body = NamesEnum(&body_names);
pub const Joint = NamesEnum(&joint_names);
pub const Site = NamesEnum(&site_names);
pub const nbody: u32 = @as(u32, spec.bodies.len) + 1; // +1 for the world
pub const njnt: u32 = counts.njnt;
...
/// Build the runtime model. The spec is consumed here and not referenced again.
pub fn build(gpa: Allocator) !Model {
return buildFromSpec(gpa, spec);
}
};Which makes the whole API this:
const Arm = robot.Spec(.{ .bodies = &.{ ... } }); // comptime: validated, named
var model: robot.Model = try Arm.build(gpa); // runtime: ordinary struct
var data: robot.Data = try robot.Data.init(gpa, &model);
data.setJointPos(&model, Arm.Joint.elbow, 0.75); // compile-checkedModel is flat parallel arrays indexed by body, joint or DOF — exactly mjModel's shape. Not a tree of structs, because every algorithm here is a linear sweep over one of those index spaces, and because a flat model is what a batched GPU rollout will want.
/// The index of the world body. Its parent is itself.
pub const world_body: u32 = 0;Body 0 is a real body — massless, jointless, at the origin — rather than a special case. That means "parent" is never optional in the hot paths, and a walk up the tree terminates naturally because the world is its own parent.
// ---- degrees of freedom ----
/// The DOF immediately above this one in the tree, or `no_dof` at a root. This chain
/// IS the sparsity pattern of the mass matrix: `M[i][j]` is nonzero exactly when `j`
/// is `i` or one of its ancestors.
dof_parent: []u32,
dof_body: []u32,
dof_jnt: []u32,
dof_armature: []f32,
// ---- mass-matrix sparsity (structure, so it lives with the model) ----
/// Nonzeros in row `i`: the length of `i`'s ancestor chain, including `i` itself.
mass_row_nonzeros: []u32,
/// Where row `i` starts in the packed value array.
mass_row_start: []u32,
/// Column index of each packed entry. Within a row the order is root-first and the
/// DIAGONAL IS LAST, which is what lets CRB walk the chain with a single decrement.
mass_col_index: []u32,This is the most load-bearing idea in Model. The mass matrix of a kinematic tree is sparse, and its sparsity comes from the ancestor relation in the tree rather than from the state, so it is fixed at build time. Two DOFs interact in M exactly when one is an ancestor of the other.
Building it is a pair of chain walks:
// ---- mass-matrix sparsity ----
// Row i holds one entry per DOF on i's path to the root, i included. Storing the
// chain root-first with the diagonal last is what lets `crb` walk it backwards with a
// single decrementing cursor.
var total: u32 = 0;
for (0..nv) |i| {
var n: u32 = 0;
var j: u32 = @intCast(i);
while (j != no_dof) : (j = m.dof_parent[j]) {
n += 1;
}
m.mass_row_nonzeros[i] = n;
m.mass_row_start[i] = total;
total += n;
}
m.mass_nonzero_count = total;
m.mass_col_index = try a.alloc(u32, total);
for (0..nv) |i| {
const adr: u32 = m.mass_row_start[i];
var slot: u32 = m.mass_row_nonzeros[i]; // fill backwards: diagonal lands last
var j: u32 = @intCast(i);
while (j != no_dof) : (j = m.dof_parent[j]) {
slot -= 1;
m.mass_col_index[adr + slot] = j;
}
}Filling backwards is the trick. Walking up from i visits the diagonal first, but we want it stored last, so the cursor starts at the end of the row and decrements. Phase 2's CRB will do the same walk with the same decrement, which is why the storage order was chosen to match.
/// The last DOF belonging to `body`, walking up until a body with DOFs is found. That is
/// the DOF a child's first DOF chains onto — welded bodies are transparent here, which is
/// exactly what makes a weld free.
fn lastDofOf(m: *const Model, body: u32) u32 {
var b: u32 = body;
while (true) {
if (m.body_dof_num[b] > 0) {
return m.body_dof_adr[b] + m.body_dof_num[b] - 1;
}
if (b == world_body) {
return no_dof;
}
b = m.body_parent[b];
}
}A body with no joints is rigidly part of its parent. In a maximal-coordinate engine that costs a weld constraint the solver has to enforce every step. Here it costs nothing: the DOF chain simply skips it. That is the thesis of the whole file, in eight lines, and it has a test:
test "welded bodies are transparent to the dof chain" {
// The middle body has no joint, so it is rigidly part of its parent. Its child's DOF
// must chain onto the FIRST body's DOF, skipping the weld entirely.
...
try expectEqual(@as(u32, 2), m.nv); // the weld adds no freedom
try expectEqual(@as(u32, 0), m.dof_parent[1]);
} const ndof: u32 = j.kind.dofCount();
for (0..ndof) |k| {
const d: u32 = dof_n + @as(u32, @intCast(k));
m.dof_body[d] = bi;
m.dof_jnt[d] = jnt_n;
m.dof_parent[d] = prev_dof;
prev_dof = d;
// Armature: an explicit value, or a small fraction of the body's own
// inertia so the default scales with the model rather than the units.
m.dof_armature[d] = j.armature orelse
default_armature_fraction * representativeInertia(mass_props, j.kind);
}Every DOF on a body chains onto the previous one — across joints as well as within them. Three hinges on one body are three links in one chain, not three parallel branches off the parent, and the difference is the sparsity pattern of the entire mass matrix.
That is also the answer to "why is ball a joint type at all, when three hinges give you three rotational DOFs?" They are not the same thing. Three sequential hinges have configurations where two axes align and a degree of freedom vanishes — gimbal lock — and at those poses M is genuinely singular. A ball joint has no such pose. The engine will assert rather than factor a singular matrix, which is the correct behaviour: the model is degenerate there, not the solver.
// qpos0: everything rests at zero except a quaternion, which rests at identity.
// zm stores a quat as (x, y, z, w), so the w lane is the one that starts at 1.
switch (j.kind) {
.free => {
// ★★ A FREE JOINT'S REFERENCE POSE IS THE BODY'S DECLARED POSE.
//
// For every other joint the body's `pos`/`rot` is a fixed offset from the
// parent and the joint moves relative to it. A free joint HAS no fixed
// offset — its seven coordinates ARE the body's pose — so leaving `qpos0`
// at zero silently discards wherever the model said the body was.
//
// Found by stacking five crates and watching all five appear at the
// origin on step zero, already interpenetrating. Nothing warned: the
// model was valid, the simulation stable, and every body simply in the
// wrong place. MuJoCo does the same thing — `qpos0` for a free joint is
// seeded from `body_pos`/`body_quat`.
m.qpos0[qpos_n + 0] = b.pos[0];
m.qpos0[qpos_n + 1] = b.pos[1];
m.qpos0[qpos_n + 2] = b.pos[2];
m.qpos0[qpos_n + 3] = b.rot[0];
m.qpos0[qpos_n + 4] = b.rot[1];
m.qpos0[qpos_n + 5] = b.rot[2];
m.qpos0[qpos_n + 6] = b.rot[3];
},
.ball => {
for (0..4) |k| {
m.qpos0[qpos_n + k] = 0;
}
m.qpos0[qpos_n + 3] = 1; // w
},
.slide, .hinge => {
m.qpos0[qpos_n] = j.ref;
},
}qpos0 is the reference configuration, and the adversarial review flagged its absence as the most serious omission in the first draft of the plan. It is not a convenience: reset() restores it, springs measure from it, and in phase 6 the constraint regularizer is scaled by the inertia evaluated at it. Without a reference pose the soft-constraint parameterization has no anchor at all.
The // w comment matters more than it looks. zm stores a quaternion (x, y, z, w); MuJoCo stores (w, x, y, z). Writing 1 into the wrong lane gives a 90° rotation at rest, in a model that otherwise looks fine.
Data is everything that changes. Its fields are grouped by which pipeline stage writes them, and each group says so — because the pipeline is imperative, and knowing what is stale is part of using this correctly:
// ---- written by `crb` / `factorM` (phase 2) ----
/// Composite inertia of each body's subtree.
crb: []Inertia,
/// Lower triangle of M, packed by the model's sparsity tables. Length `mass_nonzero_count`.
mass_matrix: []f32,
/// The LᵀDL factorization, same packing.
qLD: []f32,
/// Reciprocals of D, so the solve multiplies instead of dividing.
qLDiagInv: []f32,The whole inventory is declared in phase 0, even though phases 1–3 fill it. Growing Data ad hoc would make it the one thing phase G has to serialize and the one thing nobody has a complete picture of.
Both Model and Data carve their arrays from an arena, so deinit is one call instead of a forty-line errdefer ladder. The obvious way to write that does not work:
// WRONG — this leaks every allocation.
arena: std.heap.ArenaAllocator,
fn buildFromSpec(gpa: Allocator, comptime spec: ModelSpec) !Model {
var arena: std.heap.ArenaAllocator = .init(gpa);
const a: Allocator = arena.allocator();
...
return m; // ← m.arena is a COPY; `a` still points at the local
}An ArenaAllocator is not movable once an Allocator has been taken from it, because that allocator stores the arena struct's address. Copying the struct into the returned Model leaves every allocation registered with a stack object that is about to vanish, and deinit on the copy frees nothing. It compiles, it runs, and it leaks everything.
/// Owns every table below, so `deinit` is one call rather than a forty-line errdefer
/// ladder.
///
/// The pointer is not decoration: an `ArenaAllocator` is NOT movable once an
/// `Allocator` has been taken from it, because that allocator holds the arena
/// struct's ADDRESS. Storing it by value and returning the model would strand every
/// allocation in a copy nobody frees. (zimr has been bitten by this shape before —
/// see the wgpu_bringup use-after-scope note in claude.md.)
arena: *std.heap.ArenaAllocator, pub fn deinit(self: *Model) void {
const gpa: Allocator = self.arena.child_allocator;
self.arena.deinit();
gpa.destroy(self.arena);
self.* = undefined;
}The general rule: any struct that hands out pointers to itself cannot be returned by value.
/// Accept either a generated enum or a plain index, so a comptime-specified model and a
/// runtime-loaded one share the same API. The enum's tag value IS the index, by
/// construction in `NamesEnum`.
fn jointIndex(joint: anytype) u32 {
const T = @TypeOf(joint);
return switch (@typeInfo(T)) {
.@"enum" => @backingInt(joint),
else => @intCast(joint),
};
}This is the hybrid decision made concrete. A hand-written model gets Arm.Joint.elbow and a compile error if it misspells it; a URDF-loaded model passes an integer; both call the same accessor. Neither pays anything at runtime.
/// Read a scalar joint's position. Only valid for hinge and slide — a ball or free
/// joint has no single number, and asking for one is a programming error.
pub fn jointPos(self: *const Data, m: *const Model, joint: anytype) f32 {
const ji: u32 = jointIndex(joint);
assertf(
m.jnt_type[ji] == .hinge or m.jnt_type[ji] == .slide,
@src(),
"jointPos on a {s} joint, which has {d} position coordinates",
.{ @tagName(m.jnt_type[ji]), m.jnt_type[ji].posCount() },
);
return self.pos[m.jnt_qpos_adr[ji]];
}The assert carries the joint's actual type and coordinate count, so the message tells you what you did instead of that you did something.
pos and vel have different lengths and different geometry. Orientations live on the unit sphere in 4D; velocities live in its 3D tangent space. You cannot subtract two positions to get a velocity, and you cannot add a velocity to a position.Three functions are the only legal bridges, and everything else must go through them. Integration first:
/// Advance `pos` by `vel` over `dt`, respecting the geometry of each joint type.
pub fn integratePos(m: *const Model, pos: []f32, vel: []const f32, dt: f32) void {
assertf(pos.len == m.nq, @src(), "pos has {d} entries, model wants {d}", .{ pos.len, m.nq });
assertf(vel.len == m.nv, @src(), "vel has {d} entries, model wants {d}", .{ vel.len, m.nv });
for (0..m.njnt) |ji| {
const qadr: u32 = m.jnt_qpos_adr[ji];
const vadr: u32 = m.jnt_dof_adr[ji];
switch (m.jnt_type[ji]) {
.slide, .hinge => {
pos[qadr] += dt * vel[vadr];
},
.free => {
// Translation is ordinary integration; rotation is not.
inline for (0..3) |k| {
pos[qadr + k] += dt * vel[vadr + k];
}
const w: Vec = vec(vel[vadr + 3], vel[vadr + 4], vel[vadr + 5]);
const q: Quat = .{ pos[qadr + 3], pos[qadr + 4], pos[qadr + 5], pos[qadr + 6] };
const rotated: Quat = rotateQuatByAngularVel(q, w, dt);
inline for (0..4) |k| {
pos[qadr + 3 + k] = rotated[k];
}
},The inline for is required, not stylistic: Vec is a @Vector(4, f32) and indexing one needs a comptime-known lane. A plain for compiles everywhere else in the loop and fails exactly here, which is a small, sharp lesson about where SIMD types leak into control flow.
/// Rotate `q` by an angular velocity applied for `dt`. Exact for a constant `w`: build the
/// rotation the velocity describes over the interval, then compose.
fn rotateQuatByAngularVel(q: Quat, w: Vec, dt: f32) Quat {
const speed: f32 = length3(w);
if (speed < 1.0e-12) {
return q;
}
const axis: Vec = w / splat(speed);
const delta: Quat = zm.quatFromNormAxisAngle(axis, speed * dt);
// The velocity is in the joint's own frame, so the increment applies on the right.
return normalizeQuat(qmul(q, delta));
}This is exact for a constant angular velocity, not a first-order approximation — it builds the actual rotation of angle |w|·dt about w. The common alternative (q += ½·ω·q·dt) is cheaper and drifts off the unit sphere far faster.
The side the increment applies on decides the meaning: qmul(q, delta) composes in the joint's own frame, qmul(delta, q) would compose in the parent's. Both compile. One is right.
/// The angular velocity that rotates `from` into `to` over `1/inv_dt` seconds.
fn angularVelBetween(from: Quat, to: Quat, inv_dt: f32) Vec {
// Relative rotation in `from`'s frame, matching the composition order above.
const inv_from: Quat = .{ -from[0], -from[1], -from[2], from[3] };
var rel: Quat = qmul(inv_from, to);
// q and -q are the same rotation; pick the short way round so the velocity is minimal
// rather than taking the long path over the double cover.
if (rel[3] < 0.0) {
rel = -rel;
}
const sin_half: f32 = @sqrt(rel[0] * rel[0] + rel[1] * rel[1] + rel[2] * rel[2]);
if (sin_half < 1.0e-12) {
return vec_zero;
}
const angle: f32 = 2.0 * atan2(sin_half, rel[3]);
const axis: Vec = vec(rel[0], rel[1], rel[2]) / splat(sin_half);
return axis * splat(angle * inv_dt);
}The if (rel[3] < 0.0) flip is the double cover showing up in practice: q and −q name the same rotation, so without the flip a small rotation can be reported as a 350° one in the opposite direction. Everything still runs; the velocities are just occasionally enormous and backwards.
Note also atan2(sin_half, rel[3]) rather than 2·acos(rel[3]). Both give the angle; atan2 keeps its precision as the angle approaches zero, which is the case that occurs on literally every timestep.
/// Renormalize every quaternion in `pos`. Integration walks a quaternion off the unit
/// sphere a little each step, and the error compounds, so this runs after every advance.
pub fn normalizeQuats(m: *const Model, pos: []f32) void {
for (0..m.njnt) |ji| {
const qadr: u32 = m.jnt_qpos_adr[ji];
const base: u32 = switch (m.jnt_type[ji]) {
.free => qadr + 3,
.ball => qadr,
.slide, .hinge => continue,
};
const q: Quat = .{ pos[base], pos[base + 1], pos[base + 2], pos[base + 3] };
const fixed: Quat = normalizeQuat(q);
inline for (0..4) |k| {
pos[base + k] = fixed[k];
}
}
}The continue inside a switch that is initialising a const is a small Zig pleasure: scalar joints have no quaternion, and skipping them is expressed as part of working out where the quaternion is.
The normalization itself moved into a shared helper once phase 1 needed the same fallback a third time. Three copies of "renormalize, or return identity if it has collapsed" is three chances to get the collapse threshold subtly different:
/// Normalize a quaternion, falling back to identity if it has collapsed. Integration and
/// user input both walk quaternions off the unit sphere, and a zero-norm quaternion has no
/// orientation to recover — identity keeps the simulation running instead of emitting NaN.
fn normalizeQuat(q: Quat) Quat {
const n: f32 = @sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
return if (n > 1.0e-9) q / splat(n) else quat_identity;
}This subsystem is pure CPU maths with no device dependency, which means everything here is testable headlessly. The tests lean on invariants rather than golden numbers wherever possible, because an invariant keeps testing when the numbers legitimately change.
The round trip, which validates both halves of §7 at once:
test "position coordinates: integrate then differentiate is a round trip" {
// A free joint exercises both halves: ordinary translation and the quaternion path.
...
const dt: f32 = 0.01;
const wanted = [_]f32{ 0.5, -1.25, 2.0, 0.3, -0.7, 1.1 };
@memcpy(d.vel, &wanted);
var before: [7]f32 = undefined;
@memcpy(&before, d.pos);
integratePos(&m, d.pos, d.vel, dt);
var recovered: [6]f32 = undefined;
differentiatePos(&m, &recovered, &before, d.pos, dt);
for (wanted, recovered) |w, r| {
try expectApproxEqAbs(w, r, 1.0e-4);
}
}And the drift test, which runs long enough to matter:
test "position coordinates: quaternions stay on the unit sphere" {
...
// Spin hard, for a long time, on a deliberately awkward axis.
d.vel[0] = 7.0;
d.vel[1] = -3.5;
d.vel[2] = 11.25;
for (0..100_000) |_| {
integratePos(&m, d.pos, d.vel, 1.0 / 240.0);
normalizeQuats(&m, d.pos);
}
const n: f32 = @sqrt(d.pos[0] * d.pos[0] + d.pos[1] * d.pos[1] +
d.pos[2] * d.pos[2] + d.pos[3] * d.pos[3]);
try expectApproxEqAbs(@as(f32, 1), n, 1.0e-5);
}100,000 steps on a non-axis-aligned spin. A first-order integrator without renormalization fails this comfortably; the exact-rotation approach plus renormalization holds to 1e-5 in f32.
One place the code stops short, and says so:
fn geomsToBodyMass(geoms: []const GeomSpec) BodyMass {
var total: Inertia = .zero;
for (geoms) |g| {
const gm: f32 = g.mass orelse (shapeVolume(g.shape) * g.density);
const principal: Inertia = .{
.diag = shapeMoments(g.shape, gm),
.mass = gm,
};
total = total.add(principal.rotated(g.rot).translate(g.pos));
}
if (total.mass <= min_mass) {
return .{ .mass = 0, .com = vec_zero, .inertia = .zero };
}
// `h` is `mass · com`, so the centre of mass falls out of the sum for free.
const com: Vec = total.h / splat(total.mass);
// Shift from the body origin back to the centre of mass: the inverse of step 3.
return .{ .mass = total.mass, .com = com, .inertia = total.translate(-com) };
}Summing rotated geoms into one body inertia is where the choice of representation pays off. MuJoCo stores body_inertia as three principal moments plus a quaternion, which is compact but means combining two rotated inertias requires a symmetric-3×3 eigen-decomposition to get back to principal axes.
Inertia here keeps six numbers instead — diag and off, the full symmetric tensor. rotated() then computes I′ = R·I·RT directly, and the sum above is exact for arbitrarily rotated geoms with no decomposition anywhere. The same choice is what makes a composite inertia a plain sum, which is what makes CRB cheap (§12b).
The general point: a representation chosen to make one operation trivial often makes several others trivial too, because the operations are related. Principal axes are the natural form for storing an inertia and the wrong form for combining them.
src/robot.zig, and zig build doc-sync verifies that on every build. The prose between the blocks is not checked by anything. Where this page and the code disagree, the code is right.One forward pass over the tree, parent before child, turning generalized positions into world poses. It can be a flat loop rather than a recursive walk for one reason: Spec() rejects any model where a parent is declared after its child, so index order is topological order. A validation rule in §4 buys a simpler algorithm here.
First, the type the whole pass is written in. A pose is a real concept here rather than an implementation detail — bodies, joints and sites all have one, and composing them is the operation the tree walk is made of:
/// A rigid placement in some frame. Named rather than anonymous because a pose is a real
/// concept here — bodies, joints and sites all have one, and `compose` is how they relate.
pub const Pose = struct {
pos: Vec = vec_zero,
rot: Quat = quat_identity,
pub const identity: Pose = .{};
/// Place `local` inside `self`. Rotation composes; translation is this pose's origin
/// plus the child's offset rotated into this frame.
///
/// The order is the whole content of the function: `qmul(a, b)` applies `b` FIRST,
/// so `parent.compose(local)` reads "start at the parent, then go local", which is
/// the direction a tree walk travels. Swapping the operands compiles and puts every
/// body in the wrong place in a way that still looks like a robot.
pub fn compose(self: Pose, local: Pose) Pose {
return .{
.pos = self.pos + rotate(self.rot, local.pos),
.rot = qmul(self.rot, local.rot),
};
}
};The doc comment on compose earns its place. qmul(a, b) applies b first, so parent.compose(local) reads "start at the parent, then go local" — the direction a tree walk actually travels. Swap the operands and it compiles, runs, and puts every body in the wrong place in a way that still looks like a robot.
/// Forward kinematics: world pose of every body, joint anchor, joint axis and site.
///
/// Bodies are visited in index order, which IS parent-before-child because `Spec()`
/// rejects any spec where a parent is declared after its child. That single validation
/// rule is what lets this be a flat loop instead of a recursive walk.
///
/// The subtle step is the off-centre correction. A hinge does not rotate the body about
/// the body's own origin — it rotates it about the ANCHOR. So after composing the
/// rotation we recompute where the origin must be for the anchor to have stayed put:
///
/// xpos = xanchor − R_new · jnt_pos
///
/// Skip it and every joint whose anchor is not at the body origin swings the body through
/// an arc it should never take. Almost every real robot joint is off-centre, so this is
/// not an edge case.
pub fn kinematics(m: *const Model, d: *Data) void {
// The world never moves.
d.body_xpos[world_body] = vec_zero;
d.body_xrot[world_body] = quat_identity;
d.body_xipos[world_body] = vec_zero;
for (1..m.nbody) |bi| {
const parent: u32 = m.body_parent[bi];
const jnt_adr: u32 = m.body_jnt_adr[bi];
const jnt_num: u32 = m.body_jnt_num[bi];
var pos: Vec = undefined;
var rot: Quat = undefined;
if (jnt_num == 1 and m.jnt_type[jnt_adr] == .free) {
// A free joint IS the body's world pose; there is nothing to compose.
const qadr: u32 = m.jnt_qpos_adr[jnt_adr];
pos = vec(d.pos[qadr], d.pos[qadr + 1], d.pos[qadr + 2]);
rot = normalizeQuat(.{ d.pos[qadr + 3], d.pos[qadr + 4], d.pos[qadr + 5], d.pos[qadr + 6] });
d.jnt_xanchor[jnt_adr] = pos;
d.jnt_xaxis[jnt_adr] = m.jnt_axis[jnt_adr];
} else {
// Start at the body's rest pose relative to its parent, then let each joint
// move it away from there.
const parent_pose: Pose = .{ .pos = d.body_xpos[parent], .rot = d.body_xrot[parent] };
const rest: Pose = parent_pose.compose(.{ .pos = m.body_pos[bi], .rot = m.body_rot[bi] });
pos = rest.pos;
rot = rest.rot;
for (jnt_adr..jnt_adr + jnt_num) |ji| {
const qadr: u32 = m.jnt_qpos_adr[ji];
// Axis and anchor, in the frame the joint acts in — which is the pose as
// built SO FAR, so several joints on one body compose in declared order.
const axis: Vec = rotate(rot, m.jnt_axis[ji]);
const anchor: Vec = pos + rotate(rot, m.jnt_pos[ji]);
switch (m.jnt_type[ji]) {
.slide => {
// Sliding moves the body and leaves its orientation alone, so no
// off-centre correction applies.
pos += axis * splat(d.pos[qadr] - m.qpos0[qadr]);
},
.hinge, .ball => {
const local: Quat = if (m.jnt_type[ji] == .ball)
normalizeQuat(.{ d.pos[qadr], d.pos[qadr + 1], d.pos[qadr + 2], d.pos[qadr + 3] })
else
zm.quatFromNormAxisAngle(m.jnt_axis[ji], d.pos[qadr] - m.qpos0[qadr]);
rot = qmul(rot, local);
// Off-centre correction: put the origin back where it must be for
// the anchor not to have moved.
pos = anchor - rotate(rot, m.jnt_pos[ji]);
},
.free => unreachable, // handled above; a free joint is never beside others
}
d.jnt_xanchor[ji] = anchor;
d.jnt_xaxis[ji] = axis;
}
}
d.body_xrot[bi] = normalizeQuat(rot);
d.body_xpos[bi] = pos;
// The centre of mass, which is what the dynamics actually care about.
d.body_xipos[bi] = pos + rotate(d.body_xrot[bi], m.body_ipos[bi]);
}
// Sites ride along on their bodies. They are massless frames, so this is the only
// place they cost anything.
for (0..m.nsite) |si| {
const bi: u32 = m.site_body[si];
const body_pose: Pose = .{ .pos = d.body_xpos[bi], .rot = d.body_xrot[bi] };
const world: Pose = body_pose.compose(.{ .pos = m.site_pos[si], .rot = m.site_rot[si] });
d.site_xpos[si] = world.pos;
d.site_xrot[si] = world.rot;
}
d.stage = .position;
}Look at the hinge case:
rot = qmul(rot, local);
// Off-centre correction: put the origin back where it must be for
// the anchor not to have moved.
pos = anchor - rotate(rot, m.jnt_pos[ji]);A hinge does not rotate a body about the body's own origin — it rotates it about the anchor. So after composing the rotation we recompute where the origin must be for the anchor to have stayed put. Drop those two lines and every joint whose anchor is off the body origin swings the body through an arc it should never take. Almost every real robot joint is off-centre, so this is not an edge case, and it is why the test that isolates it exists:
test "kinematics: a hinge rotates about its ANCHOR, not the body origin" {
// The off-centre correction, isolated. A hinge whose anchor sits 1 m out along X,
// turned a quarter turn about Z, must swing the body origin onto the Y axis — if the
// correction were missing the origin would stay put and only the orientation change.Note also that axis and anchor are computed from the pose built so far, not from the body's rest pose. That is what makes several joints on one body compose in declaration order — three hinges behaving as a ball joint with per-axis limits, which is exactly the case §3's joints-as-a-list exists for.
test "kinematics: body positions match MuJoCo" {
// THE test for phase 1. `reference.zig` is generated from real MuJoCo by
// scripts/robot_oracle.py, so this compares our forward kinematics against an
// independent implementation rather than against my own arithmetic.And the loop guards against the worst failure mode a data-driven test has:
// A silently-empty loop would pass this test while checking nothing.
try expect(compared >= 3);A capsule is a cylinder plus two hemispherical caps, and the caps are the subtle part. A hemisphere's transverse moment (2/5)m·r² — the number you will find quoted — is measured about the centre of its flat face. The parallel-axis theorem needs it about the centroid, which sits 3r/8 away. Using 2/5 directly double-counts m·(3r/8)².
// * A hemisphere's transverse moment about the CENTRE OF ITS FLAT FACE is
// (2/5)m·r², the same as a full sphere, by symmetry.
// * Its centroid is NOT there — it sits 3r/8 out along the axis.
// * Parallel axis moves an inertia between a point and the CENTROID, so
// before shifting out to the capsule's centre we must first come back to
// the centroid:
// I_centroid = (2/5)m·r² − m·(3r/8)² = (2/5 − 9/64)·m·r²
// and only then push out to (half_height + 3r/8).What makes this worth flagging is that the axial moment is unaffected — it comes out right either way. Only the side moment moves, by about 0.15%. That is a value no invariant catches: it is positive, plausible, symmetric, and wrong. It is the kind of error only a comparison against an independent implementation finds, which is why the reference fixtures pin every primitive.
Zig caps comptime loop iterations to catch runaway evaluation, and the default of 1000 is reached by a perfectly ordinary humanoid — the validation pass alone is bodies × joints × geoms, and name-uniqueness is quadratic. A model of about eighteen bodies will not compile without raising it.
/// A comptime evaluation budget that scales with the model, rather than a magic number
/// that works until someone builds a slightly larger robot. The quadratic term is real:
/// `assertUniqueNames` compares every name against every later one.
fn quotaFor(comptime spec: ModelSpec) u32 {
var names: u32 = 0;
var parts: u32 = 0;
for (spec.bodies) |b| {
names += 1 + @as(u32, @intCast(b.joints.len + b.sites.len));
parts += @as(u32, @intCast(b.joints.len + b.sites.len + b.geoms.len));
}
const bodies: u32 = @intCast(spec.bodies.len);
// name-uniqueness is quadratic; everything else is linear in the parts.
return 4000 + 8 * (names * names + bodies * bodies) + 64 * (parts + bodies);
}A fixed number would work until someone built a slightly larger robot. Scaling with the spec costs nothing, since the quota is a ceiling rather than an allocation.
The measurement that matters: with the quota in place, a 17-body / 31-DOF humanoid instantiates in about 0.4 s over an empty-file baseline. That is what justifies the "comptime shape, runtime loops" split — the tempting alternative, unrolling the tree passes with inline for, would trade that 0.4 s for far more compile time and a much larger binary, in exchange for loops over tables that are already in cache.
Every code block above is checked against src/robot.zig by zig build doc-sync. A tutorial quoting drifted source teaches a version of the code that does not exist, so the check runs as a build step.
Blocks marked class="sketch" (illustrative) or class="bad" (a deliberately-wrong alternative, shown for contrast) are exempt; everything else must match line for line.
Phase 1's second half, and where the representation chosen in §2 pays off.
/// Build the shared frame each kinematic tree's dynamics are expressed in, and put every
/// body's inertia and every DOF's motion axis into it.
///
/// WHY A SHARED FRAME AT ALL. Two inertias can only be added when they are expressed about
/// the same reference point in the same orientation. `crb` in phase 2 accumulates a
/// subtree's inertia by literally summing the ten numbers of each body — which is legal
/// only because this function first moved them all into one frame. That summation is the
/// entire reason the composite-rigid-body algorithm is cheap, and this is where it is paid
/// for.
///
/// WHICH FRAME. Global ORIENTATION (so collision, which happens in world space, shares it),
/// translated to the root's subtree centre of mass. The translation is purely for floating
/// point: an inertia expressed about a distant origin has its useful bits swamped by the
/// `m·d²` parallel-axis term. MuJoCo does this at f64; we are at f32 and need it more.
///
/// The frame is per TREE, not per body — every body in one tree shares its root's subtree
/// com, which is what makes their inertias summable. `subtree_com` is computed for every
/// body anyway because sensors and the phase-3 `subtreeVel` want it.
pub fn comPos(m: *const Model, d: *Data) void {
d.requireStage(.position, "comPos");
// ---- subtree centres of mass, accumulated leaf-to-root ----
// Start each body holding its own first moment (mass × com), add children into
// parents walking backwards, then divide out the subtree mass. Backwards works
// because a parent always has a lower index than its children.
for (0..m.nbody) |bi| {
d.subtree_com[bi] = d.body_xipos[bi] * splat(m.body_mass[bi]);
}
var bi: u32 = m.nbody;
while (bi > 1) {
bi -= 1;
d.subtree_com[m.body_parent[bi]] += d.subtree_com[bi];
}
for (0..m.nbody) |i| {
const sub_mass: f32 = m.body_subtree_mass[i];
// A massless subtree has no centre of mass to speak of; its own origin is the
// only answer that stays finite, and nothing downstream weights it anyway.
d.subtree_com[i] = if (sub_mass > min_mass)
d.subtree_com[i] / splat(sub_mass)
else
d.body_xipos[i];
}
// ---- body inertias, in the shared frame ----
d.cinert[world_body] = .zero;
for (1..m.nbody) |i| {
const frame_origin: Vec = d.subtree_com[m.body_root[i]];
// Rotate the body-frame tensor into world orientation, then translate from the
// body's centre of mass out to the shared origin. Note the order: rotating a
// translated inertia is not the same as translating a rotated one.
d.cinert[i] = m.body_inertia[i]
.rotated(d.body_xrot[i])
.translate(d.body_xipos[i] - frame_origin);
}
// ---- dof motion axes, in the same frame ----
for (0..m.njnt) |ji| {
const body: u32 = m.jnt_body[ji];
const frame_origin: Vec = d.subtree_com[m.body_root[body]];
// From the frame's origin TO the joint anchor: a rotation about the anchor moves
// the origin by `axis × offset`, which is the linear half of the motion vector.
const offset: Vec = frame_origin - d.jnt_xanchor[ji];
const dof_adr: u32 = m.jnt_dof_adr[ji];
switch (m.jnt_type[ji]) {
.slide => {
// Pure translation: no angular part, and no dependence on the anchor.
d.cdof[dof_adr] = .{ .ang = vec_zero, .lin = d.jnt_xaxis[ji] };
},
.hinge => {
d.cdof[dof_adr] = dofAboutAxis(d.jnt_xaxis[ji], offset);
},
.ball => {
// Three rotations about the body's own axes, taken as the columns of its
// world rotation. Using the CHILD frame matters: a ball joint's velocity
// is defined there, and `integratePos` composes its increment on the same
// side (see `rotateQuatByAngularVel`).
inline for (0..3) |k| {
d.cdof[dof_adr + k] = dofAboutAxis(bodyAxis(d.body_xrot[body], k), offset);
}
},
.free => {
// Translation first: the three world axes, no angular part.
inline for (0..3) |k| {
d.cdof[dof_adr + k] = .{ .ang = vec_zero, .lin = worldAxis(k) };
}
// Then rotation, exactly as for a ball joint.
inline for (0..3) |k| {
d.cdof[dof_adr + 3 + k] = dofAboutAxis(bodyAxis(d.body_xrot[body], k), offset);
}
},
}
}
}The dofAboutAxis helper is three lines and carries the whole intuition for what a motion vector is:
/// The spatial motion a unit rotation about `axis` produces, when the frame's origin sits
/// `offset` from the axis. Rotating about a line that does not pass through the origin
/// moves the origin too, and `axis × offset` is exactly how much.
fn dofAboutAxis(axis: Vec, offset: Vec) Motion {
return .{ .ang = axis, .lin = cross(axis, offset) };
}mjModel stores each body's inertia as three principal moments plus a quaternion — seven numbers, and producing them requires diagonalizing a symmetric 3×3 with a Jacobi iteration. We already store the full symmetric tensor in six numbers, so we can rotate it directly and never diagonalize:
/// Re-express this inertia in a frame rotated by `q`. The rotational block transforms
/// by similarity, `I' = R·I·Rᵀ`, and the first moment just rotates.
///
/// This is the function that lets us skip MuJoCo's principal-axis storage entirely.
/// `mjModel` keeps a body's inertia as three principal moments plus a quaternion —
/// seven numbers, and producing them needs a symmetric-3×3 eigendecomposition. We
/// already store the full symmetric tensor in six, so we can rotate it directly and
/// never diagonalize. Fewer numbers, no Jacobi iteration, no degenerate-eigenvalue
/// edge cases. MuJoCo's form is the better one in C, where a diagonal inertia makes
/// the inner loops cheaper; here the six-float form composes better.
pub fn rotated(self: Inertia, q: Quat) Inertia {
// `I' = R·I·Rᵀ` expands to `I'[a][b] = rowᵃ · (I · rowᵇ)`, so we need the ROWS
// of R — and `rotate(q, e_k)` gives the COLUMNS. The rows of R are the columns of
// Rᵀ, which is the rotation by the conjugate.
//
// This is not pedantry: using the columns computes `Rᵀ·I·R` instead, which has the
// same eigenvalues and a plausible-looking diagonal, so the error hides in the
// off-diagonal terms alone. The oracle comparison caught it; nothing else would
// have, which is the entire argument for having one.
const inv: Quat = conjugate(q);
const rx: Vec = rotate(inv, vec(1, 0, 0));
const ry: Vec = rotate(inv, vec(0, 1, 0));
const rz: Vec = rotate(inv, vec(0, 0, 1));
// I·rᵃ for each basis column, then contract.
const ix: Vec = self.applyTensor(rx);
const iy: Vec = self.applyTensor(ry);
const iz: Vec = self.applyTensor(rz);
return .{
.diag = vec(dot3(rx, ix), dot3(ry, iy), dot3(rz, iz)),
.off = vec(dot3(rx, iy), dot3(rx, iz), dot3(ry, iz)),
.h = rotate(q, self.h),
.mass = self.mass,
};
}One fewer numerical routine, one float less, and no degenerate-eigenvalue edge cases. MuJoCo's form is the better one in C, where a diagonal inertia makes the inner loops cheaper; the six-float form composes better here.
One line in that function deserves attention out of proportion to its size:
// This is not pedantry: using the columns computes `Rᵀ·I·R` instead, which has the
// same eigenvalues and a plausible-looking diagonal, so the error hides in the
// off-diagonal terms alone.I' = R·I·Rᵀ expands to I'[a][b] = rowᵃ · (I · rowᵇ), so it needs the rows of R. But rotate(q, e_k) gives the columns — the rows are the columns of Rᵀ, which is the rotation by the conjugate.
Use the columns and you compute Rᵀ·I·R instead. That is not obviously wrong from any direction you might check it: the result is still symmetric, still positive-definite, has the same eigenvalues and the same trace, and its diagonal is plausible. The entire discrepancy lives in the off-diagonal terms. It is a good example of why the reference comparison exists — no invariant distinguishes the two.
Two inertias can only be added when they share a reference point and an orientation. Phase 2's CRB accumulates a subtree's inertia by literally summing the ten numbers of each body — legal only because comPos moved them all into one frame first. That summation is the entire reason the composite-rigid-body algorithm is cheap, and this function is where it is paid for.
The frame is global orientation (so world-space collision shares it) translated to the root's subtree centre of mass. The translation is purely numerical: an inertia expressed about a distant origin has its useful bits swamped by the m·d² parallel-axis term. MuJoCo needs that at f64; we are at f32 and need it more.
After MuJoCo, robot.zig is intended to meet a Zig libtorch port, with the goal of a GPU differentiable physics engine. That matters now, because two upcoming choices are much cheaper to get right than to fix.
The soft constraint model is the differentiable one. Hard complementarity contact has no gradient at the contact boundary — precisely where you need one. MuJoCo's convex soft model has a gradient everywhere, and its docs note solimp can be set to zero specifically to make contact-force onset smooth. Phase 6 was already going to be this; it now has a second, independent reason.
Phase 6's solver needs differentiable optimality conditions, not just a differentiable implementation. Autodiff through an iterative solver unrolls every iteration onto the tape — huge, slow, numerically poor. The right technique is implicit differentiation: differentiate the optimality conditions of the converged solution and solve one linear system, at a cost independent of iteration count. This constrains how the solver is written, not a wrapper added later. A solver that merely gets close enough is not differentiable in this sense.
Several earlier decisions turn out to have been the right ones for reasons that had not come up yet: the Model/Data split is exactly the parameters/state split autodiff wants; Data being a flat pointer-free buffer maps onto a tensor without restructuring; f32 is torch's working precision; and choosing CRB over Featherstone's ABA matters because ABA never produces M, which both the constraint solver and any implicit gradient need.
We can now say where every part of a robot is. This section answers the other half of the geometry-to-physics question: how heavy does it feel to move?
Hold your arm straight out and swing it from the shoulder. Now fold it at the elbow and swing again. The second is dramatically easier — same muscles, same arm, same shoulder joint. Nothing about your body changed except its configuration.
That difference is the mass matrix. M(q) encodes how heavy each direction of motion is, in the configuration you are currently in. There are two useful ways to read it:
½·vᵀMv. This is the definition M comes from, and the derivation is short. Each body’s kinetic energy is ½·m|v|² + ½·ωᵀIω, and every body’s velocity is a linear function of the joint velocities: v_body = J_body·v. Substituting and summing over bodies gives ½·vᵀ(Σ JᵀₘIₘJₘ)v, and the bracket is M. So M is symmetric by construction, and positive definite whenever no joint is free — a motion that costs zero energy would be a direction the robot can move without moving anything.M[i][j] answers: if I accelerate joint j by one unit, how much torque does that demand at joint i? The diagonal is "how heavy is this joint's own motion"; the off-diagonal is "how much does moving that joint disturb this one".The coupling reading is the one that explains the sparsity, and the sparsity is the whole reason this is tractable. Moving joint i only moves the bodies below i. So two joints interact if and only if one lies on the other's path to the root. Your left elbow and your right knee do not appear together in M, ever, in any configuration — there is no chain of rigid links connecting their motions.
M is 27×27 = 729 entries. How many are actually nonzero? Count each DOF's path to the root and sum. For a tree with a torso and four limbs the answer is a few hundred, not 729 — and for the arm alone it is a triangle. The savings grow with branching, because siblings never interact.
The Composite Rigid Body algorithm rests on a single observation.
Suppose only DOF i accelerates, and every other joint is locked. What happens? Everything below i moves together — rigidly, as a single body, because that is what locking the joints between them means. So for the purpose of computing column i of M, the entire subtree below i is one rigid body. Call its spatial inertia I_comp(i).
The rest follows. The spatial force needed to produce that acceleration is I_comp(i)·cdof_i. The torque it demands at any other DOF j is that force projected onto j's motion axis:
That is the whole algorithm. What remains is bookkeeping: computing the composite inertias, and visiting only the j that can be nonzero.
The composite inertias are nearly free, because comPos (§12b) already put every body's inertia in one shared frame. The composite of a subtree is then the sum of its parts. A backward pass over the tree, adding ten floats per body:
// ---- composite inertias: a backward sum over the tree ----
// Start each body holding its own inertia, then fold children into parents walking
// backwards. Because a parent always has a lower index, one reverse pass suffices.
//
// This sum is only legal because every `cinert` is expressed in the same frame — the
// shared subtree-COM frame `comPos` built. That is the payoff for the whole previous
// section, collected here in three lines.
@memcpy(d.crb, d.cinert);
var bi: u32 = m.nbody;
while (bi > 1) {
bi -= 1;
const parent: u32 = m.body_parent[bi];
// Do not fold into the world: it is not a real body and its inertia is meaningless.
if (parent != world_body) {
d.crb[parent] = d.crb[parent].add(d.crb[bi]);
}
}This is the moment the ten-parameter Inertia packing from §2 pays off. Had we stored a 6×6 spatial inertia, or MuJoCo's principal-moments-plus-quaternion form, this would be a matrix operation per body. As it is, it is three vector adds and a scalar add — and it is why CRB is cheap.
/// Composite Rigid Body: build the joint-space inertia matrix M.
///
/// THE IDEA, which is genuinely simple once seen. Suppose only DOF `i` accelerates and
/// every other joint is locked. Then everything below `i` moves as ONE RIGID BODY — that
/// is what locking the joints below means. Call that body's spatial inertia `I_comp(i)`.
/// The spatial force needed to produce that acceleration is `I_comp(i) · cdof_i`, and the
/// torque it demands at any DOF `j` is that force projected onto `j`'s motion axis:
///
/// M[i][j] = cdof_j · (I_comp(i) · cdof_i)
///
/// That single line is the whole algorithm. Everything else is bookkeeping: getting the
/// composite inertias (a backward sum, cheap because `comPos` put them in a shared frame)
/// and visiting only the `j` that can be nonzero (the ancestor chain).
///
/// COST. The outer loop is `nv`; the inner walks one ancestor chain. For a chain robot
/// that is O(nv²) entries but each is a 6-vector dot — and for a tree with branches it is
/// far less, because siblings never interact. This is why the sparsity is not an
/// optimisation bolted on afterwards: it is the shape of the physics.
pub fn crb(m: *const Model, d: *Data) void {
d.requireStage(.position, "crb");
// ---- composite inertias: a backward sum over the tree ----
// Start each body holding its own inertia, then fold children into parents walking
// backwards. Because a parent always has a lower index, one reverse pass suffices.
//
// This sum is only legal because every `cinert` is expressed in the same frame — the
// shared subtree-COM frame `comPos` built. That is the payoff for the whole previous
// section, collected here in three lines.
@memcpy(d.crb, d.cinert);
var bi: u32 = m.nbody;
while (bi > 1) {
bi -= 1;
const parent: u32 = m.body_parent[bi];
// Do not fold into the world: it is not a real body and its inertia is meaningless.
if (parent != world_body) {
d.crb[parent] = d.crb[parent].add(d.crb[bi]);
}
}
// ---- one row of M per DOF ----
for (0..m.nv) |i| {
const row: u32 = m.mass_row_start[i];
const nnz: u32 = m.mass_row_nonzeros[i];
// The force this DOF's unit acceleration generates, given everything below it
// moves with it.
const force: Force = d.crb[m.dof_body[i]].mul(d.cdof[i]);
// Walk the ancestor chain, writing backwards. The row is stored root-first with
// the diagonal LAST (see `Model.mass_col_index`), and walking UP from `i` visits the
// diagonal first — so the cursor starts at the end and decrements. Storage order
// and traversal order were chosen to match precisely here.
var slot: u32 = nnz;
var j: u32 = @intCast(i);
while (j != no_dof) : (j = m.dof_parent[j]) {
slot -= 1;
d.mass_matrix[row + slot] = dot6(d.cdof[j], force);
}
// Armature is rotor inertia reflected through a gearbox: a real physical mass that
// the motor must spin up, felt at this DOF alone. It lands on the diagonal, which
// is also why it is the cheapest defence against an ill-conditioned M — it is
// literally diagonal regularization that happens to be true.
d.mass_matrix[row + nnz - 1] += m.dof_armature[i];
}
}Note the cursor arithmetic in the inner loop. The row is stored root-first with the diagonal last, but walking up from i visits the diagonal first — so slot starts at the end and decrements. That storage order was chosen back in §5's M_colind specifically so this loop could be a single decrement. Two sections apart, one decision.
And armature: rotor inertia reflected through a gearbox. A real physical mass — the motor's own spinning parts, which you must accelerate to accelerate the joint — felt at that DOF alone, so it lands on the diagonal. It is also, conveniently, exactly diagonal regularization, which is the cheapest defence against an ill-conditioned M. A rare case where the physically honest thing and the numerically convenient thing are the same thing.
A 27×27 mass matrix is not something you check by looking. Three strategies, used together:
1. Compare against an independent implementation. The fixtures carry MuJoCo's M for every state. The test allows our diagonal to exceed MuJoCo's by exactly the armature default — which incidentally pins that armature lands on the diagonal and nowhere else.
2. Assert the properties that make it a mass matrix at all. These keep testing when the numbers legitimately change:
test "crb: M is symmetric, positive definite, and configuration-dependent" {
// Invariants, which keep testing when the numbers legitimately change. Together they
// say "this is a mass matrix" without reference to any particular model.
const gpa: Allocator = std.testing.allocator;
var m: Model = try DoublePendulum.build(gpa);
defer m.deinit();
var d: Data = try Data.init(gpa, &m);
defer d.deinit();
var dense: [4]f32 = undefined;
const poses = [_][2]f32{ .{ 0, 0 }, .{ 0.3, -0.7 }, .{ 1.2, 2.4 } };
var folded_diag: f32 = 0;
for (poses, 0..) |q, pi_| {
d.pos[0] = q[0];
d.pos[1] = q[1];
kinematics(&m, &d);
comPos(&m, &d);
crb(&m, &d);
massMatrixDense(&m, &d, &dense);
// Symmetric: energy does not care which order you name two DOFs in.
try expectApproxEqAbs(dense[1], dense[2], 1.0e-6);
// Positive definite: every motion carries positive kinetic energy. Checked via
// Sylvester's criterion, which for 2x2 is "positive diagonal, positive
// determinant" -- and the determinant condition is the one that catches a matrix
// that is merely positive on the diagonal.
try expect(dense[0] > 0.0);
try expect(dense[3] > 0.0);
try expect(dense[0] * dense[3] - dense[1] * dense[2] > 0.0);
if (pi_ == 0) {
folded_diag = dense[0];
}
}Symmetry says energy does not care which order you name two DOFs in. Positive-definiteness says every motion carries positive kinetic energy — checked via Sylvester's criterion, where the determinant condition is the one that catches a matrix that is merely positive on the diagonal. And then the one that would be embarrassing to omit:
// Configuration-dependent, and in the right DIRECTION: straight out (q2 = 0) the arm
// has more inertia about the shoulder than folded (q2 = 2.4 rad). If M were constant
// the whole file would be pointless, so this is worth asserting rather than assuming.
try expect(folded_diag > dense[0]);3. Test the claim the algorithm rests on. CRB is justified by "everything below a locked joint moves as one rigid body". That is a physical assertion, and it can be checked directly: weld the elbow of a two-link arm, and its shoulder inertia must equal the same geometry with a free elbow evaluated at zero. If that fails, the reasoning is wrong even when the arithmetic is right.
M[0][0] before computing it. The lower link's mass is now near the shoulder, so the shoulder inertia should be close to the upper link's alone. Then try π/2 and convince yourself the answer moves smoothly between the two extremes — that continuity is what makes M(q) differentiable, which the next project depends on.
M is symmetric and positive definite (§14), which is what lets it be factored as LᵀDL with no pivoting and no square roots. Forward dynamics needs M⁻¹(τ + Jᵀf − c), and a constraint solver will need M⁻¹Jᵀ for every contact row. So we need to apply the inverse, many times per step, at many different right-hand sides.
Computing M⁻¹ explicitly is the wrong approach for two reasons. It is more work than solving, and the inverse of a sparse matrix is generally dense. All the structure preserved so far would disappear in one step. Instead we factor once, and every later solve is two cheap sweeps.
Gaussian elimination on a sparse matrix normally creates fill-in: eliminating a variable couples together everything it touched, so new nonzeros appear and the matrix gradually fills. Managing that — choosing an elimination order that minimises it — is an entire field.
For a kinematic tree it cannot happen at all.
Row k of M holds k's ancestor chain. Take any i on that chain. Row i holds i's ancestor chain — which is a prefix of k's, because i's path to the root is literally the tail of k's path. Eliminating k only ever touches rows whose sparsity is already contained in k's own. There is nowhere for a new nonzero to go.
L occupies exactly the same storage as M — same nM entries, same index tables, no allocation, no analysis pass, no elimination-order heuristic. A general sparse solver spends real effort on all four. A kinematic tree gets them for free because the tree is a perfect elimination ordering.
And because rows are stored root-first, a prefix in the tree is a prefix in memory. The update to row i is a contiguous run starting at i's row address, aligned element for element with the head of row k — no index translation, no scatter, just a strided add of two runs. That alignment is why §5 chose root-first-with-the-diagonal-last, several sections before anything needed it.
pub fn factorM(m: *const Model, d: *Data) void {
d.requireStage(.position, "factorM");
@memcpy(d.qLD, d.mass_matrix);
var k: u32 = m.nv;
while (k > 0) {
k -= 1;
const start: u32 = m.mass_row_start[k];
const diag: u32 = start + m.mass_row_nonzeros[k] - 1;
// M is positive definite, so every pivot is positive — armature guarantees it even
// for a massless-looking DOF. A non-positive pivot means the model is degenerate
// (a zero-mass body, or an inertia that has gone bad upstream) and every number
// after this point would be meaningless.
assertf(
d.qLD[diag] > 0.0,
@src(),
"factorM: pivot {d} is {d}, but M must be positive definite " ++
"(a zero-mass body, or armature left at zero?)",
.{ k, d.qLD[diag] },
);
const inv_pivot: f32 = 1.0 / d.qLD[diag];
d.qLDiagInv[k] = inv_pivot;
// Eliminate row k from every ancestor row above it.
var adr: u32 = diag;
while (adr > start) {
adr -= 1;
const i: u32 = m.mass_col_index[adr];
const scale: f32 = -d.qLD[adr] * inv_pivot;
// Row i's pattern is a prefix of row k's, so this is element-wise over
// `rownnz[i]` contiguous entries of each. See the note above.
const dst: u32 = m.mass_row_start[i];
for (0..m.mass_row_nonzeros[i]) |t| {
d.qLD[dst + t] += scale * d.qLD[start + t];
}
}
// Normalize row k's off-diagonal part, so L has a unit diagonal.
for (start..diag) |t| {
d.qLD[t] *= inv_pivot;
}
}
d.stage = .position;
}The loop runs backward over rows because elimination proceeds from the leaves inward: a DOF can only be eliminated once everything depending on it is gone, and children always have higher indices than their parents. The assert on the pivot is not defensive noise — M is positive definite, so a non-positive pivot means the model itself is degenerate, and every number computed after that point would be meaningless.
Three passes, one per factor, in the order that undoes them:
/// Assemble the active constraint rows for the current state.
///
/// A limit end is ACTIVE when the joint is within `margin` of it. The distance is measured
/// per END, not to the nearest one:
///
/// lower: q − lo upper: hi − q
///
/// and either being below `margin` produces a row. Negative means already violated.
///
/// ★ BOTH ENDS CAN BE ACTIVE AT ONCE, and the code must allow it. It looks impossible —
/// a joint cannot be past its lower and upper stops simultaneously — but `margin` is what
/// makes it reachable: if the range is narrower than `2·margin`, the joint is within margin
/// of both ends everywhere in its travel, and MuJoCo emits two rows. A tempting shortcut
/// (`residual = min(q − lo, hi − q)`, one row) is correct for every model with the default
/// zero margin and silently wrong for a narrow range with a generous one. The two rows then
/// oppose each other and the solver balances them, which is the right behaviour: the joint
/// is being softly squeezed from both sides.
///
/// The Jacobian row is `+1` for a lower end and `−1` for an upper one, so a positive
/// constraint force always pushes the joint back INTO its range whichever end it hit. That
/// convention is what lets the solver clamp every row to `f ≥ 0` uniformly instead of
/// tracking which direction each row wants to push.
pub fn makeConstraints(m: *const Model, d: *Data) void {
d.requireStage(.position, "makeConstraints");
d.constraint_count = 0;
for (0..m.njnt) |ji| {
const range: [2]f32 = m.jnt_range[ji] orelse continue;
// `Spec()` rejects a range on a ball or free joint, so a scalar coordinate is
// guaranteed here rather than assumed.
const qadr: u32 = m.jnt_qpos_adr[ji];
const vadr: u32 = m.jnt_dof_adr[ji];
const q: f32 = d.pos[qadr];
const margin: f32 = m.jnt_limit_margin[ji];
// Lower end first, then upper — the order MuJoCo emits them in, which matters
// because the fixtures compare row by row.
const ends = [_]struct { distance: f32, jacobian: f32 }{
.{ .distance = q - range[0], .jacobian = 1.0 },
.{ .distance = range[1] - q, .jacobian = -1.0 },
};
for (ends) |end| {
if (end.distance >= margin) {
continue;
}
const row: u32 = d.constraint_count;
assertf(
row < m.constraint_capacity,
@src(),
"constraint rows overflowed: {d} active, capacity {d}",
.{ row + 1, m.constraint_capacity },
);
// One nonzero entry: this joint's own DOF.
const base: usize = row * m.nv;
@memset(d.constraint_jacobian[base .. base + m.nv], 0);
d.constraint_jacobian[base + vadr] = end.jacobian;
d.constraint_kind[row] = .limit;
d.constraint_source[row] = @intCast(ji);
d.constraint_violation[row] = end.distance - margin;
d.constraint_count = row + 1;
}
}
}Note what is absent: no iteration, no tolerance, no convergence check. This is a direct solve. Contrast a maximal-coordinate engine, which reaches the same answer by iterating a solver until joint errors are small enough — that difference is the entire trade §0 described, appearing here as the presence or absence of a while (!converged).
The factorization produces D, and the spread of D tracks the condition number of M closely enough to be useful. Since we computed it anyway, the diagnostic is free:
pub fn conditionEstimate(m: *const Model, d: *const Data) f32 {
if (m.nv == 0) {
return 1.0;
}
var lo: f32 = zm.floatMax(f32);
var hi: f32 = 0;
for (0..m.nv) |i| {
// qLDiagInv is 1/D, so the extremes swap.
const dval: f32 = 1.0 / d.qLDiagInv[i];
lo = @min(lo, dval);
hi = @max(hi, dval);
}
return if (lo > 0.0) hi / lo else zm.floatMax(f32);
}This exists because of a specific, foreseeable failure. We work in f32 (MuJoCo uses f64), and a robot with a large mass ratio — a heavy torso driving a light fingertip — can push cond(M) past what 24 bits of mantissa carry. The symptom is a simulation that goes soft or diverges with no visible cause, which is a miserable thing to debug from the outside. A number you can print turns it into a question with an answer.
The strongest test here needs no reference values at all. Pick an x, form y = M·x with the dense matrix, solve M·x' = y, and require x' = x. That exercises the factorization and all three back-substitution passes at once, and — the important part — it cannot be satisfied by a factorization that is merely self-consistent. A wrong-but-consistent factorization reproduces its own errors; a round trip through the original matrix does not let it.
Three more, each checking something the round trip alone would not:
Lᵀ·D·L by hand from the stored factor and compare to M. This pins the storage layout, not just the arithmetic.nM entries. The claim the whole section rests on, stated as a test.y from MuJoCo's mass matrix, solve with our factorization. If our M and our solve were wrong in mutually compensating ways, everything above would still pass and this would not.conditionEstimate does as you make one link a thousand times heavier than the other. Then reason about which of the two dominates the pivot spread, and whether adding armature to the light DOF helps — that is the entire content of the f32 escape hatch, and the answer is more interesting than "add a bigger number".
Spin a weight on a string and it pulls outward. Walk toward the centre of a spinning carousel and something shoves you sideways. Neither force was applied by anything — both are consequences of being in motion, and any robot that moves quickly is full of them.
Those are the centrifugal and Coriolis terms, and together with gravity they make up c(q,v): the part of the equation of motion that has nothing to do with your motors.
The mechanism is this. A joint's axis is not fixed in space. If the body carrying a hinge is already rotating, that hinge's axis is being dragged — and an axis that moves accelerates whatever is attached to it, even when the joint velocity is perfectly constant.
So each motion axis has a rate of change, and it is a spatial cross product:
pub fn comVel(m: *const Model, d: *Data) void {
d.requireStage(.position, "comVel");
d.cvel[world_body] = .zero;
for (1..m.nbody) |bi| {
var vel: Motion = d.cvel[m.body_parent[bi]];
const jnt_adr: u32 = m.body_jnt_adr[bi];
// Walk JOINTS, not DOFs. The distinction is invisible for scalar joints and
// load-bearing for ball and free ones — see the note on `rotationTriple`.
for (jnt_adr..jnt_adr + m.body_jnt_num[bi]) |ji| {
const dof: u32 = m.jnt_dof_adr[ji];
switch (m.jnt_type[ji]) {
.slide, .hinge => {
d.cdof_dot[dof] = crossMotion(vel, d.cdof[dof]);
vel = vel.add(d.cdof[dof].scale(d.vel[dof]));
},
.ball => rotationTriple(d, dof, &vel),
.free => {
// The three translation axes are GLOBAL, so nothing can rotate them:
// their rate of change is exactly zero, whatever the body is doing.
inline for (0..3) |k| {
d.cdof_dot[dof + k] = .zero;
vel = vel.add(d.cdof[dof + k].scale(d.vel[dof + k]));
}
// Then the rotations, which DO see the translation velocity.
rotationTriple(d, dof + 3, &vel);
},
}
}
d.cvel[bi] = vel;
}
d.stage = .velocity;
}crossMotion(x, x) = 0: a DOF cannot drag its own axis. Including it would add a term that must cancel exactly, which is slower and, in floating point, less accurate.
Two passes over the tree. Forward, propagating acceleration downward and computing each body's spatial force. Backward, summing each body's force into its parent — because the force a joint must supply is the total for everything hanging below it.
Run RNE with the true joint accelerations and it computes the torques that produce them — that is inverse dynamics. Run it with v̇ = 0 and it computes the torques needed to produce no acceleration, which is exactly c(q,v): gravity, Coriolis and centrifugal, and nothing else. One routine serves both, and forward dynamics uses the second form.
fn rne(m: *const Model, d: *Data, out: []f32) void {
d.requireStage(.velocity, "rne");
// Scratch, sized by the model. Both are per-body spatial quantities that never leave
// this function, so they live in `Data` rather than being allocated per call.
const cacc: []Motion = d.rne_cacc;
const cfrc: []Force = d.rne_cfrc;
// The world accelerates upward at g, so everything hanging off it feels weight.
cacc[world_body] = .{ .ang = vec_zero, .lin = -m.opt.gravity };
cfrc[world_body] = .zero;
// ---- forward: accelerations, then the force each body needs ----
for (1..m.nbody) |bi| {
var a: Motion = cacc[m.body_parent[bi]];
const dof_adr: u32 = m.body_dof_adr[bi];
for (0..m.body_dof_num[bi]) |k| {
const dof: u32 = dof_adr + @as(u32, @intCast(k));
// The moving-axis term: even at constant joint velocity, a rotating axis
// accelerates whatever is attached to it.
a = a.add(d.cdof_dot[dof].scale(d.vel[dof]));
}
cacc[bi] = a;
const inertia: Inertia = d.cinert[bi];
// Newton-Euler for a rigid body, in spatial form.
cfrc[bi] = inertia.mul(a).add(crossForce(d.cvel[bi], inertia.mul(d.cvel[bi])));
}
// ---- backward: a joint carries everything below it ----
var bi: u32 = m.nbody;
while (bi > 1) {
bi -= 1;
const parent: u32 = m.body_parent[bi];
if (parent != world_body) {
cfrc[parent] = cfrc[parent].add(cfrc[bi]);
}
}
// ---- project onto each DOF's axis ----
for (0..m.nv) |i| {
out[i] = dot6(d.cdof[i], cfrc[m.dof_body[i]]);
}
}The per-body force is f = I·a + v ×* (I·v). The first term is familiar. The second is gyroscopic, and it is why crossForce exists as a separate operator back in §2 — the duality that looked like pedantry there is doing real work here.
Gravity is never applied to any body. Instead the world is given an acceleration of −g, which propagates down the tree like any other acceleration. In a frame accelerating upward at g, weight appears as a fictitious force — which is precisely what weight is.
The engineering payoff: gravity needs no special case anywhere. Not a per-body loop, not a flag, not a term added at the end. Switching it off means passing a zero vector, and no code path changes.
RNE can compute M·a + c in a single recursion if you feed it an acceleration, and MuJoCo's mj_rne offers exactly that. We do not use it:
pub fn inverseDynamics(m: *const Model, d: *Data, acc: []const f32, out: []f32) void {
d.requireStage(.velocity, "inverseDynamics");
mulM(m, d, acc, out);
for (0..m.nv) |i| {
out[i] += d.bias_force[i];
}
}The reason is armature — rotor inertia, the motor's own spinning mass reflected through its gearbox. It is real mass that must be accelerated, but it lives in the gearbox rather than in a link, so it appears on M's diagonal and nowhere in the tree. An RNE recursion has no way to know about it.
Multiplying by the assembled M instead makes the forward/inverse round trip exact by construction — both directions use the same matrix — rather than requiring two independent recursions to agree. The distinction matters: a test that passes because two paths share a source of truth is stronger than one that passes because two derivations happened to match.
pub fn forwardDynamics(m: *const Model, d: *Data) void {
d.requireStage(.velocity, "forwardDynamics");
for (0..m.nv) |i| {
d.acc[i] = d.applied_force[i] + d.actuator_force[i] + d.passive_force[i] - d.bias_force[i];
}
solveM(m, d, d.acc);
d.stage = .force;
}Everything in this course up to here existed to make that possible.
pub fn forward(m: *const Model, d: *Data) void {
kinematics(m, d);
comPos(m, d);
crb(m, d);
factorM(m, d);
comVel(m, d);
makeConstraints(m, d);
projectConstraints(m, d);
biasForce(m, d);
passive(m, d);
actuation(m, d, m.opt.timestep);
forwardDynamics(m, d);
solveConstraints(m, d);
}
/// Advance the state by `dt` using semi-implicit Euler.
///
/// The `q` update uses the NEW velocity, not the old one — that single change is what
/// separates semi-implicit Euler from explicit Euler, and it is the difference between a
/// pendulum that gains energy until it explodes and one that oscillates stably forever.
/// Explicit Euler is not offered here; it has pedagogical value and no other kind.
fn advanceEuler(m: *const Model, d: *Data, dt: f32) void {
for (0..m.na) |i| {
d.act[i] += dt * d.act_dot[i];
}
for (0..m.nv) |i| {
d.vel[i] += dt * d.acc[i];
}
integratePos(m, d.pos, d.vel, dt);
normalizeQuats(m, d.pos);
}
/// Advance by `dt` using classical fourth-order Runge-Kutta.
///
/// Four evaluations of the dynamics per step, combined so that the error per step is
/// O(dt⁵) instead of Euler's O(dt²). Worth it when you care about energy over a long
/// horizon — a double pendulum integrated with RK4 conserves energy to a fraction of a
/// percent over ten seconds, where Euler visibly drifts.
///
/// Not worth it once contact is involved: contact makes the dynamics non-smooth, and a
/// high-order method assumes smoothness. RK4 is for the clean case, which is exactly the
/// case where you want to verify energy conservation.
fn advanceRk4(m: *const Model, d: *Data, dt: f32) void {
const nv: usize = m.nv;
// Working copies. RK4 needs the ORIGINAL state to evaluate each stage from, so the
// starting point is saved once and every stage restores from it.
const pos0: []f32 = d.rk_pos0;
const vel0: []f32 = d.rk_vel0;
@memcpy(pos0, d.pos);
@memcpy(vel0, d.vel);
// Butcher tableau for classical RK4: evaluate at 0, dt/2, dt/2, dt, and weight the
// slopes 1/6, 1/3, 1/3, 1/6.
const stage_dt = [_]f32{ 0.0, 0.5, 0.5, 1.0 };
const weight = [_]f32{ 1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0 };
var vel_sum: []f32 = d.rk_vel_sum;
var acc_sum: []f32 = d.rk_acc_sum;
@memset(vel_sum, 0);
@memset(acc_sum, 0);
inline for (0..4) |stage| {
if (stage > 0) {
// Restore and take a trial step of `stage_dt * dt` along the previous slope.
@memcpy(d.pos, pos0);
@memcpy(d.vel, vel0);
const h: f32 = stage_dt[stage] * dt;
for (0..nv) |i| {
d.vel[i] = vel0[i] + h * d.rk_acc_stage[i];
}
integratePos(m, d.pos, d.rk_vel_stage, h);
normalizeQuats(m, d.pos);
forward(m, d);
} else {
forward(m, d);
}
// Record this stage's slope, and accumulate its weighted contribution.
@memcpy(d.rk_vel_stage, d.vel);
@memcpy(d.rk_acc_stage, d.acc);
for (0..nv) |i| {
vel_sum[i] += weight[stage] * d.vel[i];
acc_sum[i] += weight[stage] * d.acc[i];
}
}
// Activation is integrated ONCE, with the last stage's rate. RK4-weighting it too
// would be more faithful, but activation dynamics are first-order and slow compared to
// the mechanics, so it is not where the error budget goes.
for (0..m.na) |i| {
d.act[i] += dt * d.act_dot[i];
}
// Apply the combined slope from the original state.
@memcpy(d.pos, pos0);
for (0..nv) |i| {
d.vel[i] = vel0[i] + dt * acc_sum[i];
}
integratePos(m, d.pos, vel_sum, dt);
normalizeQuats(m, d.pos);
}fn advanceEuler(m: *const Model, d: *Data, dt: f32) void {
for (0..m.nv) |i| {
d.vel[i] += dt * d.acc[i];
}
integratePos(m, d.pos, d.vel, dt);
normalizeQuats(m, d.pos);
}Velocity updates first, then position updates using the new velocity. That single ordering is the entire difference between semi-implicit and explicit Euler, and it is the difference between a pendulum that oscillates stably forever and one that gains energy until it explodes. Explicit Euler is not offered here; it has pedagogical value and no other kind.
Once you are integrating, comparing against a stored fixture stops working: trajectories diverge, and chaotic ones diverge fast. A double pendulum has a high Lyapunov exponent by construction. What you need instead are quantities that must hold regardless of trajectory.
Energy. With no damping, no actuation and no contact, total mechanical energy is exactly conserved in the continuous problem. Any drift is the integrator's, which makes it a direct measurement of integration quality:
// RK4 must be tight over 10 s of chaotic motion.
try expect(drift[0] < 0.001);
// And it must be better than Euler -- which is the whole reason it costs four
// evaluations per step.
try expect(drift[0] < drift[1]);
}Note the second assertion. It is not sufficient that RK4 is good; it must be better than Euler, or it is not earning its four evaluations per step.
The round trip. Apply a known force, solve for the acceleration, ask inverse dynamics what force would produce it, and require the original back. This exercises M, its factorization, c, and every frame convention between them, simultaneously.
Analytic cases. A free body under gravity follows y = y₀ − ½gt². No joints to get wrong, so it isolates gravity, the free-joint DOF layout and the integrator — and needs no oracle at all.
bias_force as the joint force and the arm holds itself still against gravity, anywhere in its workspace. Now consider what happens if the modelled masses are slightly wrong — the arm drifts, slowly, in a direction that tells you which link is mis-estimated. This is the seed of system identification, and why wanting M and c to be differentiable with respect to the model parameters is the natural next thing to want.
You control joint angles. You care about where the hand is. A Jacobian converts between them, and it is reused throughout the rest of this course.
One implementation buys a list of apparently unrelated things:
J along the contact normal;J that is a unit vector on that DOF;J transposed maps a force applied at a point back into joint torques.Most of the work is already done. cdof[i] is DOF i's motion as a spatial vector about the tree's shared frame origin. A spatial motion (ω, v) about origin O moves a point p at velocity v + ω × (p − O). So:
That is the whole computation — the work was done in comPos.
pub fn jacPoint(
m: *const Model,
d: *const Data,
body: u32,
point: Vec,
jac_p: ?[]Vec,
jac_r: ?[]Vec,
) void {
d.requireStage(.position, "jacPoint");
if (jac_p) |jp| {
assertf(jp.len == m.nv, @src(), "jac_p wants {d} columns, got {d}", .{ m.nv, jp.len });
@memset(jp, vec_zero);
}
if (jac_r) |jr| {
assertf(jr.len == m.nv, @src(), "jac_r wants {d} columns, got {d}", .{ m.nv, jr.len });
@memset(jr, vec_zero);
}
const offset: Vec = point - d.subtree_com[m.body_root[body]];
var i: u32 = lastDofOf(m, body);
while (i != no_dof) : (i = m.dof_parent[i]) {
const motion: Motion = d.cdof[i];
if (jac_p) |jp| {
jp[i] = motion.lin + cross(motion.ang, offset);
}
if (jac_r) |jr| {
jr[i] = motion.ang;
}
}
}Vec per DOF. Column i then reads as "the velocity this point gains per unit of DOF i", which is what every use above wants, and the arithmetic stays in vector vocabulary instead of becoming index expressions. The conventional layout is better when you are handing the matrix to a BLAS; this one is better when you are reading it.
Only DOFs that actually move this body get a column. The rest are structurally zero, found by walking the body's ancestor chain — the same chain crb and factorM walk. Three algorithms, one traversal.
pub fn applyForceAtPoint(
m: *const Model,
d: *const Data,
body: u32,
point: Vec,
force: Vec,
torque: Vec,
jac_p: []Vec,
jac_r: []Vec,
out: []f32,
) void {
jacPoint(m, d, body, point, jac_p, jac_r);
for (0..m.nv) |i| {
out[i] += dot3(jac_p[i], force) + dot3(jac_r[i], torque);
}
}Push on the world with your hand and every joint between your hand and the ground feels a torque. The transpose is exactly that bookkeeping, and it is why it appears in the equation of motion back in §0.
1. Finite differences — the cheapest strong test in the project. A Jacobian is by definition the derivative of the forward kinematics, so nudge each joint and measure where the point went. No reference values, no oracle, and it independently validates the frame conventions of §10 and §12: if cdof were built about the wrong origin or with a flipped sign, the derivative would not match.
const eps: f32 = 1.0e-4;
for (0..m.nv) |i| {
// Central difference: second-order accurate, so the tolerance can be tight.
var moved: [2]Vec = undefined;
inline for (.{ -eps, eps }, 0..) |delta, k| {
@memcpy(d.pos, &q);
d.pos[i] += delta;
kinematics(&m, &d);
moved[k] = d.body_xpos[body] + rotate(d.body_xrot[body], local);
}
const numeric: Vec = (moved[1] - moved[0]) / splat(2.0 * eps);
inline for (0..3) |axis| {
try expectApproxEqAbs(numeric[axis], jac_p[i][axis], 2.0e-3);
}
}2. Against the reference. Every body's COM Jacobian, in every fixture state.
3. Consistency with the dynamics. J·v must equal the velocity comVel computed for the same point — two paths to the same number sharing no code, one walking cdof, the other accumulating body velocities down the tree.
4. A case you can check by hand. Hold the arm straight down and push the tip sideways with 1 N. The shoulder must feel 1 N × 0.9 m and the elbow 1 N × 0.4 m — lever arms you could measure with a ruler:
// Hinges are about +Z, the tip is 0.9 m below the shoulder and 0.4 m below the elbow,
// and a +X force at -Y produces a +Z torque of |r| * |f|.
try expectApproxEqAbs(@as(f32, 0.9), torque[0], 1.0e-4);
try expectApproxEqAbs(@as(f32, 0.4), torque[1], 1.0e-4);That last one carries more weight than its size suggests. Every other test compares against something computed; this one compares against something understood.
Phases 0 through 2 are complete. The engine can now:
That is enough for a torque-controlled arm, for Jacobian-transpose inverse kinematics, for gravity compensation, and for the operational-space control laws most manipulation research is written in. It is not yet enough to touch anything — contact needs a constraint solver, which is where this course goes next.
jacSite at the tip, the error vector from tip to target, and the transposed Jacobian applied to that error as a joint torque — three lines, and it works because the transpose converts a task-space pull into joint-space torques. Add bias_force to cancel gravity and it will hold position too.
The dynamics so far have been smooth: forces are functions of position and velocity, and the next state follows from the current one. Contact is not smooth. A foot resting on the floor receives exactly enough upward force to stop it sinking — whatever it takes, however much that is — and zero the instant the foot lifts.
That "whatever it takes" is the whole difficulty. You cannot write the force down; you can only write down the condition it must achieve. So contact is posed as a constraint and solved for.
For one contact with normal n and separation d:
Those three together are a complementarity condition, and they are why contact needs its own solver rather than another term in c.
Stated on positions, the condition is combinatorial. To satisfy it you must decide, for every candidate contact, whether it is touching or separated — and those choices interact, because a force at one contact moves the bodies at another. With n contacts there are 2n assignments and no way to know which is right without trying it.
Differentiating twice changes the question. Instead of asking where will the bodies end up, ask what acceleration does this contact need right now. Every quantity is then linear in the unknown forces, because a = M−1(τ + JTf − c) is linear in f, and the three conditions become:
J·a − aref ≥ 0 f ≥ 0 (J·a − aref)·f = 0
That is a linear complementarity problem: standard object, standard algorithms. The sign conditions still make it harder than a plain linear solve, but the combinatorial explosion is gone.
The price is that the constraint now acts on accelerations, so an interpenetration already present is not removed by it — zero acceleration keeps the error exactly where it is. That is what aref is for: rather than demanding zero, the solver demands the acceleration that drives the existing error to zero over a chosen time constant. The next subsection is that idea.
Concretely, each contact contributes one row to J — how its separation changes with joint velocity — and one entry in aref.
Softness. Its time_const_s is how long an error takes to decay, and it is a physical knob rather than a numerical one: 0.02 s is a firm floor, 0.1 s a soft mat.
Friction says the tangential force cannot exceed μ times the normal force — a cone. Solving against a cone is harder than solving against a box, so the cone is approximated by a pyramid: four tangential directions, each with its own row, each allowed to push only.
The approximation is visible if you look for it — friction is slightly direction-dependent — and it is what MuJoCo uses by default too, for the same reason.
The rows are coupled: pushing one contact moves a body, which changes every other contact on that body. Solving them is the expensive part of a physics step, and there are two good ways.
Take each row in turn, compute the force that would satisfy it assuming the others are right, clamp it into its allowed set, move on. Repeat. It is simple, cheap per iteration, and it converges — linearly.
Linear convergence is the catch. Each sweep moves information exactly one contact along a chain, so a stack of six boxes needs six sweeps before the floor's support is felt at the top, and every sweep after that is correcting what the one before it disturbed. Measured on that stack — 96 rows, one cold solve:
1 iteration residual 26.7 5 residual 13.4 20 residual 4.83 100 residual 0.269 ← still falling
Constrained dynamics is a convex minimisation in acceleration space:
L(a) = ½·(a − afree)T M (a − afree) + ∑active ½·(Ji·a − arefi)² / Ri
The first term says "stay near the acceleration you would have had without constraints"; the second penalises each violated row. Its gradient and Hessian are both exact and cheap:
∇L = M(a − afree) + JT D r H = M + JT D J
Because H is exact rather than approximated, Newton converges quadratically. On the same stack it reaches a residual of 5×10−7 in two iterations, and the stack stands where PGS drops it.
zig build robot-bench for the number on your own machine; a figure quoted here would be a number from somebody else's.
A speed ratio hides this, and it matters more than the ratio does. Over 20 000 steps of that same standing quadruped:
pgs residual 3.0e-6 converged FALSE newton residual 1.2e-7 converged true tolerance 1.0e-6
PGS is stopping on its stall detector — min_progress — and not on tolerance. That is the linear-convergence floor from the table above, arriving at a few times the target rather than at it. It is not a bug and not a regression; it is what linear convergence on coupled rows looks like when you ask for six digits.
The simulation is fine. Three micro-newtons of constraint residual is far below anything the robot can feel, both solvers agree on the resting trunk height to four decimal places, and the stance is stable indefinitely. So the honest version of “the same answer” is: the same physics, reached to different precision.
constraintConverged returns false on every step of the flagship demo robot under the default solver. If you wire that into a health check, you have built an alarm that is always on. It reports exactly what it says — did the residual reach tolerance — and under PGS on a coupled scene the honest answer is no. Want converged to mean something? Use .newton. Want a number? Use constraintResidual and pick a threshold your mechanism actually cares about.
The objective is piecewise quadratic, not quadratic, because a unilateral row contributes only while it is violated — and the active set changes as a moves. That is exactly why Newton here needs a line search: a full step can cross into a region where different rows are active, where it is no longer the minimiser and can be much worse than where it started.
Given forces, the state has to advance. This engine offers the same four MuJoCo does, and the differences between them are not academic.
Semi-implicit Euler is the default, though "Euler" here is not fully explicit: the moment any degree of freedom is damped, the damping is integrated implicitly, by forming M + h·diag(B) and solving against that instead of M.
That is a stability requirement rather than a speed optimisation. Explicit integration of a damping force is stable only while c·h/I < 2, which is generous for a robot's shoulder and very tight for anything light. On a small arm whose wrist carries two fingers, the mass-matrix diagonal there is 0.00041 against 0.13 at the shoulder — three hundred times smaller — so a perfectly ordinary damping="0.5" puts the ratio at 2.44 and the velocity flips sign and grows every step.
Fourth-order, four force evaluations per step, and by far the most accurate on smooth problems. On a torque-free plate tumbling about its intermediate axis — the Dzhanibekov case, entirely gyroscopic, no damping or contact — RK4 conserves energy to 0.00000 and world angular momentum to 0.00003 where every other integrator drifts by a percent or more.
Both are implicit in velocity, and they differ by exactly one term: implicit includes the derivative of the Coriolis force and implicitfast does not. Everything else in that derivative — joint damping, tendon damping, actuator velocity terms — is a model constant. The Coriolis derivative is not: it changes every step, and computing it is most of the cost difference.
What it buys is gyroscopic coupling: a fast rotor whose spin feeds velocity-dependent torque into other, damped axes. Against an RK4 reference on exactly that, after one second:
dt = 1/500 euler 0.00057 implicitfast 0.00057 implicit 0.00014 rk4 0.00001 dt = 1/100 euler 0.00269 implicitfast 0.00269 implicit 0.00063 rk4 0.00002
Four times more accurate than implicitfast, at one linear solve per step against RK4's four force evaluations.
implicit is the one for a fast-tumbling body. It is not — on the torque-free plate above it was worse than implicitfast at the lower spin, and RK4 dominated both completely. Implicit methods buy stiffness, not accuracy, and a torque-free tumbler is not stiff. It is merely fast.
Reduced coordinates buy exact joints and no drift, and they pay for it in topology. A tree has no rings — so a mechanism whose links form one cannot be written down at all. That excludes a real class of machine: Cassie and Digit's parallel shins, four-bar suspensions, delta arms, most geared grippers.
The standard answer, and this engine's, is to build the spanning tree and close the remaining loops with constraints the solver enforces. The joints stay exact; the closure is approximate in the same way contact is, and to the same tolerance.
connect — two points on two bodies that must coincide. Three rows, one per world axis.weld — a fixed relative pose. Six rows: the three above, plus three for orientation.joint — one joint's coordinate as a polynomial in another's. One row, and what makes a geared gripper a mechanism rather than two joints a controller keeps in sync.Limits and contacts are unilateral: a surface may push and may not pull, so a single clamp at zero serves them. An equality is bilateral. A loop closure welds two points together and neither may leave, so its row must be free to pull as well as push — clamping it at zero gives a linkage that resists compression and comes apart under tension, which is a rubber band rather than a rod.
connect describe the same physical point in two different frames, so stating both is stating one fact twice — and getting the second wrong by a centimetre gives a mechanism that lurches on frame one and then behaves, which reads as a solver problem and is a typo. Leave the partner null and the builder computes it from the rest pose. MuJoCo does the same derivation, but only in its file format; here it is in the API, so a model assembled in code gets it too.
The models so far have been written in Zig. Real robots come as files — and the MuJoCo Menagerie is a large, free, carefully-made collection of them, which is reason enough to read its format.
The import path is three stages, and keeping them separate is what makes it testable:
mjcf.zig parses the XML into a faithful, still-MuJoCo-shaped structure: bodies, joints, geoms, actuators, keyframes, sensors, equalities, mesh assets, and the <default> class inheritance that a real file uses a hundred times over.robot_mjcf.zig converts that into this engine's ModelSpec — resolving names to indices, composing inertias, and building the runtime tables.robot_scene.zig composes several robots and loose objects into one tree, because a robot that cannot be given something to pick up is not much of a simulator.xpos for every one of its sixteen bodies — agreement to 1e−4. Then the Go1 stands on its own legs for thirty seconds. Both are things that fail loudly if any part of the chain is wrong, and neither can be argued with.
A keyframe's free-joint quaternion is w-first. MJCF writes qpos with the scalar leading; this engine stores it last. The mistake produces a robot rotated by an arbitrary amount, which looks like a coordinate-frame problem and is a four-element reorder.
<inertial> is not optional. Computed from geometry alone, the Go1's trunk came out at 7.95 kg against the 5.20 the file states, and a thigh at 0.26 against 1.01. Forward kinematics does not depend on mass, so this was invisible for eleven sessions — and everything dynamic was quietly wrong.
An imported robot could be driven long before it could be read. That is half an interface, and it is the wrong half to be missing: control needs observations, and reinforcement learning is nothing but observations.
Sensors read joint coordinates and velocities, site positions and orientations, site-frame linear and angular velocity, proper acceleration, and actuator force — verified against MuJoCo's own readings on a purpose-built fixture, because neither the Go1 nor the humanoid declares one.
robot_control.zig is deliberately a separate file. None of it is dynamics; all of it is what you do with dynamics.
A floating-base robot's Jacobian includes its root's six DOFs, and a solver free to use them "reaches" a target by teleporting the pelvis — a perfect solution to the equations and a useless one for a robot. Actuation is the mask that says which coordinates a motor can actually drive, and it exists because that mistake was made three times.
A PD controller with a torque ceiling and gravity compensation. The subtlety is in the units:
A gain in torque units is only meaningful next to an inertia, and a robot's joints do not share one. This is the same 0.13-at-the-shoulder against 0.00041-at-the-wrist ratio that forced implicit damping in §22 — a kp gentle on the first is violently unstable on the second, and nothing about the number says so. Setting scale_by_inertia divides it out, so kp becomes ω² and kv becomes 2ζω: properties of the response you want rather than of the link you happen to be pushing.
Damped least squares: Δq = JT(J·JT + λ²I)−1Δx. The damping is what keeps a singular configuration from producing an infinite step, and it is why this converges on a redundant arm where a plain pseudo-inverse would not.
Giving the target an orientation as well as a position adds three rows to the same Jacobian. That matters more than it sounds: placing a point is enough for a foot, which only has to be somewhere, and not enough for a hand, because a top-down grasp is a statement about direction. Without it the wrist ends up wherever the solver's nullspace drifted.
This engine does not detect collisions. zimrphysics does, and robot_physics.zig is the bridge: it mirrors every robot geom into the detector as a kinematic proxy, teleports those proxies to wherever the tree says they are, and converts the contacts that come back into constraint rows.
The separation is worth the seam. Collision detection is a large, fiddly, well-understood problem that has nothing to do with articulated dynamics, and an engine that owns both tends to have the two grow into each other.
Teleport, do not steer. The proxy must be placed exactly where the tree says, not moved toward it. A function that steers is reasonable for its original caller and silently wrong here, and this specific confusion cost several sessions of contact debugging.
Filter what cannot move. Two links joined by a hinge overlap near that hinge by construction — that is what a joint looks like geometrically. Reporting it as contact gives a limb that fights itself the moment it folds. MuJoCo filters the same pairs, and the rule is about welded bodies rather than adjacent ones, because a chain of jointless links is one rigid object however many links it is written as.
Know when something was moved rather than travelled. A proxy that jumped several metres in one step did not travel there: a keyframe was applied, or a reset button pressed. Swept against the line between the two poses it would find whatever lies along it and invent a contact for it. The fact travels with the data — whatever writes positions wholesale sets a flag — because relying on each caller to remember is relying on thirty-one call sites to remember.
Everything above describes how contacts get found. Who resolves them is a separate question, and the answer is: both engines do, independently.
zimrphysics resolves crate-versus-arm treating the arm as infinitely massive. robot.zig resolves the same contact treating the crate as immovable. Neither solver knows the other exists, so a shared contact is solved twice and the pair feels stiffer than it should. That is exactly right for static geometry — a floor really is immovable — and it is correct enough for a heavy arm pushing light objects, which is the case worth having first. It is wrong for a light arm and a heavy crate, and it will look like an arm that cannot be pushed.
robot_scene.zig assembles exactly that:
robots and loose bodies in one system. A contact between an arm link and a crate in the same
tree is not “external” to anything — the solver builds the relative Jacobian
Jb − Ja for it, precisely as it does for two
links of the same arm, and momentum is conserved by construction rather than
approximately. There is a test named for that property.
The general two-solver problem — one combined solve across generalized and maximal coordinates — is research-grade, and deliberately not attempted. The documented ladder, in increasing cost: feed last step’s contact impulse back as an external force (one step of lag, fixes most of it); iterate the two solvers per step; assemble one system. None of it is built, because putting the object in the tree has been enough every time so far.
Everything in §19, plus:
What is still missing: spatial tendons, deformables, SDF collision, muscle actuators. Each is something a specific model would need, and none of them is in the way of a legged robot, an arm, or a policy learning to use either.
§27 said this engine does not detect collisions — zimrphysics does, and a bridge converts what comes back into constraint rows. That separation is right, and it hides a decision that turned out to matter more than any of the dynamics above it: how a pair of shapes is asked whether they touch.
Iteratively. GJK walks a simplex toward the origin to decide overlap; EPA then grows a polytope outward to find the escape direction. One pair of routines handles every convex shape against every other. It is general, and it is what most engines do.
Analytically. Solve each pair of shapes in closed form. Sphere against box, capsule against plane, capsule against capsule — each gets its own function. MuJoCo does this, and sends only ellipsoids and meshes to an iterative solver:
/* PLANE SPHERE CAPSULE BOX */
/*PLANE*/ {0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneBox}
/*SPHERE*/ { mjc_SphereCapsule, mjc_SphereBox}
/*CAPSULE*/{ mjc_CapsuleBox}
mjc_PlaneCapsule is about twenty lines: take the capsule's two endpoints, run a sphere-plane test on each. The routine ends there.
An iterative method converges when the situation is well conditioned. Two situations that arise constantly on a humanoid are not.
Deep penetration. A capsule lowered through a floor box, asked directly:
lowest point EPA reports analytic -0.2670 -0.2670 -0.2670 -0.2870 -8.5449 -0.2870 <- escapes through the SIDE -0.3470 -8.5449 -0.3470
Past about 27 cm the nearest face is no longer the one EPA's polytope grew toward, and it reports a depth of eight and a half metres — the distance out through the side of a twelve-metre floor. The contact force that comes back points sideways.
Degenerate configurations. Two capsules with coincident axes — which is exactly how two legs rest against each other:
gap 0.010 depth -0.0880 2 points correct gap 0.000 depth -0.0980 NONE <- 9.8 cm of overlap, no contact at all gap -0.010 depth -0.1080 2 points correct
The simplex is degenerate and nothing comes back. The legs pass through each other.
Capsule against box finds the closest point on the capsule's segment to the box, then does a sphere-box test there. The segment matters: a first version tested only the two end-caps, which is exact for a capsule lying flat on the ground and returns nothing at all for one standing against a wall, where the shaft does the touching. That version walked the character controller through walls.
Finding the closest point uses a ternary search, because point-to-box distance is convex along a segment and ternary search on a convex function reaches the global minimum. An earlier alternating-projection version — clamp into the box, project back onto the segment, repeat — stalled at non-optimal fixed points whenever the segment ran nearly tangent to a face. Every failure sat on a box edge, off by up to 2 cm.
Capsule against capsule solves segment-to-segment closest approach, then a sphere-sphere test. The parallel case gets an explicit branch: when the axes are parallel the 2×2 system is singular, so it takes the midpoint of the overlapping span, which is both stable frame to frame and what two parallel capsules physically touch along.
Not by reading the code. mj_geomDistance answers the same question MuJoCo's way, and a brute-force sweep of our own geometry answers it a third way. When our routine said −0.12, MuJoCo said +0.06, and brute force agreed with us — which located the disagreement in the geometry rather than in the new routine, and led straight to the axis.
The gate now is simple and total: a humanoid standing in its home pose reports 8 contacts, 0 of them self — exactly MuJoCo's 8 and 0.
A contact does not have to be a wall. The reference acceleration from §20 is a damped spring, and its two parameters plus an impedance ramp are enough to describe a range from bone to fat:
Impedance raises the constraint's strength from min to max across a width of penetration. A min near zero with a width of a centimetre is skin, then fat, then bone. This is MuJoCo's solimp.damp_ratio above 1 is overdamped: an impact's energy goes into the contact instead of back into the body. Most of what separates flesh from rubber lives in those two numbers.time_const_s yields over milliseconds rather than instantly.Measured on a humanoid dropped limp from 1.2 m, varying only those numbers:
fat 0.000, damp 1.0: rebound 0.222 m/s sink 2.0 mm fat 0.010, damp 1.0: rebound 0.216 m/s sink 5.3 mm fat 0.030, damp 4.0: rebound 0.163 m/s sink 14.4 mm
§26 built a PD controller. Give it a target angle and it produces a torque proportional to the error. It is reactive by construction: it knows where the joint is and where you want it, and nothing else — not what the robot will do next, what the torque will cost, or that the arm is about to hit the table.
For holding a pose that is enough, and §26 measured it holding one. For anything that has to get somewhere, it is not. The standard example is a pendulum that starts hanging down and must end upright, with a motor too weak to lift it directly. The only way up is to swing the wrong way first, build energy, and come back. No feedback gain on the angle error will ever do that, because at the moment you must move away from the target the error says move towards it.
What you need instead is a plan: a whole sequence of future controls, chosen together, scored by what they will collectively achieve. This part builds that.
Start with the smallest problem that has an answer, because the general case is this one repeated.
Let the state be x and the control u. The dynamics are linear, the cost quadratic:
x' = A·x + B·u J = ½·xᵀQx + ½·uᵀRu + ½·x'ᵀP·x'
Three terms: what this state costs, what the control costs, and what the state we land in costs. Q, R and P are the weights — how much you care about each. Substituting the dynamics into the last term:
J(u) = ½·xᵀQx + ½·uᵀRu + ½·(Ax + Bu)ᵀ·P·(Ax + Bu)
This is a quadratic in u, so it has exactly one minimum and we can find it by differentiating and setting to zero. Expanding the last term and keeping only what depends on u:
∂J/∂u = R·u + Bᵀ·P·(A·x + B·u) = 0 (R + BᵀPB)·u = −BᵀPA·x u = −(R + BᵀPB)⁻¹·BᵀPA · x
That is LQR in one line, and every symbol in the code comes from it. The control is a matrix times the state — a linear feedback gain. Call it K, so u = −K·x.
Now two steps. The obvious approach — write down the cost of both, differentiate with respect to both controls at once — works and does not generalise: at fifty steps you are inverting a fifty-times-larger matrix.
The way through is to solve it backwards. Define the value function Vt(x): the total cost from time t onwards, assuming every control from here on is chosen optimally. At the end there is nothing left to choose, so it is just the terminal cost:
V_N(x) = ½·xᵀ·P_N·x
And one step earlier, the best you can do is the cost you pay now plus the best you can do afterwards:
V_t(x) = min over u of [ ½·xᵀQx + ½·uᵀRu + V_{t+1}(A·x + B·u) ]
This is the whole of dynamic programming, and it is the step to slow down on: it turns one big optimisation over every control at once into N tiny ones, each over a single u.
Now suppose Vt+1(x) = ½·xᵀPt+1x. Then the bracket is exactly the one-step problem of §32 with P = Pt+1 — so we already know the answer:
K_t = (R + BᵀP_{t+1}B)⁻¹ · BᵀP_{t+1}A
Substituting u = −Ktx back in and collecting terms gives the cost at time t, and it is again a quadratic in x:
P_t = Q + AᵀP_{t+1}A − K_tᵀ·(R + BᵀP_{t+1}B)·K_t
So the assumption reproduces itself. VN is quadratic, therefore VN−1 is quadratic, therefore all of them are. Run that backwards from the terminal cost and you get a gain at every knot. That is the Riccati recursion, and it is the backward pass.
robot_mpc.zig's main test does: it runs this on the same A and B the engine used, and requires the two to agree. On a linear system with a quadratic cost, iLQR reduces exactly to LQR, so any disagreement is a bug rather than a tolerance.
The formulas above are correct and awkward to implement: R + BᵀPB appears three times and means one thing. So the standard presentation names the pieces. Expand the bracket from §33 to second order about the current point and call the coefficients:
Q_x = l_x + Aᵀ·V_x the gradient wrt state Q_u = l_u + Bᵀ·V_x the gradient wrt control Q_xx = l_xx + Aᵀ·V_xx·A curvature wrt state Q_uu = l_uu + Bᵀ·V_xx·B curvature wrt control Q_ux = Bᵀ·V_xx·A the cross term
where l is the cost at this knot and V the value function from the knot after. Minimising over the control now reads:
k = −Q_uu⁻¹·Q_u the feedforward: what to change regardless of where we are K = −Q_uu⁻¹·Q_ux the feedback: how to react to being somewhere else
and the value function passes backwards as
V_x = Q_x + Kᵀ·Q_uu·k + Kᵀ·Q_u + Q_uxᵀ·k V_xx = Q_xx + Kᵀ·Q_uu·K + Kᵀ·Q_ux + Q_uxᵀ·K
Compare against §33 and it is the same recursion — Quu is R + BᵀPB, Qux is BᵀPA, and the Vxx line is the Pt line rearranged. Nothing new has happened; the names just stopped repeating themselves.
Real robots are not linear. A globally linear model is not required though — only one accurate near the trajectory currently under consideration:
The one change from LQR is what the gains mean. In LQR, u = −Kx is the control. In iLQR, we already have a control sequence and the gains are a correction to it:
u_new[t] = u[t] + α·k[t] + K[t]·δx[t] δx[t] = (state we are actually at) − (state we linearised about)
The α is a step size and §37 explains why it has to be there.
qpos holds quaternions and does not live in a vector space. On a fixed-base arm nq = nv and subtracting works by accident; on anything legged or floating it silently does not. The planner therefore calls the same quaternion logarithm the derivatives use:
deriv.stateDiff(m, s.delta, plan.states[...], s.states_trial[...], 1.0)
The same recursion as it is actually written. The gradient first — note the term that is easy to leave out:
const knot_delta: []const f32 = problem.knot_error[t * ndx ..][0..ndx];
for (0..ndx) |i| {
var sum: f32 = problem.cost.state[i] * knot_delta[i];
for (0..ndx) |r| {
sum += a[r * ndx + i] * s.value_x[r];
}
s.q_x[i] = sum;
}The first line of the sum is lx — the running cost's own gradient — and the loop is Aᵀ·Vx. Then the gains, which is where Quu gets inverted:
@memcpy(s.q_uu_factor, s.q_uu);
if (!cholesky(s.q_uu_factor, nu)) {
return false;
}
@memcpy(s.tmp_nu, s.q_u);
choleskySolve(s.q_uu_factor, nu, s.tmp_nu);
for (0..nu) |j| {
gains.feedforward[t * nu + j] = -s.tmp_nu[j];
}A Cholesky factorisation rather than a general inverse, for two reasons. It is about twice as fast on a symmetric matrix, and — more usefully — it fails exactly when the matrix is not positive definite, which is the condition we need to detect anyway. A general inverse would happily return a matrix and let the resulting step point uphill. §38 is about that failure.
The backward pass computed the exact optimum of the quadratic model. The model is a second-order fit to a nonlinear system, and it is only trustworthy near the point it was fitted at. Take the full step and you may land somewhere the model never described, where the cost is higher than where you started.
So the forward pass tries a decreasing sequence of step sizes and takes the first that actually reduces the true cost — recomputed by rolling the controls out through the real simulator, not by evaluating the model:
for α in [1.0, 0.5, 0.25, 0.1, 0.05, 0.01]:
roll out u + α·k + K·δx through the REAL dynamics
if cost went down: accept, stop
Rolling out through the real dynamics is what makes the test meaningful. The model's opinion of the improvement is what produced the step; the simulator's is what accepts or rejects it.
Quu is guaranteed positive definite at a minimum. Away from one it can be indefinite, and the "optimal" step from an indefinite Hessian points uphill. The standard fix is Levenberg-Marquardt: add a multiple of the identity to the diagonal before inverting. Large values drag the step towards plain gradient descent — slower, but always downhill. Raise it when a step fails, lower it when one succeeds.
That rule has a failure case.
The backward pass already knows the difference, and asking it costs nothing. It can predict how much the step should buy, from the same quantities it just computed:
ΔJ(α) = α·(kᵀQ_u) + ½·α²·(kᵀQ_uu·k)
At the optimum k → 0 and both terms vanish. If the model expects nothing, we are done — keep these gains, they are the undamped answer. If it expects a great deal and the line search still cannot deliver, then the model is wrong and regularization should rise:
const predicted: f32 = predicted_step.improvement();
if (predicted <= opt.tolerance * @max(@abs(initial), 1.0)) {
keepGains(plan);
converged = true;
iteration += 1;
break;
}@abs(current) instead puts a feedback loop in the stopping
rule: if the trajectory ever diverges the cost grows, so the bar for calling the problem
solved grows with it. Measured on a cartpole given a horizon too short to solve its task —
cost 2.2×107, tolerance 1e-4, so any predicted improvement below 2187 counted as
convergence. The optimiser did one iteration per frame and declared victory, every
frame, while the pole spun up to 58 radians.
1 iters
next to a budget of forty. Anchoring to the initial cost fixes the bar at the scale the
problem started at, so a run that is getting worse can never talk itself into stopping.
And the gains handed back must come from a pass that earned them. keepGains/restoreGains stash the last accepted set, so a caller never receives a gain from a rejected exploratory pass. In trajectory optimisation that is untidy; in MPC it is a correctness bug, because K0 is applied to the actual robot between re-solves.
The derivatives come from finite differences: nudge, re-step, subtract, divide. §24 derives the epsilon that balances truncation error against cancellation. There is a regime where no epsilon works, and the measurement shows it more clearly than a description would.
Here is B = ∂x′/∂u measured on the cart from §36's test — a linear system, where B is exactly constant at [h²/M, h/M]:
q=0.0 v=0.0 B = [4.95050e-5, 4.95050e-3] correct q=1.0 v=0.0 B = [0.00000e0, 4.95050e-3] top entry GONE ctrl=5.0 B = [3.45267e-4, 4.94703e-3] 7x too large
The arithmetic says why. A control nudge of ε moves the position by h²·ε/M ≈ 1.7e-8. Near q′ ≈ 1, f32 resolves about 1.2e-7. The signal is an order of magnitude below the last bit of the number it is being added to, so the quotient is quantisation noise, and it lands on zero or on seven times too large depending on where the rounding falls.
Raising ε until the signal clears the noise floor puts truncation error back in. There is no value that works. This is why MuJoCo is f64.
The optimiser so far solves a fixed horizon from a fixed start. Turning it into MPC is a loop:
That last point is what makes it affordable. A cold solve takes tens of iterations; a warm one, seeded with yesterday's answer shifted by a knot, usually takes one or two — because the problem barely changed.
examples/mpc_cartpole, examples/balance_flywheel and examples/crane; control limits are handled by a box-constrained Quu solve, checked against four answers worked out by hand. The three sections that follow put it to work.
Two things to take from this part. First, the whole of it is a quadratic minimisation applied recursively — §32 on paper, §33 backwards, §34 renamed, §35 relinearised. Nothing is added after §32 that is not bookkeeping. Second, three of the four bugs in this file produced plausible wrong answers rather than crashes: a 5% gain error, a feedforward that silently stopped tracking, a gain quietly halved. Each was found by an exact oracle disagreeing, and none would have been found by a plan that merely looked reasonable.
A gantry crane carries a hanging load from one place to another and has to arrive with it still. It is the same mathematics as the cartpole — a mass swinging under a driven cart — with the goal changed from balancing the pendulum up to bringing it to rest down.
A trolley runs on a rail; a payload hangs below it on a cable of length L. Command the trolley's acceleration, which is what a real crane's drive takes:
ẍ = a θ̈ = −(g/L)·sin θ − (a/L)·cos θ
The sign on a/L is the whole problem. Accelerating the trolley forward swings the payload backward. So to arrest a load that is already swinging you have to accelerate into it — decelerate early, and briefly run the trolley in reverse. The manoeuvre looks like a mistake and is the correct answer.
This is what a reactive controller cannot produce. By the time the swing needs killing the trolley is already at the target, so a gain on trolley position has nothing left to say. It arrives, and the load keeps swinging.
Four states, one control, and the model is linear about hanging — so A and B are constants and one backward pass is the exact answer. Before any of it was believed, three oracles:
The acceptance test was fixed before the code: move the payload 10 m and arrive with the angle under 0.02 rad, against a position PD given the same acceleration limit. The bar was residual swing under a tenth of the PD's.
| controller | final x | |angle| | |rate| | peak |vel| | residual swing |
|---|---|---|---|---|---|
| position PD | 9.994 | 0.1348 | 0.3129 | 2.077 | 0.2217 |
| MPC | 10.000 | 0.0001 | 0.0001 | 3.258 | 0.0002 |
The residual swing came in at a thousandth of the PD's, against a bar of a tenth. The PD is a competent position controller: it arrives at 9.994 of 10.000. It leaves the load swinging through 12.7 degrees because nothing in it refers to the payload, and there is nowhere sensible to add such a term.
One more point: the simulation is nonlinear and the plan is not. The planner only ever sees the small-angle model; the crane is never stepped without the real sin and cos. That is what real MPC does, and it turns “does the linearisation hold?” into something a run answers rather than something a comment claims.
Try it in examples/crane: two cranes, same target, same limit, one planning. Watch the trolleys rather than the loads.
The crane example succeeded on the first serious attempt. This one did not, and its failures cover more ground than its eventual success does.
A vehicle under gimballed thrust must land on a pad. Six states, two controls, and — unlike the crane and the balancing model — genuinely nonlinear: ax = T·sin(θ+δ)/m couples attitude to gimbal, so every knot needs its own Jacobians and the iLQR loop of §35 finally does the job it was built for.
The constraint that makes it interesting is the lower thrust bound. A real engine cannot throttle to zero, so “cut it and coast” does not exist. A vehicle needing less deceleration than the minimum provides has exactly one move left: lean over and waste some thrust sideways. No feedback gain represents that manoeuvre, and it falls out of the box solve for free because the bound is asymmetric — [0.4·W, 2.0·W], not a symmetric clamp.
Free fall at exactly −g; thrust equal to weight holds still for five thousand steps; leaning trades lift for travel; and the gimbal's sign checked both ways, because a test on the magnitude would pass with it inverted and a landing demo would steer itself into the ground. Jacobians against centred differences, swept off-centre in tilt and gimbal — at θ = δ = 0 half the entries vanish and a wrong derivative hides completely. All of it passed first try.
First: a destination is not a reference. Told to be at the pad — 100 m away, with a 3 s horizon — the planner found that unreachable and settled for what it could get: zero velocity. It leaned 0.474 rad to waste thrust it was not allowed to switch off, and hovered there. The answer was correct for the question asked, and the question was wrong: a reference has to be reachable inside the horizon, so a descent profile replaced it.
That failure is the instructive one, because the vehicle did exactly the thing the example exists to show — the manoeuvre the lower thrust bound forces — while completely missing the goal. A plan can be locally perfect and globally pointless, and the cost function is where that gets decided.
Second: the profile's shape is kinematics, not taste. A linear v = −0.4·h commands −2.2 m/s with 5 m left, and the vehicle faithfully delivered −2.2 against a 0.5 bar. The right shape is v = −√(2·a·h) — the fastest descent from which a given deceleration still stops you at the ground.
Third, a step already described in §40: §40 says to shift the control sequence one knot forward between solves. This planner did not. Every tick it warm-started from a seed stale by one knot, and the commanded throttle chattered between its bounds on the way down:
163% 109% 88% 61% 132% 192% 200% 77%
That is a solver re-deriving its answer from scratch under an iteration budget that assumes it does not have to. The crane got away without the shift because its problem barely changes between ticks; a descent does not have that luxury.
Fourth: the fix was applied to one axis and not the other. The descent profile replaced the altitude reference and x was left as a step target of zero at every knot — so a vehicle 30 m out was told to be over the pad immediately. The same unreachable reference that made the first version hover, still sitting there on the lateral axis. It is exactly why the centred start always landed and the offset ones never did.
Fifth, also covered earlier: §37 explains why the step size is not optional, and this solver rolled out at full feedforward every pass and kept whatever came back. On a linear model that is correct — the quadratic model is exact, the full step is the answer — which is why the crane and the balancing planner never needed a line search and never missed one.
This model is not linear, and the symptom was unmistakable: more iterations made it worse. Four passes brought the vehicle in at −1.1 m/s with 0.013 rad of tilt; sixty passes tumbled it through 6.4 radians. An optimiser that diverges as you let it work harder is overshooting rather than converging slowly, and halving the step until the cost actually falls is the standard answer.
| controller | touchdown vy | tilt | landed |
|---|---|---|---|
| position PD | −1.10 m/s | 0.002 rad | 0 of 4 |
| MPC | −0.11 m/s | 0.005 rad | 4 of 4 |
An order of magnitude inside every criterion, from every offset, and unchanged whether the solver is given four passes or sixty — which is what convergence looks like when the step size is chosen rather than assumed.
One more thing to carry: the measurement that found the chatter was printing the commanded throttle and looking at it. One line, and it would have taken an afternoon to reason toward. The measurement that found the line search was noticing that more effort made the answer worse — a recognisable shape, because it never means “try harder still”.
examples/rocket draws the throttle as flame length, so you can watch the engine that never goes out.
The previous two examples show a planner solving a problem. This one asks a narrower question: given the same robot and the same motors, when does planning beat a gain? The answer turns out to be a single property, and the ablation at the end isolates it.
A PD holding a joint against a target computes τ = kp(q* − q) − kvq̇. If the target is moving at speed v, then at steady state the arm moves at v too, so the damping term is spending kvv and the spring term must supply it:
kp·e = kv·v ⇒ e = kvv / kp
With critical-ish damping kv ≈ 2√kp that is e ≈ 2v/√kp. The error is never zero. It shrinks only as the square root of the gain, and raising the gain drives the actuator into its limit and the loop toward ringing. That is the best a feedback law without a model of the future can do, at any gain.
The planner is given the target's position at every knot of the horizon — 75 knots at 4 ms, so 0.3 seconds of future. Its cost is
J = Σk ½·(xk − x*k)TQ(xk − x*k) + ½·ukTRuk + ½·(xN − x*N)TQf(xN − x*N)
and the iLQR machinery of §35 minimises it subject to the true nonlinear dynamics, re-linearised at every knot, with the torque box enforced by the Quu solve. Because a future reference appears in the running cost, a control now can be paid for by an error avoided later. The plan leads the target instead of chasing it.
A five-segment 1.9 m arm with a 6 kg gripper, following a tilted circle, all controllers held to the same 400 N·m limit. The servo's gain was swept past its optimum so the comparison could not be a strawman:
| controller | joint-space RMS error |
|---|---|
| PD kp 2 500 | 0.685 rad |
| PD kp 20 000 | 0.287 rad |
| PD kp 90 000 | 0.156 rad — the optimum |
| PD kp 180 000 | 0.168 rad — past it, the trend reverses |
| MPC, preview removed | 0.213 rad |
| MPC with preview | 0.096 rad |
Two things follow from that table, and the second only exists because of the ablation.
The advantage is 1.63×, not 3×. Stopping the gain sweep at 20 000 — where the trend was still improving — would have doubled the apparent win. A sweep must run until it reverses, or the number quoted is whatever the sweep happened to stop at.
And preview is the whole story. Remove it — every knot referencing the target at now rather than in the future, everything else identical — and the planner scores 0.213 and loses to a well-tuned PD. Knowing the dynamics is not what wins here. Knowing the future is.
PoseHold writes data.applied_force and clears it at the top of each call; a planner drives data.ctrl and never touches that array; step applies both. Every comparison ran the servo first, so every planner run inherited the servo's final torques and fought them for its whole duration. The same planner, same weights, same reference, scored 0.096 rad run first and 2.69 rad run after a PD — its own solver cost reading 7.5 against 5554.
A second bug fell out on the way: PoseHold clamped its tracking term and added gravity compensation outside the clamp, so its real ceiling was τmax + gravity — 1589 N·m on an arm rated 400. The comment three lines above stated the correct behaviour. A comment that disagrees with its code is a bug report somebody already wrote.
See it in examples/tracking. The pink line is the distance from gripper to target; the servo's gain is a live slider that reaches past its own optimum, so the claim can be attacked rather than taken on trust.
Everything so far computes a pose from torques. Retargeting is the opposite question: a capture already contains a pose, for a skeleton that is not yours, and you want the robot to adopt it. The two skeletons differ in joint count, bone length, rest orientation, naming and units, and none of those differences is negligible.
The obvious approach is to copy each joint's rotation from the capture onto the matching robot joint. It fails for a reason worth stating precisely: a rotation is only meaningful relative to a rest pose, and the two skeletons do not share one. A shoulder rotation that means "arm forward" on the capture means something else on a robot whose arm rests at a different angle. Correcting for that needs a per-joint offset, and once the joints also differ in count — a five-link spine driving a two-link one — there is no correspondence to attach the offset to.
Copying rotations also cannot express a joint limit. If the capture's elbow bends further than the robot's can, a copied rotation is simply wrong, and clamping it changes the whole arm's reach with no way to compensate elsewhere.
Put sample points on the robot's bodies, say where the capture puts each of them, and solve for the configuration that best satisfies all of them at once:
E(q) = Σb,k wbk ‖Tb(q)·sbk − &375;bk‖² + λlim Σj barrier(qj) + λsmo ‖q ⊖ qprev‖²
where sbk is the k-th sample point in body b's frame,
Tb(q) is that body's transform at configuration q, and
&375;bk is where the capture says the point should be. This is
the same Gauss–Newton machinery as chapter 27's IK, with two extra terms.
The counting argument is what makes it work. One point on a body constrains its position. Two constrain its direction. Three non-collinear points constrain its full orientation, twist included. So aiming a bone at a target, choosing which way an elbow bends, resolving a limb's swivel and converting a rotation between two differently built skeletons are not four problems — they are one problem with different numbers of sample points, and they can all be written into the same objective.
The counting also tells you where a result will be poor before you look at it. A body carrying one sample has an unconstrained orientation; a body carrying two collinear samples has an unconstrained twist. If a bone comes out badly, count its samples first.
Joint limits as a soft barrier. Clamping a coordinate after each step removes a direction from the solve: the configuration presses against the wall, and small changes in the target flip it between the answers on either side. A barrier that grows as a coordinate approaches its limit keeps the derivative alive, so the solve slides along the limit instead. In this engine that single change is the largest improvement in frame-to-frame continuity of any in the retargeting path.
A posture term. Some configurations are genuinely ambiguous — a redundant chain has a null space, and the solve is free to wander inside it between frames. A quadratic pull toward the previous frame's configuration picks the nearby answer without changing which answers are acceptable. It prevents wandering; it cannot prevent jumping between two well-separated configurations that both fit, which is a model question rather than a weight question.
A target built from the capture's joint position asks the robot's bone to span the
capture's distance. If the two differ in proportion, that residual can never reach zero,
and a free root will slide the whole figure to spread the error. Building the target along
the capture's direction at the robot's own bone length keeps it
reachable, so nothing moves to satisfy something impossible. When the proportions do
match, targeting the capture's positions directly is better, because it puts the figure
where the performer actually was. position_pull blends between the two.
Correspondences that cannot be read off a joint — a foot's heel and toe, the orientation of a leaf body, a twist reference — are measured once from the rest pose, where both figures depict the same thing by definition, and reused every frame. That also gives a self-check: a sample whose target misses its own point at the rest pose has a wrong correspondence rather than a hard one, and is better dropped than fought.
A scale factor multiplies every target, so an error in it does not degrade the result, it destroys it. Three properties make one trustworthy. It should be pose invariant, so measure bone lengths rather than heights — a crouch must not change the scale. It should be unit cancelling, so measure the capture side from the same array the scale is applied to. And it should be anatomy matched, so sum only over the body pairs the correspondence table actually links: a capture with fingers and a five-link spine has far more bone length than an eighteen-body robot, and comparing the totals compares the wrong thing.
const scale = robot.captureScale(&model, frame); const n = robot.buildPointSamples(&model, inputs, &samples); robot.solvePointCloud(&model, &data, samples[0..n], opts);
buildPointSamples runs once per robot-capture pairing;
solvePointCloud runs per frame and leaves the pose in data. A
MatchRow table states which capture joint drives which robot body, accepts
alternative joint names, and ignores exporter prefixes, so one table serves several rigs.
See it in examples/geno_dance, which plays a skinned character and drives
a MuJoCo humanoid from the same clip.
Source: src/robot.zig (dynamics) and src/robot_mpc.zig (derivatives, planning, gait, balancing, crane, control). Showcase plan: src/notes/mpc_showcase_plan.md. Plan and decisions: src/notes/robot_port_plan.md. Concepts and the MuJoCo background: src/notes/tutorials/mujoco-tutorial.html. Reference values: scripts/robot_oracle.py → src/tests/fixtures/robot/reference.zig.