From 24dbede81e884dc0be2bb05ecd1d3f7171cba982 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 15:37:27 +0200 Subject: [PATCH 1/6] python/meshcat: fix loading of BVH model BVH models should be loaded in a raw manner after checking if they are not related to a specific file --- .../pinocchio/visualize/meshcat_visualizer.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 6bf31d2a12..7c5db7f4d3 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -134,6 +134,17 @@ def loadPrimitive(self, geometry_object): return obj + def isMesh(self, geometry_object): + """ Check whether the geometry object contains a Mesh supported by MeshCat """ + if geometry_object.meshPath == "": + return False + + _, file_extension = os.path.splitext(geometry_object.meshPath) + if file_extension.lower() in [".dae", ".obj", ".stl"]: + return True + + return False + def loadMesh(self, geometry_object): import meshcat.geometry @@ -168,10 +179,14 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): try: if WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.ShapeBase): obj = self.loadPrimitive(geometry_object) + elif self.isMesh(geometry_object): + obj = self.loadMesh(geometry_object) elif WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.BVHModelBase): obj = loadBVH(geometry_object.geometry) else: - obj = self.loadMesh(geometry_object) + msg = "The geometry object named " + geometry_object.name + " is not supported by Pinocchio/MeshCat for vizualization." + warnings.warn(msg, category=UserWarning, stacklevel=2) + return if obj is None: return except Exception as e: From 50f49cc7fc3ddd110f4bd09d381b9afb081e7ffa Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 15:49:11 +0200 Subject: [PATCH 2/6] python/meshcat: enforce scaling only for meshes --- .../python/pinocchio/visualize/meshcat_visualizer.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 7c5db7f4d3..99b7c357e5 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -259,9 +259,12 @@ def display(self, q = None): # Get mesh pose. M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)] # Manage scaling - scale = np.asarray(visual.meshScale).flatten() - S = np.diag(np.concatenate((scale,[1.0]))) - T = np.array(M.homogeneous).dot(S) + if self.isMesh(visual): + scale = np.asarray(visual.meshScale).flatten() + S = np.diag(np.concatenate((scale,[1.0]))) + T = np.array(M.homogeneous).dot(S) + else: + T = M.homogeneous # Update viewer configuration. self.viewer[self.getViewerNodeName(visual,pin.GeometryType.VISUAL)].set_transform(T) From 8d5aee6380e43bda808f60ac84f78e46d9e600da Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 15:50:18 +0200 Subject: [PATCH 3/6] python/meshcat: isMesh should be a function --- .../pinocchio/visualize/meshcat_visualizer.py | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 99b7c357e5..e611e9762d 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -13,6 +13,16 @@ except: WITH_HPP_FCL_BINDINGS = False +def isMesh(geometry_object): + """ Check whether the geometry object contains a Mesh supported by MeshCat """ + if geometry_object.meshPath == "": + return False + + _, file_extension = os.path.splitext(geometry_object.meshPath) + if file_extension.lower() in [".dae", ".obj", ".stl"]: + return True + + return False def loadBVH(bvh): import meshcat.geometry as mg @@ -134,17 +144,6 @@ def loadPrimitive(self, geometry_object): return obj - def isMesh(self, geometry_object): - """ Check whether the geometry object contains a Mesh supported by MeshCat """ - if geometry_object.meshPath == "": - return False - - _, file_extension = os.path.splitext(geometry_object.meshPath) - if file_extension.lower() in [".dae", ".obj", ".stl"]: - return True - - return False - def loadMesh(self, geometry_object): import meshcat.geometry @@ -179,7 +178,7 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): try: if WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.ShapeBase): obj = self.loadPrimitive(geometry_object) - elif self.isMesh(geometry_object): + elif isMesh(geometry_object): obj = self.loadMesh(geometry_object) elif WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.BVHModelBase): obj = loadBVH(geometry_object.geometry) From e2a8ea13142cb029b300959f579fcd04d651e8a7 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 15:59:57 +0200 Subject: [PATCH 4/6] parsers: add missing constness --- src/parsers/urdf/geometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp index f6a6e197f7..a2ca7d042f 100644 --- a/src/parsers/urdf/geometry.cpp +++ b/src/parsers/urdf/geometry.cpp @@ -360,7 +360,7 @@ namespace pinocchio FrameIndex frame_id; UrdfGeomVisitorBase::Frame frame = visitor.getBodyFrame (link_name, frame_id); - SE3 body_placement = frame.placement; + const SE3 & body_placement = frame.placement; std::size_t objectId = 0; for (typename VectorSharedT::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i) @@ -391,7 +391,7 @@ namespace pinocchio std::string meshTexturePath; bool overrideMaterial = getVisualMaterial((*i), meshTexturePath, meshColor, package_dirs); - SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin); + const SE3 geomPlacement = body_placement * convertFromUrdf((*i)->origin); std::ostringstream geometry_object_suffix; geometry_object_suffix << "_" << objectId; const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str()); From 6f14e49e63208b7c2c81dc99997337fdf730c9ae Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 17:48:33 +0200 Subject: [PATCH 5/6] python/meshcat: fix scaling when loading a mesh --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index e611e9762d..1adb5451d1 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -175,11 +175,13 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): viewer_name = self.getViewerNodeName(geometry_object, geometry_type) + is_mesh = False try: if WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.ShapeBase): obj = self.loadPrimitive(geometry_object) elif isMesh(geometry_object): obj = self.loadMesh(geometry_object) + is_mesh = True elif WITH_HPP_FCL_BINDINGS and isinstance(geometry_object.geometry, hppfcl.BVHModelBase): obj = loadBVH(geometry_object.geometry) else: @@ -209,6 +211,10 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None): material.opacity = float(meshColor[3]) self.viewer[viewer_name].set_object(obj, material) + if is_mesh: # Apply the scaling + scale = list(np.asarray(geometry_object.meshScale).flatten()) + self.viewer[viewer_name].set_property("scale",scale) + def loadViewerModel(self, rootNodeName="pinocchio", color = None): """Load the robot in a MeshCat viewer. Parameters: From 2f2b93700f2dfe620ce1f9b452fc62d3846b0281 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 16 Jul 2021 17:48:58 +0200 Subject: [PATCH 6/6] python/meshcat: mention the bug within MeshCat --- bindings/python/pinocchio/visualize/meshcat_visualizer.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 1adb5451d1..cc7d90f89b 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/bindings/python/pinocchio/visualize/meshcat_visualizer.py @@ -261,17 +261,19 @@ def display(self, q = None): pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data) for visual in self.visual_model.geometryObjects: + visual_name = self.getViewerNodeName(visual,pin.GeometryType.VISUAL) # Get mesh pose. M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)] - # Manage scaling - if self.isMesh(visual): + # Manage scaling: force scaling even if this should be normally handled by MeshCat (but there is a bug here) + if isMesh(visual): scale = np.asarray(visual.meshScale).flatten() S = np.diag(np.concatenate((scale,[1.0]))) T = np.array(M.homogeneous).dot(S) else: T = M.homogeneous + # Update viewer configuration. - self.viewer[self.getViewerNodeName(visual,pin.GeometryType.VISUAL)].set_transform(T) + self.viewer[visual_name].set_transform(T) def displayCollisions(self,visibility): """Set whether to display collision objects or not.