Whole-Body Inverse Dynamics MPC for Legged Loco-Manipulation
Lukas Molnar, Jin Cheng, Gabriele Fadini, Dongho Kang, Fatemeh Zargarbashi, Stelian Coros
AI summary
Problem
Coordinating precise motion and strong force interactions while maintaining locomotion stability is challenging for legged robots with arms, as traditional reduced-order planning and hierarchical tracking introduce complexity and dynamic inaccuracies.
Approach
The authors propose a unified whole-body MPC framework that directly optimizes joint torques and contact forces through full-order inverse dynamics, bypassing reduced-order approximations and hierarchical tracking layers.
Key results
- Real-time torque-level MPC solving at 80 Hz on hardware without separate tracking controllers
- Over 10× faster solve times using the structure-exploiting Fatrop interior-point solver
- Successful hardware demonstration of pulling 10 kg loads, pushing boxes while walking, and compliant interaction
- Inverse dynamics formulation reduces decision variables and solve time compared to forward dynamics and centroidal models
Why it matters
Simplifies the control architecture for legged manipulation robots while enabling accurate, real-time force and motion coordination for physically interactive real-world tasks.
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.