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_control_lagrange_and_state_mayer(cost_type):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
n_shooting = 10
target = np.array([[2]])
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", multi_thread=False)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0], target=target, weight=1000, multi_thread=False
)
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], target.squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_options(cost_type):
if platform == "win32" or platform == "darwin":
print("Tests for ACADOS options on Windows and Mac are skipped")
return
from bioptim.examples.acados import pendulum as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=0.6,
n_shooting=200,
)
tols = [1e-1, 1e1]
iter = []
for tol in tols:
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
solver.set_nlp_solver_tol_stat(tol)
sol = ocp.solve(solver=solver)
iter += [sol.iterations]
# Check that tol impacted convergence
for i in range(len(tols) - 1):
np.testing.assert_array_less(iter[i + 1], iter[i])
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_several_mayer(cost_type):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=10,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0, 1], target=np.array([[1.0, 2.0]]).T)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[2], target=np.array([[3.0]]))
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, -1], 1.0)
np.testing.assert_almost_equal(q[1, -1], 2.0)
np.testing.assert_almost_equal(q[2, -1], 3.0)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_one_parameter():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.getting_started import custom_parameters as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=1,
n_shooting=100,
optim_gravity=True,
optim_mass=False,
min_g=np.array([-1, -1, -10]),
max_g=np.array([1, 1, -5]),
min_m=10,
max_m=30,
target_g=np.array([0, 0, -9.81]),
target_m=20,
use_sx=True,
)
model = ocp.nlp[0].model
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="q", target=np.array([[0, 3.14]]).T, weight=100000)
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="qdot", target=np.array([[0, 0]]).T, weight=100)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=10, multi_thread=False)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.000000010, multi_thread=False)
ocp.update_objectives(objectives)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[[0, 1, 2, 3], 0] = 0
u_bounds = Bounds([-300] * model.nbQ(), [300] * model.nbQ())
ocp.update_bounds(x_bounds, u_bounds)
solver = Solver.ACADOS()
solver.set_nlp_solver_tol_eq(1e-3)
sol = ocp.solve(solver=solver)
# Check some of the results
q, qdot, tau, gravity = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.parameters["gravity_xyz"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14)), decimal=6)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)), decimal=6)
# parameters
np.testing.assert_almost_equal(gravity[-1, :], np.array([-9.81]), decimal=6)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def main():
"""
If pendulum is run as a script, it will perform the optimization using ACADOS and animates it
"""
ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=1, n_shooting=100)
# --- Solve the program --- #
sol = ocp.solve(solver=Solver.ACADOS())
# --- Show results --- #
sol.print_cost()
sol.graphs()
sol.animate()
def test_acados_one_end_constraints():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.TRACK_STATE, index=0, key="q", weight=100, target=np.array([[1]]), multi_thread=False
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False)
ocp.update_objectives(objective_functions)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[0, 0] = 0
ocp.update_bounds(x_bounds=x_bounds)
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"]
# final position
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((-2.72727272, 9.81, 0)), decimal=6)
def test_acados_bounds_not_implemented(failing):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/"
biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod")
nq = biorbd_model.nbQ()
ntau = biorbd_model.nbGeneralizedTorque()
n_cycles = 3
window_len = 5
window_duration = 0.2
x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT)
if failing == "u_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)))
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT)
elif failing == "x_bounds":
x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
else:
raise ValueError("Wrong value for failing")
mhe = MovingHorizonEstimator(
biorbd_model,
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
window_len,
window_duration,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
n_threads=4,
)
def update_functions(mhe, t, _):
return t < n_cycles
with pytest.raises(
NotImplementedError,
match=f"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT for the {failing}",
):
mhe.solve(update_functions, Solver.ACADOS())
def test_acados_no_obj(cost_type):
if platform == "win32":
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=10,
tf=2,
)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_fail_external():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import pendulum as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
final_time=1,
n_shooting=2,
)
solver = Solver.ACADOS()
solver.set_cost_type("EXTERNAL")
with pytest.raises(RuntimeError, match="EXTERNAL is not interfaced yet, please use NONLINEAR_LS"):
sol = ocp.solve(solver=solver)
def test_acados_constraints_end_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.track import track_marker_on_segment as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
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_SHOOTING, marker="m1", segment="seg_rt", axis=Axis.X
)
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"]
# final position
np.testing.assert_almost_equal(q[:, 0], np.array([2.01701330, 0, 0, 3.20057865e-01]), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57, 7.85398168e-01)), decimal=6)
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0.04648408, 9.88616194, 2.24285498, 0.864213)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((0.19389194, 9.99905781, -2.37713652, -0.19858311)), decimal=6)
def test_acados_one_lagrange(cost_type):
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import cube as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
n_shooting = 10
target = np.expand_dims(np.arange(0, n_shooting + 1), axis=0)
target[0, -1] = n_shooting - 2
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=n_shooting,
tf=2,
)
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
key="q",
node=Node.ALL,
weight=10,
index=[0],
target=target,
multi_thread=False,
)
ocp.update_objectives(objective_functions)
solver = Solver.ACADOS()
solver.set_cost_type(cost_type)
sol = ocp.solve(solver=solver)
# Check end state value
q = sol.states["q"]
np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze())
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def test_acados_constraints_all():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.track import track_marker_on_segment as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod",
n_shooting=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_sx=True,
)
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X
)
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"]
# final position
np.testing.assert_almost_equal(q[:, 0], np.array([2.28988221, 0, 0, 2.95087911e-01]), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((2.28215749, 0, 1.57, 6.62470772e-01)), decimal=6)
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0.04483914, 9.90739842, 2.24951691, 0.78496612)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.array((0.15945561, 10.03978178, -2.36075327, 0.07267697)), decimal=6)
def test_acados_fail_lls():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
from bioptim.examples.acados import static_arm as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/arm26.bioMod",
final_time=1,
n_shooting=2,
use_sx=True,
)
solver = Solver.ACADOS()
solver.set_cost_type("LINEAR_LS")
with pytest.raises(
RuntimeError, match="SUPERIMPOSE_MARKERS is an incompatible objective term with LINEAR_LS cost type"
):
sol = ocp.solve(solver=solver)
def main():
# Options
warm_start_ipopt_from_acados_solution = False
# --- Solve the program using ACADOS --- #
ocp_acados = prepare_ocp(biorbd_model_path="models/arm26.bioMod",
final_time=2,
n_shooting=51,
use_sx=True)
solver_acados = Solver.ACADOS()
solver_acados.set_convergence_tolerance(1e-3)
sol_acados = ocp_acados.solve(solver=solver_acados)
# --- Solve the program using IPOPT --- #
x_warm = sol_acados[
"qqdot"] if warm_start_ipopt_from_acados_solution else None
ocp_ipopt = prepare_ocp(
biorbd_model_path="models/arm26.bioMod",
final_time=2,
x_warm=x_warm,
n_shooting=51,
use_sx=False,
n_threads=6,
)
solver_ipopt = Solver.IPOPT()
solver_ipopt.set_linear_solver("ma57")
solver_ipopt.set_dual_inf_tol(1e-3)
solver_ipopt.set_constraint_tolerance(1e-3)
solver_ipopt.set_convergence_tolerance(1e-3)
solver_ipopt.set_maximum_iterations(100)
solver_ipopt.set_hessian_approximation("exact")
sol_ipopt = ocp_ipopt.solve(solver=solver_ipopt)
# --- Show results --- #
print("\n\n")
print("Results using ACADOS")
print(f"Final objective: {np.nansum(sol_acados.cost)}")
sol_acados.print_cost()
print(f"Time to solve: {sol_acados.real_time_to_optimize}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_ipopt.cost)}")
sol_ipopt.print_cost()
print(f"Time to solve: {sol_ipopt.real_time_to_optimize}sec")
print(f"")
visualizer = sol_acados.animate(show_now=False)
visualizer.extend(sol_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 main():
model_path = "models/cube.bioMod"
ns = 30
tf = 2
ocp = prepare_ocp(biorbd_model_path=model_path, n_shooting=ns, tf=tf)
# --- Add objective functions --- #
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=1000,
index=[0, 1],
target=np.array([[1.0, 2.0]]).T,
multi_thread=False,
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=10000,
index=[2],
target=np.array([[3.0]]),
multi_thread=False,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=1,
multi_thread=False)
ocp.update_objectives(objective_functions)
# --- Solve the program --- #
solver = Solver.ACADOS()
sol = ocp.solve(solver)
sol.graphs()
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=1,
index=[0, 1],
target=np.array([[1.0, 2.0]]).T,
multi_thread=False,
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=10000,
index=[2],
target=np.array([[3.0]]),
multi_thread=False,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10,
multi_thread=False)
ocp.update_objectives(objective_functions)
solver.set_nlp_solver_tol_stat(1e-2)
sol = ocp.solve(solver)
# --- Show results --- #
sol.graphs()
sol.animate()
def main():
biorbd_model_path = "models/cart_pendulum.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path)
solver = Solver.ACADOS() # or Solver.IPOPT()
final_time = 5
n_shoot_per_second = 100
window_len = 10
window_duration = 1 / n_shoot_per_second * window_len
n_frames_total = final_time * n_shoot_per_second - window_len - 1
x0 = np.array([0, np.pi / 2, 0, 0])
noise_std = 0.05 # STD of noise added to measurements
torque_max = 2 # Max torque applied to the model
states, markers, markers_noised, controls = generate_data(
biorbd_model,
final_time,
x0,
torque_max,
n_shoot_per_second * final_time,
noise_std,
show_plots=False)
x_init = np.zeros((biorbd_model.nbQ() * 2, window_len + 1))
u_init = np.zeros((biorbd_model.nbQ(), window_len))
torque_max = 5 # Give a bit of slack on the max torque
biorbd_model = biorbd.Model(biorbd_model_path)
mhe = prepare_mhe(
biorbd_model,
window_len=window_len,
window_duration=window_duration,
max_torque=torque_max,
x_init=x_init,
u_init=u_init,
)
def update_functions(mhe, t, _):
def target(i: int):
return markers_noised[:, :, i:i + window_len + 1]
mhe.update_objectives_target(target=target(t), list_index=0)
return t < n_frames_total # True if there are still some frames to reconstruct
# Solve the program
sol = mhe.solve(update_functions, **get_solver_options(solver))
print("ACADOS with Bioptim")
print(f"Window size of MHE : {window_duration} s.")
print(f"New measurement every : {1 / n_shoot_per_second} s.")
print(
f"Average time per iteration of MHE : {sol.solver_time_to_optimize / (n_frames_total - 1)} s."
)
print(
f"Average real time per iteration of MHE : {sol.real_time_to_optimize / (n_frames_total - 1)} s."
)
print(
f"Norm of the error on state = {np.linalg.norm(states[:, :n_frames_total] - sol.states['all'])}"
)
markers_estimated = states_to_markers(biorbd_model, sol.states["all"])
plt.plot(
markers_noised[1, :, :n_frames_total].T,
markers_noised[2, :, :n_frames_total].T,
"x",
label="Noised markers trajectory",
)
plt.gca().set_prop_cycle(None)
plt.plot(markers[1, :, :n_frames_total].T,
markers[2, :, :n_frames_total].T,
label="True markers trajectory")
plt.gca().set_prop_cycle(None)
plt.plot(markers_estimated[1, :, :].T,
markers_estimated[2, :, :].T,
"o",
label="Estimated marker trajectory")
plt.legend()
plt.figure()
plt.plot(sol.states["all"].T, "--", label="States estimate")
plt.plot(states[:, :n_frames_total].T, label="State truth")
plt.legend()
plt.show()
sol.animate()