Build a Physics Engine IUnconstrained Dynamics

The problem

Since chapter 0.1 you have called mj_step and trusted it. Write data.ctrl, call the function, read the new qpos. Thirty-three chapters of robots stand on top of that one call, and you have never once looked inside it.

Here is the question that separates using a simulator from understanding one: when mj_step advances a floating body by one timestep, what arithmetic does it actually do — and what does that arithmetic get wrong? Because it does get something wrong. Every numerical integrator does. A simulator is a machine for being wrong in a controlled, measurable way, and the entire craft of physics simulation is choosing which way to be wrong.

So we build the inside of mj_step — the unconstrained part, no contacts yet — from scratch in numpy. No MuJoCo in the dynamics; that would be circular. Just state, a force law, and the integrator, in about 270 lines. Then we measure the wrongness with the one number that cannot be argued with: energy.

Build

Setup

import argparseimport jsonimport sysfrom pathlib import Path import numpy as npimport rerun as rr # Chapter artifacts run as loose scripts from the repo root; put the root on# sys.path so `curriculum.common` resolves (same pattern as ch0.1). This chapter# is pure numpy — no torch, no mujoco — so the whole run is bitwise deterministic# on CPU: same seed, same bytes, every time (the exercises lean on that).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-condition jitter (orbit eccentricity, spring stretch)")parser.add_argument("--system", choices=["orbit", "spring", "freefall"], default="orbit", help="which conservative system to integrate")parser.add_argument("--integrator", choices=["all", "euler", "semi_implicit", "rk4"], default="all", help="'all' runs the three-way comparison — the whole point")parser.add_argument("--dt", type=float, default=0.01, help="timestep in sim seconds; raise it and every integrator's drift grows")parser.add_argument("--steps", type=int, default=2000)  # any laptop: microsecondsparser.add_argument("--smoke", action="store_true", help="fixed 600-step run for CI; two runs must match byte-for-byte")parser.add_argument("--device", choices=["cpu"], default="cpu", help="this engine is pure-numpy CPU; the flag exists for banner/tier parity")parser.add_argument("--out", type=Path, default=Path("outputs/ch3.3-engine"))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.3-engine", device=args.device)  # tier + measured wall-clock, printed firstnum_steps = 600 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 file

The flags follow the house convention: free-tier defaults first, a --smoke mode that runs a fixed number of steps so CI can compare two runs byte-for-byte. This chapter has no GPU tier and no training — it is pure numpy integration, and the full run takes 0.01 minutes on a laptop; there is nothing to wait for. The seed here controls initial conditions only — the orbit's eccentricity, the spring's stretch, the launch angle — exactly as chapter 0.1's seed controlled the shove; the physics is otherwise deterministic and the whole file is bitwise reproducible. The knobs that matter pedagogically are --dt and --steps: they are not a scaling ramp, they are the experiment.

The systems

A physics engine needs three things from you: where everything starts, how hard things push on each other, and — if you want to grade yourself — what quantity ought to stay constant. We bundle those as a system: an initial position and velocity, a force(q, v) law, and the true total energy(q, v).

