Merge pull request #626 from AntoineLeroy/PR_passive_torque

Fix passive torque example

README.md

@@ -786,6 +786,9 @@ The derivative of *qdot* is given by the `biorbd` function that includes non-acc
 ##### with_passive_torque = True
 The passive torque is taken into account in the *tau* 
 
+##### with_ligament = True
+The tau generated by the ligament is taken into account in the *tau* 
+
 #### TORQUE_DERIVATIVE_DRIVEN
 The torque derivative driven defines the states (x) as *q*, *qdot*, *tau* and the controls (u) as *taudot*. 
 The derivative of *q* is trivially *qdot*.
@@ -799,6 +802,9 @@ The derivative of *qdot* is given by the `biorbd` function that includes non-acc
 ##### with_passive_torque = True
 The passive torque is taken into account in the *tau* 
 
+##### with_ligament = True
+The tau generated by the ligament is taken into account in the *tau* 
+
 #### TORQUE_ACTIVATIONS_DRIVEN
 The torque driven defines the states (x) as *q* and *qdot* and the controls (u) as the level of activation of *tau*. 
 The derivative of *q* is trivially *qdot*.
@@ -818,6 +824,9 @@ The passive torque is taken into account in the *tau*
 ##### with_residual_torque = True
 The residual torque is taken into account in the *tau*
 
+##### with_ligament = True
+The tau generated by the ligament is taken into account in the *tau* 
+
 #### JOINTS_ACCELERATION_DRIVEN
 The joints acceleration driven defines the states (x) as *q* and *qdot* and the controls (u) as *qddot_joints*. The derivative of *q* is trivially *qdot*.
 The joints' acceleration *qddot_joints* is the acceleration of the actual joints of the `biorb_model` without its root's joints.
