diff --git a/CHANGELOG.md b/CHANGELOG.md index 6abe0d8f9..3b3dde6ab 100644 --- a/CHANGELOG.md +++ b/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)) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 77d6265b3..c41997a76 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -69,6 +69,16 @@ def hasMeshFileInfo(geometry_object): return False +def applyScalingOnHomegeneousTransform( + homogeneous_transform: np.ndarray, scale: np.ndarray +) -> np.ndarray: + assert homogeneous_transform.shape == (4, 4) + assert scale.size == 3 + scale = np.array(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): @@ -503,23 +513,47 @@ def createCapsule(length, radius, radial_resolution=30, cap_resolution=10): for j in range(nbv[0]): j_next = (j + 1) % nbv[0] indexes[index + 0] = np.array( - [j_next * stride + nbv[1], j_next * stride, j * stride] + [ + j_next * stride + nbv[1], + j_next * stride, + j * stride, + ] ) indexes[index + 1] = np.array( - [j * stride + nbv[1], j_next * stride + nbv[1], j * stride] + [ + j * stride + nbv[1], + j_next * stride + nbv[1], + j * stride, + ] ) indexes[index + 2] = np.array( - [j * stride + nbv[1] - 1, j_next * stride + nbv[1] - 1, last - 1] + [ + j * stride + nbv[1] - 1, + j_next * stride + nbv[1] - 1, + last - 1, + ] ) indexes[index + 3] = np.array( - [j_next * stride + 2 * nbv[1] - 1, j * stride + 2 * nbv[1] - 1, last] + [ + j_next * stride + 2 * nbv[1] - 1, + j * stride + 2 * nbv[1] - 1, + last, + ] ) for i in range(nbv[1] - 1): indexes[index + 4 + i * 4 + 0] = np.array( - [j_next * stride + i, j_next * stride + i + 1, j * stride + i] + [ + j_next * stride + i, + j_next * stride + i + 1, + j * stride + i, + ] ) indexes[index + 4 + i * 4 + 1] = np.array( - [j_next * stride + i + 1, j * stride + i + 1, j * stride + i] + [ + j_next * stride + i + 1, + j * stride + i + 1, + j * stride + i, + ] ) indexes[index + 4 + i * 4 + 2] = np.array( [ @@ -768,7 +802,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 +815,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 +826,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 +878,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 +1005,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):