def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
fatigue_type: str,
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
torque_level: int = 0,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
ode_solver: OdeSolver
The ode solver to use
torque_level: int
0 no residual torque, 1 with residual torque, 2 with fatigable residual torque
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
n_muscles = biorbd_model.nbMuscleTotal()
tau_min, tau_max = -10, 10
# Define fatigue parameters for each muscle and residual torque
fatigue_dynamics = FatigueList()
for i in range(n_muscles):
if fatigue_type == "xia":
fatigue_dynamics.add(XiaFatigue(LD=10, LR=10, F=0.01, R=0.002), state_only=False)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudFatigue(
LD=100, LR=100, F=0.005, R=0.005, effort_threshold=0.2, effort_factor=0.001, stabilization_factor=10
),
state_only=True,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(EffortPerception(effort_threshold=0.2, effort_factor=0.001))
else:
raise ValueError("fatigue_type not implemented")
if torque_level >= 2:
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=10, LR=10, F=5, R=10, scaling=tau_max),
),
state_only=False,
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_min
),
MichaudFatigue(
LD=10, LR=10, F=5, R=10, effort_threshold=0.15, effort_factor=0.07, scaling=tau_max
),
),
state_only=False,
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.15, effort_factor=0.001, scaling=tau_max),
),
state_only=False,
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.MUSCLE_DRIVEN, expand=False, fatigue=fatigue_dynamics, with_torque=torque_level > 0)
# Add objective functions
objective_functions = ObjectiveList()
if torque_level > 0:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=100)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker="target", second_marker="COM_hand", weight=0.01
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_FATIGUE, key="muscles", weight=1000)
# Constraint
constraint = Constraint(
ConstraintFcn.SUPERIMPOSE_MARKERS,
first_marker="target",
second_marker="COM_hand",
node=Node.END,
axes=[Axis.X, Axis.Y],
)
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, 0] = (0.07, 1.4, 0, 0)
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) if torque_level == 1 else Bounds()
u_bounds.concatenate(FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS))
u_init = InitialGuess([0] * n_tau) if torque_level == 1 else InitialGuess()
u_init.concatenate(FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraint,
ode_solver=ode_solver,
use_sx=False,
n_threads=8,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
fatigue_type: str,
split_controls: bool,
use_sx: bool = True,
) -> OptimalControlProgram:
"""
The initialization of an ocp
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
final_time: float
The time in second required to perform the task
n_shooting: int
The number of shooting points to define int the direct multiple shooting program
fatigue_type: str
The type of dynamics to apply ("xia" or "michaud")
split_controls: bool
If the tau should be split into minus and plus or a if_else should be used
use_sx: bool
If the program should be built from SX (True) or MX (False)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", expand=True)
# Fatigue parameters
fatigue_dynamics = FatigueList()
for i in range(n_tau):
if fatigue_type == "xia":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_min),
XiaFatigue(LD=100, LR=100, F=5, R=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "xia_stabilized":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_min),
XiaFatigueStabilized(LD=100, LR=100, F=5, R=10, stabilization_factor=10, scaling=tau_max),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "michaud":
fatigue_dynamics.add(
MichaudTauFatigue(
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_min,
),
MichaudFatigue(
LD=100,
LR=100,
F=0.005,
R=0.005,
effort_threshold=0.2,
effort_factor=0.001,
stabilization_factor=10,
scaling=tau_max,
),
state_only=False,
split_controls=split_controls,
),
)
elif fatigue_type == "effort":
fatigue_dynamics.add(
TauEffortPerception(
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_min),
EffortPerception(effort_threshold=0.2, effort_factor=0.001, scaling=tau_max),
split_controls=split_controls,
)
)
else:
raise ValueError("fatigue_type not implemented")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, fatigue=fatigue_dynamics, expand=True)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
x_bounds.concatenate(FatigueBounds(fatigue_dynamics, fix_first_frame=True))
if fatigue_type != "effort":
x_bounds[[5, 11], 0] = 0 # The rotation dof is passive (fatigue_ma = 0)
if fatigue_type == "xia":
x_bounds[[7, 13], 0] = 1 # The rotation dof is passive (fatigue_mr = 1)
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
x_init.concatenate(FatigueInitialGuess(fatigue_dynamics))
# Define control path constraint
u_bounds = FatigueBounds(fatigue_dynamics, variable_type=VariableType.CONTROLS)
if split_controls:
u_bounds[[1, 3], :] = 0 # The rotation dof is passive
else:
u_bounds[1, :] = 0 # The rotation dof is passive
u_init = FatigueInitialGuess(fatigue_dynamics, variable_type=VariableType.CONTROLS)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
use_sx=use_sx,
)
