Merge pull request #165 from gbionics/copilot/fix-failing-github-actions-check

Fix inverted principal-axes rotation convention in USD inertia read/write

Author: Giulero

docs/requirements.txt

@@ -1,7 +1,7 @@
 sphinx
 sphinx-rtd-theme
 sphinx-book-theme
-sphinx-shibuya
+shibuya
 sphinx-autodoc-typehints
 sphinx-book-theme
 sphinx-copybutton

src/adam/model/conversions/usd.py

@@ -71,11 +71,10 @@ def _inertia_to_principal_axes(I: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
     if np.linalg.det(eigvecs) < 0.0:
         eigvecs[:, 0] *= -1.0
 
-    # USD stores principalAxes as the rotation from the body frame in which I
-    # is expressed to the principal-inertia frame, i.e. the transpose/inverse
-    # of the matrix whose columns are the principal axes expressed in the body
-    # frame.
-    quat_wxyz = R.from_matrix(eigvecs.T).as_quat(scalar_first=True)
+    # USD stores principalAxes as the rotation from the principal-inertia
+    # frame to the body frame in which I is expressed, i.e. the matrix whose
+    # columns are the principal axes expressed in the body frame.
+    quat_wxyz = R.from_matrix(eigvecs).as_quat(scalar_first=True)
     return eigvals, quat_wxyz
 
 

src/adam/model/usd_factory/usd_model.py

@@ -217,18 +217,18 @@ def _mass_api_to_inertial(self, prim: Any) -> USDInertial:
             else self.Gf.Quatf(1.0, 0.0, 0.0, 0.0)
         )
 
-        # USD stores principalAxes as the rotation from the link frame to the
-        # principal-inertia frame (R_link_to_principal).  Rotate the diagonal
-        # inertia tensor back into the link frame directly, avoiding any
+        # USD stores principalAxes as the rotation from the principal-inertia
+        # frame to the link frame (R_principal_to_link).  Rotate the diagonal
+        # inertia tensor into the link frame directly, avoiding any
         # Euler-angle conversion and the associated gimbal-lock singularity.
         #
-        #   I_link = R^T @ diag(Ixx, Iyy, Izz) @ R
+        #   I_link = R @ diag(Ixx, Iyy, Izz) @ R^T
         #
-        # where R = R.from_quat(principal_axes) maps link → principal frame.
+        # where R = R.from_quat(principal_axes) maps principal → link frame.
         R_principal = _rotation_from_usd_quat(principal_axes)
         R_mat = R_principal.as_matrix()
         I_diag = np.diag(diagonal_inertia)
-        I_link = R_mat.T @ I_diag @ R_mat
+        I_link = R_mat @ I_diag @ R_mat.T
 
         return USDInertial(
             mass=mass,