def test_implicit_dynamics_errors(dynamics):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() +
"/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
nlp.ns = 5
nlp.cx = MX
nlp.u_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(dynamics, implicit_dynamics=True),
False,
)
# Prepare the dynamics
with pytest.raises(
TypeError,
match=re.escape(
f"{dynamics.name.lower()}() got an unexpected keyword argument "
"'implicit_dynamics'"),
):
ConfigureProblem.initialize(ocp, nlp)
def test_custom_dynamics(with_contact):
def custom_dynamic(states, controls, parameters, nlp, with_contact=False) -> tuple:
DynamicsFunctions.apply_parameters(parameters, nlp)
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
tau = DynamicsFunctions.get(nlp.controls["tau"], controls)
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot)
ddq = DynamicsFunctions.forward_dynamics(nlp, q, qdot, tau, with_contact)
return dq, ddq
def configure(ocp, nlp, with_contact=None):
ConfigureProblem.configure_q(nlp, True, False)
ConfigureProblem.configure_qdot(nlp, True, False)
ConfigureProblem.configure_tau(nlp, False, True)
ConfigureProblem.configure_dynamics_function(ocp, nlp, custom_dynamic, with_contact=with_contact)
if with_contact:
ConfigureProblem.configure_contact_function(ocp, nlp, DynamicsFunctions.forces_from_torque_driven)
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = MX
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp, "dynamics_type", Dynamics(configure, dynamic_function=custom_dynamic, with_contact=with_contact), False
)
np.random.seed(42)
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
np.testing.assert_almost_equal(
x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716]
)
np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642],
)
def test_torque_driven_implicit_soft_contacts(with_contact, cx,
implicit_contact):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() +
"/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * (2 + 3), 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ() * 2, 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
nlp.phase_idx = 0
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(DynamicsFcn.TORQUE_DRIVEN,
with_contact=with_contact,
implicit_soft_contacts=implicit_contact),
False,
)
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
np.random.seed(42)
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(
nlp.contact_forces_func(states, controls, params))
np.testing.assert_almost_equal(x_out[:, 0], [
0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131,
0.6507164, -0.2359716
])
np.testing.assert_almost_equal(contact_out[:, 0],
[-2.444071, 128.8816865, 2.7245124])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3024137,
-10.3850379, 1.6044517, 35.8023864
],
)
def test_torque_driven(with_contact, with_external_force, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() + "/examples/getting_started/models/2segments_4dof_2contacts.bioMod"
)
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 3, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(ocp, "dynamics_type", Dynamics(DynamicsFcn.TORQUE_DRIVEN, with_contact=with_contact), False)
np.random.seed(42)
if with_external_force:
external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(nlp.contact_forces_func(states, controls, params))
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.8631034, 0.3251833, 0.1195942, 0.4937956, -7.7700092, -7.5782306, 21.7073786, -16.3059315],
)
np.testing.assert_almost_equal(contact_out[:, 0], [-47.8131136, 111.1726516, -24.4449121])
else:
np.testing.assert_almost_equal(
x_out[:, 0], [0.6118529, 0.785176, 0.6075449, 0.8083973, -0.3214905, -0.1912131, 0.6507164, -0.2359716]
)
np.testing.assert_almost_equal(contact_out[:, 0], [-2.444071, 128.8816865, 2.7245124])
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.86310343, 0.32518332, 0.11959425, 0.4937956, 0.30731739, -9.97912778, 1.15263778, 36.02430956],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.61185289, 0.78517596, 0.60754485, 0.80839735, -0.30241366, -10.38503791, 1.60445173, 35.80238642],
)
def test_torque_derivative_driven_implicit(with_contact, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(
TestUtils.bioptim_folder() +
"/examples/getting_started/models/2segments_4dof_2contacts.bioMod")
nlp.ns = 5
nlp.cx = cx
nlp.phase_idx = 0
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 4, 1))
nlp.u_bounds = np.zeros((nlp.model.nbQ(), 2))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(DynamicsFcn.TORQUE_DERIVATIVE_DRIVEN,
with_contact=with_contact,
implicit_dynamics=True),
False,
)
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
np.random.seed(42)
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact:
contact_out = np.array(
nlp.contact_forces_func(states, controls, params))
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
0.8631034,
0.3251833,
0.1195942,
0.4937956,
0.0314292,
0.2492922,
0.2897515,
0.8714606,
],
)
np.testing.assert_almost_equal(contact_out[:, 0],
[-2.444071, 128.8816865, 2.7245124])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6118529,
0.785176,
0.6075449,
0.8083973,
0.3886773,
0.5426961,
0.7722448,
0.7290072,
0.8631034,
0.3251833,
0.1195942,
0.4937956,
0.0314292,
0.2492922,
0.2897515,
0.8714606,
],
)
def test_muscle_driven(with_excitations, with_contact, with_residual_torque, with_external_force, cx):
# Prepare the program
nlp = NonLinearProgram()
nlp.model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/muscle_driven_ocp/arm26_with_contact.bioMod")
nlp.ns = 5
nlp.cx = cx
nlp.x_bounds = np.zeros((nlp.model.nbQ() * 2 + nlp.model.nbMuscles(), 1))
nlp.u_bounds = np.zeros((nlp.model.nbMuscles(), 1))
ocp = OptimalControlProgram(nlp)
nlp.control_type = ControlType.CONSTANT
NonLinearProgram.add(
ocp,
"dynamics_type",
Dynamics(
DynamicsFcn.MUSCLE_DRIVEN,
with_residual_torque=with_residual_torque,
with_excitations=with_excitations,
with_contact=with_contact,
),
False,
)
np.random.seed(42)
if with_external_force:
external_forces = [np.random.rand(6, nlp.model.nbSegment(), nlp.ns)]
nlp.external_forces = BiorbdInterface.convert_array_to_external_forces(external_forces)[0]
# Prepare the dynamics
ConfigureProblem.initialize(ocp, nlp)
# Test the results
states = np.random.rand(nlp.states.shape, nlp.ns)
controls = np.random.rand(nlp.controls.shape, nlp.ns)
params = np.random.rand(nlp.parameters.shape, nlp.ns)
x_out = np.array(nlp.dynamics_func(states, controls, params))
if with_contact: # Warning this test is a bit bogus, there since the model does not have contacts
if with_residual_torque:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.07179591,
-33.76216759,
36.218136,
46.87928022,
-1.80189035,
53.3914525,
48.30056919,
63.69373374,
-28.15700995,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.29661929e00,
3.00871754e02,
-9.50353966e02,
8.60630831e00,
3.19433638e00,
2.97405608e01,
-2.02754226e01,
-2.32467778e01,
-4.19135012e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -8.06477702e00, 2.42278904e02, -7.72113478e02],
decimal=6,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80891773e00, 1.20475911e02, -4.33290914e02],
decimal=6,
)
else:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
0.91952666,
-39.04874824,
45.31834295,
55.65557816,
50.47052688,
0.36025589,
58.92377491,
29.70094194,
-15.13534937,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.72711369e00,
3.10865851e02,
-9.82724687e02,
-7.72228930e00,
-1.13759732e01,
9.51906209e01,
4.45077128e00,
-5.20261014e00,
-2.80864106e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.6158501, 0.50313626, 0.64241928, 0.91952666, -39.04874824, 45.31834295],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72711369e00, 3.10865851e02, -9.82724687e02],
decimal=6,
)
else:
if with_residual_torque:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.50313626,
0.64241928,
1.07179591,
-33.76216759,
36.218136,
46.87928022,
-1.80189035,
53.3914525,
48.30056919,
63.69373374,
-28.15700995,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.29661929e00,
3.00871754e02,
-9.50353966e02,
8.60630831e00,
3.19433638e00,
2.97405608e01,
-2.02754226e01,
-2.32467778e01,
-4.19135012e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[6.15850098e-01, 5.03136259e-01, 6.42419278e-01, -8.06477702e00, 2.42278904e02, -7.72113478e02],
decimal=6,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -3.80891773e00, 1.20475911e02, -4.33290914e02],
decimal=6,
)
else:
if with_excitations:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.6158501,
0.5031363,
0.6424193,
0.9195267,
-39.0487482,
45.3183429,
55.6555782,
50.4705269,
0.3602559,
58.9237749,
29.7009419,
-15.1353494,
],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
1.83404510e-01,
6.11852895e-01,
7.85175961e-01,
-9.72711369e00,
3.10865851e02,
-9.82724687e02,
-7.72228930e00,
-1.13759732e01,
9.51906209e01,
4.45077128e00,
-5.20261014e00,
-2.80864106e01,
],
decimal=6,
)
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[0.6158501, 0.50313626, 0.64241928, 0.91952666, -39.04874824, 45.31834295],
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[1.83404510e-01, 6.11852895e-01, 7.85175961e-01, -9.72711369e00, 3.10865851e02, -9.82724687e02],
decimal=6,
)