Added one more test for the RIGID_IMPLICIT defects

bioptim/examples/toy_examples/muscle_driven_with_contact/contact_forces_inverse_dynamics_constraint_muscle.py

@@ -70,6 +70,7 @@ def prepare_ocp(
     defects_type: DefectType,
     contact_types: list[ContactType],
     expand_dynamics=True,
+    control_type=ControlType.CONSTANT,
 ):
 
     # BioModel
@@ -179,7 +180,7 @@ def prepare_ocp(
         x_init=x_init,
         u_init=u_init,
         a_init=a_init,
-        control_type=ControlType.CONSTANT,
+        control_type=control_type,
         objective_functions=objective_functions,
         constraints=constraints,
         multinode_constraints=multinode_constraints,

bioptim/examples/toy_examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py

@@ -24,7 +24,6 @@
 from bioptim.examples.utils import ExampleUtils
 import numpy as np
 
-
 # Load track_segment_on_rt
 spec = importlib.util.spec_from_file_location(
     "data_to_track", str(Path(__file__).parent) + "/contact_forces_inequality_constraint_muscle.py"

bioptim/examples/toy_examples/torque_driven_ocp/spring_load.py

@@ -25,7 +25,6 @@
 from matplotlib import pyplot as plt
 import numpy as np
 
-
 # scenarios are based on a Mayer term (at Tf)
 # 0: maximize upward speed - expected kinematics: negative torque to get as low as possible and release
 # 1: maximize downward speed - expected kinematics: positive torque to get as high as possible and release

bioptim/gui/online_callback_server.py

@@ -29,7 +29,6 @@
     IntIterableOptional,
 )
 
-
 _DEFAULT_HOST = "localhost"
 _DEFAULT_PORT = 3050
 _HEADER_GENERIC_LEN = 1024

bioptim/limits/penalty_option.py

@@ -341,9 +341,9 @@ def _check_weight_dimensions(self, controller: PenaltyController):
         self.weight.check_and_adjust_dimensions(n_nodes, f"{self.name} weight")
 
         if (
-                not isinstance(self.weight, ConstraintWeight) and
-                not isinstance(self.weight, ObjectiveWeight) and
-                len(self.weight.shape) != 1
+            not isinstance(self.weight, ConstraintWeight)
+            and not isinstance(self.weight, ObjectiveWeight)
+            and len(self.weight.shape) != 1
         ):
             raise RuntimeError(
                 "Something went wrong, the weight should be a vector at this point. "

bioptim/misc/parameters_types.py

@@ -4,7 +4,6 @@
 from casadi import MX, SX, DM
 from .enums import Node
 
-
 Int: TypeAlias = int
 Range: TypeAlias = range
 Str: TypeAlias = str

bioptim/optimization/vector_layout.py

@@ -2,7 +2,6 @@
 import numpy as np
 from typing import Callable, Iterator, Tuple, Any
 
-
 Key = Tuple[Any, ...]  # (phase, var_type, node) or ("global", "time"/"parameters")
 KeySize = Tuple[Key, int]
 GeneratorType = Callable[

tests/shard2/test_global_muscle_driven_ocp_with_contacts.py

@@ -0,0 +1,280 @@
+import pytest
+
+import numpy as np
+import numpy.testing as npt
+from bioptim import OdeSolver, ControlType, PhaseDynamics, SolutionMerge, DefectType, ContactType, Solver
+
+from ..utils import TestUtils
+
+
+@pytest.mark.parametrize(
+    "control_type",
+    [
+        ControlType.CONSTANT,
+        ControlType.LINEAR_CONTINUOUS,
+    ],
+)
+@pytest.mark.parametrize(
+    "defects_type",
+    [
+        DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS,
+        DefectType.TAU_EQUALS_INVERSE_DYNAMICS,
+    ],
+)
+@pytest.mark.parametrize("contact_types", [[ContactType.RIGID_EXPLICIT], [ContactType.RIGID_IMPLICIT]])
+def test_contact_forces_inverse_dynamics_constraint_muscle(control_type, defects_type, contact_types):
+    """
+    Here we only test that the values of the objective and constraints at the initial guess are correct because the
+    convergence of this problem is too long to wait for.
+    Cannot have multi-node constraints in SHARED_DURING_THE_PHASE.
+    ode_solver can only be COLLOCATION for this type of implicit constraints.
+    """
+    from bioptim.examples.toy_examples.muscle_driven_with_contact import (
+        contact_forces_inverse_dynamics_constraint_muscle as ocp_module,
+    )
+
+    bioptim_folder = TestUtils.module_folder(ocp_module)
+
+    if defects_type == DefectType.TAU_EQUALS_INVERSE_DYNAMICS and ContactType.RIGID_EXPLICIT in contact_types:
+        with pytest.raises(
+            NotImplementedError, match="Inverse dynamics, cannot be used with ContactType.RIGID_EXPLICIT yet"
+        ):
+            ocp_module.prepare_ocp(
+                biorbd_model_path=bioptim_folder + "/../../models/2segments_4dof_2contacts_1muscle.bioMod",
+                phase_time=0.3,
+                n_shooting=10,
+                defects_type=defects_type,
+                contact_types=contact_types,
+            )
+        return
+
+    ocp = ocp_module.prepare_ocp(
+        biorbd_model_path=bioptim_folder + "/../../models/2segments_4dof_2contacts_1muscle.bioMod",
+        phase_time=0.3,
+        n_shooting=10,
+        defects_type=defects_type,
+        contact_types=contact_types,
+    )
+
+    solver = Solver.IPOPT()
+    solver.set_maximum_iterations(0)
+    sol = ocp.solve()
+
+    # Check the values at the initial guess
+    f = np.array(sol.cost)
+    npt.assert_equal(f.shape, (1, 1))
+    g = np.array(sol.constraints)
+
+    # TODO: TestUtils.assert_objective_value should be used, but it is broken here
+    if defects_type == DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS:
+        if ContactType.RIGID_EXPLICIT in contact_types:
+            npt.assert_almost_equal(f[0], 488.41630088)
+            npt.assert_equal(g.shape, (326, 1))
+            npt.assert_almost_equal(np.sum(g**2), 0)
+        elif ContactType.RIGID_IMPLICIT in contact_types:
+            npt.assert_almost_equal(f[0], 732.52924763)
+            npt.assert_equal(g.shape, (443, 1))
+            npt.assert_almost_equal(np.sum(g**2), 0)
+        else:
+            raise ValueError("Unexpected test configuration")
+    elif defects_type == DefectType.TAU_EQUALS_INVERSE_DYNAMICS:
+        if ContactType.RIGID_EXPLICIT in contact_types:
+            raise RuntimeError("Should be skipped above")
+        elif ContactType.RIGID_IMPLICIT in contact_types:
+            npt.assert_almost_equal(f[0], 732.52924762)
+            npt.assert_equal(g.shape, (443, 1))
+            npt.assert_almost_equal(np.sum(g**2), 0)
+        else:
+            raise ValueError("Unexpected test configuration")
+
+    # Now test the internal constraints with random values
+    g_internal = ocp.nlp[0].g_internal
+    np.random.seed(0)
+    t = 1.05
+    dt = 0.1
+    x = np.random.rand(40) * 0.1
+    x_multi = np.random.rand(80) * 0.1
+    u = np.random.rand(10) * 0.1
+    u_multi = np.random.rand(20) * 0.1
+    p = []
+    a = np.random.rand(15) * 0.1
+    a_multi = np.random.rand(30) * 0.1
+    d = []
+
+    # Since a small change in q has a large effect,the tolerance is quite high, but it does not mean these tests are not important !
+    if defects_type == DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS:
+        if ContactType.RIGID_EXPLICIT in contact_types:
+            # STATE_CONTINUITY (This is the important one since the contact are take into account in the dynamics)
+            value = g_internal[0].function[3](t, dt, x, u, p, [], d)
+            npt.assert_almost_equal(
+                np.array(value).reshape(
+                    32,
+                ),
+                np.array(
+                    [
+                        -0.0771307,
+                        0.0688179,
+                        0.0100632,
+                        -0.0133405,
+                        0.0524087,
+                        0.00946086,
+                        0.143732,
+                        0.117869,
+                        1.47277,
+                        -1.60227,
+                        1.21719,
+                        0.224233,
+                        2.62202,
+                        1.71563,
+                        -4.32498,
+                        -1.58506,
+                        -3.84431,
+                        2.00027,
+                        -1.11185,
+                        0.954616,
+                        1.60419,
+                        -1.36516,
+                        -0.486072,
+                        6.80542,
+                        6.85567,
+                        -4.86582,
+                        -2.0102,
+                        -1.22047,
+                        -2.90894,
+                        0.285695,
+                        -5.24616,
+                        -7.31138,
+                    ]
+                ),
+                decimal=4,
+            )
+        elif ContactType.RIGID_IMPLICIT in contact_types:
+            # ALGEBRAIC_STATES_CONTINUITY_Multinode_P0n0_P0n1
+            value = g_internal[6].function[6](t, dt, x_multi, u_multi, p, a_multi, d)
+            npt.assert_almost_equal(
+                np.array(value).reshape(
+                    3,
+                ),
+                np.array([-0.0629004, 0.0397554, 0.0231877]),
+                decimal=4,
+            )
+
+            # STATE_CONTINUITY (This is the important one since the defects are set in the dynamics)
+            value = g_internal[9].function[3](t, dt, x, u, p, a, d)
+            npt.assert_almost_equal(
+                np.array(value).reshape(
+                    41,
+                ),
+                np.array(
+                    [
+                        -0.0771307,
+                        0.0688179,
+                        0.0100632,
+                        -0.0133405,
+                        0.0524087,
+                        0.00946086,
+                        0.143732,
+                        0.117869,
+                        1.47277,
+                        -1.60227,
+                        1.21719,
+                        0.224233,
+                        1.34074,
+                        11.473,
+                        -1.79717,
+                        -7.85084,
+                        0.466307,
+                        1.5446,
+                        -0.505904,
+                        -3.84431,
+                        2.00027,
+                        -1.11185,
+                        0.954616,
+                        0.355899,
+                        8.39088,
+                        1.94447,
+                        0.772184,
+                        1.36189,
+                        -1.38405,
+                        0.797744,
+                        6.85567,
+                        -4.86582,
+                        -2.0102,
+                        -1.22047,
+                        -3.13449,
+                        10.1336,
+                        -4.82436,
+                        -11.2889,
+                        -5.53175,
+                        0.248094,
+                        -4.12106,
+                    ]
+                ),
+                decimal=4,
+            )
+
+    elif defects_type == DefectType.TAU_EQUALS_INVERSE_DYNAMICS:
+        if ContactType.RIGID_IMPLICIT in contact_types:
+            # ALGEBRAIC_STATES_CONTINUITY_Multinode_P0n0_P0n1
+            value = g_internal[6].function[6](t, dt, x_multi, u_multi, p, a_multi, d)
+            npt.assert_almost_equal(
+                np.array(value).reshape(
+                    3,
+                ),
+                np.array([-0.0629004, 0.0397554, 0.0231877]),
+                decimal=4,
+            )
+
+            # STATE_CONTINUITY (This is the important one since the defects are set in the dynamics)
+            value = g_internal[9].function[3](t, dt, x, u, p, a, d)
+            npt.assert_almost_equal(
+                np.array(value).reshape(
+                    41,
+                ),
+                np.array(
+                    [
+                        -0.0771307,
+                        0.0688179,
+                        0.0100632,
+                        -0.0133405,
+                        0.0524087,
+                        0.00946086,
+                        0.143732,
+                        0.117869,
+                        1.47277,
+                        -1.60227,
+                        1.21719,
+                        0.224233,
+                        -11.6701,
+                        -153.657,
+                        -8.13758,
+                        -1.89798,
+                        0.466307,
+                        1.5446,
+                        -0.505904,
+                        -3.84431,
+                        2.00027,
+                        -1.11185,
+                        0.954616,
+                        -12.1213,
+                        -115.205,
+                        -7.96739,
+                        -1.63524,
+                        1.36189,
+                        -1.38405,
+                        0.797744,
+                        6.85567,
+                        -4.86582,
+                        -2.0102,
+                        -1.22047,
+                        60.6238,
+                        -134.478,
+                        15.8761,
+                        -1.60747,
+                        -5.53175,
+                        0.248094,
+                        -4.12106,
+                    ]
+                ),
+                decimal=3,
+            )

tests/shard3/test_global_getting_started.py

@@ -31,7 +31,6 @@
 
 from ..utils import TestUtils
 
-
 # Store results for all tests
 global test_memory
 test_memory = {}

tests/shard3/test_initial_condition.py

@@ -21,7 +21,6 @@
 from bioptim.limits.path_conditions import InitialGuess
 from ..utils import TestUtils
 
-
 # TODO: Add negative test for sizes
 
 

tests/shard4/test_penalty.py

@@ -34,7 +34,6 @@
 from bioptim.optimization.optimization_variable import OptimizationVariableList
 from tests.utils import TestUtils
 
-
 N_SHOOTING = 10
 EXTERNAL_FORCE_ARRAY = np.zeros((9, N_SHOOTING))
 EXTERNAL_FORCE_ARRAY[:, 0] = [

tests/shard6/test_plots.py

@@ -6,7 +6,6 @@
 from bioptim import InterpolationType, PhaseDynamics, Solver, ControlType, OdeSolver, TorqueBiorbdModel
 from ..utils import TestUtils
 
-
 if platform.system() != "Linux":
     pytest.skip("Skipping tests on non-Linux platforms", allow_module_level=True)