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