def prepare_ocp(phase_time_constraint, use_parameter):
# --- Inputs --- #
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
ns = (20, 30, 20)
biorbd_model_path = TestUtils.bioptim_folder(
) + "/examples/optimal_time_ocp/cube.bioMod"
ode_solver = OdeSolver.RK4()
# --- Options --- #
n_phases = len(ns)
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(
ConstraintFcn.TIME_CONSTRAINT,
node=Node.END,
minimum=time_min[0],
maximum=time_max[0],
phase=phase_time_constraint,
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
parameters = ParameterList()
if use_parameter:
def my_target_function(ocp, value, target_value):
return value - target_value
def my_parameter_function(biorbd_model, value, extra_value):
biorbd_model.setGravity(biorbd.Vector3d(0, 0, 2))
min_g = -10
max_g = -6
target_g = -8
bound_gravity = Bounds(min_g,
max_g,
interpolation=InterpolationType.CONSTANT)
initial_gravity = InitialGuess((min_g + max_g) / 2)
parameter_objective_functions = Objective(
my_target_function,
weight=10,
quadratic=True,
custom_type=ObjectiveFcn.Parameter,
target_value=target_g)
parameters.add(
"gravity_z",
my_parameter_function,
initial_gravity,
bound_gravity,
size=1,
penalty_list=parameter_objective_functions,
extra_value=1,
)
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
ns,
final_time[:n_phases],
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
parameters=parameters,
)
ode_solver=ode_solver,
use_SX=use_SX,
)
if __name__ == "__main__":
model_path = "cube.bioMod"
ocp = prepare_ocp(biorbd_model_path=model_path, use_SX=True)
# --- Solve the program --- #
sol = ocp.solve(solver=Solver.ACADOS, show_online_optim=False)
result = ShowResult(ocp, sol)
result.graphs()
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_STATE,
weight=1,
states_idx=[0, 1],
target=np.array([[1.0, 2.0]]).T)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE,
weight=10000,
states_idx=[2],
target=np.array([[3.0]]))
objective_functions.add(
Objective.Lagrange.MINIMIZE_TORQUE,
weight=10,
)
ocp.update_objectives(objective_functions)
solver_options = {"nlp_solver_tol_stat": 1e-2}
sol = ocp.solve(solver=Solver.ACADOS,
show_online_optim=False,
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
excitations_ref: np.ndarray,
q_ref: np.ndarray,
use_residual_torque: bool,
kin_data_to_track: str = "markers",
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> OptimalControlProgram:
"""
Prepare the ocp to solve
Parameters
----------
biorbd_model: biorbd.Model
The loaded biorbd model
final_time: float
The time at final node
n_shooting: int
The number of shooting points
markers_ref: np.ndarray
The marker to track if 'markers' is chosen in kin_data_to_track
excitations_ref: np.ndarray
The muscle excitation (EMG) to track
q_ref: np.ndarray
The state to track if 'q' is chosen in kin_data_to_track
kin_data_to_track: str
The type of kin data to track ('markers' or 'q')
use_residual_torque: bool
If residual torque are present or not in the dynamics
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to solve
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="muscles",
target=excitations_ref)
if use_residual_torque:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau")
if kin_data_to_track == "markers":
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
node=Node.ALL,
weight=100,
target=markers_ref)
elif kin_data_to_track == "q":
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
key="q",
weight=100,
node=Node.ALL,
target=q_ref,
index=range(biorbd_model.nbQ()),
)
else:
raise RuntimeError("Wrong choice of kin_data_to_track")
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_excitations=True,
with_torque=use_residual_torque)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger
x_bounds[0].min[[0, 1], :] = -2 * np.pi
x_bounds[0].max[[0, 1], :] = 2 * np.pi
# Add muscle to the bounds
activation_min, activation_max, activation_init = 0, 1, 0.5
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
x_bounds[0][(biorbd_model.nbQ() + biorbd_model.nbQdot()):,
0] = excitations_ref[:, 0]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) +
[0] * biorbd_model.nbMuscles())
# Define control path constraint
excitation_min, excitation_max, excitation_init = 0, 1, 0.5
u_bounds = BoundsList()
u_init = InitialGuessList()
if use_residual_torque:
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[excitation_min] * biorbd_model.nbMuscles(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[excitation_max] * biorbd_model.nbMuscles(),
)
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[excitation_init] * biorbd_model.nbMuscles())
else:
u_bounds.add([excitation_min] * biorbd_model.nbMuscles(),
[excitation_max] * biorbd_model.nbMuscles())
u_init.add([excitation_init] * biorbd_model.nbMuscles())
# ------------- #
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: str = "models/cubeSym.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
tau_min, tau_max, tau_init = -100, 100, 0
dof_mappings = BiMappingList()
dof_mappings.add("q", [0, 1, None, 2, 2], [0, 1, 3], 4)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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 constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model, dof_mappings[0]))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * len(dof_mappings["q"].to_first) * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(dof_mappings["q"].to_first),
[tau_max] * len(dof_mappings["q"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mappings["q"].to_first))
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
variable_mappings=dof_mappings,
)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
x_warm=None,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -50, 50, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=10)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
objective_functions.add(Objective.Mayer.ALIGN_MARKERS,
weight=100000,
first_marker_idx=0,
second_marker_idx=1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuessOption([1.57] * biorbd_model.nbQ() +
[0] * biorbd_model.nbQdot())
else:
x_init = InitialGuessOption(x_warm,
interpolation=InterpolationType.EACH_FRAME)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads,
)
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, actuator_type=None, ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
if actuator_type and actuator_type == 1:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
if actuator_type:
if actuator_type == 1:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN)
elif actuator_type == 2:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
else:
raise ValueError("actuator_type is 1 (torque activations) or 2 (torque max constraints)")
else:
dynamics.add(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)
if actuator_type == 2:
constraints.add(ConstraintFcn.TORQUE_MAX_FROM_ACTUATORS, node=Node.ALL, min_torque=7.5)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
x_bounds[0][2, [0, -1]] = [0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([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,
constraints,
ode_solver=ode_solver,
)
# Set initial state
ocp.nlp[0].x_bounds.min[:, 0] = x0
ocp.nlp[0].x_bounds.max[:, 0] = x0
# Set initial guess
x_init = InitialGuessOption(x0, interpolation=InterpolationType.CONSTANT)
u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT)
u_init = InitialGuessOption(u0, interpolation=InterpolationType.CONSTANT)
ocp.update_initial_guess(x_init, u_init)
# Set objectives functions
objectives = ObjectiveList()
if use_activation:
if TRACK_EMG:
objectives.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=100000,
target=muscles_target_real[:, :-1])
else:
objectives.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=100000)
else:
if TRACK_EMG:
objectives.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL,
weight=100000,
target=muscles_target_real[:, :-1])
else:
objectives.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=100000)
objectives.add(Objective.Lagrange.TRACK_MARKERS,
weight=100000000,
target=markers_target[:, :, :])
def prepare_ocp(
biorbd_model_path,
number_shooting_points,
final_time,
max_torque,
X0,
U0,
target=None,
interpolation=InterpolationType.EACH_FRAME,
):
# --- 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 = -max_torque, max_torque, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
weight=1000,
target=target)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
target=X0)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0].min[:, 0] = -np.inf
x_bounds[0].max[:, 0] = np.inf
# x_bounds[0].min[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# x_bounds[0].max[:biorbd_model.nbQ(), 0] = X0[:biorbd_model.nbQ(),0]
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min, 0.0], [tau_max, 0.0]])
# Initial guesses
x = X0
u = U0
x_init = InitialGuessList()
x_init.add(x, interpolation=interpolation)
u_init = InitialGuessList()
u_init.add(u, interpolation=interpolation)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=4,
use_SX=True,
)
def prepare_nlp(biorbd_model_path="../models/Bras.bioMod"):
"""
Mix .bioMod and users data to call OptimalControlProgram constructor.
:param biorbd_model_path: path to the .bioMod file.
:param show_online_optim: bool which active live plot function.
:return: OptimalControlProgram object.
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
muscle_activated_init, muscle_fatigued_init, muscle_resting_init = 0, 0, 1
torque_min, torque_max, torque_init = -10, 10, 0
muscle_states_ratio_min, muscle_states_ratio_max = 0, 1
number_shooting_points = 30
final_time = 0.5
# --- ObjectiveFcn --- #
objective_functions = ObjectiveList()
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE,
weight=100)
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, controls_idx=[0, 1, 2, 3], weight=2000)
# --- Dynamics --- #
dynamics = DynamicsTypeOption(xia.xia_model_configuration,
dynamic_function=xia.xia_model_dynamic)
# --- Path constraints --- #
X_bounds = QAndQDotBounds(biorbd_model)
X_bounds[biorbd_model.nbQ():, 0] = 0
X_bounds[biorbd_model.nbQ():, 2] = -1.5
muscle_states_bounds = Bounds(
[muscle_states_ratio_min] * biorbd_model.nbMuscleTotal() * 3,
[muscle_states_ratio_max] * biorbd_model.nbMuscleTotal() * 3,
)
muscle_states_bounds.min[:, 0] = (
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal())
muscle_states_bounds.max[:, 0] = (
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal())
X_bounds.bounds.concatenate(muscle_states_bounds.bounds)
U_bounds = Bounds(
[torque_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_states_ratio_min] * biorbd_model.nbMuscleTotal(),
[torque_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_states_ratio_max] * biorbd_model.nbMuscleTotal(),
)
# --- Initial guess --- #
X_init = InitialConditionsOption(
[0] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot(),
InterpolationType.CONSTANT,
)
U_init = InitialConditionsOption(
[torque_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_activated_init] * biorbd_model.nbMuscleTotal(),
InterpolationType.CONSTANT,
)
muscle_states_init = InitialConditionsOption(
[muscle_activated_init] * biorbd_model.nbMuscleTotal() +
[muscle_fatigued_init] * biorbd_model.nbMuscleTotal() +
[muscle_resting_init] * biorbd_model.nbMuscleTotal(),
InterpolationType.CONSTANT,
)
X_init.initial_condition.concatenate(muscle_states_init.initial_condition)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
X_init,
U_init,
X_bounds,
U_bounds,
objective_functions=objective_functions,
nb_threads=4,
)
def prepare_ocp(
biorbd_model_path: str = "models/cubeSym.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
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_CONTROL,
index=[0, 1, 3],
key="tau",
weight=100)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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")
constraints.add(ConstraintFcn.PROPORTIONAL_CONTROL,
key="tau",
node=Node.ALL_SHOOTING,
first_dof=3,
second_dof=4,
coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][2, :] = 0 # Third dof is set to zero
x_bounds[0][biorbd_model.nbQ():,
[0, -1]] = 0 # Velocity is 0 at start and end
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model.nbQ(),
[tau_max] * biorbd_model.nbQ())
u_bounds[0][2, :] = 0 # Third dof is left uncontrolled
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbQ())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
sol = ocp.solve(solver=Solver.ACADOS, solver_options=options_acados)
data_sol = Data.get_data(ocp, sol)
X0, U0, X_out = warm_start_mhe(data_sol)
X_est[:, 0] = X_out
t0 = time.time()
# Reduce ipopt tol for moving estimation
options_ipopt["max_iter"] = 5
options_ipopt["tol"] = 1e-1
# TODO: The following loop should be move in a MHE module that yields after iteration so the user can change obj
for i in range(1, N - N_mhe):
Y_i = Y_N_[:, :, i:i + N_mhe + 1]
new_objectives = ObjectiveList()
new_objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
weight=1000,
target=Y_i,
idx=0)
new_objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
target=X0,
phase=0,
idx=1)
ocp.update_objectives(new_objectives)
# sol = ocp.solve(solver_options=options_ipopt)
sol = ocp.solve(solver=Solver.ACADOS, solver_options=options_acados)
data_sol = Data.get_data(ocp, sol, concatenate=False)
X0, U0, X_out = warm_start_mhe(data_sol)
X_est[:, i] = X_out
t1 = time.time()
def prepare_ocp(
final_time: list,
time_min: list,
time_max: list,
n_shooting: list,
biorbd_model_path: str = "models/cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the optimal control program. This example can be called as a normal single phase (all list len equals to 1)
or as a three phases (all list len equals to 3)
Parameters
----------
final_time: list
The initial guess for the final time of each phase
time_min: list
The minimal time for each phase
time_max: list
The maximal time for each phase
n_shooting: list
The number of shooting points for each phase
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The multiphase OptimalControlProgram ready to be solved
"""
# --- Options --- #
n_phases = len(n_shooting)
if n_phases != 1 and n_phases != 3:
raise RuntimeError("Number of phases must be 1 to 3")
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path))
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0)
if n_phases == 3:
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0, expand=expand)
if n_phases == 3:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0)
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[0], max_bound=time_max[0], phase=0)
if n_phases == 3:
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1
)
constraints.add(
ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[1], max_bound=time_max[1], phase=1
)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2
)
constraints.add(
ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=time_min[2], max_bound=time_max[2], phase=2
)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0
if n_phases == 3:
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
if n_phases == 3:
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if n_phases == 3:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
if n_phases == 3:
u_bounds.add(
[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()
)
u_bounds.add(
[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
if n_phases == 3:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# ------------- #
return OptimalControlProgram(
biorbd_model[:n_phases],
dynamics,
n_shooting,
final_time[:n_phases],
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,
x0,
xT,
use_SX=False,
nb_threads=1):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nbQ = biorbd_model.nbQ()
tau_min, tau_max, tau_init = -10, 10, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10000,
states_idx=np.array(range(0, nbQ)))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10000,
states_idx=np.array(range(nbQ, nbQ * 2)))
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=100)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE,
weight=1000000,
target=np.tile(xT,
(number_shooting_points + 1, 1)).T,
states_idx=np.array(range(0, nbQ)))
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = x0
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
])
# Initial guesses
x_init = InitialGuessOption(np.tile(x0, (number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_SX=use_SX,
nb_threads=nb_threads,
)
def test_acados_several_parameter():
if platform == "win32":
print("Test for ACADOS on Windows is skipped")
return
bioptim_folder = TestUtils.bioptim_folder()
parameters = TestUtils.load_module(
bioptim_folder + "/examples/getting_started/custom_parameters.py")
ocp = parameters.prepare_ocp(
biorbd_model_path=bioptim_folder +
"/examples/getting_started/pendulum.bioMod",
final_time=2,
n_shooting=100,
optim_gravity=True,
optim_mass=True,
min_g=np.array([-1, -1, -10]),
max_g=np.array([1, 1, -5]),
min_m=10,
max_m=30,
target_g=np.array([0, 0, -9.81]),
target_m=20,
use_sx=True,
)
model = ocp.nlp[0].model
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=[0, 1],
target=np.array([[0, 3.14]]).T,
weight=100000)
objectives.add(ObjectiveFcn.Mayer.TRACK_STATE,
index=[2, 3],
target=np.array([[0, 0]]).T,
weight=100)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, index=1, weight=10)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=[2, 3],
weight=0.000000010)
ocp.update_objectives(objectives)
# Path constraint
x_bounds = QAndQDotBounds(model)
x_bounds[[0, 1, 2, 3], 0] = 0
u_bounds = Bounds([-300] * model.nbQ(), [300] * model.nbQ())
ocp.update_bounds(x_bounds, u_bounds)
sol = ocp.solve(solver=Solver.ACADOS,
solver_options={"nlp_solver_tol_eq": 1e-3})
# Check some of the results
q, qdot, tau, gravity, mass = (
sol.states["q"],
sol.states["qdot"],
sol.controls["tau"],
sol.parameters["gravity_xyz"],
sol.parameters["mass"],
)
# initial and final position
np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14)), decimal=6)
# initial and final velocities
np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)), decimal=6)
np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)), decimal=6)
# parameters
np.testing.assert_almost_equal(gravity[-1, :],
np.array([-9.81]),
decimal=6)
np.testing.assert_almost_equal(mass, np.array([[20]]), decimal=6)
# Clean test folder
os.remove(f"./acados_ocp.json")
shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
n_shooting: int
The number of shooting points
final_time: float
The time at the final node
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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
marker_index=1,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
node=Node.ALL_SHOOTING,
weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Define control path constraint
n_tau = biorbd_model.nbGeneralizedTorque(
) # biorbd_model.nbGeneralizedTorque()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
# Initial guesses
# TODO put this in a function defined before and explain what it does, and what are the variables
x = np.vstack((np.zeros(
(biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2))))
Arm_init_D = np.zeros((3, 2))
Arm_init_D[1, 0] = 0
Arm_init_D[1, 1] = -np.pi + 0.01
Arm_init_G = np.zeros((3, 2))
Arm_init_G[1, 0] = 0
Arm_init_G[1, 1] = np.pi - 0.01
for i in range(2):
Arm_Quat_D = eul2quat(Arm_init_D[:, i])
Arm_Quat_G = eul2quat(Arm_init_G[:, i])
x[6:9, i] = np.reshape(Arm_Quat_D[1:], 3)
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = np.reshape(Arm_Quat_G[1:], 3)
x[13, i] = Arm_Quat_G[0]
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].min[:biorbd_model.nbQ(), 0] = x[:biorbd_model.nbQ(), 0]
x_bounds[0].max[:biorbd_model.nbQ(), 0] = x[:biorbd_model.nbQ(), 0]
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
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: str = "cube_with_forces.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Problem parameters
n_shooting = 30
final_time = 2
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# 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")
# External forces. external_forces is of len 1 because there is only one phase.
# The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node
external_forces = [
np.repeat(np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0,
5]]).T[:, :, np.newaxis],
n_shooting,
axis=2)
]
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque())
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
constraints=constraints,
external_forces=external_forces,
ode_solver=ode_solver,
)
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update objectives functions
if use_activation:
w_marker = 100000000
w_control = 100000
else:
w_marker = 100000000
w_control = 100000
objectives = ObjectiveList()
if TRACK_EMG:
w_torque = 10
objectives.add(
Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=w_control,
target=muscles_target[:, 0:Ns_mhe *
rt_ratio:rt_ratio],
)
if use_torque:
objectives.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=w_torque)
else:
w_control = 10000
w_torque = 10
objectives.add(
Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=w_control)
if use_torque:
objectives.add(Objective.Lagrange.MINIMIZE_TORQUE,
weight=500)
def prepare_ocp(biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK):
# --- Options --- #
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
number_shooting_points = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=3)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
constraints.add(ConstraintFcn.ALIGN_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -1] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
"""
By default, all state transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3)
are continuous. In the event that one (or more) state transition(s) is desired to be discontinuous,
as for example IMPACT or CUSTOM can be used as below.
"phase_pre_idx" corresponds to the index of the phase preceding the transition.
IMPACT will cause an impact related discontinuity when defining one or more contact points in the model.
CUSTOM will allow to call the custom function previously presented in order to have its own state transition.
Finally, if you want a state transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated StateTransitionFcn.Cyclic or use a continuous set at the lase phase_pre_idx.
If for some reason, you don't want the state transition to be hard constraint, you can specify a weight higher than
zero. It will thereafter be treated as a Mayer objective function with the specified weight.
"""
state_transitions = StateTransitionList()
state_transitions.add(StateTransitionFcn.IMPACT, phase_pre_idx=1)
state_transitions.add(custom_state_transition,
phase_pre_idx=2,
idx_1=1,
idx_2=3)
state_transitions.add(StateTransitionFcn.CYCLIC)
# ------------- #
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,
state_transitions=state_transitions,
)
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound,
max_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_torque=True,
with_contact=True)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=1,
)
constraints.add(
ConstraintFcn.TRACK_CONTACT_FORCES,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL_SHOOTING,
contact_index=2,
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * n_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * len(dof_mapping["tau"].to_first) +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * len(dof_mapping["tau"].to_first) +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first) +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints=constraints,
variable_mappings=dof_mapping,
)
def generate_exc_low_bounds(rate, model_path, X_est, low_exc):
# Variable of the problem
biorbd_model = biorbd.Model(model_path)
q_ref = X_est[: biorbd_model.nbQ(), :]
dq_ref = X_est[biorbd_model.nbQ() : biorbd_model.nbQ() * 2, :]
Ns = q_ref.shape[1] - 1
T = Ns / rate
# Set lower bounds for excitations on muscles in co-contraction (here triceps). Depends on co-contraction level
excitations_max = [1] * biorbd_model.nbMuscleTotal()
excitations_init = []
excitations_min = []
for i in range(len(low_exc)):
excitations_init.append([[0.05] * 6 + [low_exc[i]] * 3 + [0.05] * 10])
excitations_min.append([[0] * 6 + [low_exc[i]] * 3 + [0] * 10])
x0 = np.hstack([q_ref[:, 0], dq_ref[:, 0]])
# Build the graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, x0=x0, number_shooting_points=Ns, use_sx=True)
X_est_co = np.ndarray((len(excitations_init), biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles(), X_est.shape[1]))
U_est_co = np.ndarray((len(excitations_init), biorbd_model.nbMuscles(), Ns + 1))
# Solve for all co-contraction levels
for co in range(len(excitations_init)):
# Update excitations and to correspond to the co-contraction level
u_i = excitations_init[co]
u_mi = excitations_min[co]
u_ma = excitations_max
# Update initial guess
u_init = InitialGuess(np.tile(u_i, (ocp.nlp[0].ns, 1)).T, interpolation=InterpolationType.EACH_FRAME)
x_init = InitialGuess(
np.tile(X_est[:, 0], (ocp.nlp[0].ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME
)
ocp.update_initial_guess(x_init, u_init=u_init)
# Update bounds
u_bounds = BoundsList()
u_bounds.add(u_mi[0], u_ma, interpolation=InterpolationType.CONSTANT)
ocp.update_bounds(u_bounds=u_bounds)
# Update objectives functions
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1000)
objectives.add(
ObjectiveFcn.Lagrange.TRACK_STATE,
weight=100000,
target=X_est[: biorbd_model.nbQ(), :],
index=range(biorbd_model.nbQ()),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles()),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1000, index=range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)
)
ocp.update_objectives(objectives)
# Solve the OCP
tic = time()
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 50,
"nlp_solver_tol_comp": 1e-5,
"nlp_solver_tol_eq": 1e-5,
"nlp_solver_tol_stat": 1e-5,
},
)
print(f"Time to solve with ACADOS : {time() - tic} s")
toc = time() - tic
print(f"Total time to solve with ACADOS : {toc} s")
# Get optimal solution
data_est = Data.get_data(ocp, sol)
X_est_co[co, :, :] = np.vstack([data_est[0]["q"], data_est[0]["qdot"], data_est[0]["muscles"]])
U_est_co[co, :, :] = data_est[1]["muscles"]
return X_est_co, U_est_co
def prepare_ocp(model_path, phase_time, ns, time_min, time_max):
# --- Options --- #
# Model path
biorbd_model = [biorbd.Model(elt) for elt in model_path]
nb_phases = len(biorbd_model)
q_mapping = BidirectionalMapping(
Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]),
Mapping([0, 1, 2, 4, 7, 8, 9]))
q_mapping = q_mapping
tau_mapping = BidirectionalMapping(
Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]),
Mapping([4, 7, 8, 9]))
tau_mapping = tau_mapping
nq = len(q_mapping.reduce.map_idx)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-100,
phase=0)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# --- Constraints --- #
constraints = ConstraintList()
# Positivity constraints of the normal component of the reaction forces
contact_axes = (1, 2, 4, 5)
for i in contact_axes:
constraints.add(Constraint.CONTACT_FORCE,
phase=0,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
# Non-slipping constraints
# N.B.: Application on only one of the two feet is sufficient, as the slippage cannot occurs on only one foot.
constraints.add(
Constraint.NON_SLIPPING,
phase=0,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
# Custom constraints for positivity of CoM_dot on z axis just before the take-off
constraints.add(utils.com_dot_z,
phase=0,
node=Node.END,
min_bound=0,
max_bound=np.inf)
# Constraint arm positivity
constraints.add(Constraint.TRACK_STATE,
phase=0,
node=Node.END,
index=3,
min_bound=1.0,
max_bound=np.inf)
# Torque constraint + minimize_state
for i in range(nb_phases):
constraints.add(utils.tau_actuator_constraints,
phase=i,
node=Node.ALL,
minimal_tau=20)
objective_functions.add(Objective.Mayer.MINIMIZE_TIME,
weight=0.0001,
phase=i,
min_bound=time_min[i],
max_bound=time_max[i])
# Path constraint
nb_q = q_mapping.reduce.len
nb_q_dot = nb_q
pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47]
# Initialize x_bounds (Interpolation type is CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT)
x_bounds = BoundsList()
for i in range(nb_phases):
x_bounds.add(
QAndQDotBounds(biorbd_model[i], all_generalized_mapping=q_mapping))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_q_dot
# Initial guess for states (Interpolation type is CONSTANT)
x_init = InitialGuessList()
for i in range(nb_phases):
x_init.add(pose_at_first_node + [0] * nb_q_dot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[-500] * tau_mapping.reduce.len,
[500] * tau_mapping.reduce.len])
# Define initial guess for controls
u_init = InitialGuessList()
u_init.add([0] * tau_mapping.reduce.len)
# ------------- #
ocp = OptimalControlProgram(
biorbd_model,
dynamics,
ns,
phase_time,
x_init=x_init,
x_bounds=x_bounds,
u_init=u_init,
u_bounds=u_bounds,
objective_functions=objective_functions,
constraints=constraints,
q_mapping=q_mapping,
q_dot_mapping=q_mapping,
tau_mapping=tau_mapping,
nb_threads=4,
use_SX=False,
)
return utils.add_custom_plots(ocp, nb_phases, x_bounds, nq, minimal_tau=20)
def generate_final_data(rate, X_ref, U_ref, save_data, plot):
# Variable of the problem
biorbd_model = biorbd.Model("arm_wt_rot_scap.bioMod")
Ns = X_ref.shape[2] - 1
T = int(Ns / rate)
x0 = X_ref[0, : -biorbd_model.nbMuscles(), 0]
# Build the graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, number_shooting_points=Ns, x0=x0, use_SX=True)
X_ref_fin = X_ref
U_ref_fin = U_ref
q_init = X_ref[0, : biorbd_model.nbQ(), :]
# Run problem for all co-contraction level
for co in range(X_ref.shape[0]):
# Get reference data for co-contraction level ranging from 1 to 3
q_ref = X_ref[co, : biorbd_model.nbQ(), :]
dq_ref = X_ref[co, biorbd_model.nbQ() : biorbd_model.nbQ() * 2, :]
u_ref = U_ref[co, :, :]
x_ref_0 = np.hstack([q_ref[:, 0], dq_ref[:, 0], [0.3] * biorbd_model.nbMuscles()])
# Update initial guess
x_init = InitialGuess(np.tile(x_ref_0, (Ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
u_init = InitialGuess(u_ref[:, :-1], interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init)
# Update bounds on state
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0].concatenate(Bounds([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles()))
x_bounds[0].min[: biorbd_model.nbQ() * 2, 0] = x_ref_0[: biorbd_model.nbQ() * 2]
x_bounds[0].max[: biorbd_model.nbQ() * 2, 0] = x_ref_0[: biorbd_model.nbQ() * 2]
x_bounds[0].min[biorbd_model.nbQ() * 2 : biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles(), 0] = [
0.1
] * biorbd_model.nbMuscles()
x_bounds[0].max[biorbd_model.nbQ() * 2 : biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles(), 0] = [
1
] * biorbd_model.nbMuscles()
ocp.update_bounds(x_bounds=x_bounds)
# Update Objectives
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10, index=np.array(range(biorbd_model.nbQ()))
)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=100,
index=np.array(range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())),
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=100)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL,
weight=10000,
target=u_ref[[9, 10, 17, 18], :-1],
index=np.array([9, 10, 17, 18]),
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_STATE, weight=100000, target=q_init, index=np.array(range(biorbd_model.nbQ()))
)
# Solve OCP
ocp.update_objectives(objective_functions)
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 40,
"nlp_solver_tol_comp": 1e-4,
"nlp_solver_tol_eq": 1e-4,
"nlp_solver_tol_stat": 1e-4,
},
)
# Store optimal solutions
states, controls = Data.get_data(ocp, sol)
X_ref_fin[co, :, :] = np.concatenate((states["q"], states["qdot"], states["muscles"]))
U_ref_fin[co, :, :] = controls["muscles"]
# Save results in .bob file if flag is True
if save_data:
ocp.save_get_data(sol, f"solutions/sim_ac_{int(T * 1000)}ms_{Ns}sn_REACH2_co_level_{co}.bob")
if plot:
t = np.linspace(0, T, Ns + 1)
plt.figure("Muscles controls")
for i in range(biorbd_model.nbMuscles()):
plt.subplot(4, 5, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.step(t, U_ref[k, i, :])
plt.figure("Q")
for i in range(biorbd_model.nbQ()):
plt.subplot(2, 3, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.plot(X_ref[k, i, :])
plt.figure("Q")
plt.figure("dQ")
for i in range(biorbd_model.nbQ()):
plt.subplot(2, 3, i + 1)
for k in range(X_ref_fin.shape[0]):
plt.plot(X_ref[k, i + biorbd_model.nbQ(), :])
plt.show()
return X_ref_fin, U_ref_fin
def main():
model_path = "models/cube.bioMod"
ns = 30
tf = 2
ocp = prepare_ocp(biorbd_model_path=model_path, n_shooting=ns, tf=tf)
# --- Add objective functions --- #
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=1000,
index=[0, 1],
target=np.array([[1.0, 2.0]]).T,
multi_thread=False,
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=10000,
index=[2],
target=np.array([[3.0]]),
multi_thread=False,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=1,
multi_thread=False)
ocp.update_objectives(objective_functions)
# --- Solve the program --- #
solver = Solver.ACADOS()
sol = ocp.solve(solver)
sol.graphs()
objective_functions = ObjectiveList()
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=1,
index=[0, 1],
target=np.array([[1.0, 2.0]]).T,
multi_thread=False,
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
key="q",
weight=10000,
index=[2],
target=np.array([[3.0]]),
multi_thread=False,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10,
multi_thread=False)
ocp.update_objectives(objective_functions)
solver.set_nlp_solver_tol_stat(1e-2)
sol = ocp.solve(solver)
# --- Show results --- #
sol.graphs()
sol.animate()
def generate_state(model_path, T, Ns, nb_phase, x_phase):
# Parameters of the problem
biorbd_model = biorbd.Model(model_path)
X_est = np.zeros((biorbd_model.nbQ() * 2 + biorbd_model.nbMuscleTotal(), nb_phase * Ns + 1))
U_est = np.zeros((biorbd_model.nbMuscleTotal(), nb_phase * Ns))
# Set the joint angles target for each phase
x0 = x_phase[0, :]
# Build graph
ocp = prepare_ocp(biorbd_model=biorbd_model, final_time=T, number_shooting_points=Ns, x0=x0, use_sx=True)
x0 = np.concatenate((x0, np.array([0.2] * biorbd_model.nbMuscles())))
# Solve for each phase
for phase in range(1, nb_phase + 1):
# impose it as first state of next solve
ocp.nlp[0].x_bounds.min[:, 0] = x0
ocp.nlp[0].x_bounds.max[:, 0] = x0
# update initial guess on states
x_init = InitialGuess(np.tile(x0, (ocp.nlp[0].ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init=x_init)
# Update objectives functions
objectives = ObjectiveList()
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10, index=np.array(range(biorbd_model.nbQ())))
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=10,
index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)),
)
objectives.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=10,
index=np.array(range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())),
)
objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10)
objectives.add(
ObjectiveFcn.Mayer.TRACK_STATE,
weight=10000,
target=np.array([x_phase[phase, : biorbd_model.nbQ() * 2]]).T,
index=np.array(range(biorbd_model.nbQ() * 2)),
)
ocp.update_objectives(objectives)
sol = ocp.solve(
solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
# "nlp_solver_max_iter": 50,
"nlp_solver_tol_comp": 1e-7,
"nlp_solver_tol_eq": 1e-7,
"nlp_solver_tol_stat": 1e-7,
},
)
# get last state of previous solve
x_out, u_out, x0 = switch_phase(ocp, sol)
# Store optimal solution
X_est[:, (phase - 1) * Ns : phase * Ns] = x_out
U_est[:, (phase - 1) * Ns : phase * Ns] = u_out
# Collect last state
X_est[:, -1] = x0
return X_est
def prepare_ocp(
biorbd_model_path: str = "models/cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=100,
phase=3)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -1] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
"""
By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3)
are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous,
as for example IMPACT or CUSTOM can be used as below.
"phase_pre_idx" corresponds to the index of the phase preceding the transition.
IMPACT will cause an impact related discontinuity when defining one or more contact points in the model.
CUSTOM will allow to call the custom function previously presented in order to have its own phase transition.
Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx.
If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than
zero. It will thereafter be treated as a Mayer objective function with the specified weight.
"""
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=0,
states_mapping=BiMapping(range(3), range(3)))
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
def prepare_ocp(biorbd_model_path: str = "cube.bioMod",
ode_solver: OdeSolver = OdeSolver.RK4(),
long_optim: bool = False) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The ode solve to use
long_optim: bool
If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points)
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
n_shooting = (100, 300, 100)
else:
n_shooting = (20, 30, 20)
final_time = (2, 5, 4)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=1,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1,
phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=2,
phase=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
for bounds in x_bounds:
for i in [1, 3, 4, 5]:
bounds[i, [0, -1]] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].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,
problem_type_custom=True,
ode_solver=OdeSolver.RK,
use_SX=False):
# --- 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(Objective.Mayer.MINIMIZE_STATE,
weight=1000,
states_idx=[0, 1],
target=np.array([[1.0, 2.0]]).T)
objective_functions.add(Objective.Mayer.MINIMIZE_STATE,
weight=10000,
states_idx=[2],
target=np.array([[3.0]]))
objective_functions.add(
Objective.Lagrange.MINIMIZE_TORQUE,
weight=1,
)
# Dynamics
dynamics = DynamicsTypeList()
if problem_type_custom:
dynamics.add(custom_configure, dynamic_function=custom_dynamic)
else:
dynamics.add(DynamicsType.TORQUE_DRIVEN,
dynamic_function=custom_dynamic)
# Path constraint
x_bounds = BoundsOption(QAndQDotBounds(biorbd_model))
x_bounds[:, 0] = 0
# Initial guess
x_init = InitialGuessOption([0] *
(biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
u_bounds = BoundsOption([[tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque()])
u_init = InitialGuessOption([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,
use_SX=use_SX,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
weight: float,
ode_solver: OdeSolver = OdeSolver.COLLOCATION(),
) -> 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
weight: float
The weight applied to the SUPERIMPOSE_MARKERS final objective function. The bigger this number is, the greater
the model will try to reach the marker. This is in relation with the other objective functions
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 = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau")
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles")
objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS,
first_marker="target",
second_marker="COM_hand",
weight=weight)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = (0.07, 1.4, 0, 0)
# Initial guess
x_init = InitialGuessList()
x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot())
# Define control path constraint
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
tau_min, tau_max, tau_init = -1, 1, 0
u_bounds = BoundsList()
u_bounds.add(
[tau_min] * biorbd_model.nbGeneralizedTorque() +
[muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[muscle_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[muscle_init] * biorbd_model.nbMuscleTotal())
# ------------- #
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: str,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4(),
weight: float = 1,
) -> 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
ode_solver: OdeSolver
The ode solver to use
weight: float
The weighting of the minimize time objective function
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=weight)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, [0, -1]] = 0
x_bounds[0][n_q - 1, -1] = 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([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 = BoundsList()
u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau)
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([tau_init] * n_tau)
# ------------- #
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,
final_time,
number_shooting_points,
nb_threads,
use_SX=False):
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
torque_min, torque_max, torque_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot))
data_to_track[:, 1] = 3.14
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100.0)
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
Objective.Mayer.MINIMIZE_STATE,
weight=50000.0,
target=data_to_track.T,
node=Node.END,
)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (n_q + n_qdot))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[torque_min] * n_tau,
[torque_max] * n_tau,
])
u_bounds[0][n_tau - 1, :] = 0
u_init = InitialGuessList()
u_init.add([torque_init] * n_tau)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
nb_threads=nb_threads,
use_SX=use_SX,
)
