OpenDubsBest example
Arm control: three impedance controllers, then a simpler one
Cartesian controlJoint and null-space control
- Code I wrote
- 3,525 lines
- Language
- C++
- Tools
- Pinocchio, ROS 2
- Hardware
- 2× OpenArm
- Role
- Solo
- Status
- On the robot
Also shows: Impedance controlC++ROS 2
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).