From 764699877b365f1de2d3b79f40ce1aa560cc527e Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 18 Sep 2025 21:05:30 +0200 Subject: [PATCH 1/8] Fixed bug in holonomic torque dynamics Modified the corresponding example so it uses collocation and actually covers the code Also commented out a lot of the compute_all_states function which was unrealiable and not used in the example. We should discuss what to do with this --- .../torque_dynamics_holonomic.py | 4 +- .../holonomic_constraints/two_pendulums.py | 61 ++++++++++--------- 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py index 7e6b1b238..a6bc017d5 100644 --- a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py +++ b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py @@ -43,8 +43,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/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index e62689941..d67973676 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -44,44 +44,44 @@ def compute_all_states(sol, bio_model: HolonomicTorqueBiorbdModel): """ states = sol.decision_states(to_merge=SolutionMerge.NODES) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + # controls = sol.decision_controls(to_merge=SolutionMerge.NODES) n = states["q_u"].shape[1] - n_tau = controls["tau"].shape[1] + # n_tau = controls["tau"].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_tau + 1)) + # 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_tau + 1)) - 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, :] + # 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, 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() - 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[:, i], 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 + # 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[:, i], 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 def prepare_ocp( @@ -137,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=OdeSolver.COLLOCATION(), expand_dynamics=expand_dynamics)) # Path Constraints constraints = ConstraintList() @@ -205,7 +205,8 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + # q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + q = compute_all_states(sol, bio_model) viewer = "pyorerun" if viewer == "bioviz": From 14bf9f8782c2ce25016841cca18a369d13e46235 Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 25 Sep 2025 20:39:25 +0200 Subject: [PATCH 2/8] Restored example to original state. Added ode_solver as a kwarg for testing --- .../holonomic_constraints/two_pendulums.py | 62 +++++++++---------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/bioptim/examples/holonomic_constraints/two_pendulums.py b/bioptim/examples/holonomic_constraints/two_pendulums.py index d67973676..62f6644d6 100644 --- a/bioptim/examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/holonomic_constraints/two_pendulums.py @@ -44,44 +44,44 @@ def compute_all_states(sol, bio_model: HolonomicTorqueBiorbdModel): """ states = sol.decision_states(to_merge=SolutionMerge.NODES) - # controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) n = states["q_u"].shape[1] - # n_tau = controls["tau"].shape[1] + n_tau = controls["tau"].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_tau + 1)) + 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_tau + 1)) - # 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, :] + 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, 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() - # 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[:, i], 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 + 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[:, i], 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 def prepare_ocp( @@ -89,6 +89,7 @@ def prepare_ocp( n_shooting: int = 30, final_time: float = 1, expand_dynamics: bool = False, + ode_solver=OdeSolver.RK4(), ) -> (HolonomicTorqueBiorbdModel, OptimalControlProgram): """ Prepare the program @@ -137,7 +138,7 @@ def prepare_ocp( # Dynamics dynamics = DynamicsOptionsList() - dynamics.add(DynamicsOptions(ode_solver=OdeSolver.COLLOCATION(), expand_dynamics=expand_dynamics)) + dynamics.add(DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics)) # Path Constraints constraints = ConstraintList() @@ -205,8 +206,7 @@ def main(): print(sol.real_time_to_optimize) # --- Show results --- # - # q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) - q = compute_all_states(sol, bio_model) + q, qdot, qddot, lambdas = compute_all_states(sol, bio_model) viewer = "pyorerun" if viewer == "bioviz": From 1f8e4afb297b487a73575f5090e58b889cd0631a Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 25 Sep 2025 20:58:52 +0200 Subject: [PATCH 3/8] Updated test to also consider collocation --- tests/shard1/test_biorbd_model_holonomic.py | 358 +++++++++++++++++++- 1 file changed, 348 insertions(+), 10 deletions(-) diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 6a0ce7ec5..0aa0def19 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -1,10 +1,19 @@ import platform + import numpy as np import numpy.testing as npt import pytest from casadi import DM, MX -from bioptim import HolonomicBiorbdModel, HolonomicConstraintsFcn, HolonomicConstraintsList, Solver, SolutionMerge +from bioptim import ( + HolonomicBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + OdeSolver, + SolutionMerge, + Solver, +) + from ..utils import TestUtils @@ -194,7 +203,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.holonomic_constraints import two_pendulums @@ -206,20 +216,348 @@ 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.53993574, + 1.53854879, + 1.53402211, + 1.52847541, + 1.52669413, + 1.52479253, + 1.51664893, + 1.50364381, + 1.49186773, + 1.48846757, + 1.48496766, + 1.47103762, + 1.45101122, + 1.43428871, + 1.4296383, + 1.42491613, + 1.40664125, + 1.38154687, + 1.3614185, + 1.35593186, + 1.35040012, + 1.32929443, + 1.30098709, + 1.27874555, + 1.27274408, + 1.26671486, + 1.24387107, + 1.21357331, + 1.18999023, + 1.18365498, + 1.17730007, + 1.15329038, + 1.12158414, + 1.0969888, + 1.09039172, + 1.08377726, + 1.05880536, + 1.02586166, + 1.00032296, + 0.99347443, + 0.98660795, + 0.96068097, + 0.92646613, + 0.89993169, + 0.89281453, + 0.88567777, + 0.85871999, + 0.82312202, + 0.79549941, + 0.78808837, + 0.78065602, + 0.75257307, + 0.71547266, + 0.68667555, + 0.67894863, + 0.67119919, + 0.64191606, + 0.60323049, + 0.57320863, + 0.56515452, + 0.55707768, + 0.52656501, + 0.48627929, + 0.45504017, + 0.44666377, + 0.43826568, + 0.40655829, + 0.36474618, + 0.33236827, + 0.3236937, + 0.31499985, + 0.28220556, + 0.23903604, + 0.20566982, + 0.19674014, + 0.18779476, + 0.1540893, + 0.10981354, + 0.07566688, + 0.06653957, + 0.05740078, + 0.02300652, + -0.02207661, + -0.05676907, + -0.06603084, + -0.07529994, + -0.11015022, + -0.15574627, + -0.19076596, + -0.20010498, + -0.20944811, + -0.24455526, + -0.29043121, + -0.32561834, + -0.3349946, + -0.34437316, + -0.37960761, + -0.42562773, + -0.46090085, + -0.47029563, + -0.47969201, + -0.51499618, + -0.56110295, + -0.59642867, + -0.60583432, + -0.61524053, + -0.65057462, + -0.6966921, + -0.73199308, + -0.74138606, + -0.75077582, + -0.78600437, + -0.83187235, + -0.86688888, + -0.87619157, + -0.88548196, + -0.92023137, + -0.9652234, + -0.99937677, + -1.00842136, + -1.01743808, + -1.05099004, + -1.09401872, + -1.12636044, + -1.13487707, + -1.14334488, + -1.17463259, + -1.21420857, + -1.243506, + -1.25115111, + -1.2587228, + -1.28643644, + -1.32083863, + -1.34578378, + -1.35221472, + -1.35855226, + -1.38148194, + -1.40935091, + -1.42914168, + -1.43418725, + -1.43913685, + -1.45685819, + -1.47799261, + -1.49270762, + -1.49641559, + -1.50003391, + -1.51281478, + -1.52762174, + -1.53755795, + -1.54, + ], + [ + 1.54, + 1.54008014, + 1.54180759, + 1.54746234, + 1.55441753, + 1.55665635, + 1.55904689, + 1.56927654, + 1.58558391, + 1.60029652, + 1.60453126, + 1.60888033, + 1.62606456, + 1.65040709, + 1.67039788, + 1.67590201, + 1.68146392, + 1.70270917, + 1.73117772, + 1.75344302, + 1.75942645, + 1.76542049, + 1.78792093, + 1.81720155, + 1.83950479, + 1.84541875, + 1.8513145, + 1.87322903, + 1.9012715, + 1.9223014, + 1.92783272, + 1.93333025, + 1.95363296, + 1.97931665, + 1.9983619, + 2.00334038, + 2.00827631, + 2.02640436, + 2.04909814, + 2.0657394, + 2.07006118, + 2.07433428, + 2.08992502, + 2.10918766, + 2.12310322, + 2.1266842, + 2.13021066, + 2.14294933, + 2.15836564, + 2.16923076, + 2.1719832, + 2.1746746, + 2.18422193, + 2.19533145, + 2.20277982, + 2.20460426, + 2.20636031, + 2.21233241, + 2.21861873, + 2.22224541, + 2.22303296, + 2.22374442, + 2.22572876, + 2.2266518, + 2.2260458, + 2.2256879, + 2.22524674, + 2.22284361, + 2.21790062, + 2.21269591, + 2.21109843, + 2.20941206, + 2.20228899, + 2.19108649, + 2.18101696, + 2.17811468, + 2.17512034, + 2.16306384, + 2.14537972, + 2.1303221, + 2.12608947, + 2.1217645, + 2.10471399, + 2.08053006, + 2.06051993, + 2.05497399, + 2.04933816, + 2.02738735, + 1.99687372, + 1.9720795, + 1.96527088, + 1.95837641, + 1.9317282, + 1.89516443, + 1.86581045, + 1.85780023, + 1.849708, + 1.81858126, + 1.77622598, + 1.74248584, + 1.73331626, + 1.72406663, + 1.68859438, + 1.64057373, + 1.60250505, + 1.59218549, + 1.58178627, + 1.54199289, + 1.48833052, + 1.44594882, + 1.43448352, + 1.42294123, + 1.37889431, + 1.31979443, + 1.27335769, + 1.26083154, + 1.24824113, + 1.2004178, + 1.13681059, + 1.08728415, + 1.07399278, + 1.06066803, + 1.0104253, + 0.94451277, + 0.89391129, + 0.88043788, + 0.86697862, + 0.81668317, + 0.75175895, + 0.70267815, + 0.68971282, + 0.67680085, + 0.62886792, + 0.5675696, + 0.52147556, + 0.50931008, + 0.49719175, + 0.45211244, + 0.39403838, + 0.34989285, + 0.33816079, + 0.3264395, + 0.28252367, + 0.2252666, + 0.18131209, + 0.16958211, + 0.15784574, + 0.11375566, + 0.05610692, + 0.01181684, + 0.0, + ], + ], + decimal=6, + ) def test_example_two_pendulums_algebraic(): From 22db7b59b41fef4e8bbe0762d862a2077717cf97 Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 25 Sep 2025 21:11:11 +0200 Subject: [PATCH 4/8] Working on test --- tests/shard1/test_biorbd_model_holonomic.py | 396 +++++--------------- 1 file changed, 98 insertions(+), 298 deletions(-) diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 0aa0def19..56ccb4420 100644 --- a/tests/shard1/test_biorbd_model_holonomic.py +++ b/tests/shard1/test_biorbd_model_holonomic.py @@ -251,308 +251,108 @@ def test_example_two_pendulums(ode_solver): [ [ 1.54, - 1.53993574, - 1.53854879, - 1.53402211, - 1.52847541, - 1.52669413, - 1.52479253, - 1.51664893, - 1.50364381, - 1.49186773, - 1.48846757, - 1.48496766, - 1.47103762, - 1.45101122, - 1.43428871, - 1.4296383, - 1.42491613, - 1.40664125, - 1.38154687, - 1.3614185, - 1.35593186, - 1.35040012, - 1.32929443, - 1.30098709, - 1.27874555, - 1.27274408, - 1.26671486, - 1.24387107, - 1.21357331, - 1.18999023, - 1.18365498, - 1.17730007, - 1.15329038, - 1.12158414, - 1.0969888, - 1.09039172, - 1.08377726, - 1.05880536, - 1.02586166, - 1.00032296, - 0.99347443, - 0.98660795, - 0.96068097, - 0.92646613, - 0.89993169, - 0.89281453, - 0.88567777, - 0.85871999, - 0.82312202, - 0.79549941, - 0.78808837, - 0.78065602, - 0.75257307, - 0.71547266, - 0.68667555, - 0.67894863, - 0.67119919, - 0.64191606, - 0.60323049, - 0.57320863, - 0.56515452, - 0.55707768, - 0.52656501, - 0.48627929, - 0.45504017, - 0.44666377, - 0.43826568, - 0.40655829, - 0.36474618, - 0.33236827, - 0.3236937, - 0.31499985, - 0.28220556, - 0.23903604, - 0.20566982, - 0.19674014, - 0.18779476, - 0.1540893, - 0.10981354, - 0.07566688, - 0.06653957, - 0.05740078, - 0.02300652, - -0.02207661, - -0.05676907, - -0.06603084, - -0.07529994, - -0.11015022, - -0.15574627, - -0.19076596, - -0.20010498, - -0.20944811, - -0.24455526, - -0.29043121, - -0.32561834, - -0.3349946, - -0.34437316, - -0.37960761, - -0.42562773, - -0.46090085, - -0.47029563, - -0.47969201, - -0.51499618, - -0.56110295, - -0.59642867, - -0.60583432, - -0.61524053, - -0.65057462, - -0.6966921, - -0.73199308, - -0.74138606, - -0.75077582, - -0.78600437, - -0.83187235, - -0.86688888, - -0.87619157, - -0.88548196, - -0.92023137, - -0.9652234, - -0.99937677, - -1.00842136, - -1.01743808, - -1.05099004, - -1.09401872, - -1.12636044, - -1.13487707, - -1.14334488, - -1.17463259, - -1.21420857, - -1.243506, - -1.25115111, - -1.2587228, - -1.28643644, - -1.32083863, - -1.34578378, - -1.35221472, - -1.35855226, - -1.38148194, - -1.40935091, - -1.42914168, - -1.43418725, - -1.43913685, - -1.45685819, - -1.47799261, - -1.49270762, - -1.49641559, - -1.50003391, - -1.51281478, - -1.52762174, - -1.53755795, + 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.54008014, - 1.54180759, - 1.54746234, - 1.55441753, - 1.55665635, - 1.55904689, - 1.56927654, - 1.58558391, - 1.60029652, - 1.60453126, - 1.60888033, - 1.62606456, - 1.65040709, - 1.67039788, - 1.67590201, - 1.68146392, - 1.70270917, - 1.73117772, - 1.75344302, - 1.75942645, - 1.76542049, - 1.78792093, - 1.81720155, - 1.83950479, - 1.84541875, - 1.8513145, - 1.87322903, - 1.9012715, - 1.9223014, - 1.92783272, - 1.93333025, - 1.95363296, - 1.97931665, - 1.9983619, - 2.00334038, - 2.00827631, - 2.02640436, - 2.04909814, - 2.0657394, - 2.07006118, - 2.07433428, - 2.08992502, - 2.10918766, - 2.12310322, - 2.1266842, - 2.13021066, - 2.14294933, - 2.15836564, - 2.16923076, - 2.1719832, - 2.1746746, - 2.18422193, - 2.19533145, - 2.20277982, - 2.20460426, - 2.20636031, - 2.21233241, - 2.21861873, - 2.22224541, - 2.22303296, - 2.22374442, - 2.22572876, - 2.2266518, - 2.2260458, - 2.2256879, - 2.22524674, - 2.22284361, - 2.21790062, - 2.21269591, - 2.21109843, - 2.20941206, - 2.20228899, - 2.19108649, - 2.18101696, - 2.17811468, - 2.17512034, - 2.16306384, - 2.14537972, - 2.1303221, - 2.12608947, - 2.1217645, - 2.10471399, - 2.08053006, - 2.06051993, - 2.05497399, - 2.04933816, - 2.02738735, - 1.99687372, - 1.9720795, - 1.96527088, - 1.95837641, - 1.9317282, - 1.89516443, - 1.86581045, - 1.85780023, - 1.849708, - 1.81858126, - 1.77622598, - 1.74248584, - 1.73331626, - 1.72406663, - 1.68859438, - 1.64057373, - 1.60250505, - 1.59218549, - 1.58178627, - 1.54199289, - 1.48833052, - 1.44594882, - 1.43448352, - 1.42294123, - 1.37889431, - 1.31979443, - 1.27335769, - 1.26083154, - 1.24824113, - 1.2004178, - 1.13681059, - 1.08728415, - 1.07399278, - 1.06066803, - 1.0104253, - 0.94451277, - 0.89391129, - 0.88043788, - 0.86697862, - 0.81668317, - 0.75175895, - 0.70267815, - 0.68971282, - 0.67680085, - 0.62886792, - 0.5675696, - 0.52147556, - 0.50931008, - 0.49719175, - 0.45211244, - 0.39403838, - 0.34989285, - 0.33816079, - 0.3264395, - 0.28252367, - 0.2252666, - 0.18131209, - 0.16958211, - 0.15784574, - 0.11375566, - 0.05610692, - 0.01181684, + 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, ], ], From 17c79189d38797e5f7fc3ec8689259c26cf8f216 Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 25 Sep 2025 21:19:50 +0200 Subject: [PATCH 5/8] Adding changes from PR 1012 to avoid overwriting if PRs are merged in a different order --- bioptim/models/biorbd/viewer_pyorerun.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bioptim/models/biorbd/viewer_pyorerun.py b/bioptim/models/biorbd/viewer_pyorerun.py index 957c5829c..9713b35ef 100644 --- a/bioptim/models/biorbd/viewer_pyorerun.py +++ b/bioptim/models/biorbd/viewer_pyorerun.py @@ -87,7 +87,7 @@ def launch_rerun( solution: "SolutionData", show_now: Bool = True, tracked_markers: list[NpArray] = None, - models: BiorbdModel | list[BiorbdModel] = None, + models: BiorbdModel | list[BiorbdModel] | list[MultiBiorbdModel] = None, **kwargs: Any, ): if not isinstance(solution, (list, tuple)): From 316aa6e2d9121f2e15b64ab64ff7481a29b693ff Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 18 Sep 2025 21:53:19 +0200 Subject: [PATCH 6/8] Added more examples of using holonomic constraints Specifically using more objects, or more than one constraint, on different objects or on the same object pair. This should help newer users of holonomic constraints understand this better --- bioptim/examples/models/3bar.bioMod | 98 +++++++++ bioptim/examples/models/4bar.bioMod | 132 +++++++++++ .../examples/models/pendulum_rotule.bioMod | 35 +++ .../examples/models/two_pendulums_2.bioMod | 77 +++++++ .../models/two_pendulums_rotule.bioMod | 66 ++++++ .../holonomic_constraints/four_bar.py | 194 +++++++++++++++++ .../holonomic_constraints/three_bar.py | 202 +++++++++++++++++ .../two_pendulums_2constraint.py | 180 +++++++++++++++ .../two_pendulums_2constraint_4DOF.py | 206 ++++++++++++++++++ .../two_pendulums_rotule.py | 174 +++++++++++++++ 10 files changed, 1364 insertions(+) create mode 100644 bioptim/examples/models/3bar.bioMod create mode 100644 bioptim/examples/models/4bar.bioMod create mode 100644 bioptim/examples/models/pendulum_rotule.bioMod create mode 100644 bioptim/examples/models/two_pendulums_2.bioMod create mode 100644 bioptim/examples/models/two_pendulums_rotule.bioMod create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/four_bar.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/three_bar.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py 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/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/four_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py new file mode 100644 index 000000000..99e436c56 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py @@ -0,0 +1,194 @@ +""" +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 platform +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 three_bar import compute_all_states + + +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 = "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_states(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..037670167 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py @@ -0,0 +1,202 @@ +""" +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 platform +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 two_pendulums import compute_all_states + + +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_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 = "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, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + q = compute_all_states(sol, bio_model) + + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + sol.graphs() + + +if __name__ == "__main__": + main() 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..30006a4c2 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py @@ -0,0 +1,180 @@ +""" +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 platform +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 two_pendulums_2constraint_4DOF import compute_all_states + + +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 = "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_states(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..95b425003 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py @@ -0,0 +1,206 @@ +""" +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 platform +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, +) + + +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="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 = "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_states(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_rotule.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py new file mode 100644 index 000000000..5c299de17 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py @@ -0,0 +1,174 @@ +""" +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. +""" + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicTorqueBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OptimalControlProgram, + Solver, + SolutionMerge, + OdeSolver, + TorqueBiorbdModel, +) + +import numpy as np + +from three_bar import compute_all_states + + +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 = "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_states(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() From 6ab835c4fc05635f3315a992531bff3c5dd5d6b9 Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 8 Jan 2026 15:19:29 +0100 Subject: [PATCH 7/8] Updating to current main and separating content from the examples PR --- bioptim/examples/models/3bar.bioMod | 98 -------- bioptim/examples/models/4bar.bioMod | 132 ---------- .../examples/models/pendulum_rotule.bioMod | 35 --- .../examples/models/two_pendulums_2.bioMod | 77 ------ .../models/two_pendulums_rotule.bioMod | 66 ----- .../holonomic_constraints/four_bar.py | 194 --------------- .../holonomic_constraints/three_bar.py | 202 --------------- .../holonomic_constraints/two_pendulums.py | 231 ------------------ .../two_pendulums_2constraint.py | 180 -------------- .../two_pendulums_2constraint_4DOF.py | 206 ---------------- .../two_pendulums_rotule.py | 174 ------------- 11 files changed, 1595 deletions(-) delete mode 100644 bioptim/examples/models/3bar.bioMod delete mode 100644 bioptim/examples/models/4bar.bioMod delete mode 100644 bioptim/examples/models/pendulum_rotule.bioMod delete mode 100644 bioptim/examples/models/two_pendulums_2.bioMod delete mode 100644 bioptim/examples/models/two_pendulums_rotule.bioMod delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/four_bar.py delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/three_bar.py delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py delete mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py diff --git a/bioptim/examples/models/3bar.bioMod b/bioptim/examples/models/3bar.bioMod deleted file mode 100644 index 4f744c4f9..000000000 --- a/bioptim/examples/models/3bar.bioMod +++ /dev/null @@ -1,98 +0,0 @@ -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 deleted file mode 100644 index 541daf8f3..000000000 --- a/bioptim/examples/models/4bar.bioMod +++ /dev/null @@ -1,132 +0,0 @@ -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/pendulum_rotule.bioMod b/bioptim/examples/models/pendulum_rotule.bioMod deleted file mode 100644 index 0b37f23db..000000000 --- a/bioptim/examples/models/pendulum_rotule.bioMod +++ /dev/null @@ -1,35 +0,0 @@ -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 deleted file mode 100644 index 6f357735c..000000000 --- a/bioptim/examples/models/two_pendulums_2.bioMod +++ /dev/null @@ -1,77 +0,0 @@ -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 deleted file mode 100644 index 0146a5cd7..000000000 --- a/bioptim/examples/models/two_pendulums_rotule.bioMod +++ /dev/null @@ -1,66 +0,0 @@ -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/four_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py deleted file mode 100644 index 99e436c56..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py +++ /dev/null @@ -1,194 +0,0 @@ -""" -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 platform -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 three_bar import compute_all_states - - -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 = "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_states(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 deleted file mode 100644 index 037670167..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py +++ /dev/null @@ -1,202 +0,0 @@ -""" -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 platform -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 two_pendulums import compute_all_states - - -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_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 = "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, qdot, qddot, lambdas = compute_all_states(sol, bio_model) - q = compute_all_states(sol, bio_model) - - import bioviz - - viz = bioviz.Viz(model_path) - viz.load_movement(q) - viz.exec() - - 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 deleted file mode 100644 index 62f6644d6..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py +++ /dev/null @@ -1,231 +0,0 @@ -""" -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 platform -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, -) - - -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) - controls = sol.decision_controls(to_merge=SolutionMerge.NODES) - - n = states["q_u"].shape[1] - n_tau = controls["tau"].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_tau + 1)) - - 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, 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() - 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[:, i], 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 - - -def prepare_ocp( - biorbd_model_path: str, - n_shooting: int = 30, - final_time: float = 1, - expand_dynamics: bool = False, - ode_solver=OdeSolver.RK4(), -) -> (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(1, 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, 3], - dependent_joint_index=[1, 2], - ) - - # 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=ode_solver, 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, 1], to_first=[0, 3]) - variable_bimapping.add("qdot", to_second=[0, None, None, 1], to_first=[0, 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=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.54, 1.54]) - x_init.add("qdot_u", [0, 0]) - x_bounds["q_u"][:, 0] = [1.54, 1.54] - x_bounds["qdot_u"][:, 0] = [0, 0] - x_bounds["q_u"][0, -1] = -1.54 - 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, 1], to_first=[0, 3]) - 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, - ), - bio_model, - ) - - -def main(): - """ - Runs the optimization and animates it - """ - - model_path = "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, qdot, qddot, lambdas = compute_all_states(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.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py deleted file mode 100644 index 30006a4c2..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py +++ /dev/null @@ -1,180 +0,0 @@ -""" -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 platform -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 two_pendulums_2constraint_4DOF import compute_all_states - - -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 = "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_states(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 deleted file mode 100644 index 95b425003..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py +++ /dev/null @@ -1,206 +0,0 @@ -""" -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 platform -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, -) - - -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="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 = "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_states(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_rotule.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py deleted file mode 100644 index 5c299de17..000000000 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py +++ /dev/null @@ -1,174 +0,0 @@ -""" -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. -""" - -from bioptim import ( - BiMappingList, - BoundsList, - ConstraintList, - DynamicsOptions, - DynamicsOptionsList, - HolonomicTorqueBiorbdModel, - HolonomicConstraintsFcn, - HolonomicConstraintsList, - InitialGuessList, - ObjectiveFcn, - ObjectiveList, - OptimalControlProgram, - Solver, - SolutionMerge, - OdeSolver, - TorqueBiorbdModel, -) - -import numpy as np - -from three_bar import compute_all_states - - -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 = "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_states(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() From 6d3efbd4441713044129dbbfba9be7d5c02c87d0 Mon Sep 17 00:00:00 2001 From: p-shg Date: Thu, 8 Jan 2026 15:23:49 +0100 Subject: [PATCH 8/8] Revert "Updating to current main and separating content from the examples PR" This reverts commit 6ab835c4fc05635f3315a992531bff3c5dd5d6b9. --- bioptim/examples/models/3bar.bioMod | 98 ++++++++ bioptim/examples/models/4bar.bioMod | 132 ++++++++++ .../examples/models/pendulum_rotule.bioMod | 35 +++ .../examples/models/two_pendulums_2.bioMod | 77 ++++++ .../models/two_pendulums_rotule.bioMod | 66 +++++ .../holonomic_constraints/four_bar.py | 194 +++++++++++++++ .../holonomic_constraints/three_bar.py | 202 +++++++++++++++ .../holonomic_constraints/two_pendulums.py | 231 ++++++++++++++++++ .../two_pendulums_2constraint.py | 180 ++++++++++++++ .../two_pendulums_2constraint_4DOF.py | 206 ++++++++++++++++ .../two_pendulums_rotule.py | 174 +++++++++++++ 11 files changed, 1595 insertions(+) create mode 100644 bioptim/examples/models/3bar.bioMod create mode 100644 bioptim/examples/models/4bar.bioMod create mode 100644 bioptim/examples/models/pendulum_rotule.bioMod create mode 100644 bioptim/examples/models/two_pendulums_2.bioMod create mode 100644 bioptim/examples/models/two_pendulums_rotule.bioMod create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/four_bar.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/three_bar.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py create mode 100644 bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py 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/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/four_bar.py b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py new file mode 100644 index 000000000..99e436c56 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/four_bar.py @@ -0,0 +1,194 @@ +""" +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 platform +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 three_bar import compute_all_states + + +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 = "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_states(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..037670167 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/three_bar.py @@ -0,0 +1,202 @@ +""" +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 platform +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 two_pendulums import compute_all_states + + +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_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 = "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, qdot, qddot, lambdas = compute_all_states(sol, bio_model) + q = compute_all_states(sol, bio_model) + + import bioviz + + viz = bioviz.Viz(model_path) + viz.load_movement(q) + viz.exec() + + 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 new file mode 100644 index 000000000..62f6644d6 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py @@ -0,0 +1,231 @@ +""" +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 platform +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, +) + + +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) + controls = sol.decision_controls(to_merge=SolutionMerge.NODES) + + n = states["q_u"].shape[1] + n_tau = controls["tau"].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_tau + 1)) + + 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, 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() + 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[:, i], 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 + + +def prepare_ocp( + biorbd_model_path: str, + n_shooting: int = 30, + final_time: float = 1, + expand_dynamics: bool = False, + ode_solver=OdeSolver.RK4(), +) -> (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(1, 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, 3], + dependent_joint_index=[1, 2], + ) + + # 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=ode_solver, 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, 1], to_first=[0, 3]) + variable_bimapping.add("qdot", to_second=[0, None, None, 1], to_first=[0, 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=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.54, 1.54]) + x_init.add("qdot_u", [0, 0]) + x_bounds["q_u"][:, 0] = [1.54, 1.54] + x_bounds["qdot_u"][:, 0] = [0, 0] + x_bounds["q_u"][0, -1] = -1.54 + 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, 1], to_first=[0, 3]) + 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, + ), + bio_model, + ) + + +def main(): + """ + Runs the optimization and animates it + """ + + model_path = "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, qdot, qddot, lambdas = compute_all_states(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.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py new file mode 100644 index 000000000..30006a4c2 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py @@ -0,0 +1,180 @@ +""" +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 platform +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 two_pendulums_2constraint_4DOF import compute_all_states + + +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 = "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_states(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..95b425003 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint_4DOF.py @@ -0,0 +1,206 @@ +""" +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 platform +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, +) + + +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="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 = "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_states(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_rotule.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py new file mode 100644 index 000000000..5c299de17 --- /dev/null +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_rotule.py @@ -0,0 +1,174 @@ +""" +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. +""" + +from bioptim import ( + BiMappingList, + BoundsList, + ConstraintList, + DynamicsOptions, + DynamicsOptionsList, + HolonomicTorqueBiorbdModel, + HolonomicConstraintsFcn, + HolonomicConstraintsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OptimalControlProgram, + Solver, + SolutionMerge, + OdeSolver, + TorqueBiorbdModel, +) + +import numpy as np + +from three_bar import compute_all_states + + +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 = "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_states(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()