What I would like it to feel like

bioptim/dynamics/configure_problem.py

@@ -15,6 +15,7 @@
     ConstraintType,
     SoftContactDynamics,
     PhaseDynamics,
+    DefectType,
 )
 from ..misc.fcn_enum import FcnEnum
 from ..misc.mapping import BiMapping, Mapping
@@ -51,7 +52,7 @@ class ConfigureProblem:
         case muscles are too weak.
     configure_dynamics_function(ocp, nlp, dyn_func, **extra_params)
         Configure the dynamics of the system
-    configure_contact_function(ocp, nlp, dyn_func: Callable, **extra_params)
+    configure_rigid_contact_function(ocp, nlp, dyn_func: Callable, **extra_params)
         Configure the contact points
     configure_soft_contact_function
         Configure the soft contact function
@@ -163,6 +164,7 @@ def torque_driven(
         with_friction: bool = False,
         fatigue: FatigueList = None,
         numerical_data_timeseries: dict[str, np.ndarray] = None,
+        defects_type: DefectType = DefectType.EXPLICIT,
     ):
         """
         Configure the dynamics for a torque driven program (states are q and qdot, controls are tau)
@@ -181,16 +183,15 @@ def torque_driven(
             If the dynamic with ligament should be used
         with_friction: bool
             If the dynamic with joint friction should be used (friction = coefficients * qdot)
-        soft_contacts_dynamics: SoftContactDynamics
-            which soft contact dynamic should be used
         fatigue: FatigueList
             A list of fatigue elements
         numerical_data_timeseries: dict[str, np.ndarray]
             A list of values to pass to the dynamics at each node. Experimental external forces should be included here.
+        defects_type: DefectType
+            The type of defect to use (EXPLICIT or IMPLICIT)
         """
-
-        _check_contacts_in_biorbd_model(with_rigid_contact, nlp.model.nb_contacts, nlp.phase_idx)
-        _check_soft_contacts_dynamics(soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx)
+        _check_rigid_contacts_in_biorbd_model(with_rigid_contact, nlp)
+        _check_soft_contacts_in_biorbd_model(with_rigid_contact, nlp)
 
         # Declared rigidbody states and controls
         ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False)
