Sequences of interdependent geometric constraints are central to many multi-agent Task and Motion Planning (TAMP) problems. However, existing methods for handling such constraint sequences struggle with partially ordered tasks and dynamic agent assignments. They typically assume static assignments and cannot adapt when disturbances alter task allocations. To overcome these limitations, we introduce Graph-of-Constraints Model Predictive Control (GoC-MPC), a generalized sequence-of-constraints framework integrated with MPC. GoC-MPC naturally supports partially ordered tasks, dynamic agent coordination, and disturbance recovery. By defining constraints over tracked 3D keypoints, our method robustly solves diverse multi-agent manipulation tasks-coordinating agents and adapting online from visual observations alone, without relying on training data or environment models. Experiments demonstrate that GoC-MPC achieves higher success rates, significantly faster TAMP computation, and shorter overall paths compared to recent baselines, establishing it as an efficient and robust solution for multi-agent manipulation under real-world disturbances. Our supplementary video and code can be found at https://sites.google.com/view/goc-mpc/home .
Figures & tables
Fig. 1: GoC ( GoC ) can naturally express partially-ordered multi-agent tasks using a DAG of arbitrary system constraints. Our method, GoC-MPC , can use a GoC with constraints defined in terms of workspace keypoints to find optimal solutions for general multi-agent manipulation tasks using only visual observations. For this two-cup manipulation task, GoC-MPC finds an initial solution in 0.373 seconds, and all subsequent solutions in 0.065 seconds on average, demonstrating extremely fast performance.
Fig. 2: After waypoints W and agent assignments A have been determined, splines for each agent are threaded through the appropriate waypoints in order. The current subgraph is determined by a subset of remaining nodes R (denoted with solid borders) that shrinks as tasks are completed (denoted with dashed borders). The graph’s structure determines additional constraints that can enforce ordering or ensure synchronization between agents’ actions.
Fig. 3: We conducted experiments in two simulated and three real bimanual maniulation tasks with two UR5e arm robots. These included a distant-cup pick-and-pour task, a three-block stacking stacking task, and a tablecloth folding task (not simulated due to including a soft-body). These tasks demonstrate that our method is capable of handling partially-ordered tasks, coordination of multiple robots based on the objects they are manipulating, and synchronization between agents.
GoC-MPC (Ours)
ReKep
Success Rate
10/10
7/10
Max Time (s)
0.520±0.311
8.38±0.062
Avg. Time (s)
0.108±0.012
7.11±0.186
Total Length (m)
2.54±0.216
4.75±2.59
TABLE I: Comparison of our approach with ReKep on the Block-Stacking task shows that our method outperforms across all metrics. It achieves a 100% success rate and, on average, runs 70 × faster than ReKep, highlighting its effectiveness for reactive TAMP.
GoC-MPC (Ours)
ReKep
Success Rate
10/10
6/10
Max Time (s)
0.512±0.328
12.01±0.966
Avg. Time (s)
0.216±0.064
8.49±0.413
Total Length (m)
1.94±0.742
3.20±2.08
TABLE II: Comparison of our approach with ReKep on the Pick-and-Pour task. Our method achieves a 100% success rate, runs 40 × faster than ReKep, and produces significantly shorter path lengths than the baseline.
Fig. 4: GoC allow for agents to independently backtrack to previous stages when a disturbance is applied. In the block stacking task, a disturbance to the block in the left robot’s grasp prompts the robot to loop back to picking up the block, and the right robot’s path is slowed to the point where it smoothly reaches its original destination after the other robot has finished retrieving the disturbed block.
GoC-MPC (Ours)
ReKep
Success Rate
10/10
10/10
Max Time (s)
0.161±0.093
12.36±6.180
Avg. Time (s)
0.052±0.007
3.29±0.331
Total Length (m)
6.89±0.938
7.53±0.892
TABLE III: Comparison of our approach with ReKep on the block-stacking with external disturbances. While both methods achieved a 100% success rate, the agent-independent backtracking provided by GoC saved on average 0.64 meters of unnecessary movement compared to ReKep, executing 80 × faster in maximum time.
(# Obj, # Agents)
Success
Avg. Time (s)
Length (m)
(5,2)
5/5
0.319±0.055
10.95±1.35
(8,2)
5/5
0.976±0.196
18.53±3.83
(11,2)
4/5
2.462±0.516
27.532±4.445
(5,3)
5/5
0.365±0.103
12.19±1.14
(8,3)
4/5
1.23±0.363
21.50±4.11
(11,3)
4/5
2.650±1.051
30.570±3.783
TABLE IV: Scalability performance of GoC-MPC. The number of agents and blocks in the environment vary across experiments. Mean and standard deviation are reported over 5 randomly initialized trials.
Task
Max Time (s) ↓
Avg. Time (s) ↓
Success ↑
Block-Stacking
0.493±0.018
0.093±0.027
3/5
Pick-and-Pour
0.373±0.071
0.065±0.012
5/5
Cloth-Folding
0.134±0.009
0.057±0.002
5/5
TABLE V: Real-world Success Rates and Computation Time for our method.
The multi-robot unlabeled motion planning problem of concurrently assigning robots to goals and generating safe trajectories is central in many collaborative tasks. Recent Graph Neural Network methods offer scalable decentralized solutions but rely on simplified dynamics and simulation environments, overlooking key challenges of real-world deployment such as dynamic feasibility and communication constraints. To address these gaps, we propose a hierarchical framework that combines a Graph ATtention Planner (GATP) with a decentralized Nonlinear Model Predictive Controller (NMPC). GATP provides intermediate subgoals through multi-robot cooperation, and the NMPC enforces safety under nonlinear dynamics and actuation constraints. We evaluate our framework in both simulation and real-world quadrotor experiments. Thanks to attention mechanisms and minimal communication requirements, we demonstrate improved generalization to larger teams, robustness to communication delays up to 200 ms and practical feasibility with decentralized on-board inference.
Manohari Goarin, Yang Zhou, Giuseppe Loianno
New York University, NY 10012, USA · University of California Berkeley, Department of Electrical Engineering and Computer Sciences, Berkeley, CA 94720, USA
Safe multi-agent motion planning (MAMP) under task-induced constraints is a critical challenge in robotics. Many real-world scenarios require robots to navigate dynamic environments while adhering to manifold constraints imposed by tasks. For example, service robots must carry cups upright while avoiding collisions with humans or other robots. Despite recent advances in decentralized MAMP for high-dimensional systems, incorporating manifold constraints remains difficult. To address this, we propose a manifold-constrained Hamilton-Jacobi reachability (HJR) learning framework for decentralized MAMP. Our method solves HJR problems under manifold constraints to capture task-aware safety conditions, which are then integrated into a decentralized trajectory optimization planner. This enables robots to generate motion plans that are both safe and task-feasible without requiring assumptions about other agents' policies. Our approach generalizes across diverse manifold-constrained tasks and scales effectively to high-dimensional multi-agent manipulation problems. Experiments show that our method outperforms existing constrained motion planners and operates at speeds suitable for real-world applications. Video demonstrations are available at https://youtu.be/RYcEHMnPTH8 .
Qingyi Chen, Ruiqi Ni, Junyoung Kim +1
Department of Computer Science at Purdue University, West Lafayette, IN 47907, USA.
This paper proposes a task-oriented model predictive control (ToMPC) framework for safe and efficient robotic manipulation in open workspaces. The framework unifies collision-free motion and robot-environment interaction to address diverse scenarios. Additionally, it introduces task-oriented obstacle avoidance that leverages kinematic redundancy to enhance manipulation efficiency in obstructed environments. This complex optimization problem is solved by the alternating direction method of multipliers (ADMM), which decomposes the problem into two subproblems tackled by differential dynamic programming (DDP) and quadratic programming (QP), respectively. The effectiveness of this approach is validated in simulation and hardware experiments on a Franka Panda robotic manipulator. Results demonstrate that the framework can plan motion and/or force trajectories in real time, maximize the manipulation range while avoiding obstacles, and strictly adhere to safety-related hard constraints.
Xinyu Jia, Wenxin Wang, Jun Yang +2
Department of Biomedical Engineering, National University of Singapore, Singapore 117583 · Peng Cheng Laboratory, Shenzhen 518057, China · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798