PRIME: Physically-consistent Robotic Inertial and Motion Estimation for Legged and Humanoid Robots
Pith reviewed 2026-05-20 11:57 UTC · model grok-4.3
The pith
PRIME refines measured robot kinematics into dynamically consistent trajectories by jointly estimating contact forces and inertial parameters.
A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.
Core claim
PRIME is 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. The formulation incorporates differentiable contact dynamics with smoothed complementarity constraints and an Anitescu-style friction model, turning the problem into a tractable smooth optimization that handles contact transitions on quadrupedal and humanoid platforms.
What carries the argument
A single MAP optimization problem that enforces rigid-body dynamic consistency through a differentiable contact model with smoothed complementarity constraints and an Anitescu-style friction model.
If this is right
- Trajectory estimates satisfy rigid-body dynamics even during intermittent contact.
- Inertial parameters are identified accurately enough for use in feedback controllers.
- Force- and contact-annotated motion data become available from real-robot deployments.
- Calibrated inertial parameters improve downstream state estimation accuracy.
Where Pith is reading between the lines
- The resulting annotated trajectories could serve as higher-quality training data for learning-based controllers or foundation models.
- The batch optimization could be adapted into a receding-horizon version for online use during robot operation.
- Similar consistency constraints might be added to existing visual-inertial odometry pipelines for legged platforms.
Load-bearing premise
The smoothed complementarity constraints and Anitescu-style friction model produce a tractable optimization that accurately represents real intermittent contact dynamics without introducing large approximation errors.
What would settle it
Direct comparison of PRIME-estimated contact forces against independent force-torque sensor readings collected during the same locomotion sequences on the robot.
Figures
read the original 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.
Editorial analysis
A structured set of objections, weighed in public.
Referee Report
Summary. The paper proposes PRIME, a Maximum A Posteriori (MAP) optimization framework that refines onboard kinematic measurements and actuator commands for legged and humanoid robots into trajectories that satisfy rigid-body dynamics. It jointly estimates frictional contact forces and physically consistent inertial parameters by incorporating differentiable contact dynamics based on smoothed complementarity constraints and an Anitescu-style friction model. Evaluations on quadrupedal locomotion and the Unitree G1 humanoid are presented to demonstrate improved trajectory consistency and inertial parameter identification, with applications to state estimation, control, and data generation for learning.
Significance. If the central claims are substantiated, PRIME would provide a practical method for generating dynamically consistent, force-annotated motion reconstructions directly from real-robot proprioception. This addresses a persistent gap in contact-rich scenarios where purely kinematic estimates violate physics, and could supply higher-quality training data for robot foundation models while enabling better-calibrated inertial parameters for feedback control.
major comments (2)
- [Contact Dynamics Formulation] The tractability and accuracy claims rest on the smoothed complementarity constraints and Anitescu friction model (described in the contact dynamics section). The manuscript must quantify the approximation error these introduce during contact make/break and slip transitions, for instance by reporting the residual of the rigid-body dynamics equations on the optimized trajectories versus ground-truth force/torque measurements. Without such bounds, it is unclear whether the refined trajectories remain dynamically consistent within the tolerances required for the central claim.
- [Experimental Evaluation] The evaluation section reports improved consistency and accurate inertial identification on real robots, yet supplies no explicit quantitative metrics (e.g., RMS dynamic residual, force estimation error, or inertial parameter covariance) against baselines such as standard EKFs or prior contact-aware estimators. Inclusion of these numbers, together with ablation on smoothing parameters, is necessary to substantiate the performance claims.
minor comments (2)
- [Problem Formulation] Clarify the exact definition of the MAP objective and the weighting between kinematic, actuation, and dynamics terms; the current notation for the posterior can be ambiguous when multiple contact modes are active.
- [Results] Figures comparing raw versus PRIME-refined trajectories would benefit from overlaid force and contact-mode annotations to make the consistency improvement visually evident.
Simulated Author's Rebuttal
We thank the referee for their thoughtful and constructive review. The suggestions to strengthen the quantitative validation of the contact model and experimental comparisons are well taken. We address each major comment below and commit to revisions that directly incorporate the requested analyses.
read point-by-point responses
-
Referee: [Contact Dynamics Formulation] The tractability and accuracy claims rest on the smoothed complementarity constraints and Anitescu friction model (described in the contact dynamics section). The manuscript must quantify the approximation error these introduce during contact make/break and slip transitions, for instance by reporting the residual of the rigid-body dynamics equations on the optimized trajectories versus ground-truth force/torque measurements. Without such bounds, it is unclear whether the refined trajectories remain dynamically consistent within the tolerances required for the central claim.
Authors: We agree that explicit quantification of the approximation error from the smoothed complementarity constraints and Anitescu friction model is necessary. In the revised manuscript we will add a new subsection that reports the norm of the rigid-body dynamics residual (Newton-Euler equations) evaluated on the optimized trajectories after convergence. Because direct ground-truth force/torque sensing is unavailable on the real quadruped and Unitree G1 platforms, we will instead demonstrate that the MAP objective drives these residuals to levels comparable to sensor noise. To bound the smoothing-induced error during contact transitions we will include supplementary simulation experiments that compare against known ground-truth forces and torques, reporting peak and RMS discrepancies at make/break and slip events. An ablation over the smoothing parameter will also be provided. revision: yes
-
Referee: [Experimental Evaluation] The evaluation section reports improved consistency and accurate inertial identification on real robots, yet supplies no explicit quantitative metrics (e.g., RMS dynamic residual, force estimation error, or inertial parameter covariance) against baselines such as standard EKFs or prior contact-aware estimators. Inclusion of these numbers, together with ablation on smoothing parameters, is necessary to substantiate the performance claims.
Authors: We acknowledge that the current results would be strengthened by explicit numerical comparisons. The revised manuscript will include tables reporting RMS dynamic residuals, force estimation errors (in simulation where ground truth exists), and inertial-parameter covariance. Direct comparisons against a standard EKF and representative contact-aware estimators from the literature will be added. We will also present an ablation study on the contact smoothing parameters, showing their effect on the above metrics and on trajectory consistency. revision: yes
Circularity Check
PRIME applies standard rigid-body dynamics and established friction models within a new MAP optimization; no derivation reduces to its inputs by construction
full rationale
The paper formulates a MAP estimation problem that refines measured kinematics using rigid-body dynamics, differentiable contact models with smoothed complementarity constraints, and an Anitescu-style friction model to jointly recover trajectories, forces, and inertial parameters. These components are drawn from established literature rather than being defined in terms of the method's own outputs or fitted quantities. No equations or steps in the provided abstract and description reduce a claimed prediction or result to a tautological fit or self-citation chain; the central contribution is the optimization objective itself, which remains independent of the inputs it processes. The derivation is therefore self-contained against external benchmarks.
Axiom & Free-Parameter Ledger
axioms (2)
- domain assumption Rigid-body dynamics and contact forces govern the observed robot motion
- domain assumption Smoothed complementarity constraints and Anitescu friction model sufficiently approximate real intermittent contacts
Lean theorems connected to this paper
-
IndisputableMonolith/Foundation/RealityFromDistinction.leanreality_from_one_distinction unclear?
unclearRelation between the paper passage and the cited Recognition theorem.
PRIME ... incorporates differentiable contact dynamics with smoothed complementarity constraints and an Anitescu-style friction model, yielding a smooth optimization problem ... solved ... using Feasibility-Driven Differential Dynamic Programming (FDDP)
-
IndisputableMonolith/Cost/FunctionalEquation.leanwashburn_uniqueness_aczel unclear?
unclearRelation between the paper passage and the cited Recognition theorem.
smoothed contact dynamics ... log-barrier relaxation ... κ ... Anitescu’s cone complementarity conditions
What do these tags mean?
- matches
- The paper's claim is directly supported by a theorem in the formal canon.
- supports
- The theorem supports part of the paper's argument, but the paper may add assumptions or extra steps.
- extends
- The paper goes beyond the formal theorem; the theorem is a base layer rather than the whole result.
- uses
- The paper appears to rely on the theorem as machinery.
- contradicts
- The paper's claim conflicts with a theorem or certificate in the canon.
- unclear
- Pith found a possible connection, but the passage is too broad, indirect, or ambiguous to say the theorem truly supports the claim.
Reference graph
Works this paper leans on
-
[1]
Optimization-based control for dynamic legged robots,
P. M. Wensing, M. Posa, Y . Hu, A. Escande, N. Mansard, and A. Del Prete, “Optimization-based control for dynamic legged robots,” IEEE Transactions on Robotics, vol. 40, pp. 43–63, 2023
work page 2023
-
[2]
Multicontact locomotion of legged robots,
J. Carpentier and N. Mansard, “Multicontact locomotion of legged robots,”IEEE Transactions on Robotics, vol. 34, no. 6, pp. 1441–1460, 2018
work page 2018
-
[3]
Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation
R. Hartley, M. G. Jadidi, J. W. Grizzle, and R. M. Eustice, “Contact- aided invariant extended kalman filtering for legged robot state estima- tion,”arXiv preprint arXiv:1805.10410, 2018
work page internal anchor Pith review Pith/arXiv arXiv 2018
-
[4]
Invariant smoother for legged robot state estimation with dynamic contact event information,
Z. Yoon, J.-H. Kim, and H.-W. Park, “Invariant smoother for legged robot state estimation with dynamic contact event information,”IEEE Transactions on Robotics, vol. 40, pp. 193–212, 2023
work page 2023
-
[5]
Fast decentralized state estimation for legged robot locomotion via ekf and mhe,
J. Kang, Y . Wang, and X. Xiong, “Fast decentralized state estimation for legged robot locomotion via ekf and mhe,”IEEE Robotics and Automation Letters, vol. 9, no. 12, pp. 10 914–10 921, 2024
work page 2024
- [6]
-
[7]
Impact-invariant control: Maximizing control authority during impacts,
W. Yang and M. Posa, “Impact-invariant control: Maximizing control authority during impacts,”Autonomous Robots, vol. 49, no. 4, pp. 1–24, 2025
work page 2025
-
[8]
Fast contact-implicit model predictive control,
S. Le Cleac’h, T. A. Howell, S. Yang, C.-Y . Lee, J. Zhang, A. Bishop, M. Schwager, and Z. Manchester, “Fast contact-implicit model predictive control,”IEEE Transactions on Robotics, vol. 40, pp. 1617–1629, 2024
work page 2024
-
[9]
Inverse dynamics trajectory optimization for contact-implicit model predictive control,
V . Kurtz, A. Castro, A. ¨O. ¨Onol, and H. Lin, “Inverse dynamics trajectory optimization for contact-implicit model predictive control,” The International Journal of Robotics Research, vol. 45, no. 1, pp. 23– 40, 2026
work page 2026
-
[10]
Diffusion policy: Visuomotor policy learning via action diffusion,
C. Chi, Z. Xu, S. Feng, E. Cousineau, Y . Du, B. Burchfiel, R. Tedrake, and S. Song, “Diffusion policy: Visuomotor policy learning via action diffusion,”The International Journal of Robotics Research, vol. 44, no. 10-11, pp. 1684–1704, 2025
work page 2025
-
[11]
A survey on imitation learning for contact-rich tasks in robotics,
T. Tsuji, Y . Kato, G. Solak, H. Zhang, T. Petriˇc, F. Nori, and A. Ajoudani, “A survey on imitation learning for contact-rich tasks in robotics,”arXiv preprint arXiv:2506.13498, 2025
-
[12]
Prompt, plan, perform: Llm-based humanoid control via quantized imitation learning,
J. Sun, Q. Zhang, Y . Duan, X. Jiang, C. Cheng, and R. Xu, “Prompt, plan, perform: Llm-based humanoid control via quantized imitation learning,” in2024 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2024, pp. 16 236–16 242
work page 2024
-
[13]
OpenVLA: An Open-Source Vision-Language-Action Model
M. J. Kim, K. Pertsch, S. Karamcheti, T. Xiao, A. Balakrishna, S. Nair, R. Rafailov, E. Foster, G. Lam, P. Sanketiet al., “Openvla: An open- source vision-language-action model,”arXiv preprint arXiv:2406.09246, 2024
work page internal anchor Pith review Pith/arXiv arXiv 2024
-
[14]
A Survey on Vision-Language-Action Models for Embodied AI
Y . Ma, Z. Song, Y . Zhuang, J. Hao, and I. King, “A survey on vision-language-action models for embodied ai,”arXiv preprint arXiv:2405.14093, 2024
work page internal anchor Pith review Pith/arXiv arXiv 2024
-
[15]
What foundation models can bring for robot learning in manipulation: A survey,
D. Li, Y . Jin, Y . Sun, Y . A, H. Yu, J. Shi, X. Hao, P. Hao, H. Liu, X. Liet al., “What foundation models can bring for robot learning in manipulation: A survey,”The International Journal of Robotics Research, p. 02783649251390579, 2024
work page 2024
-
[16]
Foundation models in robotics: Applications, challenges, and the future,
R. Firoozi, J. Tucker, S. Tian, A. Majumdar, J. Sun, W. Liu, Y . Zhu, S. Song, A. Kapoor, K. Hausmanet al., “Foundation models in robotics: Applications, challenges, and the future,”The International Journal of Robotics Research, vol. 44, no. 5, pp. 701–739, 2025
work page 2025
-
[17]
Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models,
T. Pang, H. J. T. Suh, L. Yang, and R. Tedrake, “Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models,”IEEE Transactions on Robotics, vol. 39, no. 6, pp. 4691–4711, 2023
work page 2023
-
[18]
Optimization-based simulation of nonsmooth rigid multi- body dynamics,
M. Anitescu, “Optimization-based simulation of nonsmooth rigid multi- body dynamics,”Mathematical Programming, vol. 105, no. 1, pp. 113– 143, 2006
work page 2006
-
[19]
Parameter and contact force estimation of planar rigid-bodies undergoing frictional contact,
N. Fazeli, R. Kolbert, R. Tedrake, and A. Rodriguez, “Parameter and contact force estimation of planar rigid-bodies undergoing frictional contact,”The International Journal of Robotics Research, vol. 36, no. 13-14, pp. 1437–1454, 2017
work page 2017
-
[20]
A feasibility-driven approach to control-limited ddp,
C. Mastalli, W. Merkt, J. Marti-Saumell, H. Ferrolho, J. Sol `a, N. Mansard, and S. Vijayakumar, “A feasibility-driven approach to control-limited ddp,”Autonomous Robots, vol. 46, no. 8, pp. 985–1005, 2022
work page 2022
-
[21]
Robust legged robot state esti- mation using factor graph optimization,
D. Wisth, M. Camurri, and M. Fallon, “Robust legged robot state esti- mation using factor graph optimization,”IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 4507–4514, 2019
work page 2019
-
[22]
Simultaneous ground reaction force and state estimation via constrained moving horizon estimation,
J. Kang and X. Xiong, “Simultaneous ground reaction force and state estimation via constrained moving horizon estimation,” in2025 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2025, pp. 7080–7086
work page 2025
-
[23]
View: Visual-inertial external wrench estimator for legged robot,
J. Kang, H. Kim, and K.-S. Kim, “View: Visual-inertial external wrench estimator for legged robot,”IEEE Robotics and Automation Letters, vol. 8, no. 12, pp. 8366–8373, 2023
work page 2023
-
[24]
A geometric char- acterization of observability in inertial parameter identification,
P. M. Wensing, G. Niemeyer, and J.-J. E. Slotine, “A geometric char- acterization of observability in inertial parameter identification,”The International Journal of Robotics Research, vol. 43, no. 14, pp. 2274– 2302, 2024
work page 2024
-
[25]
Y . Ogawa, G. Venture, and C. Ott, “Dynamic parameters identification of a humanoid robot using joint torque sensors and/or contact forces,” in 2014 IEEE-RAS International Conference on Humanoid Robots, 2014, pp. 457–462
work page 2014
-
[26]
Physically-consistent parameter identification of robots in contact,
S. Khorshidi, M. Dawood, B. Nederkorn, M. Bennewitz, and M. Khadiv, “Physically-consistent parameter identification of robots in contact,” in2025 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2025, pp. 677–683
work page 2025
-
[27]
Multi-contact inertial param- eters estimation and localization in legged robots,
S. Martinez, R. Griffin, and C. Mastalli, “Multi-contact inertial param- eters estimation and localization in legged robots,”IEEE Robotics and Automation Letters, 2025
work page 2025
-
[28]
Differentiable simulation for physical system identification,
Q. Le Lidec, I. Kalevatykh, I. Laptev, C. Schmid, and J. Carpentier, “Differentiable simulation for physical system identification,”IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3413–3420, 2021
work page 2021
-
[29]
Y . Jiang, M. Yu, X. Zhu, M. Tomizuka, and X. Li, “Contact-implicit model predictive control for dexterous in-hand manipulation: A long- horizon and robust approach,” in2024 IEEE/RSJ International Confer- ence on Intelligent Robots and Systems (IROS), pp. 5260–5266
-
[30]
Predictive sampling: Real-time behaviour synthesis with mujoco,
T. Howell, N. Gileadi, S. Tunyasuvunakool, K. Zakka, T. Erez, and Y . Tassa, “Predictive sampling: Real-time behaviour synthesis with mujoco,”arXiv preprint arXiv:2212.00541, 2022
-
[31]
Sampling-based system identification with active exploration for legged robot sim2real learning,
N. Sobanbabu, G. He, T. He, Y . Yang, and G. Shi, “Sampling-based system identification with active exploration for legged robot sim2real learning,”arXiv preprint arXiv:2505.14266, 2025
-
[32]
An implicit time-stepping scheme for rigid body dynamics with coulomb friction,
D. Stewart and J. Trinkle, “An implicit time-stepping scheme for rigid body dynamics with coulomb friction,” inProceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), vol. 1, 2000, pp. 162–169 vol.1
work page 2000
-
[33]
Smooth parameterization of rigid-body inertia,
C. Rucker and P. M. Wensing, “Smooth parameterization of rigid-body inertia,”IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 2771– 2778, 2022
work page 2022
-
[34]
A convex quasistatic time-stepping scheme for rigid multibody systems with contact and friction,
T. Pang and R. Tedrake, “A convex quasistatic time-stepping scheme for rigid multibody systems with contact and friction,” in2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. 6614–6620
work page 2021
-
[35]
E. D. Andersen and K. D. Andersen, “The mosek interior point opti- mizer for linear programming: an implementation of the homogeneous algorithm,” inHigh performance optimization. Springer, 2000, pp. 197–232
work page 2000
-
[36]
G. Kim, D. Kang, J.-H. Kim, S. Hong, and H.-W. Park, “Contact-implicit model predictive control: Controlling diverse quadruped motions without pre-planned contact modes or trajectories,”The International Journal of Robotics Research, vol. 44, no. 3, pp. 486–510, 2025
work page 2025
-
[37]
C. K. Enders, “The performance of the full information maximum likelihood estimator in multiple regression models with missing data,” Educational and Psychological Measurement, vol. 61, no. 5, pp. 713– 740, 2001
work page 2001
-
[38]
iLQG/DDP Trajectory Optimization,
Y . Tassa, “iLQG/DDP Trajectory Optimization,” MATLAB Central File Exchange, 2022, accessed: 2026-01-31
work page 2022
-
[39]
Isaac Lab: A GPU-Accelerated Simulation Framework for Multi-Modal Robot Learning
M. Mittal, P. Roth, J. Tigue, A. Richard, O. Zhang, P. Du, A. Serrano- Munoz, X. Yao, R. Zurbr ¨ugg, N. Rudinet al., “Isaac lab: A gpu- accelerated simulation framework for multi-modal robot learning,”arXiv preprint arXiv:2511.04831, 2025
work page internal anchor Pith review Pith/arXiv arXiv 2025
-
[40]
J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The pinocchio c++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in2019 IEEE/SICE International Symposium on System Integration (SII). IEEE, 2019, pp. 614–619
work page 2019
-
[41]
Crocoddyl: An efficient and versatile framework for multi-contact optimal control,
C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayakumar, and N. Mansard, “Crocoddyl: An efficient and versatile framework for multi-contact optimal control,” in2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 2536–2542
work page 2020
-
[42]
System identification under constraints and disturbance: A bayesian estimation approach,
S. Martinez, S. Tonneau, and C. Mastalli, “System identification under constraints and disturbance: A bayesian estimation approach,”arXiv preprint arXiv:2602.16358, 2026
-
[43]
S. Caron, Q.-C. Pham, and Y . Nakamura, “Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas,” in2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 5107–5112
work page 2015
-
[44]
F. Liu, Z. Gu, Y . Cai, Z. Zhou, H. Jung, J. Jang, S. Zhao, S. Ha, Y . Chen, D. Xuet al., “Opt2skill: Imitating dynamically-feasible whole-body trajectories for versatile humanoid loco-manipulation,”IEEE Robotics and Automation Letters, 2025. VII. SUPPLEMENTARYMATERIALS The supplementary materials present additional results and analysis for the proposed P...
work page 2025
discussion (0)
Sign in with ORCID, Apple, or X to comment. Anyone can read and Pith papers without signing in.