From 7d7178012d5524aa11364d89d8f5da9c4b2608a8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 23 Oct 2024 17:52:54 -0400 Subject: [PATCH] delete: solve_frame_per_frame=False, Only true forever --- bionc/bionc_numpy/inverse_kinematics.py | 112 ++------ .../inverse_kinematics/inverse_kinematics.py | 3 +- tests/pendulum_with_force.nmod | Bin 4867 -> 4868 bytes tests/test_initial_guess_mode.py | 7 - tests/test_initial_guess_mode_no_marker.py | 46 +--- tests/test_ultimate_inverse_kinematics.py | 244 +----------------- ...est_ultimate_inverse_kinematics_heatmap.py | 28 -- 7 files changed, 31 insertions(+), 409 deletions(-) diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index 3cf720ac..4256c4b7 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/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,8 +96,6 @@ 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]. @@ -106,26 +103,21 @@ def __init__( 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] diff --git a/examples/inverse_kinematics/inverse_kinematics.py b/examples/inverse_kinematics/inverse_kinematics.py index ed7135c7..116ece06 100644 --- a/examples/inverse_kinematics/inverse_kinematics.py +++ b/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) diff --git a/tests/pendulum_with_force.nmod b/tests/pendulum_with_force.nmod index a4cef232e4f24bac75f502451f53ce9bbbf9b876..1fd3fbf3b089cf6c2c2ca3c4d231b90424a06241 100644 GIT binary patch delta 40 scmZoxYfr4E6pva)Z09tF;x%%%^(W9 diff --git a/tests/test_initial_guess_mode.py b/tests/test_initial_guess_mode.py index 2931d2f7..6408277c 100644 --- a/tests/test_initial_guess_mode.py +++ b/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, diff --git a/tests/test_initial_guess_mode_no_marker.py b/tests/test_initial_guess_mode_no_marker.py index 9801fb27..7f9110f4 100644 --- a/tests/test_initial_guess_mode_no_marker.py +++ b/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( diff --git a/tests/test_ultimate_inverse_kinematics.py b/tests/test_ultimate_inverse_kinematics.py index 97755116..3dbbfa93 100644 --- a/tests/test_ultimate_inverse_kinematics.py +++ b/tests/test_ultimate_inverse_kinematics.py @@ -1,5 +1,3 @@ -from sys import platform - import numpy as np import pytest from casadi import Function, sumsqr @@ -9923,246 +9921,6 @@ def test_inverse_kinematics_class(): ik = InverseKinematics(1, markers) -def test_ik_all_frames(): - bionc = TestUtils.bionc_folder() - module = TestUtils.load_module(bionc + "/examples/model_creation/right_side_lower_limb.py") - - # Generate c3d file - filename = module.generate_c3d_file() - # Generate model - natural_model = module.model_creation_from_measured_data(filename) - - from pyomeca import Markers - - markers = Markers.from_c3d(filename).to_numpy()[:3, :, :] # 2 frames - markers = np.repeat(markers, 2, axis=2) # 2 x 100 frames - np.random.seed(42) - markers = markers + np.random.normal(0, 0.01, markers.shape) # add n - - from bionc import InverseKinematics - - # Create inverse kinematics object - ik = InverseKinematics(natural_model, markers, solve_frame_per_frame=False) - Qopt = ik.solve("ipopt") - - np.testing.assert_almost_equal( - Qopt[:, 0], - np.array( - [ - -0.91790143, - -0.12750568, - -0.37576491, - 0.38067353, - 1.23449894, - 0.91903518, - 0.23669274, - 1.32793102, - 0.87599607, - 0.20115714, - 0.79067216, - -0.57825024, - 0.9719539, - 0.19033464, - 0.13812435, - 0.23669274, - 1.32793102, - 0.87599607, - 0.28764037, - 1.3525538, - 0.48355754, - -0.22577275, - 0.91959056, - 0.32152741, - 0.95455166, - -0.28412637, - 0.09001856, - 0.28764037, - 1.3525538, - 0.48355754, - 0.33359652, - 1.39691013, - 0.13624344, - 0.29289363, - 0.95015745, - -0.106837, - -0.9086701, - 0.18603358, - -0.37377823, - 0.33359652, - 1.39691013, - 0.13624344, - 0.23275747, - 1.42507027, - 0.04496257, - 0.08072429, - 0.99583917, - -0.04228396, - ] - ), - decimal=1e-5, - ) - np.testing.assert_almost_equal( - Qopt[:, 1], - np.array( - [ - -0.88744641, - -0.17879346, - -0.42481969, - 0.37413348, - 1.24129827, - 0.92670137, - 0.22754127, - 1.32644376, - 0.87597311, - 0.18617193, - 0.80286729, - -0.56634277, - 0.98090316, - -0.14353058, - 0.13125538, - 0.22754127, - 1.32644376, - 0.87597311, - 0.28409724, - 1.35502658, - 0.48457256, - 0.08515075, - 0.92364402, - 0.37367242, - 0.89453518, - -0.44245375, - 0.06357264, - 0.28409724, - 1.35502658, - 0.48457256, - 0.33181168, - 1.40166807, - 0.13779534, - 0.44661404, - 0.8905725, - -0.08611925, - -0.92732736, - 0.23845078, - -0.28845311, - 0.33181168, - 1.40166807, - 0.13779534, - 0.22322181, - 1.4294506, - 0.05575831, - 0.1688473, - 0.9680654, - -0.18531047, - ] - ), - decimal=1e-5, - ) - - output_optim = ik.sol() - - marker_residuals_norm = np.array( - [ - [0.13939617, 0.15267663, 0.01099867, 0.17697249], - [0.01400245, 0.02816578, 0.01225596, 0.02732723], - [0.05950754, 0.05004255, 0.00608794, 0.04928525], - [0.03229003, 0.02770785, 0.01917931, 0.03195709], - [0.02918923, 0.02740908, 0.02073021, 0.02306008], - [0.03275235, 0.03529021, 0.02127043, 0.01552597], - [0.02830206, 0.00955584, 0.00630586, 0.01430647], - [0.03019092, 0.04547031, 0.02513658, 0.00967423], - [0.03250732, 0.043104, 0.01728737, 0.13690911], - [0.01139606, 0.01614814, 0.01343105, 0.02292201], - [0.00985797, 0.01732171, 0.01096336, 0.04289803], - ] - ) - - marker_residuals_xyz = np.array( - [ - [ - [-3.13174585e-02, -3.54847301e-02, 1.07232410e-02, -3.29018930e-02], - [8.46357162e-03, 7.18958696e-03, 1.09855243e-02, 1.12116983e-02], - [-1.36583574e-02, 3.60701789e-03, -2.50832353e-03, -1.11054715e-02], - [1.43398238e-02, -1.27751928e-03, -1.88466469e-02, 1.10698376e-02], - [4.83503899e-03, 3.96815729e-03, -1.18340609e-03, -7.52338126e-03], - [3.01601877e-03, 7.32569313e-03, 1.66176602e-03, -4.90824706e-03], - [2.62714351e-03, 3.42191006e-03, 3.54983363e-03, 9.33913850e-04], - [1.80827823e-03, 1.42256982e-02, -2.08833824e-03, 1.76715838e-04], - [6.46140636e-03, 1.88465029e-03, 1.41415260e-02, 4.60648211e-02], - [5.54653889e-05, -6.73020047e-03, -1.08674374e-02, -2.05912733e-02], - [3.36906919e-03, 1.86973609e-03, -5.56773885e-03, 7.57327949e-03], - ], - [ - [1.93454367e-02, 2.89752676e-02, -2.24096957e-03, 4.17441607e-02], - [-1.07077997e-02, -2.69182629e-02, 5.35460921e-03, -2.35483365e-02], - [2.65775138e-03, 8.12997964e-03, 5.29395349e-03, -1.33361137e-03], - [-2.06554248e-02, -2.15402796e-02, -1.83803032e-03, -2.35709588e-02], - [-8.49084513e-03, -8.25897381e-03, -8.92500388e-03, -2.10075275e-02], - [4.29840314e-03, 7.49326719e-03, 2.46776579e-03, 3.94460077e-03], - [5.46882550e-03, -6.43280647e-03, 5.07881619e-03, 1.17292360e-02], - [1.94800880e-03, 1.02709117e-02, -2.28270456e-02, 7.00206279e-03], - [4.97102926e-03, 7.04173085e-03, 7.49847823e-03, 4.45976303e-03], - [3.34002701e-03, 3.03696417e-03, 7.52089030e-03, -9.89713286e-03], - [-2.17541212e-03, -1.79779840e-03, 2.61653618e-03, 1.04777437e-02], - ], - [ - [1.34447993e-01, 1.45641416e-01, -9.80213035e-04, 1.68802111e-01], - [-3.12724480e-03, 4.12652468e-03, -9.24607589e-04, 8.15787404e-03], - [5.78578702e-02, 4.92458118e-02, -1.65690369e-03, 4.79992229e-02], - [-2.02575683e-02, -1.73812873e-02, -3.04492501e-03, -1.85236117e-02], - [-2.75052573e-02, -2.58321633e-02, 1.86731226e-02, -5.81805182e-03], - [-3.23286880e-02, -3.36984259e-02, -2.10613336e-02, -1.41917258e-02], - [-2.76441061e-02, -6.18251929e-03, -1.16967825e-03, -8.13805523e-03], - [-3.00736903e-02, -4.19486276e-02, 1.03156409e-02, -6.67313072e-03], - [-3.14684728e-02, -4.24831391e-02, 6.53018289e-03, -1.28849711e-01], - [-1.08954766e-02, -1.43611891e-02, 2.39337073e-03, -1.86133998e-03], - [-9.00535941e-03, -1.71264007e-02, -9.07465590e-03, -4.09035811e-02], - ], - ] - ) - - total_marker_residuals = np.array([0.17068145, 0.18425451, 0.0531913, 0.24020238]) - max_marker_distance = ["RFWT", "RFWT", "RANI", "RFWT"] - joint_residuals = np.array( - [ - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0], - ] - ) - - total_joint_residuals = np.array([0.0, 0.0, 0.0, 0.0]) - max_joint_violation = ["hip", "hip", "hip", "hip"] - segment_rigidity_residual_norm = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - total_rigity_residuals = np.array([2.33104442e-16, 3.54446676e-16, 2.79671565e-16, 4.26486230e-15]) - max_rigidbody_violation = ["SHANK", "SHANK", "SHANK", "THIGH"] - success = [True, True, True, True] - - np.testing.assert_almost_equal(output_optim["marker_residuals_norm"], marker_residuals_norm, decimal=1e-5) - np.testing.assert_almost_equal(output_optim["marker_residuals_xyz"], marker_residuals_xyz, decimal=1e-5) - np.testing.assert_almost_equal(output_optim["total_marker_residuals"], total_marker_residuals, decimal=1e-5) - assert output_optim["max_marker_distance"] == max_marker_distance - np.testing.assert_almost_equal(output_optim["joint_residuals"], joint_residuals, decimal=1e-5) - np.testing.assert_almost_equal(output_optim["total_joint_residuals"], total_joint_residuals, decimal=1e-5) - assert output_optim["max_joint_violation"] == max_joint_violation - np.testing.assert_almost_equal( - output_optim["segment_rigidity_residual_norm"], segment_rigidity_residual_norm, decimal=1e-5 - ) - np.testing.assert_almost_equal(output_optim["total_rigidity_residuals"], total_rigity_residuals, decimal=1e-5) - assert output_optim["success"] == success - - # check if this is a macos system - if platform != "darwin": - assert output_optim["max_rigidbody_violation"] == max_rigidbody_violation - - def test_ik_frame_per_frame_extra_obj(): bionc = TestUtils.bionc_folder() module = TestUtils.load_module(bionc + "/examples/model_creation/right_side_lower_limb.py") @@ -10182,7 +9940,7 @@ def test_ik_frame_per_frame_extra_obj(): from bionc import InverseKinematics # Create inverse kinematics object - ik = InverseKinematics(natural_model, markers, solve_frame_per_frame=True) + ik = InverseKinematics(natural_model, markers) # test extra objective function extra_objective_function = Function( diff --git a/tests/test_ultimate_inverse_kinematics_heatmap.py b/tests/test_ultimate_inverse_kinematics_heatmap.py index 287e4d34..03207f7e 100644 --- a/tests/test_ultimate_inverse_kinematics_heatmap.py +++ b/tests/test_ultimate_inverse_kinematics_heatmap.py @@ -191,7 +191,6 @@ def test_global_heatmap_ik(): ik = InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) ik.solve(method="ipopt", Q_init=Q_initialize, initial_guess_mode=InitialGuessModeType.USER_PROVIDED) @@ -270,23 +269,6 @@ def test_global_heatmap_ik(): np.testing.assert_almost_equal(solutions["heatmap_confidence_2d"], expected_heatmap_confidence_2d, decimal=1e-6) -def test_error_solve_frame_per_frame(): - bionc = TestUtils.bionc_folder() - module = TestUtils.load_module(bionc + "/examples/model_creation/markerless_model.py") - - c3d_filename = module.generate_c3d_file() - - with pytest.raises( - NotImplementedError, - match=f"Not possible to solve for all frames with heatmap parameters. Please set solve_frame_per_frame=True", - ): - InverseKinematics( - model=module.model_creation_markerless(c3d_filename, False), - experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=False, - ) - - def test_error_Qinit_is_none(): bionc = TestUtils.bionc_folder() module = TestUtils.load_module(bionc + "/examples/model_creation/markerless_model.py") @@ -295,7 +277,6 @@ def test_error_Qinit_is_none(): ik = InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) with pytest.raises( ValueError, match=f"Please provide Q_init if you want to use InitialGuessModeType.USER_PROVIDED." @@ -316,7 +297,6 @@ def test_error_markers_and_heatmaps(): model=module.model_creation_markerless(c3d_filename, False), experimental_markers=experimental_markers, experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -331,7 +311,6 @@ def test_error_no_markers_and_no_heatmaps(): model=module.model_creation_markerless(c3d_filename, False), experimental_markers=None, experimental_heatmaps=None, - solve_frame_per_frame=True, ) @@ -420,7 +399,6 @@ def test_error_experimental_heatmaps_is_not_a_dictionnary(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -455,7 +433,6 @@ def test_error_first_dim_cam_param_is_not_3(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -486,7 +463,6 @@ def test_error_len_cam_param_is_not_3(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -524,7 +500,6 @@ def test_error_second_dim_cam_param_is_not_4(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -568,7 +543,6 @@ def test_error_len_gaussian_param_is_not_4(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -648,7 +622,6 @@ def test_error_first_dim_gaussian_param_is_not_5(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, ) @@ -771,5 +744,4 @@ def test_error_same_nb_cam_for_gaussian_and_cam_param(): InverseKinematics( model=module.model_creation_markerless(c3d_filename, False), experimental_heatmaps=experimental_heatmap_parameters, - solve_frame_per_frame=True, )