Kinematics & Dynamics

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.

With the accelerations right, longer runs exposed the next problem: semi-implicit Euler drifts. A frictionless double pendulum should conserve energy, yet over 10 seconds at a 2 ms step Euler's total energy wanders by up to 0.56 J. Euler updates the velocity from the current acceleration, then moves the position with that new velocity, which is simple but only first-order accurate. Velocity Verlet splits the velocity update into two half-steps around the position update and recomputes the accelerations in between, halving the drift to 0.28 J. Its weakness is that velocity-dependent forces like Coriolis are evaluated at an estimated velocity. Implicit midpoint evaluates the dynamics at the average of the old and new states, making each step time-symmetric. Because the new state isn't known in advance, it iterates four times to converge, and it stays within 0.0006 J. RK4 instead samples the dynamics four times across the step and blends the results, reaching fourth-order accuracy and staying within 0.0001 J.

Teleop and planning both lean on ssik, the lab's analytical IK solver, which I didn't write, and on the OpenArm it kept failing. I traced it to a singularity in the solver itself: the OpenArm's joint axes meet at shared points, so its link lengths are zero and every twist is a right angle, and that exact geometry breaks the Raghavan–Roth elimination the solver uses. My fix nudges those zero lengths by a tiny fraction of the arm's size, so the solver works on an arm a hair off the real one, then, since forward kinematics is cheap, refines the answer against the true arm's FK, step after step, until it's an exact solution again. The maintainer confirmed the fix would have worked while landing the same one upstream.

A major part of my design philosophy for V2 was making sure every quantity had a physical meaning. Velocity, acceleration and force each had their own type, and they only combined in physically valid ways: an acceleration integrated into a velocity, and an inertia turned a force into an acceleration. Every link, joint and actuator also carried a typed frame, so frames belonging to different kinds of objects couldn't be mixed up. The core of this model was the joint system. Each joint is defined by its motion subspace S, and a CRTP template turns that into fixed-size math, including the articulated-inertia step, so each joint type can be specialized. Because Achilles was meant to model my club's robots, I also built sensor models, starting with an encoder, with Gaussian noise, Gauss–Markov drift, a low-pass filter and scale-factor error. But when I started planning collisions and rendering, I realized the rigid architecture and very specific type flow had built me into a corner.

Until now, every torque in Achilles came from the scene file, so a robot could fall or collide but never react. Actuators let a controller outside the simulation push on a joint every step. An actuator is a direction in its joint's frame, an angular one for a torque or a linear one for a force. Attaching it projects that direction onto the joint's free axes once, so applying an effort later costs only a few multiply-adds into the joint-torque field. The simulation never owns or loops over actuators: each one is a small handle into that field, which is now double-buffered. A controller writes one half while the step reads the other, and the halves swap at the start of every step. An effort therefore acts over exactly the next step and is then gone, so a stale command can never leak into a later frame. Here, two actuators tilt a plate to balance a ball. The controller never sees the ball directly, only through a simulated camera that reports its position 30 times a second, 50 ms late, with 2 mm of noise. The plate can tilt no faster than 3 rad/s, about what a hobby servo manages. The plate starts tipped with the ball on its high corner, and the controller has to catch the ball before it rolls off, center it, then roll it around a figure-8.

To check that an effort lands on exactly the step it was meant for, I drove a six-joint arm in both Achilles and MuJoCo 3.17. Every joint has an actuator running a PD loop toward its own sine wave, with torques up to 146 N·m. Both engines start from the same state at the same 2 ms step, and each step they read the joints, compute the torques, and apply them over the next step. Over 20 seconds, Achilles stays within 10⁻⁵ rad of MuJoCo under both semi-implicit Euler and RK4, which is float32 round-off, and the torques agree to 3×10⁻⁵. If the torques land one step late, MuJoCo ends up 1 to 2 rad away from its on-time run, so a timing slip would stand out by five orders of magnitude. The free-swinging arms in my throughput benchmark drifted apart after a few seconds because they're chaotic. Here the controller keeps pulling both arms back onto their targets, so the two engines stay together for the whole run.