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 prepare_ocp(
biorbd_model_path: str,
phase_time: float,
n_shooting: int,
use_actuators: bool = False,
ode_solver: OdeSolver = OdeSolver.RK4(),
objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT",
com_constraints: bool = False,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model_path: str
The path to the bioMod file
phase_time: float
The time at the final node
n_shooting: int
The number of shooting points
use_actuators: bool
If torque or torque activation should be used for the dynamics
ode_solver: OdeSolver
The ode solver to use
objective_name: str
The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT',
'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY')
com_constraints: bool
If a constraint on the COM should be applied
Returns
-------
The OptimalControlProgram ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
if use_actuators:
tau_min, tau_max, tau_init = -1, 1, 0
else:
tau_min, tau_max, tau_init = -500, 500, 0
dof_mapping = BiMappingList()
dof_mapping.add("tau", [None, None, None, 0], [3])
# Add objective functions
objective_functions = ObjectiveList()
if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT":
objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1)
elif objective_name == "MINIMIZE_COM_POSITION":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, node=Node.ALL, axes=Axis.Z, weight=-1)
elif objective_name == "MINIMIZE_COM_VELOCITY":
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100)
# Dynamics
dynamics = DynamicsList()
if use_actuators:
dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True)
else:
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True)
# Constraints
constraints = ConstraintList()
if com_constraints:
constraints.add(
ConstraintFcn.TRACK_COM_VELOCITY,
node=Node.ALL,
min_bound=np.array([-100, -100, -100]),
max_bound=np.array([100, 100, 100]),
)
constraints.add(
ConstraintFcn.TRACK_COM_POSITION,
node=Node.ALL,
min_bound=np.array([-1, -1, -1]),
max_bound=np.array([1, 1, 1]),
)
# Path constraint
n_q = biorbd_model.nbQ()
n_qdot = n_q
pose_at_first_node = [0, 0, -0.5, 0.5]
# 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), [tau_max] * len(dof_mapping["tau"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first))
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,
ode_solver=ode_solver,
)
class JumperOcp:
n_q, n_qdot, n_tau = -1, -1, -1
mapping_list = BiMappingList()
dynamics = DynamicsList()
constraints = ConstraintList()
objective_functions = ObjectiveList()
x_bounds = BoundsList()
u_bounds = BoundsList()
phase_transitions = PhaseTransitionList()
initial_states = []
x_init = InitialGuessList()
u_init = InitialGuessList()
def __init__(self,
path_to_models,
n_phases,
n_thread=8,
control_type=ControlType.CONSTANT):
if n_phases < 1 or n_phases > 5:
raise ValueError("n_phases must be comprised between 1 and 5")
self.jumper = Jumper(path_to_models)
self.n_phases = n_phases
self.takeoff = 0 if self.n_phases == 1 else 1 # The index of takeoff phase
self.control_type = control_type
self.control_nodes = Node.ALL if self.control_type == ControlType.LINEAR_CONTINUOUS else Node.ALL_SHOOTING
self._set_dimensions_and_mapping()
self._set_initial_states()
self._set_dynamics()
self._set_constraints()
self._set_objective_functions()
self._set_boundary_conditions()
self._set_phase_transitions()
self._set_initial_guesses()
self.ocp = OptimalControlProgram(
self.jumper.models[:self.n_phases],
self.dynamics,
self.jumper.n_shoot[:self.n_phases],
self.jumper.phase_time[:self.n_phases],
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,
variable_mappings=self.mapping_list,
phase_transitions=self.phase_transitions,
n_threads=n_thread,
control_type=self.control_type,
)
def _set_initial_states(self):
initial_pose = np.array([self.jumper.body_at_first_node]).T
initial_velocity = np.array([self.jumper.initial_velocity]).T
initial_pose[:self.jumper.models[0].nbRoot(),
0] = self.jumper.find_initial_root_pose()
self.initial_states = np.concatenate((initial_pose, initial_velocity))
def _set_dimensions_and_mapping(self):
self.mapping_list.add("q", self.jumper.q_mapping.to_second.map_idx,
self.jumper.q_mapping.to_first.map_idx)
self.mapping_list.add("qdot", self.jumper.q_mapping.to_second.map_idx,
self.jumper.q_mapping.to_first.map_idx)
self.mapping_list.add("tau", self.jumper.tau_mapping.to_second.map_idx,
self.jumper.tau_mapping.to_first.map_idx)
self.n_q = len(self.mapping_list["q"].to_first)
self.n_qdot = self.n_q
self.n_tau = len(self.mapping_list["tau"].to_first)
def _set_dynamics(self):
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True) # Flat foot
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True) # Toe only
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Aerial phase
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True) # Toe only
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN,
with_contact=True) # Flat foot
def _set_constraints(self):
# Torque constrained to torqueMax
for i in range(self.n_phases):
self.constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT,
phase=i,
node=self.control_nodes,
min_torque=self.jumper.tau_min)
# Positivity of CoM_dot on z axis prior the take-off
self.constraints.add(com_dot_z,
phase=self.takeoff,
node=Node.END,
min_bound=0,
max_bound=np.inf)
# Constraint arm positivity (prevent from local minimum with arms in the back)
self.constraints.add(ConstraintFcn.TRACK_STATE,
key="q",
phase=self.takeoff,
node=Node.END,
index=3,
min_bound=0,
max_bound=np.inf)
# Floor constraints for flat foot phases
for p in self.jumper.flat_foot_phases:
if p >= self.n_phases:
break
# Do not pull on floor
for i in self.jumper.flatfoot_contact_z_idx:
self.constraints.add(ConstraintFcn.TRACK_CONTACT_FORCES,
phase=p,
node=self.control_nodes,
contact_index=i,
max_bound=np.inf)
# Non-slipping constraints
self.constraints.add( # On only one of the feet
ConstraintFcn.NON_SLIPPING,
phase=p,
node=self.control_nodes,
tangential_component_idx=self.jumper.flatfoot_non_slipping[0],
normal_component_idx=self.jumper.flatfoot_non_slipping[1],
static_friction_coefficient=self.jumper.
static_friction_coefficient,
)
# Floor constraints for toe only phases
for p in self.jumper.toe_only_phases:
if p >= self.n_phases:
break
# Do not pull on floor
for i in self.jumper.toe_contact_z_idx:
self.constraints.add(ConstraintFcn.TRACK_CONTACT_FORCES,
phase=p,
node=self.control_nodes,
contact_index=i,
max_bound=np.inf)
# The heel must remain over floor
self.constraints.add(
marker_on_floor,
phase=p,
node=Node.ALL,
index=2,
min_bound=-0.0001,
max_bound=np.inf,
marker=self.jumper.heel_marker_idx,
target=self.jumper.floor_z,
)
# Non-slipping constraints
self.constraints.add( # On only one of the feet
ConstraintFcn.NON_SLIPPING,
phase=p,
node=self.control_nodes,
tangential_component_idx=self.jumper.toe_non_slipping[0],
normal_component_idx=self.jumper.toe_non_slipping[1],
static_friction_coefficient=self.jumper.
static_friction_coefficient,
)
def _set_objective_functions(self):
# Maximize the jump height
self.objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT,
weight=-100,
phase=self.takeoff)
# Minimize the tau on root if present
for p in range(self.n_phases):
root = [
i for i in self.jumper.tau_mapping.to_second.
map_idx[:self.jumper.models[p].nbRoot()] if i is not None
]
if root:
self.objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=0.1,
phase=p,
index=root,
)
# Minimize unnecessary acceleration during for the aerial and reception phases
for p in range(self.n_phases):
if self.control_type == ControlType.LINEAR_CONTINUOUS:
self.objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_CONTROL,
key="tau",
weight=0.1,
derivative=True,
phase=p,
)
for p in range(2, self.n_phases):
self.objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
key="qdot",
weight=0.1,
derivative=True,
phase=p,
)
# Minimize time of the phase
for i in range(self.n_phases):
if self.jumper.time_min[i] != self.jumper.time_max[i]:
self.objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_TIME,
weight=0.1,
phase=i,
min_bound=self.jumper.time_min[i],
max_bound=self.jumper.time_max[i],
)
def _set_boundary_conditions(self):
for i in range(self.n_phases):
# Path constraints
self.x_bounds.add(bounds=QAndQDotBounds(
self.jumper.models[i], dof_mappings=self.mapping_list))
if i == 3 or i == 4:
# Allow greater speed in passive reception
self.x_bounds[i].max[self.jumper.heel_dof + self.n_q, :] *= 2
self.u_bounds.add([-500] * self.n_tau, [500] * self.n_tau)
# Enforce the initial pose and velocity
self.x_bounds[0][:, 0] = self.initial_states[:, 0]
# Target the final pose (except for translation)
if self.n_phases >= 4:
trans_root = self.jumper.models[self.n_phases -
1].segment(0).nbDofTrans()
self.constraints.add(
ConstraintFcn.TRACK_STATE,
key="q",
node=Node.END,
phase=self.n_phases - 1,
index=range(trans_root, self.n_q),
target=self.initial_states[trans_root:self.n_q, :],
min_bound=-0.1,
max_bound=0.1,
)
self.constraints.add(
ConstraintFcn.TRACK_STATE,
key="qdot",
node=Node.END,
phase=self.n_phases - 1,
target=self.initial_states[self.n_q:, :],
min_bound=-0.1,
max_bound=0.1,
)
def _set_initial_guesses(self):
for i in range(self.n_phases):
self.x_init.add(self.initial_states)
self.u_init.add([0] * self.n_tau)
def _set_phase_transitions(self):
if self.n_phases >= 2: # 2 contacts to 1 contact
self.phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=0)
if self.n_phases >= 3: # 1 contact to aerial
self.phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=1)
if self.n_phases >= 4: # aerial to 1 contact
self.phase_transitions.add(PhaseTransitionFcn.IMPACT,
phase_pre_idx=2)
if self.n_phases >= 5: # 1 contact to 2 contacts
self.phase_transitions.add(PhaseTransitionFcn.IMPACT,
phase_pre_idx=3)
# if self.n_phases >= 2: # The contact forces at the end of flat foot equal the beginning of the next phase
# links_0_to_1 = ((0, None), (1, None), (2, 0), (3, 1), (3, 2))
# links_1_to_2 = ((0, None), (1, None), (2, None))
# for link in links_0_to_1:
# self.constraints.add(
# contact_force_continuity,
# phase=0,
# node=Node.TRANSITION,
# idx_pre=link[0],
# idx_post=link[1],
# )
#
# for link in links_1_to_2:
# self.constraints.add(
# contact_force_continuity,
# phase=1,
# node=Node.TRANSITION,
# idx_pre=link[0],
# idx_post=link[1],
# )
if self.n_phases >= 3: # The end of the aerial
self.constraints.add(
marker_on_floor,
phase=2,
index=2,
node=Node.END,
min_bound=-0.001,
max_bound=0.001,
marker=self.jumper.toe_marker_idx,
target=self.jumper.floor_z,
)
if self.n_phases >= 4: # 2 contacts on floor
self.constraints.add(
marker_on_floor,
phase=3,
index=2,
node=Node.END,
min_bound=-0.001,
max_bound=0.001,
marker=self.jumper.heel_marker_idx,
target=self.jumper.floor_z,
)
# Allow for passive velocity at reception
if self.n_phases >= 4:
self.x_bounds[3].min[self.n_q:,
0] = 2 * self.x_bounds[3].min[self.n_q:, 0]
self.x_bounds[3].max[self.n_q:,
0] = 2 * self.x_bounds[3].max[self.n_q:, 0]
if self.n_phases >= 5:
self.x_bounds[4].min[self.n_q:,
0] = 2 * self.x_bounds[4].min[self.n_q:, 0]
self.x_bounds[4].max[self.n_q:,
0] = 2 * self.x_bounds[4].max[self.n_q:, 0]
def solve(self,
limit_memory_max_iter,
exact_max_iter,
load_path=None,
force_no_graph=False):
def warm_start(ocp, sol):
state, ctrl, param = sol.states, sol.controls, sol.parameters
u_init_guess = InitialGuessList()
x_init_guess = InitialGuessList()
for i in range(ocp.n_phases):
if ocp.n_phases == 1:
if ocp.nlp[
i].control_type == ControlType.LINEAR_CONTINUOUS:
u_init_guess.add(
ctrl["all"],
interpolation=InterpolationType.EACH_FRAME)
else:
u_init_guess.add(
ctrl["all"][:, :-1],
interpolation=InterpolationType.EACH_FRAME)
x_init_guess.add(
state["all"],
interpolation=InterpolationType.EACH_FRAME)
else:
if ocp.nlp[
i].control_type == ControlType.LINEAR_CONTINUOUS:
u_init_guess.add(
ctrl[i]["all"],
interpolation=InterpolationType.EACH_FRAME)
else:
u_init_guess.add(
ctrl[i]["all"][:, :-1],
interpolation=InterpolationType.EACH_FRAME)
x_init_guess.add(
state[i]["all"],
interpolation=InterpolationType.EACH_FRAME)
time_init_guess = InitialGuess(param["time"], name="time")
ocp.update_initial_guess(x_init=x_init_guess,
u_init=u_init_guess,
param_init=time_init_guess)
ocp.solver.set_lagrange_multiplier(sol)
# Run optimizations
if not force_no_graph:
add_custom_plots(self.ocp, self)
if load_path:
_, sol = OptimalControlProgram.load(load_path)
return sol
else:
sol = None
if limit_memory_max_iter > 0:
sol = self.ocp.solve(
show_online_optim=exact_max_iter == 0
and not force_no_graph,
solver_options={
"hessian_approximation": "limited-memory",
"max_iter": limit_memory_max_iter,
"linear_solver": "ma57"
},
)
if limit_memory_max_iter > 0 and exact_max_iter > 0:
warm_start(self.ocp, sol)
if exact_max_iter > 0:
sol = self.ocp.solve(
show_online_optim=True and not force_no_graph,
solver_options={
"hessian_approximation": "exact",
"max_iter": exact_max_iter,
"warm_start_init_point": "yes",
"linear_solver": "ma57",
},
)
return sol
def prepare_ocp(
biorbd_model_path: str = "models/double_pendulum.bioMod",
biorbd_model_path_withTranslations: str = "models/double_pendulum_with_translations.bioMod",
) -> OptimalControlProgram:
biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path_withTranslations))
# Problem parameters
n_shooting = (40, 40)
final_time = (1.5, 2.5)
tau_min, tau_max, tau_init = -200, 200, 0
# Mapping
tau_mappings = BiMappingList()
tau_mappings.add("tau", [None, 0], [1], phase=0)
tau_mappings.add("tau", [None, None, None, 0], [3], phase=1)
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, phase=1)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=Node.END, weight=-1000, axes=Axis.Z, phase=1, quadratic=False
)
objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=2, node=Node.END, weight=-100, phase=1, quadratic=False
)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=0)
constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, min_bound=0.3, max_bound=3, phase=1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=False)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[1]))
# Phase 0
x_bounds[0][1, 0] = 0
x_bounds[0][0, 0] = 3.14
x_bounds[0].min[0, -1] = 2 * 3.14
# Phase 1
x_bounds[1][[0, 1, 4, 5], 0] = 0
x_bounds[1].min[2, -1] = 3 * 3.14
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([1] * (biorbd_model[1].nbQ() + biorbd_model[1].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * len(tau_mappings[0]["tau"].to_first), [tau_max] * len(tau_mappings[0]["tau"].to_first))
u_bounds.add([tau_min] * len(tau_mappings[1]["tau"].to_first), [tau_max] * len(tau_mappings[1]["tau"].to_first))
# Control initial guess
u_init = InitialGuessList()
u_init.add([tau_init] * len(tau_mappings[0]["tau"].to_first))
u_init.add([tau_init] * len(tau_mappings[1]["tau"].to_first))
phase_transitions = PhaseTransitionList()
phase_transitions.add(
PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping([0, 1, 2, 3], [2, 3, 6, 7])
)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
constraints=constraints,
variable_mappings=tau_mappings,
phase_transitions=phase_transitions,
)
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,
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
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,
weight=-1)
# Dynamics
dynamics = DynamicsList()
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, 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,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL_SHOOTING,
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] * len(dof_mapping["tau"].to_first),
[tau_max] * len(dof_mapping["tau"].to_first))
u_init = InitialGuessList()
u_init.add([tau_init] * len(dof_mapping["tau"].to_first))
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
phase_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
variable_mappings=dof_mapping,
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",
to_second=[0, 1, None, 2, 2],
to_first=[0, 1, 3],
oppose_to_second=4)
# For convenience, if only q is defined, qdot and tau are automatically defined too
# While computing the derivatives, the states is 6 dimensions (3 for q and 3 for qdot) and controls is 3 dimensions
# However, the forward dynamics ([q, qdot, tau] => qddot) needs 5 dimensions vectors (due to the chosen model)
# 'to_second' is used to convert these 3 dimensions vectors (q, qdot and tau) to their corresponding 5 dimensions
# As discussed in the docstring at the beginning of the file, the first two dofs are conserved, the 3rd
# value is a numerical zero and the final two are equal but opposed.
# The dynamics is computed (qddot) and returns a 5 dimensions vector
# 'to_first' convert back this 5 dimensions qddot to a 3 dimensions needed by Ipopt
# the first two dofs are conserved and the 4th (index 3) is put at the last position (3rd component). The
# other dofs are ignored
# 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,
)
