def convert_array_to_external_forces(all_f_ext):
if not isinstance(all_f_ext, (list, tuple)):
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
sv_over_all_phases = []
for f_ext in all_f_ext:
f_ext = np.array(f_ext)
if len(f_ext.shape) < 2 or len(f_ext.shape) > 3:
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
if len(f_ext.shape) == 2:
f_ext = f_ext[:, :, np.newaxis]
if f_ext.shape[0] != 6:
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
sv_over_phase = []
for node in range(f_ext.shape[2]):
sv = biorbd.VecBiorbdSpatialVector()
for idx in range(f_ext.shape[1]):
sv.append(biorbd.SpatialVector(MX(f_ext[:, idx, node])))
sv_over_phase.append(sv)
sv_over_all_phases.append(sv_over_phase)
return sv_over_all_phases
def custom_dynamic(states: MX, controls: MX, parameters: MX,
nlp: NonLinearProgram) -> tuple:
"""
The dynamics of the system using an external force (see custom_dynamics for more explanation)
Parameters
----------
states: MX
The current states of the system
controls: MX
The current controls of the system
parameters: MX
The current parameters of the system
nlp: NonLinearProgram
A reference to the phase of the ocp
Returns
-------
The state derivative
"""
q, qdot, tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
force_vector = MX.zeros(6)
force_vector[5] = 100 * q[0]**2
f_ext = biorbd.VecBiorbdSpatialVector()
f_ext.append(biorbd.SpatialVector(force_vector))
qddot = nlp.model.ForwardDynamics(q, qdot, tau, f_ext).to_mx()
return qdot, qddot
def test_forward_dynamics_with_external_forces():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
# With external forces
if biorbd.currentLinearAlgebraBackend() == 1:
from casadi import Function, MX
sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
else:
sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6))
sv2 = np.array(
(11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2))
f_ext = biorbd.VecBiorbdSpatialVector([
biorbd.SpatialVector(sv1),
biorbd.SpatialVector(sv2),
])
if biorbd.currentLinearAlgebraBackend() == 1:
q_sym = MX.sym("q", m.nbQ(), 1)
qdot_sym = MX.sym("qdot", m.nbQdot(), 1)
tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1)
ForwardDynamics = Function(
"ForwardDynamics",
[q_sym, qdot_sym, tau_sym],
[m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()],
["q_sym", "qdot_sym", "tau_sym"],
["qddot"],
).expand()
qddot = ForwardDynamics(q, qdot, tau)
qddot = np.array(qddot)[:, 0]
elif biorbd.currentLinearAlgebraBackend() == 0:
# if Eigen backend is used
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
qddot_expected = np.array([
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def custom_dynamic(states, controls, parameters, nlp):
q, qdot, tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
force_vector = cas.MX.zeros(6)
force_vector[5] = 100 * q[0]**2
f_ext = biorbd.VecBiorbdSpatialVector()
f_ext.append(biorbd.SpatialVector(force_vector))
qddot = nlp.model.ForwardDynamics(q, qdot, tau, f_ext).to_mx()
return qdot, qddot
def test_forward_dynamics():
m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod")
q = np.array([i * 1.1 for i in range(m.nbQ())])
qdot = np.array([i * 1.1 for i in range(m.nbQ())])
tau = np.array([i * 1.1 for i in range(m.nbQ())])
qddot = m.ForwardDynamics(q, qdot, tau).to_array()
qddot_expected = np.array([
20.554883896960259,
-22.317642013324736,
-77.406439058256126,
17.382961188212313,
-63.426361095191858,
93.816468824985876,
106.46105024484631,
95.116641811710167,
-268.1961283528546,
2680.3632159799949,
-183.4582596257801,
755.89411812405604,
163.60239754283589,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
# With external forces
f_ext = biorbd.VecBiorbdSpatialVector([
biorbd.SpatialVector(11.1, 22.2, 33.3, 44.4, 55.5, 66.6),
biorbd.SpatialVector(11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2,
66.6 * 2),
])
qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array()
qddot_expected = np.array([
8.8871711208009998,
-13.647827029817943,
-33.606145294752132,
16.922669487341341,
-21.882821189868423,
41.15364990805439,
68.892537246574463,
-324.59756885799197,
-447.99217990207387,
18884.241415786601,
-331.24622725851572,
1364.7620674666462,
3948.4748602722384,
])
np.testing.assert_almost_equal(qddot, qddot_expected)
def convert_array_to_external_forces(all_f_ext: Union[list, tuple]) -> list:
"""
Convert external forces np.ndarray lists of external forces to values understood by biorbd
Parameters
----------
all_f_ext: Union[list, tuple]
The external forces that acts on the model (the size of the matrix should be
6 x number of external forces x number of shooting nodes OR 6 x number of shooting nodes)
Returns
-------
The same forces in a biorbd-friendly format
"""
if not isinstance(all_f_ext, (list, tuple)):
raise RuntimeError(
"f_ext should be a list of (6 x n_external_forces x n_shooting) or (6 x n_shooting) matrix"
)
sv_over_all_phases = []
for f_ext in all_f_ext:
f_ext = np.array(f_ext)
if len(f_ext.shape) < 2 or len(f_ext.shape) > 3:
raise RuntimeError(
"f_ext should be a list of (6 x n_external_forces x n_shooting) or (6 x n_shooting) matrix"
)
if len(f_ext.shape) == 2:
f_ext = f_ext[:, :, np.newaxis]
if f_ext.shape[0] != 6:
raise RuntimeError(
"f_ext should be a list of (6 x n_external_forces x n_shooting) or (6 x n_shooting) matrix"
)
sv_over_phase = []
for node in range(f_ext.shape[2]):
sv = biorbd.VecBiorbdSpatialVector()
for idx in range(f_ext.shape[1]):
sv.append(biorbd.SpatialVector(MX(f_ext[:, idx, node])))
sv_over_phase.append(sv)
sv_over_all_phases.append(sv_over_phase)
return sv_over_all_phases
def convert_array_to_external_forces(all_f_ext):
"""
Converts type of external forces from numpy array to biorbd.SpatialVector
:param all_f_ext: all external forces (numpy array of size : 6 x number of external forces x number of shooting
nodes or 6 x number of shooting nodes)
:return: sv_over_all_phases -> External phases. (biorbd.SpatialVector)
"""
if not isinstance(all_f_ext, (list, tuple)):
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
sv_over_all_phases = []
for f_ext in all_f_ext:
f_ext = np.array(f_ext)
if len(f_ext.shape) < 2 or len(f_ext.shape) > 3:
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
if len(f_ext.shape) == 2:
f_ext = f_ext[:, :, np.newaxis]
if f_ext.shape[0] != 6:
raise RuntimeError(
"f_ext should be a list of (6 x nb_external_forces x nb_shooting) or (6 x nb_shooting) matrix"
)
sv_over_phase = []
for node in range(f_ext.shape[2]):
sv = biorbd.VecBiorbdSpatialVector()
for idx in range(f_ext.shape[1]):
sv.append(biorbd.SpatialVector(MX(f_ext[:, idx, node])))
sv_over_phase.append(sv)
sv_over_all_phases.append(sv_over_phase)
return sv_over_all_phases
