def forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact(
states, controls, parameters, nlp):
"""
Returns contact forces computed from forward dynamics with contact force
(forward_dynamics_muscle_excitations_and_torque_driven_with_contact)
:param states: States. (MX.sym from CasADi)
:param controls: Controls. (MX.sym from CasADi)
:param nlp: An OptimalControlProgram class.
:return: Contact forces. (MX.sym from CasADi)
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls[nlp.shape["tau"]:]
muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
tau = muscles_tau + residual_tau
cs = nlp.model.getConstraints()
biorbd.Model.ForwardDynamicsConstraintsDirect(nlp.model, q, qdot, tau,
cs)
return cs.getForce().to_mx()
def forward_dynamics_muscle_excitations_driven(states, controls,
parameters, nlp):
"""
Forward dynamics (q, qdot, qddot -> tau) without external forces driven by muscle excitation (controls).
:param states: States. (MX.sym from CasADi)
:param controls: Controls. (MX.sym from CasADi)
:param nlp: An OptimalControlProgram class.
:return: Vertcat of derived states. (MX.sym from CasADi)
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
nq = nlp.mapping["q"].reduce.len
q = nlp.mapping["q"].expand.map(states[:nq])
qdot = nlp.mapping["q_dot"].expand.map(states[nq:])
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls
muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_activations_dot = nlp.model.activationDot(
muscles_states).to_mx()
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(
nlp.model, q, qdot, muscles_tau).to_mx()
q_dot = nlp.model.computeQdot(q, qdot).to_mx()
qdot_reduced = nlp.mapping["q"].reduce.map(q_dot)
qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot)
return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forward_dynamics_muscle_excitations_and_torque_driven_with_contact(
states, controls, parameters, nlp):
"""
Forward dynamics (q, qdot, qddot -> tau) with contact force driven by muscle excitation and
joint torques (controls).
:param states: States. (MX.sym from CasADi)
:param controls: Controls. (MX.sym from CasADi)
:param nlp: An OptimalControlProgram class.
:return: Vertcat of derived states. (MX.sym from CasADi)
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls[nlp.shape["tau"]:]
muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_activations_dot = nlp.model.activationDot(
muscles_states).to_mx()
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
tau = muscles_tau + residual_tau
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(
nlp.model, q, qdot, tau).to_mx()
q_dot = nlp.model.computeQdot(q, qdot).to_mx()
qdot_reduced = nlp.mapping["q"].reduce.map(q_dot)
qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot)
return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def muscle_forces(q: MX, qdot: MX, a: MX, controls: MX, model: biorbd.Model, use_activation=True):
"""
Compute muscle force
Parameters
----------
q: MX
Symbolic value of joint angle
qdot: MX
Symbolic value of joint velocity
a: MX
Symbolic activation
controls: int
Symbolic value of activations
model: biorbd.Model
biorbd model build with the bioMod
use_activation: bool
True if activation drive False if excitation driven
Returns
-------
List of muscle forces
"""
muscle_states = biorbd.VecBiorbdMuscleState(model.nbMuscles())
for k in range(model.nbMuscles()):
if use_activation:
muscle_states[k].setActivation(controls[k])
else:
muscle_states[k].setActivation(a[k])
muscle_states[k].setExcitation(controls[k])
return model.muscleForces(muscle_states, q, qdot).to_mx()
def forward_dynamics_torque_muscle_driven(states, controls, parameters,
nlp):
"""
Forward dynamics (q, qdot, qddot -> tau) without external forces driven by joint torques and muscles (controls).
:param states: States. (MX.sym from CasADi)
:param controls: Controls. (MX.sym from CasADi)
:param nlp: An OptimalControlProgram class.
:return: Vertcat of derived states. (MX.sym from CasADi)
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"])
muscles_activations = controls[nlp["nbTau"]:]
for k in range(nlp["nbMuscle"]):
muscles_states[k].setActivation(muscles_activations[k])
muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q,
qdot).to_mx()
tau = muscles_tau + residual_tau
qddot = biorbd.Model.ForwardDynamics(nlp["model"], q, qdot,
tau).to_mx()
q_dot = nlp["model"].computeQdot(q, qdot).to_mx()
qdot_reduced = nlp["q_mapping"].reduce.map(q_dot)
qddot_reduced = nlp["q_dot_mapping"].reduce.map(qddot)
return vertcat(qdot_reduced, qddot_reduced)
def muscles_forces(states, controls, nlp, force=None, len=None, tsl=None, pa=None, insx=None, insy=None, insz=None):
nq = nlp["q_mapping"].reduce.len
q = nlp["q_mapping"].expand.map(states[:nq])
qdot = nlp["q_dot_mapping"].expand.map(states[nq:])
biorbd_model = biorbd.Model("arm_Belaise.bioMod")
activations = states[nlp["nbQ"] + nlp["nbQdot"]:]
muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"])
for k in range(nlp["nbMuscle"]):
muscles_states[k].setActivation(activations[k])
muscles_states[k].setExcitation(controls[k])
if force is not None:
biorbd_model.muscle(k).characteristics().setForceIsoMax(force[k])
if len is not None:
biorbd_model.muscle(k).characteristics().setOptimalLength(len[k])
if tsl is not None:
biorbd_model.muscle(k).characteristics().setTendonSlackLength(tsl[k])
if pa is not None:
biorbd_model.muscle(k).characteristics().setPennationAngle(pa[k])
if insx is not None:
biorbd_model.muscle(k).position().setInsertionInLocal(biorbd.Vector3d(insx[k], insy[k], insz[k]))
return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def muscles_forces(q, qdot, act, controls, model):
muscles_states = biorbd.VecBiorbdMuscleState(model.nbMuscles())
for k in range(model.nbMuscles()):
muscles_states[k].setExcitation(controls[k])
muscles_states[k].setActivation(act[k])
# muscles_tau = model.muscularJointTorque(muscles_states, True, q, qdot).to_mx()
muscles_force = model.muscleForces(muscles_states, q, qdot).to_mx()
return muscles_force
def muscles_force(states, controls, nlp):
nq = nlp.mapping['q'].reduce.len
q = nlp.mapping['q'].expand.map(states[:nq])
qdot = nlp.mapping['q_dot'].expand.map(states[nq:])
muscles_states = biorbd.VecBiorbdMuscleState(nlp.model.nbMuscles())
for k in range(nlp.model.nbMuscles()):
muscles_states[k].setActivation(controls[k])
return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def muscles_forces(q, qdot, act, controls, model, use_activation=False):
muscles_states = biorbd.VecBiorbdMuscleState(model.nbMuscles())
for k in range(model.nbMuscles()):
if use_activation:
muscles_states[k].setActivation(controls[k])
else:
muscles_states[k].setExcitation(controls[k])
muscles_states[k].setActivation(act[k])
muscles_force = model.muscleForces(muscles_states, q, qdot).to_mx()
return muscles_force
def muscles_tau(states, controls, nlp):
nq = nlp["q_mapping"].reduce.len
q = nlp["q_mapping"].expand.map(states[:nq])
qdot = nlp["q_dot_mapping"].expand.map(states[nq:])
activations = states[nlp["nbQ"] + nlp["nbQdot"]:]
muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"])
for k in range(nlp["nbMuscle"]):
muscles_states[k].setActivation(activations[k])
muscles_states[k].setExcitation(controls[k])
muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx()
return muscles_tau
def forward_dynamics_muscle_excitations_driven(states: MX.sym,
controls: MX.sym,
parameters: MX.sym,
nlp) -> MX:
"""
Forward dynamics driven by muscle excitations.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
nq = nlp.mapping["q"].to_first.len
q = nlp.mapping["q"].to_second.map(states[:nq])
qdot = nlp.mapping["qdot"].to_second.map(states[nq:])
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls
muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_activations_dot = nlp.model.activationDot(
muscles_states).to_mx()
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(
nlp.model, q, qdot, muscles_tau).to_mx()
qdot = nlp.model.computeQdot(q, qdot).to_mx()
qdot_reduced = nlp.mapping["q"].to_first.map(qdot)
qddot_reduced = nlp.mapping["qdot"].to_first.map(qddot)
return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forward_dynamics_muscle_excitations_and_torque_driven_with_contact(
states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX:
"""
Forward dynamics driven by muscle excitations and joint torques with contact constraints..
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
Returns
----------
MX.sym
The derivative of the states
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls[nlp.shape["tau"]:]
muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_activations_dot = nlp.model.activationDot(
muscles_states).to_mx()
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
tau = muscles_tau + residual_tau
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(
nlp.model, q, qdot, tau).to_mx()
qdot = nlp.model.computeQdot(q, qdot).to_mx()
qdot_reduced = nlp.mapping["q"].to_first.map(qdot)
qddot_reduced = nlp.mapping["qdot"].to_first.map(qddot)
return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact(
states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX:
"""
Contact forces of a forward dynamics driven by muscle excitations and joint torques with contact constraints.
Parameters
----------
states: MX.sym
The state of the system
controls: MX.sym
The controls of the system
parameters: MX.sym
The parameters of the system
nlp: NonLinearProgram
The definition of the system
Returns
----------
MX.sym
The contact forces that ensure no acceleration at these contact points
"""
DynamicsFunctions.apply_parameters(parameters, nlp)
q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data(
states, controls, nlp)
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_excitation = controls[nlp.shape["tau"]:]
muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:]
for k in range(nlp.shape["muscle"]):
muscles_states[k].setExcitation(muscles_excitation[k])
muscles_states[k].setActivation(muscles_activations[k])
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
tau = muscles_tau + residual_tau
cs = nlp.model.getConstraints()
biorbd.Model.ForwardDynamicsConstraintsDirect(nlp.model, q, qdot, tau,
cs)
return cs.getForce().to_mx()
def forward_dynamics_muscle_activations_driven(states, controls,
parameters, nlp):
DynamicsFunctions.apply_parameters(parameters, nlp)
nq = nlp.mapping["q"].reduce.len
q = nlp.mapping["q"].expand.map(states[:nq])
qdot = nlp.mapping["q_dot"].expand.map(states[nq:])
muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"])
muscles_activations = controls
for k in range(nlp.shape["muscle"]):
muscles_states[k].setActivation(muscles_activations[k])
muscles_tau = nlp.model.muscularJointTorque(muscles_states, q,
qdot).to_mx()
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(
nlp.model, q, qdot, muscles_tau).to_mx()
q_dot = nlp.model.computeQdot(q, qdot).to_mx()
qdot_reduced = nlp.mapping["q"].reduce.map(q_dot)
qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot)
return vertcat(qdot_reduced, qddot_reduced)
#
# Please note that this example will work only with the Eigen backend
#
# Load a predefined model
model = biorbd.Model("../arm26.bioMod")
nq = model.nbQ()
nqdot = model.nbQdot()
nmus = model.nbMuscles()
# Choose a position/velocity to compute dynamics from
Q = np.zeros((nq, ))
Qdot = np.zeros((nqdot, ))
# Set all muscles to half of their maximal activation
emg = biorbd.VecBiorbdMuscleState()
for i in range(nmus):
emg.append(biorbd.State(0, 0.5))
# Proceed with the computation of joint torque from the muscles
Tau = model.muscularJointTorque(emg, Q, Qdot)
# Compute the generalized accelerations using the Tau from muscles.
# Please note that in forward dynamics setting, it is usually advised to
# additionnal residual torques. You would add them here to Tau.
Qddot = model.ForwardDynamics(Q, Qdot, Tau)
# Print them to the console
print(Qddot.to_array())
# As an extra, let's print the individual muscle forces
def xia_model_dynamic(states, controls, parameters, nlp):
nbq = nlp["model"].nbQ()
nbqdot = nlp["model"].nbQdot()
nb_q_qdot = nbq + nbqdot
q = states[:nbq]
qdot = states[nbq:nb_q_qdot]
active_fibers = states[nb_q_qdot:nb_q_qdot + nlp["nbMuscle"]]
fatigued_fibers = states[nb_q_qdot + nlp["nbMuscle"]:nb_q_qdot +
2 * nlp["nbMuscle"]]
resting_fibers = states[nb_q_qdot + 2 * nlp["nbMuscle"]:]
residual_tau = controls[:nlp["nbTau"]]
activation = controls[nlp["nbTau"]:]
command = MX()
comp = 0
for i in range(nlp["model"].nbMuscleGroups()):
for k in range(nlp["model"].muscleGroup(i).nbMuscles()):
develop_factor = (
nlp["model"].muscleGroup(i).muscle(k).characteristics(
).fatigueParameters().developFactor().to_mx())
recovery_factor = (
nlp["model"].muscleGroup(i).muscle(k).characteristics(
).fatigueParameters().recoveryFactor().to_mx())
command = vertcat(
command,
if_else(
lt(active_fibers[comp], activation[comp]),
(if_else(
gt(resting_fibers[comp],
activation[comp] - active_fibers[comp]),
develop_factor *
(activation[comp] - active_fibers[comp]),
develop_factor * resting_fibers[comp],
)),
recovery_factor * (activation[comp] - active_fibers[comp]),
),
)
comp += 1
restingdot = -command + Muscles.r * Muscles.R * fatigued_fibers # todo r=r when activation=0
activatedot = command - Muscles.F * active_fibers
fatiguedot = Muscles.F * active_fibers - Muscles.R * fatigued_fibers
muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"])
for k in range(nlp["nbMuscle"]):
muscles_states[k].setActivation(active_fibers[k])
# todo fix force max
muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q,
qdot).to_mx()
# todo get muscle forces and multiply them by activate [k] and same as muscularJointTorque
tau = muscles_tau + residual_tau
dxdt = MX(nlp["nx"], nlp["ns"])
if "external_forces" in nlp:
for i, f_ext in enumerate(nlp["external_forces"]):
qddot = biorbd.Model.ForwardDynamics(nlp["model"], q, qdot, tau,
f_ext).to_mx()
dxdt[:, i] = vertcat(qdot, qddot, activatedot, fatiguedot,
restingdot)
else:
qddot = biorbd.Model.ForwardDynamics(nlp["model"], q, qdot,
tau).to_mx()
dxdt = vertcat(qdot, qddot, activatedot, fatiguedot, restingdot)
return dxdt
