Merge pull request #144 from Ipuch/ik

delete: solve_frame_per_frame=False forever

bionc/bionc_numpy/inverse_kinematics.py

@@ -79,7 +79,6 @@ def __init__(
         model: BiomechanicalModel,
         experimental_markers: np.ndarray | str = None,
         experimental_heatmaps: dict[str, np.ndarray] = None,
-        solve_frame_per_frame: bool = True,
         active_direct_frame_constraints: bool = False,
         use_sx: bool = True,
     ):
@@ -97,35 +96,28 @@ def __init__(
                     - gaussian_parameters[0:2, :, :, :] is an array of the position (x, y) of the center of the gaussian.
                     - gaussian_parameters[2:4, :, :, :] is an array of the standard deviation (x, y) of the gaussian.
                     - gaussian_parameters[4,:,:,:] is an array of the magnitude of the gaussian.
-        solve_frame_per_frame : bool
-            If True, the inverse kinematics is solved frame per frame, otherwise it is solved for the whole motion
         active_direct_frame_constraints : bool
             If True, the direct frame constraints are active, otherwise they are not.
             It ensures that rigid body constraints lead to positive determinants or the matrix [u, v, w].
         use_sx : bool
             If True, the symbolic variables are SX, otherwise they are MX (SX are faster but take more memory)
         """
 
-        self._validate_input(experimental_markers, experimental_heatmaps, solve_frame_per_frame)
+        self._validate_input(experimental_markers, experimental_heatmaps)
         self._initialize_attributes(
             model,
             experimental_markers,
             experimental_heatmaps,
-            solve_frame_per_frame,
             active_direct_frame_constraints,
             use_sx,
         )
         self._setup_optimization_problem()
 
-    def _validate_input(self, experimental_markers, experimental_heatmaps, solve_frame_per_frame):
+    def _validate_input(self, experimental_markers, experimental_heatmaps):
         if experimental_markers is None and experimental_heatmaps is None:
             raise ValueError("Please provide experimental data, either marker or heatmap data")
         if experimental_markers is not None and experimental_heatmaps is not None:
             raise ValueError("Please choose between marker data and heatmap data")
-        if experimental_heatmaps is not None and not solve_frame_per_frame:
-            raise NotImplementedError(
-                "Not possible to solve for all frames with heatmap parameters. Please set solve_frame_per_frame=True"
-            )
         if experimental_heatmaps is not None:
             check_format_experimental_heatmaps(experimental_heatmaps)
 
@@ -134,7 +126,6 @@ def _initialize_attributes(
         model,
         experimental_markers,
         experimental_heatmaps,
-        solve_frame_per_frame,
         active_direct_frame_constraints,
         use_sx,
     ):
@@ -143,7 +134,6 @@ def _initialize_attributes(
 
         self.model = model
         self._model_mx = model.to_mx()
-        self._frame_per_frame = solve_frame_per_frame
         self._active_direct_frame_constraints = active_direct_frame_constraints
         self.use_sx = use_sx
 
@@ -298,10 +288,7 @@ def solve(
         options = self._get_solver_options(method, options)
         Q_init = self._get_initial_guess(Q_init, initial_guess_mode)
 
-        if self._frame_per_frame:
-            Qopt = self._solve_frame_per_frame(Q_init, initial_guess_mode, method, options)
-        else:
-            Qopt = self._solve_all_frame_together(Q_init, method, options)
+        Qopt = self._solve_frame_per_frame(Q_init, initial_guess_mode, method, options)
 
         self.Qopt = Qopt.reshape((12 * self.model.nb_segments, self.nb_frames))
         self.check_segment_determinants()
@@ -332,8 +319,6 @@ def _compute_Q_init_from_markers(self, initial_guess_mode: InitialGuessModeType)
             raise ValueError("Please provide experimental_markers in order to initialize the optimization")
         if self.experimental_heatmaps is not None:
             raise ValueError("Q_init cannot be computed from markers using heatmap data")
-        if initial_guess_mode == InitialGuessModeType.FROM_FIRST_FRAME_MARKERS and not self._frame_per_frame:
-            raise ValueError("Please set frame_per_frame to True")
 
         frame_slice = (
             slice(0, 1) if initial_guess_mode == InitialGuessModeType.FROM_FIRST_FRAME_MARKERS else slice(None)
@@ -354,8 +339,6 @@ def _validate_user_provided_Q_init(
                     f"Q_init.shape\\[1\\] must equal the number of frames ({self.nb_frames}). Currently, Q_init.shape\\[1\\] = {Q_init.shape[1]}."
                 )
         elif initial_guess_mode == InitialGuessModeType.USER_PROVIDED_FIRST_FRAME_ONLY:
-            if not self._frame_per_frame:
-                raise ValueError("Set frame_per_frame to True or use InitialGuessModeType.USER_PROVIDED.")
             if Q_init.shape[1] != 1:
                 raise ValueError("Provide only the first frame of Q_init. Currently, Q_init.shape\\[1\\] = 2")
 
@@ -433,72 +416,35 @@ def _get_constraint_bounds(self) -> tuple[np.ndarray, np.ndarray]:
             ubg = np.concatenate((ubg, np.full(self.model.nb_segments, np.inf)))
         return lbg, ubg
 
-    def _solve_all_frame_together(
-        self,
-        Q_init: np.ndarray | NaturalCoordinates,
-        method: str,
-        options: dict,
-    ):
-        constraints = self._constraints(self._Q_sym)
-        if self._active_direct_frame_constraints:
-            constraints = vertcat(constraints, self._direct_frame_constraints(self._Q_sym))
-        if self.experimental_markers is not None:
-            objective = self._objective_minimize_marker_distance(self._Q_sym, self.experimental_markers)
-        else:
-            NotImplementedError(
-                "Not possible to solve for all frames with heatmap parameters. Please set solve_frame_per_frame=True"
-            )
-        nlp = dict(
-            x=self._vert_Q_sym,
-            f=_mx_to_sx(objective, [self._vert_Q_sym]) if self.use_sx else objective,
-            g=_mx_to_sx(constraints, [self._vert_Q_sym]) if self.use_sx else constraints,
-        )
-        vertical_Q_init = Q_init.reshape((12 * self.model.nb_segments * self.nb_frames, 1))
-
-        lbg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames)
-        ubg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames)
-        if self._active_direct_frame_constraints:
-            lbg = np.concatenate((lbg, np.zeros(self.model.nb_segments * self.nb_frames)))
-            ubg = np.concatenate((ubg, np.full(self.model.nb_segments * self.nb_frames, np.inf)))
-        r, success = _solve_nlp(method, nlp, vertical_Q_init, lbg, ubg, options)
-        self.success_optim = [success] * self.nb_frames
-        Qopt = r["x"].reshape((12 * self.model.nb_segments, self.nb_frames)).toarray()
-        self.objective_function = r["f"]
-
-        return Qopt
-
     def _declare_sym_Q(self) -> tuple[MX, MX]:
-        """Declares the symbolic variables for the natural coordinates and handle single frame or multi frames"""
+        """Declares the symbolic variables for the natural coordinates and handle single frame"""
         Q_sym = []
-        nb_frames = 1 if self._frame_per_frame else self.nb_frames
-        for f in range(nb_frames):
-            Q_f_sym = []
-            for ii in range(self.model.nb_segments):
-                Q_f_sym.append(SegmentNaturalCoordinates.sym(f"_{ii}_{f}"))
-            Q_sym.append(vertcat(*Q_f_sym))
+        f = 0
+        Q_f_sym = []
+        for ii in range(self.model.nb_segments):
+            Q_f_sym.append(SegmentNaturalCoordinates.sym(f"_{ii}_{f}"))
+        Q_sym.append(vertcat(*Q_f_sym))
         Q = horzcat(*Q_sym)
         vert_Q = vertcat(*Q_sym)
         return Q, vert_Q
 
     def _objective_minimize_marker_distance(self, Q, experimental_markers) -> MX:
         """
-        Computes the objective function that minimizes marker distance and handles single frame or multi frames
+        Computes the objective function that minimizes marker distance and handles single frame
 
         Returns
         -------
         MX
             The objective function that minimizes the distance between the experimental markers and the model markers
         """
-        error_m = 0
-        nb_frames = 1 if self._frame_per_frame else self.nb_frames
-        for f in range(nb_frames):
-            Q_f = NaturalCoordinates(Q[:, f])
-            xp_markers = (
-                experimental_markers[:3, :, f] if isinstance(experimental_markers, np.ndarray) else experimental_markers
-            )
-            phim = self._model_mx.markers_constraints(xp_markers, Q_f, only_technical=True)
-            error_m += 1 / 2 * phim.T @ phim
-        return error_m
+        f = 0
+        Q_f = NaturalCoordinates(Q[:, f])
+        xp_markers = (
+            experimental_markers[:3, :, f] if isinstance(experimental_markers, np.ndarray) else experimental_markers
+        )
+        phim = self._model_mx.markers_constraints(xp_markers, Q_f, only_technical=True)
+
+        return 1 / 2 * phim.T @ phim
 
     def _objective_maximize_confidence(self, Q, camera_parameters, gaussian_parameters) -> MX:
         """
@@ -519,28 +465,26 @@ def _objective_maximize_confidence(self, Q, camera_parameters, gaussian_paramete
 
     def _constraints(self, Q) -> MX:
         """Computes the constraints and handle single frame or multi frames"""
-        nb_frames = 1 if self._frame_per_frame else self.nb_frames
         phir = []
         phik = []
-        for f in range(nb_frames):
-            Q_f = NaturalCoordinates(Q[:, f])
-            phir.append(self._model_mx.rigid_body_constraints(Q_f))
-            phik.append(self._model_mx.joint_constraints(Q_f))
+        f = 0
+        Q_f = NaturalCoordinates(Q[:, f])
+        phir.append(self._model_mx.rigid_body_constraints(Q_f))
+        phik.append(self._model_mx.joint_constraints(Q_f))
         return vertcat(*phir, *phik)
 
     def _direct_frame_constraints(self, Q):
         """Computes the direct frame constraints and handle single frame or multi frames"""
-        nb_frames = 1 if self._frame_per_frame else self.nb_frames
         direct_frame_constraints = []
-        for f in range(nb_frames):
-            Q_f = NaturalCoordinates(Q[:, f])
-            for ii in range(self.model.nb_segments):
-                u, v, w = Q_f.vector(ii).to_uvw()
-                direct_frame_constraints.append(sarrus(horzcat(u, v, w)))
+        f = 0
+        Q_f = NaturalCoordinates(Q[:, f])
+        for ii in range(self.model.nb_segments):
+            u, v, w = Q_f.vector(ii).to_uvw()
+            direct_frame_constraints.append(sarrus(horzcat(u, v, w)))
         return vertcat(*direct_frame_constraints)
 
     def check_segment_determinants(self):
-        """Checks the determinant of each segment frame"""
+        """Checks the determinant of each segment frame with the optimal solution"""
         self.segment_determinants = np.zeros((self.model.nb_segments, self.nb_frames))
         for i in range(0, self.Qopt.shape[1]):
             Qi = NaturalCoordinatesNumpy(self.Qopt)[:, i : i + 1]

examples/inverse_kinematics/inverse_kinematics.py

@@ -2,7 +2,6 @@
 This example shows how to use the InverseKinematics class to solve an inverse kinematics problem.
 """
 
-from bionc import InverseKinematics, Viz, NaturalCoordinates
 import numpy as np
 import time
 from pyomeca import Markers
@@ -28,7 +27,7 @@ def main():
     markers = markers + np.random.normal(0, 0.01, markers.shape)  # add noise
 
     # you can import the class from bionc
-    ik_solver = InverseKinematics(model, markers, solve_frame_per_frame=True)
+    ik_solver = InverseKinematics(model, markers)
 
     tic0 = time.time()
     Qopt_sqp = ik_solver.solve(method="sqpmethod")  # tend to be faster (with limited-memory hessian approximation)

tests/test_initial_guess_mode.py

@@ -19,7 +19,6 @@ def test_user_provided():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
     Qsolve = ik.solve(Q_init=Q_initialize, initial_guess_mode=InitialGuessModeType.USER_PROVIDED, method="ipopt")
 
@@ -91,7 +90,6 @@ def test_user_provided_first_frame_only():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
     ik.solve(
         Q_init=Q_initialize[:, 0:1],
@@ -167,7 +165,6 @@ def test_from_current_markers():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
     ik.solve(initial_guess_mode=InitialGuessModeType.FROM_CURRENT_MARKERS, method="ipopt")
 
@@ -239,7 +236,6 @@ def test_from_first_frame_markers():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
     ik.solve(initial_guess_mode=InitialGuessModeType.FROM_FIRST_FRAME_MARKERS, method="ipopt")
 
@@ -311,7 +307,6 @@ def test_Q_init_None_User_Provided():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
 
     with pytest.raises(
@@ -332,7 +327,6 @@ def test_Q_init_None_User_Provided_First_Frame_Only():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
 
     with pytest.raises(
@@ -353,7 +347,6 @@ def test_Q_init_Incorrect_Shape_User_Provided():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
     with pytest.raises(
         ValueError,

tests/test_initial_guess_mode_no_marker.py

@@ -1,7 +1,7 @@
 import numpy as np
 import pytest
-from pyomeca import Markers
 import re
+from pyomeca import Markers
 
 from bionc import InverseKinematics
 from bionc.bionc_numpy.enums import InitialGuessModeType
@@ -120,7 +120,6 @@ def test_exp_markers_none_from_current_markers():
     ik = InverseKinematics(
         model=module.model_creation_markerless(c3d_filename, False),
         experimental_heatmaps=XP_HEATMAP_PARAMETERS,
-        solve_frame_per_frame=True,
     )
     with pytest.raises(
         ValueError,
@@ -138,7 +137,6 @@ def test_exp_markers_none_from_first_frame_markers():
     ik = InverseKinematics(
         model=module.model_creation_markerless(c3d_filename, False),
         experimental_heatmaps=XP_HEATMAP_PARAMETERS,
-        solve_frame_per_frame=True,
     )
     with pytest.raises(
         ValueError,
@@ -147,47 +145,6 @@ def test_exp_markers_none_from_first_frame_markers():
         ik.solve(initial_guess_mode=InitialGuessModeType.FROM_FIRST_FRAME_MARKERS, method="ipopt")
 
 
-def test_from_first_frame_markers_and_frame_per_frame_is_false():
-    bionc = TestUtils.bionc_folder()
-    module = TestUtils.load_module(bionc + "/examples/model_creation/right_side_lower_limb.py")
-
-    c3d_filename = module.generate_c3d_file()
-    model = module.model_creation_from_measured_data(c3d_filename)
-    markers = Markers.from_c3d(c3d_filename, usecols=model.marker_names_technical).to_numpy()
-    ik = InverseKinematics(
-        model=model,
-        experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=False,
-    )
-    with pytest.raises(
-        ValueError,
-        match=f"Please set frame_per_frame to True",
-    ):
-        ik.solve(initial_guess_mode=InitialGuessModeType.FROM_FIRST_FRAME_MARKERS, method="ipopt")
-
-
-def test_user_provided_first_frame_only_and_frame_per_frame_is_false():
-    bionc = TestUtils.bionc_folder()
-    module = TestUtils.load_module(bionc + "/examples/model_creation/right_side_lower_limb.py")
-
-    c3d_filename = module.generate_c3d_file()
-    model = module.model_creation_from_measured_data(c3d_filename)
-    markers = Markers.from_c3d(c3d_filename, usecols=model.marker_names_technical).to_numpy()
-    Q_initialize = model.Q_from_markers(markers[:, :, 0:1])
-    ik = InverseKinematics(
-        model=model,
-        experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=False,
-    )
-    with pytest.raises(
-        ValueError,
-        match=f"Set frame_per_frame to True or use InitialGuessModeType.USER_PROVIDED.",
-    ):
-        ik.solve(
-            Q_init=Q_initialize, initial_guess_mode=InitialGuessModeType.USER_PROVIDED_FIRST_FRAME_ONLY, method="ipopt"
-        )
-
-
 def test_user_provided_first_frame_only_Q_init_too_many_frames():
     bionc = TestUtils.bionc_folder()
     module = TestUtils.load_module(bionc + "/examples/model_creation/right_side_lower_limb.py")
@@ -199,7 +156,6 @@ def test_user_provided_first_frame_only_Q_init_too_many_frames():
     ik = InverseKinematics(
         model=model,
         experimental_markers=markers[0:3, :, :],
-        solve_frame_per_frame=True,
     )
 
     with pytest.raises(