def prepare_ocp(biorbd_model_path, n_shooting, tf, ode_solver=OdeSolver.RK4(), use_sx=True): # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, tf, x_init, u_init, x_bounds, u_bounds, ode_solver=ode_solver, use_sx=use_sx, )
def solve_ocp(implicit_dynamics: bool) -> OptimalControlProgram: """ The initialization of ocp with implicit_dynamics as the only argument Parameters ---------- implicit_dynamics: bool implicit Returns ------- The OptimalControlProgram ready to be solved """ model_path = "models/pendulum.bioMod" n_shooting = 200 # The higher it is, the closer implicit and explicit solutions are. ode_solver = OdeSolver.RK2(n_integration_steps=1) time = 1 # --- Prepare the ocp with implicit dynamics --- # ocp = prepare_ocp( biorbd_model_path=model_path, final_time=time, n_shooting=n_shooting, ode_solver=ode_solver, implicit_dynamics=implicit_dynamics, ) # --- Custom Plots --- # ocp.add_plot_penalty(CostType.ALL) # --- Solve the ocp --- # sol_opt = Solver.IPOPT(show_online_optim=False) sol = ocp.solve(sol_opt) return sol
def test_plot_merged_graphs(): # Load graphs_one_phase from bioptim.examples.muscle_driven_ocp import muscle_excitations_tracker as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) # Define the problem model_path = bioptim_folder + "/models/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.1 n_shooting = 5 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data(biorbd_model, final_time, n_shooting) biorbd_model = biorbd.Model(model_path) # To prevent from free variable, the model must be reloaded ocp = ocp_module.prepare_ocp( biorbd_model, final_time, n_shooting, markers_ref, muscle_excitations_ref, x_ref[: biorbd_model.nbQ(), :].T, ode_solver=OdeSolver.RK4(), use_residual_torque=True, kin_data_to_track="markers", ) sol = ocp.solve() sol.graphs(automatically_organize=False)
def test_acados_custom_dynamics(problem_type_custom): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.getting_started import custom_dynamics as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS()) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -2], np.array((0, 9.81, -2.27903226)))
def test_acados_custom_dynamics(problem_type_custom): bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module( bioptim_folder + "/examples/getting_started/custom_dynamics.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/getting_started/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -1], np.array( (0, 9.81, -2.27903226)))
def test_soft_contact(): from bioptim.examples.torque_driven_ocp import example_soft_contact as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ode_solver = OdeSolver.RK8() ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/soft_contact_sphere.bioMod", final_time=0.37, n_shooting=37, n_threads=8, use_sx=False, ode_solver=ode_solver, ) ocp.print(to_console=True, to_graph=False) sol = ocp.solve() # Check objective function value f = np.array(sol.cost) np.testing.assert_equal(f.shape, (1, 1)) if isinstance(ode_solver, OdeSolver.RK8): np.testing.assert_almost_equal(f[0, 0], 23.679065887950706) else: np.testing.assert_almost_equal(f[0, 0], 41.58259426) # Check constraints g = np.array(sol.constraints) np.testing.assert_equal(g.shape, (228, 1)) np.testing.assert_almost_equal(g, np.zeros((228, 1))) # Check some of the results states, controls = sol.states, sol.controls q, qdot, tau = states["q"], states["qdot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0, 0)), decimal=1) np.testing.assert_almost_equal(q[:, -1], np.array([0.05, 0.0933177, -0.62620287])) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0)), decimal=4) np.testing.assert_almost_equal(qdot[:, -1], np.array([2.03004523e-01, -1.74795966e-05, -2.53770131e00])) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array([-0.16347455, 0.02123226, -13.25955361])) np.testing.assert_almost_equal(tau[:, -2], np.array([0.00862357, -0.00298151, -0.16425701]))
def main(): """ Defines a multiphase ocp and animate the results """ model = "../torque_driven_ocp/models/soft_contact_sphere.bioMod" ode_solver = OdeSolver.RK8() # Prepare OCP to reach the second marker ocp = prepare_ocp(model, 37, 0.37, ode_solver, slack=1e-4) ocp.add_plot_penalty(CostType.ALL) ocp.print(to_graph=True) # --- Solve the program --- # solv = Solver.IPOPT(show_online_optim=False, show_options=dict(show_bounds=True)) solv.set_linear_solver("mumps") solv.set_maximum_iterations(500) sol = ocp.solve(solv) sol.animate() sol.print() sol.graphs()
def main(): """ Prepare, solve and animate a time minimizer ocp using a Mayer criteria """ ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=2, n_shooting=50, ode_solver=OdeSolver.RK4()) # Let's show the objectives ocp.add_plot_penalty(CostType.OBJECTIVES) # --- Solve the program --- # sol = ocp.solve(Solver.IPOPT(show_online_optim=True)) # --- Show results --- # print( f"The optimized phase time is: {sol.parameters['time'][0, 0]}, good job Mayer!" ) sol.print() sol.animate()
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, initialize_near_solution: bool, ode_solver: OdeSolver = OdeSolver.RK4(), constr: bool = True, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time at the final node n_shooting: int The number of shooting points initialize_near_solution: bool If the initial guess should be almost the solution (this is merely to reduce the time of the tests) ode_solver: OdeSolver The ode solver to use constr: bool If the constraint should be applied (this is merely to reduce the time of the tests) use_sx: bool If SX CasADi variables should be used Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr: constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m4") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m5") constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), weight: float = 1, ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points ode_solver: OdeSolver The ode solver to use weight: float The weighting of the minimize time objective function Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() # A weight of -1 will maximize time objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=weight) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # Model path biorbd_model = ( biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), ) # Problem parameters n_shooting = (20, 20, 20, 20) final_time = (2, 5, 4, 2) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=3) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds[0][[1, 3, 4, 5], 0] = 0 x_bounds[-1][[1, 3, 4, 5], -1] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) """ By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3) are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous, as for example IMPACT or CUSTOM can be used as below. "phase_pre_idx" corresponds to the index of the phase preceding the transition. IMPACT will cause an impact related discontinuity when defining one or more contact points in the model. CUSTOM will allow to call the custom function previously presented in order to have its own phase transition. Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity) you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx. If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than zero. It will thereafter be treated as a Mayer objective function with the specified weight. """ phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping(range(3), range(3))) phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, phase_transitions=phase_transitions, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, actuator_type: int = None, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod n_shooting: int The number of shooting points final_time: float The time at final node actuator_type: int The type of actuator to use: 1 (torque activations) or 2 (torque max constraints) ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters if actuator_type and actuator_type == 1: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics dynamics = DynamicsList() if actuator_type: if actuator_type == 1: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN) elif actuator_type == 2: dynamics.add(DynamicsFcn.TORQUE_DRIVEN) else: raise ValueError( "actuator_type is 1 (torque activations) or 2 (torque max constraints)" ) else: expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") if actuator_type == 2: constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT, node=Node.ALL_SHOOTING, min_torque=7.5) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 x_bounds[0][2, [0, -1]] = [0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4(), long_optim: bool = False ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solve to use long_optim: bool If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: n_shooting = (100, 300, 100) else: n_shooting = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, quadratic=True, ) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp_custom_objectives( biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=2) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=3) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.START, quadratic=True, first_marker="m0", second_marker="m1", weight=1000, ) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.END, quadratic=True, first_marker="m0", second_marker="m2", weight=1000, ) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, tau_ref: np.ndarray, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model: biorbd.Model The loaded biorbd model final_time: float The time at final node n_shooting: int The number of shooting points markers_ref: np.ndarray The markers to track tau_ref: np.ndarray The generalized forces to track ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, axis_to_track=[Axis.Y, Axis.Z], weight=100, target=markers_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(phase_time_constraint, use_parameter): # --- Inputs --- # final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = (20, 30, 20) biorbd_model_path = TestUtils.bioptim_folder( ) + "/examples/optimal_time_ocp/cube.bioMod" ode_solver = OdeSolver.RK4() # --- Options --- # n_phases = len(ns) # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add( ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=phase_time_constraint, ) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2 for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) parameters = ParameterList() if use_parameter: def my_target_function(ocp, value, target_value): return value - target_value def my_parameter_function(biorbd_model, value, extra_value): new_gravity = MX.zeros(3, 1) new_gravity[2] = value + extra_value biorbd_model.setGravity(new_gravity) min_g = -10 max_g = -6 target_g = -8 bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=target_g) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bound_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) # ------------- # return OptimalControlProgram( biorbd_model[:n_phases], dynamics, ns, final_time[:n_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, parameters=parameters, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), use_sx: bool = True, n_threads: int = 1, ) -> 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: OdeSolver = 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) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 # Prevent the model from actively rotate u_init = InitialGuess([tau_init] * n_tau) return OptimalControlProgram( biorbd_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, )
def prepare_ocp( biorbd_model_path: str = "cube_with_forces.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # External forces. external_forces is of len 1 because there is only one phase. # The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node external_forces = [ np.repeat(np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], n_shooting, axis=2) ] # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, external_forces=external_forces, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, fatigue_type: str, ode_solver: OdeSolver = OdeSolver.COLLOCATION(), torque_level: int = 0, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The time at the final node n_shooting: int The number of shooting points fatigue_type: str The type of dynamics to apply ("xia" or "michaud") ode_solver: OdeSolver The ode solver to use torque_level: int 0 no residual torque, 1 with residual torque, 2 with fatigable residual torque Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() n_muscles = biorbd_model.nbMuscleTotal() tau_min, tau_max = -10, 10 # Define fatigue parameters for each muscle and residual torque fatigue_dynamics = FatigueList() for i in range(n_muscles): if fatigue_type == "xia": fatigue_dynamics.add(XiaFatigue(LD=10, LR=10, F=0.01, R=0.002), state_only=False) elif fatigue_type == "michaud": fatigue_dynamics.add( MichaudFatigue( LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10 ), state_only=True, ) elif fatigue_type == "effort": fatigue_dynamics.add(EffortPerception(effort_threshold=0.2, effort_factor=0.001)) else: raise ValueError("fatigue_type not implemented") if torque_level >= 2: for i in range(n_tau): if fatigue_type == "xia": fatigue_dynamics.add( XiaTauFatigue( XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_min), XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_max), ), state_only=False, ) elif fatigue_type == "michaud": fatigue_dynamics.add( MichaudTauFatigue( MichaudFatigue( LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_min ), MichaudFatigue( LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_max ), ), state_only=False, ) elif fatigue_type == "effort": fatigue_dynamics.add( TauEffortPerception( EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_min), EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_max), ), state_only=False, ) else: raise ValueError("fatigue_type not implemented") # Dynamics dynamics = Dynamics(DynamicsFcn.MUSCLE_DRIVEN, expand=False, fatigue=fatigue_dynamics, with_torque=torque_level > 0) # Add objective functions objective_functions = ObjectiveList() if torque_level > 0: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100) objective_functions.add( ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=0.01 ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_FATIGUE, key="muscles", weight=1000) # Constraint constraint = Constraint( ConstraintFcn.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", node=Node.END, axes=[Axis.X, Axis.Y], ) x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, 0] = (0.07, 1.4, 0, 0) x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True)) x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) x_init.concatenate(FatigueInitialGuess(fatigue_dynamics)) # Define control path constraint u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) if torque_level == 1 else Bounds() u_bounds.concatenate(FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS)) u_init = InitialGuess([0] * n_tau) if torque_level == 1 else InitialGuess() u_init.concatenate(FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS)) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraint, ode_solver=ode_solver, use_sx=False, n_threads=8, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, n_threads: int, use_sx: bool = False, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model final_time: float The time at the final node n_shooting: int The number of shooting points n_threads: int The number of threads to use while using multithreading ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", derivative=True) # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, n_threads=n_threads, use_sx=use_sx, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, index=[0, 1, 3], key="tau", weight=100) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") constraints.add(ConstraintFcn.PROPORTIONAL_CONTROL, key="tau", node=Node.ALL_SHOOTING, first_dof=3, second_dof=4, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][2, :] = 0 # Third dof is set to zero x_bounds[0][biorbd_model.nbQ():, [0, -1]] = 0 # Velocity is 0 at start and end # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()) u_bounds[0][2, :] = 0 # Third dof is left uncontrolled u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file n_shooting: int The number of shooting points final_time: float The time at the final node ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, index=1, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque( ) # biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) # Initial guesses # TODO put this in a function defined before and explain what it does, and what are the variables x = np.vstack((np.zeros( (biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2)))) Arm_init_D = np.zeros((3, 2)) Arm_init_D[1, 0] = 0 Arm_init_D[1, 1] = -np.pi + 0.01 Arm_init_G = np.zeros((3, 2)) Arm_init_G[1, 0] = 0 Arm_init_G[1, 1] = np.pi - 0.01 for i in range(2): Arm_Quat_D = eul2quat(Arm_init_D[:, i]) Arm_Quat_G = eul2quat(Arm_init_G[:, i]) x[6:9, i] = Arm_Quat_D[1:] x[12, i] = Arm_Quat_D[0] x[9:12, i] = Arm_Quat_G[1:] x[13, i] = Arm_Quat_G[0] x_init = InitialGuessList() x_init.add(x, interpolation=InterpolationType.LINEAR) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp_parameters( biorbd_model_path, final_time, n_shooting, optim_gravity, optim_mass, min_g, max_g, target_g, min_m, max_m, target_m, ode_solver=OdeSolver.RK4(), use_sx=False, ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model final_time: float The time at the final node n_shooting: int The number of shooting points optim_gravity: bool If the gravity should be optimized optim_mass: bool If the mass should be optimized min_g: np.ndarray The minimal value for the gravity max_g: np.ndarray The maximal value for the gravity target_g: np.ndarray The target value for the gravity min_m: float The minimal value for the mass max_m: float The maximal value for the mass target_m: float The target value for the mass ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint tau_min, tau_max, tau_init = -300, 300, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # Define the parameter to optimize parameters = ParameterList() if optim_gravity: # Give the parameter some min and max bounds bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) # and an objective function parameter_objective_functions = Objective( my_target_function, weight=1000, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_g) parameters.add( "gravity_xyz", # The name of the parameter my_parameter_function, # The function that modifies the biorbd model initial_gravity, # The initial guess bound_gravity, # The bounds size= 3, # The number of elements this particular parameter vector has penalty_list= parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([1, 1, 10.0]), extra_value=1, # You can define as many extra arguments as you want ) if optim_mass: bound_mass = Bounds(min_m, max_m, interpolation=InterpolationType.CONSTANT) initial_mass = InitialGuess((min_m + max_m) / 2) mass_objective_functions = Objective( my_target_function, weight=100, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_m) parameters.add( "mass", # The name of the parameter set_mass, # The function that modifies the biorbd model initial_mass, # The initial guess bound_mass, # The bounds size= 1, # The number of elements this particular parameter vector has penalty_list= mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([10.0]), ) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, time_min: float, time_max: float, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points time_min: float The minimal time the phase can have time_max: float The maximal time the phase can have ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min, max_bound=time_max) # Path constraint n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, phase_time: float, n_shooting: int, use_actuators: bool = False, ode_solver: OdeSolver = OdeSolver.RK4(), objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT", com_constraints: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file phase_time: float The time at the final node n_shooting: int The number of shooting points use_actuators: bool If torque or torque activation should be used for the dynamics ode_solver: OdeSolver The ode solver to use objective_name: str The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT', 'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY') com_constraints: bool If a constraint on the COM should be applied Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) if use_actuators: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -500, 500, 0 dof_mapping = BiMappingList() dof_mapping.add("tau", [None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) elif objective_name == "MINIMIZE_COM_POSITION": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, node=Node.ALL, axes=Axis.Z, weight=-1) elif objective_name == "MINIMIZE_COM_VELOCITY": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100) # Dynamics dynamics = DynamicsList() if use_actuators: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True) # Constraints constraints = ConstraintList() if com_constraints: constraints.add( ConstraintFcn.TRACK_COM_VELOCITY, node=Node.ALL, min_bound=np.array([-100, -100, -100]), max_bound=np.array([100, 100, 100]), ) constraints.add( ConstraintFcn.TRACK_COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1]), ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.5, 0.5] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first)) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mapping["tau"].to_first)) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints=constraints, variable_mappings=dof_mapping, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.IRK()) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(custom_func_track_markers, node=Node.START, first_marker="m0", second_marker="m1", method=0) constraints.add(custom_func_track_markers, node=Node.END, first_marker="m0", second_marker="m2", method=1) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound, mu, ode_solver=OdeSolver.RK4()): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 tau_mapping = BiMapping([None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) constraints.add( ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=mu, ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * tau_mapping.to_first.len, [tau_max] * tau_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.to_first.len) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, problem_type_custom: bool = True, ode_solver: OdeSolver = OdeSolver.RK4(), use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model problem_type_custom: bool If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN. They should return the same results ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True if problem_type_custom: dynamics.add(custom_configure, dynamic_function=custom_dynamic, my_additional_factor=1, expand=expand) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, loop_from_constraint: bool, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model final_time: float The time at the final node n_shooting: int The number of shooting points loop_from_constraint: bool If the looping cost should be a constraint [True] or an objective [False] ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) # First node is free but mid and last are constrained to be exactly at a certain point. # The cyclic penalty ensures that the first node and the last node are the same. x_bounds[2:6, -1] = [1.57, 0, 0, 0] # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # # A phase transition loop constraint is treated as # hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or # as a soft penalty (objective) otherwise phase_transitions = PhaseTransitionList() if loop_from_constraint: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0) else: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, phase_transitions=phase_transitions, )
def prepare_ocp( biorbd_model: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, excitations_ref: np.ndarray, q_ref: np.ndarray, use_residual_torque: bool, kin_data_to_track: str = "markers", ode_solver: OdeSolver = OdeSolver.COLLOCATION(), ) -> OptimalControlProgram: """ Prepare the ocp to solve Parameters ---------- biorbd_model: biorbd.Model The loaded biorbd model final_time: float The time at final node n_shooting: int The number of shooting points markers_ref: np.ndarray The marker to track if 'markers' is chosen in kin_data_to_track excitations_ref: np.ndarray The muscle excitation (EMG) to track q_ref: np.ndarray The state to track if 'q' is chosen in kin_data_to_track kin_data_to_track: str The type of kin data to track ('markers' or 'q') use_residual_torque: bool If residual torque are present or not in the dynamics ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to solve """ # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=excitations_ref) if use_residual_torque: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") if kin_data_to_track == "markers": objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, node=Node.ALL, weight=100, target=markers_ref) elif kin_data_to_track == "q": objective_functions.add( ObjectiveFcn.Lagrange.TRACK_STATE, key="q", weight=100, node=Node.ALL, target=q_ref, index=range(biorbd_model.nbQ()), ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_excitations=True, with_residual_torque=use_residual_torque) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger x_bounds[0].min[[0, 1], :] = -2 * np.pi x_bounds[0].max[[0, 1], :] = 2 * np.pi # Add muscle to the bounds activation_min, activation_max, activation_init = 0, 1, 0.5 x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()):, 0] = excitations_ref[:, 0] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) # Define control path constraint excitation_min, excitation_max, excitation_init = 0, 1, 0.5 u_bounds = BoundsList() u_init = InitialGuessList() if use_residual_torque: tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(), ) u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()) else: u_bounds.add([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles()) u_init.add([excitation_init] * biorbd_model.nbMuscles()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )