def test_symmetry_by_constraint(ode_solver):
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], 16.0614471616022)
# 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
q, qdot, tau = ProblemType.get_data_from_V(ocp, sol["x"])
# 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)))
def test_align_markers(ode_solver):
ocp = 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], 1317.835541713015)
# 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
q, qdot, tau = ProblemType.get_data_from_V(ocp, sol["x"])
# 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 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
q, qdot, tau, mus = ProblemType.get_data_from_V(ocp, sol["x"])
# 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
]),
)
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
q, qdot, tau, mus = ProblemType.get_data_from_V(ocp, sol["x"])
# 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
]),
)
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], 9964.984600659047)
# Check constraints
g = np.array(sol["g"])
np.testing.assert_equal(g.shape, (315, 1))
np.testing.assert_almost_equal(g, np.zeros((315, 1)))
# Check some of the results
q, qdot, tau = ProblemType.get_data_from_V(ocp, sol["x"])
# 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, 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)))
# 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.36464516)))
np.testing.assert_almost_equal(tau[1][:, -1],
np.array((0.2322581, 9.81, -0.36464516)))
markers_ref,
muscle_excitations_ref,
x_ref[:biorbd_model.nbQ(), :].T,
show_online_optim=False,
kin_data_to_track="markers",
)
# --- Solve the program --- #
sol = ocp.solve()
# --- Show the results --- #
muscle_excitations_ref = np.append(muscle_excitations_ref,
muscle_excitations_ref[-1:, :],
axis=0)
q, qdot, tau, mus = ProblemType.get_data_from_V(ocp, sol["x"])
n_q = ocp.nlp[0]["model"].nbQ()
n_mark = ocp.nlp[0]["model"].nbMarkers()
n_frames = q.shape[1]
mus_act = mus[0]
mus_exci = np.array(mus[1])
markers = np.ndarray((3, n_mark, q.shape[1]))
markers_func = []
for i in range(n_mark):
markers_func.append(
Function(
"ForwardKin",
[ocp.symbolic_states],
[biorbd_model.marker(ocp.symbolic_states[:n_q], i).to_mx()],
["q"],
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,
show_online_optim=False,
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
q, qdot, tau, mus = ProblemType.get_data_from_V(ocp, sol["x"])
# 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
]),
)
from matplotlib import pyplot as plt
from casadi import vertcat, Function
contact_forces = np.zeros((6, sum([nlp["ns"] for nlp in ocp.nlp]) + 1))
cs_map = (range(6), (0, 1, 3, 4))
for i, nlp in enumerate(ocp.nlp):
CS_func = Function(
"Contact_force_inequality",
[ocp.symbolic_states, ocp.symbolic_controls],
[nlp["model"].getConstraints().getForce().to_mx()],
["x", "u"],
["CS"],
).expand()
q, q_dot, u = ProblemType.get_data_from_V(ocp, sol["x"], i)
x = vertcat(q, q_dot)
if i == 0:
contact_forces[cs_map[i], :nlp["ns"] + 1] = CS_func(x, u)
else:
contact_forces[cs_map[i],
ocp.nlp[i - 1]["ns"]:ocp.nlp[i - 1]["ns"] +
nlp["ns"] + 1] = CS_func(x, u)
plt.plot(contact_forces.T)
plt.show()
try:
from BiorbdViz import BiorbdViz
x, _, _ = ProblemType.get_data_from_V(ocp, sol["x"])
q = np.ndarray((ocp.nlp[0]["model"].nbQ(),