From 48452b26cadc687c34c9e496a28e274b7a43e6b6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 22 Oct 2024 15:55:45 +0200 Subject: [PATCH] Add the possibility to generate animation in MeshcatVisualizer --- .../python/visualize/meshcat_visualizer.py | 46 +++++++++++++++++-- 1 file changed, 41 insertions(+), 5 deletions(-) diff --git a/bindings/python/visualize/meshcat_visualizer.py b/bindings/python/visualize/meshcat_visualizer.py index 85553c143e..9a6633034c 100644 --- a/bindings/python/visualize/meshcat_visualizer.py +++ b/bindings/python/visualize/meshcat_visualizer.py @@ -66,6 +66,22 @@ def __init__(self, zmq_url=None): self.primitive_geometries_names = [] self.arrow_names = [] self.link_visuals = dict() + self._animation = None + self._current_frame = 0 + + def start_recording_animation(self): + from meshcat.animation import Animation + self._animation = Animation() + + def stop_recording_animation(self): + self._animation = None + + def set_animation_frame(self, frame_number): + self._current_frame = frame_number + + def publish_animation(self): + if self._animation is not None: + self.viewer.set_animation(self._animation) @staticmethod def __is_mesh(geometry_object: idyn.SolidShape) -> bool: @@ -174,7 +190,11 @@ def __apply_transform_to_primitive_geomety( world_H_geometry_scaled = np.array(world_H_geometry) # Update viewer configuration. - self.viewer[viewer_name].set_transform(world_H_geometry_scaled) + if self._animation is None: + self.viewer[viewer_name].set_transform(world_H_geometry_scaled) + else: + with self._animation.at_frame(self.viewer, self._current_frame) as frame: + frame[viewer_name].set_transform(world_H_geometry_scaled) def __apply_transform(self, world_H_frame, solid_shape, viewer_name): world_H_geometry = ( @@ -187,7 +207,11 @@ def __apply_transform(self, world_H_frame, solid_shape, viewer_name): world_H_geometry_scaled = np.array(world_H_geometry).dot(extended_scale) # Update viewer configuration. - self.viewer[viewer_name].set_transform(world_H_geometry_scaled) + if self._animation is None: + self.viewer[viewer_name].set_transform(world_H_geometry_scaled) + else: + with self._animation.at_frame(self.viewer, self._current_frame) as frame: + frame[viewer_name].set_transform(world_H_geometry_scaled) def __model_exists(self, model_name): return ( @@ -509,7 +533,11 @@ def set_primitive_geometry_transform( transform[0:3, 0:3] = rotation transform[0:3, 3] = position transform[3, 3] = 1 - self.viewer[shape_name].set_transform(transform) + if self._animation is None: + self.viewer[shape_name].set_transform(transform) + else: + with self._animation.at_frame(self.viewer, self._current_frame) as frame: + frame[shape_name].set_transform(transform) def set_arrow_transform(self, origin, vector, shape_name="iDynTree"): if not self.__arrow_exists(shape_name): @@ -523,7 +551,11 @@ def set_arrow_transform(self, origin, vector, shape_name="iDynTree"): transform[3, 3] = 1 if np.linalg.norm(vector) < 1e-6: - self.viewer[shape_name].set_transform(transform) + if self._animation is None: + self.viewer[shape_name].set_transform(transform) + else: + with self._animation.at_frame(self.viewer, self._current_frame) as frame: + frame[shape_name].set_transform(transform) return # extract rotation matrix from a normalized vector @@ -543,7 +575,11 @@ def set_arrow_transform(self, origin, vector, shape_name="iDynTree"): R = np.eye(3) + skew_symmetric_matrix + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) * ((1 - c) / (s ** 2)) transform[0:3, 0:3] = R @ S - self.viewer[shape_name].set_transform(transform) + if self._animation is None: + self.viewer[shape_name].set_transform(transform) + else: + with self._animation.at_frame(self.viewer, self._current_frame) as frame: + frame[shape_name].set_transform(transform) def load_model_from_file( self, model_path: str, considered_joints=None, model_name="iDynTree", color=None