def prepare_ocp(
biorbd_model,
final_time,
number_shooting_points,
x0,
xT,
use_SX=False,
nb_threads=8,
):
# --- Options --- #
# Model path
biorbd_model = biorbd_model
nbQ = biorbd_model.nbQ()
# tau_min, tau_max, tau_init = -10, 10, 0
activation_min, activation_max, activation_init = 0, 1, 0.1
excitation_min, excitation_max, excitation_init = 0, 1, 0.2
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(range(biorbd_model.nbQ())))
objective_functions.add(Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(
range(biorbd_model.nbQ(),
biorbd_model.nbQ() * 2)))
objective_functions.add(
Objective.Lagrange.MINIMIZE_STATE,
weight=10,
states_idx=np.array(
range(biorbd_model.nbQ() * 2,
biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles())))
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10)
# objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL_DERIVATIVE, weight=100)
objective_functions.add(Objective.Mayer.TRACK_STATE,
weight=100000,
target=np.array([xT[:biorbd_model.nbQ() * 2]]).T,
states_idx=np.array(range(biorbd_model.nbQ() * 2)))
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# add muscle activation bounds
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([
[excitation_min] * biorbd_model.nbMuscleTotal(),
[excitation_max] * biorbd_model.nbMuscleTotal(),
])
# Initial guesses
x_init = InitialGuessOption(np.tile(
np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())),
(number_shooting_points + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
u0 = np.array([excitation_init] * biorbd_model.nbMuscles())
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 prepare_ocp(model_path, phase_time, number_shooting_points, min_bound,
max_bound):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
activation_min, activation_max, activation_init = 0, 1, 0.5
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(
DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
Constraint.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.75, 0.75]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * tau_mapping.reduce.len +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * tau_mapping.reduce.len +
[activation_max] * biorbd_model.nbMuscleTotal(),
])
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.reduce.len +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(
biorbd_model_path,
final_time,
number_shooting_points,
marker_velocity_or_displacement,
marker_in_first_coordinates_system,
control_type,
ode_solver=OdeSolver.RK,
):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
nq = biorbd_model.nbQ()
biorbd_model.markerNames()
# Problem parameters
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
if marker_in_first_coordinates_system:
# Marker should follow this segment (0 velocity when compare to this one)
coordinates_system_idx = 0
else:
# Marker should be static in global reference frame
coordinates_system_idx = -1
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
coordinates_system_idx=coordinates_system_idx,
index=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY,
index=6,
weight=1000)
else:
raise RuntimeError(
"Wrong choice of marker_velocity_or_displacement, actual value is "
"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
# Make sure the segments actually moves (in order to test the relative speed objective)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=6,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=7,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(nq, 2 * nq):
x_bounds[0].min[i, :] = -10
x_bounds[0].max[i, :] = 10
# Initial guess
x_init = InitialGuessList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# 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,
nb_integration_steps=5,
control_type=control_type,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str = "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
all_generalized_mapping = BiMapping([0, 1, 2, -2], [0, 1, 2])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping))
x_bounds[0][3:6, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * all_generalized_mapping.to_first.len * 2)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * all_generalized_mapping.to_first.len, [tau_max] * all_generalized_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * all_generalized_mapping.to_first.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
all_generalized_mapping=all_generalized_mapping,
)
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"):
end_crank_idx = 0
hand_marker_idx = 18
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
# Problem parameters
number_shooting_points = 30
final_time = 1.0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, index=hand_marker_idx)
objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.ALIGN_MARKERS, first_marker_idx=hand_marker_idx, second_marker_idx=end_crank_idx, node=Node.ALL
)
constraints.add(
Constraint.TRACK_STATE,
node=Node.ALL,
index=0,
target=np.linspace(0, 2 * np.pi, number_shooting_points + 1),
)
state_transitions = StateTransitionList()
state_transitions.add(state_transition_function, phase_pre_idx=0)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# Initial guess
x_init = InitialGuessList()
x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [0] * biorbd_model.nbQdot())
# 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,
constraints,
state_transitions=state_transitions,
)
def prepare_ocp(model_path, phase_time, number_shooting_points, mu):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(model_path)
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3]))
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
Constraint.CONTACT_FORCE,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
Constraint.CONTACT_FORCE,
max_bound=np.inf,
node=Node.ALL,
contact_force_idx=2,
)
constraints.add(
Constraint.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=mu,
)
# Path constraint
nb_q = biorbd_model.nbQ()
nb_qdot = nb_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# Initialize x_bounds
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot
# Initial guess
x_init = InitialGuessList()
x_init.add(pose_at_first_node + [0] * nb_qdot)
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([[tau_min] * tau_mapping.reduce.len,
[tau_max] * tau_mapping.reduce.len])
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.reduce.len)
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
number_shooting_points,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
marker_velocity_or_displacement: str,
marker_in_first_coordinates_system: bool,
control_type: ControlType,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time of the final node
n_shooting: int
The number of shooting points
marker_velocity_or_displacement: str
which type of tracking: finite difference ('disp') or by jacobian ('velo')
marker_in_first_coordinates_system: bool
If the marker to track should be expressed in the global or local reference frame
control_type: ControlType
The type of controls
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
if marker_in_first_coordinates_system:
# Marker should follow this segment (0 velocity when compare to this one)
coordinates_system_idx = 0
else:
# Marker should be static in global reference frame
coordinates_system_idx = -1
objective_functions = ObjectiveList()
if marker_velocity_or_displacement == "disp":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT,
coordinates_system_idx=coordinates_system_idx,
index=6,
weight=1000,
)
elif marker_velocity_or_displacement == "velo":
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY,
index=6,
weight=1000)
else:
raise RuntimeError(
f"Wrong choice of marker_velocity_or_displacement, actual value is "
f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.")
# Make sure the segments actually moves (in order to test the relative speed objective)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=6,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
index=7,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
nq = biorbd_model.nbQ()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(nq, 2 * nq):
x_bounds[0].min[i, :] = -10
x_bounds[0].max[i, :] = 10
# Initial guess
x_init = InitialGuessList()
x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6])
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
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,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
control_type=control_type,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
actuator_type: int = None,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
Path to the bioMod
n_shooting: int
The number of shooting points
final_time: float
The time at final node
actuator_type: int
The type of actuator to use: 1 (torque activations) or 2 (torque max constraints)
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- 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_CONTROL,
key="tau",
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:
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")
if actuator_type == 2:
constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT,
node=Node.ALL_SHOOTING,
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,
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 = "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)
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
get_all_nodes_at_once=True,
quadratic=True,
)
# 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="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)
# 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,
phase_time,
n_shooting,
min_bound,
max_bound,
mu,
ode_solver=OdeSolver.RK4()):
# --- Options --- #
# Model path
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -500, 500, 0
tau_mapping = BiMapping([None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT)
# Constraints
constraints = ConstraintList()
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=1,
)
constraints.add(
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=2,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=mu,
)
# 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] * tau_mapping.to_first.len,
[tau_max] * tau_mapping.to_first.len)
u_init = InitialGuessList()
u_init.add([tau_init] * tau_mapping.to_first.len)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
tau_mapping=tau_mapping,
ode_solver=ode_solver,
)
class gait_muscle_driven:
def __init__(self,
models,
nb_shooting,
phase_time,
q_ref,
qdot_ref,
markers_ref,
grf_ref,
moments_ref,
cop_ref,
save_path=None,
n_threads=1):
self.models = models
# Element for the optimization
self.n_phases = len(models)
self.nb_shooting = nb_shooting
self.phase_time = phase_time
self.n_threads = n_threads
# Element for the tracking
self.q_ref = q_ref
self.qdot_ref = qdot_ref
self.markers_ref = markers_ref
self.grf_ref = grf_ref
self.moments_ref = moments_ref
self.cop_ref = cop_ref
# Element from the model
self.nb_q = models[0].nbQ()
self.nb_qdot = models[0].nbQdot()
self.nb_tau = models[0].nbGeneralizedTorque()
self.nb_mus = models[0].nbMuscleTotal()
self.torque_min, self.torque_max, self.torque_init = -1000, 1000, 0
self.activation_min, self.activation_max, self.activation_init = 1e-3, 0.99, 0.1
# objective functions
self.objective_functions = ObjectiveList()
self.set_objective_function()
# dynamics
self.dynamics = DynamicsList()
self.set_dynamics()
# constraints
self.constraints = ConstraintList()
self.set_constraint()
# Phase transitions
self.phase_transition = PhaseTransitionList()
self.set_phase_transition()
# Path constraint
self.x_bounds = BoundsList()
self.u_bounds = BoundsList()
self.set_bounds()
# Initial guess
self.x_init = InitialGuessList()
self.u_init = InitialGuessList()
if save_path is not None:
self.save_path = save_path
self.set_initial_guess_from_solution()
else:
self.set_initial_guess()
# Ocp
self.ocp = OptimalControlProgram(
self.models,
self.dynamics,
self.nb_shooting,
self.phase_time,
x_init=self.x_init,
x_bounds=self.x_bounds,
u_init=self.u_init,
u_bounds=self.u_bounds,
objective_functions=self.objective_functions,
constraints=self.constraints,
phase_transitions=self.phase_transition,
n_threads=self.n_threads,
)
def set_objective_function(self):
objective.set_objective_function_heel_strike(self.objective_functions,
self.markers_ref[0],
self.grf_ref[0],
self.moments_ref[0],
self.cop_ref[0], 0)
objective.set_objective_function_flatfoot(self.objective_functions,
self.markers_ref[1],
self.grf_ref[1],
self.moments_ref[1],
self.cop_ref[1], 1)
objective.set_objective_function_forefoot(self.objective_functions,
self.markers_ref[2],
self.grf_ref[2],
self.moments_ref[2],
self.cop_ref[2], 2)
objective.set_objective_function_swing(self.objective_functions,
self.markers_ref[3],
self.grf_ref[3],
self.moments_ref[3],
self.cop_ref[3], 3)
def set_dynamics(self):
dynamics.set_muscle_driven_dynamics(self.dynamics)
def set_constraint(self):
constraint.set_constraint_heel_strike(self.constraints, 0)
constraint.set_constraint_flatfoot(self.constraints, 1)
constraint.set_constraint_forefoot(self.constraints, 2)
def set_phase_transition(self):
self.phase_transition.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=0)
self.phase_transition.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
def set_bounds(self):
for p in range(self.n_phases):
self.x_bounds.add(bounds=QAndQDotBounds(self.models[p]))
self.u_bounds.add([self.torque_min] * self.nb_tau +
[self.activation_min] * self.nb_mus,
[self.torque_max] * self.nb_tau +
[self.activation_max] * self.nb_mus)
# # without iliopsoas
# self.u_bounds[p].max[self.nb_tau + 6, :]=0.001
# self.u_bounds[p].max[self.nb_tau + 7, :] = 0.001
# without rectus femoris
# self.u_bounds[p].max[self.nb_tau + 11, :]=0.001
def set_initial_guess(self):
n_shoot = 0
for p in range(self.n_phases):
init_x = np.zeros(
(self.nb_q + self.nb_qdot, self.nb_shooting[p] + 1))
init_x[:self.nb_q, :] = self.q_ref[p]
init_x[self.nb_q:self.nb_q + self.nb_qdot, :] = self.qdot_ref[p]
self.x_init.add(init_x, interpolation=InterpolationType.EACH_FRAME)
init_u = [self.torque_init] * self.nb_tau + [self.activation_init
] * self.nb_mus
self.u_init.add(init_u)
n_shoot += self.nb_shooting[p]
def set_initial_guess_from_solution(self):
n_shoot = 0
for p in range(self.n_phases):
init_x = np.zeros(
(self.nb_q + self.nb_qdot, self.nb_shooting[p] + 1))
init_x[:self.nb_q, :] = np.load(self.save_path +
"q.npy")[:, n_shoot:n_shoot +
self.nb_shooting[p] + 1]
init_x[self.nb_q:self.nb_q +
self.nb_qdot, :] = np.load(self.save_path +
"qdot.npy")[:, n_shoot:n_shoot +
self.nb_shooting[p] +
1]
self.x_init.add(init_x, interpolation=InterpolationType.EACH_FRAME)
init_u = np.zeros((self.nb_tau + self.nb_mus, self.nb_shooting[p]))
init_u[:self.nb_tau, :] = np.load(self.save_path +
"tau.npy")[:, n_shoot:n_shoot +
self.nb_shooting[p]]
init_u[self.nb_tau:, :] = np.load(
self.save_path + "muscle.npy")[:, n_shoot:n_shoot +
self.nb_shooting[p]]
self.u_init.add(init_u, interpolation=InterpolationType.EACH_FRAME)
n_shoot += self.nb_shooting[p]
def solve(self):
sol = self.ocp.solve(
solver=Solver.IPOPT,
solver_options={
"ipopt.tol": 1e-3,
"ipopt.max_iter": 2000,
"ipopt.hessian_approximation": "exact",
"ipopt.limited_memory_max_history": 50,
#"ipopt.linear_solver": "ma57",
},
show_online_optim=False,
)
return sol
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(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2)
objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=3)
# Dynamics
dynamics = DynamicsTypeList()
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
dynamics.add(DynamicsType.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=0)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=0)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=1)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=2)
constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(QAndQDotBounds(biorbd_model[0]))
x_bounds.add(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 StateTransition.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(StateTransition.IMPACT, phase_pre_idx=1)
state_transitions.add(custom_state_transition, phase_pre_idx=2, idx_1=1, idx_2=3)
state_transitions.add(StateTransition.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="cube.bioMod",
ode_solver=OdeSolver.RK,
long_optim=False):
# --- Options --- #
# Model path
biorbd_model = (biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path))
# Problem parameters
if long_optim:
number_shooting_points = (100, 300, 100)
else:
number_shooting_points = (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.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)
# 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,
number_shooting_points,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
)
def prepare_ocp(biorbd_model_path,
phase_time,
n_shooting,
muscle_activations_ref,
contact_forces_ref,
ode_solver=OdeSolver.RK4()):
# 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
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL,
key="muscles",
target=muscle_activations_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES,
target=contact_forces_ref)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="q",
weight=0.001)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles",
weight=0.001)
# objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="torque", weight=0.001)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_DRIVEN,
with_torque=True,
with_contact=True)
# 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] * biorbd_model.nbGeneralizedTorque() +
[activation_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * biorbd_model.nbGeneralizedTorque() +
[activation_max] * biorbd_model.nbMuscleTotal(),
)
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() +
[activation_init] * biorbd_model.nbMuscleTotal())
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions=objective_functions,
ode_solver=ode_solver,
)
def prepare_ocp(
biorbd_model: biorbd.Model,
final_time: float,
n_shooting: int,
markers_ref: np.ndarray,
tau_ref: np.ndarray,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the ocp
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 markers to track
tau_ref: np.ndarray
The generalized forces to track
ode_solver: OdeSolver
The ode solver to use
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS,
axis_to_track=[Axis.Y, Axis.Z],
weight=100,
target=markers_ref)
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][:, 0] = 0
# Initial guess
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
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_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,
final_time: float,
n_shooting: int,
ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the model
final_time: float
The time of the final node
n_shooting: int
The number of shooting points
ode_solver:
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",
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.TRACK_SEGMENT_WITH_CUSTOM_RT,
node=Node.ALL,
segment="seg_rt",
rt=0)
# Path constraint
nq = biorbd_model.nbQ()
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][2, [0, -1]] = [-1.57, 1.57]
x_bounds[0][nq:, [0, -1]] = 0
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
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,
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,
ode_solver: OdeSolver = OdeSolver.RK4(),
weight: float = 1,
min_time=0,
max_time=np.inf,
) -> 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
min_time: float
The minimum time allowed for the final node
max_time: float
The maximum time allowed for the final node
Returns
-------
The OptimalControlProgram ready to be solved
"""
# --- Options --- #
biorbd_model = biorbd.Model(biorbd_model_path)
tau_min, tau_max, tau_init = -100, 100, 0
n_q = biorbd_model.nbQ()
n_qdot = biorbd_model.nbQdot()
n_tau = biorbd_model.nbGeneralizedTorque()
# Add objective functions
objective_functions = ObjectiveList()
# A weight of -1 will maximize time
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME,
weight=weight,
min_bound=min_time,
max_bound=max_time)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Path constraint
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
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(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/models/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_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)
# 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="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.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):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
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,
)
def prepare_ocp(biorbd_model_path,
final_time,
n_shooting,
x_warm=None,
use_sx=False,
n_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(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="q",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="muscles",
weight=10,
multi_thread=False)
objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS,
weight=100000,
first_marker="target",
second_marker="COM_hand")
# 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] = (1.0, 1.0, 0, 0)
# Initial guess
if x_warm is None:
x_init = InitialGuess([1.57] * biorbd_model.nbQ() +
[0] * biorbd_model.nbQdot())
else:
x_init = InitialGuess(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,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
use_sx=use_sx,
n_threads=n_threads,
)
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_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
# 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,
)
def prepare_ocp(
biorbd_model_path: str = "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_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2)
constraints.add(ConstraintFcn.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
x_bounds[0][4:8, [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()
u_bounds.add([tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ())
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,
)
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_residual_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 prepare_ocp(biorbd_model,
final_time,
x0,
nbGT,
number_shooting_points,
use_SX=False,
nb_threads=8,
use_torque=True):
# --- Options --- #
# Model path
biorbd_model = biorbd_model
nbQ = biorbd_model.nbQ()
nbGT = nbGT
nbMT = biorbd_model.nbMuscleTotal()
tau_min, tau_max, tau_init = -100, 100, 0
muscle_min, muscle_max, muscle_init = 0, 1, 0.5
activation_min, activation_max, activation_init = 0, 1, 0.2
# Add objective functions
objective_functions = ObjectiveList()
# Dynamics
dynamics = DynamicsTypeList()
if use_activation and use_torque:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
elif use_activation is not True and use_torque:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN)
elif use_activation and use_torque is not True:
dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN)
elif use_activation is not True and use_torque is not True:
dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN)
# State path constraint
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
if use_activation is not True:
x_bounds[0].concatenate(
Bounds([activation_min] * biorbd_model.nbMuscles(),
[activation_max] * biorbd_model.nbMuscles()))
# Control path constraint
u_bounds = BoundsList()
u_bounds.add([
[tau_min] * nbGT + [muscle_min] * biorbd_model.nbMuscleTotal(),
[tau_max] * nbGT + [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] * nbGT + [muscle_init] * nbMT) + 0.1
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 prepare_ocp_phase_transitions(
biorbd_model_path: str,
with_constraints: bool,
with_mayer: bool,
with_lagrange: bool,
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
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()
if with_lagrange:
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)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
weight=0,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS,
weight=0,
phase=3,
marker_index=[0, 1])
if with_mayer:
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION,
phase=0,
node=1)
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
weight=0,
phase=3,
marker_index=[0, 1])
objective_functions.add(
minimize_difference,
custom_type=ObjectiveFcn.Mayer,
node=Node.TRANSITION,
weight=100,
phase=1,
quadratic=True,
)
# 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()
if with_constraints:
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker="m0",
second_marker="m1",
phase=0,
list_index=1,
)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=2,
first_marker="m0",
second_marker="m1",
phase=0,
list_index=2)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=0,
list_index=3,
)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m1",
phase=1,
list_index=4,
)
constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker="m0",
second_marker="m2",
phase=2,
list_index=5,
)
constraints.add(custom_func_track_markers,
node=Node.ALL,
first_marker="m0",
second_marker="m1",
phase=3,
list_index=6)
# 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())
# Define phase transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=2)
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,
phase_transitions=phase_transitions,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
initialize_near_solution: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
constr: bool = True,
use_sx: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
initialize_near_solution: bool
If the initial guess should be almost the solution (this is merely to reduce the time of the tests)
ode_solver: OdeSolver
The ode solver to use
constr: bool
If the constraint should be applied (this is merely to reduce the time of the tests)
use_sx: bool
If SX CasADi variables should be used
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_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
if constr:
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.START,
first_marker_idx=0,
second_marker_idx=4)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=5)
constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS,
node=Node.ALL,
marker_idx=1,
segment_idx=2,
axis=Axis.X)
else:
constraints = ConstraintList()
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
for i in range(1, 8):
if i != 3:
x_bounds[0][i, [0, -1]] = 0
x_bounds[0][2, -1] = 1.57
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
if initialize_near_solution:
for i in range(2):
x_init[0].init[i] = 1.5
for i in range(4, 6):
x_init[0].init[i] = 0.7
for i in range(6, 8):
x_init[0].init[i] = 0.6
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
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,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
use_sx=use_sx,
)
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,
index=1,
weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model))
# 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] = Arm_Quat_D[1:]
x[12, i] = Arm_Quat_D[0]
x[9:12, i] = Arm_Quat_G[1:]
x[13, i] = Arm_Quat_G[0]
x_init = InitialGuessList()
x_init.add(x, interpolation=InterpolationType.LINEAR)
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 partial_ocp_parameters(n_phases):
if n_phases != 1 and n_phases != 3:
raise RuntimeError("n_phases should be 1 or 3")
biorbd_model_path = TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/cube.bioMod"
biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)
n_shooting = (2, 2, 2)
final_time = (2, 5, 4)
time_min = [1, 3, 0.1]
time_max = [2, 4, 0.8]
tau_min, tau_max, tau_init = -100, 100, 0
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
if n_phases > 1:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
if n_phases > 1:
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.min[i, [0, -1]] = 0
bounds.max[i, [0, -1]] = 0
x_bounds[0].min[2, 0] = 0.0
x_bounds[0].max[2, 0] = 0.0
if n_phases > 1:
x_bounds[2].min[2, [0, -1]] = [0.0, 1.57]
x_bounds[2].max[2, [0, -1]] = [0.0, 1.57]
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
if n_phases > 1:
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
if n_phases > 1:
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 > 1:
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
return (
biorbd_model[:n_phases],
n_shooting[:n_phases],
final_time[:n_phases],
time_min[:n_phases],
time_max[:n_phases],
tau_min,
tau_max,
tau_init,
dynamics,
x_bounds,
x_init,
u_bounds,
u_init,
)
def prepare_ocp(biorbd_model_path,
final_time,
number_shooting_points,
use_SX=True):
# --- 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(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100.0,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1.0)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE,
weight=50000.0,
target=data_to_track[-1:, :].T,
node=Node.END,
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=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,
use_SX=use_SX,
)
def prepare_ocp(
biorbd_model_path: str,
final_time: float,
n_shooting: int,
weight: float,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> 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_TORQUE)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL)
objective_functions.add(
ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker_idx=0, second_marker_idx=5, weight=weight
)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
# 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,
)
u_mi = excitations_min[co]
u_ma = excitations_max
# Update u_init and u_bounds
u_init = InitialGuessOption(np.tile(u_i, (ocp.nlp[0].ns, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
x_init = InitialGuessOption(np.tile(
np.concatenate((x_phase[0, :], [0.5] * biorbd_model.nbMuscles())),
(ocp.nlp[0].ns + 1, 1)).T,
interpolation=InterpolationType.EACH_FRAME)
ocp.update_initial_guess(x_init, u_init=u_init)
u_bounds = BoundsOption([u_mi, u_ma],
interpolation=InterpolationType.CONSTANT)
x_bounds = BoundsList()
x_bounds.add(QAndQDotBounds(biorbd_model))
# add muscle activation bounds
x_bounds[0].concatenate(
Bounds([0] * biorbd_model.nbMuscles(),
[1] * biorbd_model.nbMuscles()))
x_bounds[0].min[:biorbd_model.nbQ(),
0] = x_phase[0, :biorbd_model.nbQ()] - 0.1
x_bounds[0].max[:biorbd_model.nbQ(),
0] = x_phase[0, :biorbd_model.nbQ()] + 0.1
ocp.update_bounds(x_bounds=x_bounds, u_bounds=u_bounds)
for phase in range(1, nb_phase + 1):
# sol = ocp.solve(solver=Solver.IPOPT)
sol = ocp.solve(solver=Solver.ACADOS,
show_online_optim=False,
solver_options={
