From aa652a274f70fa3bb56b673dfc1ae6401794e6f4 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 13 Nov 2023 22:00:43 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Prevent=20the=20generation=20of?=
 =?UTF-8?q?=20an=20error=20when=20certain=20logged=20joints=20are=20not=20?=
 =?UTF-8?q?located=20in=20the=20URDF.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The animation of the visualized robot now relies only on the subset of joints that are both logged and present in the URDF.
---
 .../robot_visualizer/meshcat_provider.py      | 40 +++++++++++++++++--
 1 file changed, 37 insertions(+), 3 deletions(-)

diff --git a/robot_log_visualizer/robot_visualizer/meshcat_provider.py b/robot_log_visualizer/robot_visualizer/meshcat_provider.py
index 8523bee..3072b26 100644
--- a/robot_log_visualizer/robot_visualizer/meshcat_provider.py
+++ b/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",
                 )