Fix Dae support in MeshCat

CHANGELOG.md

@@ -8,12 +8,14 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ### Fixed
 - Append pinocchio optional libraries into pkg-config file ([#2322](https://github.com/stack-of-tasks/pinocchio/pull/2322))
+- Fixed support of DAE meshes with MeshCat ([#2331](https://github.com/stack-of-tasks/pinocchio/pull/2331))
 
 ### Added
 - Add getMotionAxis method to helical, prismatic, revolute and ubounded revolute joint ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
 
 ### Changed
 - Use eigenpy to expose `GeometryObject::meshMaterial` variant ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
+- GepettoViewer is no more the default viewer for RobotWrapper ([#2331](https://github.com/stack-of-tasks/pinocchio/pull/2331))
 
 ## [3.1.0] - 2024-07-04
 

bindings/python/pinocchio/robot_wrapper.py

@@ -385,16 +385,16 @@ def getViewerNodeName(self, geometry_object, geometry_type):
 
     def initViewer(self, share_data=True, *args, **kwargs):
         """Init the viewer"""
-        # Set viewer to use to gepetto-gui.
+        # Set viewer to use to MeshCat.
         if self.viz is None:
-            from .visualize import GepettoVisualizer
+            from .visualize import MeshcatVisualizer
 
             data, collision_data, visual_data = None, None, None
             if share_data:
                 data = self.data
                 collision_data = self.collision_data
                 visual_data = self.visual_data
-            self.viz = GepettoVisualizer(
+            self.viz = MeshcatVisualizer(
                 self.model,
                 self.collision_model,
                 self.visual_model,
@@ -407,7 +407,7 @@ def initViewer(self, share_data=True, *args, **kwargs):
         self.viz.initViewer(*args, **kwargs)
 
     def loadViewerModel(self, *args, **kwargs):
-        """Create the scene displaying the robot meshes in gepetto-viewer"""
+        """Create the scene displaying the robot meshes in MeshCat"""
         self.viz.loadViewerModel(*args, **kwargs)
 
     def display(self, q):

bindings/python/pinocchio/visualize/meshcat_visualizer.py

@@ -110,6 +110,7 @@ def __init__(self, dae_path: str, cache: Optional[Set[str]] = None) -> None:
             # Attributes to be specified by the user
             self.path = None
             self.material = None
+            self.intrinsic_transform = mg.tf.identity_matrix()
 
             # Raw file content
             dae_dir = os.path.dirname(dae_path)
@@ -164,13 +165,17 @@ def lower(self) -> Dict[str, Any]:
                         "format": "dae",
                         "data": self.dae_raw,
                         "resources": self.img_resources,
+                        "matrix": list(self.intrinsic_transform.flatten()),
                     },
                 },
             }
             if self.material is not None:
                 self.material.lower_in_object(data)
             return data
 
+        def set_scale(self, scale) -> None:
+            self.intrinsic_transform[:3, :3] = np.diag(scale)
+
     # end code adapted from Jiminy
 
     class Plane(mg.Geometry):
@@ -834,13 +839,16 @@ def to_material_color(rgba) -> int:
 
             if isinstance(obj, DaeMeshGeometry):
                 obj.path = meshcat_node.path
+                scale = list(np.asarray(geometry_object.meshScale).flatten())
+                obj.set_scale(scale)
                 if geometry_object.overrideMaterial:
                     obj.material = material
                 meshcat_node.window.send(obj)
             else:
                 meshcat_node.set_object(obj, material)
 
-        if is_mesh:  # Apply the scaling
+        # Apply the scaling
+        if is_mesh and not isinstance(obj, DaeMeshGeometry):
             scale = list(np.asarray(geometry_object.meshScale).flatten())
             meshcat_node.set_property("scale", scale)
 

examples/anymal-simulation.py

@@ -2,7 +2,7 @@
 import hppfcl as fcl
 import pinocchio
 from example_robot_data import load
-from pinocchio.visualize import GepettoVisualizer
+from pinocchio.visualize import MeshcatVisualizer
 from pinocchio import GeometryType
 from time import sleep
 
@@ -45,18 +45,6 @@
 
 robot.initViewer()
 robot.loadViewerModel("pinocchio")
-gui = robot.viewer.gui
-robot.display(robot.q0)
-window_id = robot.viewer.gui.getWindowID("python-pinocchio")
-
-robot.viewer.gui.setBackgroundColor1(window_id, [1.0, 1.0, 1.0, 1.0])
-robot.viewer.gui.setBackgroundColor2(window_id, [1.0, 1.0, 1.0, 1.0])
-robot.viewer.gui.addFloor("hpp-gui/floor")
-
-robot.viewer.gui.setScale("hpp-gui/floor", [0.5, 0.5, 0.5])
-robot.viewer.gui.setColor("hpp-gui/floor", [0.7, 0.7, 0.7, 1.0])
-robot.viewer.gui.setLightingMode("hpp-gui/floor", "OFF")
-
 robot.display(robot.q0)
 
 constraint_datas = [cm.createData() for cm in constraint_models]

examples/meshcat-viewer-dae.py

@@ -23,13 +23,6 @@
     urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
 )
 
-# Currently, MeshCat is not able to retrieve the scaling from DAE files. Set it manually.
-for geom in visual_model.geometryObjects:
-    s = geom.meshScale
-    s *= 0.01
-    geom.meshScale = s
-
-
 # Start a new MeshCat server and client.
 # Note: the server can also be started separately using the "meshcat-server" command in a terminal:
 # this enables the server to remain active after the current script ends.