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