PRIME: Physically-consistent Robotic Inertial and Motion Estimation for Legged and Humanoid Robots

TL;DR

PRIME is a novel optimization framework that refines robot motion estimates by ensuring physical consistency, accurately estimating contact forces and inertial parameters during contact-rich movements.

Contribution

It introduces a MAP-based approach with differentiable contact dynamics and friction modeling to improve motion estimation and inertial parameter identification in legged robots.

Findings

Enhanced trajectory consistency during contact-rich locomotion

Accurate inertial parameter estimation from real robot data

Force- and contact-annotated motion reconstructions for learning applications

Abstract

Humanoid and legged robots interact with the environment through intermittent contacts, making accurate motion estimation fundamentally dependent on reasoning about contact dynamics. However, standard sensing pipelines-whether based on onboard proprioception with Extended Kalman Filters (EKFs) or external motion capture systems-recover only kinematics, while contact forces, contact timing, and inertial parameters remain unobserved. As a result, purely kinematic reconstructions often violate rigid-body dynamics, particularly during contact-rich motions. To enable accurate motion estimation from onboard kinematics in real-world deployment, we propose PRIME (Physically-consistent Robotic Inertial and Motion Estimation), a Maximum A Posteriori (MAP) formulation that refines measured kinematics and actuator commands into a dynamically consistent trajectory while jointly estimating frictional contact forces and physically consistent inertial parameters. Our approach incorporates differentiable contact dynamics with smoothed complementarity constraints and an Anitescu-style friction model, yielding a smooth optimization problem that remains tractable across versatile contact transitions. We evaluate PRIME on contact-rich locomotion with quadrupedal robots and the Unitree G1 humanoid, demonstrating improved trajectory consistency and accurate inertial parameter identification. Beyond improving state estimation and feedback control with calibrated inertial parameters, PRIME produces force- and contact-annotated motion reconstructions from real robots in deployment, which can be used to provide high-quality data for downstream learning applications, including large-scale behavior modeling and robot foundation models.