Fix scaling support for meshcat

CHANGELOG.md

@@ -15,6 +15,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Fix the type of image paths when loading textures in the meshcat visualizer ([#2478](https://github.com/stack-of-tasks/pinocchio/pull/2478))
 - Fix meshcat examples ([#2503])[https://github.com/stack-of-tasks/pinocchio/pull/2503]
 - Fix `pinocchio-test-cpp-parallel-geometry` when built with Coal ([#2502](https://github.com/stack-of-tasks/pinocchio/pull/2502))
+- Fix scaling support for meshcat ([#2511](https://github.com/stack-of-tasks/pinocchio/pull/2511))
 
 ### Changed
 - On GNU/Linux and macOS, hide all symbols by default ([#2469](https://github.com/stack-of-tasks/pinocchio/pull/2469))

bindings/python/pinocchio/visualize/meshcat_visualizer.py

@@ -69,6 +69,12 @@ def hasMeshFileInfo(geometry_object):
     return False
 
 
+def applyScalingOnHomegeneousTransform(homogeneous_transform, scale):
+    scale = np.asarray(scale).flatten()
+    S = np.diag(np.concatenate((scale, [1.0])))
+    return homogeneous_transform @ S
+
+
 if import_meshcat_succeed:
     # Code adapted from Jiminy
     class Cone(mg.Geometry):
@@ -768,7 +774,6 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None):
         node_name = self.getViewerNodeName(geometry_object, geometry_type)
         meshcat_node = self.viewer[node_name]
 
-        is_mesh = False
         try:
             obj = None
             if WITH_HPP_FCL_BINDINGS:
@@ -782,7 +787,6 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None):
                     obj = loadOctree(geometry_object.geometry)
                 elif hasMeshFileInfo(geometry_object):
                     obj = self.loadMeshFromFile(geometry_object)
-                    is_mesh = True
                 elif isinstance(
                     geometry_object.geometry,
                     (
@@ -794,7 +798,6 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None):
                     obj = loadMesh(geometry_object.geometry)
             if obj is None and hasMeshFileInfo(geometry_object):
                 obj = self.loadMeshFromFile(geometry_object)
-                is_mesh = True
             if obj is None:
                 msg = (
                     "The geometry object named "
@@ -847,19 +850,12 @@ 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)
 
-        # 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)
-
     def loadViewerModel(
         self,
         rootNodeName="pinocchio",
@@ -981,12 +977,14 @@ def updatePlacements(self, geometry_type):
                 T = M.homogeneous
 
             # Update viewer configuration.
+            T = applyScalingOnHomegeneousTransform(T, visual.meshScale)
             self.viewer[visual_name].set_transform(T)
 
         for visual in self.static_objects:
             visual_name = self.getViewerNodeName(visual, pin.GeometryType.VISUAL)
             M: pin.SE3 = visual.placement
             T = M.homogeneous
+            T = applyScalingOnHomegeneousTransform(T, visual.meshScale)
             self.viewer[visual_name].set_transform(T)
 
     def addGeometryObject(self, obj: pin.GeometryObject, color=None):