From b021dac22e6fa80c38baca005f50078f02f91b3a Mon Sep 17 00:00:00 2001
From: Charbie <eve.charbonneau.1@umontreal.ca>
Date: Thu, 20 Feb 2025 14:01:14 +0400
Subject: [PATCH 1/2] fix: marker index in kalman filter

---
 binding/python3/rigid_body.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py
index 600b7eca..b03d953d 100644
--- a/binding/python3/rigid_body.py
+++ b/binding/python3/rigid_body.py
@@ -104,7 +104,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 +339,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(

From 392391d8721b104d4ec471fe9c986cbb688968f0 Mon Sep 17 00:00:00 2001
From: Charbie <eve.charbonneau.1@umontreal.ca>
Date: Thu, 20 Feb 2025 14:02:46 +0400
Subject: [PATCH 2/2] feat: added contact name from index

---
 binding/python3/rigid_body.py | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/binding/python3/rigid_body.py b/binding/python3/rigid_body.py
index b03d953d..29309cf4 100644
--- a/binding/python3/rigid_body.py
+++ b/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):
     """