Initialization of UnionSDF from RobotModel failed when original URDF uses not 1 "scale" attribute.

[sktometometo]

When using a urdf like below.

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="./bunny.stl" scale="10 10 10" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./bunny.stl" scale="10 10 10" />
      </geometry>
    </collision>
  </link>
</robot>

Creation of UnionSDF fails like below.

In [5]: from skrobot.models.urdf import RobotModelFromURDF

In [6]: robot = RobotModelFromURDF(urdf_file=r"C:\Users\shinjo\Desktop\test_skrobot\test.urdf")

In [7]: from skrobot.sdf import UnionSDF

In [8]: sdf = UnionSDF.from_robot_model(robot)
---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
Cell In[8], line 1
----> 1 sdf = UnionSDF.from_robot_model(robot)

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:285, in UnionSDF.from_robot_model(cls, robot_model, dim_grid)
    283 for link in robot_model.link_list:
    284     if link.collision_mesh is not None:
--> 285         sdf = link2sdf(link, robot_model.urdf_path, dim_grid=dim_grid)
    286         sdf_list.append(sdf)
    287 return cls(sdf_list)

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:89, in link2sdf(link, urdf_path, dim_grid)
     69 def link2sdf(link, urdf_path, dim_grid=30):
     70     """Convert Link to corresponding sdf
     71
     72     Parameters
   (...)
     86         created.
     87     """
---> 89     sdf = trimesh2sdf(link.collision_mesh, dim_grid=dim_grid)
     90     link.assoc(sdf, relative_coords=sdf)
     91     return sdf

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:65, in trimesh2sdf(mesh, **gridsdf_kwargs)
     63     rotation_matrix = origin[:3, :3]
     64     translation = origin[:3, 3]
---> 65     sdf.newcoords(Coordinates(pos=translation, rot=rotation_matrix))
     66 return sdf

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\base.py:209, in Coordinates.__init__(self, pos, rot, name, hook, check_validity)
    207     self._rotation = np.eye(3)
    208 else:
--> 209     self.rotation = rot
    210 if pos is None:
    211     self._translation = np.array([0, 0, 0])

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\base.py:305, in Coordinates.rotation(self, rotation)
    302 if type(rotation) in (list, tuple):
    303     rotation = np.array(rotation).astype(np.float32)
--> 305 _check_valid_rotation(rotation)
    306 self._rotation = rotation * 1.

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\math.py:120, in _check_valid_rotation(rotation)
    117     raise ValueError('Rotation must be specified as a 3x3 ndarray')
    119 if np.abs(np.linalg.det(rotation) - 1.0) > 1e-3:
--> 120     raise ValueError('Illegal rotation. Must have determinant == 1.0, '
    121                      'get {}'.format(np.linalg.det(rotation)))
    122 return rotation

ValueError: Illegal rotation. Must have determinant == 1.0, get 1000.0000000000007

[sktometometo]

This comes from invalid rotation setting to collision trimesh metadata at

scikit-robot/skrobot/utils/urdf.py

Line 2562 in cd27403

S[:3, :3] = np.diag(c.geometry.mesh.scale)

scikit-robot/skrobot/utils/urdf.py

Line 2565 in cd27403

m.metadata["origin"] = pose

[sktometometo]

What is the aim of lines around here? It looks like adjusting the pose of mesh based on scale factor.

[HiroIshida]

actually the most of the urdf.py in skrobto is copied from urdfpy. and the corresponding part is indeed unchanged from urdf
https://github.com/mmatl/urdfpy/blob/5466842899b33bd549e8f9e2a9a987bd5e37373b/urdfpy/urdf.py#L2596

I think the scaling by setting diag itself is fine.
https://chatgpt.com/share/f5a91f85-3659-4761-a529-43ca70d0b702

But these scale operation makes the rotational part invalid, so we must extract the R
https://github.com/mikedh/trimesh/blob/7853a5ebae3b35275f4d8d65cbc48bcd3a3c8a40/trimesh/transformations.py#L782

But I tihnk we can just modify the implementation so that applying the scaling factor to only the translation part of the pose. then keep the rotation matrix as original.

@sktometometo
I'm grad if you could create a PR for that if you are available