MuJoCo Robotics Lab — Part 10: Dual-Arm Coordination

Lab 6 doubles the robot count. Two UR5e arms pick up a 240 mm bar from a shared table, carry it 200 mm across the workspace, and set it down — together. Everything from the previous labs (FK, IK, RRT*, TOPP-RA, impedance control, grasp state machines) runs twice per control tick, and an object-centric coordination layer keeps the two arms locked to the same rigid body.

Problem

Single-arm manipulation is solved in this series: plan in joint space, parameterize time with TOPP-RA, track with impedance control. The instant a second arm grasps the same rigid body, that pipeline breaks in three ways.

1. Closed kinematic chain. Once both grippers close on the bar, the arms are kinematically linked through the object. Any mismatch between their commanded poses becomes an internal force squeezing or pulling the bar.
2. Temporal coupling. An arm arriving 500 ms before its partner is pushing air while the other is still approaching. A timing skew turns a cooperative lift into a one-armed drag.
3. Load sharing. If one arm's impedance is stiffer, it dominates and the other goes slack, letting the bar rotate out of the grasp.

Running two copies of the Lab 5 state machine fails on all three. The fix is a coordination layer that owns the object frame, synchronizes time, and enforces symmetric load sharing.

Theory

Closed-Chain Kinematic Constraint

Let x_L and x_R be the end-effector poses of the left and right arms, and let x_O be the pose of the object. Once both grippers close, the grasps are rigid, so:

x_L = x_O · T_L^offset
x_R = x_O · T_R^offset

where T_L^offset and T_R^offset are the constant grasp transforms in the object frame. Differentiating gives the velocity constraint on each arm:

J_L(q_L) · q̇_L = V_O        (twist of the object in the left grasp frame)
J_R(q_R) · q̇_R = V_O        (same twist expressed at the right grasp frame)

Both arms see the same object twist V_O. If the controllers disagree on V_O, the grasp frames separate and the bar either slips or is squeezed.

Grasp Matrix and Load Distribution

The wrench applied to the object is the sum of the contributions from each contact, mapped through the grasp matrix G:

w_O = G · [f_L; f_R]
G   = [ Ad(T_L^offset)^T   Ad(T_R^offset)^T ]

For a symmetric two-point grasp on a rigid bar, G has more columns than rows — the internal force (squeeze) lives in its null space. Picking f_L = f_R = 0.5 · G^+ · w_O gives the minimum-norm load split: each arm carries half the bar, no internal squeeze.

Leader-Follower vs Symmetric Coordination

Two common strategies:

• Leader-follower. One arm plans freely and the other tracks it through the object frame. Simple, but if the follower's tracking error grows, the leader has no way to know.
• Symmetric coordination. Both arms treat the object as the reference. Each arm is a peer controller with identical gains, and both pull their targets from the same ObjectFrame. Drift in one arm does not poison the other — the other arm stays anchored to the object, not to its partner.

Lab 6 uses symmetric coordination. Equal Kp/Kd on both sides means neither arm dominates, and the object frame absorbs any residual slip.

Implementation

Scene: models/dual_arm_scene.xml

A single monolithic MJCF with two UR5e + gripper trees, prefixed left_ and right_. Bases mirrored across Y:

worldbody
├── floor
├── left_base   (pos="0 -0.35 0")   → left_shoulder … left_tool0 → left_gripper
├── right_base  (pos="0  0.35 0")   → right_shoulder … right_tool0 → right_gripper
├── table
├── carry_bar   (freejoint — the shared rigid body, 240×50×40 mm)
└── target_pad

14 actuators total: [0–5] left arm, [6] left gripper, [7–12] right arm, [13] right gripper. Two equality constraints mirror the inner/outer finger joints on each gripper. The bar has a free joint so MuJoCo's contact solver handles the actual grasp physics — no artificial weld when the grippers close.

Two Pinocchio Models

Each arm loads its own (model, data, ee_frame_id) tuple from the same ur5e.urdf. This avoids frame ID collisions that come with a combined URDF and lets FK, Jacobians, and gravity compensation run per arm with zero index bookkeeping.

Object-Centric Targets

