From b2eb5d0cc1a7f5ef184a93ed1c8a5bed7fcd5c31 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Mon, 2 Sep 2024 18:16:34 +0200 Subject: [PATCH 1/6] python/meshcat: fix scaling support --- .../pinocchio/visualize/meshcat_visualizer.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 77d6265b3..1ea19698e 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/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.dot(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): From dbf51f98a8832bdc149ede4f46c7fd79d120fe71 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Thu, 12 Dec 2024 19:27:51 +0100 Subject: [PATCH 2/6] changelog: update --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) 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)) From ee3fda7c2452ed1e9841eaa82d2ff8978a8dad43 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 13 Dec 2024 11:49:30 +0100 Subject: [PATCH 3/6] python: Use @ instead of .dot --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 1ea19698e..1b8a3282b 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -72,7 +72,7 @@ def hasMeshFileInfo(geometry_object): def applyScalingOnHomegeneousTransform(homogeneous_transform, scale): scale = np.asarray(scale).flatten() S = np.diag(np.concatenate((scale, [1.0]))) - return homogeneous_transform.dot(S) + return homogeneous_transform @ S if import_meshcat_succeed: From 8b73cf8140dc917fc7673977ed41714b8f7831d1 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 13 Dec 2024 13:40:24 +0100 Subject: [PATCH 4/6] python: Add type annotation and assert on size --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 1b8a3282b..395766f09 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -69,8 +69,12 @@ def hasMeshFileInfo(geometry_object): return False -def applyScalingOnHomegeneousTransform(homogeneous_transform, scale): - scale = np.asarray(scale).flatten() +def applyScalingOnHomegeneousTransform( + homogeneous_transform: np.ndarray, scale: np.ndarray +) -> np.ndarray: + assert homogeneous_transform.shape == (4, 4) + assert scale.shape == (3,) + scale = np.array(scale).flatten() S = np.diag(np.concatenate((scale, [1.0]))) return homogeneous_transform @ S From dc53def429e0131503edf75505cb0e0515fa68cc Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 13 Dec 2024 13:41:46 +0100 Subject: [PATCH 5/6] python: Run code formatting --- .../pinocchio/visualize/meshcat_visualizer.py | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 395766f09..fc78301d1 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -513,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( [ From 1e4df2b4de40351a43c2ca9058dc66418c882351 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 13 Dec 2024 13:48:07 +0100 Subject: [PATCH 6/6] python: Add more generic check --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index fc78301d1..c41997a76 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -73,7 +73,7 @@ def applyScalingOnHomegeneousTransform( homogeneous_transform: np.ndarray, scale: np.ndarray ) -> np.ndarray: assert homogeneous_transform.shape == (4, 4) - assert scale.shape == (3,) + assert scale.size == 3 scale = np.array(scale).flatten() S = np.diag(np.concatenate((scale, [1.0]))) return homogeneous_transform @ S