Toward Holistic Planning and Control Optimization for Dual-Arm Rearrangement

TL;DR

This work tackles the challenging problem of jointly optimizing task and motion planning for dual-arm tabletop rearrangement. It introduces MODAP, a pipeline that fuses a makespan-focused task planner with cuRobo-based motion generation and TOPPRA-based trajectory refinement, enabling faster, dynamically feasible dual-arm plans. Key innovations include IK seed control, detour avoidance in path planning, and dual-arm conflict resolution strategies, augmented by real-world calibration and digital-twin transfer. Empirical results show MODAP achieving up to 40% faster execution than state-of-the-art baselines with small sim-to-real gaps, highlighting its practical impact for efficient dual-arm manipulation in constrained environments.

Abstract

Long-horizon task and motion planning (TAMP) is notoriously difficult to solve, let alone optimally, due to the tight coupling between the interleaved (discrete) task and (continuous) motion planning phases, where each phase on its own is frequently an NP-hard or even PSPACE-hard computational challenge. In this study, we tackle the even more challenging goal of jointly optimizing task and motion plans for a real dual-arm system in which the two arms operate in close vicinity to solve highly constrained tabletop multi-object rearrangement problems. Toward that, we construct a tightly integrated planning and control optimization pipeline, Makespan-Optimized Dual-Arm Planner (MODAP) that combines novel sampling techniques for task planning with state-of-the-art trajectory optimization techniques. Compared to previous state-of-the-art, MODAP produces task and motion plans that better coordinate a dual-arm system, delivering significantly improved execution time improvements while simultaneously ensuring that the resulting time-parameterized trajectory conforms to specified acceleration and jerk limits.

Figures (7)

• Figure 1: The experimental setup used in the study contains two Universal Robots UR-5e arms, each equipped with a Robotiq 2F-85 two-finger gripper. [left] A handoff operation during the plan execution to convert an object layout from the letter 'A' to the letter 'R'. [top right] The start object layout forming the letter 'A', [bottom right] The goal object layout showing the letter 'R'.
• Figure 2: An illustration of the Cooperative Dual-Arm Rearrangement (CDR) setting. [left] Illustrations of the workspace, objects' start/goal poses, and robots' nominal locations. [right] A corresponding task plan with two action steps.
• Figure 3: [left] A configuration of our dual-arm setup, also showing that the camera is mounted at the side to detect objects' poses. [right] The corresponding Dual-Arm setup in pyBullet simulation.
• Figure 4: [top] RMSD of configuration and end-effector position on a sampled trajectory executed with a maximum speed of 1.57rad/s on real robots. [bottom] The average Q value of 5 sampled trajectories executed at different maximum speeds (sim denotes execution in simulation; otherwise, the execution is on a real robot). Note that the lines of BL and BL-TP overlap with each other.
• Figure 5: Trajectories of BL, BL-TP, and MODAP for the a typical rearrangement task. Each row, from top to bottom, corresponds to a robot joint in radians from one to six. Each column shows the trajectory of the six joint angles of a robot.