Visualization fails for some more complex URDFs

[ndahn]

We have a more complex URDF that fails when passing it to the visualizer. The reason seems to be that trimesh can't compact it into one mesh and returns a trimesh.Scene instead. I have a messy monkey patch that works as follows:

• loading of the mesh is done in visualization._artists._objects_to_artist, instead of letting the Mesh artist and load_mesh handle it. If a scene is returned, one artist per submesh is added.
• the Mesh artist now accepts a trimesh.TriangleMesh instead of a filename.

However, there are some issues with this solution:

• the "one frame per mesh" convention is broken. I'm not sure if this is a breaking change and what a better solution would look like.
• the Mesh artist should still be able to load from filenames to avoid breaking changes. This is easy to fix.

try:
    import pytransform3d
    from pytransform3d import urdf
    import pytransform3d.transformations as pt
    from pytransform3d.visualizer import Artist, Sphere, Box, Cylinder, warnings

    class Mesh(Artist):
        def __init__(self, mesh, A2B=np.eye(4), s=np.ones(3), c=None,
                    convex_hull=False):
            import open3d as o3d

            if convex_hull:
                mesh.convex_hull()

            self.mesh = o3d.geometry.TriangleMesh(
                o3d.utility.Vector3dVector(mesh.vertices),
                o3d.utility.Vector3iVector(mesh.faces)
            )
            self.mesh.vertices = o3d.utility.Vector3dVector(
                np.asarray(self.mesh.vertices) * s)
            self.mesh.compute_vertex_normals()
            if c is not None:
                n_vertices = len(self.mesh.vertices)
                colors = np.zeros((n_vertices, 3))
                colors[:] = c
                self.mesh.vertex_colors = o3d.utility.Vector3dVector(colors)
            self.A2B = None
            self.set_data(A2B)

        def set_data(self, A2B):
            previous_A2B = self.A2B
            if previous_A2B is None:
                previous_A2B = np.eye(4)
            self.A2B = A2B

            self.mesh.transform(pt.invert_transform(previous_A2B, check=False))
            self.mesh.transform(self.A2B)

        @property
        def geometries(self):
            geometries : list
                List of geometries that can be added to the visualizer.
            """
            return [self.mesh]


    def _objects_to_artists(objects, convex_hull=False):
        artists = {}
        for obj in objects:
            if obj.color is None:
                color = None
            else:
                # we loose the alpha channel as it is not supported by Open3D
                color = (obj.color[0], obj.color[1], obj.color[2])
            try:
                if isinstance(obj, urdf.Sphere):
                    artist = Sphere(radius=obj.radius, c=color)
                elif isinstance(obj, urdf.Box):
                    artist = Box(obj.size, c=color)
                elif isinstance(obj, urdf.Cylinder):
                    artist = Cylinder(obj.length, obj.radius, c=color)
                else:
                    assert isinstance(obj, urdf.Mesh)
                    import trimesh
                    mesh = trimesh.load(obj.filename, force="mesh")
                    if isinstance(mesh, trimesh.Scene):
                        for name, submesh in mesh.geometry.values():
                            artist = Mesh(submesh, s=obj.scale, c=color, convex_hull=convex_hull)
                            artists[obj.frame + "_" + name] = artist
                    else:
                        artist = Mesh(mesh, s=obj.scale, c=color,
                                    convex_hull=convex_hull)
                artists[obj.frame] = artist
            except RuntimeError as e:
                warnings.warn(str(e))
        return artists

    pytransform3d.visualizer.Mesh = Mesh
    pytransform3d.visualizer._artists._objects_to_artists = _objects_to_artists
except:
    pass

[AlexanderFabisch]

Thanks for reporting @ndahn . Is the mesh that you are trying to load a collada file (.dae)? I assume this line is failing:

https://github.com/dfki-ric/pytransform3d/blob/main/pytransform3d/_mesh_loader.py#L98

Or do you use a different version? Maybe we can find a workaround for the error here?

the "one frame per mesh" convention is broken. I'm not sure if this is a breaking change and what a better solution would look like.

I don't think this is critical. We would just have to make sure that the frames are linked correctly.

[ndahn]

Yes, that's the line, as the resulting object does not have a sum() method. And yes, the URDF contains two collada files. I guess that makes more sense than blaming it on our URDF 😓

[ndahn]

The trimesh.load method provides a force='mesh' parameter, but it's not guaranteed to succeed.

[AlexanderFabisch]

Please check if this works for you: #285

[AlexanderFabisch]

@ndahn confirmed that it works in his case.

[ndahn]

Works like a charm, thank you!