feat: test_entry_size; bound_checking biorbd_model

bioptim/dynamics/configure_problem.py

@@ -302,11 +302,9 @@ def torque_driven(
     def torque_driven_free_floating_base(
         ocp,
         nlp,
-        with_contact: bool = False,
         with_passive_torque: bool = False,
         with_ligament: bool = False,
         with_friction: bool = False,
-        external_forces: list = None,
     ):
         """
         Configure the dynamics for a torque driven program with a free floating base.
@@ -320,22 +318,14 @@ def torque_driven_free_floating_base(
             A reference to the ocp
         nlp: NonLinearProgram
             A reference to the phase
-        with_contact: bool
-            If the dynamic with contact should be used
         with_passive_torque: bool
             If the dynamic with passive torque should be used
         with_ligament: bool
             If the dynamic with ligament should be used
         with_friction: bool
             If the dynamic with joint friction should be used (friction = coefficients * qdot)
-        external_forces: list[Any]
-            A list of external forces
         """
 
-        _check_contacts_in_biorbd_model(with_contact, nlp.model.nb_contacts, nlp.phase_idx)
-        _check_external_forces_format(external_forces, nlp.ns, nlp.phase_idx)
-        _check_external_forces_and_phase_dynamics(external_forces, nlp.phase_dynamics, nlp.phase_idx)
-
         nb_q = nlp.model.nb_q
         nb_qdot = nlp.model.nb_qdot
         nb_root = nlp.model.nb_root
@@ -424,17 +414,9 @@ def torque_driven_free_floating_base(
                 ocp,
                 nlp,
                 DynamicsFunctions.torque_driven_free_floating_base,
-                with_contact=with_contact,
                 with_passive_torque=with_passive_torque,
                 with_ligament=with_ligament,
                 with_friction=with_friction,
-                external_forces=external_forces,
-            )
-
-        # Configure the contact forces
-        if with_contact:
-            ConfigureProblem.configure_contact_function(
-                ocp, nlp, DynamicsFunctions.forces_from_torque_driven, external_forces=external_forces
             )
 
     @staticmethod
@@ -1092,9 +1074,10 @@ def configure_soft_contact_function(ocp, nlp):
         """
         component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"]
 
+        q = nlp.states.mx_reduced[nlp.states["q"].index]
+        qdot = nlp.states.mx_reduced[nlp.states["qdot"].index]
         global_soft_contact_force_func = nlp.model.soft_contact_forces(
-            nlp.states.mx_reduced[nlp.states["q"].index],
-            nlp.states.mx_reduced[nlp.states["qdot"].index],
+            nlp.states["q"].mapping.to_second.map(q), nlp.states["qdot"].mapping.to_second.map(qdot)
         )
         nlp.soft_contact_forces_func = Function(
             "soft_contact_forces_func",

bioptim/dynamics/dynamics_functions.py

@@ -196,11 +196,9 @@ def torque_driven_free_floating_base(
         parameters: MX.sym,
         algebraic_states: MX.sym,
         nlp,
-        with_contact: bool,
         with_passive_torque: bool,
         with_ligament: bool,
         with_friction: bool,
-        external_forces: list = None,
     ) -> DynamicsEvaluation:
         """
         Forward dynamics driven by joint torques without actuation of the free floating base, optional external forces can be declared.
@@ -219,16 +217,12 @@ def torque_driven_free_floating_base(
             The algebraic states of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_contact: bool
-            If the dynamic with contact should be used
         with_passive_torque: bool
             If the dynamic with passive torque should be used
         with_ligament: bool
             If the dynamic with ligament should be used
         with_friction: bool
             If the dynamic with friction should be used
-        external_forces: list[Any]
-            The external forces
 
         Returns
         ----------
@@ -256,7 +250,7 @@ def torque_driven_free_floating_base(
 
         tau_full = vertcat(MX.zeros(nlp.model.nb_root), tau_joints)
 
-        ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact, external_forces)
+        ddq = DynamicsFunctions.forward_dynamics(nlp, q_full, qdot_full, tau_full, with_contact=False)
         dxdt = MX(n_q + n_qdot, ddq.shape[1])
         dxdt[:n_q, :] = horzcat(*[dq for _ in range(ddq.shape[1])])
         dxdt[n_q:, :] = ddq