def test_plot_merged_graphs():
# Load graphs_one_phase
from bioptim.examples.muscle_driven_ocp import muscle_excitations_tracker as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
# Define the problem
model_path = bioptim_folder + "/models/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 0.1
n_shooting = 5
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_excitations_ref = ocp_module.generate_data(biorbd_model, final_time, n_shooting)
biorbd_model = biorbd.Model(model_path) # To prevent from free variable, the model must be reloaded
ocp = ocp_module.prepare_ocp(
biorbd_model,
final_time,
n_shooting,
markers_ref,
muscle_excitations_ref,
x_ref[: biorbd_model.nbQ(), :].T,
ode_solver=OdeSolver.RK4(),
use_residual_torque=True,
kin_data_to_track="markers",
)
sol = ocp.solve()
sol.graphs(automatically_organize=False)
def test_plot_graphs_multi_phases():
# Load graphs_one_phase
from bioptim.examples.getting_started import example_multiphase as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(biorbd_model_path=bioptim_folder + "/models/cube.bioMod")
sol = ocp.solve()
sol.graphs(automatically_organize=False)
def test_plot_graphs_one_phase():
# 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=30,
final_time=2,
)
ocp.add_plot_penalty(CostType.ALL)
sol = ocp.solve()
sol.graphs(automatically_organize=False)
def test_plot_graphs_for_implicit_constraints():
from bioptim.examples.getting_started import example_implicit_dynamics as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod",
n_shooting=5,
final_time=1,
implicit_dynamics=True,
)
ocp.add_plot_penalty(CostType.ALL)
sol = ocp.solve()
if sys.platform != "linux":
sol.graphs(automatically_organize=False)
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_track_markers(ode_solver, actuator_type):
# Load track_markers
from bioptim.examples.torque_driven_ocp import track_markers_with_torque_actuators as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ode_solver = ode_solver()
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=30,
final_time=2,
actuator_type=actuator_type,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 19767.53312569522)
# Check constraints
g = np.array(sol.constraints)
if not actuator_type:
np.testing.assert_equal(g.shape, (186, 1))
else:
np.testing.assert_equal(g.shape, (366, 1))
np.testing.assert_almost_equal(g[:186], np.zeros((186, 1)))
# 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((1, 0, 0)))
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((1.4516128810214546, 9.81, 2.2790322540381487)))
np.testing.assert_almost_equal(tau[:, -2], np.array((-1.4516128810214546, 9.81, -2.2790322540381487)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol)
def test_track_markers_with_actuators(ode_solver):
# Load track_markers
from bioptim.examples.torque_driven_ocp import track_markers_with_torque_actuators as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ode_solver = ode_solver()
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=30,
final_time=2,
actuator_type=1,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 204.18087334169184)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (186, 1))
np.testing.assert_almost_equal(g, np.zeros((186, 1)))
# 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((1, 0, 0)))
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.2140175, 0.981, 0.3360075)))
np.testing.assert_almost_equal(tau[:, -2], np.array((-0.2196496, 0.981, -0.3448498)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol)
def test_console_objective_functions():
# Load graphs_one_phase
from bioptim.examples.getting_started import example_multiphase as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ocp = ocp_module.prepare_ocp(biorbd_model_path=bioptim_folder + "/models/cube.bioMod")
sol = ocp.solve()
ocp = sol.ocp # We will override ocp with known and controlled values for the test
sol.constraints = np.array([range(sol.constraints.shape[0])]).T / 10
# Create some consistent answer
sol.solver_time_to_optimize = 1.2345
sol.real_time_to_optimize = 5.4321
def override_penalty(pen: list[PenaltyOption]):
for cmp, p in enumerate(pen):
if p:
name = p.name.replace("->", "_").replace(" ", "_")
x = MX.sym("x", *p.weighted_function.sparsity_in("i0").shape)
u = MX.sym("u", *p.weighted_function.sparsity_in("i1").shape)
param = MX.sym("param", *p.weighted_function.sparsity_in("i2").shape)
weight = MX.sym("weight", *p.weighted_function.sparsity_in("i3").shape)
target = MX.sym("target", *p.weighted_function.sparsity_in("i4").shape)
dt = MX.sym("dt", *p.weighted_function.sparsity_in("i5").shape)
p.function = Function(name, [x, u, param], [np.array([range(cmp, len(p.rows) + cmp)]).T])
p.function_non_threaded = p.function
p.weighted_function = Function(
name, [x, u, param, weight, target, dt], [np.array([range(cmp + 1, len(p.rows) + cmp + 1)]).T]
)
p.weighted_function_non_threaded = p.weighted_function
override_penalty(ocp.g_internal) # Override constraints in the ocp
override_penalty(ocp.g) # Override constraints in the ocp
override_penalty(ocp.J_internal) # Override objectives in the ocp
override_penalty(ocp.J) # Override objectives in the ocp
for nlp in ocp.nlp:
override_penalty(nlp.g_internal) # Override constraints in the nlp
override_penalty(nlp.g) # Override constraints in the nlp
override_penalty(nlp.J_internal) # Override objectives in the nlp
override_penalty(nlp.J) # Override objectives in the nlp
captured_output = io.StringIO() # Create StringIO object
sys.stdout = captured_output # and redirect stdout.
sol.print()
expected_output = (
"Solver reported time: 1.2345 sec\n"
"Real time: 5.4321 sec\n"
"\n"
"---- COST FUNCTION VALUES ----\n"
"PHASE 0\n"
"MINIMIZE_CONTROL: 60.00 (weighted 120.0)\n"
"\n"
"PHASE 1\n"
"MINIMIZE_CONTROL: 90.00 (weighted 180.0)\n"
"minimize_difference: 6.00 (weighted 9.0)\n"
"\n"
"PHASE 2\n"
"MINIMIZE_CONTROL: 60.00 (weighted 120.0)\n"
"\n"
"Sum cost functions: 429.0\n"
"------------------------------\n"
"\n"
"--------- CONSTRAINTS ---------\n"
"PHASE 0\n"
"CONTINUITY: 420.0\n"
"PHASE_TRANSITION 0->1: 27.0\n"
"SUPERIMPOSE_MARKERS: 6.0\n"
"SUPERIMPOSE_MARKERS: 9.0\n"
"\n"
"PHASE 1\n"
"CONTINUITY: 630.0\n"
"PHASE_TRANSITION 1->2: 27.0\n"
"SUPERIMPOSE_MARKERS: 6.0\n"
"\n"
"PHASE 2\n"
"CONTINUITY: 420.0\n"
"SUPERIMPOSE_MARKERS: 6.0\n"
"\n"
"------------------------------\n"
)
sys.stdout = sys.__stdout__ # Reset redirect.
assert captured_output.getvalue() == expected_output
def test_track_markers_changing_constraints(ode_solver):
# Load track_markers
from bioptim.examples.torque_driven_ocp import track_markers_with_torque_actuators as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ode_solver = ode_solver()
ocp = ocp_module.prepare_ocp(
biorbd_model_path=bioptim_folder + "/models/cube.bioMod",
n_shooting=30,
final_time=2,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Add a new constraint and reoptimize
new_constraints = ConstraintList()
new_constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker="m0", second_marker="m2", list_index=2
)
ocp.update_constraints(new_constraints)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 20370.211697123825)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (189, 1))
np.testing.assert_almost_equal(g, np.zeros((189, 1)))
# 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((1, 0, 0)))
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((4.2641129, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -2], np.array((1.36088709, 9.81, -2.27903226)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol)
# Replace constraints and reoptimize
new_constraints = ConstraintList()
new_constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m2", list_index=0
)
new_constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker="m0", second_marker="m3", list_index=2
)
ocp.update_constraints(new_constraints)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 31670.93770220887)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (189, 1))
np.testing.assert_almost_equal(g, np.zeros((189, 1)))
# 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)))
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((-5.625, 21.06, 2.2790323)))
np.testing.assert_almost_equal(tau[:, -2], np.array((-5.625, 21.06, -2.27903226)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol)
def test_trampo_quaternions():
# Load trampo_quaternion
from bioptim.examples.torque_driven_ocp import trampo_quaternions as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
# Define the problem
model_path = bioptim_folder + "/models/TruncAnd2Arm_Quaternion.bioMod"
final_time = 0.25
n_shooting = 5
ocp = ocp_module.prepare_ocp(model_path, n_shooting, final_time)
sol = ocp.solve()
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -41.491609816961535)
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (130, 1))
np.testing.assert_almost_equal(g, np.zeros((130, 1)), decimal=6)
# 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([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0])
)
np.testing.assert_almost_equal(
q[:, -1],
np.array(
[
3.14159267,
3.14159267,
3.14159267,
-0.78539816,
0.6154797,
-0.07516336,
0.23662774,
-0.69787559,
0.23311438,
0.22930573,
0.62348603,
0.38590688,
0.63453499,
0.64012494,
]
),
)
# initial and final velocities
np.testing.assert_almost_equal(
qdot[:, 0],
np.array(
[
12.56193009,
12.5198592,
13.67105918,
-2.66942572,
2.64460582,
-2.16473217,
2.89069185,
-4.74193932,
4.88561749,
4.18495164,
5.12235989,
1.65628252,
]
),
)
np.testing.assert_almost_equal(
qdot[:, -1],
np.array(
[
12.59374119,
12.65603932,
11.46119531,
-4.11706327,
1.84777845,
1.92003246,
-1.99624566,
-7.67384307,
0.97705102,
-0.0532827,
7.28333747,
2.68097813,
]
),
)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.zeros((12,)), decimal=6)
np.testing.assert_almost_equal(tau[:, -2], np.zeros((12,)), decimal=6)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, decimal_value=6)
def test_track_marker_2D_pendulum(ode_solver):
# Load muscle_activations_contact_tracker
from bioptim.examples.torque_driven_ocp import track_markers_2D_pendulum as ocp_module
bioptim_folder = os.path.dirname(ocp_module.__file__)
ode_solver = ode_solver()
# Define the problem
model_path = bioptim_folder + "/models/pendulum.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 2
n_shooting = 30
# Generate data to fit
np.random.seed(42)
markers_ref = np.random.rand(3, 2, n_shooting + 1)
tau_ref = np.random.rand(2, n_shooting)
if isinstance(ode_solver, OdeSolver.IRK):
tau_ref = tau_ref * 5
ocp = ocp_module.prepare_ocp(biorbd_model, final_time, n_shooting, markers_ref, tau_ref, ode_solver=ode_solver)
sol = ocp.solve()
# Check constraints
g = np.array(sol.constraints)
np.testing.assert_equal(g.shape, (n_shooting * 4, 1))
np.testing.assert_almost_equal(g, np.zeros((n_shooting * 4, 1)))
# Check some of the results
q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"]
if isinstance(ode_solver, OdeSolver.IRK):
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 290.6751231)
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((0.64142484, 2.85371719)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((3.46921861, 3.24168308)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((9.11770196, -13.83677175)))
np.testing.assert_almost_equal(tau[:, -2], np.array((1.16836132, 4.77230548)))
elif isinstance(ode_solver, OdeSolver.RK8):
pass
else:
# Check objective function value
f = np.array(sol.cost)
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 281.8560713312711)
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((0.8367364, 3.37533055)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((3.2688391, 3.88242643)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((6.93890241, -12.76433504)))
np.testing.assert_almost_equal(tau[:, -2], np.array((0.13156876, 0.93749913)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol)
