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_activation_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() * 2, 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_ACTIVATIONS_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.8631, 0.32518, 0.11959, 0.4938, 19.01887, 18.51503,
-53.08574, 58.48719
],
decimal=5,
)
np.testing.assert_almost_equal(
contact_out[:, 0], [109.8086936, 3790.3932439, -3571.7858574])
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
0.61185289, 0.78517596, 0.60754485, 0.80839735, 0.78455384,
-0.16844256, -1.56184114, 1.97658587
],
decimal=5,
)
np.testing.assert_almost_equal(
contact_out[:, 0], [-7.88958997, 329.70828173, -263.55516549])
else:
if with_external_force:
np.testing.assert_almost_equal(
x_out[:, 0],
[
8.63103426e-01,
3.25183322e-01,
1.19594246e-01,
4.93795596e-01,
1.73558072e01,
-4.69891264e01,
1.81396922e02,
3.61170139e03,
],
decimal=5,
)
else:
np.testing.assert_almost_equal(
x_out[:, 0],
[
6.11852895e-01,
7.85175961e-01,
6.07544852e-01,
8.08397348e-01,
-2.38262975e01,
-5.82033454e01,
1.27439020e02,
3.66531163e03,
],
decimal=5,
)
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,
],
)