@@ -847,6 +856,9 @@ The derivative of *a* is computed by the `biorbd` function: `adot = model.activa
 ##### with_passive_torque = True
 The passive torque is taken into account in the *tau*
 
+##### with_ligament = True
+The tau generated by the ligament is taken into account in the *tau* 
+
 #### CUSTOM
 This leaves the user to define both the configuration (what are the states and controls) and to define the dynamic function. 
 CUSTOM should not be called by the user, but the user should pass the configure_function directly. 

bioptim/dynamics/configure_problem.py

@@ -156,6 +156,7 @@ def torque_driven(
         nlp,
         with_contact: bool = False,
         with_passive_torque: bool = False,
+        with_ligament: bool = False,
         rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
         soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE,
         fatigue: FatigueList = None,
@@ -171,8 +172,10 @@ def torque_driven(
             A reference to the phase
         with_contact: bool
             If the dynamic with contact should be used
-        with_passive_torque : bool
+        with_passive_torque: bool
             If the dynamic with passive torque should be used
+        with_ligament: bool
+            If the dynamic with ligament should be used
         rigidbody_dynamics: RigidBodyDynamics
             which rigidbody dynamics should be used
         soft_contacts_dynamics: SoftContactDynamics
@@ -231,6 +234,7 @@ def torque_driven(
                 phase=nlp.phase_idx,
                 with_contact=with_contact,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
             )
             if with_contact:
                 # qddot is continuous with RigidBodyDynamics.DAE_INVERSE_DYNAMICS_JERK
@@ -259,6 +263,7 @@ def torque_driven(
                 with_contact=with_contact,
                 phase=nlp.phase_idx,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
             )
 
         # Declared soft contacts controls
@@ -277,6 +282,7 @@ def torque_driven(
                 fatigue=fatigue,
                 rigidbody_dynamics=rigidbody_dynamics,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
             )
 
         # Configure the contact forces
@@ -299,6 +305,7 @@ def torque_derivative_driven(
         nlp,
         with_contact=False,
         with_passive_torque: bool = False,
+        with_ligament: bool = False,
         rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
         soft_contacts_dynamics: SoftContactDynamics = SoftContactDynamics.ODE,
     ):
@@ -315,6 +322,8 @@ def torque_derivative_driven(
             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
         rigidbody_dynamics: RigidBodyDynamics
             which rigidbody dynamics should be used
         soft_contacts_dynamics: SoftContactDynamics
@@ -371,6 +380,7 @@ def torque_derivative_driven(
                 with_contact=with_contact,
                 rigidbody_dynamics=rigidbody_dynamics,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
             )
 
         if with_contact:
@@ -387,7 +397,12 @@ def torque_derivative_driven(
 
     @staticmethod
     def torque_activations_driven(
-        ocp, nlp, with_contact: bool = False, with_passive_torque: bool = False, with_residual_torque: bool = False
+        ocp,
+        nlp,
+        with_contact: bool = False,
+        with_passive_torque: bool = False,
+        with_residual_torque: bool = False,
+        with_ligament: bool = False,
     ):
         """
         Configure the dynamics for a torque driven program (states are q and qdot, controls are tau activations).
@@ -406,6 +421,8 @@ def torque_activations_driven(
             If the dynamic with passive torque should be used
         with_residual_torque: bool
             If the dynamic with a residual torque should be used
+        with_ligament: bool
+            If the dynamic with ligament should be used
 
         """
 
@@ -429,6 +446,7 @@ def torque_activations_driven(
                 with_contact=with_contact,
                 with_passive_torque=with_passive_torque,
                 with_residual_torque=with_residual_torque,
+                with_ligament=with_ligament,
             )
 
         if with_contact:
@@ -489,6 +507,7 @@ def muscle_driven(
         with_residual_torque: bool = False,
         with_contact: bool = False,
         with_passive_torque: bool = False,
+        with_ligament: bool = False,
         rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
     ):
         """
@@ -514,6 +533,8 @@ def muscle_driven(
             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
         rigidbody_dynamics: RigidBodyDynamics
             which rigidbody dynamics should be used
 
@@ -543,6 +564,7 @@ def muscle_driven(
                 penalty_type=ConstraintType.IMPLICIT,
                 phase=nlp.phase_idx,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
             )
 
         if nlp.dynamics_type.dynamic_function:
@@ -556,6 +578,7 @@ def muscle_driven(
                 fatigue=fatigue,
                 with_residual_torque=with_residual_torque,
                 with_passive_torque=with_passive_torque,
+                with_ligament=with_ligament,
                 rigidbody_dynamics=rigidbody_dynamics,
             )
 

bioptim/dynamics/dynamics_functions.py

@@ -75,6 +75,7 @@ def torque_driven(
         nlp,
         with_contact: bool,
         with_passive_torque: bool,
+        with_ligament: bool,
         rigidbody_dynamics: RigidBodyDynamics,
         fatigue: FatigueList,
     ) -> DynamicsEvaluation:
@@ -93,8 +94,10 @@ def torque_driven(
             The definition of the system
         with_contact: bool
             If the dynamic with contact should be used
-        with_passive_torque : bool
+        with_passive_torque: bool
             If the dynamic with passive torque should be used
+        with_ligament: bool
+            If the dynamic with ligament should be used
         rigidbody_dynamics: RigidBodyDynamics
             which rigidbody dynamics should be used
         fatigue : FatigueList
@@ -113,6 +116,7 @@ def torque_driven(
 
         tau = DynamicsFunctions.__get_fatigable_tau(nlp, states, controls, fatigue)
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         if (
             rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS
@@ -227,6 +231,7 @@ def torque_activations_driven(
         with_contact: bool,
         with_passive_torque: bool,
         with_residual_torque: bool,
+        with_ligament: bool,
     ):
         """
         Forward dynamics driven by joint torques activations.
@@ -247,7 +252,8 @@ def torque_activations_driven(
             If the dynamic with passive torque should be used
         with_residual_torque: bool
             If the dynamic should be added with residual torques
-
+        with_ligament: bool
+            If the dynamic with ligament should be used
 
         Returns
         ----------
@@ -259,11 +265,14 @@ def torque_activations_driven(
         qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
         tau_activation = DynamicsFunctions.get(nlp.controls["tau"], controls)
 
-        if with_residual_torque:
-            tau_residual = DynamicsFunctions.get(nlp.controls["residual_tau"], controls)
         tau = nlp.model.torque(tau_activation, q, qdot)
-        tau = (tau + tau_residual) if with_residual_torque else tau
-        tau = (tau + nlp.model.passive_joint_torque(q, qdot)) if with_passive_torque else tau
+        if with_passive_torque:
+            tau += nlp.model.passive_joint_torque(q, qdot)
+        if with_residual_torque:
+            tau += DynamicsFunctions.get(nlp.controls["residual_tau"], controls)
+        if with_ligament:
+            tau += nlp.model.ligament_joint_torque(q, qdot)
+
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
         ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact)
 
@@ -280,6 +289,7 @@ def torque_derivative_driven(
         rigidbody_dynamics: RigidBodyDynamics,
         with_contact: bool,
         with_passive_torque: bool,
+        with_ligament: bool,
     ) -> DynamicsEvaluation:
         """
         Forward dynamics driven by joint torques, optional external forces can be declared.
@@ -300,6 +310,8 @@ def torque_derivative_driven(
             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
 
         Returns
         ----------
@@ -312,6 +324,7 @@ def torque_derivative_driven(
 
         tau = DynamicsFunctions.get(nlp.states["tau"], states)
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
         dtau = DynamicsFunctions.get(nlp.controls["taudot"], controls)
@@ -339,7 +352,12 @@ def torque_derivative_driven(
 
     @staticmethod
     def forces_from_torque_driven(
-        states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_passive_torque: bool = False
+        states: MX.sym,
+        controls: MX.sym,
+        parameters: MX.sym,
+        nlp,
+        with_passive_torque: bool = False,
+        with_ligament: bool = False,
     ) -> MX:
         """
         Contact forces of a forward dynamics driven by joint torques with contact constraints.
@@ -354,8 +372,10 @@ def forces_from_torque_driven(
             The parameters of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_passive_torque : bool
+        with_passive_torque: bool
             If the dynamic with passive torque should be used
+        with_ligament: bool
+            If the dynamic with ligament should be used
 
         Returns
         ----------
@@ -371,12 +391,18 @@ def forces_from_torque_driven(
         qdot = DynamicsFunctions.get(qdot_nlp, qdot_var)
         tau = DynamicsFunctions.get(tau_nlp, tau_var)
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         return nlp.model.contact_forces(q, qdot, tau, nlp.external_forces)
 
     @staticmethod
     def forces_from_torque_activation_driven(
-        states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_passive_torque: bool = False
+        states: MX.sym,
+        controls: MX.sym,
+        parameters: MX.sym,
+        nlp,
+        with_passive_torque: bool = False,
+        with_ligament: bool = False,
     ) -> MX:
         """
         Contact forces of a forward dynamics driven by joint torques with contact constraints.
@@ -391,8 +417,10 @@ def forces_from_torque_activation_driven(
             The parameters of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_passive_torque : bool
+        with_passive_torque: bool
             If the dynamic with passive torque should be used
+        with_ligament: bool
+            If the dynamic with ligament should be used
 
         Returns
         ----------
@@ -408,6 +436,7 @@ def forces_from_torque_activation_driven(
         tau_activations = DynamicsFunctions.get(tau_nlp, tau_var)
         tau = nlp.model.torque(tau_activations, q, qdot)
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         return nlp.model.contact_forces(q, qdot, tau, nlp.external_forces)
 
@@ -419,6 +448,7 @@ def muscles_driven(
         nlp,
         with_contact: bool,
         with_passive_torque: bool = False,
+        with_ligament: bool = False,
         rigidbody_dynamics: RigidBodyDynamics = RigidBodyDynamics.ODE,
         with_residual_torque: bool = False,
         fatigue=None,
@@ -440,6 +470,8 @@ def muscles_driven(
             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
         rigidbody_dynamics: RigidBodyDynamics
             which rigidbody dynamics should be used
         fatigue: FatigueDynamicsList
@@ -497,6 +529,7 @@ def muscles_driven(
 
         tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
 
@@ -542,7 +575,12 @@ def muscles_driven(
 
     @staticmethod
     def forces_from_muscle_driven(
-        states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp, with_passive_torque: bool = False
+        states: MX.sym,
+        controls: MX.sym,
+        parameters: MX.sym,
+        nlp,
+        with_passive_torque: bool = False,
+        with_ligament: bool = False,
     ) -> MX:
         """
         Contact forces of a forward dynamics driven by muscles activations and joint torques with contact constraints.
@@ -557,9 +595,10 @@ def forces_from_muscle_driven(
             The parameters of the system
         nlp: NonLinearProgram
             The definition of the system
-        with_passive_torque : bool
+        with_passive_torque: bool
             If the dynamic with passive torque should be used
-
+        with_ligament: bool
+            If the dynamic with ligament should be used
         Returns
         ----------
         MX.sym
@@ -576,6 +615,7 @@ def forces_from_muscle_driven(
 
         tau = muscles_tau + residual_tau if residual_tau is not None else muscles_tau
         tau = tau + nlp.model.passive_joint_torque(q, qdot) if with_passive_torque else tau
+        tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau
 
         return nlp.model.contact_forces(q, qdot, tau, nlp.external_forces)
 

bioptim/examples/getting_started/example_external_forces.py

@@ -26,7 +26,6 @@
     ConstraintList,
     ConstraintFcn,
     BoundsList,
-
     InitialGuessList,
     OdeSolver,
     Solver,