# A "system" is the physics: an initial state (position q, velocity v, mass m),# a FORCE law force(q, v) -> np.ndarray, and the true total ENERGY energy(q, v)# that a perfect integrator would hold constant forever. Positions and# velocities are 3-vectors so a planar orbit reads as an (x, y) curve in rerun# and a body could later swing in 3D. Chapter 3.4 keeps this exact (q, v, m,# force) shape and adds a constraint force term — nothing here has to change.  def build_system(name: str, rng: np.random.Generator) -> dict:    """Return {q0, v0, m, force, energy} for a conservative test system.     The seed jitters ONE initial-condition knob per system (orbit shape, spring    stretch, launch angle) so `--seed` is load-bearing while the physics — and    therefore the energy-drift ordering — stays the same across seeds.    """    if name == "orbit":        # A unit point mass orbiting a fixed attractor at the origin under an        # inverse-square pull (Newtonian gravity, GM = mu = 1). Circular when        # speed == 1; the seed nudges the speed so the true orbit is a closed        # ellipse — the honest baseline the spiral of Euler will violate.        mu, m = 1.0, 1.0        speed = 1.0 + rng.uniform(-0.05, 0.05)        q0 = np.array([1.0, 0.0, 0.0])        v0 = np.array([0.0, speed, 0.0])         def force(q, v):            return -mu * m * q / (np.linalg.norm(q) ** 3)  # points at the attractor, ~1/r^2         def energy(q, v):            return 0.5 * m * (v @ v) - mu * m / np.linalg.norm(q)  # kinetic + gravitational potential     elif name == "spring":        # A mass on an ideal (Hooke) spring: force pulls straight back toward        # the origin, strength proportional to stretch. Undamped, so it should        # oscillate at fixed amplitude — total energy dead flat — forever.        k, m = 4.0, 1.0  # angular frequency sqrt(k/m) = 2 rad/s        stretch = 1.0 + rng.uniform(-0.1, 0.1)        q0 = np.array([stretch, 0.0, 0.0])        v0 = np.array([0.0, 0.0, 0.0])         def force(q, v):            return -k * q  # Hooke's law: restoring, linear in displacement         def energy(q, v):            return 0.5 * m * (v @ v) + 0.5 * k * (q @ q)  # kinetic + spring potential     else:  # freefall — a non-oscillatory sanity check (see the honest caveat below)        # A body launched under constant gravity. Energy is conserved too, but        # the motion never RETURNS, so the symplectic advantage does not show:        # every integrator's energy error here grows linearly and one-signed.        # That contrast is the lesson — drift is about BOUNDED systems.        g, m = 9.81, 1.0        vx = 3.0 + rng.uniform(-0.5, 0.5)        q0 = np.array([0.0, 0.0, 10.0])        v0 = np.array([vx, 0.0, 0.0])        gravity = np.array([0.0, 0.0, -g])         def force(q, v):            return m * gravity  # constant downward pull         def energy(q, v):            return 0.5 * m * (v @ v) + m * g * q[2]  # kinetic + height potential     return {"q0": q0, "v0": v0, "m": m, "force": force, "energy": energy}

We give you three. An orbit: a point mass pulled toward a fixed attractor by an inverse-square law — Newtonian gravity, the two-body problem. A spring: a mass pulled back toward the origin in proportion to its stretch, Hooke's law, the harmonic oscillator that every branch of physics reduces to eventually. And a freefall: a body under constant gravity, which we include precisely because it will break the neat story the other two tell — hold that thought.

Read the shape of the state, because chapter 3.4 inherits it exactly. Position q, velocity v, mass m, and a force that is a pure function of the two. When 3.4 adds constraints — a joint, a rod, a floor the body cannot pass through — it adds a constraint force to this same force, and nothing else in the file has to move. Unconstrained dynamics is the base case; constraints are a term you add.

The integrators

Here is the whole chapter in three functions. Each one takes the current (q, v) and returns the next (q, v), one timestep dt later. They differ by almost nothing — and the almost-nothing is everything.

# Each integrator advances (q, v) by one timestep dt given the force law and# mass. They differ only in WHICH state they evaluate the force/velocity at —# and that one difference is the whole difference between an orbit that holds# and an orbit that flies apart. All three are one screen of numpy.  def euler_step(q, v, m, force, dt):    """Explicit (forward) Euler: evaluate everything at the OLD state.     q_{n+1} = q_n + dt * v_n ;  v_{n+1} = v_n + dt * a_n. Simplest possible    update — and it feeds energy into a bounded system every single step.    """    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: update velocity FIRST, then step    position with the NEW velocity. One line reordered from explicit Euler,    and the payoff is bounded energy — this is (a cousin of) what MuJoCo uses.    """    a = force(q, v) / m    v_next = v + dt * a    q_next = q + dt * v_next  # <-- the whole trick: NEW velocity, not old    return q_next, v_next  def rk4_step(q, v, m, force, dt):    """Classical 4th-order Runge-Kutta on the first-order system    (q, v)' = (v, force(q, v) / m). Four force samples per step, weighted    1-2-2-1: far more accurate per step than Euler, but NOT symplectic — its    small error still accumulates one direction over long horizons.    """    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 = {"euler": [217, 76, 64], "semi_implicit": [76, 175, 80], "rk4": [64, 115, 217]}

