def test_add_new_plot(): # Load graphs_one_phase PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/align_markers_with_torque_actuators.py") graphs_one_phase = importlib.util.module_from_spec(spec) spec.loader.exec_module(graphs_one_phase) ocp = graphs_one_phase.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod", number_shooting_points=20, final_time=0.5, ) sol = ocp.solve(solver_options={"max_iter": 1}) # Saving/loading files reset the plot settings to normal save_name = "test_plot.bo" ocp.save(sol, save_name) # Test 1 - Working plot ocp.add_plot("My New Plot", lambda x, u, p: x[0:2, :]) ShowResult(ocp, sol).graphs(automatically_organize=False) # Test 2 - Combine using combine_to is not allowed ocp, sol = OptimalControlProgram.load(save_name) with pytest.raises(RuntimeError): ocp.add_plot("My New Plot", lambda x, u, p: x[0:2, :], combine_to="NotAllowed") # Test 3 - Create a completely new plot ocp, sol = OptimalControlProgram.load(save_name) ocp.add_plot("My New Plot", lambda x, u, p: x[0:2, :]) ocp.add_plot("My Second New Plot", lambda x, p, u: x[0:2, :]) ShowResult(ocp, sol).graphs(automatically_organize=False) # Test 4 - Combine to the first using fig_name ocp, sol = OptimalControlProgram.load(save_name) ocp.add_plot("My New Plot", lambda x, u, p: x[0:2, :]) ocp.add_plot("My New Plot", lambda x, u, p: x[0:2, :]) ShowResult(ocp, sol).graphs(automatically_organize=False) # Delete the saved file os.remove(save_name)
def test_plot_graphs_multi_phases(): # Load graphs_one_phase PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/multiphase_align_markers.py") graphs_multi_phases = importlib.util.module_from_spec(spec) spec.loader.exec_module(graphs_multi_phases) ocp = graphs_multi_phases.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod") sol = ocp.solve() plt = ShowResult(ocp, sol) plt.graphs(automatically_organize=False)
def test_plot_graphs_one_phase(): # Load graphs_one_phase PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/align_markers_with_torque_actuators.py") graphs_one_phase = importlib.util.module_from_spec(spec) spec.loader.exec_module(graphs_one_phase) ocp = graphs_one_phase.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod", number_shooting_points=30, final_time=2, ) sol = ocp.solve() plt = ShowResult(ocp, sol) plt.graphs(automatically_organize=False)
def test_plot_merged_graphs(): # Load graphs_one_phase PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "align_markers", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/muscle_excitations_tracker.py") merged_graphs = importlib.util.module_from_spec(spec) spec.loader.exec_module(merged_graphs) # Define the problem model_path = str( PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod" biorbd_model = biorbd.Model(model_path) final_time = 0.5 nb_shooting = 9 # Generate random data to fit np.random.seed(42) t, markers_ref, x_ref, muscle_excitations_ref = merged_graphs.generate_data( biorbd_model, final_time, nb_shooting) biorbd_model = biorbd.Model( model_path ) # To prevent from non free variable, the model must be reloaded ocp = merged_graphs.prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, muscle_excitations_ref, x_ref[:biorbd_model.nbQ(), :].T, use_residual_torque=True, kin_data_to_track="markers", ) sol = ocp.solve() plt = ShowResult(ocp, sol) plt.graphs(automatically_organize=False)
pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path="pendulum.bioMod", final_time=2, number_shooting_points=10, nb_threads=2, ) X = InitialGuess([0, 0, 0, 0]) U = InitialGuess([-1, 1]) # --- Single shooting --- # sol_simulate_single_shooting = Simulate.from_controls_and_initial_states( ocp, X, U, single_shoot=True) result_single = ShowResult(ocp, sol_simulate_single_shooting) result_single.graphs() # --- Multiple shooting --- # sol_simulate_multiple_shooting = Simulate.from_controls_and_initial_states( ocp, X, U, single_shoot=False) result_multiple = ShowResult(ocp, sol_simulate_multiple_shooting) result_multiple.graphs() # --- Single shooting --- # result_single.animate() # --- Multiple shooting --- # result_multiple.animate()
# ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, ) if __name__ == "__main__": ocp = prepare_ocp("cube.bioMod", number_shooting_points=30, final_time=2, use_actuators=False) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show results --- # result = ShowResult(ocp, sol) result.animate()
print("Results using ACADOS") print(f"Final objective: {np.nansum(sol_obj_acados)}") analyse_acados = ObjectivePrinter(ocp_acados, sol_obj_acados) analyse_acados.by_function() print(f"Time to solve: {sol_acados['time_tot']}sec") print(f"") print( f"Results using Ipopt{'' if warm_start_ipopt_from_acados_solution else ' not'} " f"warm started from ACADOS solution" ) print(f"Final objective : {np.nansum(sol_obj_ipopt)}") analyse_ipopt = ObjectivePrinter(ocp_ipopt, sol_obj_ipopt) analyse_ipopt.by_function() print(f"Time to solve: {sol_ipopt['time_tot']}sec") print(f"") result_acados = ShowResult(ocp_acados, sol_acados) result_ipopt = ShowResult(ocp_ipopt, sol_ipopt) visualizer = result_acados.animate(show_now=False) visualizer.extend(result_ipopt.animate(show_now=False)) # Update biorbd-viz by hand so they can be visualized simultaneously should_continue = True while should_continue: for i, b in enumerate(visualizer): if b.vtk_window.is_active: b.update() else: should_continue = False
def plot_torque(torque, torques_to_plot): return torque[torques_to_plot, :] def plot_force(forces, forces_to_plot): return forces[forces_to_plot, :] ocp.add_plot("Torque", lambda x, u, p: plot_torque(torques, range(nlp["nbQ"])), PlotType.PLOT) ocp.add_plot("MusclesForces", lambda x, u, p: plot_force(force, (range(nlp["nbMuscle"]))), PlotType.PLOT, legend=(nlp["muscleNames"])) # # -- Plot all moment arm on a same graph -- # # plt.figure("ElbowMomentArm") # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[0, :], label=nlp["muscleNames"][0]) # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[1, :], label=nlp["muscleNames"][1]) # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[2, :], label=nlp["muscleNames"][2]) # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[3, :], label=nlp["muscleNames"][3]) # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[4, :], label=nlp["muscleNames"][4]) # plt.plot(np.linspace(0, 1, nlp["ns"]), moment_elbow[5, :], label=nlp["muscleNames"][5]) # plt.legend() # --- Show result ---# result = ShowResult(ocp, sol) # result.animate() result.graphs() plt.show()
InterpolationType.CONSTANT, ) muscle_states_init = InitialConditions( [muscle_activated_init] * ocp.nlp[0]["model"].nbMuscles() + [muscle_fatigued_init] * ocp.nlp[0]["model"].nbMuscles() + [muscle_resting_init] * ocp.nlp[0]["model"].nbMuscles(), InterpolationType.CONSTANT, ) X.concatenate(muscle_states_init) # --- Simulate --- # sol_simulate = Simulate.from_controls_and_initial_states(ocp, X, U, single_shoot=True) # --- Save for biorbdviz --- # OptimalControlProgram.save_get_data(ocp, sol_simulate, f"results/simulate.bob", interpolate_nb_frames=100) # --- Graph --- # result_single = ShowResult(ocp, sol_simulate) result_single.graphs() # todo multiphase print("ok") # result_single.animate()
) if __name__ == "__main__": print(f"Show the bounds") for interpolation_type in InterpolationType: print(f"Solving problem using {interpolation_type} bounds") ocp = prepare_ocp("cube.bioMod", number_shooting_points=30, final_time=2, interpolation_type=interpolation_type) sol = ocp.solve() print("\n") # Print the last solution result_plot = ShowResult(ocp, sol) result_plot.graphs(adapt_graph_size_to_bounds=True) for interpolation_type in InterpolationType: print(f"Solving problem using {interpolation_type} bounds") ocp = prepare_ocp("cube.bioMod", number_shooting_points=30, final_time=2, interpolation_type=interpolation_type) sol = ocp.solve() print("\n") # Print the last solution result_plot = ShowResult(ocp, sol) result_plot.graphs(adapt_graph_size_to_bounds=False)
return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, tau_mapping=tau_mapping, ) if __name__ == "__main__": model_path = "2segments_4dof_2contacts.bioMod" t = 0.5 ns = 20 ocp = prepare_ocp(model_path=model_path, phase_time=t, number_shooting_points=ns, use_actuators=False) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show results --- # result = ShowResult(ocp, sol) result.animate(nb_frames=40)
# ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ) if __name__ == "__main__": # changer le path quand ce sera pret ocp = prepare_ocp( "TruncAnd2Arm_Quaternion.bioMod", number_shooting_points=5, final_time=0.25, ) sol = ocp.solve() print("\n") # Print the last solution result_plot = ShowResult(ocp, sol) result_plot.graphs()
u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ) if __name__ == "__main__": ocp = prepare_ocp(biorbd_model_path="arm26_with_contact.bioMod", final_time=2, number_shooting_points=20) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show results --- # result = ShowResult(ocp, sol) result.animate(show_meshes=False) result.graphs()
def main(args=None): init = None if args: init = args[:-1] pwd = args[-1] save_path = "2p_init_"+str(init[0])+"_"+str(init[1])+"_"+str(init[2])+"_"+str(init[3])+"_sol.bo" if os.path.exists(save_path): return model_path = ( "../models/jumper2contacts.bioMod", "../models/jumper1contacts.bioMod", ) time_min = [0.2, 0.05] time_max = [0.5, 1] phase_time = [0.6, 0.2] number_shooting_points = [30, 15] tic = time() ocp = prepare_ocp( model_path=model_path, phase_time=phase_time, ns=number_shooting_points, time_min=time_min, time_max=time_max, init=init, ) sol = ocp.solve( show_online_optim=False, solver_options={"hessian_approximation": "limited-memory", "max_iter": 200} ) utils.warm_start_nmpc(sol, ocp) ocp.solver.set_lagrange_multiplier(sol) sol = ocp.solve( show_online_optim=True, solver_options={"hessian_approximation": "exact", "max_iter": 1000, "warm_start_init_point": "yes", } ) toc = time() - tic print(f"Time to solve : {toc}sec") if init: ocp.save(sol, save_path) ocp.save_get_data(sol, save_path + 'b') ssh = SSHClient() ssh.load_system_host_keys() ssh.connect('pariterre.net', username='******', password=pwd) with SCPClient(ssh.get_transport()) as scp: scp.put(save_path, save_path) scp.get(save_path) scp.put(save_path + 'b', save_path + 'b') scp.get(save_path + 'b') result = ShowResult(ocp, sol) result.animate(nb_frames=241)
# --- Access to all iterations --- # if sol_iterations: # If the processor is too fast, this will be empty since it is attached to the update function nb_iter = len(sol_iterations) third_iteration = sol_iterations[2] # --- Print objective cost --- # print(f"Final objective value : {np.nansum(sol_obj)} \n") analyse = Objective.Printer(ocp, sol_obj) analyse.by_function() analyse.by_nodes() # --- Save result of get_data --- # ocp.save_get_data( sol, "pendulum.bob", sol_iterations) # you don't have to specify the extension ".bob" # --- Load result of get_data --- # with open("pendulum.bob", "rb") as file: data = pickle.load(file) # --- Save the optimal control program and the solution --- # ocp.save(sol, "pendulum.bo") # you don't have to specify the extension ".bo" # --- Load the optimal control program and the solution --- # ocp_load, sol_load = OptimalControlProgram.load("pendulum.bo") # --- Show results --- # result = ShowResult(ocp_load, sol_load) result.animate()
biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, ode_solver=ode_solver, use_SX=use_SX, ) if __name__ == "__main__": ocp = prepare_ocp( biorbd_model_path="pendulum.bioMod", final_time=3, number_shooting_points=100, min_g=-10, max_g=-6, target_g=-8 ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Get the results --- # states, controls, params = Data.get_data(ocp, sol, get_parameters=True) length = params["gravity_z"][0, 0] print(length) # --- Show results --- # ShowResult(ocp, sol).animate(nb_frames=200)