[RTM] Added contact_index for bioptim

binding/python3/rigid_body.py

@@ -26,6 +26,26 @@ def marker_index(model, marker_name: str) -> int:
     except ValueError:
         raise ValueError(f"{marker_name} is not in the biorbd model")
 
+def contact_index(model, contact_name: str) -> int:
+    """
+    Return the index in the model of the desired contact point.
+    A ValueError is raised if the marker is not in the model
+
+    Parameters
+    ----------
+    model: biorbd.Model
+        The biorbd model
+    contact_name: str
+        The name of the contact to get the index from
+    Returns
+    -------
+    The index of the contact.
+    """
+
+    try:
+        return [n.to_string() for n in model.contactNames()].index(contact_name)
+    except ValueError:
+        raise ValueError(f"{contact_name} is not in the biorbd model")
 
 def segment_index(model, segment_name):
     """
@@ -104,7 +124,7 @@ def extended_kalman_filter(
 
     index_in_c3d = np.array(tuple(labels.index(name) if name in labels else -1 for name in marker_names))
     markers_in_c3d = np.ndarray((3, len(index_in_c3d), n_frames)) * np.nan
-    markers_in_c3d[:, index_in_c3d >= 0, :] = data[:3, index_in_c3d[index_in_c3d >= 0], frames] / 1000  # To meter
+    markers_in_c3d[:, index_in_c3d[index_in_c3d >= 0], :] = data[:3, index_in_c3d[index_in_c3d >= 0], frames] / 1000  # To meter
 
     # Create a Kalman filter structure
     freq = c3d["parameters"]["POINT"]["RATE"]["value"][0]
@@ -339,6 +359,8 @@ def solve(self, method: str = "lm"):
             raise ValueError('This method is not implemented please use "trf", "lm" or "only_lm" as argument')
 
         for f in range(self.nb_frames):
+            if f % 100 == 0:
+                print(f"Frame {f}/{self.nb_frames}")
             if initial_method != "lm":
                 x0 = (
                     np.array(