[BUG FIX] Apply the per-geom density in MJCF asset.

genesis/engine/entities/rigid_entity/rigid_entity.py

@@ -1009,11 +1009,6 @@ def _align_free_roots(self):
                 # and estimated masses would make the anchor density-dependent, and a kinematic entity has no density
                 # to fall back on, so alignment could differ from the rigid counterpart. Require all-or-none and raise
                 # otherwise.
-                if None in mass_explicit:
-                    gs.raise_exception(
-                        f"Entity '{self.uid}': a link of an aligned free body mixes geoms with and without an "
-                        "authored density. Author a density on all of its geoms or none of them."
-                    )
                 if len(mass_explicit) > 1:
                     gs.raise_exception(
                         f"Entity '{self.uid}': an aligned free body mixes explicit (mass or authored per-geom "
@@ -1243,6 +1238,7 @@ def _postprocess_geoms_info(self, morph, g_infos, is_robot):
         if isinstance(self.material, gs.materials.Rigid) and self.material.rho is not None:
             for g_info in g_infos:
                 g_info.pop("density", None)
+                g_info.pop("mass", None)
 
         cg_infos, vg_infos = [], []
         for g_info in g_infos:
@@ -1315,13 +1311,19 @@ def _finalize_inertial(
     ):
         """Compute a link's load-time inertial data (see 'LinkInertialInfo').
 
-        The align-anchor inertial weighs each collision geom by its authored density, falling back to unit density,
-        so it never needs the material density - which a kinematic entity does not have. The geometry hint consumed
-        by 'RigidLink._build' uses the resolved material density as fallback instead, and falls back to the visual
-        geoms for a link without collision geometry.
+        The align-anchor inertial weighs each collision geom by its authored density, falling back to the default
+        density, so a link mixing authored and unauthored geoms still composes at the right relative weights. The
+        anchor never needs the material density - which a kinematic entity does not have - because a material that
+        states one drops every authored density (see '_add_by_info'), leaving the anchor density-independent. The
+        geometry hint consumed by 'RigidLink._build' uses the resolved material density instead, and falls back to
+        the visual geoms for a link without collision geometry.
         """
+        if self._solver._enable_mujoco_compatibility:
+            rho_default = RHO_MUJOCO
+        else:
+            rho_default = RHO_ROBOT if is_robot else RHO_OBJECT
         if cg_infos:
-            hint = compose_inertial_from_g_infos(cg_infos, rho=1.0)
+            hint = compose_inertial_from_g_infos(cg_infos, rho=rho_default)
         else:
             hint = InertialProperties(0.0, np.zeros(3, dtype=gs.np_float), np.zeros((3, 3), dtype=gs.np_float))
         props = finalize_inertial(
@@ -1330,25 +1332,22 @@ def _finalize_inertial(
         if explicit_mass is not None and explicit_mass > 0.0:
             is_mass_explicit = True
         else:
-            geoms_with_density = sum(g_info.get("density") is not None for g_info in cg_infos)
-            if geoms_with_density == 0:
-                is_mass_explicit = False
-            elif geoms_with_density == len(cg_infos):
-                is_mass_explicit = True
-            else:
-                is_mass_explicit = None
+            # A geom the asset weighs is anchored at that weight, and one it does not is anchored at the default the
+            # dynamics falls back on too, so any link the asset weighs at all anchors at its own dynamics mass.
+            is_mass_explicit = any(
+                g_info.get("density") is not None or g_info.get("mass") is not None for g_info in cg_infos
+            )
 
         dynamics_hint = None
         if isinstance(self.material, gs.materials.Rigid):
-            rho = self.material.rho
-            if rho is None:
-                if self._solver._enable_mujoco_compatibility:
-                    rho = RHO_MUJOCO
-                else:
-                    rho = RHO_ROBOT if is_robot else RHO_OBJECT
+            rho = self.material.rho if self.material.rho is not None else rho_default
             hint_g_infos = cg_infos if cg_infos else vg_infos
             if hint_g_infos:
                 dynamics_hint = compose_inertial_from_g_infos(hint_g_infos, rho)
+                # Collision geoms stating a zero mass leave nothing to weigh, so the visual geoms stand in as they
+                # do for a link carrying no collision geometry at all.
+                if dynamics_hint.mass <= gs.EPS and hint_g_infos is not vg_infos and vg_infos:
+                    dynamics_hint = compose_inertial_from_g_infos(vg_infos, rho)
             else:
                 dynamics_hint = InertialProperties(
                     0.0, np.zeros(3, dtype=gs.np_float), np.zeros((3, 3), dtype=gs.np_float)

genesis/engine/entities/rigid_entity/rigid_link.py

@@ -146,20 +146,28 @@ def compose_inertial_from_g_infos(g_infos: Sequence[dict], rho: float) -> Inerti
     g_infos : list[dict]
         Parsed geom infos to compute inertial from.
     rho : float
-        Material density (kg/m^3), used for every geom info without its own authored density.
+        Material density (kg/m^3), used for every geom info that states neither a mass nor a density of its own.
     """
-    geoms_inertial_info = tuple(
-        GeomInertialInfo(
-            get_local_inertial_from_geom_info(
-                {"type": gs.GEOM_TYPE.MESH, "mesh": g_info["vmesh"]} if "vmesh" in g_info else g_info,
-                rho if g_info.get("density") is None else g_info["density"],
-            ),
-            np.asarray(g_info.get("pos", gu.zero_pos()), dtype=gs.np_float),
-            np.asarray(g_info.get("quat", gu.identity_quat()), dtype=gs.np_float),
+    geoms_inertial_info = []
+    for g_info in g_infos:
+        info = {"type": gs.GEOM_TYPE.MESH, "mesh": g_info["vmesh"]} if "vmesh" in g_info else g_info
+        geom_mass = g_info.get("mass")
+        if geom_mass is None:
+            props = get_local_inertial_from_geom_info(info, rho if g_info.get("density") is None else g_info["density"])
+        else:
+            # Unit density yields the volume as the mass, and both scale linearly with it, so a stated mass rescales
+            # them exactly. A volume-less geom (a plane) weighs nothing whatever it states.
+            props = get_local_inertial_from_geom_info(info, 1.0)
+            if props.mass > gs.EPS:
+                props = InertialProperties(geom_mass, props.com, props.i * (geom_mass / props.mass))
+        geoms_inertial_info.append(
+            GeomInertialInfo(
+                props,
+                np.asarray(g_info.get("pos", gu.zero_pos()), dtype=gs.np_float),
+                np.asarray(g_info.get("quat", gu.identity_quat()), dtype=gs.np_float),
+            )
         )
-        for g_info in g_infos
-    )
-    return compose_inertial_properties(geoms_inertial_info)
+    return compose_inertial_properties(tuple(geoms_inertial_info))
 
 
 class LinkInertial(NamedTuple):
@@ -179,14 +187,13 @@ class LinkInertialInfo(NamedTuple):
 
     Computed while the parsed geom infos (and their authored per-geom densities) are still available, and consumed
     by the post-load passes. 'props' feeds the align anchor. 'is_mass_explicit' feeds the all-or-none source check
-    in '_align_free_roots': True when the mass is explicit in the asset (an explicit mass, or an authored density on
-    every geom), False for a pure geometry estimate (the true mass is a uniform material-density rescale of it), and
-    None when the link mixes geoms with and without an authored density (neither explicit nor uniformly rescalable).
+    in '_align_free_roots': True when the asset weighs the link (an explicit mass, or a density on any of its geoms),
+    False for a pure geometry estimate, whose true mass is a uniform material-density rescale of it.
     'hint' is the material-density-resolved geometry estimate consumed by 'RigidLink._build' (None for kinematic
     entities, which have no dynamics)."""
 
     props: LinkInertial
-    is_mass_explicit: bool | None
+    is_mass_explicit: bool
     hint: InertialProperties | None
 
 

genesis/utils/mesh.py

@@ -507,6 +507,7 @@ def postprocess_collision_geoms(
         geom_type = g_info.get("type")
         friction = g_info.get("friction")
         density = g_info.get("density")
+        geom_mass = g_info.get("mass")
         sol_params = g_info.get("sol_params")
         key_parts += [
             np.ascontiguousarray(mesh.verts),
@@ -516,6 +517,7 @@ def postprocess_collision_geoms(
             int(g_info.get("conaffinity", 0)),
             float("nan") if friction is None else float(friction),
             float("nan") if density is None else float(density),
+            float("nan") if geom_mass is None else float(geom_mass),
             np.zeros(0) if sol_params is None else np.ascontiguousarray(sol_params, dtype=np.float64),
             np.ascontiguousarray(g_info.get("pos", gu.zero_pos()), dtype=np.float64),
             np.ascontiguousarray(g_info.get("quat", gu.identity_quat()), dtype=np.float64),
@@ -679,6 +681,10 @@ def _postprocess_collision_geoms_impl(
                     first_g_info = g_infos[fusion_group[0]]
                     if (
                         first_g_info["type"] not in (gs.GEOM_TYPE.PLANE, gs.GEOM_TYPE.TERRAIN)
+                        # A fused geom carries one mass at most, which cannot stand for what several stated, so a
+                        # geom stating its own mass never joins a group.
+                        and first_g_info.get("mass") is None
+                        and g_info.get("mass") is None
                         and all(first_g_info.get(name) == g_info.get(name) for name in ("contype", "conaffinity"))
                         and all(
                             np.allclose(first_g_info.get(name, np.nan), g_info.get(name, np.nan), equal_nan=True)
@@ -799,7 +805,15 @@ def _postprocess_collision_geoms_impl(
                     )
                     for tmesh in tmeshes
                 ]
-                _g_infos += [{**g_info, **dict(mesh=mesh)} for mesh in meshes]
+                hull_infos = [{**g_info, **dict(mesh=mesh)} for mesh in meshes]
+                geom_mass = g_info.get("mass")
+                if geom_mass is not None:
+                    # A stated mass is extensive, so each hull takes the share of it that its volume represents.
+                    volumes = np.array([mesh.trimesh.volume for mesh in meshes])
+                    volume_total = volumes.sum()
+                    for hull_info, volume in zip(hull_infos, volumes):
+                        hull_info["mass"] = geom_mass * volume / volume_total if volume_total > gs.EPS else 0.0
+                _g_infos += hull_infos
             else:
                 _g_infos.append(g_info)
         g_infos = _g_infos

genesis/utils/mjcf.py

@@ -3,6 +3,7 @@
 from pathlib import Path
 from itertools import chain
 from bisect import bisect_right
+from typing import NamedTuple
 
 # Note the importing mujoco with env var `MUJOCO_GL=EGL` forcibly defines `PYOPENGL_PLATFORM=egl`
 import mujoco
@@ -22,6 +23,26 @@
 
 
 MIN_TIMECONST = np.finfo(np.double).eps
+# Mujoco resolves every geom's density, so a geom authoring this value is indistinguishable from one authoring none.
+MUJOCO_DEFAULT_DENSITY = 1000.0
+
+
+class GeomMassSource(NamedTuple):
+    """What a geom states about its own mass, as the two alternatives Mujoco accepts. Both None when it states
+    nothing, and 'mass' wins over 'density' where the asset gives both, as it does for Mujoco."""
+
+    density: float | None
+    mass: float | None
+
+
+class MjcfModel(NamedTuple):
+    """A compiled Mujoco model and what each of its geoms states about its mass, indexed by geom id.
+
+    The compiled model folds both into the body mass, so they are read off the spec while still available.
+    """
+
+    model: mujoco.MjModel
+    geoms_mass_source: tuple[GeomMassSource, ...]
 
 
 def get_model_name(file_path):
@@ -225,7 +246,15 @@ def build_model(
         with open(os.devnull, "w") as stderr, redirect_libc_stderr(stderr):
             # Parse updated URDF file as a string
             data = ET.tostring(root, encoding="utf8")
-            mj = mujoco.MjModel.from_xml_string(data)
+            # Compiling through the spec keeps the authored densities reachable, its geom ids matching the model's.
+            spec = mujoco.MjSpec.from_string(data)
+            mj = spec.compile()
+            geoms_mass_source = [GeomMassSource(None, None)] * mj.ngeom
+            for spec_geom in spec.geoms:
+                # Mujoco leaves the mass NaN unless the geom states one, and resolves the density either way.
+                geom_mass = None if np.isnan(spec_geom.mass) else spec_geom.mass
+                geom_density = spec_geom.density if spec_geom.density != MUJOCO_DEFAULT_DENSITY else None
+                geoms_mass_source[spec_geom.id] = GeomMassSource(geom_density, geom_mass)
 
             # Special treatment for URDF
             if is_urdf_file:
@@ -242,11 +271,12 @@ def build_model(
                 mj.geom_solref[:, 0] = MIN_TIMECONST
                 mj.eq_solref[:, 0] = MIN_TIMECONST
     elif isinstance(xml, mujoco.MjModel):
-        mj = xml
+        # An already-compiled model has no spec to read what its geoms state from.
+        mj, geoms_mass_source = xml, [GeomMassSource(None, None)] * xml.ngeom
     else:
         gs.raise_exception(f"'{xml}' is not a valid MJCF or URDF file.")
 
-    return mj
+    return MjcfModel(mj, tuple(geoms_mass_source))
 
 
 def parse_xml(morph, surface, rigid_options=None):
@@ -258,7 +288,7 @@ def parse_xml(morph, surface, rigid_options=None):
 
     # Build model from XML (either URDF or MJCF)
     exclude_ground_plane = isinstance(morph, gs.morphs.MJCF) and morph.exclude_ground_plane
-    mj = build_model(
+    mj, geoms_mass_source = build_model(
         morph.file,
         not morph.visualization,
         morph.default_armature,
@@ -273,7 +303,7 @@ def parse_xml(morph, surface, rigid_options=None):
     #     gs.logger.warning("(MJCF) Tendon not supported")
 
     # Parse all geometries grouped by parent joint (or world)
-    links_g_infos = parse_geoms(mj, morph.scale, surface, morph.file)
+    links_g_infos = parse_geoms(mj, geoms_mass_source, morph.scale, surface, morph.file)
 
     # Parse all bodies (links and joints)
     l_infos, links_j_infos = parse_links(mj, morph.scale)
@@ -517,7 +547,7 @@ def parse_links(mj, scale):
     return l_infos, j_infos
 
 
-def parse_geom(mj, i_g, scale, surface, xml_path):
+def parse_geom(mj, i_g, geom_mass_source, scale, surface, xml_path):
     mj_geom = mj.geom(i_g)
 
     geom_size = mj_geom.size
@@ -740,6 +770,12 @@ def parse_geom(mj, i_g, scale, surface, xml_path):
         "friction_rolling": mj_geom.friction[2] if mj_geom.condim[0] >= 6 else 0.0,
         "sol_params": np.concatenate((mj_geom.solref, mj_geom.solimp)),
     }
+    # Fusion grouping diffs these keys against a nan default when absent, so a None would raise instead.
+    if geom_mass_source.density is not None:
+        info["density"] = geom_mass_source.density
+    if geom_mass_source.mass is not None:
+        # A density needs no scaling, whereas a stated mass tracks the volume the morph scale gives the geom.
+        info["mass"] = geom_mass_source.mass * scale**3
     if is_col:
         info["mesh"] = mesh
     else:
@@ -748,7 +784,7 @@ def parse_geom(mj, i_g, scale, surface, xml_path):
     return info
 
 
-def parse_geoms(mj, scale, surface, xml_path):
+def parse_geoms(mj, geoms_mass_source, scale, surface, xml_path):
     links_g_info = [[] for _ in range(mj.nbody)]
 
     # Loop over all geometries sequentially
@@ -758,7 +794,7 @@ def parse_geoms(mj, scale, surface, xml_path):
             continue
 
         # try parsing a given geometry
-        g_info = parse_geom(mj, i_g, scale, surface, xml_path)
+        g_info = parse_geom(mj, i_g, geoms_mass_source[i_g], scale, surface, xml_path)
         if g_info is None:
             continue
 

tests/rigid/conftest.py

@@ -436,6 +436,52 @@ def joint_with_partial_dynamics(joint_damping, joint_friction):
     return urdf
 
 
+@pytest.fixture(scope="session")
+def authored_geom_mass_mjcf():
+    """Generate an MJCF whose identical boxes state their mass as a geom density, a default-class density, a geom
+    mass, and not at all, beside bodies pairing a stated geom with an unstated one, fusing two stated masses, and
+    leaving a weightless collision geom beside a visual one."""
+    mjcf = ET.Element("mujoco", model="authored_geom_mass")
+    default = ET.SubElement(mjcf, "default")
+    ET.SubElement(ET.SubElement(default, "default", {"class": "light"}), "geom", density="100")
+
+    worldbody = ET.SubElement(mjcf, "worldbody")
+    for name, attrib in (
+        ("on_geom", dict(density="250")),
+        ("on_class", {"class": "light"}),
+        ("on_mass", dict(mass="5")),
+        ("unstated", {}),
+    ):
+        body = ET.SubElement(worldbody, "body", name=name, pos="0.0 0.0 1.0")
+        ET.SubElement(body, "freejoint")
+        ET.SubElement(body, "geom", type="box", size="0.1 0.1 0.1", **attrib)
+
+    mixed = ET.SubElement(worldbody, "body", name="mixed", pos="0.0 0.0 1.0")
+    ET.SubElement(mixed, "freejoint")
+    ET.SubElement(mixed, "geom", type="box", size="0.1 0.1 0.1", pos="-0.3 0.0 0.0", density="250")
+    ET.SubElement(mixed, "geom", type="box", size="0.1 0.1 0.1", pos="0.3 0.0 0.0")
+
+    # A stated mass is extensive, so these two must survive the geom fusion that a shared mass source invites.
+    fused = ET.SubElement(worldbody, "body", name="fused", pos="0.0 0.0 1.0")
+    ET.SubElement(fused, "freejoint")
+    ET.SubElement(fused, "geom", type="box", size="0.1 0.1 0.1", pos="-0.3 0.0 0.0", mass="5")
+    ET.SubElement(fused, "geom", type="box", size="0.1 0.1 0.1", pos="0.3 0.0 0.0", mass="5")
+
+    # Convex decomposition splits one stated mass across the hulls it produces.
+    asset = ET.SubElement(mjcf, "asset")
+    ET.SubElement(asset, "mesh", name="bunny", file=os.path.join(get_assets_dir(), "meshes/bunny.obj"))
+    decomposed = ET.SubElement(worldbody, "body", name="decomposed", pos="0.0 0.0 1.0")
+    ET.SubElement(decomposed, "freejoint")
+    ET.SubElement(decomposed, "geom", type="mesh", mesh="bunny", mass="5")
+
+    # A collision geom weighing nothing leaves the visual geom to carry the link.
+    weightless = ET.SubElement(worldbody, "body", name="weightless", pos="0.0 0.0 1.0")
+    ET.SubElement(weightless, "freejoint")
+    ET.SubElement(weightless, "geom", type="box", size="0.1 0.1 0.1", mass="0")
+    ET.SubElement(weightless, "geom", type="box", size="0.1 0.1 0.1", contype="0", conaffinity="0")
+    return ET.tostring(mjcf, encoding="unicode")
+
+
 @pytest.fixture(scope="session")
 def undefined_inertia():
     """Generate a URDF with a single link that has no inertial element."""