Whole-Body Inverse Dynamics MPC for Legged Loco-Manipulation

TL;DR

The paper presents a real-time, torque-level whole-body MPC for loco-manipulation that directly optimizes joint torques using full-body inverse dynamics. By formulating the OCP with a floating-base model, inverse-dynamics path constraints, adaptive time steps, and an efficient Fatrop solver implemented via Pinocchio and CasADi, the approach achieves real-time performance (80 Hz) and unified control of locomotion and manipulation on a Unitree B2 with a Z1 arm. Key contributions include the torque-level MPC formulation, a flexible open-source optimization framework, and hardware-demonstrated loco-manipulation tasks such as pulling heavy payloads and wiping tasks, along with a thorough discussion of sim-to-real discrepancies and future directions. The work advances practical, high-fidelity planning-and-control pipelines for legged systems performing physically interactive tasks with strong coupling between body and manipulator. Overall, the method enables emergent, dynamically consistent whole-body behaviors without a separate low-level tracking controller, contributing a scalable path toward robust loco-manipulation in real-world settings.

Abstract

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.

Figures (6)

• Figure 1: Unitree B2 quadruped with a Z1 manipulator arm: Pulling a 10kg load in stance (top), pushing a box against the wall while walking (bottom left), and demonstrating compliant human interaction (bottom right).
• Figure 2: The pipeline of whole-body inverse dynamics MPC.
• Figure 3: Whole-body adaptability: The MPC generates coordinated full-body motions to track an arm reference trajectory while trotting, automatically adjusting the base pose to reach both low and high targets.
• Figure 4: Disturbance rejection: The base velocity is perturbed laterally by 0.8m/s while trotting in place. The arm end-effector and base lateral velocities are visualized, along with the rear leg calf torques (RL and RR).
• Figure 5: Hardware demonstration of loco-manipulation tasks: Pulling a 10kg load while walking (left) and accurately wiping a whiteboard (right).