diff --git a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py index 540376b2c..0d437f19e 100644 --- a/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py +++ b/bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py @@ -55,8 +55,8 @@ def dynamics( defects = None if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION): - slope_q = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) - slope_qdot = DynamicsFunctions.get(nlp.states_dot["qddot_u"], nlp.states_dot.scaled.cx) + slope_q = DynamicsFunctions.get(nlp.states_dot["q_u"], nlp.states_dot.scaled.cx) + slope_qdot = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx) if nlp.dynamics_type.ode_solver.defects_type == DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS: qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau) derivative = vertcat(qdot_u, qddot_u) diff --git a/bioptim/examples/models/3bar.bioMod b/bioptim/examples/models/3bar.bioMod new file mode 100644 index 000000000..4f744c4f9 --- /dev/null +++ b/bioptim/examples/models/3bar.bioMod @@ -0,0 +1,98 @@ +version 4 + +segment Seg0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg1 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker CoM1 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg2 + RT 0 0 0 xyz 0 1 0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 5 + marker marker_5 + parent Seg2 + position 0 0 0 + endmarker + + // Marker 6 + marker marker_6 + parent Seg2 + position 0 0 -1 + endmarker + + // Marker CoM3 + marker CoM3 + parent Seg2 + position -0.0005 0.0688 -0.9542 + endmarker \ No newline at end of file diff --git a/bioptim/examples/models/4bar.bioMod b/bioptim/examples/models/4bar.bioMod new file mode 100644 index 000000000..541daf8f3 --- /dev/null +++ b/bioptim/examples/models/4bar.bioMod @@ -0,0 +1,132 @@ +version 4 + +segment Seg0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 0 + marker marker_0 + parent Seg0 + position 0 0 0 + endmarker + + // Marker 1 + marker marker_1 + parent Seg0 + position 0 0 -1 + endmarker + + // Marker 2 + marker CoM0 + parent Seg0 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg1 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 3 + marker marker_3 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg1 + position 0 0 -1 + endmarker + + // Marker CoM1 + marker CoM1 + parent Seg1 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg2 + RT 0 0 0 xyz 0 1 0 + rotations x + ranges + -10*pi 10*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 5 + marker marker_5 + parent Seg2 + position 0 0 0 + endmarker + + // Marker 6 + marker marker_6 + parent Seg2 + position 0 0 -1 + endmarker + + // Marker CoM3 + marker CoM3 + parent Seg2 + position -0.0005 0.0688 -0.9542 + endmarker + +segment Seg3 + translations yz + rotations x + ranges + -5 5 + -5 5 + -3*pi 3*pi + mass 0.25 + inertia + 0.00391 0.0000 0.0000 + 0.0000 0.0335 -0.0032 + 0.0000 -0.0032 0.0090 + com -0.0005 0.0688 -0.9542 + meshfile mesh/pendulum.STL +endsegment + + // Marker 7 + marker marker_7 + parent Seg3 + position 0 0 0 + endmarker + + // Marker 8 + marker marker_8 + parent Seg3 + position 0 0 -1 + endmarker + + // Marker CoM4 + marker CoM4 + parent Seg3 + position -0.0005 0.0688 -0.9542 + endmarker diff --git a/bioptim/examples/models/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 index 5c6b29830..d19bfbfc2 100644 --- a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py +++ b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums.py @@ -136,7 +136,7 @@ def prepare_ocp( # Dynamics dynamics = DynamicsOptionsList() - dynamics.add(DynamicsOptions(ode_solver=OdeSolver.RK4(), expand_dynamics=expand_dynamics)) + dynamics.add(DynamicsOptions(ode_solver=ode_solver, expand_dynamics=expand_dynamics)) # Path Constraints constraints = ConstraintList() diff --git a/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py b/bioptim/examples/toy_examples/holonomic_constraints/two_pendulums_2constraint.py new file mode 100644 index 000000000..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() diff --git a/tests/shard1/test_biorbd_model_holonomic.py b/tests/shard1/test_biorbd_model_holonomic.py index 490fcbe65..c5d138f82 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 @@ -193,7 +202,8 @@ def test_model_holonomic(): ) -def test_example_two_pendulums(): +@pytest.mark.parametrize("ode_solver", [OdeSolver.RK4(), OdeSolver.COLLOCATION()]) +def test_example_two_pendulums(ode_solver): """Test the holonomic_constraints/two_pendulums example""" from bioptim.examples.toy_examples.holonomic_constraints import two_pendulums @@ -205,20 +215,148 @@ def test_example_two_pendulums(): n_shooting=10, final_time=1, expand_dynamics=False, + ode_solver=ode_solver, ) # --- Solve the ocp --- # sol = ocp.solve(Solver.IPOPT()) states = sol.decision_states(to_merge=SolutionMerge.NODES) - npt.assert_almost_equal( - states["q_u"], - [ - [1.54, 1.433706, 1.185046, 0.891157, 0.561607, 0.191792, -0.206511, -0.614976, -1.018383, -1.356253, -1.54], - [1.54, 1.669722, 1.924726, 2.127746, 2.226937, 2.184007, 1.972105, 1.593534, 1.06751, 0.507334, 0.0], - ], - decimal=6, - ) + if isinstance(ode_solver, OdeSolver.RK4): + npt.assert_almost_equal( + states["q_u"], + [ + [ + 1.54, + 1.433706, + 1.185046, + 0.891157, + 0.561607, + 0.191792, + -0.206511, + -0.614976, + -1.018383, + -1.356253, + -1.54, + ], + [1.54, 1.669722, 1.924726, 2.127746, 2.226937, 2.184007, 1.972105, 1.593534, 1.06751, 0.507334, 0.0], + ], + decimal=6, + ) + + elif isinstance(ode_solver, OdeSolver.COLLOCATION): + npt.assert_almost_equal( + states["q_u"], + [ + [ + 1.54, + 1.53947255, + 1.52829032, + 1.4918706, + 1.44772412, + 1.43369574, + 1.41898275, + 1.35974706, + 1.27447461, + 1.20430304, + 1.18502396, + 1.16556562, + 1.09140745, + 0.99166684, + 0.91261473, + 0.8911305, + 0.86947629, + 0.78671683, + 0.67482281, + 0.585796, + 0.56157817, + 0.53715892, + 0.44378604, + 0.31793766, + 0.21860716, + 0.19175812, + 0.1647666, + 0.0623588, + -0.07337049, + -0.1784637, + -0.20655284, + -0.23470758, + -0.34109507, + -0.48052131, + -0.58686908, + -0.6150277, + -0.64319721, + -0.74951225, + -0.88766222, + -0.99132882, + -1.01844059, + -1.0452361, + -1.14223391, + -1.25827372, + -1.33704454, + -1.35629421, + -1.3747351, + -1.43601269, + -1.49800372, + -1.53258632, + -1.54, + ], + [ + 1.54, + 1.54064515, + 1.55431106, + 1.59922772, + 1.65296364, + 1.66973502, + 1.68705873, + 1.75370598, + 1.84171006, + 1.90763103, + 1.92475754, + 1.94158775, + 2.00129161, + 2.07058481, + 2.11662288, + 2.12778938, + 2.13845817, + 2.17380886, + 2.20803169, + 2.2242531, + 2.22699357, + 2.22904554, + 2.2304308, + 2.21629085, + 2.19241948, + 2.18407247, + 2.17489014, + 2.1330352, + 2.06082354, + 1.99228721, + 1.97216828, + 1.95123087, + 1.86495558, + 1.73527252, + 1.62449462, + 1.59357668, + 1.56195524, + 1.43562583, + 1.25495762, + 1.10760291, + 1.06752078, + 1.0272699, + 0.8761086, + 0.68415659, + 0.54376452, + 0.50733629, + 0.47128493, + 0.3387245, + 0.16767637, + 0.03546255, + 0.0, + ], + ], + decimal=6, + ) def test_example_two_pendulums_algebraic():