def prepare_ocp( biorbd_model, final_time, number_shooting_points, x0, xT, use_SX=False, nb_threads=8, ): # --- Options --- # # Model path biorbd_model = biorbd_model nbQ = biorbd_model.nbQ() # tau_min, tau_max, tau_init = -10, 10, 0 activation_min, activation_max, activation_init = 0, 1, 0.1 excitation_min, excitation_max, excitation_init = 0, 1, 0.2 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=10, states_idx=np.array(range(biorbd_model.nbQ()))) objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=10, states_idx=np.array( range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2))) objective_functions.add( Objective.Lagrange.MINIMIZE_STATE, weight=10, states_idx=np.array( range(biorbd_model.nbQ() * 2, biorbd_model.nbQ() * 2 + biorbd_model.nbMuscles()))) objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10) # objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL_DERIVATIVE, weight=100) objective_functions.add(Objective.Mayer.TRACK_STATE, weight=100000, target=np.array([xT[:biorbd_model.nbQ() * 2]]).T, states_idx=np.array(range(biorbd_model.nbQ() * 2))) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # State path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # add muscle activation bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Control path constraint u_bounds = BoundsList() u_bounds.add([ [excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal(), ]) # Initial guesses x_init = InitialGuessOption(np.tile( np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())), (number_shooting_points + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME) u0 = np.array([excitation_init] * biorbd_model.nbMuscles()) u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_SX=use_SX, nb_threads=nb_threads, )
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound, max_bound): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) tau_min, tau_max, tau_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT) # Dynamics dynamics = DynamicsTypeList() dynamics.add( DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( Constraint.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( Constraint.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * nb_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([ [tau_min] * tau_mapping.reduce.len + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * tau_mapping.reduce.len + [activation_max] * biorbd_model.nbMuscleTotal(), ]) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.reduce.len + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, )
def prepare_ocp( biorbd_model_path, final_time, number_shooting_points, marker_velocity_or_displacement, marker_in_first_coordinates_system, control_type, ode_solver=OdeSolver.RK, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() biorbd_model.markerNames() # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions if marker_in_first_coordinates_system: # Marker should follow this segment (0 velocity when compare to this one) coordinates_system_idx = 0 else: # Marker should be static in global reference frame coordinates_system_idx = -1 objective_functions = ObjectiveList() if marker_velocity_or_displacement == "disp": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, coordinates_system_idx=coordinates_system_idx, index=6, weight=1000, ) elif marker_velocity_or_displacement == "velo": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, index=6, weight=1000) else: raise RuntimeError( "Wrong choice of marker_velocity_or_displacement, actual value is " "{marker_velocity_or_displacement}, should be 'velo' or 'disp'.") # Make sure the segments actually moves (in order to test the relative speed objective) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=6, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=7, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(nq, 2 * nq): x_bounds[0].min[i, :] = -10 x_bounds[0].max[i, :] = 10 # Initial guess x_init = InitialGuessList() x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6]) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_integration_steps=5, control_type=control_type, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "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 all_generalized_mapping = BiMapping([0, 1, 2, -2], [0, 1, 2]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * all_generalized_mapping.to_first.len * 2) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * all_generalized_mapping.to_first.len, [tau_max] * all_generalized_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * all_generalized_mapping.to_first.len) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, all_generalized_mapping=all_generalized_mapping, )
def prepare_ocp(biorbd_model_path="HandSpinner.bioMod"): end_crank_idx = 0 hand_marker_idx = 18 # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Problem parameters number_shooting_points = 30 final_time = 1.0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, index=hand_marker_idx) objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add( Constraint.ALIGN_MARKERS, first_marker_idx=hand_marker_idx, second_marker_idx=end_crank_idx, node=Node.ALL ) constraints.add( Constraint.TRACK_STATE, node=Node.ALL, index=0, target=np.linspace(0, 2 * np.pi, number_shooting_points + 1), ) state_transitions = StateTransitionList() state_transitions.add(state_transition_function, phase_pre_idx=0) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # Initial guess x_init = InitialGuessList() x_init.add([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [0] * biorbd_model.nbQdot()) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [ [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ] ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, state_transitions=state_transitions, )
def prepare_ocp(model_path, phase_time, number_shooting_points, mu): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) tau_min, tau_max, tau_init = -500, 500, 0 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( Constraint.CONTACT_FORCE, max_bound=np.inf, node=Node.ALL, contact_force_idx=1, ) constraints.add( Constraint.CONTACT_FORCE, max_bound=np.inf, node=Node.ALL, contact_force_idx=2, ) constraints.add( Constraint.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=mu, ) # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q pose_at_first_node = [0, 0, -0.5, 0.5] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * nb_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * tau_mapping.reduce.len, [tau_max] * tau_mapping.reduce.len]) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, marker_velocity_or_displacement: str, marker_in_first_coordinates_system: bool, control_type: ControlType, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare an ocp that targets some marker velocities, either by finite differences or by jacobian Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time of the final node n_shooting: int The number of shooting points marker_velocity_or_displacement: str which type of tracking: finite difference ('disp') or by jacobian ('velo') marker_in_first_coordinates_system: bool If the marker to track should be expressed in the global or local reference frame control_type: ControlType The type of controls ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions if marker_in_first_coordinates_system: # Marker should follow this segment (0 velocity when compare to this one) coordinates_system_idx = 0 else: # Marker should be static in global reference frame coordinates_system_idx = -1 objective_functions = ObjectiveList() if marker_velocity_or_displacement == "disp": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, coordinates_system_idx=coordinates_system_idx, index=6, weight=1000, ) elif marker_velocity_or_displacement == "velo": objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_MARKERS_VELOCITY, index=6, weight=1000) else: raise RuntimeError( f"Wrong choice of marker_velocity_or_displacement, actual value is " f"{marker_velocity_or_displacement}, should be 'velo' or 'disp'.") # Make sure the segments actually moves (in order to test the relative speed objective) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=6, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=7, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint nq = biorbd_model.nbQ() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(nq, 2 * nq): x_bounds[0].min[i, :] = -10 x_bounds[0].max[i, :] = 10 # Initial guess x_init = InitialGuessList() x_init.add([1.5, 1.5, 0.0, 0.0, 0.7, 0.7, 0.6, 0.6]) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, control_type=control_type, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, actuator_type: int = None, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod n_shooting: int The number of shooting points final_time: float The time at final node actuator_type: int The type of actuator to use: 1 (torque activations) or 2 (torque max constraints) ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters if actuator_type and actuator_type == 1: tau_min, tau_max, tau_init = -1, 1, 0 else: 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) # Dynamics dynamics = DynamicsList() if actuator_type: if actuator_type == 1: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN) elif actuator_type == 2: dynamics.add(DynamicsFcn.TORQUE_DRIVEN) else: raise ValueError( "actuator_type is 1 (torque activations) or 2 (torque max constraints)" ) else: 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") if actuator_type == 2: constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT, node=Node.ALL_SHOOTING, min_torque=7.5) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 x_bounds[0][2, [0, -1]] = [0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path: str = "cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4(), long_optim: bool = False) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solve to use long_optim: bool If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: n_shooting = (100, 300, 100) else: n_shooting = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() 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( 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) # 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) # 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])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -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())) # 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_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()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
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 tau_mapping = BiMapping([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) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) constraints.add( ConstraintFcn.NON_SLIPPING, node=Node.ALL, 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] * tau_mapping.to_first.len, [tau_max] * tau_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.to_first.len) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, ode_solver=ode_solver, )
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
def prepare_ocp(biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK): # --- Options --- # # 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 number_shooting_points = (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(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=3) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) dynamics.add(DynamicsType.TORQUE_DRIVEN) dynamics.add(DynamicsType.TORQUE_DRIVEN) dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=2) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=3) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model[0])) x_bounds.add(QAndQDotBounds(biorbd_model[0])) x_bounds.add(QAndQDotBounds(biorbd_model[0])) x_bounds.add(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 state 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) state 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 state transition. Finally, if you want a state transition (continuous or not) between the last and the first phase (cyclicity) you can use the dedicated StateTransition.Cyclic or use a continuous set at the lase phase_pre_idx. If for some reason, you don't want the state 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. """ state_transitions = StateTransitionList() state_transitions.add(StateTransition.IMPACT, phase_pre_idx=1) state_transitions.add(custom_state_transition, phase_pre_idx=2, idx_1=1, idx_2=3) state_transitions.add(StateTransition.CYCLIC) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, state_transitions=state_transitions, )
def prepare_ocp(biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK, long_optim=False): # --- Options --- # # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: number_shooting_points = (100, 300, 100) else: number_shooting_points = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() 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) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=0) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=0) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=1) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=2) # 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])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -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())) # 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_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()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, muscle_activations_ref, contact_forces_ref, ode_solver=OdeSolver.RK4()): # 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 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTROL, key="muscles", target=muscle_activations_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=0.001) # objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="torque", weight=0.001) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True, with_contact=True) # 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] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model: biorbd.Model, final_time: float, n_shooting: int, markers_ref: np.ndarray, tau_ref: np.ndarray, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model: biorbd.Model The loaded biorbd model final_time: float The time at final node n_shooting: int The number of shooting points markers_ref: np.ndarray The markers to track tau_ref: np.ndarray The generalized forces to track ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, axis_to_track=[Axis.Y, Axis.Z], weight=100, target=markers_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the model final_time: float The time of the final node n_shooting: int The number of shooting points ode_solver: The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # 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.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt=0) # Path constraint nq = biorbd_model.nbQ() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][2, [0, -1]] = [-1.57, 1.57] x_bounds[0][nq:, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4(), weight: float = 1, min_time=0, max_time=np.inf, ) -> OptimalControlProgram: """ Prepare the optimal control program Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The initial guess for the final time n_shooting: int The number of shooting points ode_solver: OdeSolver The ode solver to use weight: float The weighting of the minimize time objective function min_time: float The minimum time allowed for the final node max_time: float The maximum time allowed for the final node Returns ------- The OptimalControlProgram ready to be solved """ # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() # A weight of -1 will maximize time objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=weight, min_bound=min_time, max_bound=max_time) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(phase_time_constraint, use_parameter): # --- Inputs --- # final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = (20, 30, 20) biorbd_model_path = TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/models/cube.bioMod" ode_solver = OdeSolver.RK4() # --- Options --- # n_phases = len(ns) # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters 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) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2) # 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.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=phase_time_constraint, ) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2 for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -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())) # 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_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()) parameters = ParameterList() if use_parameter: def my_target_function(ocp, value, target_value): return value - target_value def my_parameter_function(biorbd_model, value, extra_value): new_gravity = MX.zeros(3, 1) new_gravity[2] = value + extra_value biorbd_model.setGravity(new_gravity) min_g = -10 max_g = -6 target_g = -8 bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=target_g ) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bound_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) # ------------- # return OptimalControlProgram( biorbd_model[:n_phases], dynamics, ns, final_time[:n_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, parameters=parameters, )
def prepare_ocp(biorbd_model_path, final_time, n_shooting, x_warm=None, use_sx=False, n_threads=1): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -50, 50, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="muscles", weight=10, multi_thread=False) objective_functions.add(ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand") # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (1.0, 1.0, 0, 0) # Initial guess if x_warm is None: x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) else: x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, n_threads=n_threads, )
def prepare_ocp(biorbd_model_path: str = "cube_with_forces.bioMod", ode_solver: OdeSolver = OdeSolver.RK4()) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The 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 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # External forces. external_forces is of len 1 because there is only one phase. # The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node external_forces = [ np.repeat( np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], n_shooting, axis=2 ) ] # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, external_forces=external_forces, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "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 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][4:8, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
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_residual_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, final_time, x0, nbGT, number_shooting_points, use_SX=False, nb_threads=8, use_torque=True): # --- Options --- # # Model path biorbd_model = biorbd_model nbQ = biorbd_model.nbQ() nbGT = nbGT nbMT = biorbd_model.nbMuscleTotal() tau_min, tau_max, tau_init = -100, 100, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 activation_min, activation_max, activation_init = 0, 1, 0.2 # Add objective functions objective_functions = ObjectiveList() # Dynamics dynamics = DynamicsTypeList() if use_activation and use_torque: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) elif use_activation is not True and use_torque: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) elif use_activation and use_torque is not True: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN) elif use_activation is not True and use_torque is not True: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # State path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) if use_activation is not True: x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Control path constraint u_bounds = BoundsList() u_bounds.add([ [tau_min] * nbGT + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * nbGT + [muscle_max] * biorbd_model.nbMuscleTotal(), ]) # Initial guesses x_init = InitialGuessOption(np.tile(x0, (number_shooting_points + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME) u0 = np.array([tau_init] * nbGT + [muscle_init] * nbMT) + 0.1 u_init = InitialGuessOption(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_SX=use_SX, nb_threads=nb_threads, )
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_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) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, weight=0, phase=3) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS, weight=0, phase=3, marker_index=[0, 1]) 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(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, weight=0, phase=3, marker_index=[0, 1]) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, 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, list_index=1, ) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=2, first_marker="m0", second_marker="m1", phase=0, list_index=2) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0, list_index=3, ) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1, list_index=4, ) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2, list_index=5, ) constraints.add(custom_func_track_markers, node=Node.ALL, first_marker="m0", second_marker="m1", phase=3, list_index=6) # 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) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, phase_transitions=phase_transitions, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, initialize_near_solution: bool, ode_solver: OdeSolver = OdeSolver.RK4(), constr: bool = True, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time at the final node n_shooting: int The number of shooting points initialize_near_solution: bool If the initial guess should be almost the solution (this is merely to reduce the time of the tests) ode_solver: OdeSolver The ode solver to use constr: bool If the constraint should be applied (this is merely to reduce the time of the tests) use_sx: bool If SX CasADi variables should be used Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr: constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5) constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=Axis.X) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file n_shooting: int The number of shooting points final_time: float The time at the final node ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_MARKERS, index=1, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque( ) # biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) # Initial guesses # TODO put this in a function defined before and explain what it does, and what are the variables x = np.vstack((np.zeros( (biorbd_model.nbQ(), 2)), np.ones((biorbd_model.nbQdot(), 2)))) Arm_init_D = np.zeros((3, 2)) Arm_init_D[1, 0] = 0 Arm_init_D[1, 1] = -np.pi + 0.01 Arm_init_G = np.zeros((3, 2)) Arm_init_G[1, 0] = 0 Arm_init_G[1, 1] = np.pi - 0.01 for i in range(2): Arm_Quat_D = eul2quat(Arm_init_D[:, i]) Arm_Quat_G = eul2quat(Arm_init_G[:, i]) x[6:9, i] = Arm_Quat_D[1:] x[12, i] = Arm_Quat_D[0] x[9:12, i] = Arm_Quat_G[1:] x[13, i] = Arm_Quat_G[0] x_init = InitialGuessList() x_init.add(x, interpolation=InterpolationType.LINEAR) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def partial_ocp_parameters(n_phases): if n_phases != 1 and n_phases != 3: raise RuntimeError("n_phases should be 1 or 3") biorbd_model_path = TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/cube.bioMod" biorbd_model = biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path) n_shooting = (2, 2, 2) final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] tau_min, tau_max, tau_init = -100, 100, 0 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) if n_phases > 1: dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) if n_phases > 1: x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds.min[i, [0, -1]] = 0 bounds.max[i, [0, -1]] = 0 x_bounds[0].min[2, 0] = 0.0 x_bounds[0].max[2, 0] = 0.0 if n_phases > 1: x_bounds[2].min[2, [0, -1]] = [0.0, 1.57] x_bounds[2].max[2, [0, -1]] = [0.0, 1.57] x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) if n_phases > 1: x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) if n_phases > 1: 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()) if n_phases > 1: u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) return ( biorbd_model[:n_phases], n_shooting[:n_phases], final_time[:n_phases], time_min[:n_phases], time_max[:n_phases], tau_min, tau_max, tau_init, dynamics, x_bounds, x_init, u_bounds, u_init, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, use_SX=True): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() data_to_track = np.zeros((number_shooting_points + 1, n_q + n_qdot)) data_to_track[:, 1] = 3.14 # Add objective functions objective_functions = ObjectiveList() objective_functions.add( ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100.0, ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=1.0) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, weight=50000.0, target=data_to_track[-1:, :].T, node=Node.END, ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([torque_min] * n_tau, [torque_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_SX=use_SX, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, weight: float, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod final_time: float The time at the final node n_shooting: int The number of shooting points weight: float The weight applied to the SUPERIMPOSE_MARKERS final objective function. The bigger this number is, the greater the model will try to reach the marker. This is in relation with the other objective functions ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add( ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, first_marker_idx=0, second_marker_idx=5, weight=weight ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (0.07, 1.4, 0, 0) # Initial guess x_init = InitialGuessList() x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) # Define control path constraint muscle_min, muscle_max, muscle_init = 0, 1, 0.5 tau_min, tau_max, tau_init = -1, 1, 0 u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
u_mi = excitations_min[co] u_ma = excitations_max # Update u_init and u_bounds u_init = InitialGuessOption(np.tile(u_i, (ocp.nlp[0].ns, 1)).T, interpolation=InterpolationType.EACH_FRAME) x_init = InitialGuessOption(np.tile( np.concatenate((x_phase[0, :], [0.5] * biorbd_model.nbMuscles())), (ocp.nlp[0].ns + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME) ocp.update_initial_guess(x_init, u_init=u_init) u_bounds = BoundsOption([u_mi, u_ma], interpolation=InterpolationType.CONSTANT) x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # add muscle activation bounds x_bounds[0].concatenate( Bounds([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles())) x_bounds[0].min[:biorbd_model.nbQ(), 0] = x_phase[0, :biorbd_model.nbQ()] - 0.1 x_bounds[0].max[:biorbd_model.nbQ(), 0] = x_phase[0, :biorbd_model.nbQ()] + 0.1 ocp.update_bounds(x_bounds=x_bounds, u_bounds=u_bounds) for phase in range(1, nb_phase + 1): # sol = ocp.solve(solver=Solver.IPOPT) sol = ocp.solve(solver=Solver.ACADOS, show_online_optim=False, solver_options={