def warm_start_mhe(ocp, sol):
data = Data.get_data(ocp, sol)
q = data[0]["q"]
dq = data[0]["q_dot"]
tau = []
if use_activation:
act = data[1]["muscles"]
x = np.vstack([q, dq])
u = act
else:
act = data[0]["muscles"]
exc = data[1]["muscles"]
x = np.vstack([q, dq, act])
u = exc
w_tau = 'tau' in data[1].keys()
if w_tau:
tau = data[1]["tau"]
u = np.vstack([tau, act])
x0 = np.hstack((x[:, 1:], np.tile(
x[:, [-1]],
1))) # discard oldest estimate of the window, duplicates youngest
u0 = u[:, 1:] # discard oldest estimate of the window
x_out = x[:, 0]
u_out = u[:, 0]
return x0, u0, x_out, u_out
def save_and_load(sol, ocp, test_solve_of_loaded=False):
file_path = "test.bo"
ocp.save(sol, file_path)
ocp_load, sol_load = OptimalControlProgram.load(file_path)
TestUtils.deep_assert(sol, sol_load)
TestUtils.deep_assert(sol_load, sol)
if test_solve_of_loaded:
sol_from_load = ocp_load.solve()
TestUtils.deep_assert(sol, sol_from_load)
TestUtils.deep_assert(sol_from_load, sol)
TestUtils.deep_assert(ocp_load, ocp)
TestUtils.deep_assert(ocp, ocp_load)
os.remove(file_path)
file_path_bob = "test.bob"
ocp.save_get_data(sol,
file_path_bob,
interpolate_nb_frames=-1,
concatenate=True)
data = Data.get_data(ocp,
sol,
file_path_bob,
interpolate_nb_frames=-1,
concatenate=True)
with open(file_path_bob, "rb") as file:
data_load = pickle.load(file)["data"]
TestUtils.deep_assert(data, data_load)
TestUtils.deep_assert(data_load, data)
os.remove(file_path_bob)
def test_acados_constraints_end_all():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"constraint",
str(PROJECT_FOLDER) + "/examples/align/align_marker_on_segment.py",
)
constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(constraint)
ocp = constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/align/cube_and_line.bioMod",
number_shooting_points=30,
final_time=2,
initialize_near_solution=True,
constr=False,
use_SX=True,
)
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=5)
constraints.add(ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker_idx=1,
segment_idx=2,
axis=(Axe.X))
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# final position
np.testing.assert_almost_equal(q[:, 0],
np.array([2, 0, 0, -0.139146705]),
decimal=6)
np.testing.assert_almost_equal(q[:, -1],
np.array((2, 0, 1.57, -0.139146705)),
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, 9.81, 2.27903226, 0)),
decimal=6)
np.testing.assert_almost_equal(tau[:, -1],
np.array((0, 9.81, -2.27903226, 0)),
decimal=6)
def test_align_and_minimize_marker_velocity(ode_solver):
# Load align_and_minimize_marker_velocity
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_and_minimize_marker_velocity",
str(PROJECT_FOLDER) +
"/examples/align/align_and_minimize_marker_velocity.py",
)
align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_and_minimize_marker_velocity)
ocp = align_and_minimize_marker_velocity.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/align/cube_and_line.bioMod",
number_shooting_points=5,
final_time=1,
marker_velocity_or_displacement="velo",
marker_in_first_coordinates_system=True,
control_type=ControlType.CONSTANT,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -80.20048585400944)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(
q[:, 0], np.array([7.18708669e-01, -4.45703930e-01, -3.14159262e00,
0]))
np.testing.assert_almost_equal(
q[:, -1], np.array([1.08646846e00, -3.86731175e-01, 3.14159262e00, 0]))
# initial and final velocities
np.testing.assert_almost_equal(
qdot[:, 0], np.array([3.78330878e-01, 3.70214281, 10, 0]))
np.testing.assert_almost_equal(
qdot[:, -1], np.array([3.77168521e-01, -3.40782793, 10, 0]))
# # initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([-4.52216174e-02, 9.25170010e-01, 0, 0]))
np.testing.assert_almost_equal(tau[:, -1],
np.array([4.4260355e-02, 1.4004583, 0, 0]))
# # save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def switch_phase(ocp, sol):
data = Data.get_data(ocp, sol)
q = data[0]["q"]
dq = data[0]["qdot"]
act = data[0]["muscles"]
exc = data[1]["muscles"]
x = np.vstack([q, dq, act])
return x[:, :-1], exc[:, :-1], x[:, -1]
def test_pendulum_max_time_mayer_constrained(ode_solver):
# Load pendulum_min_time_Mayer
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"pendulum_min_time_Mayer", str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum_min_time_Mayer.py"
)
pendulum_min_time_Mayer = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum_min_time_Mayer)
ocp = pendulum_min_time_Mayer.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
ode_solver=ode_solver,
max_time=1,
weight=-1,
)
sol = ocp.solve()
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6)
# Check some of the results
states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True)
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
tf = param["time"][0, 0]
# 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, 3.14)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)))
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -1)
if ode_solver == OdeSolver.IRK:
# initial and final controls
np.testing.assert_almost_equal(tau[1, 0], np.array(0))
np.testing.assert_almost_equal(tau[1, -1], np.array(0))
else:
# initial and final controls
np.testing.assert_almost_equal(tau[1, 0], np.array(0))
np.testing.assert_almost_equal(tau[1, -1], np.array(0))
# optimized time
np.testing.assert_almost_equal(tf, 1.0)
# save and load
TestUtils.save_and_load(sol, ocp, True)
def test_state_transitions():
# Load state_transitions
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"state_transitions",
str(PROJECT_FOLDER) +
"/examples/getting_started/custom_phase_transitions.py")
state_transitions = importlib.util.module_from_spec(spec)
spec.loader.exec_module(state_transitions)
ocp = state_transitions.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod")
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 110875.0772043361)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (515, 1))
np.testing.assert_almost_equal(g, np.zeros((515, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"], concatenate=False)
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[0][:, 0], np.array((1, 0, 0)))
np.testing.assert_almost_equal(q[-1][:, -1], np.array((1, 0, 0)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[0][:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[-1][:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(
tau[0][:, 0], np.array((0.9598672, 9.7085598, -0.0623733)))
np.testing.assert_almost_equal(tau[-1][:, -1],
np.array((0, 1.2717052e01, 1.1487805e00)))
# cyclic continuity (between phase 3 and phase 0)
np.testing.assert_almost_equal(q[-1][:, -1], q[0][:, 0])
# Continuity between phase 0 and phase 1
np.testing.assert_almost_equal(q[0][:, -1], q[1][:, 0])
# save and load
with pytest.raises(PicklingError,
match="import of module 'state_transitions' failed"):
TestUtils.save_and_load(sol, ocp, True)
# simulate
with pytest.raises(AssertionError,
match="Arrays are not almost equal to 7 decimals"):
TestUtils.simulate(sol, ocp)
def test_align_markers(ode_solver, actuator_type):
# Load align_markers
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_markers",
str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/align_markers_with_torque_actuators.py")
align_markers = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_markers)
ocp = align_markers.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/cube.bioMod",
number_shooting_points=30,
final_time=2,
actuator_type=actuator_type,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 19767.53312569522)
# Check constraints
g = np.array(sol["g"])
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
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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[:, -1], np.array((-1.4516128810214546, 9.81, -2.2790322540381487)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp, decimal_value=6)
def test_cyclic_objective(ode_solver):
# Load initial_guess
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"initial_guess",
str(PROJECT_FOLDER) +
"/examples/getting_started/example_cyclic_movement.py")
cyclic_movement = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cyclic_movement)
np.random.seed(42)
ocp = cyclic_movement.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
final_time=1,
number_shooting_points=10,
loop_from_constraint=False,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 56851.88181545)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (67, 1))
np.testing.assert_almost_equal(g, np.zeros((67, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(
q[:, 0], np.array([1.60205103, -0.01069317, 0.62477988]))
np.testing.assert_almost_equal(q[:, -1], np.array([1, 0, 1.57]))
# initial and final velocities
np.testing.assert_almost_equal(
qdot[:, 0], np.array((0.12902365, 0.09340155, -0.20256713)))
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([9.89210954, 9.39362112, -15.53061197]))
np.testing.assert_almost_equal(
tau[:, -1], np.array([17.16370432, 9.78643138, -26.94701577]))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
def test_custom_constraint_align_markers(ode_solver):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"custom_constraint",
str(PROJECT_FOLDER) + "/examples/getting_started/custom_constraint.py")
custom_constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(custom_constraint)
ocp = custom_constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
ode_solver=ode_solver)
sol = ocp.solve()
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (186, 1))
np.testing.assert_almost_equal(g, np.zeros((186, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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)))
if ode_solver == OdeSolver.IRK:
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 19767.53312569523)
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array((1.4516129, 9.81, 2.27903226)))
np.testing.assert_almost_equal(
tau[:, -1], np.array((-1.45161291, 9.81, -2.27903226)))
else:
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 19767.533125695223)
np.testing.assert_almost_equal(
tau[:, 0], np.array(
(1.4516128810214546, 9.81, 2.2790322540381487)))
np.testing.assert_almost_equal(
tau[:, -1],
np.array((-1.4516128810214546, 9.81, -2.2790322540381487)))
def test_multiphase_time_constraint():
# Load time_constraint
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"multiphase_time_constraint",
str(PROJECT_FOLDER) +
"/examples/optimal_time_ocp/multiphase_time_constraint.py")
multiphase_time_constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(multiphase_time_constraint)
ocp = multiphase_time_constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/optimal_time_ocp/cube.bioMod",
final_time=(2, 5, 4),
time_min=[1, 3, 0.1],
time_max=[2, 4, 0.8],
number_shooting_points=(20, 30, 20),
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 55582.04125059745)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (444, 1))
np.testing.assert_almost_equal(g, np.zeros((444, 1)))
# Check some of the results
states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True)
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
tf = param["time"][0, 0]
# 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((5.71428583, 9.81, 0)))
np.testing.assert_almost_equal(tau[:, -1],
np.array((-8.92857121, 9.81, -14.01785679)))
# optimized time
np.testing.assert_almost_equal(tf, 1.0)
# save and load
TestUtils.save_and_load(sol, ocp, True)
def test_acados_one_end_constraints():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"constraint",
str(PROJECT_FOLDER) + "/examples/acados/cube.py",
)
constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(constraint)
ocp = constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod",
nbs=10,
tf=2,
)
model = ocp.nlp[0].model
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=0,
weight=100,
target=np.array([[1]]))
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
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.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
ocp.update_constraints(constraints)
sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0})
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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[:, -1],
np.array((-2.72727272, 9.81, 0)),
decimal=6)
def test_align_and_minimize_marker_displacement_global():
# Load align_and_minimize_marker_velocity
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_and_minimize_marker_velocity",
str(PROJECT_FOLDER) + "/examples/align/align_and_minimize_marker_velocity.py",
)
align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_and_minimize_marker_velocity)
ocp = align_and_minimize_marker_velocity.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod",
number_shooting_points=5,
final_time=1,
marker_velocity_or_displacement="disp",
marker_in_first_coordinates_system=False,
control_type=ControlType.CONSTANT,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -143.5854887928483)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0.71797344, -0.44573002, -3.00001922, 0.02378758]), decimal=2)
np.testing.assert_almost_equal(q[:, -1], np.array([1.08530972, -0.3869361, 2.99998083, -0.02378757]), decimal=2)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([0.37791617, 3.70167396, 10.0, 10.0]), decimal=2)
np.testing.assert_almost_equal(qdot[:, -1], np.array([0.37675299, -3.40771446, 10.0, 10.0]), decimal=2)
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([-4.52595667e-02, 9.25475333e-01, -4.34001849e-08, -9.24667407e01]), decimal=2
)
np.testing.assert_almost_equal(
tau[:, -1], np.array([4.42976253e-02, 1.40077846e00, -7.28864793e-13, 9.24667396e01]), decimal=2
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_align_and_minimize_marker_displacement_RT():
# Load align_and_minimize_marker_velocity
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_and_minimize_marker_velocity",
str(PROJECT_FOLDER) + "/examples/align/align_and_minimize_marker_velocity.py",
)
align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_and_minimize_marker_velocity)
ocp = align_and_minimize_marker_velocity.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod",
number_shooting_points=5,
final_time=1,
marker_velocity_or_displacement="disp",
marker_in_first_coordinates_system=True,
control_type=ControlType.CONSTANT,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -200.80194174353494)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0.02595694, -0.57073004, -1.00000001, 1.57079633]))
np.testing.assert_almost_equal(q[:, -1], np.array([0.05351275, -0.44696334, 1.00000001, 1.57079633]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([5.03822848, 4.6993718, 10.0, -10.0]))
np.testing.assert_almost_equal(qdot[:, -1], np.array([-4.76267039, -3.46170478, 10.0, 10.0]))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([-2.62720589e01, 4.40828815e00, -5.68180291e-08, 2.61513716e-08])
)
np.testing.assert_almost_equal(
tau[:, -1], np.array([-2.62720590e01, 4.40828815e00, 5.68179790e-08, 2.61513677e-08])
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_align_segment_on_rt():
# Load align_segment_on_rt
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_segment_on_rt",
str(PROJECT_FOLDER) + "/examples/align/align_segment_on_rt.py")
align_segment_on_rt = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_segment_on_rt)
ocp = align_segment_on_rt.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/align/cube_and_line.bioMod",
final_time=0.5,
number_shooting_points=8,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 197120.95524154368)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (91, 1))
np.testing.assert_almost_equal(g, np.zeros((91, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0],
np.array([0.30543155, 0, -1.57, -1.57]))
np.testing.assert_almost_equal(q[:, -1],
np.array([0.30543155, 0, 1.57, 1.57]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([0, 9.81, 66.98666900582079, 66.98666424580644]))
np.testing.assert_almost_equal(
tau[:, -1],
np.array([-0, 9.81, -66.98666900582079, -66.98666424580644]))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_align_markers_with_actuators(ode_solver):
# Load align_markers
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_markers",
str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/align_markers_with_torque_actuators.py")
align_markers = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_markers)
ocp = align_markers.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/cube.bioMod",
number_shooting_points=30,
final_time=2,
actuator_type=1,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 204.18087334169184)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (186, 1))
np.testing.assert_almost_equal(g, np.zeros((186, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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[:, -1],
np.array((-0.2196496, 0.981, -0.3448498)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_initial_guesses(interpolation, ode_solver):
# Load initial_guess
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"initial_guess",
str(PROJECT_FOLDER) +
"/examples/getting_started/custom_initial_guess.py")
initial_guess = importlib.util.module_from_spec(spec)
spec.loader.exec_module(initial_guess)
np.random.seed(42)
ocp = initial_guess.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
final_time=1,
number_shooting_points=5,
initial_guess=interpolation,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 13954.735)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (36, 1))
np.testing.assert_almost_equal(g, np.zeros((36, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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([5.0, 9.81, 7.85]))
np.testing.assert_almost_equal(tau[:, -1], np.array([-5.0, 9.81, -7.85]))
# save and load
if interpolation in [InterpolationType.CUSTOM]:
with pytest.raises(AttributeError):
TestUtils.save_and_load(sol, ocp, True)
else:
TestUtils.save_and_load(sol, ocp, True)
def test_time_constraint():
# Load time_constraint
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"time_constraint",
str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/time_constraint.py")
time_constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(time_constraint)
ocp = time_constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/optimal_time_ocp/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
time_min=0.6,
time_max=1,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 1451.2202233368012)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True)
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
tf = param["time"][0, 0]
# 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, 3.14)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((22.49775, 0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((-33.9047809, 0)))
# optimized time
np.testing.assert_almost_equal(tf, 1.0)
# save and load
TestUtils.save_and_load(sol, ocp, True)
def test_symmetry_by_constraint(ode_solver):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"symmetry_by_constraint",
str(PROJECT_FOLDER) +
"/examples/symmetrical_torque_driven_ocp/symmetry_by_constraint.py",
)
symmetry_by_constraint = importlib.util.module_from_spec(spec)
spec.loader.exec_module(symmetry_by_constraint)
ocp = symmetry_by_constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/symmetrical_torque_driven_ocp/cubeSym.bioMod",
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 240.92170742402698)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (276, 1))
np.testing.assert_almost_equal(g, np.zeros((276, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(
q[:, 0], np.array((-0.2, -1.1797959, 0.20135792, -0.20135792)))
np.testing.assert_almost_equal(
q[:, -1], np.array((0.2, -0.7797959, -0.20135792, 0.20135792)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array((1.16129033, 1.16129033, -0.58458751, 0.58458751)))
np.testing.assert_almost_equal(
tau[:, -1],
np.array((-1.16129033, -1.16129033, 0.58458751, -0.58458751)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_pendulum(nb_threads, use_SX):
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"pendulum",
str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py")
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
nb_threads=nb_threads,
use_SX=use_SX,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 6657.974502951726)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# 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, 3.14)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((16.25734477, 0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((-25.59944635, 0)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
def test_align_marker_on_segment():
# Load align_marker_on_segment
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_marker_on_segment",
str(PROJECT_FOLDER) + "/examples/align/align_marker_on_segment.py")
align_marker_on_segment = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_marker_on_segment)
ocp = align_marker_on_segment.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/align/cube_and_line.bioMod",
final_time=0.5,
number_shooting_points=8,
initialize_near_solution=True,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 42127.04677760122)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (88, 1))
np.testing.assert_almost_equal(g, np.zeros((88, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([1, 0, 0, 0.46364761]))
np.testing.assert_almost_equal(q[:, -1], np.array([2, 0, 1.57,
0.78539785]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([23.6216587, 12.2590703, 31.520697, 12.9472294]))
np.testing.assert_almost_equal(
tau[:, -1], np.array([-16.659525, 14.5872277, -36.1009998, 4.417834]))
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_pendulum_min_time_lagrange():
# Load pendulum_min_time_Lagrange
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"pendulum_min_time_Lagrange",
str(PROJECT_FOLDER) +
"/examples/optimal_time_ocp/pendulum_min_time_Lagrange.py")
pendulum_min_time_Lagrange = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum_min_time_Lagrange)
ocp = pendulum_min_time_Lagrange.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/optimal_time_ocp/pendulum.bioMod",
final_time=2,
number_shooting_points=10,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 0.062092703196434854)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)), decimal=6)
# Check some of the results
states, controls, param = Data.get_data(ocp, sol["x"], get_parameters=True)
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
tf = param["time"][0, 0]
# 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, 3.14)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((59.9529745, 0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((-99.9980341, 0)))
# optimized time
np.testing.assert_almost_equal(tf, 0.6209270319643485)
# save and load
TestUtils.save_and_load(sol, ocp, True)
def test_cyclic_constraint(ode_solver):
# Load initial_guess
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"initial_guess",
str(PROJECT_FOLDER) +
"/examples/getting_started/example_cyclic_movement.py")
cyclic_movement = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cyclic_movement)
np.random.seed(42)
ocp = cyclic_movement.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
final_time=1,
number_shooting_points=10,
loop_from_constraint=True,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 78921.61000000016)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (73, 1))
np.testing.assert_almost_equal(g, np.zeros((73, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([1, 0, 1.57]))
np.testing.assert_almost_equal(q[:, -1], np.array([1, 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([20.0, 9.81, -31.4]))
np.testing.assert_almost_equal(tau[:, -1], np.array([20.0, 9.81, -31.4]))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
def test_align_and_minimize_marker_velocity_linear_controls():
# Load align_and_minimize_marker_velocity
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"align_and_minimize_marker_velocity",
str(PROJECT_FOLDER) + "/examples/align/align_and_minimize_marker_velocity.py",
)
align_and_minimize_marker_velocity = importlib.util.module_from_spec(spec)
spec.loader.exec_module(align_and_minimize_marker_velocity)
ocp = align_and_minimize_marker_velocity.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod",
number_shooting_points=5,
final_time=1,
marker_velocity_or_displacement="velo",
marker_in_first_coordinates_system=True,
control_type=ControlType.LINEAR_CONTINUOUS,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], -80.28869898410233)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (40, 1))
np.testing.assert_almost_equal(g, np.zeros((40, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[2:, 0], np.array([-3.14159264, 0]))
np.testing.assert_almost_equal(q[2:, -1], np.array([3.14159264, 0]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[2:, 0], np.array([10, 0]))
np.testing.assert_almost_equal(qdot[2:, -1], np.array([10, 0]))
# initial and final controls
np.testing.assert_almost_equal(tau[2:, 0], np.array([-8.495542, 0]), decimal=5)
np.testing.assert_almost_equal(tau[2:, -1], np.array([8.495541, 0]), decimal=5)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp, decimal_value=6)
def test_maximize_predicted_height_CoM_with_actuators():
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"maximize_predicted_height_CoM",
str(PROJECT_FOLDER) +
"/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py",
)
maximize_predicted_height_CoM = importlib.util.module_from_spec(spec)
spec.loader.exec_module(maximize_predicted_height_CoM)
ocp = maximize_predicted_height_CoM.prepare_ocp(
model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_with_contact/2segments_4dof_2contacts.bioMod",
phase_time=0.5,
number_shooting_points=20,
use_actuators=True,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 0.21850679397314332)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (160, 1))
np.testing.assert_almost_equal(g, np.zeros((160, 1)), decimal=6)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0.0, 0.0, -0.5, 0.5)))
np.testing.assert_almost_equal(
q[:, -1], np.array((-0.2393758, 0.0612086, -0.0006739, 0.0006739)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(
qdot[:, -1],
np.array(
(-4.8768219e-01, 3.2867302e-04, 9.7536459e-01, -9.7536459e-01)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.550905)))
np.testing.assert_almost_equal(tau[:, -1], np.array(-0.0050623))
# save and load
TestUtils.save_and_load(sol, ocp, False)
def warm_start_nmpc(sol, ocp):
state, ctrl, param = Data.get_data(ocp,
sol,
concatenate=False,
get_parameters=True)
u_init = InitialGuessList()
x_init = InitialGuessList()
for i in range(ocp.nb_phases):
u_init.add(np.concatenate([ctrl[d][i][:, :-1] for d in ctrl]),
interpolation=InterpolationType.EACH_FRAME)
x_init.add(np.concatenate([state[d][i] for d in state]),
interpolation=InterpolationType.EACH_FRAME)
time = InitialGuess(param["time"], name="time")
ocp.update_initial_guess(x_init=x_init, u_init=u_init, param_init=time)
def test_maximize_predicted_height_CoM(ode_solver):
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"maximize_predicted_height_CoM",
str(PROJECT_FOLDER) +
"/examples/torque_driven_with_contact/maximize_predicted_height_CoM.py",
)
maximize_predicted_height_CoM = importlib.util.module_from_spec(spec)
spec.loader.exec_module(maximize_predicted_height_CoM)
ocp = maximize_predicted_height_CoM.prepare_ocp(
model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_with_contact/2segments_4dof_2contacts.bioMod",
phase_time=0.5,
number_shooting_points=20,
use_actuators=False,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 0.7592028279017864)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (160, 1))
np.testing.assert_almost_equal(g, np.zeros((160, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0.0, 0.0, -0.5, 0.5)))
np.testing.assert_almost_equal(
q[:, -1], np.array((0.1189651, -0.0904378, -0.7999996, 0.7999996)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(
qdot[:, -1], np.array((1.2636414, -1.3010929, -3.6274687, 3.6274687)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-22.1218282)))
np.testing.assert_almost_equal(tau[:, -1], np.array(0.2653957))
# save and load
TestUtils.save_and_load(sol, ocp, False)
def test_external_forces():
# Load external_forces
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"external_forces",
str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/external_forces.py")
external_forces = importlib.util.module_from_spec(spec)
spec.loader.exec_module(external_forces)
ocp = external_forces.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/cube_with_forces.bioMod", )
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 9875.88768746912)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (246, 1))
np.testing.assert_almost_equal(g, np.zeros((246, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], controls["tau"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(q[:, -1], np.array((0, 2, 0, 0)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0, 0)))
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.71322593, 0, 0)))
np.testing.assert_almost_equal(tau[:, 10], np.array((0, 7.71100122, 0, 0)))
np.testing.assert_almost_equal(tau[:, 20], np.array((0, 5.70877651, 0, 0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((0, 3.90677425, 0, 0)))
# save and load
TestUtils.save_and_load(sol, ocp, True)
# simulate
TestUtils.simulate(sol, ocp)
def warm_start_mhe(ocp, sol, use_activation=False):
# Define problem variable
states, controls = Data.get_data(ocp, sol)
q, qdot = states["q"], states["qdot"]
u = controls["muscles"]
x = np.vstack([q, qdot]) if use_activation else np.vstack([q, qdot, states["muscles"]])
w_tau = "tau" in controls.keys()
if w_tau:
u = np.vstack([controls["tau"], u])
else:
u = u
# Prepare data to return
x0 = np.hstack((x[:, 1:], np.tile(x[:, [-1]], 1))) # discard oldest estimate of the window, duplicates youngest
u0 = u[:, 1:]
x_out = x[:, 0]
u_out = u[:, 0]
return x0, u0, x_out, u_out
def test_custom_problem_type_and_dynamics(problem_type_custom, ode_solver):
# Load pendulum
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"custom_problem_type_and_dynamics",
str(PROJECT_FOLDER) + "/examples/getting_started/custom_dynamics.py",
)
pendulum = importlib.util.module_from_spec(spec)
spec.loader.exec_module(pendulum)
ocp = pendulum.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.bioMod",
problem_type_custom=problem_type_custom,
ode_solver=ode_solver,
)
sol = ocp.solve()
# Check objective function value
f = np.array(sol["f"])
np.testing.assert_equal(f.shape, (1, 1))
np.testing.assert_almost_equal(f[0, 0], 19767.5331257)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (186, 1))
np.testing.assert_almost_equal(g, np.zeros((186, 1)))
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau = states["q"], states["q_dot"], 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.4516129, 9.81, 2.27903226)))
np.testing.assert_almost_equal(tau[:, -1],
np.array((-1.45161291, 9.81, -2.27903226)))