pith. sign in

arxiv: 2605.17681 · v1 · pith:6WUNSHVZnew · submitted 2026-05-17 · 💻 cs.RO

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

classification 💻 cs.RO
keywords motion estimationlegged robotscontact dynamicsinertial parametersMAP optimizationhumanoid robotsphysically consistent estimationfriction model
0
0 comments X

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.

The paper presents PRIME as a Maximum A Posteriori optimization that starts from onboard sensor data and actuator commands on legged and humanoid robots. It adjusts the motion data until the entire trajectory satisfies rigid-body dynamics while also recovering the unknown frictional forces at each contact and the robot's inertial properties. Standard kinematic filters ignore these forces and parameters, so their outputs often break physical laws during walking or jumping. By embedding a differentiable contact model directly into the estimator, PRIME produces reconstructions that can feed directly into control loops or training datasets for behavior models.

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

These are editorial extensions of the paper, not claims the author makes directly.

  • 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

Figures reproduced from arXiv: 2605.17681 by Jiarong Kang, Kunzhao Ren, Tao Pang, Xiaobin Xiong.

Figure 1
Figure 1. Figure 1: Overview of PRIME: PRIME reconstructs physically consistent robot trajectories and hardware-matched inertial parameters from kinematic mea￾surements and onboard actuator sensing using a parameter full-information estimation framework with differentiable contact dynamics. parameters remain latent. As a result, reconstructed motions often violate rigid-body dynamics, particularly during phases involving inte… view at source ↗
Figure 2
Figure 2. Figure 2: (a) Trajectories of the Hopper. (b) Convergence of the inertial [PITH_FULL_IMAGE:figures/full_fig_p006_2.png] view at source ↗
Figure 3
Figure 3. Figure 3: Quadrupedal robot Go2 motion estimation and inertia identification experiments with a 4.6 kg payload attached beneath the torso. (a) A 10 s segment [PITH_FULL_IMAGE:figures/full_fig_p007_3.png] view at source ↗
Figure 4
Figure 4. Figure 4: Humanoid G1 motion estimation experiment. (a) Simulation results showing the reconstructed trajectory closely aligned with the ground truth. [PITH_FULL_IMAGE:figures/full_fig_p007_4.png] view at source ↗
Figure 5
Figure 5. Figure 5: Equivalent inertia-box of hardware experiments (a) Go2; (b) G1. [PITH_FULL_IMAGE:figures/full_fig_p008_5.png] view at source ↗
Figure 6
Figure 6. Figure 6: Force comparison for the G1 humanoid (left foot) over a dancing [PITH_FULL_IMAGE:figures/full_fig_p008_6.png] view at source ↗
Figure 7
Figure 7. Figure 7: (a) Robot experimental configurations for Unitree Go2 and G1. Point [PITH_FULL_IMAGE:figures/full_fig_p010_7.png] view at source ↗
Figure 8
Figure 8. Figure 8: Force estimation for the G1 humanoid right foot during a dancing [PITH_FULL_IMAGE:figures/full_fig_p011_8.png] view at source ↗
Figure 9
Figure 9. Figure 9: Humanoid G1 simulation force estimation using a four-point contact [PITH_FULL_IMAGE:figures/full_fig_p011_9.png] view at source ↗
Figure 11
Figure 11. Figure 11: Effects of the smoothing parameter of contact dynamics on the [PITH_FULL_IMAGE:figures/full_fig_p012_11.png] view at source ↗
Figure 12
Figure 12. Figure 12: Effects of the smoothing parameter of contact dynamics on the [PITH_FULL_IMAGE:figures/full_fig_p012_12.png] view at source ↗
Figure 16
Figure 16. Figure 16: Joint torque estimation of Go2 over diverse motion. [PITH_FULL_IMAGE:figures/full_fig_p013_16.png] view at source ↗
Figure 13
Figure 13. Figure 13: Floating-base estimation of Go2 over diverse motions. The roll [PITH_FULL_IMAGE:figures/full_fig_p013_13.png] view at source ↗
Figure 17
Figure 17. Figure 17: Floating base estimation of G1 humanoid over a dancing motion. [PITH_FULL_IMAGE:figures/full_fig_p013_17.png] view at source ↗
Figure 18
Figure 18. Figure 18: Joint position estimation of G1 humanoid over a dancing motion. [PITH_FULL_IMAGE:figures/full_fig_p013_18.png] view at source ↗
Figure 20
Figure 20. Figure 20: Joint torque estimation of G1 humanoid over a dancing motion. [PITH_FULL_IMAGE:figures/full_fig_p014_20.png] view at source ↗
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.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit. Tearing a paper down is the easy half of reading it; the pith above is the substance, this is the friction.

Referee Report

2 major / 2 minor

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)
  1. [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.
  2. [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)
  1. [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.
  2. [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

2 responses · 0 unresolved

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
  1. 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

  2. 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

0 steps flagged

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

0 free parameters · 2 axioms · 0 invented entities

Review performed on abstract only; full derivation, parameter choices, and model assumptions cannot be audited without the manuscript body.

axioms (2)
  • domain assumption Rigid-body dynamics and contact forces govern the observed robot motion
    Invoked as the target consistency constraint in the MAP objective.
  • domain assumption Smoothed complementarity constraints and Anitescu friction model sufficiently approximate real intermittent contacts
    Required to keep the optimization tractable and differentiable.

pith-pipeline@v0.9.0 · 5777 in / 1380 out tokens · 45298 ms · 2026-05-20T11:57:48.323065+00:00 · methodology

discussion (0)

Sign in with ORCID, Apple, or X to comment. Anyone can read and Pith papers without signing in.

Lean theorems connected to this paper

Citations machine-checked in the Pith Canon. Every link opens the source theorem in the public Lean library.

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

44 extracted references · 44 canonical work pages · 4 internal anchors

  1. [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

  2. [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

  3. [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

  4. [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

  5. [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

  6. [6]

    Karen Liu

    Y . Ze, S. Zhao, W. Wang, A. Kanazawa, R. Duan, P. Abbeel, G. Shi, J. Wu, and C. K. Liu, “Twist2: Scalable, portable, and holistic humanoid data collection system,”arXiv preprint arXiv:2511.02832, 2025

  7. [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

  8. [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

  9. [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

  10. [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

  11. [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. [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

  13. [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

  14. [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

  15. [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

  16. [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

  17. [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

  18. [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

  19. [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

  20. [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

  21. [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

  22. [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

  23. [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

  24. [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

  25. [25]

    Dynamic parameters identification of a humanoid robot using joint torque sensors and/or contact forces,

    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

  26. [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

  27. [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

  28. [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

  29. [29]

    Contact-implicit model predictive control for dexterous in-hand manipulation: A long- horizon and robust approach,

    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. [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. [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. [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

  33. [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

  34. [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

  35. [35]

    The mosek interior point opti- mizer for linear programming: an implementation of the homogeneous algorithm,

    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

  36. [36]

    Contact-implicit model predictive control: Controlling diverse quadruped motions without pre-planned contact modes or trajectories,

    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

  37. [37]

    The performance of the full information maximum likelihood estimator in multiple regression models with missing data,

    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

  38. [38]

    iLQG/DDP Trajectory Optimization,

    Y . Tassa, “iLQG/DDP Trajectory Optimization,” MATLAB Central File Exchange, 2022, accessed: 2026-01-31

  39. [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

  40. [40]

    The pinocchio c++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,

    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

  41. [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

  42. [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. [43]

    Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas,

    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

  44. [44]

    Opt2skill: Imitating dynamically-feasible whole-body trajectories for versatile humanoid loco-manipulation,

    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...