diff --git a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py index 540376b2c..0d437f19e 100644 --- a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py +++ b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py @@ -55,8 +55,8 @@ def dynamics( defects = None if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): - slope_q = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) - slope_qdot = DynamicsFunctions.get(nlp.states_dot["qddot_u"], nlp.states_dot.scaled.cx) + slope_q = DynamicsFunctions.get(nlp.states_dot["q_u"], nlp.states_dot.scaled.cx) + slope_qdot = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) if nlp.dynamics_type.ode_solver.defects_type == DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS: qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau) derivative = vertcat(qdot_u, qddot_u) diff --git a/bioptim/examples/models/3bar.bioMod b/bioptim/examples/models/3bar.bioMod new file mode 100644 index 000000000..4f744c4f9 --- /dev/null +++ b/bioptim/examples/models/3bar.bioMod @@ -0,0 +1,98 @@ +version 4 + +segment Seg0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg1 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker CoM1 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg2 + RT 0 0 0 xyz 0 1 0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 5 + marker marker_5 + parent Seg2 + position 0 0 0 + endmarker + + // Marker 6 + marker marker_6 + parent Seg2 + position 0 0 -1 + endmarker + + // Marker CoM3 + marker CoM3 + parent Seg2 + position -0.0005 0.0688 -0.9542 + endmarker \ No newline at end of file diff --git a/bioptim/examples/models/4bar.bioMod b/bioptim/examples/models/4bar.bioMod new file mode 100644 index 000000000..541daf8f3 --- /dev/null +++ b/bioptim/examples/models/4bar.bioMod @@ -0,0 +1,132 @@ +version 4 + +segment Seg0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg1 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker CoM1 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg2 + RT 0 0 0 xyz 0 1 0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 5 + marker marker_5 + parent Seg2 + position 0 0 0 + endmarker + + // Marker 6 + marker marker_6 + parent Seg2 + position 0 0 -1 + endmarker + + // Marker CoM3 + marker CoM3 + parent Seg2 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg3 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 7 + marker marker_7 + parent Seg3 + position 0 0 0 + endmarker + + // Marker 8 + marker marker_8 + parent Seg3 + position 0 0 -1 + endmarker + + // Marker CoM4 + marker CoM4 + parent Seg3 + position -0.0005 0.0688 -0.9542 + endmarker diff --git a/bioptim/examples/models/arm26_w_pendulum.bioMod b/bioptim/examples/models/arm26_w_pendulum.bioMod new file mode 100644 index 000000000..55d6a8a7a --- /dev/null +++ b/bioptim/examples/models/arm26_w_pendulum.bioMod @@ -0,0 +1,453 @@ +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 + + + +// 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 + + +segment Seg1 + RT -pi/2 0 pi/2 xyz 0 0 0.17 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.025 + inertia + 0.000391 0.0000 0.0000 + 0.0000 0.00335 -0.00032 + 0.0000 -0.00032 0.00090 + com -0.00005 0.00688 -0.09542 + meshfile mesh/pendulum.STL + meshscale 0.1 0.1 0.1 +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -0.1 + endmarker + + // Marker 5 + marker CoM1 + parent Seg1 + position -0.00005 0.00688 -0.09542 + endmarker diff --git a/bioptim/examples/models/pendulum_rotule.bioMod b/bioptim/examples/models/pendulum_rotule.bioMod new file mode 100644 index 000000000..0b37f23db --- /dev/null +++ b/bioptim/examples/models/pendulum_rotule.bioMod @@ -0,0 +1,35 @@ +version 4 + +segment Seg0 + rotations x y z + ranges + -10*pi 10*pi + -10*pi 10*pi + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + diff --git a/bioptim/examples/models/two_pendulums_2.bioMod b/bioptim/examples/models/two_pendulums_2.bioMod new file mode 100644 index 000000000..6f357735c --- /dev/null +++ b/bioptim/examples/models/two_pendulums_2.bioMod @@ -0,0 +1,77 @@ +version 4 + +segment Seg0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + + marker DownOffset + parent Seg0 + position 0.5 0 -1 + endmarker + +segment Seg1 + translations xyz + rotations y + ranges + -5 5 + -5 5 + -5 5 + -3pi 3pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker 5 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker + + marker MidTige + parent Seg1 + position 0 0 -0.5 + endmarker diff --git a/bioptim/examples/models/two_pendulums_rotule.bioMod b/bioptim/examples/models/two_pendulums_rotule.bioMod new file mode 100644 index 000000000..0146a5cd7 --- /dev/null +++ b/bioptim/examples/models/two_pendulums_rotule.bioMod @@ -0,0 +1,66 @@ +version 4 + +segment Seg0 + rotations xy + ranges + -10*pi 10*pi + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker CoM0 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg1 + RT 0 0 0 xyz 0 1 0 + rotations xy + ranges + -10*pi 10*pi + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 2 + marker marker_2 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker CoM1 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker diff --git a/bioptim/examples/toy_examples/holonomic_constraints/__init__.py b/bioptim/examples/toy_examples/holonomic_constraints/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py new file mode 100644 index 000000000..ee8cf9069 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup.py @@ -0,0 +1,196 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from casadi import DM + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicTorqueBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OptimalControlProgram, + Solver, + SolutionMerge, + OdeSolver, +) + +from bioptim.examples.utils import ExampleUtils + + +def compute_all_states(sol, bio_model: HolonomicTorqueBiorbdModel): + """ + Compute all the states from the solution of the optimal control program + + Parameters + ---------- + bio_model: HolonomicTorqueBiorbdModel + The biorbd model + sol: + The solution of the optimal control program + + Returns + ------- + + """ + + states = sol.decision_states(to_merge=SolutionMerge.NODES) + n = states["q_u"].shape[1] + q = np.zeros((bio_model.nb_q, n)) + q_v_init = DM.zeros(bio_model.nb_dependent_joints, n) + + for i in range(n): + q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init[:, i]).toarray() + q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze() + + return q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="COM_hand", + marker_2="marker_3", + index=slice(0, 2), + local_frame_index=1, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0, 1, 4], + dependent_joint_index=[2, 3], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, multi_thread=False) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + variable_bimapping.add("qdot", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + x_bounds["q_u"][2, 0] = 0 + x_bounds["q_u"][2, -1] = -np.pi + x_bounds["qdot_u"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess + x_init = InitialGuessList() + x_init["q_u"] = [0.2] * 3 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3) + u_bounds["tau"][2, :] = 0 + u_init = InitialGuessList() + # u_init.add("tau", [tau_init] * 2) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + n_threads=8, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + import pyorerun + + model_path = ExampleUtils.folder + "/models/arm26_w_pendulum.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + print(sol.real_time_to_optimize) + + print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"]) + + # --- Show results --- # + q = compute_all_states(sol, bio_model) + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py new file mode 100644 index 000000000..55ed33693 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle.py @@ -0,0 +1,171 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + SolutionMerge, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .arm26_pendulum_swingup import compute_all_states +from .custom_dynamics import HolonomicMusclesBiorbdModel + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicMusclesBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="COM_hand", + marker_2="marker_3", + index=slice(0, 2), + local_frame_index=1, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicMusclesBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0, 1, 4], + dependent_joint_index=[2, 3], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=1, multi_thread=False) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.COLLOCATION(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + variable_bimapping.add("qdot", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + x_bounds["q_u"][0, 0] = 0 + x_bounds["q_u"][1, 0] = np.pi / 2 + x_bounds["q_u"][2, 0] = 0 + x_bounds["q_u"][2, -1] = -np.pi + x_bounds["qdot_u"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess + x_init = InitialGuessList() + x_init["q_u"] = [0.2] * 3 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3) + u_bounds.add("muscles", min_bound=[0] * 6, max_bound=[1] * 6) + u_bounds["tau"][:, :] = 0 + u_bounds["tau"][-1, :] = 0 + u_init = InitialGuessList() + # u_init.add("tau", [tau_init] * 2) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + n_threads=24, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + import pyorerun + + model_path = ExampleUtils.folder + "/models/arm26_w_pendulum.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + print(sol.real_time_to_optimize) + + print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"]) + + # --- Show results --- # + q = compute_all_states(sol, bio_model) + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py new file mode 100644 index 000000000..ba755abd5 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/arm26_pendulum_swingup_muscle_algebraic.py @@ -0,0 +1,190 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + ControlType, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + Node, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + SolutionMerge, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .arm26_pendulum_swingup import compute_all_states +from .custom_dynamics import AlgebraicHolonomicMusclesBiorbdModel, constraint_holonomic, constraint_holonomic_end + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, + control_type: ControlType = ControlType.CONSTANT, +) -> (AlgebraicHolonomicMusclesBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="COM_hand", + marker_2="marker_3", + index=slice(0, 2), + local_frame_index=1, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = AlgebraicHolonomicMusclesBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0, 1, 4], + dependent_joint_index=[2, 3], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=1, multi_thread=False) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.COLLOCATION(), expand_dynamics=expand_dynamics)) + + # Boundaries + u_variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + u_variable_bimapping.add("q", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + u_variable_bimapping.add("qdot", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + + v_variable_bimapping = BiMappingList() + v_variable_bimapping.add("q", to_second=[None, None, 0, 1, None], to_first=[2, 3]) + + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=u_variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=u_variable_bimapping) + + x_bounds["q_u"][0, 0] = 0 + x_bounds["q_u"][1, 0] = np.pi / 2 + x_bounds["q_u"][2, 0] = 0 + x_bounds["q_u"][2, -1] = -np.pi + x_bounds["qdot_u"][:, [0, -1]] = 0 # Start and end without any velocity + + a_bounds = BoundsList() + a_bounds.add("q_v", bio_model.bounds_from_ranges("q", mapping=v_variable_bimapping)) + + # Initial guess + x_init = InitialGuessList() + x_init["q_u"] = [0.2] * 3 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + tau_variable_bimapping = BiMappingList() + tau_variable_bimapping.add("tau", to_second=[0, 1, None, None, 2], to_first=[0, 1, 4]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min] * 3, max_bound=[tau_max] * 3) + u_bounds.add("muscles", min_bound=[0] * 6, max_bound=[1] * 6) + u_bounds["tau"][:, :] = 0 + u_bounds["tau"][-1, :] = 0 + u_init = InitialGuessList() + # u_init.add("tau", [tau_init] * 2) + + # Path Constraints + constraints = ConstraintList() + constraints.add( + constraint_holonomic, + node=Node.ALL_SHOOTING, + ) + constraints.add( + constraint_holonomic_end, + node=Node.END, + ) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=tau_variable_bimapping, + constraints=constraints, + control_type=control_type, + n_threads=24, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + import pyorerun + + model_path = ExampleUtils.folder + "/models/arm26_w_pendulum.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + print(sol.real_time_to_optimize) + + print(sol.decision_states(to_merge=SolutionMerge.NODES)["q_u"]) + + # --- Show results --- # + q = compute_all_states(sol, bio_model) + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/common.py b/bioptim/examples/toy_examples/holonomic_constraints/common.py new file mode 100644 index 000000000..224aa222c --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/common.py @@ -0,0 +1,34 @@ +import numpy as np +from casadi import DM + +from bioptim import HolonomicTorqueBiorbdModel, SolutionMerge + + +def compute_all_q(sol, bio_model: HolonomicTorqueBiorbdModel): + """ + Compute all the states from the solution of the optimal control program + + Parameters + ---------- + bio_model: HolonomicTorqueBiorbdModel + The biorbd model + sol: + The solution of the optimal control program + + Returns + ------- + + """ + + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + n = states["q_u"].shape[1] + + q = np.zeros((bio_model.nb_q, n)) + + q_v_init = DM.zeros(bio_model.nb_dependent_joints, n) + for i in range(n): + q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init[:, i]).toarray() + q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze() + + return q diff --git a/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py b/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py index 4db2512ed..3bd5eb734 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/custom_dynamics.py @@ -1,14 +1,30 @@ +import os + +print(os.getenv("PYTHONPATH")) + +from typing import Callable + +import numpy as np +from casadi import vertcat, DM + +import biorbd from bioptim import ( - PenaltyController, + ConfigureVariables, + Controls, DynamicsEvaluation, DynamicsFunctions, - OdeSolver, - HolonomicTorqueBiorbdModel, - ConfigureVariables, + HolonomicBiorbdModel, HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + HolonomicTorqueDynamics, + OdeSolver, + ParameterList, + PenaltyController, + States, + DefectType, + TorqueDynamics, + MusclesDynamics, ) -from bioptim.examples.utils import ExampleUtils -from casadi import vertcat def constraint_holonomic_end( @@ -146,3 +162,194 @@ def dynamics( defects = vertcat(slope_q_u, slope_qdot_u) - dxdt return DynamicsEvaluation(dxdt=dxdt, defects=defects) + + +class HolonomicMusclesDynamics(HolonomicTorqueDynamics): + + def __init__(self): + super().__init__() + self.state_configuration = [States.Q_U, States.QDOT_U] + self.control_configuration = [Controls.TAU, Controls.MUSCLE_EXCITATION] + self.algebraic_configuration = [] + self.functions = [ + ConfigureVariables.configure_qv, + ConfigureVariables.configure_qdotv, + ConfigureVariables.configure_lagrange_multipliers_function, + ] + self.with_residual_torque = True + + @property + def control_configuration_functions(self): + return [Controls.TAU, Controls.MUSCLE_EXCITATION] + + def get_basic_variables( + self, + nlp, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + ): + + # Get variables from the right place + # q = DynamicsFunctions.get(nlp.states["q"], states) + # qdot = DynamicsFunctions.get(nlp.states["qdot"], states) + q_u = DynamicsFunctions.get(nlp.states["q_u"], states) + qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) + q_v_init = DM.zeros(nlp.model.nb_dependent_joints) + mus_activations = DynamicsFunctions.get(nlp.controls["muscles"], controls) + fatigue_states, mus_activations = DynamicsFunctions.get_fatigue_states(states, nlp, mus_activations) + + q = nlp.model.compute_q()(q_u, q_v_init) + qdot = nlp.model.compute_qdot()(q, qdot_u) + + # Compute the torques due to muscles + muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations, fatigue_states) + + # Add additional torques + if self.with_residual_torque: + muscles_tau += DynamicsFunctions.get_fatigable_tau(nlp, states, controls) + muscles_tau += DynamicsFunctions.collect_tau(nlp, q, qdot, parameters) + + # Get external forces + external_forces = nlp.get_external_forces( + "external_forces", states, controls, algebraic_states, numerical_timeseries + ) + return q_u, qdot_u, muscles_tau, external_forces, mus_activations + + def dynamics( + self, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + nlp, + ): + + # q_u = DynamicsFunctions.get(nlp.states["q_u"], states) + # qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) + # tau = DynamicsFunctions.get(nlp.controls["tau"], controls) # Get torques from muscles + residual torques + q_u, qdot_u, tau, _, _ = self.get_basic_variables( + nlp, states, controls, parameters, algebraic_states, numerical_timeseries + ) + q_v_init = DM.zeros(nlp.model.nb_dependent_joints) + + qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau) + dxdt = vertcat(qdot_u, qddot_u) + + defects = None + if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): + slope_q_u = DynamicsFunctions.get(nlp.states_dot["q_u"], nlp.states_dot.scaled.cx) + slope_qdot_u = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) + defects = vertcat(slope_q_u, slope_qdot_u) - dxdt + + return DynamicsEvaluation(dxdt=dxdt, defects=defects) + + def get_rigid_contact_forces(self, time, states, controls, parameters, algebraic_states, numerical_timeseries, nlp): + return + + @property + def extra_dynamics(self): + return None + + +class HolonomicMusclesBiorbdModel(HolonomicBiorbdModel, HolonomicMusclesDynamics): + def __init__( + self, + bio_model: str | biorbd.Model, + friction_coefficients: np.ndarray = None, + parameters: ParameterList = None, + holonomic_constraints: HolonomicConstraintsList | None = None, + dependent_joint_index: list[int] | tuple[int, ...] = None, + independent_joint_index: list[int] | tuple[int, ...] = None, + ): + HolonomicBiorbdModel.__init__(self, bio_model, friction_coefficients, parameters) + if holonomic_constraints is not None: + self.set_holonomic_configuration(holonomic_constraints, dependent_joint_index, independent_joint_index) + HolonomicMusclesDynamics.__init__(self) + + def serialize(self) -> tuple[Callable, dict]: + return HolonomicMusclesBiorbdModel, dict(bio_model=self.path, friction_coefficients=self.friction_coefficients) + + +class AlgebraicHolonomicMusclesBiorbdModel(HolonomicMusclesBiorbdModel): + def __init__( + self, + bio_model_path: str, + holonomic_constraints: HolonomicConstraintsList = None, + independent_joint_index: tuple[int] = None, + dependent_joint_index: tuple[int] = None, + ): + super().__init__( + bio_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=independent_joint_index, + dependent_joint_index=dependent_joint_index, + ) + + @property + def algebraic_configuration_functions(self): + return super().algebraic_configuration_functions + [configure_qv] + + def get_basic_variables( + self, + nlp, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + ): + + q_u = DynamicsFunctions.get(nlp.states["q_u"], states) + qdot_u = DynamicsFunctions.get(nlp.states["qdot_u"], states) + q_v = DynamicsFunctions.get(nlp.algebraic_states["q_v"], algebraic_states) + mus_activations = DynamicsFunctions.get(nlp.controls["muscles"], controls) + fatigue_states, mus_activations = DynamicsFunctions.get_fatigue_states(states, nlp, mus_activations) + + q = nlp.model.compute_q()(q_u, q_v) + qdot = nlp.model.compute_qdot()(q, qdot_u) + + # Compute the torques due to muscles + muscles_tau = DynamicsFunctions.compute_tau_from_muscle(nlp, q, qdot, mus_activations, fatigue_states) + + # Add additional torques + if self.with_residual_torque: + muscles_tau += DynamicsFunctions.get_fatigable_tau(nlp, states, controls) + muscles_tau += DynamicsFunctions.collect_tau(nlp, q, qdot, parameters) + + # Get external forces + external_forces = nlp.get_external_forces( + "external_forces", states, controls, algebraic_states, numerical_timeseries + ) + return q_u, qdot_u, muscles_tau, external_forces, mus_activations + + def dynamics( + self, + time, + states, + controls, + parameters, + algebraic_states, + numerical_timeseries, + nlp, + ): + + q_u, qdot_u, tau, _, _ = self.get_basic_variables( + nlp, states, controls, parameters, algebraic_states, numerical_timeseries + ) + q_v = DynamicsFunctions.get(nlp.algebraic_states["q_v"], algebraic_states) + + qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v, tau) + dxdt = vertcat(qdot_u, qddot_u) + + defects = None + if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): + slope_q_u = DynamicsFunctions.get(nlp.states_dot["q_u"], nlp.states_dot.scaled.cx) + slope_qdot_u = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) + defects = vertcat(slope_q_u, slope_qdot_u) - dxdt + + return DynamicsEvaluation(dxdt=dxdt, defects=defects) diff --git a/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py new file mode 100644 index 000000000..e756f2f0a --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py @@ -0,0 +1,192 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicTorqueBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OptimalControlProgram, + Solver, + OdeSolver, +) + +from bioptim.examples.utils import ExampleUtils + +from .common import compute_all_q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "pendulum1", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_1", + marker_2="marker_3", + index=slice(1, 3), + local_frame_index=0, + ) + + holonomic_constraints.add( + "pendulum2", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_6", + marker_2="marker_7", + index=slice(1, 3), + local_frame_index=3, + ) + + holonomic_constraints.add( + "distal_link", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_4", + marker_2="marker_8", + index=slice(1, 3), + local_frame_index=1, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0, 4], + dependent_joint_index=[1, 2, 3, 5, 6, 7], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, None, None, None, 1, None, None, None], to_first=[0, 4]) + variable_bimapping.add("qdot", to_second=[0, None, None, None, 1, None, None, None], to_first=[0, 4]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [0, 0]) + x_init.add("qdot_u", [0, 0]) + x_bounds["q_u"][:, 0] = [1.54 / 2, 0] + x_bounds["qdot_u"][:, 0] = [0, 0] + x_bounds["q_u"][0, -1] = -1.54 / 2 + x_bounds["q_u"][1, -1] = 0 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, None, None, None, 1, None, None, None], to_first=[0, 4]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min] * 2, max_bound=[tau_max] * 2) + u_init = InitialGuessList() + u_init.add("tau", [tau_init] * 2) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + n_threads=8, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = ExampleUtils.folder + "/models/4bar.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + print(sol.real_time_to_optimize) + + # --- Show results --- # + q = compute_all_q(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py new file mode 100644 index 000000000..4c1d7fb28 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py @@ -0,0 +1,178 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .common import compute_all_q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraintsP01", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_1", + marker_2="marker_3", + index=slice(1, 3), + local_frame_index=0, + ) + holonomic_constraints.add( + "holonomic_constraintsP12", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_4", + marker_2="marker_6", + index=slice(1, 3), + local_frame_index=2, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0], + dependent_joint_index=[1, 2, 3, 4], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, None, None, None, None], to_first=[0]) + variable_bimapping.add("qdot", to_second=[0, None, None, None, None], to_first=[0]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [1.3]) + x_init.add("qdot_u", [0]) + x_bounds["q_u"][0, 0] = 1.3 + x_bounds["qdot_u"][0, 0] = 0 + x_bounds["q_u"][0, -1] = 0 + # x_bounds["q_u"][1, -1] = 0 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, None, None, None, None], to_first=[0]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min], max_bound=[tau_max]) + u_init = InitialGuessList() + u_init.add("tau", [tau_init]) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = ExampleUtils.folder + "/models/3bar.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + + q = compute_all_q(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py index 5c6b29830..c0e0c1baa 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py @@ -10,16 +10,16 @@ ConstraintList, DynamicsOptions, DynamicsOptionsList, - HolonomicTorqueBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, InitialGuessList, ObjectiveFcn, ObjectiveList, + OdeSolver, OptimalControlProgram, - Solver, SolutionMerge, - OdeSolver, + Solver, ) from bioptim.examples.utils import ExampleUtils from casadi import DM @@ -88,6 +88,7 @@ def prepare_ocp( n_shooting: int = 30, final_time: float = 1, expand_dynamics: bool = False, + ode_solver=OdeSolver.RK4(), ) -> tuple[HolonomicTorqueBiorbdModel, OptimalControlProgram]: """ Prepare the program @@ -136,7 +137,7 @@ def prepare_ocp( # Dynamics dynamics = DynamicsOptionsList() - dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + dynamics.add(DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics)) # Path Constraints constraints = ConstraintList() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py new file mode 100644 index 000000000..6c4e2dde1 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py @@ -0,0 +1,178 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .common import compute_all_q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_1", + marker_2="marker_3", + index=slice(0, 3), + local_frame_index=0, + ) + holonomic_constraints.add( + "holonomic_constraints2", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="DownOffset", + marker_2="MidTige", + index=slice(0, 1), + local_frame_index=0, + ) + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0], + dependent_joint_index=[1, 2, 3, 4], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) + # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, None, None, None, None], to_first=[0]) + variable_bimapping.add("qdot", to_second=[0, None, None, None, None], to_first=[0]) + + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + x_bounds["q_u"][0, 0] = -0.5 + x_bounds["q_u"][0, -1] = 0.5 + x_bounds["qdot_u"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [-0.5]) + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, None, None, None, None], to_first=[0]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min], max_bound=[tau_max]) + u_init = InitialGuessList() + # u_init.add("tau", [tau_init]) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = ExampleUtils.folder + "/models/two_pendulums_2.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) + print(sol.real_time_to_optimize) + + # --- Show results --- # + q = compute_all_q(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py new file mode 100644 index 000000000..2cbd4dd2d --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py @@ -0,0 +1,175 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .common import compute_all_q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_0", + marker_2="marker_3", + index=slice(1, 3), + local_frame_index=0, + ) + holonomic_constraints.add( + "holonomic_constraints2", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_1", + marker_2="marker_4", + index=slice(2, 3), + local_frame_index=0, + ) + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[0], + dependent_joint_index=[1, 2, 3], + ) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) + # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[0, None, None, None], to_first=[0]) + variable_bimapping.add("qdot", to_second=[0, None, None, None], to_first=[0]) + x_bounds = BoundsList() + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + x_bounds["q_u"][0, 0] = 0.5 + x_bounds["q_u"][:, 0] = -0.5 + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [0.2]) + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[0, None, None, None], to_first=[0]) + u_bounds = BoundsList() + # u_bounds.add("tau", min_bound=[tau_min], max_bound=[tau_max]) + u_init = InitialGuessList() + # u_init.add("tau", [tau_init]) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = ExampleUtils.folder + "/models/two_pendulums.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT()) + print(sol.real_time_to_optimize) + + # --- Show results --- # + q = compute_all_q(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py index c48d044b9..992d5d62f 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_algebraic.py @@ -8,16 +8,17 @@ BiMappingList, BoundsList, ConstraintList, - DynamicsOptionsList, + CostType, DynamicsOptions, - HolonomicBiorbdModel, + DynamicsOptionsList, HolonomicConstraintsFcn, HolonomicConstraintsList, InitialGuessList, + Node, ObjectiveFcn, ObjectiveList, + OdeSolver, OptimalControlProgram, - Solver, SolutionMerge, Node, CostType, @@ -34,59 +35,7 @@ constraint_holonomic_end, ) - -def compute_all_states(sol, bio_model: HolonomicBiorbdModel): - """ - Compute all the states from the solution of the optimal control program - - Parameters - ---------- - bio_model: HolonomicBiorbdModel - The biorbd model - sol: - The solution of the optimal control program - - Returns - ------- - - """ - - states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - - n = states["q_u"].shape[1] - - q = np.zeros((bio_model.nb_q, n)) - qdot = np.zeros((bio_model.nb_q, n)) - qddot = np.zeros((bio_model.nb_q, n)) - lambdas = np.zeros((bio_model.nb_dependent_joints, n)) - tau = np.zeros((bio_model.nb_tau, n)) - - for i, independent_joint_index in enumerate(bio_model.independent_joint_index): - tau[independent_joint_index, :-1] = controls["tau"][i, :] - for i, dependent_joint_index in enumerate(bio_model.dependent_joint_index): - tau[dependent_joint_index, :-1] = controls["tau"][i, :] - - q_v_init = DM.zeros(bio_model.nb_dependent_joints) - for i in range(n): - q_v_i = bio_model.compute_q_v()(states["q_u"][:, i], q_v_init).toarray() - q[:, i] = bio_model.state_from_partition(states["q_u"][:, i][:, np.newaxis], q_v_i).toarray().squeeze() - qdot[:, i] = bio_model.compute_qdot()(q[:, i], states["qdot_u"][:, i]).toarray().squeeze() - qddot_u_i = ( - bio_model.partitioned_forward_dynamics()(states["q_u"][:, i], states["qdot_u"][:, i], q_v_init, tau[:, i]) - .toarray() - .squeeze() - ) - qddot[:, i] = bio_model.compute_qddot()(q[:, i], qdot[:, i], qddot_u_i).toarray().squeeze() - lambdas[:, i] = ( - bio_model.compute_the_lagrangian_multipliers()( - states["q_u"][:, i][:, np.newaxis], states["qdot_u"][:, i], q_v_init[:, i], tau[:, i] - ) - .toarray() - .squeeze() - ) - - return q, qdot, qddot, lambdas +from .custom_dynamics import ModifiedHolonomicTorqueBiorbdModel, constraint_holonomic, constraint_holonomic_end def prepare_ocp( @@ -95,7 +44,7 @@ def prepare_ocp( final_time: float = 1, expand_dynamics: bool = False, ode_solver: OdeSolver = OdeSolver.COLLOCATION(polynomial_degree=2), -) -> tuple[HolonomicBiorbdModel, OptimalControlProgram]: +) -> tuple[ModifiedHolonomicTorqueBiorbdModel, OptimalControlProgram]: """ Prepare the program @@ -253,7 +202,8 @@ def main(): viz.exec() if viewer == "pyorerun": - from pyorerun import BiorbdModel as PyorerunBiorbdModel, PhaseRerun + from pyorerun import BiorbdModel as PyorerunBiorbdModel + from pyorerun import PhaseRerun pyomodel = PyorerunBiorbdModel(biorbd_model_path) viz = PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py new file mode 100644 index 000000000..320e57efe --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py @@ -0,0 +1,172 @@ +""" +This example presents how to implement a holonomic constraint in bioptim. +The simulation is two single pendulum that are forced to be coherent with a holonomic constraint. It is then a double +pendulum simulation. +""" + +import numpy as np +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + HolonomicTorqueBiorbdModel, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OptimalControlProgram, + Solver, +) +from bioptim.examples.utils import ExampleUtils + +from .common import compute_all_q + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, +): + """ + Prepare the program + + Parameters + ---------- + biorbd_model_path: str + The path of the biorbd model + n_shooting: int + The number of shooting points + final_time: float + The time at the final node + expand_dynamics: bool + If the dynamics function should be expanded. Please note, this will solve the problem faster, but will slow down + the declaration of the OCP, so it is a trade-off. Also depending on the solver, it may or may not work + (for instance IRK is not compatible with expanded dynamics) + + Returns + ------- + The ocp ready to be solved + """ + # Create a holonomic constraint to create a double pendulum from two single pendulums + holonomic_constraints = HolonomicConstraintsList() + holonomic_constraints.add( + "holonomic_constraints", + HolonomicConstraintsFcn.superimpose_markers, + marker_1="marker_1", + marker_2="marker_3", + index=slice(0, 3), + local_frame_index=0, + ) + + # The rotations (joint 0 and 3) are independent. The translations (joint 1 and 2) are constrained by the holonomic + # constraint + bio_model = HolonomicTorqueBiorbdModel( + biorbd_model_path, + holonomic_constraints=holonomic_constraints, + independent_joint_index=[1], + dependent_joint_index=[0, 2, 3], + ) + # bio_model = TorqueBiorbdModel(biorbd_model_path) + + # Add objective functions + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", multi_thread=False) + # objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, min_bound=0.5, max_bound=0.6) + + # Dynamics + dynamics = DynamicsOptionsList() + dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + + # Path Constraints + constraints = ConstraintList() + + # Boundaries + variable_bimapping = BiMappingList() + # The rotations (joint 0 and 2) are independent. The rotations (joint 1 and 3) are constrained by the holonomic + # constraint, so we need to map the states and controls to only compute the dynamics of the independent joints + # The dynamics of the dependent joints will be computed from the holonomic constraint + variable_bimapping.add("q", to_second=[None, 0, None, None], to_first=[1]) + variable_bimapping.add("qdot", to_second=[None, 0, None, None], to_first=[1]) + x_bounds = BoundsList() + # q_u and qdot_u are the states of the independent joints + x_bounds["q_u"] = bio_model.bounds_from_ranges("q", mapping=variable_bimapping) + x_bounds["qdot_u"] = bio_model.bounds_from_ranges("qdot", mapping=variable_bimapping) + + # Initial guess + x_init = InitialGuessList() + x_init.add("q_u", [np.pi / 6]) + x_init.add("qdot_u", [0]) + x_bounds["q_u"][:, 0] = [np.pi / 6] + x_bounds["qdot_u"][:, 0] = 0 + x_bounds["q_u"][:, -1] = [-np.pi / 4] + # x_bounds["q_u"][1, -1] = 0 + + # Define control path constraint + tau_min, tau_max, tau_init = -100, 100, 0 + # # Only the rotations are controlled + variable_bimapping.add("tau", to_second=[None, 0, None, None], to_first=[1]) + u_bounds = BoundsList() + u_bounds.add("tau", min_bound=[tau_min], max_bound=[tau_max]) + u_init = InitialGuessList() + u_init.add("tau", [tau_init]) + + # ------------- # + + return ( + OptimalControlProgram( + bio_model, + n_shooting, + final_time, + dynamics=dynamics, + x_bounds=x_bounds, + u_bounds=u_bounds, + x_init=x_init, + u_init=u_init, + objective_functions=objective_functions, + variable_mappings=variable_bimapping, + constraints=constraints, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = ExampleUtils.folder + "/models/two_pendulums_rotule.bioMod" + ocp, bio_model = prepare_ocp(biorbd_model_path=model_path) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=False)) + print(sol.real_time_to_optimize) + + q = compute_all_q(sol, bio_model) + + viewer = "pyorerun" + if viewer == "bioviz": + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + if viewer == "pyorerun": + import pyorerun + + viz = pyorerun.PhaseRerun(t_span=np.concatenate(sol.decision_time()).squeeze()) + viz.add_animated_model(pyorerun.BiorbdModel(model_path), q=q) + + viz.rerun("double_pendulum") + + sol.graphs() + + +if __name__ == "__main__": + main() diff --git a/environment.yml b/environment.yml index 5dda2bc7a..862d0de8f 100644 --- a/environment.yml +++ b/environment.yml @@ -8,4 +8,5 @@ dependencies: - matplotlib - pyqt - pyqtgraph -- python-graphviz \ No newline at end of file +- python-graphviz +- numpy <2.4 \ No newline at end of file diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 490fcbe65..25ef9084d 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -4,7 +4,14 @@ import pytest from casadi import DM, MX -from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge +from bioptim import ( + HolonomicBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + Solver, + SolutionMerge, + OdeSolver, +) from ..utils import TestUtils @@ -193,7 +200,8 @@ def test_model_holonomic(): ) -def test_example_two_pendulums(): +@pytest.mark.parametrize("ode_solver", [OdeSolver.RK4(), OdeSolver.COLLOCATION()]) +def test_example_two_pendulums(ode_solver): """Test the holonomic_constraints/two_pendulums example""" from bioptim.examples.toy_examples.holonomic_constraints import two_pendulums @@ -205,20 +213,148 @@ def test_example_two_pendulums(): n_shooting=10, final_time=1, expand_dynamics=False, + ode_solver=ode_solver, ) # --- Solve the ocp --- # sol = ocp.solve(Solver.IPOPT()) states = sol.decision_states(to_merge=SolutionMerge.NODES) - npt.assert_almost_equal( - states["q_u"], - [ - [1.54, 1.433706, 1.185046, 0.891157, 0.561607, 0.191792, -0.206511, -0.614976, -1.018383, -1.356253, -1.54], - [1.54, 1.669722, 1.924726, 2.127746, 2.226937, 2.184007, 1.972105, 1.593534, 1.06751, 0.507334, 0.0], - ], - decimal=6, - ) + if isinstance(ode_solver, OdeSolver.RK4): + npt.assert_almost_equal( + states["q_u"], + [ + [ + 1.54, + 1.433706, + 1.185046, + 0.891157, + 0.561607, + 0.191792, + -0.206511, + -0.614976, + -1.018383, + -1.356253, + -1.54, + ], + [1.54, 1.669722, 1.924726, 2.127746, 2.226937, 2.184007, 1.972105, 1.593534, 1.06751, 0.507334, 0.0], + ], + decimal=6, + ) + + elif isinstance(ode_solver, OdeSolver.COLLOCATION): + npt.assert_almost_equal( + states["q_u"], + [ + [ + 1.54, + 1.53947255, + 1.52829032, + 1.4918706, + 1.44772412, + 1.43369574, + 1.41898275, + 1.35974706, + 1.27447461, + 1.20430304, + 1.18502396, + 1.16556562, + 1.09140745, + 0.99166684, + 0.91261473, + 0.8911305, + 0.86947629, + 0.78671683, + 0.67482281, + 0.585796, + 0.56157817, + 0.53715892, + 0.44378604, + 0.31793766, + 0.21860716, + 0.19175812, + 0.1647666, + 0.0623588, + -0.07337049, + -0.1784637, + -0.20655284, + -0.23470758, + -0.34109507, + -0.48052131, + -0.58686908, + -0.6150277, + -0.64319721, + -0.74951225, + -0.88766222, + -0.99132882, + -1.01844059, + -1.0452361, + -1.14223391, + -1.25827372, + -1.33704454, + -1.35629421, + -1.3747351, + -1.43601269, + -1.49800372, + -1.53258632, + -1.54, + ], + [ + 1.54, + 1.54064515, + 1.55431106, + 1.59922772, + 1.65296364, + 1.66973502, + 1.68705873, + 1.75370598, + 1.84171006, + 1.90763103, + 1.92475754, + 1.94158775, + 2.00129161, + 2.07058481, + 2.11662288, + 2.12778938, + 2.13845817, + 2.17380886, + 2.20803169, + 2.2242531, + 2.22699357, + 2.22904554, + 2.2304308, + 2.21629085, + 2.19241948, + 2.18407247, + 2.17489014, + 2.1330352, + 2.06082354, + 1.99228721, + 1.97216828, + 1.95123087, + 1.86495558, + 1.73527252, + 1.62449462, + 1.59357668, + 1.56195524, + 1.43562583, + 1.25495762, + 1.10760291, + 1.06752078, + 1.0272699, + 0.8761086, + 0.68415659, + 0.54376452, + 0.50733629, + 0.47128493, + 0.3387245, + 0.16767637, + 0.03546255, + 0.0, + ], + ], + decimal=6, + ) def test_example_two_pendulums_algebraic(): @@ -381,3 +517,1346 @@ def test_example_two_pendulums_algebraic(): ), decimal=6, ) + + +def test_example_three_bar(): + """Test the holonomic_constraints/three_bar example""" + from bioptim.examples.toy_examples.holonomic_constraints import three_bar + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = three_bar.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/3bar.bioMod", + n_shooting=10, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + 1.3, + 1.28112296, + 1.22535242, + 1.13876849, + 1.02751913, + 0.89519494, + 0.74384486, + 0.57524378, + 0.39190694, + 0.19799513, + 0.0, + ] + ], + decimal=6, + ) + + +def test_example_four_bar(): + """Test the holonomic_constraints/four_bar example""" + from bioptim.examples.toy_examples.holonomic_constraints import four_bar + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = four_bar.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/4bar.bioMod", + n_shooting=30, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + 0.77, + 0.76754753, + 0.76031326, + 0.74858082, + 0.73266023, + 0.71288935, + 0.6896361, + 0.66330062, + 0.63432063, + 0.60319444, + 0.5705717, + 0.53750942, + 0.50529314, + 0.47306849, + 0.4387326, + 0.39365688, + 0.33550945, + 0.2727274, + 0.20440837, + 0.13396314, + 0.06433518, + -0.00455076, + -0.07445804, + -0.1471207, + -0.22344713, + -0.30363321, + -0.38765837, + -0.47567902, + -0.56818751, + -0.66598663, + -0.77, + ], + [ + 0.0, + -0.0020824, + -0.00821695, + -0.01830149, + -0.03236938, + -0.05048038, + -0.0726346, + -0.09875238, + -0.1287542, + -0.16275482, + -0.20132215, + -0.24535886, + -0.29370214, + -0.33909243, + -0.37268925, + -0.39294823, + -0.40355002, + -0.39595389, + -0.37802695, + -0.35578337, + -0.32816605, + -0.29365283, + -0.25313732, + -0.20969424, + -0.16682135, + -0.12698646, + -0.09139836, + -0.0605062, + -0.03456109, + -0.01404837, + 0.0, + ], + ], + decimal=6, + ) + + +def test_example_two_pendulums_2constraint_4DOF(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import two_pendulums_2constraint_4DOF + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = two_pendulums_2constraint_4DOF.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/two_pendulums.bioMod", + n_shooting=10, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + -0.5, + -0.47400031, + -0.40886377, + -0.31068409, + -0.18888455, + -0.05543956, + 0.07635419, + 0.19339095, + 0.28422433, + 0.34019268, + 0.35608811, + ] + ], + decimal=6, + ) + + +def test_example_two_pendulums_2constraint(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import two_pendulums_2constraint + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = two_pendulums_2constraint.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/two_pendulums_2.bioMod", + n_shooting=10, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + -0.5, + -0.47400672, + -0.39832961, + -0.28240044, + -0.14064867, + 0.01147023, + 0.15989866, + 0.29301816, + 0.40093225, + 0.47362475, + 0.5, + ] + ], + decimal=6, + ) + + +def test_example_two_pendulums_rotule(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import two_pendulums_rotule + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = two_pendulums_rotule.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/two_pendulums_rotule.bioMod", + n_shooting=10, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + 0.52359878, + 0.49719372, + 0.41660698, + 0.28429009, + 0.10904561, + -0.09605531, + -0.31061947, + -0.50569474, + -0.65398428, + -0.74758078, + -0.78539816, + ] + ], + decimal=6, + ) + + +def test_example_arm26_pendulum_swingup(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import arm26_pendulum_swingup + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = arm26_pendulum_swingup.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/arm26_w_pendulum.bioMod", + n_shooting=30, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + -6.23253084e-01, + -6.38484167e-01, + -6.68922578e-01, + -6.81792550e-01, + -6.55238890e-01, + -5.87796062e-01, + -4.89966451e-01, + -3.76043646e-01, + -2.59102081e-01, + -1.48895389e-01, + -5.20034953e-02, + 2.53932736e-02, + 7.47309323e-02, + 8.76935179e-02, + 6.12325532e-02, + -1.52507329e-03, + -9.10955428e-02, + -1.94140944e-01, + -2.97712860e-01, + -3.90212593e-01, + -4.61042809e-01, + -5.01160801e-01, + -5.04847569e-01, + -4.71389909e-01, + -4.05573563e-01, + -3.16705448e-01, + -2.16476676e-01, + -1.17080223e-01, + -3.07197021e-02, + 2.99772919e-02, + 5.23003485e-02, + ], + [ + 3.80567092e-01, + 4.51818983e-01, + 6.23662754e-01, + 8.05120614e-01, + 9.30553547e-01, + 9.83693526e-01, + 9.77409213e-01, + 9.35719215e-01, + 8.82442686e-01, + 8.35220491e-01, + 8.04531259e-01, + 8.00362705e-01, + 8.40009103e-01, + 9.41814395e-01, + 1.11224555e00, + 1.34321756e00, + 1.61055797e00, + 1.88083666e00, + 2.12537366e00, + 2.32559227e00, + 2.47363237e00, + 2.57049953e00, + 2.62094933e00, + 2.62962518e00, + 2.60031256e00, + 2.53707423e00, + 2.44633497e00, + 2.33899114e00, + 2.23156343e00, + 2.14666431e00, + 2.11304119e00, + ], + [ + 0.00000000e00, + 2.19071925e-02, + 8.06926144e-02, + 1.66816334e-01, + 2.70564080e-01, + 3.80497065e-01, + 4.82560923e-01, + 5.63694198e-01, + 6.12524398e-01, + 6.20141635e-01, + 5.80996906e-01, + 4.91848034e-01, + 3.50419451e-01, + 1.56747128e-01, + -8.41634283e-02, + -3.57931966e-01, + -6.41571324e-01, + -9.16060238e-01, + -1.17294667e00, + -1.40983578e00, + -1.62710399e00, + -1.82869131e00, + -2.02079309e00, + -2.20817341e00, + -2.39205176e00, + -2.57114387e00, + -2.74170772e00, + -2.89615124e00, + -3.02328610e00, + -3.11002119e00, + -3.14159265e00, + ], + ], + decimal=6, + ) + + +def test_example_arm26_pendulum_swingup_muscle(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import arm26_pendulum_swingup_muscle + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = arm26_pendulum_swingup_muscle.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/arm26_w_pendulum.bioMod", + n_shooting=30, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + 0.00000000e00, + -3.20620080e-05, + -7.22737278e-04, + -2.95404501e-03, + -5.63924692e-03, + -6.49005147e-03, + -7.44120424e-03, + -1.23041824e-02, + -2.16864947e-02, + -3.11639947e-02, + -3.40183367e-02, + -3.70432103e-02, + -5.02028401e-02, + -7.15901095e-02, + -9.11251000e-02, + -9.67782100e-02, + -1.02636806e-01, + -1.26605153e-01, + -1.62430055e-01, + -1.93215657e-01, + -2.01885869e-01, + -2.10736867e-01, + -2.45435943e-01, + -2.94093000e-01, + -3.33840955e-01, + -3.44774564e-01, + -3.55823516e-01, + -3.98072073e-01, + -4.54910190e-01, + -4.99606632e-01, + -5.11656422e-01, + -5.23739586e-01, + -5.69147128e-01, + -6.28166932e-01, + -6.72816216e-01, + -6.84573949e-01, + -6.96250019e-01, + -7.39135696e-01, + -7.92336556e-01, + -8.30465979e-01, + -8.40175985e-01, + -8.49677979e-01, + -8.83314439e-01, + -9.21855611e-01, + -9.46785879e-01, + -9.52697267e-01, + -9.58289149e-01, + -9.76331668e-01, + -9.92490110e-01, + -9.98834344e-01, + -9.99597271e-01, + -9.99995036e-01, + -9.98551302e-01, + -9.89799356e-01, + -9.77968499e-01, + -9.74086523e-01, + -9.69981207e-01, + -9.53456348e-01, + -9.29990775e-01, + -9.11227010e-01, + -9.06184652e-01, + -9.01122130e-01, + -8.81898582e-01, + -8.56759436e-01, + -8.37838636e-01, + -8.32883913e-01, + -8.27946707e-01, + -8.09404808e-01, + -7.85340850e-01, + -7.67085834e-01, + -7.62252280e-01, + -7.57419485e-01, + -7.39182477e-01, + -7.15118888e-01, + -6.96416053e-01, + -6.91386302e-01, + -6.86330724e-01, + -6.67071745e-01, + -6.41203964e-01, + -6.20752421e-01, + -6.15205311e-01, + -6.09614819e-01, + -5.88228319e-01, + -5.59333655e-01, + -5.36409182e-01, + -5.30185937e-01, + -5.23914212e-01, + -4.99944540e-01, + -4.67660510e-01, + -4.42166181e-01, + -4.35267039e-01, + -4.28324533e-01, + -4.01895309e-01, + -3.66576993e-01, + -3.38924873e-01, + -3.31478891e-01, + -3.24002617e-01, + -2.95697827e-01, + -2.58261881e-01, + -2.29261624e-01, + -2.21498827e-01, + -2.13725121e-01, + -1.84491458e-01, + -1.46302787e-01, + -1.17083535e-01, + -1.09314554e-01, + -1.01558643e-01, + -7.26247823e-02, + -3.53760500e-02, + -7.29152103e-03, + 1.15378024e-04, + 7.48066662e-03, + 3.46628706e-02, + 6.89600515e-02, + 9.42902119e-02, + 1.00893971e-01, + 1.07423714e-01, + 1.31152927e-01, + 1.60235905e-01, + 1.81077138e-01, + 1.86419341e-01, + 1.91658860e-01, + 2.10283440e-01, + 2.32176279e-01, + 2.47191245e-01, + 2.50945089e-01, + 2.54592465e-01, + 2.67307303e-01, + 2.81730339e-01, + 2.91261524e-01, + 2.93592960e-01, + 2.95870417e-01, + 3.04207591e-01, + 3.14515826e-01, + 3.21882445e-01, + 3.23752774e-01, + 3.25632869e-01, + 3.33139282e-01, + 3.43495577e-01, + 3.51382102e-01, + 3.53423056e-01, + 3.55450956e-01, + 3.63029886e-01, + 3.72308078e-01, + 3.78486893e-01, + 3.79950859e-01, + 3.81334667e-01, + 3.85790931e-01, + 3.89592744e-01, + 3.90787335e-01, + 3.90843002e-01, + ], + [ + 1.57079633e00, + 1.57070683e00, + 1.56877430e00, + 1.56244859e00, + 1.55465633e00, + 1.55214408e00, + 1.54963079e00, + 1.54182624e00, + 1.53548604e00, + 1.53352726e00, + 1.53342243e00, + 1.53358406e00, + 1.53736159e00, + 1.54960770e00, + 1.56434639e00, + 1.56902273e00, + 1.57404700e00, + 1.59627475e00, + 1.63279146e00, + 1.66601729e00, + 1.67557322e00, + 1.68532662e00, + 1.72284498e00, + 1.77351446e00, + 1.81321380e00, + 1.82386568e00, + 1.83439811e00, + 1.87157611e00, + 1.91428524e00, + 1.94235745e00, + 1.94912944e00, + 1.95552450e00, + 1.97548072e00, + 1.99181022e00, + 1.99678133e00, + 1.99698930e00, + 1.99672126e00, + 1.99142238e00, + 1.97425977e00, + 1.95318876e00, + 1.94640747e00, + 1.93912560e00, + 1.90723862e00, + 1.85469384e00, + 1.80586461e00, + 1.79157553e00, + 1.77675767e00, + 1.71650563e00, + 1.62652338e00, + 1.54835936e00, + 1.52611510e00, + 1.50340802e00, + 1.41535923e00, + 1.29372722e00, + 1.19538561e00, + 1.16845027e00, + 1.14150204e00, + 1.04306198e00, + 9.21943120e-01, + 8.35634220e-01, + 8.13697318e-01, + 7.92169294e-01, + 7.14666065e-01, + 6.22231124e-01, + 5.58760093e-01, + 5.42994035e-01, + 5.27638109e-01, + 4.73047855e-01, + 4.09347337e-01, + 3.66429173e-01, + 3.55855200e-01, + 3.45601623e-01, + 3.09610923e-01, + 2.68406366e-01, + 2.41017169e-01, + 2.34301495e-01, + 2.27807912e-01, + 2.05218281e-01, + 1.79715374e-01, + 1.62960615e-01, + 1.58876766e-01, + 1.54942802e-01, + 1.41427677e-01, + 1.26563232e-01, + 1.17118160e-01, + 1.14868968e-01, + 1.12728071e-01, + 1.05624097e-01, + 9.84557299e-02, + 9.44808344e-02, + 9.36358519e-02, + 9.28804178e-02, + 9.08415615e-02, + 9.00335035e-02, + 9.08065201e-02, + 9.12144976e-02, + 9.17104157e-02, + 9.43848689e-02, + 9.98127571e-02, + 1.05468918e-01, + 1.07196552e-01, + 1.09024400e-01, + 1.16850090e-01, + 1.29379581e-01, + 1.40759110e-01, + 1.44048120e-01, + 1.47457779e-01, + 1.61451462e-01, + 1.82532621e-01, + 2.00814099e-01, + 2.05989285e-01, + 2.11314763e-01, + 2.32853359e-01, + 2.64595386e-01, + 2.91635511e-01, + 2.99224638e-01, + 3.07008184e-01, + 3.38258820e-01, + 3.83750693e-01, + 4.22063757e-01, + 4.32751871e-01, + 4.43680627e-01, + 4.87198283e-01, + 5.49624470e-01, + 6.01465572e-01, + 6.15820485e-01, + 6.30412117e-01, + 6.87375487e-01, + 7.66460244e-01, + 8.30283417e-01, + 8.47710340e-01, + 8.65207631e-01, + 9.30593835e-01, + 1.01512930e00, + 1.07926290e00, + 1.09625261e00, + 1.11301976e00, + 1.17246183e00, + 1.24233209e00, + 1.29052647e00, + 1.30263879e00, + 1.31436154e00, + 1.35418626e00, + 1.39682255e00, + 1.42284132e00, + 1.42884489e00, + 1.43444363e00, + 1.45171211e00, + 1.46561777e00, + 1.46984373e00, + 1.47003907e00, + ], + [ + 0.00000000e00, + -3.69853872e-05, + -8.38371941e-04, + -3.46533231e-03, + -6.66746507e-03, + -7.68849847e-03, + -8.83255584e-03, + -1.46985317e-02, + -2.60077458e-02, + -3.73719424e-02, + -4.07793805e-02, + -4.43777462e-02, + -5.98551189e-02, + -8.44521151e-02, + -1.06360344e-01, + -1.12604114e-01, + -1.19028804e-01, + -1.44831438e-01, + -1.82084258e-01, + -2.12994678e-01, + -2.21534824e-01, + -2.30194881e-01, + -2.63716563e-01, + -3.09709927e-01, + -3.46544679e-01, + -3.56576492e-01, + -3.66702404e-01, + -4.05603813e-01, + -4.58428783e-01, + -5.00379502e-01, + -5.11750893e-01, + -5.23208283e-01, + -5.66999155e-01, + -6.25948082e-01, + -6.72529109e-01, + -6.85155148e-01, + -6.97865587e-01, + -7.46286656e-01, + -8.11049720e-01, + -8.61690577e-01, + -8.75295808e-01, + -8.88942417e-01, + -9.40485507e-01, + -1.00830794e00, + -1.06071058e00, + -1.07475359e00, + -1.08883098e00, + -1.14196938e00, + -1.21265328e00, + -1.26859526e00, + -1.28385569e00, + -1.29919882e00, + -1.35686588e00, + -1.43254040e00, + -1.49101266e00, + -1.50666700e00, + -1.52224141e00, + -1.57892342e00, + -1.64847044e00, + -1.69837862e00, + -1.71119877e00, + -1.72384419e00, + -1.76992481e00, + -1.82724101e00, + -1.86944804e00, + -1.88048734e00, + -1.89144961e00, + -1.93200404e00, + -1.98362784e00, + -2.02229775e00, + -2.03247896e00, + -2.04261083e00, + -2.08020291e00, + -2.12833957e00, + -2.16465355e00, + -2.17425823e00, + -2.18383389e00, + -2.21952096e00, + -2.26555730e00, + -2.30048373e00, + -2.30973997e00, + -2.31897454e00, + -2.35343101e00, + -2.39788741e00, + -2.43154523e00, + -2.44044773e00, + -2.44932129e00, + -2.48235304e00, + -2.52475640e00, + -2.55667488e00, + -2.56508894e00, + -2.57346403e00, + -2.60454108e00, + -2.64420153e00, + -2.67387865e00, + -2.68167631e00, + -2.68942782e00, + -2.71810862e00, + -2.75451948e00, + -2.78161920e00, + -2.78871806e00, + -2.79576644e00, + -2.82177689e00, + -2.85463030e00, + -2.87893907e00, + -2.88528286e00, + -2.89157116e00, + -2.91468371e00, + -2.94362591e00, + -2.96481876e00, + -2.97031336e00, + -2.97574440e00, + -2.99556735e00, + -3.02003833e00, + -3.03766100e00, + -3.04218284e00, + -3.04663166e00, + -3.06267949e00, + -3.08200669e00, + -3.09551393e00, + -3.09891340e00, + -3.10222757e00, + -3.11388962e00, + -3.12716503e00, + -3.13576099e00, + -3.13781028e00, + -3.13975605e00, + -3.14610970e00, + -3.15207499e00, + -3.15481161e00, + -3.15526781e00, + -3.15562439e00, + -3.15622576e00, + -3.15530450e00, + -3.15332736e00, + -3.15261784e00, + -3.15188007e00, + -3.14930693e00, + -3.14646739e00, + -3.14463929e00, + -3.14418617e00, + -3.14377182e00, + -3.14277291e00, + -3.14226273e00, + -3.14204669e00, + -3.14197530e00, + -3.14190633e00, + -3.14171569e00, + -3.14160546e00, + -3.14159152e00, + -3.14159265e00, + ], + ], + decimal=6, + ) + + +def test_example_arm26_pendulum_swingup_muscle_algebraic(): + """Test the holonomic_constraints/two_pendulums example""" + from bioptim.examples.toy_examples.holonomic_constraints import arm26_pendulum_swingup_muscle_algebraic + + bioptim_folder = TestUtils.bioptim_folder() + + # --- Prepare the ocp --- # + ocp, model = arm26_pendulum_swingup_muscle_algebraic.prepare_ocp( + biorbd_model_path=bioptim_folder + "/examples/models/arm26_w_pendulum.bioMod", + n_shooting=30, + final_time=1, + expand_dynamics=False, + ) + + # --- Solve the ocp --- # + sol = ocp.solve(Solver.IPOPT()) + states = sol.decision_states(to_merge=SolutionMerge.NODES) + + npt.assert_almost_equal( + states["q_u"], + [ + [ + 0.00000000e00, + -3.20620080e-05, + -7.22737278e-04, + -2.95404501e-03, + -5.63924692e-03, + -6.49005147e-03, + -7.44120424e-03, + -1.23041824e-02, + -2.16864947e-02, + -3.11639947e-02, + -3.40183367e-02, + -3.70432103e-02, + -5.02028401e-02, + -7.15901095e-02, + -9.11251000e-02, + -9.67782100e-02, + -1.02636806e-01, + -1.26605153e-01, + -1.62430055e-01, + -1.93215657e-01, + -2.01885869e-01, + -2.10736867e-01, + -2.45435943e-01, + -2.94093000e-01, + -3.33840955e-01, + -3.44774564e-01, + -3.55823516e-01, + -3.98072073e-01, + -4.54910190e-01, + -4.99606632e-01, + -5.11656422e-01, + -5.23739586e-01, + -5.69147128e-01, + -6.28166932e-01, + -6.72816216e-01, + -6.84573949e-01, + -6.96250019e-01, + -7.39135696e-01, + -7.92336556e-01, + -8.30465979e-01, + -8.40175985e-01, + -8.49677979e-01, + -8.83314439e-01, + -9.21855611e-01, + -9.46785879e-01, + -9.52697267e-01, + -9.58289149e-01, + -9.76331668e-01, + -9.92490110e-01, + -9.98834344e-01, + -9.99597271e-01, + -9.99995036e-01, + -9.98551302e-01, + -9.89799356e-01, + -9.77968499e-01, + -9.74086523e-01, + -9.69981207e-01, + -9.53456348e-01, + -9.29990775e-01, + -9.11227010e-01, + -9.06184652e-01, + -9.01122130e-01, + -8.81898582e-01, + -8.56759436e-01, + -8.37838636e-01, + -8.32883913e-01, + -8.27946707e-01, + -8.09404808e-01, + -7.85340850e-01, + -7.67085834e-01, + -7.62252280e-01, + -7.57419485e-01, + -7.39182477e-01, + -7.15118888e-01, + -6.96416053e-01, + -6.91386302e-01, + -6.86330724e-01, + -6.67071745e-01, + -6.41203964e-01, + -6.20752421e-01, + -6.15205311e-01, + -6.09614819e-01, + -5.88228319e-01, + -5.59333655e-01, + -5.36409182e-01, + -5.30185937e-01, + -5.23914212e-01, + -4.99944540e-01, + -4.67660510e-01, + -4.42166181e-01, + -4.35267039e-01, + -4.28324533e-01, + -4.01895309e-01, + -3.66576993e-01, + -3.38924873e-01, + -3.31478891e-01, + -3.24002617e-01, + -2.95697827e-01, + -2.58261881e-01, + -2.29261624e-01, + -2.21498827e-01, + -2.13725121e-01, + -1.84491458e-01, + -1.46302787e-01, + -1.17083535e-01, + -1.09314554e-01, + -1.01558643e-01, + -7.26247823e-02, + -3.53760500e-02, + -7.29152103e-03, + 1.15378024e-04, + 7.48066662e-03, + 3.46628706e-02, + 6.89600515e-02, + 9.42902119e-02, + 1.00893971e-01, + 1.07423714e-01, + 1.31152927e-01, + 1.60235905e-01, + 1.81077138e-01, + 1.86419341e-01, + 1.91658860e-01, + 2.10283440e-01, + 2.32176279e-01, + 2.47191245e-01, + 2.50945089e-01, + 2.54592465e-01, + 2.67307303e-01, + 2.81730339e-01, + 2.91261524e-01, + 2.93592960e-01, + 2.95870417e-01, + 3.04207591e-01, + 3.14515826e-01, + 3.21882445e-01, + 3.23752774e-01, + 3.25632869e-01, + 3.33139282e-01, + 3.43495577e-01, + 3.51382102e-01, + 3.53423056e-01, + 3.55450956e-01, + 3.63029886e-01, + 3.72308078e-01, + 3.78486893e-01, + 3.79950859e-01, + 3.81334667e-01, + 3.85790931e-01, + 3.89592744e-01, + 3.90787335e-01, + 3.90843002e-01, + ], + [ + 1.57079633e00, + 1.57070683e00, + 1.56877430e00, + 1.56244859e00, + 1.55465633e00, + 1.55214408e00, + 1.54963079e00, + 1.54182624e00, + 1.53548604e00, + 1.53352726e00, + 1.53342243e00, + 1.53358406e00, + 1.53736159e00, + 1.54960770e00, + 1.56434639e00, + 1.56902273e00, + 1.57404700e00, + 1.59627475e00, + 1.63279146e00, + 1.66601729e00, + 1.67557322e00, + 1.68532662e00, + 1.72284498e00, + 1.77351446e00, + 1.81321380e00, + 1.82386568e00, + 1.83439811e00, + 1.87157611e00, + 1.91428524e00, + 1.94235745e00, + 1.94912944e00, + 1.95552450e00, + 1.97548072e00, + 1.99181022e00, + 1.99678133e00, + 1.99698930e00, + 1.99672126e00, + 1.99142238e00, + 1.97425977e00, + 1.95318876e00, + 1.94640747e00, + 1.93912560e00, + 1.90723862e00, + 1.85469384e00, + 1.80586461e00, + 1.79157553e00, + 1.77675767e00, + 1.71650563e00, + 1.62652338e00, + 1.54835936e00, + 1.52611510e00, + 1.50340802e00, + 1.41535923e00, + 1.29372722e00, + 1.19538561e00, + 1.16845027e00, + 1.14150204e00, + 1.04306198e00, + 9.21943120e-01, + 8.35634220e-01, + 8.13697318e-01, + 7.92169294e-01, + 7.14666065e-01, + 6.22231124e-01, + 5.58760093e-01, + 5.42994035e-01, + 5.27638109e-01, + 4.73047855e-01, + 4.09347337e-01, + 3.66429173e-01, + 3.55855200e-01, + 3.45601623e-01, + 3.09610923e-01, + 2.68406366e-01, + 2.41017169e-01, + 2.34301495e-01, + 2.27807912e-01, + 2.05218281e-01, + 1.79715374e-01, + 1.62960615e-01, + 1.58876766e-01, + 1.54942802e-01, + 1.41427677e-01, + 1.26563232e-01, + 1.17118160e-01, + 1.14868968e-01, + 1.12728071e-01, + 1.05624097e-01, + 9.84557299e-02, + 9.44808344e-02, + 9.36358519e-02, + 9.28804178e-02, + 9.08415615e-02, + 9.00335035e-02, + 9.08065201e-02, + 9.12144976e-02, + 9.17104157e-02, + 9.43848689e-02, + 9.98127571e-02, + 1.05468918e-01, + 1.07196552e-01, + 1.09024400e-01, + 1.16850090e-01, + 1.29379581e-01, + 1.40759110e-01, + 1.44048120e-01, + 1.47457779e-01, + 1.61451462e-01, + 1.82532621e-01, + 2.00814099e-01, + 2.05989285e-01, + 2.11314763e-01, + 2.32853359e-01, + 2.64595386e-01, + 2.91635511e-01, + 2.99224638e-01, + 3.07008184e-01, + 3.38258820e-01, + 3.83750693e-01, + 4.22063757e-01, + 4.32751871e-01, + 4.43680627e-01, + 4.87198283e-01, + 5.49624470e-01, + 6.01465572e-01, + 6.15820485e-01, + 6.30412117e-01, + 6.87375487e-01, + 7.66460244e-01, + 8.30283417e-01, + 8.47710340e-01, + 8.65207631e-01, + 9.30593835e-01, + 1.01512930e00, + 1.07926290e00, + 1.09625261e00, + 1.11301976e00, + 1.17246183e00, + 1.24233209e00, + 1.29052647e00, + 1.30263879e00, + 1.31436154e00, + 1.35418626e00, + 1.39682255e00, + 1.42284132e00, + 1.42884489e00, + 1.43444363e00, + 1.45171211e00, + 1.46561777e00, + 1.46984373e00, + 1.47003907e00, + ], + [ + 0.00000000e00, + -3.69853872e-05, + -8.38371941e-04, + -3.46533231e-03, + -6.66746507e-03, + -7.68849847e-03, + -8.83255584e-03, + -1.46985317e-02, + -2.60077458e-02, + -3.73719424e-02, + -4.07793805e-02, + -4.43777462e-02, + -5.98551189e-02, + -8.44521151e-02, + -1.06360344e-01, + -1.12604114e-01, + -1.19028804e-01, + -1.44831438e-01, + -1.82084258e-01, + -2.12994678e-01, + -2.21534824e-01, + -2.30194881e-01, + -2.63716563e-01, + -3.09709927e-01, + -3.46544679e-01, + -3.56576492e-01, + -3.66702404e-01, + -4.05603813e-01, + -4.58428783e-01, + -5.00379502e-01, + -5.11750893e-01, + -5.23208283e-01, + -5.66999155e-01, + -6.25948082e-01, + -6.72529109e-01, + -6.85155148e-01, + -6.97865587e-01, + -7.46286656e-01, + -8.11049720e-01, + -8.61690577e-01, + -8.75295808e-01, + -8.88942417e-01, + -9.40485507e-01, + -1.00830794e00, + -1.06071058e00, + -1.07475359e00, + -1.08883098e00, + -1.14196938e00, + -1.21265328e00, + -1.26859526e00, + -1.28385569e00, + -1.29919882e00, + -1.35686588e00, + -1.43254040e00, + -1.49101266e00, + -1.50666700e00, + -1.52224141e00, + -1.57892342e00, + -1.64847044e00, + -1.69837862e00, + -1.71119877e00, + -1.72384419e00, + -1.76992481e00, + -1.82724101e00, + -1.86944804e00, + -1.88048734e00, + -1.89144961e00, + -1.93200404e00, + -1.98362784e00, + -2.02229775e00, + -2.03247896e00, + -2.04261083e00, + -2.08020291e00, + -2.12833957e00, + -2.16465355e00, + -2.17425823e00, + -2.18383389e00, + -2.21952096e00, + -2.26555730e00, + -2.30048373e00, + -2.30973997e00, + -2.31897454e00, + -2.35343101e00, + -2.39788741e00, + -2.43154523e00, + -2.44044773e00, + -2.44932129e00, + -2.48235304e00, + -2.52475640e00, + -2.55667488e00, + -2.56508894e00, + -2.57346403e00, + -2.60454108e00, + -2.64420153e00, + -2.67387865e00, + -2.68167631e00, + -2.68942782e00, + -2.71810862e00, + -2.75451948e00, + -2.78161920e00, + -2.78871806e00, + -2.79576644e00, + -2.82177689e00, + -2.85463030e00, + -2.87893907e00, + -2.88528286e00, + -2.89157116e00, + -2.91468371e00, + -2.94362591e00, + -2.96481876e00, + -2.97031336e00, + -2.97574440e00, + -2.99556735e00, + -3.02003833e00, + -3.03766100e00, + -3.04218284e00, + -3.04663166e00, + -3.06267949e00, + -3.08200669e00, + -3.09551393e00, + -3.09891340e00, + -3.10222757e00, + -3.11388962e00, + -3.12716503e00, + -3.13576099e00, + -3.13781028e00, + -3.13975605e00, + -3.14610970e00, + -3.15207499e00, + -3.15481161e00, + -3.15526781e00, + -3.15562439e00, + -3.15622576e00, + -3.15530450e00, + -3.15332736e00, + -3.15261784e00, + -3.15188007e00, + -3.14930693e00, + -3.14646739e00, + -3.14463929e00, + -3.14418617e00, + -3.14377182e00, + -3.14277291e00, + -3.14226273e00, + -3.14204669e00, + -3.14197530e00, + -3.14190633e00, + -3.14171569e00, + -3.14160546e00, + -3.14159152e00, + -3.14159265e00, + ], + ], + decimal=6, + )