def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
time_min: float,
time_max: float,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the optimal control program
Parameters
----------
biorbd_model_path: str
The path to the bioMod
final_time: float
The initial guess for the final time
n_shooting: int
The number of shooting points
time_min: float
The minimal time the phase can have
time_max: float
The maximal time the phase can have
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = Constraint(ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
min_bound=time_min,
max_bound=time_max)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g, ode_solver=OdeSolver.RK, use_SX=False
):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -30, 30, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# Define the parameter to optimize
# Give the parameter some min and max bounds
parameters = ParameterList()
bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target=target_g
)
parameters.add(
"gravity_z", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=1, # The number of elements this particular parameter vector has
penalty_list=parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter
extra_value=1, # You can define as many extra arguments as you want
)
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
ode_solver=ode_solver,
use_SX=use_SX,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
interpolation_type: InterpolationType = InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT,
) -> OptimalControlProgram:
"""
Prepare the ocp for the specified interpolation type
Parameters
----------
biorbd_model_path: str
The path to the biorbd model
n_shooting: int
The number of shooting point
final_time: float
The movement time
interpolation_type: InterpolationType
The requested InterpolationType
Returns
-------
The OCP fully prepared and ready to be solved
"""
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1")
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2")
# Path constraints
if interpolation_type == InterpolationType.CONSTANT:
x_min = [-100] * (nq + nqdot)
x_max = [100] * (nq + nqdot)
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT)
u_min = [tau_min] * ntau
u_max = [tau_max] * ntau
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT)
elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x_min = np.random.random((6, 3)) * (-10) - 5
x_max = np.random.random((6, 3)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
u_min = np.random.random((3, 3)) * tau_min + tau_min / 2
u_max = np.random.random((3, 3)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
elif interpolation_type == InterpolationType.LINEAR:
x_min = np.random.random((6, 2)) * (-10) - 5
x_max = np.random.random((6, 2)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR)
u_min = np.random.random((3, 2)) * tau_min + tau_min / 2
u_max = np.random.random((3, 2)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR)
elif interpolation_type == InterpolationType.EACH_FRAME:
x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5
x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.EACH_FRAME)
u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.EACH_FRAME)
elif interpolation_type == InterpolationType.SPLINE:
spline_time = np.hstack((0, np.sort(np.random.random((3,)) * final_time), final_time))
x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5
x_max = np.random.random((nq + nqdot, 5)) * 10 + 5
u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2
u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2
x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.SPLINE, t=spline_time)
u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.SPLINE, t=spline_time)
elif interpolation_type == InterpolationType.CUSTOM:
# The custom functions refer to the ones at the beginning of the file.
# For this particular instance, they emulate a Linear interpolation
extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting}
extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting}
x_bounds = Bounds(
custom_x_bounds_min, custom_x_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_x
)
u_bounds = Bounds(
custom_u_bounds_min, custom_u_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_u
)
else:
raise NotImplementedError("Not implemented yet")
# Initial guess
x_init = InitialGuess([0] * (nq + nqdot))
u_init = InitialGuess([tau_init] * ntau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK4()) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.START,
quadratic=True,
first_marker="m0",
second_marker="m1",
weight=1000,
)
objective_functions.add(
custom_func_track_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.END,
quadratic=True,
first_marker="m0",
second_marker="m2",
weight=1000,
)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
number_shooting_points = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE)
objective_functions.add(
custom_func_align_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.START,
quadratic=True,
first_marker_idx=0,
second_marker_idx=1,
weight=1000,
)
objective_functions.add(
custom_func_align_markers,
custom_type=ObjectiveFcn.Mayer,
node=Node.END,
quadratic=True,
first_marker_idx=0,
second_marker_idx=2,
weight=1000,
)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.IRK()
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(custom_func_track_markers,
node=Node.START,
first_marker="m0",
second_marker="m1",
method=0)
constraints.add(custom_func_track_markers,
node=Node.END,
first_marker="m0",
second_marker="m2",
method=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
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 == "xia_stabilized":
fatigue_dynamics.add(
XiaFatigueStabilized(LD=10, LR=10, F=0.01, R=0.002, stabilization_factor=10), 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 == "xia_stabilized":
fatigue_dynamics.add(
XiaTauFatigue(
XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=10, scaling=tau_min),
XiaFatigueStabilized(LD=10, LR=10, F=5, R=10, stabilization_factor=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_parameters(
biorbd_model_path,
final_time,
n_shooting,
optim_gravity,
optim_mass,
min_g,
max_g,
target_g,
min_m,
max_m,
target_m,
ode_solver=OdeSolver.RK4(),
use_sx=False,
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
optim_gravity: bool
If the gravity should be optimized
optim_mass: bool
If the mass should be optimized
min_g: np.ndarray
The minimal value for the gravity
max_g: np.ndarray
The maximal value for the gravity
target_g: np.ndarray
The target value for the gravity
min_m: float
The minimal value for the mass
max_m: float
The maximal value for the mass
target_m: float
The target value for the mass
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
tau_min, tau_max, tau_init = -300, 300, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
# Define the parameter to optimize
parameters = ParameterList()
if optim_gravity:
# Give the parameter some min and max bounds
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
# and an initial condition
initial_gravity = InitialGuess((min_g + max_g) / 2)
# and an objective function
parameter_objective_functions = Objective(
my_target_function,
weight=1000,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_g)
parameters.add(
"gravity_xyz", # The name of the parameter
my_parameter_function, # The function that modifies the biorbd model
initial_gravity, # The initial guess
bound_gravity, # The bounds
size=
3, # The number of elements this particular parameter vector has
penalty_list=
parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([1, 1, 10.0]),
extra_value=1, # You can define as many extra arguments as you want
)
if optim_mass:
bound_mass = Bounds(min_m,
max_m,
interpolation=InterpolationType.CONSTANT)
initial_mass = InitialGuess((min_m + max_m) / 2)
mass_objective_functions = Objective(
my_target_function,
weight=100,
quadratic=False,
custom_type=ObjectiveFcn.Parameter,
target=target_m)
parameters.add(
"mass", # The name of the parameter
set_mass, # The function that modifies the biorbd model
initial_mass, # The initial guess
bound_mass, # The bounds
size=
1, # The number of elements this particular parameter vector has
penalty_list=
mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter
scaling=np.array([10.0]),
)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
parameters=parameters,
ode_solver=ode_solver,
use_sx=use_sx,
)
Problem.configure_q_qdot(nlp, as_states=True, as_controls=False)
Problem.configure_tau(nlp, as_states=False, as_controls=True)
Problem.configure_forward_dyn_func(ocp, nlp, custom_dynamic)
# Model path
m = biorbd.Model("mass_point.bioMod")
m.setGravity(biorbd.Vector3d(0, 0, 0))
# Add objective functions (high upward velocity at end point)
objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE,
index=1,
weight=-1)
# Dynamics
dynamics = Dynamics(custom_configure, dynamic_function=custom_dynamic)
# Path constraint
x_bounds = QAndQDotBounds(m)
x_bounds[:, 0] = [0] * m.nbQ() + [0] * m.nbQdot()
x_bounds.min[:, 1] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot()
x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot()
# Initial guess
x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot()))
# Define control path constraint
u_bounds = Bounds([-100] * m.nbGeneralizedTorque(),
[0] * m.nbGeneralizedTorque())
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4(),
use_sx: bool = True,
n_threads: int = 1,
) -> 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
ode_solver: OdeSolver = OdeSolver.RK4()
Which type of OdeSolver to use
use_sx: bool
If the SX variable should be used instead of MX (can be extensive on RAM)
n_threads: int
The number of threads to use in the paralleling (1 = no parallel computing)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
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,
ode_solver=ode_solver,
use_sx=use_sx,
n_threads=n_threads,
)
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,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK1(n_integration_steps=1),
use_sx: bool = False,
n_threads: int = 1,
implicit_dynamics: bool = False,
) -> 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
ode_solver: OdeSolver = OdeSolver.RK4()
Which type of OdeSolver to use
use_sx: bool
If the SX variable should be used instead of MX (can be extensive on RAM)
n_threads: int
The number of threads to use in the paralleling (1 = no parallel computing)
implicit_dynamics: bool
implicit
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN,
implicit_dynamics=implicit_dynamics)
# Path constraint
tau_min, tau_max, tau_init = -100, 100, 0
# Be careful to let the accelerations not to much bounded to find the same solution in implicit dynamics
if implicit_dynamics:
qddot_min, qddot_max, qddot_init = -1000, 1000, 0
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_qddot = biorbd_model.nbQddot()
n_tau = biorbd_model.nbGeneralizedTorque()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
# There are extra controls in implicit dynamics which are joint acceleration qddot.
if implicit_dynamics:
u_bounds = Bounds([tau_min] * n_tau + [qddot_min] * n_qddot,
[tau_max] * n_tau + [qddot_max] * n_qddot)
else:
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[1, :] = 0 # Prevent the model from actively rotate
if implicit_dynamics:
u_init = InitialGuess([0] * (n_tau + n_qddot))
else:
u_init = InitialGuess([0] * n_tau)
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,
ode_solver=ode_solver,
use_sx=use_sx,
n_threads=n_threads,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
n_threads: int,
use_sx: bool = False,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
n_threads: int
The number of threads to use while using multithreading
ode_solver: OdeSolver
The type of ode solver used
use_sx: bool
If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve)
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[:, [0, -1]] = 0
x_bounds[1, -1] = 3.14
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_init = InitialGuess([0] * (n_q + n_qdot))
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[n_tau - 1, :] = 0
u_init = InitialGuess([tau_init] * n_tau)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
n_threads=n_threads,
use_sx=use_sx,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
initial_guess=InterpolationType.CONSTANT,
ode_solver=OdeSolver.RK,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
nqdot = biorbd_model.nbQdot()
ntau = biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2)
# Path constraint and control path constraints
x_bounds = QAndQDotBounds(biorbd_model)
x_bounds[1:6, [0, -1]] = 0
x_bounds[2, -1] = 1.57
u_bounds = Bounds([tau_min] * ntau, [tau_max] * ntau)
# Initial guesses
t = None
extra_params_x = {}
extra_params_u = {}
if initial_guess == InterpolationType.CONSTANT:
x = [0] * (nq + nqdot)
u = [tau_init] * ntau
elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0],
[2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81,
-2.28]]).T
elif initial_guess == InterpolationType.LINEAR:
x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T
u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T
elif initial_guess == InterpolationType.EACH_FRAME:
x = np.random.random((nq + nqdot, number_shooting_points + 1))
u = np.random.random((ntau, number_shooting_points))
elif initial_guess == InterpolationType.SPLINE:
# Bound spline assume the first and last point are 0 and final respectively
t = np.hstack((0, np.sort(np.random.random(
(3, )) * final_time), final_time))
x = np.random.random((nq + nqdot, 5))
u = np.random.random((ntau, 5))
elif initial_guess == InterpolationType.CUSTOM:
# The custom function refers to the one at the beginning of the file. It emulates a Linear interpolation
x = custom_init_func
u = custom_init_func
extra_params_x = {
"my_values": np.random.random((nq + nqdot, 2)),
"nb_shooting": number_shooting_points
}
extra_params_u = {
"my_values": np.random.random((ntau, 2)),
"nb_shooting": number_shooting_points
}
else:
raise RuntimeError("Initial guess not implemented yet")
x_init = InitialGuess(x,
t=t,
interpolation=initial_guess,
**extra_params_x)
u_init = InitialGuess(u,
t=t,
interpolation=initial_guess,
**extra_params_u)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
