fix: parse origin element of visual and collision of urdf

skrobot/model/robot_model.py

@@ -1724,7 +1724,7 @@ def _meshes_from_urdf_visual(self, visual):
                     mesh.visual.face_colors = visual.material.color
 
             mesh.apply_transform(visual.origin)
-            mesh.metadata["origin"] = visual.origin
+            mesh.metadata["origin"] = np.eye(4)  # because already applied
             meshes.append(mesh)
         return meshes
 

skrobot/utils/urdf.py

@@ -1283,7 +1283,9 @@ def origin(self, value):
     @classmethod
     def _from_xml(cls, node, path):
         kwargs = cls._parse(node, path)
-        kwargs['origin'] = parse_origin(node)
+        geometry_node = [n for n in node.getchildren()
+                         if n.tag == 'geometry'][0]
+        kwargs['origin'] = parse_origin(geometry_node)
         return Collision(**kwargs)
 
     def _to_xml(self, parent, path):
@@ -1376,7 +1378,9 @@ def material(self, value):
     @classmethod
     def _from_xml(cls, node, path):
         kwargs = cls._parse(node, path)
-        kwargs['origin'] = parse_origin(node)
+        geometry_node = [n for n in node.getchildren()
+                         if n.tag == 'geometry'][0]
+        kwargs['origin'] = parse_origin(geometry_node)
         return Visual(**kwargs)
 
     def _to_xml(self, parent, path):
@@ -2558,9 +2562,10 @@ def collision_mesh(self):
                     pose = c.origin
                     if c.geometry.mesh is not None:
                         if c.geometry.mesh.scale is not None:
-                            pose[3, :3] *= c.geometry.mesh.scale
+                            m.vertices *= c.geometry.mesh.scale
                     m.apply_transform(pose)
-                    m.metadata["origin"] = pose
+                    # because we already applied the pose, origin should be identity
+                    m.metadata["origin"] = np.eye(4)
                     meshes.append(m)
             if len(meshes) == 0:
                 return None

tests/skrobot_tests/test_urdf.py

@@ -2,10 +2,13 @@
 import shutil
 import sys
 import tempfile
+import trimesh
 import unittest
 
 import numpy as np
 
+from skrobot.coordinates.math import rpy_angle
+from skrobot.coordinates.math import rpy_matrix
 from skrobot.data import bunny_objpath
 from skrobot.data import fetch_urdfpath
 from skrobot.models.urdf import RobotModelFromURDF
@@ -14,6 +17,36 @@
 
 class TestURDF(unittest.TestCase):
 
+    def setUp(self):
+        td = tempfile.mkdtemp()
+        urdf_file = """
+        <robot name="myfirst">
+          <link name="base_link">
+            <visual>
+              <geometry>
+                <mesh filename="./bunny.obj" scale="10 10 10" />
+                <origin rpy="0.1 0.2 0.3" xyz="1 2 3" />
+              </geometry>
+            </visual>
+            <collision>
+              <geometry>
+                <mesh filename="./bunny.obj" scale="10 10 10" />
+                <origin rpy="0.1 0.2 0.3" xyz="1 2 3" />
+              </geometry>
+            </collision>
+          </link>
+        </robot>
+        """
+        # write urdf file
+        with open(os.path.join(td, 'temp.urdf'), 'w') as f:
+            f.write(urdf_file)
+
+        shutil.copy(bunny_objpath(), os.path.join(td, 'bunny.obj'))
+        self.temp_urdf_dir = td
+
+    def tearDown(self):
+        shutil.rmtree(self.temp_urdf_dir)
+
     def test_load_urdfmodel(self):
         urdfpath = fetch_urdfpath()
         # Absolute path
@@ -38,35 +71,24 @@ def test_load_urdfmodel_with_simplification(self):
         with mesh_simplify_factor(0.1):
             RobotModelFromURDF(urdf_file=fetch_urdfpath())
 
-    def test_load_urdfmodel_with_scale_parameter(self):
-        td = tempfile.mkdtemp()
-        urdf_file = """
-        <robot name="myfirst">
-          <link name="base_link">
-            <visual>
-              <geometry>
-                <mesh filename="./bunny.obj" scale="10 10 10" />
-              </geometry>
-            </visual>
-            <collision>
-              <geometry>
-                <mesh filename="./bunny.obj" scale="10 10 10" />
-              </geometry>
-            </collision>
-          </link>
-        </robot>
-        """
-        # write urdf file
-        with open(os.path.join(td, 'temp.urdf'), 'w') as f:
-            f.write(urdf_file)
-
-        shutil.copy(bunny_objpath(), os.path.join(td, 'bunny.obj'))
-        urdf_file = os.path.join(td, 'temp.urdf')
+    def test_load_urdfmodel_origin_parse(self):
+        urdf_file = os.path.join(self.temp_urdf_dir, 'temp.urdf')
         dummy_robot = RobotModelFromURDF(urdf_file=urdf_file)
-        origin = dummy_robot.base_link.collision_mesh.metadata["origin"]
-        rot = origin[:3, :3]
-        determinant = np.linalg.det(rot)
-        self.assertAlmostEqual(determinant, 1.0, places=5)
-        # TODO(HiroIshida): check if trans is correctly scaled
-        # however, to check this, we must fix the issue:
-        # https://github.com/mmatl/urdfpy/issues/17
+
+        # create ground truth bunny vertices
+        bunny_mesh = trimesh.load_mesh(bunny_objpath())
+        bunny_verts = bunny_mesh.vertices
+        bunny_verts_scaled = bunny_verts * 10
+        rotmat = rpy_matrix(0.3, 0.2, 0.1)  # skrobot uses reversed order
+        trans = np.array([1., 2., 3.])
+        bunny_verts_deformed_gt = np.dot(bunny_verts_scaled, rotmat.T) + trans
+
+        for attr in ['visual_mesh', 'collision_mesh', 'visual_mesh']:
+            mesh = getattr(dummy_robot.base_link, attr)
+            if isinstance(mesh, list):
+                mesh =mesh[0]
+            self.assertTrue(np.allclose(mesh.vertices, bunny_verts_deformed_gt))
+
+            # origin must be np.eye(4) because the transformation is already applied
+            origin = mesh.metadata['origin']
+            self.assertTrue(np.allclose(origin, np.eye(4)))