From 849e8189c2b6c97a2451e8c951a7791ffba9f3df Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Mon, 22 Jan 2024 20:07:54 +0400 Subject: [PATCH 1/6] Various tests with parameters --- .../getting_started/double_pendulum.py | 165 ++++++++++++ .../models/double_pendulum2.bioMod | 56 ++++ bioptim/examples/getting_started/pendulum.py | 9 +- ...imize_maximum_torque_by_extra_parameter.py | 179 ++++++++----- ...ax_torque_by_extra_parameter_multiphase.py | 247 ++++++++++++++++++ .../examples/torque_driven_ocp/spring_load.py | 56 +++- 6 files changed, 638 insertions(+), 74 deletions(-) create mode 100644 bioptim/examples/getting_started/double_pendulum.py create mode 100644 bioptim/examples/getting_started/models/double_pendulum2.bioMod create mode 100644 bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py diff --git a/bioptim/examples/getting_started/double_pendulum.py b/bioptim/examples/getting_started/double_pendulum.py new file mode 100644 index 000000000..c20e281d3 --- /dev/null +++ b/bioptim/examples/getting_started/double_pendulum.py @@ -0,0 +1,165 @@ +""" +A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward +while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. + +This simple example is a good place to start investigating bioptim as it describes the most common dynamics out there +(the joint torque driven), it defines an objective function and some boundaries and initial guesses + +During the optimization process, the graphs are updated real-time (even though it is a bit too fast and short to really +appreciate it). Finally, once it finished optimizing, it animates the model using the optimal solution +""" + +import platform + +from bioptim import ( + OptimalControlProgram, + DynamicsFcn, + Dynamics, + BoundsList, + InitialGuessList, + ObjectiveFcn, + ObjectiveList, + OdeSolver, + OdeSolverBase, + CostType, + Solver, + BiorbdModel, + ControlType, + PhaseDynamics, +) + + +def prepare_ocp( + biorbd_model_path: str, + final_time: float, + n_shooting: int, + ode_solver: OdeSolverBase = OdeSolver.COLLOCATION(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + control_type: ControlType = ControlType.CONSTANT, +) -> OptimalControlProgram: + """ + The initialization of an ocp + + Parameters + ---------- + biorbd_model_path: str + The path to the biorbd model + final_time: float + The time in second required to perform the task + n_shooting: int + The number of shooting points to define int the direct multiple shooting program + ode_solver: OdeSolverBase = OdeSolver.RK4() + Which type of OdeSolver to use + use_sx: bool + If the SX variable should be used instead of MX (can be extensive on RAM) + n_threads: int + The number of threads to use in the paralleling (1 = no parallel computing) + phase_dynamics: PhaseDynamics + If the dynamics equation within a phase is unique or changes at each node. + PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within + a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces + are applied at each 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) + control_type: ControlType + The type of the controls + + Returns + ------- + The OptimalControlProgram ready to be solved + """ + + bio_model = BiorbdModel(biorbd_model_path) + + # Add objective functions + + + objective_functions = ObjectiveList() + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=0, weight=1) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=500) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=2, weight=500) + + + # Dynamics + dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) + + # Path bounds + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + + x_bounds["q"][:, [0, -1]] = 0 # Start and end at 0... + x_bounds["q"][1, -1] = 3.14 # ...but end with pendulum 180 degrees rotated + + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess (optional since it is 0, we show how to initialize anyway) + x_init = InitialGuessList() + x_init["q"] = [0] * bio_model.nb_q + x_init["qdot"] = [0] * bio_model.nb_qdot + + # Define control path bounds + n_tau = bio_model.nb_tau + u_bounds = BoundsList() + u_bounds["tau"] = [-150, -2, -2], [150, 2, 2] # Limit the strength of the pendulum to (-100 to 100)... + + # Initial guess (optional since it is 0, we show how to initialize anyway) + # u_init = InitialGuessList() + # u_init["tau"] = [0] * n_tau + + return OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + # x_init=x_init, + # u_init=u_init, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + ode_solver=ode_solver, + use_sx=use_sx, + n_threads=n_threads, + control_type=control_type, + ) + + +def main(): + """ + If pendulum is run as a script, it will perform the optimization and animates it + """ + + # --- Prepare the ocp --- # + ocp = prepare_ocp(biorbd_model_path="models/double_pendulum2.bioMod", final_time=1, n_shooting=100, n_threads=5) + + # Custom plots + ocp.add_plot_penalty(CostType.ALL) + + # --- If one is interested in checking the conditioning of the problem, they can uncomment the following line --- # + # ocp.check_conditioning() + + # --- Print ocp structure --- # + ocp.print(to_console=False, to_graph=False) + + # --- Solve the ocp. Please note that online graphics only works with the Linux operating system --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + sol.print_cost() + + # --- Show the results (graph or animation) --- # + # sol.graphs(show_bounds=True) + sol.animate(n_frames=100) + + # # --- Save the solution --- # + # import pickle + # with open("pendulum.pkl", "wb") as file: + # del sol.ocp + # pickle.dump(sol, file) + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/getting_started/models/double_pendulum2.bioMod b/bioptim/examples/getting_started/models/double_pendulum2.bioMod new file mode 100644 index 000000000..03e7584de --- /dev/null +++ b/bioptim/examples/getting_started/models/double_pendulum2.bioMod @@ -0,0 +1,56 @@ +version 4 + +segment Seg1 + translations y + rotations x + ranges + -2 2 + -10*pi 10*pi + mass 1 + inertia + 0.0391 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 1 + marker marker_1 + parent Seg1 + position 0 0 0 + endmarker + + // Marker 2 + marker marker_2 + parent Seg1 + position 0 0 -1 + endmarker + +segment Seg2 + parent Seg1 + rotations x + rtinmatrix 0 + rt 0 0 0 xyz 0 0 -1 + ranges + -10*pi 10*pi + mass 1 + inertia + 0.0391 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 Seg2 + position 0 0 0 + endmarker + + // Marker 4 + marker marker_4 + parent Seg2 + position 0 0 -1 + endmarker diff --git a/bioptim/examples/getting_started/pendulum.py b/bioptim/examples/getting_started/pendulum.py index f5deec315..20025d8de 100644 --- a/bioptim/examples/getting_started/pendulum.py +++ b/bioptim/examples/getting_started/pendulum.py @@ -17,8 +17,10 @@ Dynamics, BoundsList, InitialGuessList, + ObjectiveList, ObjectiveFcn, Objective, + Node, OdeSolver, OdeSolverBase, CostType, @@ -77,7 +79,12 @@ def prepare_ocp( bio_model = BiorbdModel(biorbd_model_path) # Add objective functions - objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") + + objective_functions = ObjectiveList() + #Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight=-1) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight=-2) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight= 1) + # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) diff --git a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py index a99445a68..a8c728c69 100644 --- a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py +++ b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py @@ -1,7 +1,7 @@ """ -This example is inspired from the clear pike circle gymnastics skill. It is composed of two pendulums +This example is inspired from the giant circle gymnastics skill. It is composed of two pendulums representing the trunk and legs segments (only the hip flexion is actuated). The objective is to minimize the -maximum torque (minmax) of the hip flexion while performing the clear pike circle motion. The maximum torque is included to the +maximum torque (minmax) of the hip flexion while performing the giant circle. The maximum torque is included to the problem as a parameter, all torque interval re constrained to be smaller than this parameter, this parameter is the minimized. """ @@ -15,6 +15,7 @@ DynamicsFcn, ObjectiveList, ConstraintList, + ConstraintFcn, BoundsList, InitialGuessList, Node, @@ -25,16 +26,16 @@ BiorbdModel, PenaltyController, ParameterObjectiveList, - ParameterConstraintList, - ConstraintFcn, ) -def custom_constraint_parameters(controller: PenaltyController) -> MX: - tau = controller.controls["tau"].cx_start - max_param = controller.parameters["max_tau"].cx - val = max_param - tau - return val + +def custom_constraint_max_tau(controller: PenaltyController) -> MX: + return controller.parameters["max_tau"].cx - controller.controls["tau"].cx + + +def custom_constraint_min_tau(controller: PenaltyController) -> MX: + return controller.parameters["min_tau"].cx - controller.controls["tau"].cx def my_parameter_function(bio_model: biorbd.Model, value: MX): @@ -44,109 +45,94 @@ def my_parameter_function(bio_model: biorbd.Model, value: MX): def prepare_ocp( bio_model_path: str = "models/double_pendulum.bioMod", ) -> OptimalControlProgram: - bio_model = (BiorbdModel(bio_model_path), BiorbdModel(bio_model_path)) + bio_model = BiorbdModel(bio_model_path) # Problem parameters - n_shooting = (40, 40) - final_time = (1, 1) - tau_min, tau_max, tau_init = -300, 300, 0 + n_shooting = 50 + final_time = 1 + tau_min, tau_max, tau_init = -10, 10, 0 # Mapping tau_mappings = BiMappingList() - tau_mappings.add("tau", to_second=[None, 0], to_first=[1], phase=0) - tau_mappings.add("tau", to_second=[None, 0], to_first=[1], phase=1) + tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) # Define the parameter to optimize parameters = ParameterList() + parameter_init = InitialGuessList() + parameter_bounds = BoundsList() parameters.add( - "max_tau", # The name of the parameter - my_parameter_function, # The function that modifies the biorbd model + "max_tau", + my_parameter_function, + size=1, + ) + parameters.add( + "min_tau", + my_parameter_function, size=1, ) - parameter_bounds = BoundsList() - parameter_init = InitialGuessList() + parameter_init["max_tau"] = 1 + parameter_init["min_tau"] = -1 - parameter_init["max_tau"] = 0 parameter_bounds.add("max_tau", min_bound=0, max_bound=tau_max, interpolation=InterpolationType.CONSTANT) + parameter_bounds.add("min_tau", min_bound=tau_min, max_bound=0, interpolation=InterpolationType.CONSTANT) - # Add phase independant objective functions + # Add phase independent objective functions parameter_objectives = ParameterObjectiveList() - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=1000, quadratic=True) - - # Add phase independant constraint functions - parameter_constraints = ParameterConstraintList() - parameter_constraints.add(ConstraintFcn.TRACK_PARAMETER, min_bound=-100, max_bound=100) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=-10, quadratic=False) # Add objective functions objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, phase=0, min_bound=0.1, max_bound=3) - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, phase=1, min_bound=0.1, max_bound=3) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, phase=0) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, phase=1) + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1, phase=0, min_bound=1, max_bound=5) + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=0) # Constraints constraints = ConstraintList() - constraints.add(custom_constraint_parameters, node=Node.ALL, min_bound=0, max_bound=tau_max) + + # constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, target=3.8) + + constraints.add(custom_constraint_max_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max) + constraints.add(custom_constraint_min_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) - dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) # Path constraint x_bounds = BoundsList() - x_bounds.add(key="q", bounds=bio_model[0].bounds_from_ranges("q"), phase=0) - x_bounds.add(key="qdot", bounds=bio_model[0].bounds_from_ranges("qdot"), phase=0) - x_bounds.add(key="q", bounds=bio_model[1].bounds_from_ranges("q"), phase=1) - x_bounds.add(key="qdot", bounds=bio_model[1].bounds_from_ranges("qdot"), phase=1) - - # change model bound for -pi, pi - for i in range(len(bio_model)): - x_bounds[i]["q"].min[1, :] = -np.pi - x_bounds[i]["q"].max[1, :] = np.pi - - # Phase 0 - x_bounds[0]["q"][0, 0] = np.pi - x_bounds[0]["q"][1, 0] = 0 - x_bounds[0]["q"].min[1, -1] = 6 * np.pi / 8 - 0.1 - x_bounds[0]["q"].max[1, -1] = 6 * np.pi / 8 + 0.1 - - # Phase 1 - x_bounds[1]["q"][0, -1] = 3 * np.pi - x_bounds[1]["q"][1, -1] = 0 - - # Initial guess - x_init = InitialGuessList() - x_init.add(key="q", initial_guess=[0] * bio_model[0].nb_q, phase=0) - x_init.add(key="qdot", initial_guess=[0] * bio_model[0].nb_qdot, phase=0) - x_init.add(key="q", initial_guess=[1] * bio_model[1].nb_q, phase=1) - x_init.add(key="qdot", initial_guess=[1] * bio_model[1].nb_qdot, phase=1) + x_bounds.add(key="q", bounds=bio_model.bounds_from_ranges("q")) + x_bounds.add(key="qdot", bounds=bio_model.bounds_from_ranges("qdot")) + + x_bounds["q"].min[0, :] = 0 + x_bounds["q"].max[0, :] = 3 * np.pi + + x_bounds["q"].min[1, :] = -np.pi / 3 + x_bounds["q"].max[1, :] = np.pi / 5 + + x_bounds["q"][0, 0] = np.pi + 0.01 + x_bounds["q"][1, 0] = 0 + x_bounds["qdot"][0, 0] = 0 + x_bounds["qdot"][1, 0] = 0 + + x_bounds["q"][0, -1] = 3 * np.pi + x_bounds["q"][1, -1] = 0 # Define control path constraint n_tau = len(tau_mappings[0]["tau"].to_first) u_bounds = BoundsList() u_bounds.add(key="tau", min_bound=[tau_min] * n_tau, max_bound=[tau_max] * n_tau, phase=0) - u_bounds.add(key="tau", min_bound=[tau_min] * n_tau, max_bound=[tau_max] * n_tau, phase=1) - - # Control initial guess - u_init = InitialGuessList() - u_init.add(key="tau", initial_guess=[tau_init] * n_tau, phase=0) - u_init.add(key="tau", initial_guess=[tau_init] * n_tau, phase=1) return OptimalControlProgram( bio_model, dynamics, n_shooting, final_time, - x_init=x_init, - u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, parameter_objectives=parameter_objectives, - parameter_constraints=parameter_constraints, parameter_bounds=parameter_bounds, parameter_init=parameter_init, constraints=constraints, @@ -161,11 +147,66 @@ def main(): # --- Solve the ocp --- # sol = ocp.solve() + # sol.print_cost() # --- Show results --- # - sol.animate() - sol.graphs(show_bounds=True) + # sol.animate() +# sol.graphs(show_bounds=True) + + + import matplotlib + from matplotlib import pyplot as plt + matplotlib.use('Qt5Agg') + + states = sol.decision_states() + controls = sol.decision_controls() + + q = np.array([item.flatten() for item in states["q"]]) + qdot = np.array([item.flatten() for item in states["qdot"]]) + tau = np.vstack([ + np.array([item.flatten() for item in controls["tau"]]), + np.array([[np.nan]]) + ]) + time = np.array([item.full().flatten()[0] for item in sol.stepwise_time()]) + + + fig, axs = plt.subplots(2, 2, figsize=(10, 15)) + + # Plotting q solutions for both DOFs + axs[0, 0].plot(time, q) + axs[0, 0].set_title("Joint coordinates") + axs[0, 0].set_ylabel("q") + axs[0, 0].set_xlabel("Time [s]") + axs[0, 0].grid(True) + + axs[0, 1].plot(time, qdot) + axs[0, 1].set_title("Joint velocities") + axs[0, 1].set_ylabel("qdot") + axs[0, 1].set_xlabel("Time [s]") + axs[0, 1].grid(True) + + axs[1, 0].step(time, tau) + axs[1, 0].set_title("Generalized forces") + axs[1, 0].set_ylabel("tau") + axs[1, 0].set_xlabel("Time [s]") + axs[1, 0].grid(True) + axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["max_tau"], 'k--') + axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_tau"], 'k--') + + axs[1, 1].plot(sol.constraints[ocp.n_shooting * (ocp.nlp[0].model.nb_q + ocp.nlp[0].model.nb_qdot):], "o") + axs[1, 1].set_title("Constaints (continuity excluded)") + + plt.tight_layout() + plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) + + # Display the plot + plt.show() + + print("Duration: ", time[-1]) + print('sum tau**2 dt = ', np.nansum(tau**2 * time[1])) + print('min-max tau: ', np.nanmin(tau), np.nanmax(tau)) if __name__ == "__main__": main() + diff --git a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py new file mode 100644 index 000000000..19e68934e --- /dev/null +++ b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py @@ -0,0 +1,247 @@ +""" +This example is inspired from the clear pike circle gymnastics skill. It is composed of two pendulums +representing the trunk and legs segments (only the hip flexion is actuated). The objective is to minimize the +extreme torque (minmax) of the hip flexion while performing the clear pike circle motion. The extreme torques are included to the +problem as parameters, all torque intervals are constrained to be smaller than this parameter, these parameters are +minimized. +""" + +import numpy as np +import biorbd_casadi as biorbd +from casadi import MX +from bioptim import ( + OptimalControlProgram, + DynamicsList, + DynamicsFcn, + ObjectiveList, + ConstraintList, + ConstraintFcn, + BoundsList, + InitialGuessList, + Node, + ObjectiveFcn, + BiMappingList, + ParameterList, + InterpolationType, + BiorbdModel, + PenaltyController, + ParameterObjectiveList, +) + + +def custom_constraint_max_tau(controller: PenaltyController) -> MX: + return controller.parameters["max_tau"].cx - controller.controls["tau"].cx + + +def custom_constraint_min_tau(controller: PenaltyController) -> MX: + return controller.parameters["min_tau"].cx - controller.controls["tau"].cx + +def custom_constraint_min_max_tau(controller: PenaltyController) -> MX: + mini = controller.parameters["min_max_tau"].cx[0] - controller.controls["tau"].cx + maxi = controller.parameters["min_max_tau"].cx[1] - controller.controls["tau"].cx + return controller.parameters["min_max_tau"].cx - controller.controls["tau"].cx#[mini, maxi] + + +def my_parameter_function(bio_model: biorbd.Model, value: MX): + return + + +def prepare_ocp( + bio_model_path: str = "models/double_pendulum.bioMod", +) -> OptimalControlProgram: + bio_model = (BiorbdModel(bio_model_path), BiorbdModel(bio_model_path)) + + # Problem parameters + n_shooting = (30, 30) + final_time = (2, 3) + tau_min, tau_max, tau_init = -40, 40, 0 + + # Dynamics + dynamics = DynamicsList() + dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) + dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False) + + # Mapping + tau_mappings = BiMappingList() + tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) + tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) + + + constraints = ConstraintList() + + # Define the parameter to optimize + parameters = ParameterList() + parameter_init = InitialGuessList() + parameter_bounds = BoundsList() + parameter_objectives = ParameterObjectiveList() + + method = 1 + + if method == 1: + parameters.add("max_tau", my_parameter_function, size=1, ) + parameters.add("min_tau", my_parameter_function, size=1, ) + parameters.add("to_test", my_parameter_function, size=1, ) + + + parameter_init["max_tau"] = 1 + parameter_init["min_tau"] = -1 + parameter_init["to_test"] = 1000 + + + parameter_bounds.add("max_tau", min_bound=0, max_bound=tau_max, interpolation=InterpolationType.CONSTANT) + parameter_bounds.add("min_tau", min_bound=tau_min, max_bound=0, interpolation=InterpolationType.CONSTANT) + parameter_bounds.add("to_test", min_bound=100, max_bound=1000, interpolation=InterpolationType.CONSTANT) + + # Add phase independent objective functions + + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=10, quadratic=True) + + # Constraints + constraints.add(custom_constraint_max_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max) + constraints.add(custom_constraint_min_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0) + # + constraints.add(custom_constraint_max_tau, phase=1, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max) + constraints.add(custom_constraint_min_tau, phase=1, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0) + + elif method == 2: #todo: does not work + parameters.add("min_max_tau", my_parameter_function, size=2, ) + parameter_init["min_max_tau"] = [-1, 1] + parameter_bounds.add("min_max_tau", + min_bound=[tau_min, 0], + max_bound=[0, tau_max], + interpolation=InterpolationType.CONSTANT + ) + + # Add phase independent objective functions + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_max_tau", weight=10, quadratic=True) + + # Constraints + + for phase in range(len(n_shooting)): + constraints.add( + custom_constraint_min_max_tau, + phase=phase, node=Node.ALL_SHOOTING, + min_bound=[tau_min, 0], max_bound=[0, tau_max] + ) + + objective_functions = ObjectiveList() + # Add objective functions + for phase in range(len(n_shooting)): + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1e-5, phase=phase, min_bound=0.5, max_bound=3) #was w=10 + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1e-5, phase=phase) #was w=1 + + + + constraints.add( + ConstraintFcn.TRACK_STATE, key="q", phase=0, node=Node.START, target=[np.pi + 0.01, 0] + ) # Initial state + constraints.add(ConstraintFcn.TRACK_STATE, key="qdot", phase=0, node=Node.START, target=[0, 0]) + constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=1, phase=1, node=Node.START, target=np.pi) + constraints.add(ConstraintFcn.TRACK_STATE, key="q", phase=1, node=Node.END, target=[3 * np.pi, 0]) # Final state + + for i in range(len(bio_model)): + constraints.add( + ConstraintFcn.BOUND_STATE, + key="q", + phase=i, + node=Node.ALL, + min_bound=[np.pi, -np.pi / 3], + max_bound=[3 * np.pi, np.pi], + ), + + return OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + objective_functions=objective_functions, + parameter_objectives=parameter_objectives, + parameter_bounds=parameter_bounds, + parameter_init=parameter_init, + constraints=constraints, + variable_mappings=tau_mappings, + parameters=parameters, + ) + + +def main(): + # --- Prepare the ocp --- # + ocp = prepare_ocp() + + # --- Solve the ocp --- # + sol = ocp.solve() + + print(sol.parameters) + # sol.print_cost() + + import matplotlib + from matplotlib import pyplot as plt + matplotlib.use('Qt5Agg') + + states = sol.decision_states() + controls = sol.decision_controls() + + q = np.vstack([ + np.array([item.flatten() for item in states[0]["q"]]), + np.array([item.flatten() for item in states[1]["q"]]) + ]) + qdot = np.vstack([ + np.array([item.flatten() for item in states[0]["qdot"]]), + np.array([item.flatten() for item in states[1]["qdot"]]) + ]) + tau = np.vstack([ + np.array([item.flatten() for item in controls[0]["tau"]]), + np.array([[np.nan]]), + np.array([item.flatten() for item in controls[1]["tau"]]), + np.array([[np.nan]]), + ]) + time = np.vstack([ + np.array([item.full().flatten()[0] for item in sol.stepwise_time()[0]]).reshape((-1, 1)), + np.array([item.full().flatten()[0] for item in sol.stepwise_time()[1]]).reshape((-1, 1)) + sol.stepwise_time()[0][-1].full(), + ]) + + fig, axs = plt.subplots(2, 2, figsize=(10, 15)) + + # Plotting q solutions for both DOFs + axs[0, 0].plot(time, q) + axs[0, 0].set_title("Joint coordinates") + axs[0, 0].set_ylabel("q") + axs[0, 0].set_xlabel("Time [s]") + axs[0, 0].grid(True) + + axs[0, 1].plot(time, qdot) + axs[0, 1].set_title("Joint velocities") + axs[0, 1].set_ylabel("qdot") + axs[0, 1].set_xlabel("Time [s]") + axs[0, 1].grid(True) + + axs[1, 0].step(time, tau) + axs[1, 0].set_title("Generalized forces") + axs[1, 0].set_ylabel("tau") + axs[1, 0].set_xlabel("Time [s]") + axs[1, 0].grid(True) + + if len(sol.parameters) == 1: + for i in range(2): + axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_max_tau"][i], 'k--') + else: + axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["max_tau"], 'k--') + axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_tau"], 'k--') + + plt.tight_layout() + plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) + + # Display the plot + plt.show() + + + + # --- Show results --- # + sol.animate() + sol.graphs(show_bounds=True) + + +if __name__ == "__main__": + main() + diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py index e4661ae0b..b09fc9dab 100644 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -21,6 +21,7 @@ Solver, DynamicsEvaluation, PhaseDynamics, + Node, ) @@ -55,9 +56,9 @@ def custom_dynamic( tau = DynamicsFunctions.get(nlp.controls["tau"], controls) force_vector = MX.zeros(6) - force_vector[5] = 100 * q[0] ** 2 + force_vector[5] = 100 * q[0] ** 2 #spring force - qddot = nlp.model.forward_dynamics(q, qdot, tau, [["Point", force_vector]]) + qddot = nlp.model.forward_dynamics(q, qdot, tau, external_forces=[["Point", force_vector]]) return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) @@ -89,7 +90,8 @@ def prepare_ocp( m.set_gravity(np.array((0, 0, 0))) # Add objective functions (high upward velocity at end point) - objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="qdot", index=0, weight=-1) + objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="qdot", index=0, weight=-1, + node=Node.END,)# quadratic=False) # Dynamics dynamics = Dynamics( @@ -123,10 +125,56 @@ def prepare_ocp( def main(): ocp = prepare_ocp() - + ocp.print(to_graph=True, to_console=False) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + + + + import matplotlib + from matplotlib import pyplot as plt + matplotlib.use('Qt5Agg') + + states = sol.decision_states() + controls = sol.decision_controls() + + q = np.array([item.flatten() for item in states["q"]]) + qdot = np.array([item.flatten() for item in states["qdot"]]) + tau = np.vstack([ + np.array([item.flatten() for item in controls["tau"]]), + np.array([[np.nan]]) + ]) + time = np.array([item.full().flatten()[0] for item in sol.stepwise_time()]) + + + fig, axs = plt.subplots(2, 2, figsize=(10, 15)) + + # Plotting q solutions for both DOFs + axs[0, 0].plot(time, q) + axs[0, 0].set_title("Joint coordinates") + axs[0, 0].set_ylabel("q") + axs[0, 0].set_xlabel("Time [s]") + axs[0, 0].grid(True) + + axs[0, 1].plot(time, qdot) + axs[0, 1].set_title("Joint velocities") + axs[0, 1].set_ylabel("qdot") + axs[0, 1].set_xlabel("Time [s]") + axs[0, 1].grid(True) + + axs[1, 0].step(time, tau) + axs[1, 0].set_title("Generalized forces") + axs[1, 0].set_ylabel("tau") + axs[1, 0].set_xlabel("Time [s]") + axs[1, 0].grid(True) + + plt.tight_layout() + plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) + + # Display the plot + plt.show() + # --- Show results --- # sol.animate() From de8fc77f6b88506d181ebbe8e69e9f1397e2bb75 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Mon, 22 Jan 2024 20:21:08 +0400 Subject: [PATCH 2/6] Fix Miminize Parameters --- .../minimize_maximum_torque_by_extra_parameter.py | 4 ++-- .../minmax_torque_by_extra_parameter_multiphase.py | 6 +++--- bioptim/limits/penalty.py | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py index a8c728c69..899a41699 100644 --- a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py +++ b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py @@ -80,8 +80,8 @@ def prepare_ocp( # Add phase independent objective functions parameter_objectives = ParameterObjectiveList() - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=-10, quadratic=False) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=False) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=-10, quadratic=True) # Add objective functions objective_functions = ObjectiveList() diff --git a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py index 19e68934e..b2bc862f0 100644 --- a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py +++ b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py @@ -75,7 +75,7 @@ def prepare_ocp( parameter_bounds = BoundsList() parameter_objectives = ParameterObjectiveList() - method = 1 + method = 2 if method == 1: parameters.add("max_tau", my_parameter_function, size=1, ) @@ -128,8 +128,8 @@ def prepare_ocp( objective_functions = ObjectiveList() # Add objective functions for phase in range(len(n_shooting)): - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=1e-5, phase=phase, min_bound=0.5, max_bound=3) #was w=10 - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1e-5, phase=phase) #was w=1 + objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10, phase=phase, min_bound=0.5, max_bound=3) #was w=10 + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=phase) #was w=1 diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py index 824f946d3..df31adbbb 100644 --- a/bioptim/limits/penalty.py +++ b/bioptim/limits/penalty.py @@ -1264,7 +1264,7 @@ def minimize_parameter(penalty: PenaltyOption, controller: PenaltyController, ke penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic penalty.multi_thread = True if penalty.multi_thread is None else penalty.multi_thread - return controller.parameters.cx + return controller.parameters.cx if key is None or key == "all" else controller.parameters[key].cx @staticmethod def add(ocp, nlp): From 67e9a8a3265f5f62d3066a12e49d90d26fef98a8 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Thu, 25 Jan 2024 15:40:54 +0100 Subject: [PATCH 3/6] provide better examples for minmax problems with parameters --- ...imize_maximum_torque_by_extra_parameter.py | 159 +++++----- ...ax_torque_by_extra_parameter_multiphase.py | 273 +++++++++++------- 2 files changed, 256 insertions(+), 176 deletions(-) diff --git a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py index 899a41699..38cfab27c 100644 --- a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py +++ b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py @@ -3,7 +3,7 @@ representing the trunk and legs segments (only the hip flexion is actuated). The objective is to minimize the maximum torque (minmax) of the hip flexion while performing the giant circle. The maximum torque is included to the problem as a parameter, all torque interval re constrained to be smaller than this parameter, this parameter is the -minimized. +minimized. Two options are provided and compared to define initial and final states (0: bounds; 1: constraints) """ import numpy as np @@ -27,7 +27,7 @@ PenaltyController, ParameterObjectiveList, ) - +from matplotlib import pyplot as plt def custom_constraint_max_tau(controller: PenaltyController) -> MX: @@ -43,6 +43,7 @@ def my_parameter_function(bio_model: biorbd.Model, value: MX): def prepare_ocp( + method_bound_states: int = 0, bio_model_path: str = "models/double_pendulum.bioMod", ) -> OptimalControlProgram: bio_model = BiorbdModel(bio_model_path) @@ -80,8 +81,8 @@ def prepare_ocp( # Add phase independent objective functions parameter_objectives = ParameterObjectiveList() - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=False) - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=-10, quadratic=True) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) + parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=10, quadratic=True) # Add objective functions objective_functions = ObjectiveList() @@ -111,18 +112,20 @@ def prepare_ocp( x_bounds["q"].min[1, :] = -np.pi / 3 x_bounds["q"].max[1, :] = np.pi / 5 - x_bounds["q"][0, 0] = np.pi + 0.01 - x_bounds["q"][1, 0] = 0 - x_bounds["qdot"][0, 0] = 0 - x_bounds["qdot"][1, 0] = 0 + if method_bound_states == 0: + x_bounds["q"][0, 0] = np.pi + 0.01 + x_bounds["q"][1, 0] = 0 + x_bounds["qdot"][0, 0] = 0 + x_bounds["qdot"][1, 0] = 0 + + x_bounds["q"][0, -1] = 3 * np.pi + x_bounds["q"][1, -1] = 0 + else: + constraints.add(ConstraintFcn.TRACK_STATE, node=Node.START, key="q", target=[np.pi + 0.01, 0]) + constraints.add(ConstraintFcn.TRACK_STATE, node=Node.START, key="qdot", target=[0, 0]) + constraints.add(ConstraintFcn.TRACK_STATE, node=Node.END, key="q", target=[3 * np.pi, 0]) - x_bounds["q"][0, -1] = 3 * np.pi - x_bounds["q"][1, -1] = 0 - # Define control path constraint - n_tau = len(tau_mappings[0]["tau"].to_first) - u_bounds = BoundsList() - u_bounds.add(key="tau", min_bound=[tau_min] * n_tau, max_bound=[tau_max] * n_tau, phase=0) return OptimalControlProgram( bio_model, @@ -130,7 +133,6 @@ def prepare_ocp( n_shooting, final_time, x_bounds=x_bounds, - u_bounds=u_bounds, objective_functions=objective_functions, parameter_objectives=parameter_objectives, parameter_bounds=parameter_bounds, @@ -143,70 +145,83 @@ def prepare_ocp( def main(): # --- Prepare the ocp --- # - ocp = prepare_ocp() - - # --- Solve the ocp --- # - sol = ocp.solve() - # sol.print_cost() - - # --- Show results --- # - # sol.animate() -# sol.graphs(show_bounds=True) - - - import matplotlib - from matplotlib import pyplot as plt - matplotlib.use('Qt5Agg') - - states = sol.decision_states() - controls = sol.decision_controls() - - q = np.array([item.flatten() for item in states["q"]]) - qdot = np.array([item.flatten() for item in states["qdot"]]) - tau = np.vstack([ - np.array([item.flatten() for item in controls["tau"]]), - np.array([[np.nan]]) - ]) - time = np.array([item.full().flatten()[0] for item in sol.stepwise_time()]) - - - fig, axs = plt.subplots(2, 2, figsize=(10, 15)) - - # Plotting q solutions for both DOFs - axs[0, 0].plot(time, q) - axs[0, 0].set_title("Joint coordinates") - axs[0, 0].set_ylabel("q") - axs[0, 0].set_xlabel("Time [s]") - axs[0, 0].grid(True) - - axs[0, 1].plot(time, qdot) - axs[0, 1].set_title("Joint velocities") - axs[0, 1].set_ylabel("qdot") - axs[0, 1].set_xlabel("Time [s]") - axs[0, 1].grid(True) - - axs[1, 0].step(time, tau) - axs[1, 0].set_title("Generalized forces") - axs[1, 0].set_ylabel("tau") - axs[1, 0].set_xlabel("Time [s]") - axs[1, 0].grid(True) - axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["max_tau"], 'k--') - axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_tau"], 'k--') - - axs[1, 1].plot(sol.constraints[ocp.n_shooting * (ocp.nlp[0].model.nb_q + ocp.nlp[0].model.nb_qdot):], "o") - axs[1, 1].set_title("Constaints (continuity excluded)") + fig, axs = plt.subplots(1, 3) + axs[0].set_title("Joint coordinates") + axs[0].set_ylabel("q [°]") + axs[1].set_title("Joint velocities") + axs[1].set_ylabel("qdot [°/s]") + axs[2].set_title("Generalized forces") + axs[2].set_ylabel("tau [Nm]") + for ax in [0, 1, 2]: + axs[ax].set_xlabel("Time [s]") + axs[ax].grid(True) + + for method_bound_states in range(2): + # --- Prepare the ocp --- # + ocp = prepare_ocp(method_bound_states=method_bound_states) + + # --- Solve the ocp --- # + sol = ocp.solve() + # sol.print_cost() + + + # --- Show results --- # + # sol.animate() + # sol.graphs(show_bounds=True) + + states = sol.decision_states() + controls = sol.decision_controls() + + q = np.array([item.flatten() for item in states["q"]]) + qdot = np.array([item.flatten() for item in states["qdot"]]) + tau = np.vstack([np.array([item.flatten() for item in controls["tau"]]), np.array([[np.nan]])]) + time = np.array([item.full().flatten()[0] for item in sol.stepwise_time()]) + + print("Duration: ", time[-1]) + print("sum tau**2 dt = ", np.nansum(tau**2 * time[1])) + print("min-max tau: ", np.nanmin(tau), np.nanmax(tau)) + + # Plotting q solutions for both DOFs + axs[0].plot(time, q * 180 / np.pi) + axs[1].plot(time, qdot * 180 / np.pi) + axs[2].step(time, tau) + xlim = np.asarray(axs[2].get_xlim()) + xlim = np.hstack([xlim, np.nan, xlim]) + min_max = np.hstack( + [np.ones([2]) * sol.parameters["min_tau"], np.nan, np.ones([2]) * sol.parameters["max_tau"]] + ) + axs[2].plot(xlim, min_max, "k--") plt.tight_layout() plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) # Display the plot + axs[0].legend( + { + "q_0 (method 0)", + "q_1 (method 0)", + "q_0 (method 1)", + "q_1 (method 1)", + } + ) + axs[1].legend( + { + "qdot_0 (method 0)", + "qdot_1 (method 0)", + "qdot_0 (method 1)", + "qdot_1 (method 1)", + } + ) + axs[2].legend( + { + "tau_1 (method 0)", + "min-max (method 0)", + "tau_1 (method 1)", + "min-max (method 1)", + } + ) plt.show() - print("Duration: ", time[-1]) - print('sum tau**2 dt = ', np.nansum(tau**2 * time[1])) - print('min-max tau: ', np.nanmin(tau), np.nanmax(tau)) - if __name__ == "__main__": main() - diff --git a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py index b2bc862f0..e06d5ce32 100644 --- a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py +++ b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py @@ -1,9 +1,13 @@ """ This example is inspired from the clear pike circle gymnastics skill. It is composed of two pendulums -representing the trunk and legs segments (only the hip flexion is actuated). The objective is to minimize the +representing the arma-trunk and legs segments (only the hip flexion is actuated). The objective is to minimize the extreme torque (minmax) of the hip flexion while performing the clear pike circle motion. The extreme torques are included to the problem as parameters, all torque intervals are constrained to be smaller than this parameter, these parameters are -minimized. +minimized using 3 approches: +0. min and max are independent parameters which are minimized with quadratic=True, weight=10 +1. min and max are independent parameters which are minimized with quadratic=False, weights= -10 and 10 +2. min and max are declared together are miminized with quadratic=True, weight=10 +All graphical results are presented using pyplot """ import numpy as np @@ -27,6 +31,7 @@ PenaltyController, ParameterObjectiveList, ) +from matplotlib import pyplot as plt def custom_constraint_max_tau(controller: PenaltyController) -> MX: @@ -36,10 +41,11 @@ def custom_constraint_max_tau(controller: PenaltyController) -> MX: def custom_constraint_min_tau(controller: PenaltyController) -> MX: return controller.parameters["min_tau"].cx - controller.controls["tau"].cx + def custom_constraint_min_max_tau(controller: PenaltyController) -> MX: mini = controller.parameters["min_max_tau"].cx[0] - controller.controls["tau"].cx maxi = controller.parameters["min_max_tau"].cx[1] - controller.controls["tau"].cx - return controller.parameters["min_max_tau"].cx - controller.controls["tau"].cx#[mini, maxi] + return controller.parameters["min_max_tau"].cx - controller.controls["tau"].cx # [mini, maxi] def my_parameter_function(bio_model: biorbd.Model, value: MX): @@ -47,6 +53,7 @@ def my_parameter_function(bio_model: biorbd.Model, value: MX): def prepare_ocp( + parameter_option: int = 0, bio_model_path: str = "models/double_pendulum.bioMod", ) -> OptimalControlProgram: bio_model = (BiorbdModel(bio_model_path), BiorbdModel(bio_model_path)) @@ -66,7 +73,6 @@ def prepare_ocp( tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) tau_mappings.add("tau", to_second=[None, 0], to_first=[1]) - constraints = ConstraintList() # Define the parameter to optimize @@ -75,63 +81,82 @@ def prepare_ocp( parameter_bounds = BoundsList() parameter_objectives = ParameterObjectiveList() - method = 2 - - if method == 1: - parameters.add("max_tau", my_parameter_function, size=1, ) - parameters.add("min_tau", my_parameter_function, size=1, ) - parameters.add("to_test", my_parameter_function, size=1, ) - + if parameter_option == 0 or parameter_option == 1: + parameters.add( + "max_tau", + my_parameter_function, + size=1, + ) + parameters.add( + "min_tau", + my_parameter_function, + size=1, + ) parameter_init["max_tau"] = 1 parameter_init["min_tau"] = -1 - parameter_init["to_test"] = 1000 - parameter_bounds.add("max_tau", min_bound=0, max_bound=tau_max, interpolation=InterpolationType.CONSTANT) parameter_bounds.add("min_tau", min_bound=tau_min, max_bound=0, interpolation=InterpolationType.CONSTANT) - parameter_bounds.add("to_test", min_bound=100, max_bound=1000, interpolation=InterpolationType.CONSTANT) # Add phase independent objective functions - - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="max_tau", weight=10, quadratic=True) - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_tau", weight=10, quadratic=True) + parameter_objectives.add( + ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, + key="max_tau", + weight=10, + quadratic=True if parameter_option == 0 else False, + ) + + parameter_objectives.add( + ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, + key="min_tau", + weight=10 if parameter_option == 0 else -10, + quadratic=True if parameter_option == 0 else False, + ) # Constraints - constraints.add(custom_constraint_max_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max) - constraints.add(custom_constraint_min_tau, phase=0, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0) - # - constraints.add(custom_constraint_max_tau, phase=1, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max) - constraints.add(custom_constraint_min_tau, phase=1, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0) - - elif method == 2: #todo: does not work - parameters.add("min_max_tau", my_parameter_function, size=2, ) + for phase in range(len(bio_model)): + constraints.add( + custom_constraint_max_tau, phase=phase, node=Node.ALL_SHOOTING, min_bound=0, max_bound=tau_max + ) + constraints.add( + custom_constraint_min_tau, phase=phase, node=Node.ALL_SHOOTING, min_bound=tau_min, max_bound=0 + ) + + elif parameter_option == 2: + parameters.add( + "min_max_tau", + my_parameter_function, + size=2, + ) parameter_init["min_max_tau"] = [-1, 1] - parameter_bounds.add("min_max_tau", - min_bound=[tau_min, 0], - max_bound=[0, tau_max], - interpolation=InterpolationType.CONSTANT - ) + parameter_bounds.add( + "min_max_tau", min_bound=[tau_min, 0], max_bound=[0, tau_max], interpolation=InterpolationType.CONSTANT + ) # Add phase independent objective functions - parameter_objectives.add(ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_max_tau", weight=10, quadratic=True) + parameter_objectives.add( + ObjectiveFcn.Parameter.MINIMIZE_PARAMETER, key="min_max_tau", weight=10, quadratic=True + ) # Constraints - for phase in range(len(n_shooting)): + for phase in range(len(bio_model)): constraints.add( custom_constraint_min_max_tau, - phase=phase, node=Node.ALL_SHOOTING, - min_bound=[tau_min, 0], max_bound=[0, tau_max] + phase=phase, + node=Node.ALL_SHOOTING, + min_bound=[tau_min, 0], + max_bound=[0, tau_max], ) objective_functions = ObjectiveList() # Add objective functions for phase in range(len(n_shooting)): - objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10, phase=phase, min_bound=0.5, max_bound=3) #was w=10 - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=phase) #was w=1 - - + objective_functions.add( + ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10, phase=phase, min_bound=0.5, max_bound=3 + ) # was w=10 + objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=phase) # was w=1 constraints.add( ConstraintFcn.TRACK_STATE, key="q", phase=0, node=Node.START, target=[np.pi + 0.01, 0] @@ -167,81 +192,121 @@ def prepare_ocp( def main(): # --- Prepare the ocp --- # - ocp = prepare_ocp() - - # --- Solve the ocp --- # - sol = ocp.solve() - - print(sol.parameters) - # sol.print_cost() - - import matplotlib - from matplotlib import pyplot as plt - matplotlib.use('Qt5Agg') - - states = sol.decision_states() - controls = sol.decision_controls() - - q = np.vstack([ - np.array([item.flatten() for item in states[0]["q"]]), - np.array([item.flatten() for item in states[1]["q"]]) - ]) - qdot = np.vstack([ - np.array([item.flatten() for item in states[0]["qdot"]]), - np.array([item.flatten() for item in states[1]["qdot"]]) - ]) - tau = np.vstack([ - np.array([item.flatten() for item in controls[0]["tau"]]), - np.array([[np.nan]]), - np.array([item.flatten() for item in controls[1]["tau"]]), - np.array([[np.nan]]), - ]) - time = np.vstack([ - np.array([item.full().flatten()[0] for item in sol.stepwise_time()[0]]).reshape((-1, 1)), - np.array([item.full().flatten()[0] for item in sol.stepwise_time()[1]]).reshape((-1, 1)) + sol.stepwise_time()[0][-1].full(), - ]) - - fig, axs = plt.subplots(2, 2, figsize=(10, 15)) - - # Plotting q solutions for both DOFs - axs[0, 0].plot(time, q) - axs[0, 0].set_title("Joint coordinates") - axs[0, 0].set_ylabel("q") - axs[0, 0].set_xlabel("Time [s]") - axs[0, 0].grid(True) - - axs[0, 1].plot(time, qdot) - axs[0, 1].set_title("Joint velocities") - axs[0, 1].set_ylabel("qdot") - axs[0, 1].set_xlabel("Time [s]") - axs[0, 1].grid(True) - - axs[1, 0].step(time, tau) - axs[1, 0].set_title("Generalized forces") - axs[1, 0].set_ylabel("tau") - axs[1, 0].set_xlabel("Time [s]") - axs[1, 0].grid(True) - - if len(sol.parameters) == 1: - for i in range(2): - axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_max_tau"][i], 'k--') - else: - axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["max_tau"], 'k--') - axs[1, 0].step(axs[1, 0].get_xlim(), np.ones([2]) * sol.parameters["min_tau"], 'k--') + fig, axs = plt.subplots(1, 3) + axs[0].set_title("Joint coordinates") + axs[0].set_ylabel("q [°]") + axs[1].set_title("Joint velocities") + axs[1].set_ylabel("qdot [°/s]") + axs[2].set_title("Generalized forces") + axs[2].set_ylabel("tau [Nm]") + for ax in [0, 1, 2]: + axs[ax].set_xlabel("Time [s]") + axs[ax].grid(True) + + for parameter_option in range(3): + ocp = prepare_ocp(parameter_option=parameter_option) + + # --- Solve the ocp --- # + sol = ocp.solve() + + print(sol.parameters) + + # --- Show results --- # + # sol.animate() + # sol.graphs(show_bounds=True) + # sol.print_cost() + + states = sol.decision_states() + controls = sol.decision_controls() + + q = np.vstack( + [ + np.array([item.flatten() for item in states[0]["q"]]), + np.array([item.flatten() for item in states[1]["q"]]), + ] + ) + qdot = np.vstack( + [ + np.array([item.flatten() for item in states[0]["qdot"]]), + np.array([item.flatten() for item in states[1]["qdot"]]), + ] + ) + tau = np.vstack( + [ + np.array([item.flatten() for item in controls[0]["tau"]]), + np.array([[np.nan]]), + np.array([item.flatten() for item in controls[1]["tau"]]), + np.array([[np.nan]]), + ] + ) + time = np.vstack( + [ + np.array([item.full().flatten()[0] for item in sol.stepwise_time()[0]]).reshape((-1, 1)), + np.array([item.full().flatten()[0] for item in sol.stepwise_time()[1]]).reshape((-1, 1)) + + sol.stepwise_time()[0][-1].full(), + ] + ) + + # Plotting q solutions for both DOFs + axs[0].plot(time, q * 180 / np.pi) + axs[1].plot(time, qdot * 180 / np.pi) + axs[2].step(time, tau) + xlim = np.asarray(axs[2].get_xlim()) + xlim = np.hstack([xlim, np.nan, xlim]) + if parameter_option == 2: + min_max = np.hstack( + [ + np.ones([2]) * sol.parameters["min_max_tau"][0], + np.nan, + np.ones([2]) * sol.parameters["min_max_tau"][1], + ] + ) + else: + min_max = np.hstack( + [np.ones([2]) * sol.parameters["min_tau"], np.nan, np.ones([2]) * sol.parameters["max_tau"]] + ) + + axs[2].plot(xlim, min_max, "k--") plt.tight_layout() plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) # Display the plot - plt.show() + axs[0].legend( + { + "q_0 (method 0)", + "q_1 (method 0)", + "q_0 (method 1)", + "q_1 (method 1)", + "q_0 (method 2)", + "q_1 (method 2)", + } + ) + axs[1].legend( + { + "qdot_0 (method 0)", + "qdot_1 (method 0)", + "qdot_0 (method 1)", + "qdot_1 (method 1)", + "qdot_0 (method 2)", + "qdot_1 (method 2)", + } + ) + axs[2].legend( + { + "tau_1 (method 0)", + "min-max (method 0)", + "tau_1 (method 1)", + "min-max (method 1)", + "tau_1 (method 2)", + "min-max (method 2)", + } + ) - # --- Show results --- # - sol.animate() - sol.graphs(show_bounds=True) + plt.show() if __name__ == "__main__": main() - From 3bf28e2145e19a17df13fa510d23f54a288742ce Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Sun, 28 Jan 2024 14:54:23 +0400 Subject: [PATCH 4/6] Remove some files --- .../getting_started/double_pendulum.py | 165 ---------------- .../models/double_pendulum2.bioMod | 56 ------ bioptim/examples/getting_started/pendulum.py | 165 ---------------- .../examples/torque_driven_ocp/spring_load.py | 183 ------------------ 4 files changed, 569 deletions(-) delete mode 100644 bioptim/examples/getting_started/double_pendulum.py delete mode 100644 bioptim/examples/getting_started/models/double_pendulum2.bioMod delete mode 100644 bioptim/examples/getting_started/pendulum.py delete mode 100644 bioptim/examples/torque_driven_ocp/spring_load.py diff --git a/bioptim/examples/getting_started/double_pendulum.py b/bioptim/examples/getting_started/double_pendulum.py deleted file mode 100644 index c20e281d3..000000000 --- a/bioptim/examples/getting_started/double_pendulum.py +++ /dev/null @@ -1,165 +0,0 @@ -""" -A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward -while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. - -This simple example is a good place to start investigating bioptim as it describes the most common dynamics out there -(the joint torque driven), it defines an objective function and some boundaries and initial guesses - -During the optimization process, the graphs are updated real-time (even though it is a bit too fast and short to really -appreciate it). Finally, once it finished optimizing, it animates the model using the optimal solution -""" - -import platform - -from bioptim import ( - OptimalControlProgram, - DynamicsFcn, - Dynamics, - BoundsList, - InitialGuessList, - ObjectiveFcn, - ObjectiveList, - OdeSolver, - OdeSolverBase, - CostType, - Solver, - BiorbdModel, - ControlType, - PhaseDynamics, -) - - -def prepare_ocp( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - ode_solver: OdeSolverBase = OdeSolver.COLLOCATION(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, - control_type: ControlType = ControlType.CONSTANT, -) -> OptimalControlProgram: - """ - The initialization of an ocp - - Parameters - ---------- - biorbd_model_path: str - The path to the biorbd model - final_time: float - The time in second required to perform the task - n_shooting: int - The number of shooting points to define int the direct multiple shooting program - ode_solver: OdeSolverBase = OdeSolver.RK4() - Which type of OdeSolver to use - use_sx: bool - If the SX variable should be used instead of MX (can be extensive on RAM) - n_threads: int - The number of threads to use in the paralleling (1 = no parallel computing) - phase_dynamics: PhaseDynamics - If the dynamics equation within a phase is unique or changes at each node. - PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within - a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces - are applied at each 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) - control_type: ControlType - The type of the controls - - Returns - ------- - The OptimalControlProgram ready to be solved - """ - - bio_model = BiorbdModel(biorbd_model_path) - - # Add objective functions - - - objective_functions = ObjectiveList() - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=0, weight=1) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=500) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=2, weight=500) - - - # Dynamics - dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) - - # Path bounds - x_bounds = BoundsList() - x_bounds["q"] = bio_model.bounds_from_ranges("q") - - x_bounds["q"][:, [0, -1]] = 0 # Start and end at 0... - x_bounds["q"][1, -1] = 3.14 # ...but end with pendulum 180 degrees rotated - - x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") - x_bounds["qdot"][:, [0, -1]] = 0 # Start and end without any velocity - - # Initial guess (optional since it is 0, we show how to initialize anyway) - x_init = InitialGuessList() - x_init["q"] = [0] * bio_model.nb_q - x_init["qdot"] = [0] * bio_model.nb_qdot - - # Define control path bounds - n_tau = bio_model.nb_tau - u_bounds = BoundsList() - u_bounds["tau"] = [-150, -2, -2], [150, 2, 2] # Limit the strength of the pendulum to (-100 to 100)... - - # Initial guess (optional since it is 0, we show how to initialize anyway) - # u_init = InitialGuessList() - # u_init["tau"] = [0] * n_tau - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - # x_init=x_init, - # u_init=u_init, - x_bounds=x_bounds, - u_bounds=u_bounds, - objective_functions=objective_functions, - ode_solver=ode_solver, - use_sx=use_sx, - n_threads=n_threads, - control_type=control_type, - ) - - -def main(): - """ - If pendulum is run as a script, it will perform the optimization and animates it - """ - - # --- Prepare the ocp --- # - ocp = prepare_ocp(biorbd_model_path="models/double_pendulum2.bioMod", final_time=1, n_shooting=100, n_threads=5) - - # Custom plots - ocp.add_plot_penalty(CostType.ALL) - - # --- If one is interested in checking the conditioning of the problem, they can uncomment the following line --- # - # ocp.check_conditioning() - - # --- Print ocp structure --- # - ocp.print(to_console=False, to_graph=False) - - # --- Solve the ocp. Please note that online graphics only works with the Linux operating system --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) - sol.print_cost() - - # --- Show the results (graph or animation) --- # - # sol.graphs(show_bounds=True) - sol.animate(n_frames=100) - - # # --- Save the solution --- # - # import pickle - # with open("pendulum.pkl", "wb") as file: - # del sol.ocp - # pickle.dump(sol, file) - - -if __name__ == "__main__": - main() diff --git a/bioptim/examples/getting_started/models/double_pendulum2.bioMod b/bioptim/examples/getting_started/models/double_pendulum2.bioMod deleted file mode 100644 index 03e7584de..000000000 --- a/bioptim/examples/getting_started/models/double_pendulum2.bioMod +++ /dev/null @@ -1,56 +0,0 @@ -version 4 - -segment Seg1 - translations y - rotations x - ranges - -2 2 - -10*pi 10*pi - mass 1 - inertia - 0.0391 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 1 - marker marker_1 - parent Seg1 - position 0 0 0 - endmarker - - // Marker 2 - marker marker_2 - parent Seg1 - position 0 0 -1 - endmarker - -segment Seg2 - parent Seg1 - rotations x - rtinmatrix 0 - rt 0 0 0 xyz 0 0 -1 - ranges - -10*pi 10*pi - mass 1 - inertia - 0.0391 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 Seg2 - position 0 0 0 - endmarker - - // Marker 4 - marker marker_4 - parent Seg2 - position 0 0 -1 - endmarker diff --git a/bioptim/examples/getting_started/pendulum.py b/bioptim/examples/getting_started/pendulum.py deleted file mode 100644 index 20025d8de..000000000 --- a/bioptim/examples/getting_started/pendulum.py +++ /dev/null @@ -1,165 +0,0 @@ -""" -A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward -while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. - -This simple example is a good place to start investigating bioptim as it describes the most common dynamics out there -(the joint torque driven), it defines an objective function and some boundaries and initial guesses - -During the optimization process, the graphs are updated real-time (even though it is a bit too fast and short to really -appreciate it). Finally, once it finished optimizing, it animates the model using the optimal solution -""" - -import platform - -from bioptim import ( - OptimalControlProgram, - DynamicsFcn, - Dynamics, - BoundsList, - InitialGuessList, - ObjectiveList, - ObjectiveFcn, - Objective, - Node, - OdeSolver, - OdeSolverBase, - CostType, - Solver, - BiorbdModel, - ControlType, - PhaseDynamics, -) - - -def prepare_ocp( - biorbd_model_path: str, - final_time: float, - n_shooting: int, - ode_solver: OdeSolverBase = OdeSolver.RK4(), - use_sx: bool = True, - n_threads: int = 1, - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, - control_type: ControlType = ControlType.CONSTANT, -) -> OptimalControlProgram: - """ - The initialization of an ocp - - Parameters - ---------- - biorbd_model_path: str - The path to the biorbd model - final_time: float - The time in second required to perform the task - n_shooting: int - The number of shooting points to define int the direct multiple shooting program - ode_solver: OdeSolverBase = OdeSolver.RK4() - Which type of OdeSolver to use - use_sx: bool - If the SX variable should be used instead of MX (can be extensive on RAM) - n_threads: int - The number of threads to use in the paralleling (1 = no parallel computing) - phase_dynamics: PhaseDynamics - If the dynamics equation within a phase is unique or changes at each node. - PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within - a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces - are applied at each 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) - control_type: ControlType - The type of the controls - - Returns - ------- - The OptimalControlProgram ready to be solved - """ - - bio_model = BiorbdModel(biorbd_model_path) - - # Add objective functions - - objective_functions = ObjectiveList() - #Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight=-1) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight=-2) - objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", quadratic=False, weight= 1) - - - # Dynamics - dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) - - # Path bounds - x_bounds = BoundsList() - x_bounds["q"] = bio_model.bounds_from_ranges("q") - x_bounds["q"][:, [0, -1]] = 0 # Start and end at 0... - x_bounds["q"][1, -1] = 3.14 # ...but end with pendulum 180 degrees rotated - x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") - x_bounds["qdot"][:, [0, -1]] = 0 # Start and end without any velocity - - # Initial guess (optional since it is 0, we show how to initialize anyway) - x_init = InitialGuessList() - x_init["q"] = [0] * bio_model.nb_q - x_init["qdot"] = [0] * bio_model.nb_qdot - - # Define control path bounds - n_tau = bio_model.nb_tau - u_bounds = BoundsList() - u_bounds["tau"] = [-100] * n_tau, [100] * n_tau # Limit the strength of the pendulum to (-100 to 100)... - u_bounds["tau"][1, :] = 0 # ...but remove the capability to actively rotate - - # Initial guess (optional since it is 0, we show how to initialize anyway) - u_init = InitialGuessList() - u_init["tau"] = [0] * n_tau - - return OptimalControlProgram( - bio_model, - dynamics, - n_shooting, - final_time, - x_init=x_init, - u_init=u_init, - x_bounds=x_bounds, - u_bounds=u_bounds, - objective_functions=objective_functions, - ode_solver=ode_solver, - use_sx=use_sx, - n_threads=n_threads, - control_type=control_type, - ) - - -def main(): - """ - If pendulum is run as a script, it will perform the optimization and animates it - """ - - # --- Prepare the ocp --- # - ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=1, n_shooting=400, n_threads=2) - - # Custom plots - ocp.add_plot_penalty(CostType.ALL) - - # --- If one is interested in checking the conditioning of the problem, they can uncomment the following line --- # - # ocp.check_conditioning() - - # --- Print ocp structure --- # - ocp.print(to_console=False, to_graph=False) - - # --- Solve the ocp. Please note that online graphics only works with the Linux operating system --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) - sol.print_cost() - - # --- Show the results (graph or animation) --- # - # sol.graphs(show_bounds=True) - sol.animate(n_frames=100) - - # # --- Save the solution --- # - # import pickle - # with open("pendulum.pkl", "wb") as file: - # del sol.ocp - # pickle.dump(sol, file) - - -if __name__ == "__main__": - main() diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py deleted file mode 100644 index b09fc9dab..000000000 --- a/bioptim/examples/torque_driven_ocp/spring_load.py +++ /dev/null @@ -1,183 +0,0 @@ -""" -This trivial spring example targets to have the highest upward velocity. It is however only able to load a spring by -pulling downward and afterward to let it go so it gains velocity. It is designed to show how one can use the external -forces to interact with the body. -""" - -import platform - -from casadi import MX, vertcat -import numpy as np -from bioptim import ( - BiorbdModel, - OptimalControlProgram, - Dynamics, - ConfigureProblem, - Objective, - DynamicsFunctions, - ObjectiveFcn, - BoundsList, - NonLinearProgram, - Solver, - DynamicsEvaluation, - PhaseDynamics, - Node, -) - - -def custom_dynamic( - time: MX, states: MX, controls: MX, parameters: MX, algebraic_states: MX, nlp: NonLinearProgram -) -> DynamicsEvaluation: - """ - The dynamics of the system using an external force (see custom_dynamics for more explanation) - - Parameters - ---------- - time: MX - The current time of the system - states: MX - The current states of the system - controls: MX - The current controls of the system - parameters: MX - The current parameters of the system - algebraic_states: MX - The current algebraic states of the system - nlp: NonLinearProgram - A reference to the phase of the ocp - - Returns - ------- - The state derivative - """ - - q = DynamicsFunctions.get(nlp.states["q"], states) - qdot = DynamicsFunctions.get(nlp.states["qdot"], states) - tau = DynamicsFunctions.get(nlp.controls["tau"], controls) - - force_vector = MX.zeros(6) - force_vector[5] = 100 * q[0] ** 2 #spring force - - qddot = nlp.model.forward_dynamics(q, qdot, tau, external_forces=[["Point", force_vector]]) - - return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) - - -def custom_configure(ocp: OptimalControlProgram, nlp: NonLinearProgram): - """ - The configuration of the dynamics (see custom_dynamics for more explanation) - - Parameters - ---------- - ocp: OptimalControlProgram - A reference to the ocp - nlp: NonLinearProgram - A reference to the phase of the ocp - """ - ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) - ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False) - ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True) - ConfigureProblem.configure_dynamics_function(ocp, nlp, custom_dynamic) - - -def prepare_ocp( - biorbd_model_path: str = "models/mass_point.bioMod", - phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, - expand_dynamics: bool = True, -): - # BioModel path - m = BiorbdModel(biorbd_model_path) - m.set_gravity(np.array((0, 0, 0))) - - # Add objective functions (high upward velocity at end point) - objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="qdot", index=0, weight=-1, - node=Node.END,)# quadratic=False) - - # Dynamics - dynamics = Dynamics( - custom_configure, - dynamic_function=custom_dynamic, - expand_dynamics=expand_dynamics, - phase_dynamics=phase_dynamics, - ) - - # Path constraint - x_bounds = BoundsList() - x_bounds["q"] = [-1] * m.nb_q, [1] * m.nb_q - x_bounds["q"][:, 0] = 0 - x_bounds["qdot"] = [-100] * m.nb_qdot, [100] * m.nb_qdot - x_bounds["qdot"][:, 0] = 0 - - # Define control path constraint - u_bounds = BoundsList() - u_bounds["tau"] = [-100] * m.nb_tau, [0] * m.nb_tau - - return OptimalControlProgram( - m, - dynamics, - n_shooting=30, - phase_time=0.5, - x_bounds=x_bounds, - u_bounds=u_bounds, - objective_functions=objective_functions, - ) - - -def main(): - ocp = prepare_ocp() - ocp.print(to_graph=True, to_console=False) - # --- Solve the program --- # - sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) - - - - - import matplotlib - from matplotlib import pyplot as plt - matplotlib.use('Qt5Agg') - - states = sol.decision_states() - controls = sol.decision_controls() - - q = np.array([item.flatten() for item in states["q"]]) - qdot = np.array([item.flatten() for item in states["qdot"]]) - tau = np.vstack([ - np.array([item.flatten() for item in controls["tau"]]), - np.array([[np.nan]]) - ]) - time = np.array([item.full().flatten()[0] for item in sol.stepwise_time()]) - - - fig, axs = plt.subplots(2, 2, figsize=(10, 15)) - - # Plotting q solutions for both DOFs - axs[0, 0].plot(time, q) - axs[0, 0].set_title("Joint coordinates") - axs[0, 0].set_ylabel("q") - axs[0, 0].set_xlabel("Time [s]") - axs[0, 0].grid(True) - - axs[0, 1].plot(time, qdot) - axs[0, 1].set_title("Joint velocities") - axs[0, 1].set_ylabel("qdot") - axs[0, 1].set_xlabel("Time [s]") - axs[0, 1].grid(True) - - axs[1, 0].step(time, tau) - axs[1, 0].set_title("Generalized forces") - axs[1, 0].set_ylabel("tau") - axs[1, 0].set_xlabel("Time [s]") - axs[1, 0].grid(True) - - plt.tight_layout() - plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) - - # Display the plot - plt.show() - - # --- Show results --- # - sol.animate() - - -if __name__ == "__main__": - main() From 399bc755374e35db3f566d564f8f3a95e15bcbb3 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 2 Feb 2024 13:12:37 +0400 Subject: [PATCH 5/6] improved figures --- ...imize_maximum_torque_by_extra_parameter.py | 55 +++++++---------- ...ax_torque_by_extra_parameter_multiphase.py | 61 +++++++------------ 2 files changed, 44 insertions(+), 72 deletions(-) diff --git a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py index 38cfab27c..539c9b602 100644 --- a/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py +++ b/bioptim/examples/torque_driven_ocp/minimize_maximum_torque_by_extra_parameter.py @@ -125,8 +125,6 @@ def prepare_ocp( constraints.add(ConstraintFcn.TRACK_STATE, node=Node.START, key="qdot", target=[0, 0]) constraints.add(ConstraintFcn.TRACK_STATE, node=Node.END, key="q", target=[3 * np.pi, 0]) - - return OptimalControlProgram( bio_model, dynamics, @@ -156,15 +154,16 @@ def main(): axs[ax].set_xlabel("Time [s]") axs[ax].grid(True) - for method_bound_states in range(2): + linestyles = ["solid", "dashed"] + + for i, linestyle in enumerate(linestyles): # --- Prepare the ocp --- # - ocp = prepare_ocp(method_bound_states=method_bound_states) + ocp = prepare_ocp(method_bound_states=i) # --- Solve the ocp --- # sol = ocp.solve() # sol.print_cost() - # --- Show results --- # # sol.animate() # sol.graphs(show_bounds=True) @@ -182,44 +181,34 @@ def main(): print("min-max tau: ", np.nanmin(tau), np.nanmax(tau)) # Plotting q solutions for both DOFs - axs[0].plot(time, q * 180 / np.pi) - axs[1].plot(time, qdot * 180 / np.pi) - axs[2].step(time, tau) + axs[0].plot( + time, + q * 180 / np.pi, + linestyle=linestyle, + label=[f"q_0 (method {i})", f"q_1 (method {i})"], + ) + axs[1].plot( + time, + qdot * 180 / np.pi, + linestyle=linestyle, + label=[f"qdot_0 (method {i})", f"qdot_1 (method {i})"], + ) + axs[2].step(time, tau, linestyle=linestyle, label=f"tau (method {i})", where="post") xlim = np.asarray(axs[2].get_xlim()) xlim = np.hstack([xlim, np.nan, xlim]) min_max = np.hstack( [np.ones([2]) * sol.parameters["min_tau"], np.nan, np.ones([2]) * sol.parameters["max_tau"]] ) - axs[2].plot(xlim, min_max, "k--") + axs[2].plot(xlim, min_max, linestyle=(0, (5, 10)), label=f"params - bounds (method {i})") plt.tight_layout() plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) # Display the plot - axs[0].legend( - { - "q_0 (method 0)", - "q_1 (method 0)", - "q_0 (method 1)", - "q_1 (method 1)", - } - ) - axs[1].legend( - { - "qdot_0 (method 0)", - "qdot_1 (method 0)", - "qdot_0 (method 1)", - "qdot_1 (method 1)", - } - ) - axs[2].legend( - { - "tau_1 (method 0)", - "min-max (method 0)", - "tau_1 (method 1)", - "min-max (method 1)", - } - ) + axs[0].legend() + axs[1].legend() + axs[2].legend() + plt.show() diff --git a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py index e06d5ce32..1f2d81877 100644 --- a/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py +++ b/bioptim/examples/torque_driven_ocp/minmax_torque_by_extra_parameter_multiphase.py @@ -199,12 +199,15 @@ def main(): axs[1].set_ylabel("qdot [°/s]") axs[2].set_title("Generalized forces") axs[2].set_ylabel("tau [Nm]") + + linestyles = ["solid", "dashed", "dotted"] + for ax in [0, 1, 2]: axs[ax].set_xlabel("Time [s]") axs[ax].grid(True) - for parameter_option in range(3): - ocp = prepare_ocp(parameter_option=parameter_option) + for i, linestyle in enumerate(linestyles): + ocp = prepare_ocp(parameter_option=i) # --- Solve the ocp --- # sol = ocp.solve() @@ -248,12 +251,22 @@ def main(): ) # Plotting q solutions for both DOFs - axs[0].plot(time, q * 180 / np.pi) - axs[1].plot(time, qdot * 180 / np.pi) - axs[2].step(time, tau) + axs[0].plot( + time, + q * 180 / np.pi, + linestyle=linestyle, + label=[f"q_0 (method {i})", f"q_1 (method {i})"], + ) + axs[1].plot( + time, + qdot * 180 / np.pi, + linestyle=linestyle, + label=[f"qdot_0 (method {i})", f"qdot_1 (method {i})"], + ) + axs[2].step(time, tau, linestyle=linestyle, label=f"tau (method {i})", where="post") xlim = np.asarray(axs[2].get_xlim()) xlim = np.hstack([xlim, np.nan, xlim]) - if parameter_option == 2: + if i == 2: min_max = np.hstack( [ np.ones([2]) * sol.parameters["min_max_tau"][0], @@ -266,44 +279,14 @@ def main(): [np.ones([2]) * sol.parameters["min_tau"], np.nan, np.ones([2]) * sol.parameters["max_tau"]] ) - axs[2].plot(xlim, min_max, "k--") + axs[2].plot(xlim, min_max, linestyle=(0, (5, 10)), label=f"params - bounds (method {i})") plt.tight_layout() plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.3) # Display the plot - axs[0].legend( - { - "q_0 (method 0)", - "q_1 (method 0)", - "q_0 (method 1)", - "q_1 (method 1)", - "q_0 (method 2)", - "q_1 (method 2)", - } - ) - - axs[1].legend( - { - "qdot_0 (method 0)", - "qdot_1 (method 0)", - "qdot_0 (method 1)", - "qdot_1 (method 1)", - "qdot_0 (method 2)", - "qdot_1 (method 2)", - } - ) - - axs[2].legend( - { - "tau_1 (method 0)", - "min-max (method 0)", - "tau_1 (method 1)", - "min-max (method 1)", - "tau_1 (method 2)", - "min-max (method 2)", - } - ) + for ax in range(3): + axs[ax].legend() plt.show() From 43546d1e9b4df4d5f03264317e53f518a0204653 Mon Sep 17 00:00:00 2001 From: mickaelbegon Date: Fri, 2 Feb 2024 13:17:24 +0400 Subject: [PATCH 6/6] Re-add pendulum.py and spring_load.py --- bioptim/examples/getting_started/pendulum.py | 158 ++++++++++ .../examples/torque_driven_ocp/spring_load.py | 275 ++++++++++++++++++ 2 files changed, 433 insertions(+) create mode 100644 bioptim/examples/getting_started/pendulum.py create mode 100644 bioptim/examples/torque_driven_ocp/spring_load.py diff --git a/bioptim/examples/getting_started/pendulum.py b/bioptim/examples/getting_started/pendulum.py new file mode 100644 index 000000000..f5deec315 --- /dev/null +++ b/bioptim/examples/getting_started/pendulum.py @@ -0,0 +1,158 @@ +""" +A very simple yet meaningful optimal control program consisting in a pendulum starting downward and ending upward +while requiring the minimum of generalized forces. The solver is only allowed to move the pendulum sideways. + +This simple example is a good place to start investigating bioptim as it describes the most common dynamics out there +(the joint torque driven), it defines an objective function and some boundaries and initial guesses + +During the optimization process, the graphs are updated real-time (even though it is a bit too fast and short to really +appreciate it). Finally, once it finished optimizing, it animates the model using the optimal solution +""" + +import platform + +from bioptim import ( + OptimalControlProgram, + DynamicsFcn, + Dynamics, + BoundsList, + InitialGuessList, + ObjectiveFcn, + Objective, + OdeSolver, + OdeSolverBase, + CostType, + Solver, + BiorbdModel, + ControlType, + PhaseDynamics, +) + + +def prepare_ocp( + biorbd_model_path: str, + final_time: float, + n_shooting: int, + ode_solver: OdeSolverBase = OdeSolver.RK4(), + use_sx: bool = True, + n_threads: int = 1, + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + control_type: ControlType = ControlType.CONSTANT, +) -> OptimalControlProgram: + """ + The initialization of an ocp + + Parameters + ---------- + biorbd_model_path: str + The path to the biorbd model + final_time: float + The time in second required to perform the task + n_shooting: int + The number of shooting points to define int the direct multiple shooting program + ode_solver: OdeSolverBase = OdeSolver.RK4() + Which type of OdeSolver to use + use_sx: bool + If the SX variable should be used instead of MX (can be extensive on RAM) + n_threads: int + The number of threads to use in the paralleling (1 = no parallel computing) + phase_dynamics: PhaseDynamics + If the dynamics equation within a phase is unique or changes at each node. + PhaseDynamics.SHARED_DURING_THE_PHASE is much faster, but lacks the capability to have changing dynamics within + a phase. A good example of when PhaseDynamics.ONE_PER_NODE should be used is when different external forces + are applied at each 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) + control_type: ControlType + The type of the controls + + Returns + ------- + The OptimalControlProgram ready to be solved + """ + + bio_model = BiorbdModel(biorbd_model_path) + + # Add objective functions + objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") + + # Dynamics + dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand_dynamics=expand_dynamics, phase_dynamics=phase_dynamics) + + # Path bounds + x_bounds = BoundsList() + x_bounds["q"] = bio_model.bounds_from_ranges("q") + x_bounds["q"][:, [0, -1]] = 0 # Start and end at 0... + x_bounds["q"][1, -1] = 3.14 # ...but end with pendulum 180 degrees rotated + x_bounds["qdot"] = bio_model.bounds_from_ranges("qdot") + x_bounds["qdot"][:, [0, -1]] = 0 # Start and end without any velocity + + # Initial guess (optional since it is 0, we show how to initialize anyway) + x_init = InitialGuessList() + x_init["q"] = [0] * bio_model.nb_q + x_init["qdot"] = [0] * bio_model.nb_qdot + + # Define control path bounds + n_tau = bio_model.nb_tau + u_bounds = BoundsList() + u_bounds["tau"] = [-100] * n_tau, [100] * n_tau # Limit the strength of the pendulum to (-100 to 100)... + u_bounds["tau"][1, :] = 0 # ...but remove the capability to actively rotate + + # Initial guess (optional since it is 0, we show how to initialize anyway) + u_init = InitialGuessList() + u_init["tau"] = [0] * n_tau + + return OptimalControlProgram( + bio_model, + dynamics, + n_shooting, + final_time, + x_init=x_init, + u_init=u_init, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + ode_solver=ode_solver, + use_sx=use_sx, + n_threads=n_threads, + control_type=control_type, + ) + + +def main(): + """ + If pendulum is run as a script, it will perform the optimization and animates it + """ + + # --- Prepare the ocp --- # + ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=1, n_shooting=400, n_threads=2) + + # Custom plots + ocp.add_plot_penalty(CostType.ALL) + + # --- If one is interested in checking the conditioning of the problem, they can uncomment the following line --- # + # ocp.check_conditioning() + + # --- Print ocp structure --- # + ocp.print(to_console=False, to_graph=False) + + # --- Solve the ocp. Please note that online graphics only works with the Linux operating system --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + sol.print_cost() + + # --- Show the results (graph or animation) --- # + # sol.graphs(show_bounds=True) + sol.animate(n_frames=100) + + # # --- Save the solution --- # + # import pickle + # with open("pendulum.pkl", "wb") as file: + # del sol.ocp + # pickle.dump(sol, file) + + +if __name__ == "__main__": + main() diff --git a/bioptim/examples/torque_driven_ocp/spring_load.py b/bioptim/examples/torque_driven_ocp/spring_load.py new file mode 100644 index 000000000..7bdf3853c --- /dev/null +++ b/bioptim/examples/torque_driven_ocp/spring_load.py @@ -0,0 +1,275 @@ +""" +This trivial spring example targets to have the highest upward velocity. It is however only able to load a spring by +pulling downward and afterward to let it go so it gains velocity. It is designed to show how one can use the external +forces to interact with the body. +""" + +import platform + +from casadi import MX, vertcat, sign +import numpy as np +from bioptim import ( + BiorbdModel, + OptimalControlProgram, + Dynamics, + ConfigureProblem, + ObjectiveList, + DynamicsFunctions, + ObjectiveFcn, + BoundsList, + NonLinearProgram, + Solver, + DynamicsEvaluation, + PhaseDynamics, + SolutionMerge, +) +from matplotlib import pyplot as plt + +# scenarios are based on a Mayer term (at Tf) +# 0: maximize upward speed - expected kinematics: negative torque to get as low as possible and release +# 1: maximize downward speed - expected kinematics: positive torque to get as high as possible and release +# 2: minimize quadratic speed - expected kinematics: no torque no move +# 3: maximize quadratic speed - as in 1 +# 4-7 same as 0-3 but for COMdot + +scenarios = { + 0: { + "label": "max qdot(T)", + "quad": False, + "sign": -1, + "tau_min": -100, + "tau_max": 0, + "check_tau": -1, + "check_qdot(T)": 1, + }, + 1: { + "label": "min qdot(T)", + "quad": False, + "sign": 1, + "tau_min": 0, + "tau_max": 100, + "check_tau": 1, + "check_qdot(T)": -1, + }, + 2: { + "label": "min qdot(T)**2", + "quad": True, + "sign": 1, + "tau_min": -100, + "tau_max": 100, + "check_tau": 1, + "check_qdot(T)": 1, + }, + 3: { + "label": "max qdot(T)**2", + "quad": True, + "sign": -1, + "tau_min": -100, + "tau_max": 0, + "check_tau": -1, + "check_qdot(T)": 1, + }, + 4: { + "label": "max COMdot(T)", + "quad": False, + "sign": -1, + "tau_min": -100, + "tau_max": 0, + "check_tau": -1, + "check_qdot(T)": 1, + }, + 5: { + "label": "min COMdot(T)", + "quad": False, + "sign": 1, + "tau_min": 0, + "tau_max": 100, + "check_tau": 1, + "check_qdot(T)": -1, + }, + 6: { + "label": "min COMdot(T)**2", + "quad": True, + "sign": 1, + "tau_min": -100, + "tau_max": 100, + "check_tau": 1, + "check_qdot(T)": 1, + }, + 7: { + "label": "max COMdot(T)**2", + "quad": True, + "sign": -1, + "tau_min": -100, + "tau_max": 0, + "check_tau": -1, + "check_qdot(T)": 1, + }, +} + + +def custom_dynamic( + time: MX, states: MX, controls: MX, parameters: MX, algebraic_states: MX, nlp: NonLinearProgram +) -> DynamicsEvaluation: + """ + The dynamics of the system using an external force (see custom_dynamics for more explanation) + + Parameters + ---------- + time: MX + The current time of the system + states: MX + The current states of the system + controls: MX + The current controls of the system + parameters: MX + The current parameters of the system + algebraic_states: MX + The current algebraic states of the system + nlp: NonLinearProgram + A reference to the phase of the ocp + + Returns + ------- + The state derivative + """ + + q = DynamicsFunctions.get(nlp.states["q"], states) + qdot = DynamicsFunctions.get(nlp.states["qdot"], states) + tau = DynamicsFunctions.get(nlp.controls["tau"], controls) + + force_vector = MX.zeros(6) + stiffness = 100 + force_vector[5] = -sign(q[0]) * stiffness * q[0] ** 2 # traction-compression spring + + qddot = nlp.model.forward_dynamics(q, qdot, tau, [["Point", force_vector]]) + + return DynamicsEvaluation(dxdt=vertcat(qdot, qddot), defects=None) + + +def custom_configure(ocp: OptimalControlProgram, nlp: NonLinearProgram): + """ + The configuration of the dynamics (see custom_dynamics for more explanation) + + Parameters + ---------- + ocp: OptimalControlProgram + A reference to the ocp + nlp: NonLinearProgram + A reference to the phase of the ocp + """ + ConfigureProblem.configure_q(ocp, nlp, as_states=True, as_controls=False) + ConfigureProblem.configure_qdot(ocp, nlp, as_states=True, as_controls=False) + ConfigureProblem.configure_tau(ocp, nlp, as_states=False, as_controls=True) + ConfigureProblem.configure_dynamics_function(ocp, nlp, custom_dynamic) + + +def prepare_ocp( + biorbd_model_path: str = "models/mass_point.bioMod", + phase_dynamics: PhaseDynamics = PhaseDynamics.SHARED_DURING_THE_PHASE, + expand_dynamics: bool = True, + phase_time: float = 0.5, + n_shooting: float = 30, + scenario=1, +): + # BioModel path + m = BiorbdModel(biorbd_model_path) + m.set_gravity(np.array((0, 0, 0))) + + weight = 1 + + # Add objective functions (high upward velocity at end point) + objective_functions = ObjectiveList() + + if "qdot" in scenarios[scenario]["label"]: + objective_functions.add( + ObjectiveFcn.Mayer.MINIMIZE_STATE, + key="qdot", + index=0, + weight=weight * scenarios[scenario]["sign"], + quadratic=scenarios[scenario]["quad"], + ) + elif "COMdot" in scenarios[scenario]["label"]: + objective_functions.add( + ObjectiveFcn.Mayer.MINIMIZE_COM_VELOCITY, + index=2, + weight=weight * scenarios[scenario]["sign"], + quadratic=scenarios[scenario]["quad"], + ) + + objective_functions.add( + ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, + key="tau", + weight=1e-5, + quadratic=True, + ) + + # Dynamics + dynamics = Dynamics( + custom_configure, + dynamic_function=custom_dynamic, + expand_dynamics=expand_dynamics, + phase_dynamics=phase_dynamics, + ) + + # Path constraint + x_bounds = BoundsList() + x_bounds["q"] = [-1] * m.nb_q, [1] * m.nb_q + x_bounds["q"][:, 0] = 0 + x_bounds["qdot"] = [-100] * m.nb_qdot, [100] * m.nb_qdot + x_bounds["qdot"][:, 0] = 0 + + # Define control path constraint + u_bounds = BoundsList() + u_bounds["tau"] = scenarios[scenario]["tau_min"] * m.nb_tau, scenarios[scenario]["tau_max"] * m.nb_tau + + return OptimalControlProgram( + m, + dynamics, + n_shooting=n_shooting, + phase_time=phase_time, + x_bounds=x_bounds, + u_bounds=u_bounds, + objective_functions=objective_functions, + ) + + +def main(): + phase_time = 0.5 + n_shooting = 30 + fig, axs = plt.subplots(1, 3) + + for scenario in range(8): # in [1]: # + print(scenarios[scenario]["label"]) + ocp = prepare_ocp(phase_time=phase_time, n_shooting=n_shooting, scenario=scenario) + + ocp.print(to_console=True, to_graph=False) + + # --- Solve the program --- # + sol = ocp.solve(Solver.IPOPT(show_online_optim=platform.system() == "Linux")) + q = sol.decision_states(to_merge=SolutionMerge.NODES)["q"] + qdot = sol.decision_states(to_merge=SolutionMerge.NODES)["qdot"] + tau = sol.decision_controls(to_merge=SolutionMerge.NODES)["tau"] + time = np.linspace(0, phase_time, n_shooting + 1) + eps = 1e-6 + + axs[0].plot(time, q.flatten(), label=scenarios[scenario]["label"]) + axs[0].set_title("q") + + axs[1].plot(time, qdot.flatten(), label=scenarios[scenario]["label"]) + axs[1].set_title("qdot") + + axs[2].step(time, np.hstack([tau.flatten(), np.nan]), label=scenarios[scenario]["label"]) + axs[2].set_title("tau") + + # --- Show results --- # + sol.print_cost() + # sol.graphs() + # sol.animate() + + axs[2].legend() + plt.show() + + +if __name__ == "__main__": + main()