Enhance MeshcatVisualizer to support base frame in load_model method (#…

…1229)

bindings/python/visualize/meshcat_visualizer.py

@@ -590,7 +590,7 @@ def set_primitive_geometry_property(
             else:
                 with self._animation.at_frame(self.viewer, self._current_frame) as frame:
                     raise NotImplementedError("The set_property method is not implemented for animations.")
-    
+
     def load_model_from_file(
         self, model_path: str, considered_joints=None, model_name="iDynTree", color=None
     ):
@@ -621,19 +621,36 @@ def load_model_from_file(
 
         self.load_model(model=model_loader.model(), model_name=model_name, color=color)
 
-    def load_model(self, model: idyn.Model, model_name="iDynTree", color=None):
+    def load_model(self, model: idyn.Model, base_link = None, model_name="iDynTree", color=None):
 
         # check if the model already exist
         if self.__model_exists(model_name):
             msg = "The model named: " + model_name + " already exists."
             warnings.warn(msg, category=UserWarning, stacklevel=2)
-            return
+            return False
 
         self.model[model_name] = model.copy()
         self.traversal[model_name] = idyn.Traversal()
         self.link_pos[model_name] = idyn.LinkPositions()
 
-        self.model[model_name].computeFullTreeTraversal(self.traversal[model_name])
+        if base_link is None:
+            if not self.model[model_name].computeFullTreeTraversal(self.traversal[model_name]):
+                msg = "Unable to compute the full traversal for the model named:" + model_name + "."
+                warnings.warn(msg, category=UserWarning, stacklevel=2)
+                return False
+        else:
+            base_link_index = self.model[model_name].getLinkIndex(base_link)
+            link_invalid_index = -1 # idyn.LINK_INVALID_INDEX
+            if base_link_index == link_invalid_index:
+                msg = "The link " + base_link + " not found in the model named: " + model_name + "."
+                warnings.warn(msg, category=UserWarning, stacklevel=2)
+                return False
+
+            if not self.model[model_name].computeFullTreeTraversal(self.traversal[model_name], base_link_index):
+                msg = "Unable to compute the full traversal for the model named: " + model_name + " starting from the link " + base_link + "."
+                warnings.warn(msg, category=UserWarning, stacklevel=2)
+                return False
+
         self.link_pos[model_name].resize(self.model[model_name])
 
         self.__add_model_geometry_to_viewer(
@@ -642,6 +659,8 @@ def load_model(self, model: idyn.Model, model_name="iDynTree", color=None):
             color=color,
         )
 
+        return True
+
     def load_sphere(self, radius, shape_name="iDynTree", color=None):
         sphere = idyn.Sphere()
         sphere.setRadius(radius)