Robot safety

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.

The sentry localizes with an Odin1, a lidar-inertial sensor that relocalizes into a prebuilt map. Before the team decided whether the whole localization system could rely on it alone, we wanted to know how accurate it is and where it is weak. So I built a test platform that measures it against the motion capture room, whose OptiTrack tracks the robot to a fraction of a millimeter. It starts and stops the sensor, records every run, guides whoever is driving with live mocap feedback, and can also drive the robot itself through the Jetson and the robot's controller, with a watchdog and speed caps. A campaign day runs a fixed protocol: calibration spins, a mapping drive, figure-8 laps at set speeds, ten-minute holds, back-and-forth motion to measure latency, cold starts, and outages where someone covers the sensor.