Real-time control

Each arm is a 7-DOF OpenArm whose Damiao motors run their own impedance loop: the Thor sends position, velocity and torque targets with gains over CAN-FD, and each motor closes the loop itself. I measured the stock ROS 2 stack first. A goal is accepted in 2.55 ms, and with both arms holding one block, 47 ms passes between a command to one arm and the torque spike in the other (σ = 4.7 ms).

What I was after was axial compliance in Cartesian space: a gripper that holds firm along some axes and gives along others. I wrote three controllers with Pinocchio to get it (Cartesian, joint and null space). Pure Cartesian impedance gave that compliance, but it left the redundant elbow free to drift off the collision-checked path. The combined controller ran joint-space impedance and subtracted the Cartesian terms from it, keeping the axial compliance and control of the null space, but it was numerically unstable. The null-space controller worked, but I switched off it for the much lower latency of the motors' own impedance loops: in the end we gave up Cartesian compliance for speed.

What I settled on is simpler: the planned trajectory goes straight to the motors, plus gravity compensation at 500 Hz from Pinocchio's RNEA (the recursive Newton-Euler algorithm), with every goal checked against position and torque limits in both joint space and Cartesian space (through forward kinematics).

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.

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.