def test_align_segment_on_rt(ode_solver):
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,
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], 12320.059717265229)
# 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.305837645, 6.07684988e-18, -1.57, -1.57]))
np.testing.assert_almost_equal(q[:, -1], np.array([0.305837645, 2.34331392e-17, 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([1.09038782e-23, 9.81, 66.9866667, 66.9866667]))
np.testing.assert_almost_equal(tau[:, -1], np.array([-1.61910771e-23, 9.81, -66.9866667, -66.9866667]))
# save and load
TestUtils.save_and_load(sol, ocp, False)
def test_align_segment_on_rt(ode_solver):
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,
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], 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)
def test_custom_constraint_align_markers(ode_solver):
ocp = custom_constraint.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/getting_started/cube.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], 19767.533125695223)
# 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.4516128810214546, 9.81, 2.2790322540381487)))
np.testing.assert_almost_equal(
tau[:, -1], np.array((-1.4516128810214546, 9.81, -2.2790322540381487)))
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_muscle_driven_ocp():
# Load static_arm
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"static_arm",
str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/static_arm.py",
)
static_arm = importlib.util.module_from_spec(spec)
spec.loader.exec_module(static_arm)
ocp = static_arm.prepare_ocp(str(PROJECT_FOLDER) +
"/examples/muscle_driven_ocp/arm26.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.14350464848810182)
# 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 = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls[
"tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0.07, 1.4]))
np.testing.assert_almost_equal(q[:, -1], np.array([-0.9451058, 3.0704789]))
# 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.4115254, -0.5586797]))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array([0.0014793, 0.0052082]))
np.testing.assert_almost_equal(tau[:, -1],
np.array([-0.0002795, 0.0006926]))
np.testing.assert_almost_equal(
mus[:, 0],
np.array([
2.2869218e-06, 1.6503522e-01, 1.0002514e-01, 4.0190181e-06,
4.1294041e-06, 1.0396051e-01
]),
)
np.testing.assert_almost_equal(
mus[:, -1],
np.array([
4.2599283e-03, 3.2188697e-05, 3.1307377e-05, 2.0121186e-03,
2.0048373e-03, 1.8235679e-03
]),
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
def test_align_and_minimize_marker_velocity():
# 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,
)
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 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[:, 0], np.array([0.79919667, -0.11729546, -3.1415926, 0]))
np.testing.assert_almost_equal(
q[:, -1], np.array([0.80144593, -0.10039572, 3.1415926, 0]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0],
np.array([1.8420249e-03, 1.0703158, 10, 0]))
np.testing.assert_almost_equal(
qdot[:, -1], np.array([2.9192347e-03, -0.96476530, 10, 0]))
# # initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([5.6477202e-03, 3.9487332, -8.4955414, 0]))
np.testing.assert_almost_equal(
tau[:, -1], np.array([-9.9579303e-03, 4.0992343, 8.4955414, 0]))
# # save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp, decimal_value=6)
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_muscle_with_contact_driven_ocp():
ocp = static_arm_with_contact.prepare_ocp(
str(PROJECT_FOLDER) +
"/examples/muscle_driven_ocp/arm26_with_contact.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.12145850795787627)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (60, 1))
np.testing.assert_almost_equal(g, np.zeros((60, 1)), decimal=6)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls[
"tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0, 0.07, 1.4]))
np.testing.assert_almost_equal(
q[:, -1], np.array([0.00806796, -0.95744612, 3.09501746]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0.0, 0.0]))
np.testing.assert_almost_equal(
qdot[:, -1], np.array([0.00060479, 0.34617159, -0.42910107]))
# initial and final controls
np.testing.assert_almost_equal(
tau[:, 0], np.array([-4.67191123e-05, 4.58080354e-03, 4.23701953e-03]))
np.testing.assert_almost_equal(
tau[:, -1],
np.array([-1.19345958e-05, -1.34810157e-03, 3.12378179e-03]))
np.testing.assert_almost_equal(
mus[:, 0],
np.array([
6.86280305e-06, 1.68961894e-01, 8.70635867e-02, 2.47160155e-05,
2.53946780e-05, 8.45438966e-02
]),
)
np.testing.assert_almost_equal(
mus[:, -1],
np.array([
1.96721725e-02, 3.42398501e-05, 3.29454916e-05, 8.61757459e-03,
8.57946846e-03, 7.07152302e-03
]),
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
def test_muscle_activations_and_states_tracking():
# Define the problem
model_path = str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod"
biorbd_model = biorbd.Model(model_path)
final_time = 2
nb_shooting = 9
# Generate random data to fit
np.random.seed(42)
t, markers_ref, x_ref, muscle_activations_ref = muscle_activations_tracker.generate_data(
biorbd_model, final_time, nb_shooting
)
biorbd_model = biorbd.Model(model_path) # To allow for non free variable, the model must be reloaded
ocp = muscle_activations_tracker.prepare_ocp(
biorbd_model,
final_time,
nb_shooting,
markers_ref,
muscle_activations_ref,
x_ref[: biorbd_model.nbQ(), :].T,
kin_data_to_track="q",
)
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], 1.4506639252752042e-06)
# 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)), decimal=6)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([-1.13043502e-05, -1.35629661e-05]))
np.testing.assert_almost_equal(q[:, -1], np.array([-0.49387966, -1.44924784]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([-8.66527631e-05, -1.31486656e-04]))
np.testing.assert_almost_equal(qdot[:, -1], np.array([0.8780829, -2.6474387]))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array([-1.55359644e-06, 1.26569700e-05]))
np.testing.assert_almost_equal(tau[:, -1], np.array([-7.41845169e-06, -7.67568954e-07]))
np.testing.assert_almost_equal(
mus[:, 0], np.array([0.37439688, 0.95073361, 0.73203047, 0.59855246, 0.15592687, 0.15595739]),
)
np.testing.assert_almost_equal(
mus[:, -1], np.array([0.54685367, 0.18482085, 0.96945157, 0.77512036, 0.93947405, 0.89483397]),
)
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_multiphase_align_markers(ode_solver):
ocp = multiphase_align_markers.prepare_ocp(
biorbd_model_path=str(PROJECT_FOLDER) +
"/examples/torque_driven_ocp/cube.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], 17672.950313589874)
# 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 = 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[0][:, -1], np.array((2, 0, 0)))
np.testing.assert_almost_equal(q[1][:, 0], np.array((2, 0, 0)))
np.testing.assert_almost_equal(q[1][:, -1], np.array((1, 0, 0)))
np.testing.assert_almost_equal(q[2][:, 0], np.array((1, 0, 0)))
np.testing.assert_almost_equal(q[2][:, -1], np.array((2, 0, 1.57)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[0][:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[0][:, -1], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[1][:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[1][:, -1], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[2][:, 0], np.array((0, 0, 0)))
np.testing.assert_almost_equal(qdot[2][:, -1], np.array((0, 0, 0)))
# initial and final controls
np.testing.assert_almost_equal(tau[0][:, 0], np.array(
(1.42857142, 9.81, 0)))
np.testing.assert_almost_equal(tau[0][:, -1],
np.array((-1.42857144, 9.81, 0)))
np.testing.assert_almost_equal(tau[1][:, 0],
np.array((-0.2322581, 9.81, 0.0)))
np.testing.assert_almost_equal(tau[1][:, -1],
np.array((0.2322581, 9.81, -0.0)))
np.testing.assert_almost_equal(tau[2][:, 0],
np.array((0.35714285, 9.81, 0.56071428)))
np.testing.assert_almost_equal(tau[2][:, -1],
np.array((-0.35714285, 9.81, -0.56071428)))
# save and load
TestUtils.save_and_load(sol, ocp, False)
def test_cyclic_objective():
# 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,
)
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.881815451816)
# 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_time_constraint(ode_solver):
# 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_muscle_driven_ocp():
ocp = static_arm.prepare_ocp(str(PROJECT_FOLDER) +
"/examples/muscle_driven_ocp/arm26.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.12145862454010191)
# 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 = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls[
"tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0.07, 1.4]))
np.testing.assert_almost_equal(q[:, -1],
np.array([-0.95746379, 3.09503322]))
# 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.34608528, -0.42902204]))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0],
np.array([0.00458156, 0.00423722]))
np.testing.assert_almost_equal(tau[:, -1],
np.array([-0.00134814, 0.00312362]))
np.testing.assert_almost_equal(
mus[:, 0],
np.array([
6.86190724e-06, 1.68975654e-01, 8.70683950e-02, 2.47147957e-05,
2.53934274e-05, 8.45479390e-02
]),
)
np.testing.assert_almost_equal(
mus[:, -1],
np.array([
1.96717613e-02, 3.42406388e-05, 3.29456098e-05, 8.61728932e-03,
8.57918458e-03, 7.07096066e-03
]),
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
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_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_muscle_activations_with_contact_driven_ocp():
# Load static_arm_with_contact
PROJECT_FOLDER = Path(__file__).parent / ".."
spec = importlib.util.spec_from_file_location(
"static_arm_with_contact", str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/static_arm_with_contact.py"
)
static_arm_with_contact = importlib.util.module_from_spec(spec)
spec.loader.exec_module(static_arm_with_contact)
ocp = static_arm_with_contact.prepare_ocp(
str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26_with_contact.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.1435025030068162)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (60, 1))
np.testing.assert_almost_equal(g, np.zeros((60, 1)), decimal=6)
# Check some of the results
states, controls = Data.get_data(ocp, sol["x"])
q, qdot, tau, mus = states["q"], states["q_dot"], controls["tau"], controls["muscles"]
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array([0, 0.07, 1.4]))
np.testing.assert_almost_equal(q[:, -1], np.array([0.0081671, -0.9450881, 3.0704626]))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0.0, 0.0]))
np.testing.assert_almost_equal(qdot[:, -1], np.array([0.0009398, 0.4116121, -0.5587618]))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array([-3.9652660e-07, 1.4785825e-03, 5.2079505e-03]))
np.testing.assert_almost_equal(tau[:, -1], np.array([-2.7248808e-06, -2.7952503e-04, 6.9262306e-04]))
np.testing.assert_almost_equal(
mus[:, 0], np.array([2.2873915e-06, 1.6502014e-01, 1.0001872e-01, 4.0192359e-06, 4.1296273e-06, 1.0395487e-01])
)
np.testing.assert_almost_equal(
mus[:, -1], np.array([4.2599697e-03, 3.2187363e-05, 3.1307175e-05, 2.0116712e-03, 2.0043861e-03, 1.8230214e-03])
)
# save and load
TestUtils.save_and_load(sol, ocp, False)
# simulate
TestUtils.simulate(sol, ocp)
def test_pendulum_min_time_lagrange(ode_solver):
# 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_initial_guesses(interpolation):
# 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,
)
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.SPLINE, InterpolationType.CUSTOM]:
with pytest.raises(AttributeError):
TestUtils.save_and_load(sol, ocp, True)
else:
TestUtils.save_and_load(sol, ocp, True)
def test_align_markers(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.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,
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], 19767.53312569522)
# 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.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)
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(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_cyclic_constraint():
# 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,
)
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_markers_with_actuators():
# 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.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,
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], 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_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 test_maximize_predicted_height_CoM_with_actuators(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=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 test_custom_problem_type_and_dynamics(problem_type_custom):
# 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,
)
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)))
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,
)
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_simulate_from_initial_single_shoot():
# 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=4,
)
X = InitialConditions([-1, -2, 1, 0.5])
U = InitialConditions(np.array([[-0.1, 0], [1, 2]]).T,
interpolation=InterpolationType.LINEAR)
sol_simulate_single_shooting = Simulate.from_controls_and_initial_states(
ocp, X, U, single_shoot=True)
# Check some of the results
states, controls = Data.get_data(ocp, sol_simulate_single_shooting["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, -2.0)))
np.testing.assert_almost_equal(q[:, -1], np.array(
(-0.59371229, 2.09731719)))
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5)))
np.testing.assert_almost_equal(qdot[:, -1],
np.array((1.38153013, -0.60425128)))
# initial and final controls
np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0)))
np.testing.assert_almost_equal(tau[:, -1], np.array((1.0, 2.0)))