diff --git a/bioptim/examples/toy_examples/muscle_driven_with_contact/contact_forces_inverse_dynamics_constraint_muscle.py b/bioptim/examples/toy_examples/muscle_driven_with_contact/contact_forces_inverse_dynamics_constraint_muscle.py index 0712706c1..9413e175c 100644 --- a/bioptim/examples/toy_examples/muscle_driven_with_contact/contact_forces_inverse_dynamics_constraint_muscle.py +++ b/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, diff --git a/bioptim/examples/toy_examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py b/bioptim/examples/toy_examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py index a3339921f..a84846f28 100644 --- a/bioptim/examples/toy_examples/muscle_driven_with_contact/muscle_activations_contacts_tracker.py +++ b/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" diff --git a/bioptim/examples/toy_examples/torque_driven_ocp/spring_load.py b/bioptim/examples/toy_examples/torque_driven_ocp/spring_load.py index 85a290691..c25846ffb 100644 --- a/bioptim/examples/toy_examples/torque_driven_ocp/spring_load.py +++ b/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 diff --git a/bioptim/gui/online_callback_server.py b/bioptim/gui/online_callback_server.py index 1708866a1..c51563a6c 100644 --- a/bioptim/gui/online_callback_server.py +++ b/bioptim/gui/online_callback_server.py @@ -29,7 +29,6 @@ IntIterableOptional, ) - _DEFAULT_HOST = "localhost" _DEFAULT_PORT = 3050 _HEADER_GENERIC_LEN = 1024 diff --git a/bioptim/limits/penalty_option.py b/bioptim/limits/penalty_option.py index 51a1dd283..6fdb413b7 100644 --- a/bioptim/limits/penalty_option.py +++ b/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. " diff --git a/bioptim/misc/parameters_types.py b/bioptim/misc/parameters_types.py index 5b9e02e1b..6facbc178 100644 --- a/bioptim/misc/parameters_types.py +++ b/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 diff --git a/bioptim/optimization/vector_layout.py b/bioptim/optimization/vector_layout.py index d0af05989..8dca79004 100644 --- a/bioptim/optimization/vector_layout.py +++ b/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[ diff --git a/tests/shard2/test_global_muscle_driven_ocp_with_contacts.py b/tests/shard2/test_global_muscle_driven_ocp_with_contacts.py new file mode 100644 index 000000000..1b00a68e6 --- /dev/null +++ b/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, + ) diff --git a/tests/shard3/test_global_getting_started.py b/tests/shard3/test_global_getting_started.py index 059f4dd70..c72e8475f 100644 --- a/tests/shard3/test_global_getting_started.py +++ b/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 = {} diff --git a/tests/shard3/test_initial_condition.py b/tests/shard3/test_initial_condition.py index 5a12d0cb3..49747a6d7 100644 --- a/tests/shard3/test_initial_condition.py +++ b/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 diff --git a/tests/shard4/test_penalty.py b/tests/shard4/test_penalty.py index e6d27fa97..acf759476 100644 --- a/tests/shard4/test_penalty.py +++ b/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] = [ diff --git a/tests/shard6/test_plots.py b/tests/shard6/test_plots.py index 46fd754cb..a0d125817 100644 --- a/tests/shard6/test_plots.py +++ b/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)