The problem
In chapter 3.3 you built the inside of mj_step for a body that could go
anywhere the force sent it: state, a force law, three integrators, and energy
drift as the number that told you how wrong each integrator was. Bodies fell
freely.
A robot is the opposite of free. Its links are bolted together at joints. A
pendulum bob may only move on a circle a fixed distance from its pivot; the
second bob of a double pendulum may only move on a circle around the first. That
"may only" is a constraint — a rule the state must satisfy, written
g(q) = 0. For a distance joint, g measures how far the link's length is from
its rest length; g = 0 means the rod is exactly right.
Here is the question the chapter turns on: gravity pulls the bob straight down, but the rod will not let it go straight down — so what force does the rod apply instead, and how do you compute it? You cannot write it down in advance the way you wrote gravity, because it depends on gravity, on the current velocity, on every other constraint in the chain, all at once. The constraint force is not a law you know. It is a value you solve for. Learning to solve for it — and then watching it drift and fighting the drift — is the whole chapter, in about 350 lines of numpy on top of last chapter's engine.
Build
Setup
import argparseimport jsonimport sysfrom pathlib import Path import numpy as npimport rerun as rr # Loose script from the repo root (same pattern as ch3.3); put the root on# sys.path so curriculum.common resolves. Pure numpy — no torch, no mujoco for# the dynamics — so every run is bitwise reproducible on CPU: same seed, same# bytes. np.linalg.solve is deterministic for the same inputs on one machine.sys.path.insert(0, str(Path(__file__).resolve().parents[3]))from curriculum.common.device import banner # noqa: E402 parser = argparse.ArgumentParser(description=__doc__)parser.add_argument("--seed", type=int, default=0, help="seeds the initial-angle jitter; the chain is chaotic so this diverges fast (still bitwise-deterministic)")parser.add_argument("--system", choices=["pendulum", "double", "triple"], default="double", help="a 1-, 2-, or 3-link chain of distance constraints")parser.add_argument("--integrator", choices=["euler", "semi_implicit", "rk4"], default="semi_implicit", help="ch3.3's integrators, reused verbatim; semi-implicit is the sane default for constraints")parser.add_argument("--stabilization", choices=["all", "none", "baumgarte"], default="all", help="'all' runs the none-vs-baumgarte comparison — the whole point")parser.add_argument("--baumgarte", type=float, default=20.0, help="Baumgarte frequency omega (rad/s); the feedback is 2*omega*gdot + omega^2*g, critically damped")parser.add_argument("--dt", type=float, default=0.005, help="timestep in sim seconds; the constrained system is stiff, so smaller than ch3.3")parser.add_argument("--steps", type=int, default=4000) # any laptop: millisecondsparser.add_argument("--smoke", action="store_true", help="fixed 800-step run for CI; two runs must match byte-for-byte")parser.add_argument("--device", choices=["cpu"], default="cpu", help="pure-numpy CPU; the flag exists for banner/tier parity")parser.add_argument("--out", type=Path, default=Path("outputs/ch3.4-constraints"))parser.add_argument("--rerun", dest="rerun", action="store_true", default=True) # recording is the default; opt OUT, not inparser.add_argument("--no-rerun", dest="rerun", action="store_false", help="skip the .rrd recording (CI smoke)")args = parser.parse_args() banner("ch3.4-constraints", device=args.device) # tier + measured wall-clock, printed firstnum_steps = 800 if args.smoke else args.steps # smoke length is FIXED so CI can diff runs exactlyargs.out.mkdir(parents=True, exist_ok=True)rng = np.random.default_rng(args.seed) # PCG64 — the only source of randomness in this fileSame house conventions as ch3.3: free-tier defaults first, a --smoke mode with
a fixed step count so CI can diff two runs byte-for-byte, no GPU tier, no
training.
| cpu-laptop | expected wall-clock on cpu-laptop: ~0.01 min (measured) | measured |
| mps | wall-clock on mps: not yet measured | pending |
| t4 | expected wall-clock on t4: ~0.03 min (measured) | measured |
| 4090 | wall-clock on 4090: not yet measured | pending |
The whole run is about 0.01 minutes — a chain of point masses and one small
linear solve per step is microseconds of arithmetic; there is nothing to wait
for. One flag does more work than its size suggests: --seed nudges exactly one
knob, the launch angle, by at most 0.05 radians — the same way ch3.3's seed
nudged the orbit speed. Because the double pendulum is chaotic, that hair's-width
nudge diverges fast, yet each seed's run is still bitwise reproducible. Hold onto
that; it is the whole of exercise 3. The other knobs carry the lesson directly:
--stabilization (the comparison), --baumgarte (the gain you will learn to
distrust), and --dt (shrink it and watch the whole thing converge).
The systems
Last chapter a "system" was one body. Now it is a chain: N point masses,
each tied to its neighbor by a distance lock. We keep ch3.3's (q, v, m) shape
and simply stack it — q and v become (N, 3), m becomes (N, 1) so
force / m still broadcasts per particle. The genuinely new fields are the
constraint topology: pairs, which particle is roped to which, and lengths,
each link's rest length.
# A "system" is now a CHAIN of point masses linked by distance constraints. We# keep ch3.3's (q, v, m) shape and generalize it to N particles: q and v are# (N, 3) stacks of positions/velocities, m is (N, 1) so force / m broadcasts per# particle exactly as ch3.3's scalar divide did. The new fields are the# constraint TOPOLOGY: `pairs` (which particle is tied to which; the pivot end# is a fixed anchor, encoded as partner -1) and `lengths` (each link's rest# length L). Motion lives in the x-y plane (gravity along -y) so a swinging# chain reads as an (x, y) curve in rerun, same as ch3.3's planar orbit. PIVOT = np.array([0.0, 0.0, 0.0]) # the fixed anchor the first link hangs fromGRAVITY = np.array([0.0, -9.81, 0.0]) def build_system(name: str, rng: np.random.Generator) -> dict: """Return {q0, v0, m, pairs, lengths} for an n-link pendulum chain. Links start stretched along +x (max gravitational torque — the dramatic drop) with the whole chain tilted by a small seeded angle. The seed jitters ONE knob (that launch angle) so --seed is load-bearing; because the chain is chaotic the trajectory diverges across seeds, but the drift ORDERING does not (naive drifts, stabilized holds — checked seeds 0/1/2). """ n_links = {"pendulum": 1, "double": 2, "triple": 3}[name] tilt = rng.uniform(-0.05, 0.05) # radians off horizontal; the only randomness direction = np.array([np.cos(-tilt), np.sin(-tilt), 0.0]) # slightly below +x length = 1.0 # every link has rest length 1 q0 = np.array([PIVOT + (i + 1) * length * direction for i in range(n_links)]) v0 = np.zeros((n_links, 3)) # released from rest m = np.ones((n_links, 1)) # unit point masses # Link k ties particle k to its predecessor; particle 0's predecessor is the # fixed pivot, encoded as partner index -1. pairs = [(i, i - 1) for i in range(n_links)] # (i, -1) for the first link lengths = np.full(n_links, length) return {"q0": q0, "v0": v0, "m": m, "pairs": pairs, "lengths": lengths} def external_force(system: dict, q: np.ndarray) -> np.ndarray: """Gravity on every particle: m * g. Shape (N, 3), same as ch3.3's force.""" return system["m"] * GRAVITY # (N,1) * (3,) -> (N,3)Three systems, all the same code with a different link count: a pendulum
(one link), a double pendulum (two — the chaotic classic), and a triple.
They start stretched out along the x-axis, horizontal, which is the position of
maximum gravitational torque — the dramatic drop. Gravity points along -y, so
the whole thing swings in the x-y plane and reads as a flat (x, y) curve in
rerun, exactly like ch3.3's orbit. external_force is just gravity, m * g,
returned in the same (N, 3) shape ch3.3's force returned — because to the
integrator, that is all a force is.
The constraint force
This is the new idea, and it is worth reading slowly. A distance constraint is
g_k(q) = ½(|d_k|² − L_k²) = 0
where d_k is the vector along link k. We want the constraint force that keeps
this true. The trick is to differentiate g = 0 with respect to time — twice.
Once gives the velocity condition ġ = J v = 0 (the link's length is not
changing), where J = ∂g/∂q is the constraint Jacobian, and for a distance
constraint J is just d written into the moving particle's slots. Twice gives
the acceleration condition:
g̈ = J a + J̇ v = 0
Now substitute the equation of motion a = M⁻¹(f_ext + Jᵀλ) — the constraint
force is Jᵀλ, where λ is an unknown vector of Lagrange multipliers, one
per constraint. The accelerations drop out and you are left with a small linear
system in λ alone:
(J M⁻¹ Jᵀ) λ = −(J M⁻¹ f_ext + J̇ v)
Solve it — the matrix is C × C for C constraints, symmetric positive
definite, two or three rows for our chains — and you have the multipliers. Apply
Jᵀλ as an added force, and the integrator does the rest. That last sentence is
the entire seam with chapter 3.3: a constraint is a force you add, and the
integrators from last chapter never learn that force now hides a linear solve.
# THE new idea. A distance constraint reads g_k(q) = 1/2 (|d_k|^2 - L_k^2) = 0,# where d_k is the vector along link k (bob minus pivot, or bob minus bob). Its# Jacobian J_k = dg_k/dq is just d_k on the moving end(s). Differentiate the# constraint twice — gdot = J v, gddot = J a + Jdot v — set gddot = 0, and the# unknown constraint force J^T lambda drops out of a small linear system:# (J M^-1 J^T) lambda = -(J M^-1 f_ext + Jdot v)# Solve for the Lagrange multipliers lambda, apply J^T lambda as an added force.# For a distance constraint Jdot v works out to |d_dot|^2 = |v_i - v_j|^2. def constraint_terms(system: dict, q: np.ndarray, v: np.ndarray): """Assemble J and the per-constraint g, gdot, and Jdot v for the current state. J is (C, 3N) — one row per constraint, the link vector written into the moving particle's 3 columns (and minus it into its partner's, if not the pivot). g is the position error, gdot = J v the velocity error, jdotv the velocity-squared term that makes gddot = J a + Jdot v exact. """ pairs, lengths = system["pairs"], system["lengths"] n_particles, n_con = q.shape[0], len(pairs) jac = np.zeros((n_con, 3 * n_particles)) g = np.zeros(n_con) gdot = np.zeros(n_con) jdotv = np.zeros(n_con) for k, (i, j) in enumerate(pairs): if j < 0: # first link: partner is the fixed pivot (zero velocity) d, dv = q[i] - PIVOT, v[i] jac[k, 3 * i:3 * i + 3] = d else: # link between two moving bobs d, dv = q[i] - q[j], v[i] - v[j] jac[k, 3 * i:3 * i + 3] = d jac[k, 3 * j:3 * j + 3] = -d g[k] = 0.5 * (d @ d - lengths[k] ** 2) # 0 when the link is exactly length L gdot[k] = d @ dv # = J v for this row jdotv[k] = dv @ dv # Jdot v = |d_dot|^2 return jac, g, gdot, jdotv def constraint_force(system: dict, q: np.ndarray, v: np.ndarray, baumgarte: float) -> np.ndarray: """Solve for the Lagrange multipliers and return the constraint force (N, 3). baumgarte = 0 is the NAIVE solve (enforce gddot = 0 and hope g stays 0). A positive omega replaces gddot = 0 with the critically-damped target gddot + 2*omega*gdot + omega^2*g = 0, feeding the drift back as a restoring term — the whole difference between a length that creeps and one that holds. """ jac, g, gdot, jdotv = constraint_terms(system, q, v) minv = np.repeat(1.0 / system["m"].reshape(-1), 3) # diag(M^-1) as a (3N,) vector f_ext = external_force(system, q).reshape(-1) # (3N,) a_mat = jac @ (minv[:, None] * jac.T) # J M^-1 J^T, (C, C), SPD b = -(jac @ (minv * f_ext) + jdotv) # naive right-hand side if baumgarte > 0.0: b -= 2.0 * baumgarte * gdot + baumgarte ** 2 * g # Baumgarte feedback lam = np.linalg.solve(a_mat, b) # the Lagrange multipliers return (jac.T @ lam).reshape(q.shape) # J^T lambda, back to (N, 3) def make_force(system: dict, baumgarte: float): """Bundle gravity + the solved constraint force into ch3.3's force(q, v) shape. THIS is the seam: the integrator sees one force law; inside it, the constraint is an added term solved on the fly. Nothing in the integrators changes from ch3.3. """ def force(q, v): return external_force(system, q) + constraint_force(system, q, v, baumgarte) return forceThe J̇ v term is the one piece of calculus you cannot skip: for a distance
constraint it works out to |ḋ|², the squared relative velocity across the link.
Drop it and you are solving the wrong equation. (Exercise 2 makes you supply it.)
The integrators, unchanged
# ch3.3's three integrators, reused VERBATIM (the doctrine's "repetition is the# lesson"): they advance (q, v) by dt given any force law and mass, and never# knew or cared that `force` now hides a linear solve. q, v are (N, 3); m is# (N, 1); force returns (N, 3); every operation below broadcasts per particle. def euler_step(q, v, m, force, dt): """Explicit (forward) Euler: everything evaluated at the OLD state.""" a = force(q, v) / m return q + dt * v, v + dt * a def semi_implicit_step(q, v, m, force, dt): """Semi-implicit (symplectic) Euler: new velocity first, then step position with it. ch3.3's bounded-energy default; the sane base for constraints too. """ a = force(q, v) / m v_next = v + dt * a q_next = q + dt * v_next # NEW velocity, not old return q_next, v_next def rk4_step(q, v, m, force, dt): """Classical RK4 on (q, v)' = (v, force / m). Four force samples per step — which here means four constraint solves per step, all handled by `force`. """ def deriv(q, v): return v, force(q, v) / m k1q, k1v = deriv(q, v) k2q, k2v = deriv(q + 0.5 * dt * k1q, v + 0.5 * dt * k1v) k3q, k3v = deriv(q + 0.5 * dt * k2q, v + 0.5 * dt * k2v) k4q, k4v = deriv(q + dt * k3q, v + dt * k3v) q_next = q + (dt / 6.0) * (k1q + 2.0 * k2q + 2.0 * k3q + k4q) v_next = v + (dt / 6.0) * (k1v + 2.0 * k2v + 2.0 * k3v + k4v) return q_next, v_next INTEGRATORS = {"euler": euler_step, "semi_implicit": semi_implicit_step, "rk4": rk4_step}COLORS = {"none": [217, 76, 64], "baumgarte": [76, 175, 80]} # red drifts, green holdsThese are ch3.3's three integrators, copied in verbatim — the repetition is the
point. They take (q, v), a mass, and a force, and step forward. They do not
know and do not care that force now solves a linear system on every call (RK4
solves it four times per step, once at each stage — which is exactly correct).
Semi-implicit Euler is the sane default for constraints, so it is the default
here.
Simulate and measure
# Run one (integrator, stabilization) choice over the whole horizon and record,# at every step: WHERE the last bob is (the trajectory), how badly the links are# STRETCHED (the constraint violation — the honesty metric, parallel to ch3.3's# energy), and the total ENERGY (which a good integrator keeps bounded even for# the chaotic double pendulum). Pure function of its inputs — bitwise reproducible. def energy(system: dict, q: np.ndarray, v: np.ndarray) -> float: """Total mechanical energy: kinetic + gravitational potential (U = -m g.q). Ideal constraint forces do NO work (they act along the link, perpendicular to allowed motion), so a perfect solve conserves this. Drift here is a second, independent honesty check on the constraint force. """ kinetic = 0.5 * float(np.sum(system["m"] * v * v)) potential = -float(np.sum(system["m"] * (q @ GRAVITY)[:, None])) return kinetic + potential def constraint_violation(system: dict, q: np.ndarray) -> float: """Worst link-length error over the chain: max_k | |d_k| - L_k |. This is the number the whole chapter turns on. Zero means every link is exactly its rest length; a growing value is the pendulum literally coming apart in the arithmetic. """ worst = 0.0 for k, (i, j) in enumerate(system["pairs"]): d = q[i] - (PIVOT if j < 0 else q[j]) worst = max(worst, abs(float(np.linalg.norm(d)) - system["lengths"][k])) return worst def simulate(step_fn, system: dict, force, dt: float, steps: int): """Integrate for `steps`. Returns (tip_trajectory, violations, energies). tip_trajectory: (steps + 1, 3) — the LAST bob's path (what you watch swing). """ q = system["q0"].astype(float).copy() v = system["v0"].astype(float).copy() m = system["m"] tip = np.empty((steps + 1, 3)) violations = np.empty(steps + 1) energies = np.empty(steps + 1) tip[0], violations[0], energies[0] = q[-1], constraint_violation(system, q), energy(system, q, v) for i in range(steps): q, v = step_fn(q, v, m, force, dt) tip[i + 1] = q[-1] violations[i + 1] = constraint_violation(system, q) energies[i + 1] = energy(system, q, v) return tip, violations, energies def log_rerun(name: str, tip: np.ndarray, violations: np.ndarray, energies: np.ndarray) -> None: """Draw the tip's path (static) and stream the constraint-violation and energy curves for one stabilization choice — the honesty curves, overlaid. """ rr.log(f"world/{name}", rr.LineStrips3D([tip], colors=[COLORS[name]]), static=True) for i, (viol, e) in enumerate(zip(violations, energies)): rr.set_time("step", sequence=i) rr.log(f"violation/{name}", rr.Scalars([float(viol)])) # the hero panel rr.log(f"energy/{name}", rr.Scalars([float(e)]))The loop is ch3.3's, and it records two honesty numbers at every step. The first
is the constraint violation: the worst link-length error over the chain,
max_k | |d_k| − L_k |. Zero means every rod is exactly its rest length; a
growing value is the pendulum literally stretching. This is the chapter's headline
metric, and it plays exactly the role energy drift played last chapter. The
second is the total energy, which a correct constraint force leaves alone —
constraint forces act along the rod, perpendicular to the motion they permit, so
they do no work. Energy is a second, independent check that the solve is right.
The measurement
The report region is the driver. It runs the chosen stabilization mode — or, by default, both — records the honesty curves to rerun, and prints the comparison that is this chapter's deliverable.
# Run the chosen stabilization(s), print the drift table, write metrics.json.# The deliverable is the comparison: WITHOUT stabilization the max link-length# error is orders of magnitude larger than WITH it — the measured reason a real# engine never trusts the naive acceleration-level solve alone.modes = {"none": 0.0, "baumgarte": args.baumgarte}names = list(modes) if args.stabilization == "all" else [args.stabilization]system = build_system(args.system, rng)step_fn = INTEGRATORS[args.integrator] if args.rerun: rr.init("zero2robot/ch3.4-constraints", spawn=False) rr.save(str(args.out / "constraints.rrd")) rr.log("world/pivot", rr.Points3D([PIVOT], radii=[0.03]), static=True) results: dict[str, dict] = {}# Characteristic energy scale for the drift ratio: the chain starts at y ~ 0 so# its initial energy is ~0 — a useless normalizer. m*g*L (summed) is the natural# scale of the potential swings, and it is fixed, so energy_rel_max is honest.escale = float(np.sum(system["m"]) * np.linalg.norm(GRAVITY) * np.sum(system["lengths"]))for name in names: force = make_force(system, modes[name]) tip, violations, energies = simulate(step_fn, system, force, args.dt, num_steps) results[name] = { "max_violation": float(np.max(violations)), "final_violation": float(violations[-1]), "energy_rel_max": float(np.max(np.abs(energies - energies[0])) / escale), "tip_final": [round(float(x), 6) for x in tip[-1]], } if args.rerun: log_rerun(name, tip, violations, energies) metrics = { "system": args.system, "integrator": args.integrator, "dt": args.dt, "steps": num_steps, "seed": args.seed, "baumgarte": args.baumgarte, # 12 decimals: a well-stabilized run's violation is ~1e-4 and rounding it to # fewer digits would erase the signal the exercises measure. "results": {k: {kk: (round(vv, 12) if isinstance(vv, float) else vv) for kk, vv in v.items()} for k, v in results.items()},}(args.out / "metrics.json").write_text(json.dumps(metrics, indent=2, sort_keys=True) + "\n") sim_seconds = num_steps * args.dtprint(f"integrated {args.system} ({args.integrator}) for {num_steps} steps of {args.dt} s -> {sim_seconds:.2f} s of sim time")print(f"{'stabilization':<16}{'max |len err|':>16}{'final |len err|':>18}{'energy rel_max':>16}")for name in names: r = results[name] print(f"{name:<16}{r['max_violation']:>16.4e}{r['final_violation']:>18.4e}{r['energy_rel_max']:>16.4e}")if args.stabilization == "all": # The headline, stated as an ordering so it survives any seed on any tier # (measured factor is 5x-17x across pendulum/double/triple; 3x is the robust floor). ok = results["none"]["max_violation"] > 3.0 * results["baumgarte"]["max_violation"] verdict = "as expected" if ok else "UNEXPECTED — investigate" print(f"constraint drift none >> baumgarte (stabilization holds the links): {verdict}")print(f"metrics: {args.out / 'metrics.json'}")if args.rerun: print(f"recording: {args.out / 'constraints.rrd'} — open it with: rerun {args.out / 'constraints.rrd'}")Run the default double pendulum for twenty seconds of sim time, once with no stabilization and once with it, and read the worst link-length error:
python constraints.py --seed 0
| stabilization | worst link-length error | final link error | energy drift |
|---|---|---|---|
| none | 0.384 | 0.384 | 0.43 |
| Baumgarte | 0.023 | 0.0014 | 0.32 |
The naive solve enforced g̈ = 0 — it froze the acceleration of the length
error — but it never enforced g = 0 itself. So the length error, tiny at first,
has nowhere to be pushed back to. Each step's small discretization error in g
survives into the next, and they pile up: 0.384, meaning the worst rod is 38%
off its length. The worst error equals the final error, which means it never came
back — it is still climbing when the run ends. This is the stretching pendulum
from the opening.
Fixing the drift: Baumgarte stabilization
The fix is due to Baumgarte (1972) and is one line. Instead of aiming for
g̈ = 0, aim for a g that decays back to zero like a critically damped spring:
g̈ + 2ω ġ + ω² g = 0
The ω² g term pushes an existing position error back toward zero; the 2ω ġ
term damps it so it does not overshoot. Fold those two terms into the
right-hand side of the same linear solve and the worst error drops from 0.384 to
0.023 — sixteen times smaller — and the final error settles to 0.0014 and
stays there. The rods hold. That factor is seed-robust: across seeds 0/1/2 on the
pendulum, double, and triple, the naive error runs 5× to 17× the stabilized one.
The ordering is the rock — trust it before you trust any single exact number.
Is the math even right?
A wrong constraint solver can look plausible and still be nonsense, so verify it. Run the single pendulum under RK4 with no stabilization at all:
python constraints.py --system pendulum --integrator rk4 --stabilization none
The worst link error is 8.8e-6 and the energy drift is 1.2e-5 — the
naive solve, given an accurate enough integrator, keeps the rod to five decimal
places and conserves energy to five. Both errors shrink as you shrink dt
(semi-implicit roughly first-order, RK4 far steeper). That convergence is the
proof: the drift is discretization error going to zero the way it should, not a
bug. The Jacobian and the multipliers are correct. What drifts in the default run
is not the physics; it is cheap first-order integration of a stiff system — and
that is precisely the regime a real-time engine lives in.
The honest cost, and why MuJoCo looks the way it does
Baumgarte is not free, and the honesty of this chapter is in saying so. Look at
the energy column: stabilization holds the rod but its feedback force does a
little work, so energy conservation is only slightly better, not perfect. And
ω is a knob you had to pick — 20 worked; too large and dt·ω approaches 1 and
the correction itself goes unstable; too small and the drift creeps back. You
have traded a drift problem for a tuning problem, and hidden a small energy leak
inside the fix.
This is the exact pain that shaped MuJoCo. Rather than enforce hard equality
constraints and paper over the drift with a hand-tuned Baumgarte gain, MuJoCo
uses soft constraints: it builds the same J M⁻¹ Jᵀ matrix you just built,
but regularizes it and solves a convex optimization whose stiffness and damping
are principled, physical parameters instead of a magic ω. Every strange-looking
choice in MuJoCo's constraint documentation — softness, the reference
acceleration, the impedance — is an answer to a problem you just felt in your own
hands. That is the "read the real thing" segment for this chapter.
One honest boundary on what you built, since a robotics reader will ask. We solved
the constraint at the acceleration level — enforce g̈, hand the force to the
integrator — because it keeps the Lagrange-multiplier spine bare and reuses ch3.3's
integrators untouched. Real-time engines more often solve at the velocity level,
trading multipliers for impulses, and articulated mechanisms are usually handled by
Featherstone's articulated-body algorithm, which scales linearly in the number of
links instead of paying for a dense J M⁻¹ Jᵀ solve. Same physics, different
bookkeeping; the acceleration-level view is the one that makes the constraint force
visible as a force, which is the thing this chapter is for.
Break It
Two seeds, 0 and 1, differing by at most 0.05 radians at launch. Run the double pendulum at each and look at where the tip finishes after twenty seconds: the two tips end up 1.1 units apart — an order-of-one difference from a hair's-width start. That is chaos, and this little engine reproduces it faithfully.
Now run seed 0 twice. The two runs are bitwise identical, down to the last bit of the last float. Both things are true at once: the double pendulum is deterministic (same seed, same bytes, always) and unpredictable (a negligible change in the start becomes a total change in the finish). Those are not the same word, and conflating them is one of the most common confusions in all of simulation. Determinism is a property of the arithmetic; predictability is a property of the system. Exercise 3 makes you watch both at once.
Read the real thing
The chapter kept promising this, so here it is: the exact J M⁻¹ Jᵀ you built by
hand, sitting inside a production solver, doing everything you did and four things
you skipped.
Start with your own file. The whole idea lives in constraints.py's
#constraints region — constraint_force assembles J M⁻¹ Jᵀ, forms the
right-hand side −(J M⁻¹ f_ext + J̇v), folds in the Baumgarte feedback
2ω ġ + ω² g, and hands the result to one np.linalg.solve. One dense symmetric
solve, one hard equality per rod, one hand-tuned ω. That is the teaching skeleton.
Now the real one. The course pins google-deepmind/mujoco at tag 3.10.0, and
the constraint system is built in src/engine/engine_core_constraint.c (verified
at that tag, ~2869 lines). mj_makeConstraint is the top-level driver;
mj_projectConstraint forms the same J M⁻¹ Jᵀ you did, storing it as efc_AR.
But look at what wraps it. mj_makeImpedance fills efc_R — a diagonal
regularizer added to the diagonal, so the system actually solved is
J M⁻¹ Jᵀ + R, not the bare matrix — and mj_referenceConstraint computes
efc_aref = −B·vel − K·(pos − margin). Read that formula against your Baumgarte
term. It is the same shape — a stiffness on position error, a damping on velocity
error — except K and B are not a magic ω you guessed; they come from the
physical solref/solimp parameters (mj_assignRef, mj_assignImp), so the
Baumgarte gain becomes a time constant and damping ratio you can actually reason about.
One honesty note on the map. meta.yaml's focus reads "engine_core_constraint /
the PGS-or-CG solver," but at 3.10.0 those are two files: the constraint system is
built in engine_core_constraint.c, while the solve iterations live next door
in src/engine/engine_solver.c — mj_solPGS, mj_solCG, and mj_solNewton
(Newton is MuJoCo's default). Same pipeline, two stops.
What they add, and why. Your one dense np.linalg.solve becomes three iterative
solvers that scale to thousands of constraints without ever forming a dense
matrix. Your one constraint type — a rod — becomes the full model: equality welds,
joint limits, dry friction, and the one-sided contacts of chapter 3.5, all packed
into the same efc_* arrays. And the + R regularization is this chapter's last
section made real — because the matrix is regularized, the constraint is soft,
the solve is a convex optimization that always has an answer, and drift is bounded
by physical stiffness you set rather than a gain you tune until it stops exploding.
You built the spine; they armored it.
Read next, in order: src/engine/engine_core_constraint.c — find
mj_referenceConstraint and read efc_aref against your Baumgarte line; then
mj_makeImpedance for where efc_R and the soft K, B come from; then
src/engine/engine_solver.c's mj_solNewton to watch the regularized
J M⁻¹ Jᵀ + R finally get solved.
What you built, and what comes next
You built joints. Given a rule the state must obey, you can now solve for the force that enforces it, apply it as one more term in ch3.3's force law, and measure — honestly — how well it holds. You met the drift that every hard constraint suffers and the stabilization that tames it, along with the bill that stabilization quietly runs up.
Chapter 3.5 takes the last and hardest step: contact. A joint is a constraint that is always active — the rod is always exactly length L. A contact is a constraint that switches on and off — the foot pushes on the floor only when it is touching the floor, and never pulls. That one-sidedness (an inequality, not an equality) is where physics engines get genuinely hard, and where the sim artifacts you have watched since chapter 0.1 finally get explained.