Add invalid link com diagnosis.

Author: duburcqa

genesis/engine/entities/rigid_entity/rigid_entity.py

@@ -24,7 +24,7 @@
 from genesis.utils import terrain as tu
 from genesis.utils import urdf as uu
 from genesis.utils.urdf import compose_inertial_properties, rotate_inertia
-from genesis.utils.misc import DeprecationError, broadcast_tensor, ti_to_torch
+from genesis.utils.misc import DeprecationError, broadcast_tensor, ti_to_numpy, ti_to_torch
 from genesis.engine.states.entities import RigidEntityState
 
 from ..base_entity import Entity
@@ -613,7 +613,7 @@ def _load_scene(self, morph, surface):
             # Unified parser handles both articulations and rigid bodies
             l_infos, links_j_infos, links_g_infos, eqs_info = parse_usd_rigid_entity(morph, surface)
 
-        # Make sure that the inertia matrix of all links is valid
+        # Make sure that the spatial inertia of all links is valid
         if not morph.recompute_inertia:
             for l_info in l_infos:
                 inertia_i = l_info.get("inertial_i")
@@ -3285,17 +3285,15 @@ def get_mass(self):
             an array of shape (n_envs,) with per-environment masses.
         """
         if self._enable_heterogeneous:
-            # Use solver's batched links_info for accurate per-environment masses
-            all_links_mass = self._solver.links_info.inertial_mass.to_numpy()
-            links_idx = np.arange(self.link_start, self.link_end)
-            # Shape: (n_links, n_envs) -> sum over links axis
-            return all_links_mass[links_idx].sum(axis=0)
-        else:
-            # Original behavior: sum link masses to scalar
-            mass = 0.0
-            for link in self.links:
-                mass += link.get_mass()
-            return mass
+            links_idx = slice(self.link_start, self.link_end)
+            links_mass = ti_to_numpy(self._solver.links_info.inertial_mass, None, links_idx, transpose=True)
+            return links_mass.sum(axis=1)
+
+        # Original behavior: sum link masses to scalar
+        mass = 0.0
+        for link in self.links:
+            mass += link.get_mass()
+        return mass
 
     # ------------------------------------------------------------------------------------
     # --------------------------------- naming methods -----------------------------------

genesis/engine/entities/rigid_entity/rigid_link.py

@@ -21,6 +21,7 @@
 
 # If mass is too small, we do not care much about spatial inertia discrepancy
 MASS_EPS = 0.005
+AABB_EPS = 0.002
 INERTIA_RATIO_MAX = 100.0
 
 
@@ -137,6 +138,8 @@ def _build(self):
         hint_mass = 0.0
         hint_com = np.zeros(3, dtype=gs.np_float)
         hint_inertia = np.zeros((3, 3), dtype=gs.np_float)
+        aabb_min = np.full((3,), float("inf"), dtype=gs.np_float)
+        aabb_max = np.full((3,), float("-inf"), dtype=gs.np_float)
         if not self._is_fixed:
             # Determine which geom list to use: geoms first, then vgeoms, then fallback
             if self._geoms:
@@ -220,15 +223,37 @@ def _build(self):
                     hint_mass, hint_com, hint_inertia, geom_mass, geom_com_link, geom_inertia_link
                 )
 
+            # Compute the bounding box of the links using both visual and collision geometries to be conservative
+            for geoms, is_visual in zip((self._geoms, self._vgeoms), (False, True)):
+                for geom in geoms:
+                    verts = geom.init_vverts if is_visual else geom.init_verts
+                    verts = gu.transform_by_trans_quat(verts, geom._init_pos, geom._init_quat)
+                    aabb_min = np.minimum(aabb_min, verts.min(axis=0))
+                    aabb_max = np.maximum(aabb_max, verts.max(axis=0))
+
         # Make sure that provided spatial inertia is consistent with the estimate from the geometries if not fixed
         if hint_mass > MASS_EPS:
+            if self._inertial_pos is not None:
+                tol = (aabb_max - aabb_min) * AABB_EPS + AABB_EPS
+                if not ((aabb_min - tol < self._inertial_pos) & (self._inertial_pos < aabb_max + tol)).all():
+                    com_str: list[str] = []
+                    aabb_str: list[str] = []
+                    for name, pos, axis_min, axis_max in zip(("x", "y", "z"), self._inertial_pos, aabb_min, aabb_max):
+                        com_str.append(f"{name}={pos:0.3f}")
+                        aabb_str.append(f"{name}=({axis_min:0.3f}, {axis_max:0.3f})")
+                    gs.logger.warning(
+                        f"Link '{self._name}' has dubious center of mass [{', '.join(com_str)}] compared to the "
+                        f"bounding box from geometry [{', '.join(aabb_str)}]."
+                    )
+
             if self._inertial_mass is not None:
                 if not (hint_mass / INERTIA_RATIO_MAX <= self._inertial_mass <= INERTIA_RATIO_MAX * hint_mass):
                     gs.logger.warning(
                         f"Link '{self._name}' has dubious mass {self._inertial_mass:0.3f} compared to the estimate "
                         f"from geometry {hint_mass:0.3f} given material density {rho:0.0f}."
                     )
                 hint_inertia *= self._inertial_mass / hint_mass
+
             if self._inertial_i is not None:
                 inertia_diag = np.diag(self._inertial_i)
                 hint_inertia_diag = np.diag(hint_inertia)

genesis/utils/mjcf.py

@@ -144,7 +144,7 @@ def build_model(xml, discard_visual, default_armature=None, merge_fixed_links=Fa
             compiler.attrib |= dict(
                 fusestatic="false",
                 strippath="false",
-                inertiafromgeom="false",  # This option is unreliable, so doing this ourselves
+                inertiafromgeom="false",
                 balanceinertia="false",
                 discardvisual="true" if discard_visual else "false",
                 autolimits="true",
@@ -173,7 +173,7 @@ def build_model(xml, discard_visual, default_armature=None, merge_fixed_links=Fa
             # Special treatment for URDF
             if is_urdf_file:
                 # Discard placeholder inertias that were used to avoid parsing failure
-                for i, link in enumerate(robot.links):
+                for link in robot.links:
                     if link.inertial is None:
                         body = mj.body(link.name)
                         body.inertia[:] = 0.0

genesis/utils/usd/usd_geometry.py

@@ -53,7 +53,7 @@ def parse_prim_geoms(
 
     if link_prim is not None and prim.IsA(UsdGeom.Gprim):
         # parse materials
-        geom_surface, geom_uvname, surface_id, bake_success = context.apply_surface(prim, surface)
+        geom_surface, geom_uvname, _surface_id, bake_success = context.apply_surface(prim, surface)
         gprim = UsdGeom.Gprim(prim)
         uvs = {geom_uvname: None}
 

genesis/utils/usd/usd_rigid_entity.py

@@ -94,11 +94,6 @@ def _parse_link(
         l_info["inertial_quat"] = usd_quat_to_numpy(mass_api.GetPrincipalAxesAttr().Get())
         l_info["inertial_i"] = np.diag(usd_pos_to_numpy(mass_api.GetDiagonalInertiaAttr().Get()))
         l_info["inertial_mass"] = float(mass_api.GetMassAttr().Get() or 0.0)
-    else:
-        l_info["inertial_pos"] = gu.zero_pos()
-        l_info["inertial_quat"] = gu.identity_quat()
-        l_info["inertial_i"] = None
-        l_info["inertial_mass"] = None
 
     j_infos = []
     for joint_prim, parent_idx, is_body1 in joints:
@@ -310,10 +305,11 @@ def _parse_link(
 
     if abs(1.0 - morph.scale) > gs.EPS:
         l_info["pos"] *= morph.scale
-        l_info["inertial_pos"] *= morph.scale
-        if l_info["inertial_mass"] is not None:
+        if l_info.get("inertial_pos") is not None:
+            l_info["inertial_pos"] *= morph.scale
+        if l_info.get("inertial_mass") is not None:
             l_info["inertial_mass"] *= morph.scale**3
-        if l_info["inertial_i"] is not None:
+        if l_info.get("inertial_i") is not None:
             l_info["inertial_i"] *= morph.scale**5
         l_info["invweight"][:] = -1.0
         for j_info in j_infos: