Safety-Critical Centralized Nonlinear MPC for Cooperative Payload Transportation by Two Quadrupedal Robots
Authors: Ruturaj S. Sambhus, Yicheng Zeng, Kapi Ketan Mehta, Jeeseop Kim, Kaveh Akbari Hamed
Organizations: R.S. Sambhus, Y. Zeng, K. K. Mehta, and K. Akbari Hamed are with the Department of Mechanical Engineering, Virginia Tech, Blacksburg, VA 24061, USA · J. Kim is with The University of Texas at El Paso, El Paso, TX 79968, USA
This paper presents a safety-critical centralized nonlinear model predictive control (NMPC) framework for cooperative payload transportation by two quadrupedal robots. The interconnected robot-payload system is modeled as a discrete-time nonlinear differential-algebraic system, capturing the coupled dynamics through holonomic constraints and interaction wrenches. To ensure safety in complex environments, we develop a control barrier function (CBF)-based NMPC formulation that enforces collision avoidance constraints for both the robots and the payload. The proposed approach retains the interaction wrenches as decision variables, resulting in a structured DAE-constrained optimal control problem that enables efficient real-time implementation. The effectiveness of the algorithm is validated through extensive hardware experiments on two Unitree Go2 platforms performing cooperative payload transportation in cluttered environments under mass and inertia uncertainty and external push disturbances.
Figures & tables
Fig. 1: Snapshot of two Unitree Go2 quadrupedal robots cooperatively transporting a shared payload using the proposed CBF-based centralized NMPC framework in a cluttered environment with cylindrical obstacles. The framework autonomously adjusts the orientations of the robots and the payload to avoid collisions while maintaining safe and stable transportation.
Fig. 2: Overview of the proposed layered control framework. The high-level CBF-based NMPC computes optimal trajectories for the interconnected robot–payload SRB system under holonomic constraints, while low-level nonlinear whole-body controllers enforce full-order robot dynamics.
Fig. 3: Illustration of the interconnected SRB model of the robotic agents and the shared payload, showing the rigid holonomic coupling constraints and interaction wrenches.
Fig. 4: Illustration of the rigid mechanism between the robots and the payload, with additional masses placed in the basket. The connection enforces translational rigidity, restricts relative roll and pitch motions, and allows relative yaw motion between the robots and the payload.
Fig. 5: Snapshots of all experiments: (a) Experiment 1 with a nominal payload of 5 kg; (b) Experiment 2 with an unmodeled payload of 11.2 kg; (c) Experiment 3 with an unmodeled payload of 11.2 kg under external push disturbances; and (d)–(e) Experiment 4 with different obstacle layouts.
Experiment
Mean
Std. Dev.
Exp. 1 (Nominal Payload)
11.42
0.6
Exp. 2 (Robustness to Payload Uncertainty)
11.56
0.42
Exp. 3 (Robustness to Disturbances)
11.45
0.5
TABLE I: Statistics of NMPC solve time (in milliseconds) across three experiments
Fig. 6: Reference and actual CoM velocity trajectories along the xy -axes for agent 1 during nominal cooperative payload transportation with a 5 kg payload. The yaw plot corresponds to the payload orientation, which is autonomously adjusted by the proposed algorithm to safely navigate through obstacles. The figure also shows the optimal GRFs and interaction wrench profiles (forces and torques) computed by the high-level CBF-based NMPC. The second column illustrates the HOCBF functions hi,ℓ for each agent and the six obstacles. All HOCBF functions remain nonnegative throughout the experiment, certifying safety for both the robots and the payload.
Fig. 7: CoM trajectories of the robotic agents and the shared payload in the xy -plane. The rigid mechanism is illustrated by a black line connecting the three systems. The obstacles are represented by solid circles, and the dashed regions around them denote the corresponding safe sets. The plot presents results for Experiments 1, 3, and 4.
Fig. 8: Reference and actual CoM velocity trajectories along the xy -axes for agent 1 during cooperative payload transportation with an uncertain payload of 11.2 kg, subject to unmodeled external push disturbances. The yaw plot corresponds to the payload orientation, which is autonomously adjusted by the proposed algorithm to safely navigate through obstacles. The figure also shows the optimal GRFs and interaction wrench profiles (forces and torques) computed by the high-level CBF-based NMPC. The second column illustrates the HOCBF functions hi,ℓ for each agent and the six obstacles, with the time intervals of external pushes highlighted. The HOCBF functions remain nonnegative throughout the experiment, except for a minor and transient violation in h2,6 immediately following a push disturbance; the controller subsequently drives agent 2 back to the safe set.
This paper presents a safety-critical distributed nonlinear model predictive control (DNMPC) framework for cooperative payload transportation by teams of quadrupedal robots. The proposed approach models the robotic team and the shared payload as a dynamically coupled networked system with rigid holonomic coupling constraints arising from cooperative transportation. To enable distributed real-time optimization, the centralized finite-horizon optimal control problem is decomposed into parallel local NMPC subproblems coordinated through the alternating direction method of multipliers (ADMM). The resulting distributed framework enforces consensus over both payload-state and interaction-wrench trajectories while explicitly incorporating acceleration-level holonomic coupling constraints within the distributed predictive control formulation. Safety-critical obstacle avoidance constraints for both the robotic agents and payload are enforced using higher-order control barrier functions (HOCBFs). The framework is validated through numerical simulations with teams of two, three, and four quadrupedal robots transporting shared payloads in cluttered environments. Real-time experiments on two- and three-robot teams demonstrate safe and robust transportation under payload uncertainty and external disturbances. Compared with centralized NMPC, the proposed framework achieves up to 23% reduction in average NLP solve time while maintaining comparable closed-loop performance. Ablation studies further demonstrate robustness to communication delays and show that explicit payload-state consensus and holonomic constraints substantially improve payload tracking and distributed coordination over existing wrench-only consensus formulations.
Ruturaj S. Sambhus, Kapi Ketan Mehta, Yicheng Zeng +1
Department of Mechanical Engineering, Virginia Tech, Blacksburg, VA 24061, USA
This paper presents a hierarchical density-based model predictive control framework for safe collaborative manipulation by multiple quadrupedal robots. The framework enables a team of robots to push a shared object to a desired pose using only the initial and goal poses, without requiring a precomputed reference trajectory. A centralized box-level MPC optimizes contact forces while enforcing a control-density constraint for goal convergence and obstacle avoidance. Each robot then solves its own distributed robot-level whole-body MPC, under a stated shared-information assumption, to track its moving contact location while accounting for static obstacles and the time-varying positions of neighboring robots. The approach is evaluated in MuJoCo using whole-body contact dynamics for two and three Unitree Go2 quadrupeds collaboratively pushing rigid objects through narrow passages. Comparisons with matched Control Barrier Function and RRT* based tracking baselines demonstrate the effectiveness of the proposed density-based formulation for push-only, force- and torque-coupled manipulation tasks. Implementation videos are available at https://jaggu2606.github.io/go2-density-mpc-pushing/
Jagannath Prasad Sahoo, Sriram S. K. S. Narayanan, Umesh Vaidya
Center for Artificial Intelligence and Robotics, Indian Institute of Technology Mandi, India · Department of Mechanical Engineering, Clemson University, Clemson, SC 29630, USA
This paper proposes a fully decentralized model predictive control (MPC) framework with control barrier function (CBF) constraints for safety-critical trajectory planning in multi-robot legged systems. The incorporation of CBF constraints introduces explicit inter-agent coupling, which prevents direct decomposition of the resulting optimal control problems. To address this challenge, we reformulate the centralized safety-critical MPC problem using a structured distributed optimization framework based on the alternating direction method of multipliers (ADMM). By introducing a novel node-edge splitting formulation with consensus constraints, the proposed approach decomposes the global problem into independent node-local and edge-local quadratic programs that can be solved in parallel using only neighbor-to-neighbor communication. This enables fully decentralized trajectory optimization with symmetric computational load across agents while preserving safety and dynamic feasibility. The proposed framework is integrated into a hierarchical locomotion control architecture for quadrupedal robots, combining high-level distributed trajectory planning, mid-level nonlinear MPC enforcing single rigid body dynamics, and low-level whole-body control enforcing full-order robot dynamics. The effectiveness of the proposed approach is demonstrated through hardware experiments on two Unitree Go2 quadrupedal robots and numerical simulations involving up to four robots navigating uncertain environments with rough terrain and external disturbances. The results show that the proposed distributed formulation achieves performance comparable to centralized MPC while reducing the average per-cycle planning time by up to 51% in the four-agent case, enabling efficient real-time decentralized implementation.
Yicheng Zeng, Ruturaj S. Sambhus, Basit Muhammad Imran +3
Department of Mechanical Engineering, Virginia Tech, Blacksburg, VA 24061, USA · The University of Texas at El Paso, El Paso, TX 79968, USA