[BUG FIX] Fix support of unspecified origin in URDF. (#2499)

Author: duburcqa

genesis/engine/entities/rigid_entity/rigid_link.py

@@ -659,11 +659,18 @@ def _build(self):
                     )
             if self._inertial_mass is None:
                 self._inertial_mass = hint_mass
-            if self._inertial_pos is None:
+            if self._inertial_pos is None or self._inertial_i is None:
+                if self._inertial_pos is not None and self._inertial_i is None:
+                    gs.logger.warning(
+                        f"Ignoring center of mass of link '{self.name}' because inertia matrix is not specified."
+                    )
+                elif self._inertial_pos is None and self._inertial_i is not None:
+                    gs.logger.warning(
+                        f"Ignoring inertia matrix of link '{self.name}' because center of mass is not specified."
+                    )
                 self._inertial_pos = hint_com
-            self._inertial_quat = gu.identity_quat()
-            if self._inertial_i is None:
                 self._inertial_i = hint_inertia
+            self._inertial_quat = gu.identity_quat()
 
         # FIXME: Setting zero mass even for fixed links breaks physics for some reason...
         # For non-fixed links, it must be non-zero in case for coupling with deformable body solvers.

genesis/ext/urdfpy/urdf.py

@@ -1100,12 +1100,12 @@ def origin(self):
 
     @origin.setter
     def origin(self, value):
-        self._origin = configure_origin(value)
+        self._origin = configure_origin(value, default=True)
 
     @classmethod
     def _from_xml(cls, node, root, path):
         kwargs = cls._parse(node, root, path)
-        kwargs["origin"] = parse_origin(node)
+        kwargs["origin"] = parse_origin(node, default=True)
         return Collision(**kwargs)
 
     def _to_xml(self, parent, path):
@@ -1196,7 +1196,7 @@ def origin(self):
 
     @origin.setter
     def origin(self, value):
-        self._origin = configure_origin(value)
+        self._origin = configure_origin(value, default=True)
 
     @property
     def material(self):
@@ -1213,7 +1213,7 @@ def material(self, value):
     @classmethod
     def _from_xml(cls, node, root, path):
         kwargs = cls._parse(node, root, path)
-        kwargs["origin"] = parse_origin(node)
+        kwargs["origin"] = parse_origin(node, default=True)
         return Visual(**kwargs)
 
     def _to_xml(self, parent, path):
@@ -1296,11 +1296,11 @@ def origin(self):
 
     @origin.setter
     def origin(self, value):
-        self._origin = configure_origin(value)
+        self._origin = configure_origin(value, default=False)
 
     @classmethod
     def _from_xml(cls, node, root, path):
-        origin = parse_origin(node)
+        origin = parse_origin(node, default=False)
         mass = float(node.find("mass").attrib["value"])
         n = node.find("inertia")
         xx = float(n.attrib["ixx"])
@@ -1314,7 +1314,8 @@ def _from_xml(cls, node, root, path):
 
     def _to_xml(self, parent, path):
         node = ET.Element("inertial")
-        node.append(unparse_origin(self.origin))
+        if self.origin is not None:
+            node.append(unparse_origin(self.origin))
         mass = ET.Element("mass")
         mass.attrib["value"] = str(self.mass)
         node.append(mass)
@@ -2226,7 +2227,7 @@ def origin(self):
 
     @origin.setter
     def origin(self, value):
-        self._origin = configure_origin(value)
+        self._origin = configure_origin(value, default=True)
 
     @property
     def limit(self):
@@ -2436,7 +2437,7 @@ def _from_xml(cls, node, root, path):
         if axis is not None:
             axis = np.fromstring(axis.attrib["xyz"], sep=" ")
         kwargs["axis"] = axis
-        kwargs["origin"] = parse_origin(node)
+        kwargs["origin"] = parse_origin(node, default=True)
         return Joint(**kwargs)
 
     def _to_xml(self, parent, path):

genesis/ext/urdfpy/utils.py

@@ -132,7 +132,7 @@ def xyz_rpy_to_matrix(xyz_rpy):
     return matrix
 
 
-def parse_origin(node):
+def parse_origin(node, *, default):
     """Find the ``origin`` subelement of an XML node and convert it
     into a 4x4 homogenous transformation matrix.
 
@@ -149,8 +149,11 @@ def parse_origin(node):
         ``origin`` child. Defaults to the identity matrix if no ``origin``
         child was found.
     """
-    matrix = np.eye(4, dtype=np.float64)
     origin_node = node.find("origin")
+    if not default and origin_node is None:
+        return
+
+    matrix = np.eye(4, dtype=np.float64)
     if origin_node is not None:
         if "xyz" in origin_node.attrib:
             matrix[:3, 3] = np.fromstring(origin_node.attrib["xyz"], sep=" ")
@@ -273,7 +276,7 @@ def load_meshes(filename):
     return meshes
 
 
