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 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 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 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():
"""
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 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():
ocp = prepare_ocp()
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
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():
"""
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():
"""
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():
"""
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():
"""
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():
"""
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():
"""
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():
"""
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_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 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 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():
"""
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():
"""
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 main():
model_path = "../torque_driven_ocp/models/2segments_4dof_2contacts.bioMod"
t = 0.3
ns = 10
mu = 0.2
ocp = prepare_ocp(
biorbd_model_path=model_path,
phase_time=t,
n_shooting=ns,
min_bound=50,
max_bound=np.inf,
mu=mu,
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate()
def test_mhe_redim_xbounds_not_implemented():
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)
x_bounds = Bounds(
np.zeros((nq * 2, window_len + 1)),
np.zeros((nq * 2, window_len + 1)),
interpolation=InterpolationType.EACH_FRAME,
)
u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)))
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="The MHE is not implemented yet for x_bounds not being "
"CONSTANT or CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT",
):
mhe.solve(update_functions, Solver.IPOPT())
def test_cyclic_nmpc():
def update_functions(_nmpc, cycle_idx, _sol):
return cycle_idx < n_cycles # True if there are still some cycle to perform
from bioptim.examples.moving_horizon_estimation import cyclic_nmpc as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
n_cycles = 3
cycle_len = 20
nmpc = ocp_module.prepare_nmpc(
model_path=bioptim_folder + "/models/arm2.bioMod",
cycle_len=cycle_len,
cycle_duration=1,
max_torque=50,
)
sol = nmpc.solve(update_functions, solver=Solver.IPOPT())
# 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 * cycle_len))
np.testing.assert_almost_equal(
q[:, 0], np.array((-3.14159265, 0.12979378, 2.77623291)))
np.testing.assert_almost_equal(
q[:, -1], np.array((2.82743339, 0.63193395, 2.68235056)))
# initial and final velocities
np.testing.assert_almost_equal(
qdot[:, 0], np.array((6.28268908, -11.63289399, 0.37215021)))
np.testing.assert_almost_equal(
qdot[:, -1], np.array((6.28368154, -7.73180135, 3.56900657)))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array((0.01984925, 17.53758229, -1.92204945)))
np.testing.assert_almost_equal(tau[:, -2],
np.array(
(0.01984925, -3.09892348, 0.23160067)),
decimal=6)
def main():
"""
Prepare and solve and animate a reaching task ocp
"""
ocp = prepare_ocp(
biorbd_model_path="models/arm26_constant.bioMod",
final_time=0.8,
n_shooting=50,
fatigue_type="effort",
torque_level=1,
)
# --- Solve the program --- #
solver = Solver.IPOPT(show_online_optim=True)
solver.set_hessian_approximation("exact")
sol = ocp.solve(solver)
sol.print_cost()
# --- Show results --- #
sol.animate(show_meshes=True)
def main():
"""
Prepare, solve and animate a time minimizer ocp using a Lagrange criteria
"""
ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod",
final_time=2,
n_shooting=50)
# Let's show the objectives
ocp.add_plot_penalty(CostType.OBJECTIVES)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
print(
f"The optimized phase time is: {sol.parameters['time'][0, 0]}, good job Lagrange!"
)
sol.animate()
def main():
"""
Prepares and solves a maximal velocity at center of mass program and animates it
"""
model_path = "models/2segments_4dof_2contacts.bioMod"
t = 0.5
ns = 20
ocp = prepare_ocp(
biorbd_model_path=model_path,
phase_time=t,
n_shooting=ns,
use_actuators=False,
objective_name="MINIMIZE_COM_VELOCITY",
com_constraints=True,
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
sol.animate(n_frames=40)
def main():
"""
Prepare, solve and animate a free time ocp
"""
time_min = 0.6
time_max = 1
ocp = prepare_ocp(
biorbd_model_path="models/pendulum.bioMod",
final_time=2,
n_shooting=50,
time_min=time_min,
time_max=time_max,
)
# --- Solve the program --- #
sol = ocp.solve(Solver.IPOPT(show_online_optim=True))
# --- Show results --- #
print(f"The optimized phase time is: {sol.parameters['time'][0, 0]}")
sol.animate()
def main():
"""
Defines a multiphase ocp and animate the results
"""
model = "../torque_driven_ocp/models/soft_contact_sphere.bioMod"
ode_solver = OdeSolver.RK8()
# Prepare OCP to reach the second marker
ocp = prepare_ocp(model, 37, 0.37, ode_solver, slack=1e-4)
ocp.add_plot_penalty(CostType.ALL)
ocp.print(to_graph=True)
# --- Solve the program --- #
solv = Solver.IPOPT(show_online_optim=False,
show_options=dict(show_bounds=True))
solv.set_linear_solver("mumps")
solv.set_maximum_iterations(500)
sol = ocp.solve(solv)
sol.animate()
sol.print()
sol.graphs()
