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 custom_dynamic(
states: Union[MX, SX],
controls: Union[MX, SX],
parameters: Union[MX, SX],
nlp: NonLinearProgram,
my_additional_factor=1,
) -> tuple:
"""
The custom dynamics function that provides the derivative of the states: dxdt = f(x, u, p)
Parameters
----------
states: Union[MX, SX]
The state of the system
controls: Union[MX, SX]
The controls of the system
parameters: Union[MX, SX]
The parameters acting on the system
nlp: NonLinearProgram
A reference to the phase
my_additional_factor: int
An example of an extra parameter sent by the user
Returns
-------
The derivative of the states in the tuple[Union[MX, SX]] format
"""
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)
# You can directly call biorbd function (as for ddq) or call bioptim accessor (as for dq)
dq = DynamicsFunctions.compute_qdot(nlp, q, qdot) * my_additional_factor
ddq = nlp.model.ForwardDynamics(q, qdot, tau).to_mx()
return dq, ddq