def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
if pn.u is None:
return None
u = pn.nlp.controls
return MX.zeros(u.shape), u.cx, MX.zeros(u.shape)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/models/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path), Dynamics(DynamicsFcn.TORQUE_DRIVEN), 30, 2, constraints=constraints
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
ocp.solve()
def test_custom_constraint_mx_fail():
def custom_mx_fail(pn):
return MX(0), vertcat(*pn.u), MX(0)
bioptim_folder = TestUtils.bioptim_folder()
model_path = bioptim_folder + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(RuntimeError, match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
def test_custom_constraint_mx_fail():
def custom_mx_fail(ocp, nlp, t, x, u, p):
return MX(0), vertcat(*u), MX(0)
PROJECT_FOLDER = Path(__file__).parent / ".."
model_path = str(PROJECT_FOLDER) + "/examples/getting_started/cube.bioMod"
constraints = ConstraintList()
constraints.add(custom_mx_fail, node=Node.ALL)
ocp = OptimalControlProgram(
biorbd.Model(model_path),
Dynamics(DynamicsFcn.TORQUE_DRIVEN),
30,
2,
constraints=constraints,
)
with pytest.raises(
RuntimeError,
match="Ipopt doesn't support SX/MX types in constraints bounds"):
sol = ocp.solve(show_online_optim=True)
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
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
x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot()
x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot()
x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot()
# Initial guess
x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot()))
# Define control path constraint
u_bounds = Bounds([-100] * m.nbGeneralizedTorque(),
[0] * m.nbGeneralizedTorque())
u_init = InitialGuess([0] * m.nbGeneralizedTorque())
ocp = OptimalControlProgram(
m,
dynamics,
number_shooting_points=30,
phase_time=0.5,
x_init=x_init,
u_init=u_init,
x_bounds=x_bounds,
u_bounds=u_bounds,
objective_functions=objective_functions,
)
# --- Solve the program --- #
sol = ocp.solve(show_online_optim=True)
# --- Show results --- #
result = ShowResult(ocp, sol)
result.animate()
class ViolinOcp:
# TODO Get these values from a better method
tau_min, tau_max, tau_init = -100, 100, 0
# TODO add external forces?
# TODO Warm starting when updating the objective_bow_target
# TODO All the logic from NMPC
# TODO include the muscle fatigue dynamics, constraints and objectives
# dynamics.add(xia.xia_model_configuration, dynamic_function=xia.xia_model_dynamic)
def __init__(
self,
model_path: str,
violin: Violin,
bow: Bow,
n_cycles: int,
bow_starting: BowPosition.TIP,
init_file: str = None,
use_muscles: bool = True,
time_per_cycle: float = 1,
n_shooting_per_cycle: int = 30,
solver: Solver = Solver.IPOPT,
n_threads: int = 8,
):
self.model_path = model_path
self.model = biorbd.Model(self.model_path)
self.n_q = self.model.nbQ()
self.n_tau = self.model.nbGeneralizedTorque()
self.use_muscles = use_muscles
self.n_mus = self.model.nbMuscles() if self.use_muscles else 0
self.violin = violin
self.bow = bow
self.bow_starting = bow_starting
self.n_cycles = n_cycles
self.n_shooting_per_cycle = n_shooting_per_cycle
self.n_shooting = self.n_shooting_per_cycle * self.n_cycles
self.time_per_cycle = time_per_cycle
self.time = self.time_per_cycle * self.n_cycles
self.solver = solver
self.n_threads = n_threads
if self.use_muscles:
self.dynamics = Dynamics(
DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN)
else:
self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN)
self.x_bounds = Bounds()
self.u_bounds = Bounds()
self._set_bounds()
self.x_init = InitialGuess()
self.u_init = InitialGuess()
self._set_initial_guess(init_file)
self.objective_functions = ObjectiveList()
self._set_generic_objective_functions()
self.constraints = ConstraintList()
self._set_generic_constraints()
self._set_generic_ocp()
if use_muscles:
online_muscle_torque(self.ocp)
def _set_generic_objective_functions(self):
# Regularization objectives
self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE,
weight=0.01,
list_index=0)
if self.use_muscles:
self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
index=self.violin.virtual_tau,
weight=0.01,
list_index=1)
self.objective_functions.add(
ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL,
weight=10,
list_index=2)
else:
self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE,
weight=0.01,
list_index=1)
self.objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_QDDOT,
weight=0.01,
list_index=3)
# Keep the bow align at 90 degrees with the violin
self.objective_functions.add(
ObjectiveFcn.Lagrange.TRACK_SEGMENT_WITH_CUSTOM_RT,
weight=1000,
segment_idx=self.bow.segment_idx,
rt_idx=self.violin.rt_on_string,
list_index=4)
def _set_generic_constraints(self):
# Keep the bow in contact with the violin
if self.solver == Solver.IPOPT:
self.constraints.add(
ConstraintFcn.SUPERIMPOSE_MARKERS,
node=Node.ALL,
first_marker_idx=self.bow.contact_marker,
second_marker_idx=self.violin.bridge_marker,
)
else:
self.objective_functions.add(
ObjectiveFcn.Lagrange.SUPERIMPOSE_MARKERS,
node=Node.ALL,
first_marker_idx=self.bow.contact_marker,
second_marker_idx=self.violin.bridge_marker,
list_index=6,
weight=1000,
)
# Keep the bow in contact with the violin, but allow for prediction error
# for j in range(1, 5):
# constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
# node=j,
# min_bound=0,
# max_bound=0,
# first_marker_idx=Bow.contact_marker,
# second_marker_idx=violin.bridge_marker, list_index=j)
# for j in range(5, nb_shooting + 1):
# constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS,
# node=j,
# min_bound=-10**(j-14), #-10**(j-14) gives 25 iterations
# max_bound=10**(j-14), # (j-4)/10 gives 21 iterations
# first_marker_idx=Bow.contact_marker,
# second_marker_idx=violin.bridge_marker, list_index=j)
# if self.use_muscles:
# if self.n_cycles >= 3:
# self.constraints.add(
# ConstraintFcn.TRACK_TORQUE,
# index=self.violin.virtual_tau,
# min_bound=-3,
# max_bound=3,
# node=list(range(self.n_shooting_per_cycle, self.n_shooting - self.n_shooting_per_cycle + 1)),
# )
# else:
# self.constraints.add(
# ConstraintFcn.TRACK_TORQUE,
# index=self.violin.virtual_tau,
# min_bound=-15,
# max_bound=15,
# node=Node.ALL,
# )
def _set_bounds(self):
self.x_bounds = QAndQDotBounds(self.model)
self.x_bounds[:self.n_q, 0] = self.violin.q(self.bow_starting)
self.x_bounds[self.n_q:, 0] = 0
# self.x_bounds.min[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) - 0.01
# self.x_bounds.max[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) + 0.01
u_min = [self.tau_min] * self.n_tau + [0] * self.n_mus
u_max = [self.tau_max] * self.n_tau + [1] * self.n_mus
self.u_bounds = Bounds(u_min, u_max)
def _set_initial_guess(self, init_file):
if init_file is None:
x_init = np.zeros((self.n_q * 2, 1))
x_init[:self.n_q, 0] = self.violin.q(self.bow_starting)
u_init = np.zeros((self.n_tau + self.n_mus, 1))
self.x_init = InitialGuess(x_init)
self.u_init = InitialGuess(u_init)
else:
_, sol = ViolinOcp.load(init_file)
self.x_init = InitialGuess(
sol.states["all"], interpolation=InterpolationType.EACH_FRAME)
self.u_init = InitialGuess(
sol.controls["all"][:, :-1],
interpolation=InterpolationType.EACH_FRAME)
def set_bow_target_objective(self,
bow_target: np.ndarray,
weight: float = 10000,
sol: Solution = None):
new_objectives = Objective(
ObjectiveFcn.Lagrange.TRACK_STATE,
node=Node.ALL,
weight=weight,
target=bow_target,
index=self.bow.hair_idx,
list_index=5,
)
self.ocp.update_objectives(new_objectives)
def _set_generic_ocp(self):
self.ocp = OptimalControlProgram(
biorbd_model=self.model,
dynamics=self.dynamics,
n_shooting=self.n_shooting,
phase_time=self.time,
x_init=self.x_init,
u_init=self.u_init,
x_bounds=self.x_bounds,
u_bounds=self.u_bounds,
objective_functions=self.objective_functions,
constraints=self.constraints,
use_sx=self.solver == Solver.ACADOS,
n_threads=self.n_threads,
)
def solve(self, **opts: Any) -> Solution:
return self.ocp.solve(solver=self.solver, **opts)
@staticmethod
def load(file_path: str):
return MovingHorizonEstimator.load(file_path)
def save(self, sol: Solution, stand_alone: bool = False):
try:
os.mkdir("results")
except FileExistsError:
pass
t = time.localtime(time.time())
if stand_alone:
self.ocp.save(sol,
f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}_out.bo",
stand_alone=True)
else:
self.ocp.save(sol,
f"results/{t.tm_year}_{t.tm_mon}_{t.tm_mday}.bo",
stand_alone=False)
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
