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)