@@ -199,7 +200,7 @@ def torque_driven(
         ConfigureProblem.configure_qddot(ocp, nlp, False, False, True)
 
         # Declared soft contacts controls
-        if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT:
+        if with_soft_contact and defects_type == DefectType.IMPLICIT:
             ConfigureProblem.configure_soft_contact_forces(ocp, nlp, False, True)
 
         # Configure the actual ODE of the dynamics
@@ -219,19 +220,28 @@ def torque_driven(
 
         # Configure the contact forces
         if with_rigid_contact:
-            ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven)
+            if defects_type == DefectType.EXPLICIT:
+                ConfigureProblem.configure_rigid_contact_function(
+                    ocp,
+                    nlp,
+                    DynamicsFunctions.forces_from_torque_driven,
+                )
+            elif defects_type == DefectType.IMPLICIT:
+                ConfigureProblem.configure
+            else:
+                raise ValueError("defects_type must be either DefectType.EXPLICIT or DefectType.IMPLICIT if with_rigid_contact is True")
 
         # Configure the soft contact forces
         ConfigureProblem.configure_soft_contact_function(ocp, nlp)
 
-        # Algebraic constraints of soft contact forces if needed
-        if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT:
-            ocp.implicit_constraints.add(
-                ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS,
-                node=Node.ALL_SHOOTING,
-                penalty_type=ConstraintType.IMPLICIT,
-                phase=nlp.phase_idx,
-            )
+        # # Algebraic constraints of soft contact forces if needed
+        # if soft_contacts_dynamics == SoftContactDynamics.CONSTRAINT:
+        #     ocp.implicit_constraints.add(
+        #         ImplicitConstraintFcn.SOFT_CONTACTS_EQUALS_SOFT_CONTACTS_DYNAMICS,
+        #         node=Node.ALL_SHOOTING,
+        #         penalty_type=ConstraintType.IMPLICIT,
+        #         phase=nlp.phase_idx,
+        #     )
 
     @staticmethod
     def torque_driven_free_floating_base(
@@ -541,9 +551,8 @@ def torque_derivative_driven(
             A list of values to pass to the dynamics at each node. Experimental external forces should be included here.
 
         """
-        _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
-
-        _check_soft_contacts_dynamics(soft_contacts_dynamics, nlp.model.nb_soft_contacts, nlp.phase_idx)
+        _check_rigid_contacts_in_biorbd_model(with_rigid_contact, nlp)
+        _check_soft_contacts_in_biorbd_model(with_soft_contact, nlp)
 
         ConfigureProblem.configure_q(ocp, nlp, True, False)
         ConfigureProblem.configure_qdot(ocp, nlp, True, False)
@@ -560,14 +569,14 @@ def torque_derivative_driven(
                 ocp,
                 nlp,
                 DynamicsFunctions.torque_derivative_driven,
-                with_contact=with_contact,
+                with_rigid_contact=with_rigid_contact,
                 with_passive_torque=with_passive_torque,
                 with_ligament=with_ligament,
                 with_friction=with_friction,
             )
 
         if with_rigid_contact:
-            ConfigureProblem.configure_contact_function(
+            ConfigureProblem.configure_rigid_contact_function(
                 ocp,
                 nlp,
                 DynamicsFunctions.forces_from_torque_driven,
@@ -614,8 +623,7 @@ def torque_activations_driven(
         numerical_data_timeseries: dict[str, np.ndarray]
             A list of values to pass to the dynamics at each node. Experimental external forces should be included here.
         """
-
-        _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
+        _check_rigid_contacts_in_biorbd_model(with_rigid_contact, nlp)
 
         ConfigureProblem.configure_q(ocp, nlp, True, False)
         ConfigureProblem.configure_qdot(ocp, nlp, True, False)
@@ -736,7 +744,7 @@ def muscle_driven(
         numerical_data_timeseries: dict[str, np.ndarray]
             A list of values to pass to the dynamics at each node. Experimental external forces should be included here.
         """
-        _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
+        _check_rigid_contacts_in_biorbd_model(with_rigid_contact, nlp)
 
         if fatigue is not None and "tau" in fatigue and not with_residual_torque:
             raise RuntimeError("Residual torques need to be used to apply fatigue on torques")
@@ -1128,7 +1136,7 @@ def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params):
                     )
 
     @staticmethod
-    def configure_contact_function(ocp, nlp, contact_func: Callable, **extra_params):
+    def configure_rigid_contact_function(ocp, nlp, contact_func: Callable, **extra_params):
         """
         Configure the contact points
 
@@ -1975,6 +1983,7 @@ def __init__(
         skip_continuity: bool = False,
         state_continuity_weight: float | None = None,
         phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE,
+        defects_type: DefectType = DefectType.EXPLICIT,
         numerical_data_timeseries: dict[str, np.ndarray] = None,
         **extra_parameters: Any,
     ):
@@ -1996,6 +2005,8 @@ def __init__(
             otherwise it is an objective
         phase_dynamics: PhaseDynamics
             If the dynamics should be shared between the nodes or not
+        defects_type:
+            The type of defect to use (EXPLICIT or IMPLICIT).
         numerical_data_timeseries: dict[str, np.ndarray]
             The numerical timeseries at each node. ex: the experimental external forces data should go here.
         """
@@ -2023,6 +2034,7 @@ def __init__(
         self.state_continuity_weight = state_continuity_weight
         self.phase_dynamics = phase_dynamics
         self.numerical_data_timeseries = numerical_data_timeseries
+        self.defects_type = defects_type
 
 
 class DynamicsList(UniquePerPhaseOptionList):
@@ -2077,23 +2089,10 @@ def _check_numerical_timeseries_format(numerical_timeseries: np.ndarray, n_shoot
             f"where the list is the number of shooting points of the phase "
         )
 
+def _check_soft_contacts_in_biorbd_model(with_soft_contact: bool, nlp):
+    if with_soft_contact and nlp.model.nb_soft_contacts == 0:
+        raise ValueError(f"No contact defined in the .bioMod of phase {nlp.phase_idx}, set with_rigid_contact to False.")
 
-def _check_soft_contacts_dynamics(
-    soft_contacts_dynamics: SoftContactDynamics,
-    nb_soft_contacts,
-    phase_idx: int,
-):
-    if nb_soft_contacts != 0:
-        if (
-            soft_contacts_dynamics != SoftContactDynamics.CONSTRAINT
-            and soft_contacts_dynamics != SoftContactDynamics.ODE
-        ):
-            raise ValueError(
-                f"Phase {phase_idx} has soft contacts but the soft_contacts_dynamics is not "
-                f"SoftContactDynamics.CONSTRAINT or SoftContactDynamics.ODE."
-            )
-
-
-def _check_contacts_in_biorbd_model(with_contact: bool, nb_contacts: int, phase_idx: int):
-    if with_contact and nb_contacts == 0:
-        raise ValueError(f"No contact defined in the .bioMod of phase {phase_idx}, set with_contact to False.")
+def _check_rigid_contacts_in_biorbd_model(with_rigid_contact: bool, nlp):
+    if with_rigid_contact and nlp.model.nb_contacts == 0:
+        raise ValueError(f"No contact defined in the .bioMod of phase {nlp.phase_idx}, set with_soft_contact to False.")

bioptim/dynamics/dynamics_functions.py

@@ -155,9 +155,29 @@ def torque_driven(
         defects = None
         # TODO: contacts and fatigue to be handled with implicit dynamics
         if nlp.ode_solver.defects_type == DefectType.IMPLICIT:
-            if not with_contact and fatigue is None:
+            if fatigue:
+                raise NotImplementedError("Fatigue is not implemented with implicit dynamics yet.")
+            elif with_rigid_contact:
                 qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx)
-                tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_contact, external_forces)
+                tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_rigid_contact, external_forces)
+                defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1])
+
+                dq_defects = []
+                for _ in range(tau_id.shape[1]):
+                    dq_defects.append(
+                        dq
+                        - DynamicsFunctions.compute_qdot(
+                            nlp,
+                            q,
+                            DynamicsFunctions.get(nlp.states_dot.scaled["qdot"], nlp.states_dot.scaled.cx),
+                        )
+                    )
+                defects[: dq.shape[0], :] = horzcat(*dq_defects)
+                # We modified on purpose the size of the tau to keep the zero in the defects in order to respect the dynamics
+                defects[dq.shape[0] :, :] = tau - tau_id
+            else:
+                qddot = DynamicsFunctions.get(nlp.states_dot["qddot"], nlp.states_dot.scaled.cx)
+                tau_id = DynamicsFunctions.inverse_dynamics(nlp, q, qdot, qddot, with_rigid_contact, external_forces)
                 defects = nlp.cx(dq.shape[0] + tau_id.shape[0], tau_id.shape[1])
 
                 dq_defects = []
@@ -484,7 +504,7 @@ def torque_activations_driven(
             The numerical timeseries of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_contact: bool
+        with_rigid_contact: bool
             If the dynamic with contact should be used
         with_passive_torque: bool
             If the dynamic with passive torque should be used
@@ -514,7 +534,7 @@ def torque_activations_driven(
         external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries)
 
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
-        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces)
+        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_rigid_contact, external_forces)
 
         dq = horzcat(*[dq for _ in range(ddq.shape[1])])
 
@@ -529,7 +549,7 @@ def torque_derivative_driven(
         algebraic_states,
         numerical_timeseries,
         nlp,
-        with_contact: bool,
+        with_rigid_contact: bool,
         with_passive_torque: bool,
         with_ligament: bool,
         with_friction: bool,
@@ -553,7 +573,7 @@ def torque_derivative_driven(
             The numerical timeseries of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_contact: bool
+        with_rigid_contact: bool
             If the dynamic with contact should be used
         with_passive_torque: bool
             If the dynamic with passive torque should be used
@@ -580,7 +600,7 @@ def torque_derivative_driven(
         dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls)
 
         external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries)
-        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces)
+        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_rigid_contact, external_forces)
         dxdt = nlp.cx(nlp.states.shape, ddq.shape[1])
         dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])])
         dxdt[nlp.states["qdot"].index, :] = ddq
@@ -700,7 +720,7 @@ def muscles_driven(
         algebraic_states,
         numerical_timeseries,
         nlp,
-        with_contact: bool,
+        with_rigid_contact: bool,
         with_passive_torque: bool = False,
         with_ligament: bool = False,
         with_friction: bool = False,
@@ -726,7 +746,7 @@ def muscles_driven(
             The numerical timeseries of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_contact: bool
+        with_rigid_contact: bool
             If the dynamic with contact should be used
         with_passive_torque: bool
             If the dynamic with passive torque should be used
@@ -793,7 +813,7 @@ def muscles_driven(
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
 
         external_forces = nlp.get_external_forces(states, controls, algebraic_states, numerical_timeseries)
-        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact, external_forces)
+        ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_rigid_contact, external_forces)
         dxdt = nlp.cx(nlp.states.shape, ddq.shape[1])
         dxdt[nlp.states["q"].index, :] = horzcat(*[dq for _ in range(ddq.shape[1])])
         dxdt[nlp.states["qdot"].index, :] = ddq
@@ -970,7 +990,7 @@ def forward_dynamics(
         q: MX | SX,
         qdot: MX | SX,
         tau: MX | SX,
-        with_contact: bool,
+        with_rigid_contact: bool,
         external_forces: list = None,
     ):
         """
@@ -986,7 +1006,7 @@ def forward_dynamics(
             The value of qdot from "get"
         tau: MX | SX
             The value of tau from "get"
-        with_contact: bool
+        with_rigid_contact: bool
             If the dynamics with contact should be used
         external_forces: list[]
             The external forces
@@ -1003,7 +1023,7 @@ def forward_dynamics(
             qdot_var_mapping = BiMapping([i for i in range(qdot.shape[0])], [i for i in range(qdot.shape[0])]).to_first
 
         external_forces = [] if external_forces is None else external_forces
-        qddot = nlp.model.forward_dynamics(with_contact=with_contact)(
+        qddot = nlp.model.forward_dynamics(with_rigid_contact=with_rigid_contact)(
             q,
             qdot,
             tau,
@@ -1018,7 +1038,7 @@ def inverse_dynamics(
         q: MX | SX,
         qdot: MX | SX,
         qddot: MX | SX,
-        with_contact: bool,
+        with_rigid_contact: bool,
         external_forces: MX = None,
     ):
         """
@@ -1034,7 +1054,7 @@ def inverse_dynamics(
             The value of qdot from "get"
         qddot: MX | SX
             The value of qddot from "get"
-        with_contact: bool
+        with_rigid_contact: bool
             If the dynamics with contact should be used
         external_forces: MX
             The external forces
@@ -1044,9 +1064,9 @@ def inverse_dynamics(
         Torques in tau
         """
         if external_forces is None:
-            tau = nlp.model.inverse_dynamics(with_contact=with_contact)(q, qdot, qddot, [], nlp.parameters.cx)
+            tau = nlp.model.inverse_dynamics(with_rigid_contact=with_rigid_contact)(q, qdot, qddot, [], nlp.parameters.cx)
         else:
-            tau = nlp.model.inverse_dynamics(with_contact=with_contact)(
+            tau = nlp.model.inverse_dynamics(with_rigid_contact=with_rigid_contact)(
                 q, qdot, qddot, external_forces, nlp.parameters.cx
             )
         return tau  # We ignore on purpose the mapping to keep zeros in the defects of the dynamic.

bioptim/dynamics/ode_solvers.py

@@ -71,7 +71,7 @@ def defects_type(self) -> DefectType:
             return DefectType.NOT_APPLICABLE
 
         @property
-        def defect_type(self) -> DefectType:
+        def defects_type(self) -> DefectType:
             return DefectType.NOT_APPLICABLE
 
         @property