ObjectFrame owns the bar's pose and derives each arm's EE target from the same source of truth. compute_bimanual_configs() runs DLS IK for both arms at four task phases (home, pregrasp, grasp, place), placing each gripper on its assigned X-end of the bar — left at ~0.35 m, right at ~0.55 m. If the object moves, both arms recompute from the same pose.

Synchronized Trajectories

Each arm plans independently with RRT* and parameterizes its path with TOPP-RA. The raw trajectories almost never have equal durations, so coordination_layer.sync_trajectories() pads the shorter one (holding the final waypoint) so both arms share a single time axis before execution:

def sync_trajectories(traj_L, traj_R):
    t_L, q_L, qd_L = traj_L
    t_R, q_R, qd_R = traj_R
    T = max(t_L[-1], t_R[-1])

    t_sync = np.linspace(0.0, T, int(T / DT) + 1)
    q_L_s  = _resample_hold(t_L, q_L,  t_sync)
    qd_L_s = _resample_hold(t_L, qd_L, t_sync, zero_after_end=True)
    q_R_s  = _resample_hold(t_R, q_R,  t_sync)
    qd_R_s = _resample_hold(t_R, qd_R, t_sync, zero_after_end=True)
    return t_sync, q_L_s, qd_L_s, q_R_s, qd_R_s

After sync, both arms start and stop at the same tick — no more "one arm grabbing while the other is still travelling".

Bimanual Controller

A single BimanualController.step() applies torques to both arms inside one MuJoCo step. Gravity compensation comes from each Pinocchio model independently:

def step(self, mj_data, q_d_L, qd_d_L, q_d_R, qd_d_R):
    q_L  = mj_data.qpos[LEFT_QPOS];  qd_L = mj_data.qvel[LEFT_QPOS]
    q_R  = mj_data.qpos[RIGHT_QPOS]; qd_R = mj_data.qvel[RIGHT_QPOS]

    g_L = pin.computeGeneralizedGravity(self.pin_L, self.data_L, q_L)
    g_R = pin.computeGeneralizedGravity(self.pin_R, self.data_R, q_R)

    tau_L = self.Kp * (q_d_L - q_L) + self.Kd * (qd_d_L - qd_L) + g_L
    tau_R = self.Kp * (q_d_R - q_R) + self.Kd * (qd_d_R - qd_R) + g_R

    mj_data.ctrl[LEFT_CTRL]  = clip_torques(tau_L)
    mj_data.ctrl[RIGHT_CTRL] = clip_torques(tau_R)

Same Kp, same Kd, same control law — symmetric by construction.

State Machine

BimanualStateMachine drives the full cycle:

IDLE → PLAN_APPROACH → EXEC_APPROACH → DESCEND → CLOSE
     → LIFT → PLAN_TRANSPORT → EXEC_TRANSPORT
     → DESCEND_PLACE → RELEASE → RETRACT → DONE

Each state advances both arms together. Gripper actions happen in lockstep — both close in CLOSE, both open in RELEASE. The planning states (PLAN_APPROACH, PLAN_TRANSPORT) call RRT* and TOPP-RA per arm, then immediately hand the results to sync_trajectories() before execution.

Results

Headless simulation, full pick–carry–place cycle, both arms running at 1 kHz impedance control on a single MuJoCo step.

The tracking numbers are inherited from Lab 5's per-arm impedance controller — the dual-arm layer does not degrade them. The interesting result is qualitative: once the timing was synchronized and the gains matched, cooperative transport "just worked" on the first integrated run. The bar never rotated out of the grasp, and the two end-effector trajectories in the logs are mirror images of each other across the Y axis.

Key Takeaways

• Make the object the reference, not the other arm. Object-centric targets isolate failures; leader-follower schemes couple them.
• Synchronize time before you synchronize motion. Pad the shorter trajectory and resample onto a shared time axis before executing — otherwise one arm is always grabbing air.
• Symmetric gains prevent internal squeeze. Equal Kp/Kd on both sides is a cheap substitute for explicitly solving the grasp-matrix null space.
• Two independent Pinocchio models beat one combined URDF. No frame ID collisions, gravity compensation is trivially per-arm.
• Workspace geometry is a design lever. Placing bases at y = ±0.35 m with each arm owning its own X-end of the bar means arm-arm collision checking becomes unnecessary.

Resources

• Explore this lab on NotebookLM
• Source code on GitHub
• Full YouTube playlist