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

1. Problem Statement

The paper addresses the challenges of Task and Motion Planning (TAMP) for multi-agent robotic systems operating in dynamic, real-world environments.

• Core Challenge: Existing TAMP methods often rely on sequences of constraints that impose a total order on tasks and assume static agent assignments.
• Limitations of Prior Work:
  • Rigid Ordering: They cannot naturally handle partially ordered tasks (where agents can work in parallel), leading to unnecessary idle time.
  • Static Assignment: They assign agents to specific subtasks beforehand. If a disturbance occurs (e.g., an object is moved or a grasp fails), the system cannot easily reassign agents or adapt the plan, often causing the entire team to stall.
  • Model Dependency: Many methods require precise world models or extensive training data, limiting their generalizability.
• Goal: Develop a reactive planning framework that supports dynamic agent coordination, handles partial task ordering, recovers from disturbances online, and operates using only visual observations without pre-trained models.

2. Methodology: GoC-MPC

The authors propose Graph-of-Constraints Model Predictive Control (GoC-MPC), a framework that generalizes constraint sequences into a Directed Acyclic Graph (DAG) structure.

A. Graph-of-Constraints (GoC)

Instead of a linear sequence, the task is represented as a graph $G = (V, E, \Phi_V, \Phi_E)$:

• Nodes ($V$): Represent "waypoint" constraints (specific states the system must reach).
• Edges ($E$): Represent "path" constraints (conditions that must hold while transitioning between waypoints).
• Partial Ordering: The DAG structure naturally encodes dependencies, allowing independent branches to be executed in parallel.
• Dynamic Agent Assignment: Constraints are defined over a dynamic assignment matrix $A \in \{0, 1\}^{K \times M}$, where $M$ agents are assigned to $K$ subtasks. This allows the optimizer to decide which agent performs which subtask during the planning phase.
• Constraint Representation: Constraints are defined over tracked 3D keypoints (using Relational Keypoint constraints or ReKeps). This avoids the need for explicit object models or training data, relying instead on geometric relationships observed visually.

B. Optimization Decomposition

Solving the full multi-agent TAMP problem with dynamic assignments is computationally expensive. GoC-MPC decomposes the problem into three sub-problems solved within a Receding Horizon Control (MPC) loop:

1. Waypoints and Assignments Sub-Problem:

   • Goal: Determine optimal waypoints ($W$) and agent-to-task assignments ($A$).
   • Method: Formulated as a Mixed-Integer Non-Linear Program (MINLP). It minimizes the geodesic distance between waypoints while satisfying waypoint and edge constraints.
   • Dynamics Approximation: Uses a surrogate constraint $\bar{h}$ to approximate physical dynamics (e.g., if a robot holds an object, the object moves rigidly with it).

2. Agent Splines Problem:

   • Goal: Generate smooth trajectories (cubic splines) for each agent passing through their assigned waypoints.
   • Method: Formulated as a Quadratic Program (QP). It optimizes time deltas ($\Delta$) and velocities to minimize acceleration/jerk.
   • Synchronization: Enforces timing constraints derived from the graph structure:
     • Ordering ($E_\leq$): Ensures Agent A reaches a node before Agent B.
     • Synchronization ($E_=$): Ensures multiple agents reach a convergence node simultaneously.

3. Short Receding-Horizon Problem:

   • Goal: Compute a short-horizon trajectory ($H$ steps) that tracks the reference spline while avoiding immediate collisions and respecting fine-grained dynamics.
   • Method: A QP that minimizes the distance to the reference trajectory plus collision/reachability costs.

C. Reactive Execution Cycle

The algorithm operates in a loop (Algorithm 1):

• Phase Progression: If an agent reaches a waypoint within a time threshold $\tau$ and constraints are satisfied, the node is removed from the "remaining nodes" set, allowing parallel agents to proceed.
• Backtracking: If a disturbance violates an edge constraint (e.g., an object is pushed), the system identifies the violated edge, adds the source node back to the remaining set, and re-plans. This allows agents to independently backtrack to previous stages without halting the entire team.

3. Key Contributions

1. Graph-of-Constraints (GoC): A novel formalism that extends constraint-based planning to support partially ordered tasks and dynamic agent assignments, overcoming the rigidity of linear sequences.
2. GoC-MPC Algorithm: A real-time, decomposition-based MPC framework that solves the complex GoC optimization problem efficiently by breaking it into waypoint assignment, spline timing, and short-horizon tracking.
3. Disturbance Recovery: The ability to perform coordinated backtracking. If one agent fails or is disturbed, the system re-optimizes assignments and paths for that specific agent while allowing others to continue or synchronize appropriately.
4. Model-Free Visual Execution: The method relies solely on visual observations of 3D keypoints, eliminating the need for precise system models or large datasets.

4. Experimental Results

The authors evaluated GoC-MPC on three bimanual tasks: Block Stacking, Pick-and-Pour, and Tablecloth Folding.

• Performance vs. Baselines (ReKep):
  • Success Rate: GoC-MPC achieved 100% success in static and disturbance scenarios, compared to 70-60% for the baseline (ReKep).
  • Computation Speed: GoC-MPC was 40x to 80x faster than ReKep (e.g., 0.05s vs 3.29s per cycle in disturbance scenarios).
  • Path Efficiency: Generated significantly shorter total path lengths (e.g., 2.54m vs 4.75m in block stacking) by optimizing parallel execution and avoiding unnecessary idle time.
• Robustness to Disturbances:
  • In experiments where objects were pushed or grasps failed, GoC-MPC successfully re-assigned agents and backtracked only the affected agent. The baseline often failed or required the entire team to reset.
  • GoC-MPC saved an average of 0.64 meters of unnecessary movement in disturbed scenarios compared to the baseline.
• Scalability:
  • The system remained effective with up to 4 agents and 11 objects, maintaining high success rates and low computation times (under 3.6s even for the largest scale).
• Real-World Validation:
  • Deployed on dual UR5e robots.
  • Achieved high success rates (3/5 to 5/5) in physical tasks, with computation times averaging 0.057s to 0.093s per cycle. Failures were attributed to visual occlusions, not planning logic.

5. Significance

This work represents a significant step forward in reactive multi-robot manipulation. By moving from linear constraint sequences to graph-based structures, GoC-MPC enables:

• True Parallelism: Robots can work simultaneously on independent subtasks rather than waiting for a linear sequence.
• Resilience: Systems can recover from real-world disturbances (slippage, collisions, external pushes) without human intervention or complete replanning.
• Generalization: The reliance on geometric keypoints rather than specific object models makes the approach applicable to a wide variety of tasks and objects without retraining.

The paper establishes GoC-MPC as a highly efficient, robust, and scalable solution for complex multi-agent tasks in unstructured environments.