Explicit Euler is the one everyone writes first, because it is the definition of a derivative with the limit removed: new position is old position plus velocity times dt; new velocity is old velocity plus acceleration times dt. Everything on the right-hand side is the old state. It is correct in the limit dt → 0 and wrong for every dt you can actually afford.

Semi-implicit Euler changes one line. Update the velocity first — then step the position using the new velocity, not the old one. That is the entire difference. It looks like a rounding-error-sized rearrangement. It is the difference between an orbit that holds for a million steps and an orbit that flies apart, and we are about to measure exactly that. This reordering is what makes the method symplectic, and it is a close cousin of what MuJoCo does by default. (Readers coming from a numerical-methods course will recognize it as the first-order member of the symplectic family whose better-known relative is leapfrog / Störmer–Verlet.)

RK4 takes the accuracy question seriously. Instead of one estimate of the derivative it takes four — at the start, twice at the midpoint, once at the end — and blends them 1-2-2-1. Per step it is dramatically more accurate than either Euler. It is also, and this is the twist the chapter turns on, not symplectic.

Simulate and measure

# Run one integrator over the whole horizon and record, at every step, WHERE# the body is (the trajectory) and HOW MUCH total energy it has (the honesty# metric). Pure function of its inputs — no rerun, no globals — so the smoke# run is bitwise reproducible and the exercises can call it directly.  def simulate(step_fn, system: dict, dt: float, steps: int):    """Integrate `system` for `steps` steps. Returns (trajectory, energies).     trajectory: (steps + 1, 3) positions; energies: (steps + 1,) total energy.    """    q = system["q0"].astype(float).copy()    v = system["v0"].astype(float).copy()    m, force, energy = system["m"], system["force"], system["energy"]     trajectory = np.empty((steps + 1, 3))    energies = np.empty(steps + 1)    trajectory[0], energies[0] = q, energy(q, v)    for i in range(steps):        q, v = step_fn(q, v, m, force, dt)        trajectory[i + 1], energies[i + 1] = q, energy(q, v)    return trajectory, energies  def energy_drift(energies: np.ndarray) -> dict:    """Relative energy error vs the starting energy — the drift numbers.     rel_final: signed drift at the end (Euler: large +, semi-implicit: ~0).    rel_max:   worst absolute excursion over the whole run (bounded for a               symplectic integrator, unbounded for explicit Euler).    """    e0 = energies[0]    scale = abs(e0) if e0 != 0.0 else 1.0    return {        "e0": float(e0),        "e_final": float(energies[-1]),        "rel_final": float((energies[-1] - e0) / scale),        "rel_max": float(np.max(np.abs(energies - e0)) / scale),    }  def log_rerun(name: str, trajectory: np.ndarray, energies: np.ndarray) -> None:    """Draw the path (static) and stream energy-vs-step for one integrator."""    rr.log(f"world/{name}", rr.LineStrips3D([trajectory], colors=[COLORS[name]]), static=True)    for i, e in enumerate(energies):        rr.set_time("step", sequence=i)        rr.log(f"energy/{name}", rr.Scalars([float(e)]))  # overlaid drift curves, one per integrator

The loop is the ch0.1 rhythm with the black box removed: compute the force, step, record. What we record alongside the trajectory is the total energy at every step, because for all three of our conservative systems energy is exactly the thing that must not change. A perfect integrator would hold it flat. The amount it fails to is the energy drift, and we report it two ways: the signed drift at the end of the run, and the largest excursion anywhere along it.

Running all three and printing the comparison is the deliverable — three drift numbers that say, in order, "runs away", "holds", "holds best":

