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