Bimanual manipulation

The lab plans with its own MuJoCo planning library, mj_manipulator, which planned one arm at a time. With two arms on one torso, two plans that are each safe can still hit each other. I added an ArmGroup to it that plans all arms' joints together, so every step is checked arm against arm. On the clip's goals, the two arms planned separately collide in 58 of 210 frames; ArmGroup's plan bends one gripper about 16 cm around the other and touches in none. The same group plans to joint goals, gripper poses and lockstep Cartesian paths, keeps 5 mm between all geometry, retimes the trajectory at 125 Hz, and re-checks it at every sample. I made single-arm planning a one-arm group, and ArmGroup is what plans both arms on the robot today.

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.

For demonstrations and live driving, I built teleop with a Meta Quest. The apps I tried had no passthrough and left me dizzy, so I wrote my own: instead of an app on the headset, the robot serves a page the Quest's browser opens in AR, so the operator still sees the room, and the page streams both controllers' poses, grip and trigger to the robot over a websocket. Holding a grip engages its arm, and the trigger works the gripper. The headset's page goes dark once it enters AR, so the robot also broadcasts that live telemetry to any other browser pointed at it, and someone at a laptop can watch exactly what the operator is sending. It matches the old SDK's interface, so switching was a one-line change. I built it against the arms in MuJoCo first; at first the loop couldn't keep up, and the bottleneck turned out to be publishing the sim viewer over the network, not the control.