Linear algebra

V3 replaces V2's Eigen with my own spatial algebra on xsimd, and stores every field as a structure of arrays: each component of each field, say the x angular velocity of every joint, is its own contiguous, aligned array. Joints are sorted by their depth in the tree, and each group of SIMD lanes holds joints at the same depth. With many copies of one robot, that puts the same joint of different robots side by side, so one instruction can load, update or store a quantity for eight robots at once (eight lanes with AVX2, four in the default SSE2 build). A Valgrind trace of the compiled binary confirms it: 88.9% of all accesses to simulation data are whole, aligned 8-lane vector loads or stores. The payoff is that filling the lanes is nearly free. On a seven-link arm, one robot takes 3.1 µs per step, while eight robots in the 8-lane build take 4.3 µs, 5.7× the robot-steps per second.

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⁻⁶.

Our robots know where they are from dead wheels: small unpowered tracking wheels with encoders that roll along the floor. When the engineer robot moved to new odometry pods with three of them, I rewrote the team's Kalman-filter odometry for them; I had taught myself Kalman filters from Kalman Filter from the Ground Up. The first version was one 8-state filter fusing the three wheels with the IMU. Its covariance between heading and the x and y motion turned out to be negligible, so I split it into three independent two-state filters, for x, y and heading, each updated by one measurement: every step now multiplies 2×2 matrices and divides by a number, instead of working through 8×8 matrices and inverting a 7×7 one, and the wheels alone turned out to be enough without the IMU. To tune it, I drove fixed paths (straight forward, straight sideways, spins and a square), logged the raw wheels, and replayed them offline to fit each pod's position, angle and wheel radius and the filter's noise, checking the result against the IMU's heading. Our target was under 10 cm of error over the engineer's path, the most our vision system could correct for, and the tuned odometry met it.

Our chassis followed paths with pure pursuit: aim at a point a fixed distance down the path and ramp the speed up toward it. The robot's accelerations stay implicit in that, so it has no idea how hard it can brake or how fast it can take a corner, and at speed it swings wide of turns and overshoots where it should stop. I proposed replacing it with path tracking that models those accelerations explicitly, and led the feature from idea to implementation: I wrote the design and gathered the resources behind it, from spline fitting and arc-length reparametrization to the Riccati equation and a study that found LQR tracks more accurately than pure pursuit. First, a cubic spline through the waypoints is reparametrized by arc length, the speed at each point is capped by its curvature, and a forward and a backward pass keep every speed-up and every stop within what the robot can deliver. Then an LQR tracks that profile on a world-frame model of the chassis, where its dynamics stay constant, and its force is rotated into the robot's frame. I designed it and built the framework; the comparison here runs the finished MVP against our pure pursuit in simulation.