🐛 Prevent the generation of an error when certain logged joints are n…

…ot located in the URDF.

The animation of the visualized robot now relies only on the subset of joints that are both logged and present in the URDF.

robot_log_visualizer/robot_visualizer/meshcat_provider.py

@@ -57,16 +57,42 @@ def check_if_model_exist(folder_path, model):
             path = folder_path / Path(model)
             return path.is_dir()
 
+        # Find the index of the model joints in the considered joints
+        # This function is required if some of the considered joints are not in the model
+        # For instance in underactuated robots
+        def find_model_joints(model_name, considered_joints):
+            ml = idyn.ModelLoader()
+            ml.loadModelFromFile(model_name)
+            model_joints_index = []
+            model = ml.model()
+            number_of_joints = model.getNrOfJoints()
+            for i in range(number_of_joints):
+                joint_name = model.getJointName(i)
+                if joint_name in considered_joints:
+                    # find the index of the joint in the considered joints
+                    index = considered_joints.index(joint_name)
+                    model_joints_index.append(index)
+
+            return model_joints_index
+
         self._is_model_loaded = False
 
         # Load the model
         model_loader = idyn.ModelLoader()
 
+        self.model_joints_index = []
         # In this case the user specify the model path
         if self.custom_model_path:
+            self.model_joints_index = find_model_joints(
+                self.custom_model_path, considered_joints
+            )
+            considered_model_joints = [
+                considered_joints[i] for i in self.model_joints_index
+            ]
+
             model_loader.loadReducedModelFromFile(
                 self.custom_model_path,
-                considered_joints,
+                considered_model_joints,
                 "urdf",
                 [self.custom_package_dir],
             )
@@ -93,9 +119,15 @@ def check_if_model_exist(folder_path, model):
             if not model_found_in_env_folders:
                 return False
 
-            model_loader.loadReducedModelFromFile(
+            self.model_joints_index = find_model_joints(
                 self.custom_model_path, considered_joints
             )
+            considered_model_joints = [
+                considered_joints[i] for i in self.model_joints_index
+            ]
+            model_loader.loadReducedModelFromFile(
+                self.custom_model_path, considered_model_joints
+            )
 
         if not model_loader.isValid():
             return False
@@ -124,7 +156,9 @@ def run(self):
                 self.meshcat_visualizer.set_multibody_system_state(
                     base_position,
                     base_rotation,
-                    joint_value=joints[self._signal_provider.index, :],
+                    joint_value=joints[
+                        self._signal_provider.index, self.model_joints_index
+                    ],
                     model_name="robot",
                 )