diff --git a/CHANGELOG.md b/CHANGELOG.md index 5decbe56fc..77593c5da9 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/bindings/python/pinocchio/robot_wrapper.py b/bindings/python/pinocchio/robot_wrapper.py index 5d8aa45973..906a49bf5f 100644 --- a/bindings/python/pinocchio/robot_wrapper.py +++ b/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): diff --git a/bindings/python/pinocchio/visualize/meshcat_visualizer.py b/bindings/python/pinocchio/visualize/meshcat_visualizer.py index 38d6af9926..18c0b557b4 100644 --- a/bindings/python/pinocchio/visualize/meshcat_visualizer.py +++ b/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,6 +165,7 @@ def lower(self) -> Dict[str, Any]: "format": "dae", "data": self.dae_raw, "resources": self.img_resources, + "matrix": list(self.intrinsic_transform.flatten()), }, }, } @@ -171,6 +173,9 @@ def lower(self) -> Dict[str, Any]: 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) diff --git a/examples/anymal-simulation.py b/examples/anymal-simulation.py index 6a2bef0194..9e0a1f3ac5 100644 --- a/examples/anymal-simulation.py +++ b/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] diff --git a/examples/meshcat-viewer-dae.py b/examples/meshcat-viewer-dae.py index 8ad9d6fa70..78e832fada 100644 --- a/examples/meshcat-viewer-dae.py +++ b/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.