def main():
model_path = "models/arm2.bioMod"
torque_max = 50
cycle_duration = 1
cycle_len = 20
n_cycles_to_advance = 1
n_cycles_simultaneous = 3
n_cycles = 4
nmpc = prepare_nmpc(
model_path,
cycle_len=cycle_len,
cycle_duration=cycle_duration,
n_cycles_to_advance=n_cycles_to_advance,
n_cycles_simultaneous=n_cycles_simultaneous,
max_torque=torque_max,
)
def update_functions(_nmpc: MultiCyclicNonlinearModelPredictiveControl,
cycle_idx: int, _sol: Solution):
return cycle_idx < n_cycles # True if there are still some cycle to perform
# Solve the program
sol = nmpc.solve(
update_functions,
solver=Solver.IPOPT(show_online_optim=True),
n_cycles_simultaneous=n_cycles_simultaneous,
)
sol.print()
sol.graphs()
sol.animate(n_frames=100)
def assert_warm_start(ocp,
sol,
state_decimal=2,
control_decimal=2,
param_decimal=2):
ocp.set_warm_start(sol)
solver = Solver.IPOPT()
solver.set_maximum_iterations(0)
solver.set_initialization_options(1e-10)
sol_warm_start = ocp.solve(solver)
if ocp.n_phases > 1:
for i in range(ocp.n_phases):
np.testing.assert_almost_equal(sol_warm_start.states[i]["all"],
sol.states[i]["all"],
decimal=state_decimal)
np.testing.assert_almost_equal(
sol_warm_start.controls[i]["all"],
sol.controls[i]["all"],
decimal=control_decimal)
else:
np.testing.assert_almost_equal(sol_warm_start.states["all"],
sol.states["all"],
decimal=state_decimal)
np.testing.assert_almost_equal(sol_warm_start.controls["all"],
sol.controls["all"],
decimal=control_decimal)
np.testing.assert_almost_equal(sol_warm_start.parameters["all"],
sol.parameters["all"],
decimal=param_decimal)
def main():
"""
Generate random data, then create a tracking problem, and finally solve it and plot some relevant information
"""
# Define the problem
biorbd_model = biorbd.Model("models/arm26.bioMod")
final_time = 0.5
n_shooting_points = 30
use_residual_torque = True
# Generate random data to fit
t, markers_ref, x_ref, muscle_excitations_ref = generate_data(
biorbd_model, final_time, n_shooting_points)
# Track these data
biorbd_model = biorbd.Model(
"models/arm26.bioMod"
) # To allow for non free variable, the model must be reloaded
ocp = prepare_ocp(
biorbd_model,
final_time,
n_shooting_points,
markers_ref,
muscle_excitations_ref,
x_ref[:biorbd_model.nbQ(), :],
use_residual_torque=use_residual_torque,
kin_data_to_track="q",
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show the results --- #
q = sol.states["q"]
n_q = ocp.nlp[0].model.nbQ()
n_mark = ocp.nlp[0].model.nbMarkers()
n_frames = q.shape[1]
markers = np.ndarray((3, n_mark, q.shape[1]))
symbolic_states = MX.sym("x", n_q, 1)
markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers,
symbolic_states)
for i in range(n_frames):
markers[:, :, i] = markers_func(q[:, i])
plt.figure("Markers")
n_steps_ode = ocp.nlp[0].ode_solver.steps + 1 if ocp.nlp[
0].ode_solver.is_direct_collocation else 1
for i in range(markers.shape[1]):
plt.plot(np.linspace(0, 2, n_shooting_points + 1),
markers_ref[:, i, :].T, "k")
plt.plot(np.linspace(0, 2, n_shooting_points * n_steps_ode + 1),
markers[:, i, :].T, "r--")
plt.xlabel("Time")
plt.ylabel("Markers Position")
# --- Plot --- #
plt.show()
def main():
"""
Create multiple new plot and add new stuff to them using a custom defined function
"""
# Prepare the Optimal Control Program
ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod",
final_time=2,
n_shooting=50)
# Add my lovely new plot
ocp.add_plot("My New Extra Plot",
lambda t, x, u, p: custom_plot_callback(x, [0, 1, 3]),
plot_type=PlotType.PLOT)
ocp.add_plot(
"My New Extra Plot",
lambda t, x, u, p: custom_plot_callback(x, [1, 3]),
plot_type=PlotType.STEP,
axes_idx=[1, 2],
)
ocp.add_plot(
"My Second New Extra Plot",
lambda t, x, u, p: custom_plot_callback(x, [1, 3]),
plot_type=PlotType.INTEGRATED,
axes_idx=[1, 2],
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=False))
# --- Show results --- #
sol.graphs()
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 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_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_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 main():
"""
Solve and print the optimized value for the gravity and animate the solution
"""
optim_gravity = True
optim_mass = True
ocp = prepare_ocp(
biorbd_model_path="models/pendulum.bioMod",
final_time=3,
n_shooting=100,
optim_gravity=optim_gravity,
optim_mass=optim_mass,
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,
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=False))
# --- Get the results --- #
if optim_gravity:
gravity = sol.parameters["gravity_xyz"]
print(f"Optimized gravity: {gravity[:, 0]}")
if optim_mass:
mass = sol.parameters["mass"]
print(f"Optimized mass: {mass[0, 0]}")
# --- Show results --- #
sol.animate(n_frames=200)
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 main():
ocp = prepare_ocp()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
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():
"""
Prepares and solves an ocp that has quaternion in it. Animates the results
"""
ocp = prepare_ocp("models/TruncAnd2Arm_Quaternion.bioMod",
n_shooting=5,
final_time=0.25)
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# Print the last solution
sol.animate(n_frames=-1)
def main():
"""
Prepare and solve and animate a reaching task ocp
"""
ocp = prepare_ocp(biorbd_model_path="models/arm26.bioMod", final_time=0.5, n_shooting=50, weight=1000)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate(show_meshes=True)
def main():
"""
Solve an ocp with external forces and animates the solution
"""
ocp = prepare_ocp()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Runs and animate the program
"""
ocp = prepare_ocp("models/cube.bioMod", n_shooting=30, final_time=2, loop_from_constraint=True)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Solves an ocp where the symmetry must be respected, and animates it
"""
ocp = prepare_ocp()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Defines a multiphase ocp and animate the results
"""
ocp = prepare_ocp(long_optim=False)
ocp.add_plot_penalty(CostType.ALL)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Runs the optimization and animates it
"""
model_path = "models/cube.bioMod"
ocp = prepare_ocp(biorbd_model_path=model_path)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Defines a multiphase ocp and animate the results
"""
ocp = prepare_ocp()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=False))
sol.print_cost()
sol.graphs()
# --- Show results --- #
sol.animate()
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 main():
"""
Solves an ocp where the symmetry is enforced by constraints, and animates it
"""
ocp = prepare_ocp()
# Objective and constraints plots
ocp.add_plot_penalty()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def test_add_new_plot():
# Load graphs_one_phase
from bioptim.examples.torque_driven_ocp import track_markers_with_torque_actuators 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=20,
final_time=0.5,
)
solver = Solver.IPOPT()
solver.set_maximum_iterations(1)
sol = ocp.solve(solver)
# 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 t, x, u, p: x[0:2, :])
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 t, 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 t, x, u, p: x[0:2, :])
ocp.add_plot("My Second New Plot", lambda t, x, p, u: x[0:2, :])
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 t, x, u, p: x[0:2, :])
ocp.add_plot("My New Plot", lambda t, x, u, p: x[0:2, :])
sol.graphs(automatically_organize=False)
# Add the plot of objectives and constraints to this mess
ocp.add_plot_penalty(CostType.ALL)
sol.graphs(automatically_organize=False)
# Delete the saved file
os.remove(save_name)
def test_multi_cyclic_nmpc():
def update_functions(_nmpc, cycle_idx, _sol):
return cycle_idx < n_cycles_total # True if there are still some cycle to perform
from bioptim.examples.moving_horizon_estimation import multi_cyclic_nmpc as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
n_cycles_simultaneous = 2
n_cycles_to_advance = 1
n_cycles_total = 3
cycle_len = 20
nmpc = ocp_module.prepare_nmpc(
model_path=bioptim_folder + "/models/arm2.bioMod",
cycle_len=cycle_len,
cycle_duration=1,
n_cycles_simultaneous=n_cycles_simultaneous,
n_cycles_to_advance=n_cycles_to_advance,
max_torque=50,
)
sol = nmpc.solve(update_functions,
solver=Solver.IPOPT(),
n_cycles_simultaneous=n_cycles_simultaneous)
# 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_equal(q.shape, (3, n_cycles_total * cycle_len))
np.testing.assert_almost_equal(
q[:, 0], np.array((-12.56637061, 1.04359174, 1.03625065)))
np.testing.assert_almost_equal(
q[:, -1], np.array((-6.59734457, 0.89827771, 1.0842402)))
# initial and final velocities
np.testing.assert_almost_equal(
qdot[:, 0], np.array((6.28293718, 2.5617072, -0.00942694)))
np.testing.assert_almost_equal(
qdot[:, -1], np.array((6.28343343, 3.28099958, -1.27304428)))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array((0.00992505, 4.88488618, 2.4400698)))
np.testing.assert_almost_equal(tau[:, -2],
np.array(
(0.00992505, 9.18387711, 5.22418771)),
decimal=6)
def main():
"""
Prepare and solve and animate a reaching task ocp
"""
ocp = prepare_ocp(biorbd_model_path="models/arm26_with_contact.bioMod",
final_time=1,
n_shooting=30,
weight=1000)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.print_cost()
sol.animate()
def main():
"""
Prepares, solves and animates the program
"""
ocp = prepare_ocp(
biorbd_model_path="models/cube_and_line.bioMod",
n_shooting=30,
final_time=1,
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Solve and animate the solution
"""
model_path = "models/cube.bioMod"
ocp = prepare_ocp(biorbd_model_path=model_path)
# Custom plots
ocp.add_plot_penalty()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def main():
"""
Run a multiphase problem with free time phases and animate the results
"""
final_time = [2, 5, 4]
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = [20, 30, 20]
ocp = prepare_ocp(final_time=final_time, time_min=time_min, time_max=time_max, n_shooting=ns)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
param = sol.parameters
print(f"The optimized phase time are: {param['time'][0, 0]}s, {param['time'][1, 0]}s and {param['time'][2, 0]}s.")
sol.animate()
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_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)
