Graph-of-Constraints Model Predictive Control for Reactive Multi-agent Task and Motion Planning

Anastasios Manganaris; Jeremy Lu; Ahmed H. Qureshi; Suresh Jagannathan

cs.ROMar 19, 2026

Graph-of-Constraints Model Predictive Control for Reactive Multi-agent Task and Motion Planning

Authors: Anastasios Manganaris, Jeremy Lu, Ahmed H. Qureshi, Suresh Jagannathan

Organizations: Department of Computer Science, Purdue University, West Lafayette, IN, USA, 47907.

Abstract

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.

Panel of Figure 2 — 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.

Panel of Figure 3 — 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	$\boldsymbol{10/10}$	$7/10$
Max Time (s)	$\boldsymbol{0.520\pm 0.311}$	$8.38\pm 0.062$
Avg. Time (s)	$\boldsymbol{0.108\pm 0.012}$	$7.11\pm 0.186$
Total Length (m)	$\boldsymbol{2.54\pm 0.216}$	$4.75\pm 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

\times

faster than ReKep, highlighting its effectiveness for reactive TAMP.

	GoC-MPC (Ours)	ReKep
Success Rate	$\boldsymbol{10/10}$	$6/10$
Max Time (s)	$\boldsymbol{0.512\pm 0.328}$	$12.01\pm 0.966$
Avg. Time (s)	$\boldsymbol{0.216\pm 0.064}$	$8.49\pm 0.413$
Total Length (m)	$\boldsymbol{1.94\pm 0.742}$	$3.20\pm 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

\times

faster than ReKep, and produces significantly shorter path lengths than the baseline.

Panel of Figure 6 — 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	$\boldsymbol{10/10}$	$10/10$
Max Time (s)	$\boldsymbol{0.161\pm 0.093}$	$12.36\pm 6.180$
Avg. Time (s)	$\boldsymbol{0.052\pm 0.007}$	$3.29\pm 0.331$
Total Length (m)	$\boldsymbol{6.89\pm 0.938}$	$7.53\pm 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

\times

faster in maximum time.

(# Obj, # Agents)	Success	Avg. Time (s)	Length (m)
$(5,2)$	5/5	$0.319\pm 0.055$	$10.95\pm 1.35$
$(8,2)$	5/5	$0.976\pm 0.196$	$18.53\pm 3.83$
$(11,2)$	4/5	$2.462\pm 0.516$	$27.532\pm 4.445$
$(5,3)$	5/5	$0.365\pm 0.103$	$12.19\pm 1.14$
$(8,3)$	4/5	$1.23\pm 0.363$	$21.50\pm 4.11$
$(11,3)$	4/5	$2.650\pm 1.051$	$30.570\pm 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) $\downarrow$	Avg. Time (s) $\downarrow$	Success $\uparrow$
Block-Stacking	$0.493\pm 0.018$	$0.093\pm 0.027$	$3/5$
Pick-and-Pour	$0.373\pm 0.071$	$0.065\pm 0.012$	$5/5$
Cloth-Folding	$0.134\pm 0.009$	$0.057\pm 0.002$	$5/5$

TABLE V: Real-world Success Rates and Computation Time for our method.

Multi-Agent Planning

Multi-Agent Planning coordinates multiple decision-makers to assign tasks, plan actions and paths, and resolve conflicts under shared constraints.

See all 121 papers

Graph Neural Planning and Predictive Control for Multi-Robot Communication-Constrained Unlabeled Motion Planning
May 19, 2026Manohari Goarin, Yang Zhou, Giuseppe LoiannoMulti-Robot Motion Planning Nonlinear MPC
New York University, NY 10012, USA · University of California Berkeley, Department of Electrical Engineering and Computer Sciences, Berkeley, CA 94720, USA
Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning
Nov 5, 2025Qingyi Chen, Ruiqi Ni, Junyoung Kim +1Multi-Robot Motion Planning Constrained Motion Planning
Department of Computer Science at Purdue University, West Lafayette, IN 47907, USA.
ToMPC: Task-oriented Model Predictive Control via ADMM for Safe Robotic Manipulation
Mar 14, 2026Xinyu Jia, Wenxin Wang, Jun Yang +2Safe Robot Manipulation Robotic Manipulation
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

May 19, 2026J/K move · Enter open · S save

Graph Neural Planning and Predictive Control for Multi-Robot Communication-Constrained Unlabeled Motion Planning

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

Multi-Robot Motion Planning Nonlinear MPC Graph-Based Planning Multi-Robot Task Allocation

Open paper AskPDF arXiv

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.

Abstract

Multi-Agent Planning

Explore similar work

Graph Neural Planning and Predictive Control for Multi-Robot Communication-Constrained Unlabeled Motion Planning

Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning

ToMPC: Task-oriented Model Predictive Control via ADMM for Safe Robotic Manipulation