DAIRLab / dair_pll

DAIRLab Physics-based Learning Library
BSD 3-Clause "New" or "Revised" License
10 stars 5 forks source link

More inertia fixes #4

Closed ebianchi closed 1 year ago

ebianchi commented 1 year ago

The previous PR with inertial parameter fixes did not sufficiently address the disparity in how inertial parameters in pi format require moments of inertia w.r.t. centers of mass, while inertial parameters in theta format require moments of inertia w.r.t. body origins. That previous PR managed to load inertial parameters in a way that made the original pi parameters match those provided in the URDF. However, it neglected to convert to/from theta format accurately, given pi format did not contain the correct moments of inertia expected by the Rucker and Wensing pi-to-theta conversion.

To address this discrepancy, this PR proposes to replace any mention of pi parameters with one of two alternatives: pi_o or pi_cm, both in the original pi format but with moments of inertia expressed w.r.t. the body origin or the body center of mass, respectively. Inertial parameters can be converted between these two via InertialParameterConverter.pi_o_to_pi_cm() and InertialParameterConverter.pi_cm_to_pi_o(), which perform the conversions using the Parallel Axis Theorem.

Thus, theta format is calculated via Rucker and Wensing's method, starting from pi_o format.

This PR primarily involves the additional necessary functionality in inertia.py to represent and convert to/from the new inertial parameter formats. Secondary files multibody_terms.py and urdf_utils.py feature variable name changes to clarify pi_o versus pi_cm format and function call changes to reference new methods in inertia.py.

ebianchi commented 1 year ago

The latest commit (e470600) corrected a bug in the pi_o to pi_cm and pi_cm to pi_o conversions which turned positive any negative I_xy, I_xz, I_yz off-diagonal moments of inertia. None of my original test systems matched this scenario, so I had missed it earlier. Thanks to some more extensive testing and @mposa for identifying this behavior.

ebianchi commented 1 year ago

I addressed and/or responded to each of the comments. The results of some lingering conversations include: