def _get_target_load(self, var: FatigueModel, suffix: str, nlp, controls, index: int):
if self.model_type() not in nlp.controls:
raise NotImplementedError(f"Fatigue dynamics without {self.model_type()} controls is not implemented yet")
val = DynamicsFunctions.get(nlp.controls[f"{self.model_type()}_{suffix}"], controls)[index, :]
if not self.split_controls:
if var.scaling < 0:
val = if_else(lt(val, 0), val, 0)
else:
val = if_else(gt(val, 0), val, 0)
return val / var.scaling
def apply_dynamics(self, target_load, *states):
ma, mr, mf = states
# Implementation of Xia dynamics
c = if_else(
lt(ma, target_load),
if_else(gt(mr, target_load - ma), self.LD * (target_load - ma),
self.LD * mr),
self.LR * (target_load - ma),
)
ma_dot = c - self.F * ma
mr_dot = -c + self.R * mf
mf_dot = self.F * ma - self.R * mf
return vertcat(ma_dot, mr_dot, mf_dot)
def apply_dynamics(self, target_load, *states):
# Implementation of modified Xia dynamics
ma, mr, mf_xia, mf_long = states
c = if_else(
lt(ma, target_load),
if_else(gt(mr, target_load - ma), self.LD * (target_load - ma),
self.LD * mr),
self.LR * (target_load - ma),
)
fatigue_load = target_load - self.effort_threshold
fatigue_dyn = self.effort_factor * if_else(gt(fatigue_load, 0),
1 - mf_long, -mf_long)
ma_dot = c - self.F * ma - if_else(gt(fatigue_load, 0), fatigue_dyn, 0)
mr_dot = -c + self.R * mf_xia - if_else(lt(fatigue_load, 0),
fatigue_dyn, 0)
mf_dot = self.F * ma - self.R * mf_xia
mf_long_dot = fatigue_dyn + self.stabilization_factor * (
1 - ma - mr - mf_xia - mf_long)
return vertcat(ma_dot, mr_dot, mf_dot, mf_long_dot)
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
def if_greater(a, b, if_result, else_result):
return ca.if_else(ca.gt(a, b), if_result, else_result)