def prepare_ocp(
biorbd_model_path: str,
n_shooting: int,
final_time: float,
loop_from_constraint: bool,
ode_solver: OdeSolver = OdeSolver.RK4(),
) -> OptimalControlProgram:
"""
Prepare the program
Parameters
----------
biorbd_model_path: str
The path of the biorbd model
final_time: float
The time at the final node
n_shooting: int
The number of shooting points
loop_from_constraint: bool
If the looping cost should be a constraint [True] or an objective [False]
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
biorbd_model = biorbd.Model(biorbd_model_path)
# Add objective functions
objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100)
# Dynamics
dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.MID,
first_marker_idx=0,
second_marker_idx=2)
constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.END,
first_marker_idx=0,
second_marker_idx=1)
# Path constraint
x_bounds = QAndQDotBounds(biorbd_model)
# First node is free but mid and last are constrained to be exactly at a certain point.
# The cyclic penalty ensures that the first node and the last node are the same.
x_bounds[2:6, -1] = [1.57, 0, 0, 0]
# Initial guess
x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()))
# Define control path constraint
tau_min, tau_max, tau_init = -100, 100, 0
u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(),
[tau_max] * biorbd_model.nbGeneralizedTorque())
u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque())
# ------------- #
# A phase transition loop constraint is treated as
# hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or
# as a soft penalty (objective) otherwise
phase_transitions = PhaseTransitionList()
if loop_from_constraint:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0)
else:
phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
def prepare_ocp(
biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()
) -> OptimalControlProgram:
"""
Parameters
----------
biorbd_model_path: str
The path to the bioMod
ode_solver: OdeSolver
The type of ode solver used
Returns
-------
The ocp ready to be solved
"""
# Model path
biorbd_model = (
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
biorbd.Model(biorbd_model_path),
)
# Problem parameters
n_shooting = (20, 20, 20, 20)
final_time = (2, 5, 4, 2)
tau_min, tau_max, tau_init = -100, 100, 0
# Add objective functions
objective_functions = ObjectiveList()
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3)
# Dynamics
dynamics = DynamicsList()
expand = False if isinstance(ode_solver, OdeSolver.IRK) else True
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand)
# Constraints
constraints = ConstraintList()
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -1] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
"""
By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3)
are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous,
as for example IMPACT or CUSTOM can be used as below.
"phase_pre_idx" corresponds to the index of the phase preceding the transition.
IMPACT will cause an impact related discontinuity when defining one or more contact points in the model.
CUSTOM will allow to call the custom function previously presented in order to have its own phase transition.
Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity)
you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx.
If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than
zero. It will thereafter be treated as a Mayer objective function with the specified weight.
"""
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping(range(3), range(3)))
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5)
phase_transitions.add(PhaseTransitionFcn.CYCLIC)
return OptimalControlProgram(
biorbd_model,
dynamics,
n_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
ode_solver=ode_solver,
phase_transitions=phase_transitions,
)
def prepare_ocp_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_TORQUE,
weight=100,
phase=0)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=1)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=2)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=100,
phase=3)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY,
weight=0,
phase=3,
axis=None)
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(
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)
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)
constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
node=2,
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(custom_func_track_markers,
node=Node.ALL,
first_marker="m0",
second_marker="m1",
phase=3)
# Path constraint
x_bounds = BoundsList()
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0]))
x_bounds[0][[1, 3, 4, 5], 0] = 0
x_bounds[-1][[1, 3, 4, 5], -1] = 0
x_bounds[0][2, 0] = 0.0
x_bounds[2][2, [0, -1]] = [0.0, 1.57]
# Initial guess
x_init = InitialGuessList()
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot()))
# Define control path constraint
u_bounds = BoundsList()
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(),
[tau_max] * biorbd_model[0].nbGeneralizedTorque())
u_init = InitialGuessList()
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque())
# Define phase transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=2,
idx_1=1,
idx_2=3)
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 = "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,
)
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 __init__(self,
model_paths,
n_shoot,
time_min,
phase_time,
time_max,
initial_pose,
n_thread=1):
self.models = []
self._load_models(model_paths)
# Element for the optimization
self.n_phases = 5
self.n_shoot = n_shoot
self.time_min = time_min
self.phase_time = phase_time
self.time_max = time_max
self.takeoff = 1 # The index of takeoff phase
self.flat_foot_phases = 0, 4 # The indices of flat foot phases
self.toe_only_phases = 1, 3 # The indices of toe only phases
# Elements from the model
self.initial_states = []
self._set_initial_states(initial_pose, [0, 0, 0, 0, 0, 0, 0])
self.heel_and_toe_idx = (
1, 2, 4, 5
) # Contacts indices of heel and toe in bioMod 2 contacts
self.toe_idx = (1, 3) # Contacts indices of toe in bioMod 1 contact
self.n_q, self.n_qdot, self.n_tau = -1, -1, -1
self.q_mapping, self.qdot_mapping, self.tau_mapping = None, None, None
self._set_dimensions_and_mapping()
# Prepare the optimal control program
self.dynamics = DynamicsList()
self._set_dynamics()
self.constraints = ConstraintList()
self.tau_min = 20
self._set_constraints()
self.objective_functions = ObjectiveList()
self._set_objective_functions()
self.x_bounds = BoundsList()
self.u_bounds = BoundsList()
self._set_boundary_conditions()
self.phase_transitions = PhaseTransitionList()
self._set_phase_transitions()
self.x_init = InitialGuessList()
self.u_init = InitialGuessList()
self._set_initial_guesses()
self.ocp = OptimalControlProgram(
self.models,
self.dynamics,
self.n_shoot,
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,
q_mapping=self.q_mapping,
qdot_mapping=self.q_mapping,
tau_mapping=self.tau_mapping,
phase_transitions=self.phase_transitions,
n_threads=n_thread,
)
class Jumper5Phases:
def __init__(self,
model_paths,
n_shoot,
time_min,
phase_time,
time_max,
initial_pose,
n_thread=1):
self.models = []
self._load_models(model_paths)
# Element for the optimization
self.n_phases = 5
self.n_shoot = n_shoot
self.time_min = time_min
self.phase_time = phase_time
self.time_max = time_max
self.takeoff = 1 # The index of takeoff phase
self.flat_foot_phases = 0, 4 # The indices of flat foot phases
self.toe_only_phases = 1, 3 # The indices of toe only phases
# Elements from the model
self.initial_states = []
self._set_initial_states(initial_pose, [0, 0, 0, 0, 0, 0, 0])
self.heel_and_toe_idx = (
1, 2, 4, 5
) # Contacts indices of heel and toe in bioMod 2 contacts
self.toe_idx = (1, 3) # Contacts indices of toe in bioMod 1 contact
self.n_q, self.n_qdot, self.n_tau = -1, -1, -1
self.q_mapping, self.qdot_mapping, self.tau_mapping = None, None, None
self._set_dimensions_and_mapping()
# Prepare the optimal control program
self.dynamics = DynamicsList()
self._set_dynamics()
self.constraints = ConstraintList()
self.tau_min = 20
self._set_constraints()
self.objective_functions = ObjectiveList()
self._set_objective_functions()
self.x_bounds = BoundsList()
self.u_bounds = BoundsList()
self._set_boundary_conditions()
self.phase_transitions = PhaseTransitionList()
self._set_phase_transitions()
self.x_init = InitialGuessList()
self.u_init = InitialGuessList()
self._set_initial_guesses()
self.ocp = OptimalControlProgram(
self.models,
self.dynamics,
self.n_shoot,
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,
q_mapping=self.q_mapping,
qdot_mapping=self.q_mapping,
tau_mapping=self.tau_mapping,
phase_transitions=self.phase_transitions,
n_threads=n_thread,
)
def _load_models(self, model_paths):
self.models = [biorbd.Model(elt) for elt in model_paths]
def _set_initial_states(self, initial_pose, initial_velocity):
self.initial_states = np.array([list(initial_pose) + initial_velocity
]).T
def _set_dimensions_and_mapping(self):
q_mapping = BiMapping([0, 1, 2, None, 3, None, 3, 4, 5, 6, 4, 5, 6],
[0, 1, 2, 4, 7, 8, 9])
self.q_mapping = [q_mapping for _ in range(self.n_phases)]
self.qdot_mapping = [q_mapping for _ in range(self.n_phases)]
tau_mapping = BiMapping(
[None, None, None, None, 0, None, 0, 1, 2, 3, 1, 2, 3],
[4, 7, 8, 9])
self.tau_mapping = [tau_mapping for _ in range(self.n_phases)]
self.n_q = q_mapping.to_first.len
self.n_qdot = self.n_q
self.n_tau = tau_mapping.to_first.len
def _set_dynamics(self):
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Flat foot
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Toe only
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Aerial phase
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Toe only
self.dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Flat foot
def _set_constraints(self):
# Torque constrained to torqueMax
for i in range(self.n_phases):
self.constraints.add(maximal_tau,
phase=i,
node=Node.ALL,
minimal_tau=self.tau_min)
# Positivity of CoM_dot on z axis prior the take-off
self.constraints.add(com_dot_z,
phase=1,
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,
phase=self.takeoff,
node=Node.END,
index=3,
min_bound=1.0,
max_bound=np.inf)
# Floor constraints for flat foot phases
for p in self.flat_foot_phases:
# Do not pull on floor
for i in self.heel_and_toe_idx:
self.constraints.add(ConstraintFcn.CONTACT_FORCE,
phase=p,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
# Non-slipping constraints
self.constraints.add( # On only one of the feet
ConstraintFcn.NON_SLIPPING,
phase=p,
node=Node.ALL,
normal_component_idx=(1, 2),
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
# Floor constraints for toe only phases
for p in self.toe_only_phases:
# Do not pull on floor
for i in self.toe_idx:
self.constraints.add(ConstraintFcn.CONTACT_FORCE,
phase=p,
node=Node.ALL,
contact_force_idx=i,
max_bound=np.inf)
# Non-slipping constraints
self.constraints.add( # On only one of the feet
ConstraintFcn.NON_SLIPPING,
phase=p,
node=Node.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.5,
)
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 unnecessary movement during for the aerial and reception phases
for p in range(2, 5):
self.objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_STATE_DERIVATIVE,
weight=0.1,
phase=p,
index=range(self.n_q, self.n_q + self.n_qdot),
)
for i in range(self.n_phases):
# Minimize time of the phase
if self.time_min[i] != self.time_max[i]:
self.objective_functions.add(
ObjectiveFcn.Mayer.MINIMIZE_TIME,
weight=0.1,
phase=i,
min_bound=self.time_min[i],
max_bound=self.time_max[i],
)
def _set_boundary_conditions(self):
for i in range(self.n_phases):
# Path constraints
self.x_bounds.add(
bounds=QAndQDotBounds(self.models[i],
q_mapping=self.q_mapping[i],
qdot_mapping=self.qdot_mapping[i]))
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) and velocity
self.objective_functions.add(
ObjectiveFcn.Mayer.TRACK_STATE,
phase=self.n_phases - 1,
index=range(2, self.n_q + self.n_qdot),
target=self.initial_states[2:, :],
)
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):
# Phase transition
self.phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=0)
self.phase_transitions.add(PhaseTransitionFcn.CONTINUOUS,
phase_pre_idx=1)
self.phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=2)
self.phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=3)
# The end of the aerial
self.constraints.add(toe_on_floor,
phase=2,
node=Node.END,
min_bound=-0.001,
max_bound=0.001)
self.constraints.add(heel_on_floor,
phase=3,
node=Node.END,
min_bound=-0.001,
max_bound=0.001)
# Allow for passive velocity at reception
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]
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_nmpc(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):
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 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={
"linear_solver": "ma57",
"hessian_approximation": "limited-memory",
"max_iter": limit_memory_max_iter,
},
)
if limit_memory_max_iter > 0 and exact_max_iter > 0:
warm_start_nmpc(self.ocp, sol)
if exact_max_iter > 0:
sol = self.ocp.solve(
show_online_optim=True and not force_no_graph,
solver_options={
"linear_solver": "ma57",
"hessian_approximation": "exact",
"max_iter": exact_max_iter,
"warm_start_init_point": "yes",
},
)
return sol
def prepare_ocp(
biorbd_model: tuple,
final_time: list,
nb_shooting: list,
markers_ref: list,
grf_ref: list,
q_ref: list,
qdot_ref: list,
M_ref: list,
CoP: list,
nb_threads: int,
) -> OptimalControlProgram:
"""
Prepare the ocp
Parameters
----------
biorbd_model: tuple
Tuple of bioMod (1 bioMod for each phase)
final_time: list
List of the time at the final node.
The length of the list corresponds to the phase number
nb_shooting: list
List of the number of shooting points
markers_ref: list
List of the array of markers trajectories to track
grf_ref: list
List of the array of ground reaction forces to track
q_ref: list
List of the array of joint trajectories.
Those trajectories were computed using Kalman filter
They are used as initial guess
qdot_ref: list
List of the array of joint velocities.
Those velocities were computed using Kalman filter
They are used as initial guess
M_ref: list
List of the array of ground reaction moments to track
CoP: list
List of the array of the measured center of pressure trajectory
nb_threads:int
The number of threads used
Returns
-------
The OptimalControlProgram ready to be solved
"""
# Problem parameters
nb_phases = len(biorbd_model)
nb_q = biorbd_model[0].nbQ()
nb_qdot = biorbd_model[0].nbQdot()
nb_tau = biorbd_model[0].nbGeneralizedTorque()
nb_mus = biorbd_model[0].nbMuscleTotal()
min_bound, max_bound = 0, np.inf
torque_min, torque_max, torque_init = -1000, 1000, 0
activation_min, activation_max, activation_init = 1e-3, 1.0, 0.1
# Add objective functions
markers_pelvis = [0, 1, 2, 3]
markers_anat = [4, 9, 10, 11, 12, 17, 18]
markers_tissus = [5, 6, 7, 8, 13, 14, 15, 16]
markers_foot = [19, 20, 21, 22, 23, 24, 25]
objective_functions = ObjectiveList()
for p in range(nb_phases):
objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE,
weight=1,
index=range(nb_q),
target=q_ref[p],
phase=p,
quadratic=True)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=1000,
index=markers_anat,
target=markers_ref[p][:, markers_anat, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100000,
index=markers_pelvis,
target=markers_ref[p][:, markers_pelvis, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100000,
index=markers_foot,
target=markers_ref[p][:, markers_foot, :],
phase=p,
quadratic=True,
)
objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_MARKERS,
weight=100,
index=markers_tissus,
target=markers_ref[p][:, markers_tissus, :],
phase=p,
quadratic=True,
)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=0.001,
index=(10),
phase=p,
quadratic=True)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=1,
index=(6, 7, 8, 9, 11),
phase=p,
quadratic=True)
objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10,
phase=p,
quadratic=True)
objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE,
weight=0.1,
phase=p,
quadratic=True)
# --- track contact forces for the stance phase ---
for p in range(nb_phases - 1):
objective_functions.add(
track_sum_contact_forces, # track contact forces
grf=grf_ref[p],
custom_type=ObjectiveFcn.Lagrange,
node=Node.ALL,
weight=0.1,
quadratic=True,
phase=p,
)
for p in range(1, nb_phases - 1):
objective_functions.add(
track_sum_contact_moments,
CoP=CoP[p],
M_ref=M_ref[p],
custom_type=ObjectiveFcn.Lagrange,
node=Node.ALL,
weight=0.01,
quadratic=True,
phase=p,
)
# Dynamics
dynamics = DynamicsList()
for p in range(nb_phases - 1):
dynamics.add(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT,
phase=p)
dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN, phase=3)
# Constraints
constraints = ConstraintList()
constraints.add( # null speed for the first phase --> non sliding contact point
ConstraintFcn.TRACK_MARKERS_VELOCITY,
node=Node.START,
index=26,
phase=0,
)
# --- phase flatfoot ---
constraints.add( # positive vertical forces
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=(1, 2, 5),
phase=1,
)
constraints.add( # non slipping y
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(1, 2, 5),
tangential_component_idx=4,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # non slipping x m5
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(2, 5),
tangential_component_idx=3,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # non slipping x heel
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=1,
tangential_component_idx=0,
static_friction_coefficient=0.2,
phase=1,
)
constraints.add( # forces heel at zeros at the end of the phase
get_last_contact_force_null,
node=Node.ALL,
contact_name="Heel_r",
phase=1,
)
# --- phase forefoot ---
constraints.add( # positive vertical forces
ConstraintFcn.CONTACT_FORCE,
min_bound=min_bound,
max_bound=max_bound,
node=Node.ALL,
contact_force_idx=(2, 4, 5),
phase=2,
)
constraints.add(
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=(2, 4, 5),
tangential_component_idx=1,
static_friction_coefficient=0.2,
phase=2,
)
constraints.add( # non slipping x m1
ConstraintFcn.NON_SLIPPING,
node=Node.ALL,
normal_component_idx=2,
tangential_component_idx=0,
static_friction_coefficient=0.2,
phase=2,
)
constraints.add(
get_last_contact_force_null,
node=Node.ALL,
contact_name="all",
phase=2,
)
# Phase Transitions
phase_transitions = PhaseTransitionList()
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=0)
phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1)
# Path constraint
x_bounds = BoundsList()
u_bounds = BoundsList()
for p in range(nb_phases):
x_bounds.add(bounds=QAndQDotBounds(biorbd_model[p]))
u_bounds.add(
[torque_min] * nb_tau + [activation_min] * nb_mus,
[torque_max] * nb_tau + [activation_max] * nb_mus,
)
# Initial guess
x_init = InitialGuessList()
u_init = InitialGuessList()
n_shoot = 0
for p in range(nb_phases):
init_x = np.zeros((nb_q + nb_qdot, nb_shooting[p] + 1))
init_x[:nb_q, :] = q_ref[p]
init_x[nb_q:nb_q + nb_qdot, :] = qdot_ref[p]
x_init.add(init_x, interpolation=InterpolationType.EACH_FRAME)
init_u = [torque_init] * nb_tau + [activation_init] * nb_mus
u_init.add(init_u)
n_shoot += nb_shooting[p]
# ------------- #
return OptimalControlProgram(
biorbd_model,
dynamics,
nb_shooting,
final_time,
x_init,
u_init,
x_bounds,
u_bounds,
objective_functions,
constraints,
phase_transitions=phase_transitions,
n_threads=nb_threads,
)
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,
)
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