Whole-Body Inverse Dynamics MPC for Legged Loco-Manipulation

TL;DR

This paper introduces a whole-body inverse dynamics model predictive control framework for legged robots with manipulators, enabling real-time, physically consistent loco-manipulation tasks such as pulling, pushing, and wiping.

Contribution

It presents a unified MPC approach that directly optimizes joint torques using full inverse dynamics, allowing emergent whole-body behaviors for complex loco-manipulation.

Findings

Achieves real-time control at 80 Hz on hardware.

Successfully performs complex loco-manipulation tasks.

Demonstrates physically consistent behaviors in real-world experiments.

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.