Collision detection

Before contacts can be resolved, they have to be found, and that happens in two phases. Every body carries a single "sphere-swept box," a box whose surface is pushed outward by a radius, so one shape type covers spheres, capsules, boxes and rounded boxes. The broad phase cheaply narrows the search. Each frame, it wraps every body in a world-aligned bounding box and tests every pair for overlap. It skips pairs whose collision groups exclude each other and bodies directly joined by a joint. The narrow phase then examines only the remaining candidates. For each pair, it computes a contact normal and up to four contact points, each with a signed distance that turns negative once the shapes overlap. Contacts within a small margin are kept even before the shapes touch, so the solver can react early. Both phases treat every pair the same way and mask out irrelevant results instead of branching, which keeps them SIMD- and GPU-friendly.

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.