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 = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
tau = DynamicsFunctions.get(nlp.controls["tau"], controls)
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 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