diff --git a/README.md b/README.md index 7e4f03580..9835e299d 100644 --- a/README.md +++ b/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. diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py index c0ee7669d..743d2b071 100644 --- a/bioptim/dynamics/configure_problem.py +++ b/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, ) diff --git a/bioptim/dynamics/dynamics_functions.py b/bioptim/dynamics/dynamics_functions.py index 471b84d2f..0d3638108 100644 --- a/bioptim/dynamics/dynamics_functions.py +++ b/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) diff --git a/bioptim/examples/getting_started/example_external_forces.py b/bioptim/examples/getting_started/example_external_forces.py index 67a6927eb..34d5139ad 100644 --- a/bioptim/examples/getting_started/example_external_forces.py +++ b/bioptim/examples/getting_started/example_external_forces.py @@ -26,7 +26,6 @@ ConstraintList, ConstraintFcn, BoundsList, - InitialGuessList, OdeSolver, Solver, diff --git a/bioptim/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod b/bioptim/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod new file mode 100644 index 000000000..37e9b31d4 --- /dev/null +++ b/bioptim/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod @@ -0,0 +1,426 @@ +version 3 +gravity 0 -9.81 0 + +// SEGMENT DEFINITION + +segment base + meshfile mesh/ground_ribs.vtp + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + // Markers + marker target + parent base + position 0.15 0.15 0.17 + endmarker + marker r_acromion + parent base + position -0.01256 0.04 0.17 + endmarker + + +segment r_humerus_translation + parent base + RTinMatrix 1 + RT + 1.0 0.0 0.0 -0.017545 + 0.0 1.0 0.0 -0.007 + 0.0 0.0 1.0 0.17 + 0.0 0.0 0.0 1.0 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_humerus_rotation1 + parent r_humerus_translation + RTinMatrix 1 + RT + 0.9975010776109747 0.039020807762349584 -0.058898019716436364 0.0 + -0.038952964437603196 0.9992383982621832 0.0022999999889266845 0.0 + 0.05894291073968768 0.0 0.9982613551938856 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ -1 pi + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_humerus_rotation2 + parent r_humerus_rotation1 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_humerus_rotation3 + parent r_humerus_rotation2 + RTinMatrix 1 + RT + 0.0 -0.0588981755023151 0.9982639956056206 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.9982639956056206 0.0588981755023151 0.0 + 0.0 0.0 0.0 1.0 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_humerus + parent r_humerus_rotation3 + RTinMatrix 1 + RT + 0.039020807762349605 0.9992383982621836 0.0 0.0 + -0.11754676602826802 0.004590265714620227 0.9930567391931666 0.0 + 0.9923004254548464 -0.03874987611716229 0.11763635808301447 0.0 + 0.0 0.0 0.0 1.0 + mass 1.8645719999999999 + inertia + 0.01481 0.0 0.0 + 0.0 0.004551 0.0 + 0.0 0.0 0.013193 + com 0 -0.18049599999999999 0 + meshfile mesh/arm_r_humerus.vtp +endsegment + + // Markers + marker r_humerus_epicondyle + parent r_humerus + position 0.0050000000000000001 -0.29039999999999999 0.029999999999999999 + endmarker + marker COM_arm + parent r_humerus + position 0 -0.18049599999999999 0 + endmarker + + +segment r_ulna_radius_hand_translation + parent r_humerus + RTinMatrix 1 + RT + 1.0 0.0 0.0 0.0061 + 0.0 1.0 0.0 -0.2904 + 0.0 0.0 1.0 -0.0123 + 0.0 0.0 0.0 1.0 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_ulna_radius_hand_rotation1 + parent r_ulna_radius_hand_translation + RTinMatrix 1 + RT + 0.801979522152563 -0.5953053712684071 0.04940000998917986 0.0 + 0.5941792022021661 0.8034995425879125 0.036600009991983457 0.0 + -0.06148106796684942 3.469446951953614e-18 0.9981082497813831 0.0 + 0.0 0.0 0.0 1.0 + rotations z + rangesQ 0 pi + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_ulna_radius_hand_rotation2 + parent r_ulna_radius_hand_rotation1 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_ulna_radius_hand_rotation3 + parent r_ulna_radius_hand_rotation2 + RTinMatrix 1 + RT + 0.0 0.049433130424779516 0.998777435476196 0.0 + 1.0 0.0 0.0 0.0 + 0.0 0.998777435476196 -0.049433130424779516 0.0 + 0.0 0.0 0.0 1.0 + inertia + 0 0 0 + 0 0 0 + 0 0 0 +endsegment + + +segment r_ulna_radius_hand + parent r_ulna_radius_hand_rotation3 + RTinMatrix 1 + RT + -0.5953053712684069 0.803499542587912 0.0 0.0 + 0.08898397360606149 0.06592740211634747 0.9938487963928239 0.0 + 0.7985570533031812 0.5916435267212894 -0.11074551868375905 0.0 + 0.0 0.0 0.0 1.0 + mass 1.5343150000000001 + inertia + 0.019281 0.0 0.0 + 0.0 0.001571 0.0 + 0.0 0.0 0.020062 + com 0 -0.181479 0 + meshfile mesh/arm_r_ulna.vtp +endsegment + + // Markers + marker r_radius_styloid + parent r_ulna_radius_hand + position -0.0011000000000000001 -0.23558999999999999 0.094299999999999995 + endmarker + marker COM_hand + parent r_ulna_radius_hand + position 0 -0.181479 0 + endmarker + +ligament lig1 + Type linearspring + stiffness 2000 + origin r_humerus + Insertion r_ulna_radius_hand + OriginPosition 0 -0.28 0 + InsertionPosition -0.02 0 0.02 + ligamentslacklength 0.0174 + dampingfactor 0 + maxshorteningspeed 1 +endligament + +// MUSCLE DEFINITION + +// base > r_ulna_radius_hand +musclegroup base_to_r_ulna_radius_hand + OriginParent base + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlong + Type hilldegrootefatigable + statetype degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.053650000000000003 -0.013729999999999999 0.14723 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.13400000000000001 + maximalForce 798.51999999999998 + tendonSlackLength 0.14299999999999999 + pennationAngle 0.20943951 + maxVelocity 10 + fatigueParameters + Type Xia + fatiguerate 0.01 + recoveryrate 0.002 + developfactor 10 + recoveryfactor 10 + endfatigueparameters + endmuscle + + viapoint TRIlong-P2 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.027140000000000001 -0.11441 -0.0066400000000000001 + endviapoint + viapoint TRIlong-P3 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlong-P4 + parent r_humerus + muscle TRIlong + musclegroup base_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BIClong + Type degroote + statetype degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition -0.039234999999999999 0.00347 0.14795 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1157 + maximalForce 624.29999999999995 + tendonSlackLength 0.27229999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BIClong-P2 + parent base + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position -0.028944999999999999 0.01391 0.15639 + endviapoint + viapoint BIClong-P3 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.021309999999999999 0.017930000000000001 0.010279999999999999 + endviapoint + viapoint BIClong-P4 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.023779999999999999 -0.00511 0.01201 + endviapoint + viapoint BIClong-P5 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01345 -0.02827 0.0013600000000000001 + endviapoint + viapoint BIClong-P6 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01068 -0.077359999999999998 -0.00165 + endviapoint + viapoint BIClong-P7 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 0.00024000000000000001 + endviapoint + viapoint BIClong-P8 + parent r_humerus + muscle BIClong + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + + muscle BICshort + Type degroote + statetype degroote + musclegroup base_to_r_ulna_radius_hand + OriginPosition 0.0046750000000000003 -0.01231 0.13475000000000001 + InsertionPosition 0.0075100000000000002 -0.048390000000000002 0.02179 + optimalLength 0.1321 + maximalForce 435.56 + tendonSlackLength 0.1923 + pennationAngle 0 + maxVelocity 10 + endmuscle + + viapoint BICshort-P2 + parent base + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position -0.0070749999999999997 -0.040039999999999999 0.14507 + endviapoint + viapoint BICshort-P3 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.011169999999999999 -0.075759999999999994 -0.011010000000000001 + endviapoint + viapoint BICshort-P4 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.01703 -0.12125 -0.010789999999999999 + endviapoint + viapoint BICshort-P5 + parent r_humerus + muscle BICshort + musclegroup base_to_r_ulna_radius_hand + position 0.022800000000000001 -0.1754 -0.0063 + endviapoint + +// r_humerus > r_ulna_radius_hand +musclegroup r_humerus_to_r_ulna_radius_hand + OriginParent r_humerus + InsertionParent r_ulna_radius_hand +endmusclegroup + + muscle TRIlat + Type degroote + statetype degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0059899999999999997 -0.12645999999999999 0.00428 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.098000000000000004 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRIlat-P2 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.023439999999999999 -0.14527999999999999 0.0092800000000000001 + endviapoint + viapoint TRIlat-P3 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRIlat-P4 + parent r_humerus + muscle TRIlat + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle TRImed + Type degroote + statetype degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition -0.0083800000000000003 -0.13694999999999999 -0.0090600000000000003 + InsertionPosition -0.021899999999999999 0.010460000000000001 -0.00077999999999999999 + optimalLength 0.1138 + maximalForce 624.29999999999995 + tendonSlackLength 0.090800000000000006 + pennationAngle 0.15707963 + maxVelocity 10 + endmuscle + + viapoint TRImed-P2 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.026009999999999998 -0.15139 -0.010800000000000001 + endviapoint + viapoint TRImed-P3 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.03184 -0.22636999999999999 -0.01217 + endviapoint + viapoint TRImed-P4 + parent r_humerus + muscle TRImed + musclegroup r_humerus_to_r_ulna_radius_hand + position -0.017430000000000001 -0.26756999999999997 -0.01208 + endviapoint + + muscle BRA + Type degroote + statetype degroote + musclegroup r_humerus_to_r_ulna_radius_hand + OriginPosition 0.0067999999999999996 -0.1739 -0.0035999999999999999 + InsertionPosition -0.0032000000000000002 -0.023900000000000001 0.00089999999999999998 + optimalLength 0.085800000000000001 + maximalForce 987.25999999999999 + tendonSlackLength 0.053499999999999999 + pennationAngle 0 + maxVelocity 10 + endmuscle diff --git a/bioptim/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod b/bioptim/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod new file mode 100644 index 000000000..7e2cf824d --- /dev/null +++ b/bioptim/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod @@ -0,0 +1,61 @@ +version 3 + +// General informations + +//__________________________________________ +// Informations about Spring segment + // Segment + segment ground + parent base + RTinMatrix 0 + RT 0 0 0 xyz 0 0 0 + endsegment + + + +// Informations about Point segment + // Segment + segment Mass + RT 0 0 0 xyz 0 0 0 + translations z + mass 2 + inertia + 1 0 0 + 0 1 0 + 0 0 1 + com 0 0 0 + + endsegment + + // Markers + marker PointMarker + parent Mass + position 0 0 0 + endmarker + + + // Actuators Seg0 + actuator Mass + type Constant + dof TransZ + direction positive + Tmax 100.000000 + endactuator + actuator Mass + type Constant + dof TransZ + direction negative + Tmax 50.000000 + endactuator + +ligament lig1 + Type linearspring + stiffness 200 + origin ground + Insertion Mass + OriginPosition 0 0 0 + InsertionPosition 0 0 0 + ligamentslacklength 0 + dampingfactor 0 + maxshorteningspeed 1 +endligament diff --git a/bioptim/examples/torque_driven_ocp/models/pendulum_with_passive_torque.bioMod b/bioptim/examples/torque_driven_ocp/models/pendulum_with_passive_torque.bioMod index af56418ce..8c22b6750 100644 --- a/bioptim/examples/torque_driven_ocp/models/pendulum_with_passive_torque.bioMod +++ b/bioptim/examples/torque_driven_ocp/models/pendulum_with_passive_torque.bioMod @@ -28,17 +28,8 @@ endsegment endmarker passivetorque Seg1 - type exponential + type constant dof RotX - k1 -1.5 - k2 0 - b1 -10 - b2 0 - taueq 4 - wmax 16 - qmid 0 - deltap 0 - sv 1 - pbeta 8 + torque 1 endpassivetorque diff --git a/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py b/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py new file mode 100644 index 000000000..ca48b80e4 --- /dev/null +++ b/bioptim/examples/torque_driven_ocp/ocp_mass_with_ligament.py @@ -0,0 +1,113 @@ +""" +This is a simple example in which a mass is dropped and held by a ligament that plays the role of a spring without +damping, it uses the model mass_point_with_ligament.bioMod +""" + +from bioptim import ( + OptimalControlProgram, + DynamicsList, + ObjectiveList, + ObjectiveFcn, + BoundsList, + InitialGuessList, + OdeSolver, + BiorbdModel, + DynamicsFcn, + RigidBodyDynamics, +) + + +def prepare_ocp( + biorbd_model_path: str, + use_sx: bool = False, + ode_solver=OdeSolver.RK4(), + rigidbody_dynamics=RigidBodyDynamics.ODE, + with_ligament=False, +) -> OptimalControlProgram: + """ + Prepare the ocp + Parameters + ---------- + biorbd_model_path: str + The path to the bioMod + use_sx: bool + If the project should be build in mx [False] or sx [True] + ode_solver: OdeSolver + The type of integrator + Returns + ------- + The OptimalControlProgram ready to be solved + """ + # Model path + bio_model = BiorbdModel(biorbd_model_path) + + # ConfigureProblem parameters + number_shooting_points = 100 + final_time = 2 + tau_min, tau_max, tau_init = -100, 100, 0 + qddot_min, qddot_max, qddot_init = -1000, 1000, 0 + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10000000) + + # Dynamics + dynamics = DynamicsList() + dynamics.add(DynamicsFcn.TORQUE_DRIVEN, rigidbody_dynamics=rigidbody_dynamics, with_ligament=with_ligament) + + # Path constraint + x_bounds = bio_model.bounds_from_ranges(["q", "qdot"]) + x_bounds[0, 0] = 0 + x_bounds[1, 0] = 0 + # Define control path constraint + u_bounds = BoundsList() + u_init = InitialGuessList() + + if rigidbody_dynamics == RigidBodyDynamics.ODE: + u_bounds.add( + [tau_min] * bio_model.nb_tau, + [tau_max] * bio_model.nb_tau, + ) + u_init.add([tau_init] * bio_model.nb_tau) + elif ( + rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS + or rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS + ): + u_bounds.add( + [tau_min] * bio_model.nb_tau + [qddot_min] * bio_model.nb_qddot, + [tau_max] * bio_model.nb_tau + [qddot_max] * bio_model.nb_qddot, + ) + u_init.add([tau_init] * bio_model.nb_tau + [qddot_init] * bio_model.nb_qddot) + else: + raise NotImplementedError("other dynamics are not implemented yet") + # Initial guess + x_init = InitialGuessList() + x_init.add([0] * bio_model.nb_q + [0] * bio_model.nb_qdot) + + return OptimalControlProgram( + bio_model, + dynamics, + number_shooting_points, + final_time, + x_init, + u_init, + x_bounds, + u_bounds, + objective_functions, + ode_solver=ode_solver, + n_threads=8, + use_sx=use_sx, + ) + + +def main(): + model_path = "./models/mass_point_with_ligament.bioMod" + ocp = prepare_ocp(biorbd_model_path=model_path, with_ligament=True) + + # --- Solve the program --- # + sol = ocp.solve() + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py index 200ab587e..343d33259 100644 --- a/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py +++ b/bioptim/examples/torque_driven_ocp/pendulum_with_passive_torque.py @@ -10,7 +10,6 @@ OptimalControlProgram, DynamicsFcn, Dynamics, - Bounds, InitialGuess, ObjectiveFcn, Objective, @@ -19,6 +18,7 @@ Solver, BiorbdModel, RigidBodyDynamics, + Bounds, ) @@ -27,7 +27,7 @@ def prepare_ocp( final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), - rigidbody_dynamics=RigidBodyDynamics.ODE, + rigidbody_dynamics=RigidBodyDynamics.DAE_INVERSE_DYNAMICS, with_passive_torque=False, ) -> OptimalControlProgram: """ @@ -80,10 +80,28 @@ def prepare_ocp( # Define control path constraint n_tau = bio_model.nb_tau tau_min, tau_max, tau_init = -100, 100, 0 - u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) - u_bounds[1, :] = 0 # Prevent the model from actively rotate - - u_init = InitialGuess([tau_init] * n_tau) + qddot_min, qddot_max, qddot_init = -1000, 1000, 0 + + if rigidbody_dynamics == RigidBodyDynamics.ODE: + u_bounds = Bounds( + [tau_min] * bio_model.nb_tau, + [tau_max] * bio_model.nb_tau, + ) + u_init = InitialGuess([tau_init] * bio_model.nb_tau) + u_bounds[1, :] = 0 # Prevent the model from actively rotate + + elif ( + rigidbody_dynamics == RigidBodyDynamics.DAE_FORWARD_DYNAMICS + or rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS + ): + u_bounds = Bounds( + [tau_min] * bio_model.nb_tau + [qddot_min] * bio_model.nb_qddot, + [tau_max] * bio_model.nb_tau + [qddot_max] * bio_model.nb_qddot, + ) + u_init = InitialGuess([tau_init] * bio_model.nb_tau + [qddot_init] * bio_model.nb_qddot) + u_bounds[1, :] = 0 + else: + raise NotImplementedError("dynamic not implemented yet") return OptimalControlProgram( bio_model, diff --git a/bioptim/interfaces/biomodel.py b/bioptim/interfaces/biomodel.py index e8458b52b..a93ac3412 100644 --- a/bioptim/interfaces/biomodel.py +++ b/bioptim/interfaces/biomodel.py @@ -195,6 +195,9 @@ def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: def passive_joint_torque(self, q, qdot) -> MX: """Get the passive joint torque""" + def ligament_joint_torque(self, q, qdot) -> MX: + """Get the ligament joint torque""" + def bounds_from_ranges(self, variables: str | list[str, ...], mapping: BiMapping | BiMappingList = None) -> Bounds: """ Create bounds from ranges of the model depending on the variable chosen, such as q, qdot, qddot diff --git a/bioptim/interfaces/biorbd_model.py b/bioptim/interfaces/biorbd_model.py index 99b2ed7c1..7ee8ee819 100644 --- a/bioptim/interfaces/biorbd_model.py +++ b/bioptim/interfaces/biorbd_model.py @@ -303,6 +303,9 @@ def contact_forces(self, q, qdot, tau, external_forces: list = None) -> MX: def passive_joint_torque(self, q, qdot) -> MX: return self.model.passiveJointTorque(q, qdot).to_mx() + def ligament_joint_torque(self, q, qdot) -> MX: + return self.model.ligamentsJointTorque(q, qdot).to_mx() + def _q_mapping(self, mapping: BiMapping = None) -> BiMapping: if mapping is None: mapping = {} diff --git a/bioptim/limits/constraints.py b/bioptim/limits/constraints.py index cd4a1e95c..13e0a923a 100644 --- a/bioptim/limits/constraints.py +++ b/bioptim/limits/constraints.py @@ -314,7 +314,12 @@ def time_constraint(_: Constraint, all_pn: PenaltyNodeList, **unused_param): @staticmethod def qddot_equals_forward_dynamics( - _: Constraint, all_pn: PenaltyNodeList, with_contact: bool, with_passive_torque: bool, **unused_param + _: Constraint, + all_pn: PenaltyNodeList, + with_contact: bool, + with_passive_torque: bool, + with_ligament: bool, + **unused_param, ): """ Compute the difference between symbolic joint accelerations and forward dynamic results @@ -330,6 +335,8 @@ def qddot_equals_forward_dynamics( True if the contact dynamics is handled with_passive_torque: bool True if the passive torque dynamics is handled + with_ligament: bool + True if the ligament dynamics is handled **unused_param: dict Since the function does nothing, we can safely ignore any argument """ @@ -340,6 +347,7 @@ def qddot_equals_forward_dynamics( passive_torque = nlp.model.passive_joint_torque(q, qdot) tau = nlp.states["tau"].mx if "tau" in nlp.states else nlp.controls["tau"].mx tau = tau + passive_torque if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau qddot = nlp.controls["qddot"].mx if "qddot" in nlp.controls else nlp.states["qddot"].mx if with_contact: @@ -357,7 +365,12 @@ def qddot_equals_forward_dynamics( @staticmethod def tau_equals_inverse_dynamics( - _: Constraint, all_pn: PenaltyNodeList, with_contact: bool, with_passive_torque: bool, **unused_param + _: Constraint, + all_pn: PenaltyNodeList, + with_contact: bool, + with_passive_torque: bool, + with_ligament: bool, + **unused_param, ): """ Compute the difference between symbolic joint torques and inverse dynamic results @@ -373,6 +386,8 @@ def tau_equals_inverse_dynamics( True if the contact dynamics is handled with_passive_torque: bool True if the passive torque dynamics is handled + with_ligament: bool + True if the ligament dynamics is handled **unused_param: dict Since the function does nothing, we can safely ignore any argument """ @@ -384,6 +399,7 @@ def tau_equals_inverse_dynamics( qddot = nlp.states["qddot"].mx if "qddot" in nlp.states else nlp.controls["qddot"].mx passive_torque = nlp.model.passive_joint_torque(q, qdot) tau = tau + passive_torque if with_passive_torque else tau + tau = tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else tau if nlp.external_forces: raise NotImplementedError( @@ -441,7 +457,7 @@ def implicit_marker_acceleration(_: Constraint, all_pn: PenaltyNodeList, contact @staticmethod def tau_from_muscle_equal_inverse_dynamics( - _: Constraint, all_pn: PenaltyNodeList, with_passive_torque: bool, **unused_param + _: Constraint, all_pn: PenaltyNodeList, with_passive_torque: bool, with_ligament: bool, **unused_param ): """ Compute the difference between symbolic joint torques from muscle and inverse dynamic results @@ -455,6 +471,8 @@ def tau_from_muscle_equal_inverse_dynamics( The penalty node elements with_passive_torque: bool True if the passive torque dynamics is handled + with_ligament: bool + True if the ligament dynamics is handled **unused_param: dict Since the function does nothing, we can safely ignore any argument """ @@ -469,6 +487,7 @@ def tau_from_muscle_equal_inverse_dynamics( muscles_states[k].setActivation(muscle_activations[k]) muscle_tau = nlp.model.muscle_joint_torque(muscles_states, q, qdot) muscle_tau = muscle_tau + passive_torque if with_passive_torque else muscle_tau + muscle_tau = muscle_tau + nlp.model.ligament_joint_torque(q, qdot) if with_ligament else muscle_tau qddot = nlp.states["qddot"].mx if "qddot" in nlp.states else nlp.controls["qddot"].mx if nlp.external_forces: diff --git a/tests/test_global_getting_started.py b/tests/test_global_getting_started.py index 26600b88c..c5657df4f 100644 --- a/tests/test_global_getting_started.py +++ b/tests/test_global_getting_started.py @@ -5,6 +5,7 @@ import pickle import sys import re +import sys import pytest import numpy as np @@ -842,6 +843,10 @@ def test_custom_problem_type_and_dynamics(problem_type_custom, ode_solver): @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.RK8, OdeSolver.IRK]) def test_example_external_forces(ode_solver): + if sys.platform == "win32" and (ode_solver == OdeSolver.RK8 or ode_solver == OdeSolver.IRK): + # This test does not work on Windows for the CI + return + from bioptim.examples.getting_started import example_external_forces as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) @@ -1117,6 +1122,9 @@ def test_example_multiphase(ode_solver): @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.IRK]) def test_contact_forces_inequality_greater_than_constraint(ode_solver): + if sys.platform == "win32" and ode_solver == OdeSolver.IRK: + return + from bioptim.examples.getting_started import example_inequality_constraint as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) @@ -1227,6 +1235,10 @@ def test_contact_forces_inequality_lesser_than_constraint(ode_solver): @pytest.mark.parametrize("ode_solver", [OdeSolver.RK4, OdeSolver.RK8, OdeSolver.IRK]) def test_multinode_constraints(ode_solver): + if sys.platform == "win32" and (ode_solver == OdeSolver.RK8 or ode_solver == OdeSolver.IRK): + # This test does not work on Windows for the CI + return + from bioptim.examples.getting_started import example_multinode_constraints as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) diff --git a/tests/test_global_torque_driven_with_contact_ocp.py b/tests/test_global_torque_driven_with_contact_ocp.py index f21402e52..a97f7503e 100644 --- a/tests/test_global_torque_driven_with_contact_ocp.py +++ b/tests/test_global_torque_driven_with_contact_ocp.py @@ -24,7 +24,8 @@ def test_maximize_predicted_height_CoM(ode_solver, objective_name, com_constrain from bioptim.examples.torque_driven_ocp import maximize_predicted_height_CoM as ocp_module # no rk8 on windows - if sys.platform == "win32" and ode_solver == OdeSolver.RK8: # it actually works but not with the CI + if sys.platform == "win32" and (ode_solver == OdeSolver.RK8 or ode_solver == OdeSolver.IRK): + # This test does not pass on the CI return bioptim_folder = os.path.dirname(ocp_module.__file__) diff --git a/tests/test_ligaments.py b/tests/test_ligaments.py new file mode 100644 index 000000000..dcfbcc365 --- /dev/null +++ b/tests/test_ligaments.py @@ -0,0 +1,371 @@ +import pytest + +import numpy as np +from casadi import MX, SX +from bioptim import ( + ConfigureProblem, + ControlType, + RigidBodyDynamics, + BiorbdModel, + NonLinearProgram, + DynamicsFcn, + Dynamics, + ConstraintList, + Solver, +) +from bioptim.optimization.optimization_vector import OptimizationVector +from .utils import TestUtils +import os + + +class OptimalControlProgram: + def __init__(self, nlp): + self.n_phases = 1 + self.nlp = [nlp] + self.v = OptimizationVector(self) + self.implicit_constraints = ConstraintList() + + +@pytest.mark.parametrize("cx", [MX, SX]) +@pytest.mark.parametrize( + "with_ligament", + [ + False, + True, + ], +) +def test_torque_driven_with_ligament(with_ligament, cx): + # Prepare the program + nlp = NonLinearProgram() + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" + ) + nlp.ns = 5 + nlp.cx = cx + nlp.x_scaling = {} + nlp.xdot_scaling = {} + nlp.u_scaling = {} + + nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + ocp = OptimalControlProgram(nlp) + nlp.control_type = ControlType.CONSTANT + NonLinearProgram.add( + ocp, + "dynamics_type", + Dynamics(DynamicsFcn.TORQUE_DRIVEN, rigidbody_dynamics=RigidBodyDynamics.ODE, with_ligament=with_ligament), + False, + ) + phase_index = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "phase_idx", phase_index, False) + use_states_from_phase_idx = [i for i in range(ocp.n_phases)] + use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] + use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) + + np.random.seed(42) + + # Prepare the dynamics + ConfigureProblem.initialize(ocp, nlp) + + # Test the results + states = np.random.rand(nlp.states.shape, nlp.ns) + controls = np.random.rand(nlp.controls.shape, nlp.ns) + params = np.random.rand(nlp.parameters.shape, nlp.ns) + x_out = np.array(nlp.dynamics_func(states, controls, params)) + if with_ligament: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.1559945, -47.2537196], + ) + else: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.1559945, -9.7997078], + ) + + +@pytest.mark.parametrize("cx", [MX, SX]) +@pytest.mark.parametrize("with_ligament", [False, True]) +def test_torque_derivative_driven_with_ligament(with_ligament, cx): + # Prepare the program + nlp = NonLinearProgram() + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" + ) + nlp.ns = 5 + nlp.cx = cx + nlp.x_scaling = {} + nlp.xdot_scaling = {} + nlp.u_scaling = {} + + nlp.x_bounds = np.zeros((nlp.model.nb_q * 3, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + ocp = OptimalControlProgram(nlp) + nlp.control_type = ControlType.CONSTANT + + NonLinearProgram.add( + ocp, + "dynamics_type", + Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN, with_ligament=with_ligament), + False, + ) + + phase_index = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "phase_idx", phase_index, False) + use_states_from_phase_idx = [i for i in range(ocp.n_phases)] + use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] + use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) + + np.random.seed(42) + + # Prepare the dynamics + ConfigureProblem.initialize(ocp, nlp) + + # Test the results + states = np.random.rand(nlp.states.shape, nlp.ns) + controls = np.random.rand(nlp.controls.shape, nlp.ns) + params = np.random.rand(nlp.parameters.shape, nlp.ns) + x_out = np.array(nlp.dynamics_func(states, controls, params)) + if with_ligament: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.1559945, -47.2537196, 0.1834045], + ) + else: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.1559945, -9.7997078, 0.1834045], + ) + + +@pytest.mark.parametrize("cx", [MX, SX]) +@pytest.mark.parametrize("with_ligament", [False, True]) +def test_torque_activation_driven_with_ligament(with_ligament, cx): + # Prepare the program + nlp = NonLinearProgram() + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/torque_driven_ocp/models/mass_point_with_ligament.bioMod" + ) + nlp.ns = 5 + nlp.cx = cx + nlp.x_scaling = {} + nlp.xdot_scaling = {} + nlp.u_scaling = {} + nlp.x_bounds = np.zeros((nlp.model.nb_q * 2, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_q, 1)) + ocp = OptimalControlProgram(nlp) + nlp.control_type = ControlType.CONSTANT + NonLinearProgram.add( + ocp, + "dynamics_type", + Dynamics(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_ligament=with_ligament), + False, + ) + phase_index = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "phase_idx", phase_index, False) + use_states_from_phase_idx = [i for i in range(ocp.n_phases)] + use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] + use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) + + np.random.seed(42) + # Prepare the dynamics + ConfigureProblem.initialize(ocp, nlp) + + # Test the results + states = np.random.rand(nlp.states.shape, nlp.ns) + controls = np.random.rand(nlp.controls.shape, nlp.ns) + params = np.random.rand(nlp.parameters.shape, nlp.ns) + x_out = np.array(nlp.dynamics_func(states, controls, params)) + if with_ligament: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.155995, -46.234787], + decimal=6, + ) + else: + np.testing.assert_almost_equal( + x_out[:, 0], + [0.15599, -8.78078], + decimal=5, + ) + + +@pytest.mark.parametrize("cx", [MX, SX]) +@pytest.mark.parametrize("with_ligament", [False, True]) +def test_muscle_driven_with_ligament(with_ligament, cx): + # Prepare the program + nlp = NonLinearProgram() + nlp.model = BiorbdModel( + TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/models/arm26_with_ligament.bioMod" + ) + nlp.ns = 5 + nlp.cx = cx + nlp.x_scaling = {} + nlp.xdot_scaling = {} + nlp.u_scaling = {} + nlp.x_bounds = np.zeros((nlp.model.nb_q * 2 + nlp.model.nb_muscles, 1)) + nlp.u_bounds = np.zeros((nlp.model.nb_muscles, 1)) + + ocp = OptimalControlProgram(nlp) + nlp.control_type = ControlType.CONSTANT + NonLinearProgram.add( + ocp, + "dynamics_type", + Dynamics( + DynamicsFcn.MUSCLE_DRIVEN, + rigidbody_dynamics=RigidBodyDynamics.ODE, + with_ligament=with_ligament, + ), + False, + ) + phase_index = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "phase_idx", phase_index, False) + use_states_from_phase_idx = [i for i in range(ocp.n_phases)] + use_states_dot_from_phase_idx = [i for i in range(ocp.n_phases)] + use_controls_from_phase_idx = [i for i in range(ocp.n_phases)] + NonLinearProgram.add(ocp, "use_states_from_phase_idx", use_states_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_states_dot_from_phase_idx", use_states_dot_from_phase_idx, False) + NonLinearProgram.add(ocp, "use_controls_from_phase_idx", use_controls_from_phase_idx, False) + + np.random.seed(42) + + # Prepare the dynamics + ConfigureProblem.initialize(ocp, nlp) + + # Test the results + states = np.random.rand(nlp.states.shape, nlp.ns) + controls = np.random.rand(nlp.controls.shape, nlp.ns) + params = np.random.rand(nlp.parameters.shape, nlp.ns) + x_out = np.array(nlp.dynamics_func(states, controls, params)) + + if with_ligament: + np.testing.assert_almost_equal( + x_out[:, 0], + [2.0584494e-02, 1.8340451e-01, -6.0300944e00, -9.4582028e01], + decimal=6, + ) + else: + np.testing.assert_almost_equal( + x_out[:, 0], + [2.0584494e-02, 1.8340451e-01, -7.3880194e00, -9.0642142e01], + decimal=6, + ) + + +@pytest.mark.parametrize( + "rigidbody_dynamics", + [ + RigidBodyDynamics.DAE_FORWARD_DYNAMICS, + RigidBodyDynamics.DAE_INVERSE_DYNAMICS, + ], +) +@pytest.mark.parametrize( + "with_ligament", + [ + False, + True, + ], +) +def test_ocp_mass_ligament(rigidbody_dynamics, with_ligament): + from bioptim.examples.torque_driven_ocp import ocp_mass_with_ligament as ocp_module + + bioptim_folder = os.path.dirname(ocp_module.__file__) + + # Define the problem + biorbd_model_path = bioptim_folder + "/models/mass_point_with_ligament.bioMod" + + ocp = ocp_module.prepare_ocp( + biorbd_model_path, + rigidbody_dynamics=rigidbody_dynamics, + with_ligament=with_ligament, + ) + solver = Solver.IPOPT() + + # solver.set_maximum_iterations(10) + sol = ocp.solve(solver) + + # Check some results + q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] + + if rigidbody_dynamics == RigidBodyDynamics.DAE_INVERSE_DYNAMICS: + if with_ligament: + # initial and final position + np.testing.assert_almost_equal(q[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(q[:, -1], np.array([0.0194773])) + # initial and final velocities + np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(qdot[:, -1], np.array([-2.3061592])) + # initial and final controls + np.testing.assert_almost_equal( + tau[:, 0], + np.array([2.158472e-16]), + decimal=6, + ) + np.testing.assert_almost_equal(tau[:, -2], np.array([1.423733e-17]), decimal=6) + + else: + # initial and final position + np.testing.assert_almost_equal(q[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(q[:, -1], np.array([-3.1415927])) + # initial and final velocities + np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(qdot[:, -1], np.array([-7.2608855])) + # initial and final controls + np.testing.assert_almost_equal( + tau[:, 0], + np.array([24.594638]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([0.123591]), + decimal=6, + ) + + else: + if with_ligament: + # initial and final position + np.testing.assert_almost_equal(q[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(q[:, -1], np.array([0.0194773])) + # initial and final velocities + np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(qdot[:, -1], np.array([-2.3061592])) + # initial and final controls + np.testing.assert_almost_equal( + tau[:, 0], + np.array([2.158472e-16]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([1.423733e-17]), + decimal=6, + ) + + else: + # initial and final position + np.testing.assert_almost_equal(q[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(q[:, -1], np.array([-3.1415927])) + # initial and final velocities + np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0])) + np.testing.assert_almost_equal(qdot[:, -1], np.array([-7.2608855])) + # initial and final controls + np.testing.assert_almost_equal( + tau[:, 0], + np.array([24.594638]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([0.123591]), + decimal=6, + ) diff --git a/tests/test_passive_torque.py b/tests/test_passive_torque.py index 980d4a8fb..8f7b004c8 100644 --- a/tests/test_passive_torque.py +++ b/tests/test_passive_torque.py @@ -409,14 +409,14 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque): # Define the problem biorbd_model_path = bioptim_folder + "/models/pendulum_with_passive_torque.bioMod" - final_time = 0.1 - n_shooting = 5 + final_time = 1 + n_shooting = 30 ocp = ocp_module.prepare_ocp( biorbd_model_path, final_time, n_shooting, - rigidbody_dynamics=RigidBodyDynamics.ODE, + rigidbody_dynamics=rigidbody_dynamics, with_passive_torque=with_passive_torque, ) solver = Solver.IPOPT() @@ -436,8 +436,12 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque): np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array([37.2828933, 0.0]), decimal=6) - np.testing.assert_almost_equal(tau[:, -2], np.array([-4.9490898, 0.0]), decimal=6) + np.testing.assert_almost_equal( + tau[:, 0], + np.array([-1.071535, 0.0]), + decimal=6, + ) + np.testing.assert_almost_equal(tau[:, -2], np.array([-19.422394, 0.0]), decimal=6) else: # initial and final position @@ -447,8 +451,16 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque): np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array([-70.3481693, 0.0]), decimal=6) - np.testing.assert_almost_equal(tau[:, -2], np.array([-35.5389502, 0.0]), decimal=6) + np.testing.assert_almost_equal( + tau[:, 0], + np.array([2.531529, 0.0]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([-18.254416, 0.0]), + decimal=6, + ) else: if with_passive_torque: @@ -459,8 +471,16 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque): np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array([37.2828933, 0.0]), decimal=6) - np.testing.assert_almost_equal(tau[:, -2], np.array([-4.9490898, 0.0]), decimal=6) + np.testing.assert_almost_equal( + tau[:, 0], + np.array([1.587319, 0.0]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([-39.19793, 0.0]), + decimal=6, + ) else: # initial and final position @@ -470,5 +490,13 @@ def test_pendulum_passive_torque(rigidbody_dynamics, with_passive_torque): np.testing.assert_almost_equal(qdot[:, 0], np.array([0.0, 0.0])) np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0, 0.0])) # initial and final controls - np.testing.assert_almost_equal(tau[:, 0], np.array([-70.3481693, 0.0]), decimal=6) - np.testing.assert_almost_equal(tau[:, -2], np.array([-35.5389502, 0.0]), decimal=6) + np.testing.assert_almost_equal( + tau[:, 0], + np.array([2.606971, 0.0]), + decimal=6, + ) + np.testing.assert_almost_equal( + tau[:, -2], + np.array([-24.611219, 0.0]), + decimal=6, + )