# Run the chosen integrator(s), print the drift table, and write metrics.json.# The comparison is the deliverable: three energy numbers that say, in order,# "runs away", "holds", "holds best" — the measured reason engines pick one.names = list(INTEGRATORS) if args.integrator == "all" else [args.integrator]system = build_system(args.system, rng) if args.rerun:    rr.init("zero2robot/ch3.3-engine", spawn=False)    rr.save(str(args.out / "engine.rrd"))    rr.log("world/attractor", rr.Points3D([[0.0, 0.0, 0.0]], radii=[0.03]), static=True)  # the orbit's focus drifts: dict[str, dict] = {}for name in names:    trajectory, energies = simulate(INTEGRATORS[name], system, args.dt, num_steps)    drifts[name] = energy_drift(energies)    drifts[name]["final_pos"] = [round(float(x), 6) for x in trajectory[-1]]    if args.rerun:        log_rerun(name, trajectory, energies) metrics = {    "system": args.system,    "dt": args.dt,    "steps": num_steps,    "seed": args.seed,    # 12 decimals, not fewer: RK4's drift is ~1e-11, and rounding it to zero    # would erase the very signal the dt-order exercise measures.    "drift": {k: {kk: round(vv, 12) if isinstance(vv, float) else vv for kk, vv in v.items()} for k, v in drifts.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} for {num_steps} steps of {args.dt} s -> {sim_seconds:.2f} s of sim time")print(f"{'integrator':<14}{'E0':>12}{'E_final':>14}{'rel_final':>14}{'rel_max':>14}")for name in names:    d = drifts[name]    print(f"{name:<14}{d['e0']:>12.4f}{d['e_final']:>14.4f}{d['rel_final']:>+14.4e}{d['rel_max']:>14.4e}")if args.integrator == "all" and args.system in ("orbit", "spring"):    # The headline, stated as an ordering so it survives any seed on any tier.    ok = abs(drifts["euler"]["rel_final"]) > abs(drifts["semi_implicit"]["rel_max"]) > drifts["rk4"]["rel_max"]    verdict = "as expected" if ok else "UNEXPECTED — investigate"    print(f"energy drift ordering  euler >> semi_implicit(bounded) > rk4:  {verdict}")print(f"metrics: {args.out / 'metrics.json'}")if args.rerun:    print(f"recording: {args.out / 'engine.rrd'} — open it with: rerun {args.out / 'engine.rrd'}")

The measurement

Run the orbit under all three integrators for twenty seconds of sim time:

python engine.py --seed 0

and read the drift (seed 0, CPU):

Read that table slowly, because three different things are true in it.

Explicit Euler gains 22% of its energy over three orbits, and the worst excursion equals the final one, which means it never came back — the error is monotone, one-directional, unbounded. More steps, more energy, wider spiral, forever. This is the third orbit from the opening.

Semi-implicit Euler's drift is four orders of magnitude smaller, but the number that matters is not its size, it is its shape: the worst excursion (0.032%) is larger than the final drift (0.0098%), which means the energy went up, came back, went down, came back. It oscillates around the true value and stays inside a band — a band that does not grow with the length of the run. That bounded-ness is the symplectic property, and it is worth more to a physics engine than raw accuracy, because it means a simulation can run indefinitely without cooking off or grinding to a halt.

RK4's drift is eleven orders of magnitude down — for this run it is exact to the precision we can measure. So why does anyone ever pick semi-implicit over it? Because RK4's tiny error, unlike semi-implicit's, is not bounded: it creeps one direction. On this twenty-second run you cannot see it. On a run a thousand times longer, the symplectic method's bounded wobble beats RK4's slow, honest creep. Accuracy per step and stability over time are different virtues, and an engine that must never fall over chooses the second. (Exercise 3 makes you watch RK4's error shrink by a factor of sixteen when you halve the timestep — fourth order — while Euler's only halves.)

Break It

Run the same comparison on freefall:

python engine.py --system freefall

and the neat story falls apart. Explicit Euler gains energy (+9.3%), semi-implicit loses the same amount (−9.3%), and neither is bounded. The symplectic advantage vanished.

It vanished because freefall never comes back. A body thrown under constant gravity recedes forever; there is no orbit to close, no oscillation to stay inside of. The bounded-energy property of symplectic integrators is a statement about periodic and bounded motion — orbits, springs, pendulums, walking gaits — not about a rock falling into infinity. This is the honest caveat, and it is why "semi-implicit conserves energy" is a claim you should never make without the word bounded and the word oscillatory attached. The integrator did not get better or worse; the system stopped being the kind of system the guarantee is about.

