Loco-manipulation demands coordinated whole-body motion to manipulate objects effectively while maintaining locomotion stability, presenting significant challenges for both planning and control. In this work, we propose a whole-body model predictive control (MPC) framework that directly optimizes joint torques through full-order inverse dynamics, enabling unified motion and force planning and execution within a single predictive layer. This approach allows emergent, physically consistent whole-body behaviors that account for the system’s dynamics and physical constraints.
We implement our MPC formulation using open software frameworks (Pinocchio and CasADi), along with the state-of-the-art interior-point solver Fatrop. In real-world experiments on a Unitree B2 quadruped equipped with a Unitree Z1 manipulator arm, our MPC formulation achieves real-time performance at 80 Hz. We demonstrate loco-manipulation tasks that demand fine control over the end-effector’s position and force to perform real-world interactions like pulling heavy loads, pushing boxes, and wiping whiteboards.
The whole-body MPC directly optimizes joint torques by enforcing full-order inverse dynamics through the Recursive Newton-Euler Algorithm (RNEA). It runs at 80 Hz using the interior-point solver Fatrop, which exploits the block-sparse structure of stage-wise constraints. The resulting torque commands are interpolated at 500 Hz and passed directly to the motors, along with PD feedback on joint positions and velocities (only required on hardware). This yields a simple and unified control pipeline that does not require a separate low-level whole-body controller to ensure dynamic feasibility.
