ROS 2

For localization and a map, the base carries two RPLIDAR A1 2D lidars, one at the front and one at the back. Bringing them up, with the team, was mostly fighting USB: the two units have identical USB IDs, so udev rules name each by the hub port it's plugged into, and they power up a few seconds apart so neither times out.

Mapping and Nav2 expect one scan, so I wrote a node that merges the two. The two scans never line up on their own: nothing guarantees the lidars spin at exactly the same rate or finish a revolution at the same moment, and each reaches the Thor with its own latency. So the node pairs scans by their timestamps (any two within 0.1 s), looks up each scan's transform into the robot's frame at that scan's own timestamp, resamples both to the finer of the two angular resolutions, and keeps the nearest return in each direction of one 360° scan, stamped halfway between the two and dropping hits on the robot itself. On top of it, I set up slam_toolbox and Nav2, which drove the robot point to point in simulation.

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.

OpenDubs runs on an NVIDIA Jetson Thor, so the first job was getting it ready for the whole team. I flashed JetPack onto it, got its IO talking to the rest of the robot, and set up the Docker environment we all develop in: a ROS 2 image with the build tools, Makefile and VS Code tasks to build and debug inside it, and per-person git identities so four people can commit from one shared machine.

The first version of the base drove its four mecanum wheels through a Pixhawk running ArduPilot, over MAVROS; it has since dropped the Pixhawk. With another intern, I brought up that drive code, then added what the rest of the stack needed: a URDF and an SDF with collision geometry for RViz and Gazebo, and a service that records topics to rosbags. Every test after this was a bag I could replay.

The base has since been rebuilt by others in the lab (I didn't work on it), without the Pixhawk, and carries everything else: a 51.2 V LiFePO4 battery, an emergency stop on the deck and a status display, all wired into one aluminum frame, with the arms, the Thor and the lidars riding on top.

I ran the same characterization on a WayPonDEV LD19 2D lidar, for obstacle mapping, with the Odin providing the pose. It scans at a steady 10 Hz with 8–14 ms of jitter, about 503 points per scan 0.72° apart, and skips 1–2% of scans. Range noise is 3–6 mm at 1–2 m, 6–11 mm at 2–3 m and 7–28 mm at 3–5 m: well under a 5 cm map cell, even at 5 m. At most 0.2% of returns are isolated false points and 1–2.4% are outliers, mostly at object edges where a beam half-hits the edge and half the background, which a filter removes easily. It never dropped off USB in the 40 minutes it ran, and the longest gap between scans was one skipped scan. The LD19 was good enough for our use, but we bought a 3D lidar anyway: the Odin's field of view is small, and a 3D lidar sees much more of the area around the robot.