Read the real thing

The engine you have called since chapter 0.1 is C, and it is readable. The unconstrained update you just built by hand is the core of MuJoCo's mj_step, so read the original against your own three functions. Pinned to google-deepmind/mujoco at tag 3.10.0 — the version this course pins.

The integrator choice. Your report region hard-codes the three-way comparison; MuJoCo makes it a field. mjModel.opt.integrator is the int integrator; line of the option struct in include/mujoco/mjmodel.h, and its four legal values live in the mjtIntegrator enum in include/mujoco/mjtype.h: mjINT_EULER = 0, mjINT_RK4, mjINT_IMPLICIT, mjINT_IMPLICITFAST. (Heads-up: that enum moved in 3.x — older code and blog posts point at mjmodel.h; at 3.10.0 it is mjtype.h.) The default is value 0, and the source comments it in three words: // semi-implicit Euler. That is this chapter's headline, confirmed in the enum — of every integrator MuJoCo ships, the one it reaches for by default is your semi_implicit_step, not the "more accurate" RK4. mj_step in src/engine/engine_forward.c is the switch that dispatches on that field: mjINT_EULER → mj_Euler, mjINT_RK4 → mj_RungeKutta(m, d, 4).

mj_Euler. Your semi_implicit_step updates velocity, then steps position with the new velocity. mj_Euler (src/engine/engine_forward.c, forwarding to mj_EulerSkip) is that same method, and its comment says so word for word — "Euler integrator, semi-implicit in velocity." Its no-damping branch is your two lines. What it adds is the case your toy skips: when a DOF has damping it does not integrate velocity explicitly — it factorizes the mass matrix plus h·diag(B) and solves, integrating that damping implicitly for stability, plus actuator dynamics and sleep filtering. Same skeleton, hardened for a real robot.

mj_RungeKutta. Your rk4_step writes the 1-2-2-1 blend by hand. mj_RungeKutta in the same file drives it from a Butcher tableau (RK4_A, RK4_B) so one loop serves any order, re-runs the whole forward dynamics per stage through mj_forwardSkip (each of the four samples respects contacts and constraints — which your flat force law has none of), and advances position with mj_integratePos, which is quaternion-aware where yours is plain q + dt·v in Euclidean space.

What the pure-numpy version omits, then, is two things. First, the mjINT_IMPLICIT / mjINT_IMPLICITFAST integrators (mj_implicit / mj_implicitSkip, same file) — implicit-in-velocity methods that use the RNE force derivative to stay stable under stiff damping and gyroscopic terms, the precise stability-over-accuracy trade this chapter measured, taken one rung further than semi-implicit Euler. Second, the constraint solver whose qfrc_constraint feeds qacc before any integrator runs — that is chapter 3.4.

Read next, in order: include/mujoco/mjtype.h for the mjtIntegrator enum (your three functions, named); then mj_Euler / mj_EulerSkip in src/engine/engine_forward.c (your semi-implicit step, plus damping); then mj_RungeKutta in the same file (your RK4, generalized); and last the mj_step switch that ties the field to the function — the black box you have called since chapter 0.1, finally open.

What you built, and what comes next

You built the inside of mj_step for the case with no contacts: state, a force law, three integrators, and a measured, defensible reason to prefer one. Every integrator option in MuJoCo's documentation — Euler, implicit, implicitfast, RK4 — is a point on the trade you just measured by hand.

Chapter 3.4 keeps this exact (q, v, m, force) interface and asks the next question: what if the body cannot go where the force sends it — because it is bolted to a joint, or resting on a floor? That is a constraint, and a constraint is a force you solve for rather than write down. The base case is done.

Exercises

Three, in exercises/. One predict-then-run where you commit — before the code is allowed to answer — to which of the three integrators sends the orbit's energy climbing without bound. One code-completion: explicit Euler is handed to you whole, and you turn it into semi-implicit Euler and RK4 by changing exactly what this chapter says changes. And one more predict-then-run that halves the timestep and makes you match each integrator's error-shrink factor to its order — Euler's error roughly halving, RK4's dropping by about sixteen.