def tau_actuator_constraints(ocp, nlp, t, x, u, p, minimal_tau=None):
nq = nlp.mapping["q"].reduce.len
q = [nlp.mapping["q"].expand.map(mx[:nq]) for mx in x]
q_dot = [nlp.mapping["q_dot"].expand.map(mx[nq:]) for mx in x]
min_bound = []
max_bound = []
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q,
nlp.q_dot)
for i in range(len(u)):
bound = func(q[i], q_dot[i])
if minimal_tau:
min_bound.append(nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:,
1])))
max_bound.append(nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:,
0])))
else:
min_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 1]))
max_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 0]))
obj = vertcat(*u)
min_bound = vertcat(*min_bound)
max_bound = vertcat(*max_bound)
return (
vertcat(np.zeros(min_bound.shape),
np.ones(max_bound.shape) * -np.inf),
vertcat(obj + min_bound, obj - max_bound),
vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)),
)
def maximal_tau(nodes: PenaltyNodes, minimal_tau):
nlp = nodes.nlp
nq = nlp.mapping["q"].to_first.len
q = [nlp.mapping["q"].to_second.map(mx[:nq]) for mx in nodes.x]
qdot = [nlp.mapping["qdot"].to_second.map(mx[nq:]) for mx in nodes.x]
min_bound = []
max_bound = []
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q,
nlp.qdot)
for n in range(len(nodes.u)):
bound = func(q[n], qdot[n])
min_bound.append(nlp.mapping["tau"].to_first.map(
if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:, 1])))
max_bound.append(nlp.mapping["tau"].to_first.map(
if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:, 0])))
obj = vertcat(*nodes.u)
min_bound = vertcat(*min_bound)
max_bound = vertcat(*max_bound)
return (
vertcat(np.zeros(min_bound.shape),
np.ones(max_bound.shape) * -np.inf),
vertcat(obj + min_bound, obj - max_bound),
vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)),
)
def torque_max_from_actuators(
constraint: Constraint,
all_pn: PenaltyNodeList,
min_torque=None,
):
"""
Non linear maximal values of joint torques computed from the torque-position-velocity relationship
Parameters
----------
constraint: Constraint
The actual constraint to declare
all_pn: PenaltyNodeList
The penalty node elements
min_torque: float
Minimum joint torques. This prevent from having too small torques, but introduces an if statement
"""
# TODO: Add index to select the u (control_idx)
nlp = all_pn.nlp
q = [
nlp.variable_mappings["q"].to_second.map(
mx[nlp.states["q"].index, :]) for mx in all_pn.x
]
qdot = [
nlp.variable_mappings["qdot"].to_second.map(
mx[nlp.states["qdot"].index, :]) for mx in all_pn.x
]
if min_torque and min_torque < 0:
raise ValueError(
"min_torque cannot be negative in tau_max_from_actuators")
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax,
nlp.states["q"].mx,
nlp.states["qdot"].mx)
constraint.min_bound = np.repeat([0, -np.inf], nlp.controls.shape)
constraint.max_bound = np.repeat([np.inf, 0], nlp.controls.shape)
for i in range(len(all_pn.u)):
bound = func(q[i], qdot[i])
if min_torque:
min_bound = nlp.variable_mappings["tau"].to_first.map(
if_else(lt(bound[:, 1], min_torque), min_torque,
bound[:, 1]))
max_bound = nlp.variable_mappings["tau"].to_first.map(
if_else(lt(bound[:, 0], min_torque), min_torque,
bound[:, 0]))
else:
min_bound = nlp.variable_mappings["tau"].to_first.map(
bound[:, 1])
max_bound = nlp.variable_mappings["tau"].to_first.map(
bound[:, 0])
ConstraintFunction.add_to_penalty(
all_pn.ocp, all_pn,
vertcat(
*[all_pn.u[i] + min_bound, all_pn.u[i] - max_bound]),
constraint)
def torque_max_from_q_and_qdot(constraint: Constraint,
all_pn: PenaltyNodeList,
min_torque=None):
"""
Non linear maximal values of joint torques computed from the torque-position-velocity relationship
Parameters
----------
constraint: Constraint
The actual constraint to declare
all_pn: PenaltyNodeList
The penalty node elements
min_torque: float
Minimum joint torques. This prevent from having too small torques, but introduces an if statement
"""
nlp = all_pn.nlp
if min_torque and min_torque < 0:
raise ValueError(
"min_torque cannot be negative in tau_max_from_actuators")
bound = nlp.model.torqueMax(nlp.states["q"].mx,
nlp.states["qdot"].mx)
min_bound = BiorbdInterface.mx_to_cx(
"min_bound",
nlp.controls["tau"].mapping.to_first.map(bound[1].to_mx()),
nlp.states["q"],
nlp.states["qdot"],
)
max_bound = BiorbdInterface.mx_to_cx(
"max_bound",
nlp.controls["tau"].mapping.to_first.map(bound[0].to_mx()),
nlp.states["q"],
nlp.states["qdot"],
)
if min_torque:
min_bound = if_else(lt(min_bound, min_torque), min_torque,
min_bound)
max_bound = if_else(lt(max_bound, min_torque), min_torque,
max_bound)
value = vertcat(nlp.controls["tau"].cx + min_bound,
nlp.controls["tau"].cx - max_bound)
n_rows = constraint.rows if constraint.rows else int(
value.shape[0] / 2)
constraint.min_bound = [0] * n_rows + [-np.inf] * n_rows
constraint.max_bound = [np.inf] * n_rows + [0] * n_rows
return value
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 torque_max_from_actuators(constraint,
ocp,
nlp,
t,
x,
u,
p,
min_torque=None):
# TODO: Add index to select the u (control_idx)
nq = nlp.mapping["q"].reduce.len
q = [nlp.mapping["q"].expand.map(mx[:nq]) for mx in x]
q_dot = [nlp.mapping["q_dot"].expand.map(mx[nq:]) for mx in x]
if min_torque and min_torque < 0:
raise ValueError(
"min_torque cannot be negative in tau_max_from_actuators")
func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax,
nlp.q, nlp.q_dot)
constraint.min_bound = np.repeat([0, -np.inf], nlp.nu)
constraint.max_bound = np.repeat([np.inf, 0], nlp.nu)
for i in range(len(u)):
bound = func(q[i], q_dot[i])
if min_torque:
min_bound = nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 1], min_torque), min_torque,
bound[:, 1]))
max_bound = nlp.mapping["tau"].reduce.map(
if_else(lt(bound[:, 0], min_torque), min_torque,
bound[:, 0]))
else:
min_bound = nlp.mapping["tau"].reduce.map(bound[:, 1])
max_bound = nlp.mapping["tau"].reduce.map(bound[:, 0])
ConstraintFunction.add_to_penalty(
ocp, nlp, vertcat(*[u[i] + min_bound, u[i] - max_bound]),
constraint)
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_less(a, b, if_result, else_result):
return ca.if_else(ca.lt(a, b), if_result, else_result)