-def configure_origin(value):
+def configure_origin(value, *, default):
     """Convert a value into a 4x4 transform matrix.
 
     Parameters
@@ -288,8 +291,8 @@ def configure_origin(value):
         The created matrix.
     """
     if value is None:
-        value = np.eye(4, dtype=np.float64)
-    elif isinstance(value, (list, tuple, np.ndarray)):
+        return np.eye(4, dtype=np.float64) if default else None
+    if isinstance(value, (list, tuple, np.ndarray)):
         value = np.asanyarray(value, dtype=np.float64)
         if value.shape == (6,):
             value = xyz_rpy_to_matrix(value)

genesis/utils/urdf.py

@@ -127,17 +127,18 @@ def parse_urdf(morph, surface):
         # we compute urdf's invweight later
         l_info["invweight"] = np.full((2,), fill_value=-1.0)
 
-        if link.inertial is None:
-            l_info["inertial_pos"] = gu.zero_pos()
-            l_info["inertial_quat"] = gu.identity_quat()
-            l_info["inertial_i"] = None
-            l_info["inertial_mass"] = None
-
-        else:
-            l_info["inertial_pos"] = link.inertial.origin[:3, 3]
-            l_info["inertial_quat"] = gu.R_to_quat(link.inertial.origin[:3, :3])
-            l_info["inertial_i"] = link.inertial.inertia
-            l_info["inertial_mass"] = link.inertial.mass
+        l_info["inertial_pos"] = None
+        l_info["inertial_quat"] = gu.identity_quat()
+        l_info["inertial_i"] = None
+        l_info["inertial_mass"] = None
+        if link.inertial is not None:
+            if link.inertial.origin is not None:
+                l_info["inertial_pos"] = link.inertial.origin[:3, 3]
+                l_info["inertial_quat"] = gu.R_to_quat(link.inertial.origin[:3, :3])
+            if link.inertial.inertia is not None:
+                l_info["inertial_i"] = link.inertial.inertia
+            if link.inertial.mass is not None:
+                l_info["inertial_mass"] = link.inertial.mass
 
         for geom_prop in (*link.collisions, *link.visuals):
             geometry = geom_prop.geometry.geometry
@@ -348,16 +349,14 @@ def parse_urdf(morph, surface):
     # Apply scaling factor
     for l_info, link_j_infos, link_g_infos in zip(l_infos, links_j_infos, links_g_infos):
         l_info["pos"] *= morph.scale
-        l_info["inertial_pos"] *= morph.scale
-
+        if l_info["inertial_pos"] is not None:
+            l_info["inertial_pos"] *= morph.scale
         if l_info["inertial_mass"] is not None:
             l_info["inertial_mass"] *= morph.scale**3
         if l_info["inertial_i"] is not None:
             l_info["inertial_i"] *= morph.scale**5
-
         for j_info in link_j_infos:
             j_info["pos"] *= morph.scale
-
         for g_info in link_g_infos:
             g_info["pos"] *= morph.scale
 
@@ -478,22 +477,23 @@ def merge_inertia(link1, link2):
     """Combine two links with fixed joint."""
     if link2.inertial is None:
         return
+    elif link2.inertial.origin is None:
+        link2.inertial.origin = np.eye(4, dtype=np.float64)
 
     if link1.inertial is None:
         link1.inertial = link2.inertial
         return
+    elif link1.inertial.origin is None:
+        link1.inertial.origin = np.eye(4, dtype=np.float64)
 
     m1 = link1.inertial.mass
     m2 = link2.inertial.mass
 
-    com1 = link1.inertial.origin[:3, 3]
-    com2 = link2.inertial.origin[:3, 3]
-
-    R1 = link1.inertial.origin[:3, :3]
-    R2 = link2.inertial.origin[:3, :3]
+    com1, R1 = link1.inertial.origin[:3, 3], link1.inertial.origin[:3, :3]
+    com2, R2 = link2.inertial.origin[:3, 3], link2.inertial.origin[:3, :3]
 
     combined_mass = m1 + m2
-    if combined_mass > 0:
+    if combined_mass > gs.EPS:
         combined_com = (m1 * com1 + m2 * com2) / combined_mass
     else:
         combined_com = com1
@@ -533,12 +533,12 @@ def update_subtree(links, joints, root_name, transform):
 
     # Apply the transformation to the current link
     if current_link.inertial is not None:
-        current_link.inertial.origin = transform @ current_link.inertial.origin
-
-    for geom in current_link.visuals:
-        geom.origin = transform @ geom.origin
+        if current_link.inertial.origin is None:
+            current_link.inertial.origin = transform
+        else:
+            current_link.inertial.origin = transform @ current_link.inertial.origin
 
-    for geom in current_link.collisions:
+    for geom in (*current_link.visuals, *current_link.collisions):
         geom.origin = transform @ geom.origin
 
     for joint in joints: