Testing and validation

For V3, I wrote a full design document before writing any code. The target was concrete: simulate my club's six-robot arena of holonomic turret robots fast enough to train RL policies on a laptop CPU, since we don't have much GPU compute. Training takes millions of steps, so throughput was the goal, and V3 doubled as a performance learning exercise. One rule drove most decisions: only costs paid every frame matter. Only joint state, plus the contact solver's last solution, persists between frames. Everything else is either static data built at load time or scratch memory rebuilt each step and reused between passes. That rule became three pieces: fields; small operations that declare exactly which fields they read and write; and traversals that apply those operations across the joint tree, all checked at compile time. What the compiler can't check, the engine checks as it starts, and CI runs death tests confirming that a bad setup, like a model that doesn't fit its layout or an overflowing arena, stops it with a clear message instead of quietly corrupting a run. I kept V2's physically meaningful types but moved them into the math rather than the structure, so adding collisions or rendering meant adding passes instead of rewiring classes.

V2's dynamics were wrong in ways only a reference could show: brought back to life next to MuJoCo, a one-link arm swung exactly 4× too fast (it used the free-body inverse inertia, dropping the parallel-axis term), and a two-link arm moved in the wrong shape. V3 runs the full Articulated Body Algorithm, with Coriolis and centrifugal bias terms, parent-to-body transforms and gravity as a fictitious base acceleration, and steps it forward with semi-implicit Euler, the same default MuJoCo uses. Here all three drive the same arm with the same shoulder torque and no gravity, since V2 never had any. V3 stays within 3×10⁻⁵ rad of MuJoCo for all 4 seconds, while V2 has swung the shoulder 4.5 times as far and folded the elbow forward instead of letting it trail behind.

MuJoCo is only a reference, so I also checked V3 against an exact answer. A double pendulum is chaotic and has no closed-form trajectory: two simulations that differ by a single rounding error eventually go completely different ways. Its equations of motion do have a closed form, though. Lagrangian mechanics reduces them to a 2×2 linear system that gives the exact joint accelerations at any given position and velocity. I derived that system independently of the engine's code, then compared it with V3's accelerations at every state of a 10-second run. Because each state is checked on its own, errors can't build up and chaos can't amplify them. The two agree to within 3×10⁻⁶, which is float32 round-off, and V3 matches MuJoCo to the same 3×10⁻⁶.

Detecting contacts is only half the battle; working out what happens next is the rest. Once accelerations and contacts are known, the solver adjusts the accelerations so bodies respect their contacts. It uses a "soft contact" model in which each contact acts like a stiff spring-damper, pushing harder the deeper and faster two bodies collide. Friction follows Coulomb's law, whose cone of allowed forces is approximated by a four-sided pyramid of push-only edges, letting bodies grip or slide. Each frame, the solver minimizes

½ (a − a₀)ᵀ M (a − a₀) + Σᵢ ½ Dᵢ (Jᵢ a − a_ref,ᵢ)²,

staying close to the unconstrained acceleration a₀ while penalizing contact violations. It warm-starts from the previous frame's solution when that costs less, then runs a fixed 20 conjugate-gradient iterations to suit SIMD and GPUs, never building the mass matrix. The corrected acceleration then goes to the integrator.

A two-arm plan is only safe if both arms run it together: it was checked with each arm where the other would be at the same instant. On hardware, though, mj_manipulator_ros, the lab's layer that runs those plans on the robot, ran multi-arm plans one arm after the other, which doubled the time and passed through states the planner never checked. Replaying the crossing plan that way in MuJoCo and checking every fourth of its states, the arms intersect in 97 of 1,400.

I rebuilt execution so every arm starts its trajectory on one shared start stamp: the trajectories must share their timestamps, one stamp a moment in the future is computed once, and each arm sends its goal from its own thread against that stamp, so the motors start together however long each goal takes to send. If any arm fails or is aborted, a shared event cancels the others mid-motion. Running two arms from two threads also meant making the shared code thread-safe: both threads stepped one MuJoCo model, and concurrent runs crashed until I serialized every access to it behind one lock. To bring it all up on the real robot safely, I built a hardware-in-the-loop bring-up protocol.