def test_plot_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 allow for 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,
kin_data_to_track="markers",
)
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)
def test_plot_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():
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_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_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.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_add_new_plot():
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(options_ipopt={"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: 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: 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: x[0:2, :])
ocp.add_plot("My Second New Plot", lambda x, 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: x[0:2, :])
ocp.add_plot("My New Plot", lambda x, u: x[0:2, :])
ShowResult(ocp, sol).graphs(automatically_organize=False)
# Delete the saved file
os.remove(save_name)
marker_ref = mat_contents["markers"]
marker_ref = marker_ref[:, 4:, :]
mat_contents = sio.loadmat("./data/x_init.mat")
x_init = mat_contents["x_init"][: 2 * 6, :]
ocp, sol = OptimalControlProgram.load("activation_driven_ipopt.bo")
states, controls = Data.get_data(ocp, sol["x"])
biorbd_model = biorbd.Model("arm_Belaise.bioMod")
q = states["q"]
qdot = states["q_dot"]
x = vertcat(states["q"], states["q_dot"])
u = controls["muscles"]
nlp = ocp.nlp[0]
result = ShowResult(ocp, sol)
# result.animate()
# result.graphs()
with open("sol_marker_activation_tracking_acados.bob", 'rb' ) as file :
data = pickle.load(file)
states_ac = data['data'][0]
controls_ac = data['data'][1]
q_ac = states_ac['q']
qdot_ac = states_ac['q_dot']
u_ac = controls_ac['muscles']
final_time = 1
n_shooting_points = q_ac.shape[1]
t = np.linspace(0, final_time, n_shooting_points)
for i in range(q_ac.shape[0]):
u_init = InitialConditionsList()
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.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()
u_init = InitialConditionsList()
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()
return OptimalControlProgram(
biorbd_model,
problem_type,
number_shooting_points,
phase_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions,
constraints,
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)
ocp = prepare_ocp(biorbd_model_path="pendulum.bioMod",
final_time=3,
number_shooting_points=100,
nb_threads=4)
# --- Solve the program --- #
tic = time()
sol = ocp.solve(show_online_optim=False)
toc = time() - tic
print(f"Time to solve : {toc}sec")
# --- Save result of get_data --- #
ocp.save_get_data(
sol, "pendulum.bob") # 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)["data"]
# --- 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.graphs()
result.animate()
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 = InitialConditions([0, 0, 0, 0])
U = InitialConditions([-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()
"../models/jumper1contacts.bioMod",
"../models/jumper2contacts.bioMod",
)
# time_min = [0.1, 0.3, 0.2, 0.1, 0.1]
# time_max = [0.4, 0.6, 2, 0.4, 0.4]
phase_time = [0.2, 0.4, 1, 0.3, 0.3]
number_shooting_points = [20, 20, 20, 20, 20]
tic = time()
run_and_save_ocp(model_path,
phase_time=phase_time,
number_shooting_points=number_shooting_points)
ocp, sol = OptimalControlProgram.load("../Results/jumper5phases_sol.bo")
# ocp = prepare_ocp(model_path=model_path, phase_time=phase_time, number_shooting_points=number_shooting_points, use_symmetry=True)
# sol = ocp.solve(show_online_optim=False, options_ipopt={"hessian_approximation": "limited-memory", "max_iter": 2})
# --- Show results --- #
# param = Data.get_data(ocp, sol["x"], get_states=False, get_controls=False, get_parameters=True)
# print(
# f"The optimized phases times are: {param['time'][0, 0]}s, {param['time'][1, 0]}s, {param['time'][2, 0]}s, {param['time'][3, 0]}s and {param['time'][4, 0]}s."
# )
toc = time() - tic
print(f"Time to solve : {toc}sec")
result = ShowResult(ocp, sol)
result.graphs()
result.animate(nb_frames=61)
)
toc = time() - tic
print(f"Time to solve with ACADOS: {toc}sec")
ocp = prepare_ocp(biorbd_model_path="arm26.bioMod",
final_time=2,
number_shooting_points=51,
use_SX=False)
tic = time()
sol_ip = ocp.solve(
solver=Solver.IPOPT,
show_online_optim=False,
solver_options={
"tol": 1e-3,
"dual_inf_tol": 1e-3,
"constr_viol_tol": 1e-3,
"compl_inf_tol": 1e-3,
"linear_solver": "ma57",
},
)
toc = time() - tic
print(f"Time to solve with IPOPT: {toc}sec")
# --- Show results --- #
result_ac = ShowResult(ocp, sol_ac)
result_ip = ShowResult(ocp, sol_ip)
# result_ac.graphs()
# result_ip.graphs()
result_ac.animate(title="ferf")
result_ip.animate()
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
)
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)
u_init = InitialConditionsOption([tau_init] * ntau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
if __name__ == "__main__":
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)
import time
import sys
from biorbd_optim import OptimalControlProgram, ShowResult, Data
from up_and_down_bow import xia_model_dynamic, xia_model_configuration, xia_model_fibers, xia_initial_fatigue_at_zero
file_path = "/home/theophile/Documents/programmation/ViolinOptimalControl/optimal_control_python/results/2020_7_29_upDown.bo"
if len(sys.argv) > 1:
file_path = str(sys.argv[1])
if not isinstance(file_path, str):
t = time.localtime(time.time())
file_path = f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_upDown.bo"
ocp, sol = OptimalControlProgram.load(file_path)
d = Data.get_data(ocp, sol)
result = ShowResult(ocp, sol)
result.graphs()
# ------------- #
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()
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
if __name__ == "__main__":
ocp = prepare_ocp(
biorbd_model_path="pendulum.bioMod", final_time=3, number_shooting_points=41, nb_threads=4, use_SX=True
)
# --- Solve the program --- #
tic = time()
sol_ip = ocp.solve(solver=Solver.IPOPT)
toc = time() - tic
print(f"Time to solve with IPOPT: {toc}sec")
tic = time()
sol_ac = ocp.solve(solver=Solver.ACADOS)
toc = time() - tic
print(f"Time to solve with ACADOS: {toc}sec")
# --- Show results --- #
result_ip = ShowResult(ocp, sol_ip)
result_ac = ShowResult(ocp, sol_ac)
result_ip.graphs()
result_ac.graphs()
result_ip.animate()
result_ac.animate()
"CoM_dot",
lambda x, u, p: plot_CoM_dot(x, "../models/jumper2contacts.bioMod"),
phase_number=0,
plot_type=PlotType.PLOT,
)
ocp.add_plot(
"CoM_dot",
lambda x, u, p: plot_CoM_dot(x, "../models/jumper1contacts.bioMod"),
phase_number=1,
plot_type=PlotType.PLOT,
)
sol = ocp.solve(show_online_optim=True,
solver_options={
"hessian_approximation": "exact",
"max_iter": 1000
})
#--- Show results --- #
# param = Data.get_data(ocp, sol["x"], get_states=False, get_controls=False, get_parameters=True)
# print(
# f"The optimized phases times are: {param['time'][0, 0]}s and {param['time'][1, 0]}s."
# )
toc = time() - tic
print(f"Time to solve : {toc}sec")
result = ShowResult(ocp, sol)
result.graphs(adapt_graph_size_to_bounds=True)
result.animate(nb_frames=61)