def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, time_min, time_max): # --- 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() # Add objective functions objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE} # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ({ "type": Constraint.TIME_CONSTRAINT, "minimum": time_min, "maximum": time_max, }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[:, [0, -1]] = 0 X_bounds.max[:, [0, -1]] = 0 X_bounds.min[n_q - 1, -1] = 3.14 X_bounds.max[n_q - 1, -1] = 3.14 # Initial guess X_init = InitialConditions([0] * (n_q + n_qdot)) # Define control path constraint U_bounds = Bounds(min_bound=[torque_min] * n_tau, max_bound=[torque_max] * n_tau) U_bounds.min[n_tau - 1, :] = 0 U_bounds.max[n_tau - 1, :] = 0 U_init = InitialConditions([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, )
def prepare_ocp(model_path, phase_time, number_shooting_points): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_init = -500, 500, 0 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ( { "type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1 }, { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1 / 100 }, ) # Dynamics problem_type = ProblemType.torque_driven_with_contact # Constraints constraints = () # 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 = QAndQDotBounds(biorbd_model) X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot) # Define control path constraint U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len, max_bound=[torque_max] * tau_mapping.reduce.len) U_init = InitialConditions([torque_init] * tau_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, 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, number_shooting_points, final_time, use_actuators=False, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters if use_actuators: torque_min, torque_max, torque_init = -1, 1, 0 else: torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100} # Dynamics if use_actuators: problem_type = ProblemType.torque_activations_driven else: problem_type = ProblemType.torque_driven # Constraints constraints = ( {"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker_idx": 0, "second_marker_idx": 1,}, {"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 2,}, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[3:6, [0, -1]] = 0 X_bounds.max[3:6, [0, -1]] = 0 X_bounds.min[2, [0, -1]] = [0, 1.57] X_bounds.max[2, [0, -1]] = [0, 1.57] # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, 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="cubeSym.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 torque_min, torque_max, torque_init = -100, 100, 0 all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]), Mapping([0, 1, 2])) # Add objective functions objective_functions = {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100} # Dynamics variable_type = ProblemType.torque_driven # Constraints constraints = ( {"type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1,}, {"type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2,}, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model, all_generalized_mapping) for i in range(3, 6): X_bounds.first_node_min[i] = 0 X_bounds.last_node_min[i] = 0 X_bounds.first_node_max[i] = 0 X_bounds.last_node_max[i] = 0 # Initial guess X_init = InitialConditions([0] * all_generalized_mapping.reduce.len * 2) # Define control path constraint U_bounds = Bounds( [torque_min] * all_generalized_mapping.reduce.len, [torque_max] * all_generalized_mapping.reduce.len, ) U_init = InitialConditions([torque_init] * all_generalized_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, number_shooting_points, final_time, objective_functions, X_init, U_init, X_bounds, U_bounds, constraints, ode_solver=ode_solver, all_generalized_mapping=all_generalized_mapping, show_online_optim=show_online_optim, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, nb_threads): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -1000000, 1000000, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = () # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[:, 0] = 0 X_bounds.max[:, 0] = 0 X_bounds.min[:n_q, -1] = 1 X_bounds.max[:n_q, -1] = 1 X_bounds.min[n_q:, -1] = 0 X_bounds.max[n_q:, -1] = 0 # Initial guess X_init = InitialConditions([0] * (n_q + n_qdot)) # Define control path constraint U_bounds = Bounds(min_bound=[torque_min] * n_tau, max_bound=[torque_max] * n_tau) # Control initial guess U_init = InitialConditions([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, nb_threads=nb_threads, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -1, 1, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ( {"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1}, {"type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, "weight": 1}, {"type": Objective.Mayer.ALIGN_MARKERS, "first_marker_idx": 0, "second_marker_idx": 5, "weight": 1,}, ) # Dynamics problem_type = ProblemType.muscle_activations_and_torque_driven # Constraints constraints = () # Path constraint X_bounds = QAndQDotBounds(biorbd_model) # Set the initial position X_bounds.min[:, 0] = (0.07, 1.4, 0, 0) X_bounds.max[:, 0] = (0.07, 1.4, 0, 0) # Initial guess X_init = InitialConditions([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) U_init = InitialConditions( [torque_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal() ) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, )
def prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, excitations_ref, q_ref, with_residual_torque, kin_data_to_track="markers", ): # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.5 # Add objective functions objective_functions = [ {"type": Objective.Lagrange.TRACK_MUSCLES_CONTROL, "weight": 1, "data_to_track": excitations_ref}, ] if with_residual_torque: objective_functions.append({"type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1}) if kin_data_to_track == "markers": objective_functions.append( {"type": Objective.Lagrange.TRACK_MARKERS, "weight": 100, "data_to_track": markers_ref}, ) elif kin_data_to_track == "q": objective_functions.append( { "type": Objective.Lagrange.TRACK_STATE, "weight": 100, "data_to_track": q_ref, "states_idx": range(biorbd_model.nbQ()), }, ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics if with_residual_torque: variable_type = ProblemType.muscle_excitations_and_torque_driven else: variable_type = ProblemType.muscle_excitations_driven # Constraints constraints = () # Path constraint X_bounds = QAndQDotBounds(biorbd_model) # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger X_bounds.min[[0, 1], :] = -2 * np.pi X_bounds.max[[0, 1], :] = 2 * np.pi # Add muscle to the bounds X_bounds.concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles()) ) # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) # Define control path constraint if with_residual_torque: U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(), ) U_init = InitialConditions( [torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles() ) else: U_bounds = Bounds([excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles(),) U_init = InitialConditions([excitation_init] * biorbd_model.nbMuscles()) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, nb_shooting, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, loop_from_constraint, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = [{ "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 }] # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.MID, "first_marker_idx": 0, "second_marker_idx": 2, }, { "type": Constraint.TRACK_STATE, "instant": Instant.MID, "states_idx": 2, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 1, }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[2:6, -1] = [1.57, 0, 0, 0] X_bounds.max[2:6, -1] = [1.57, 0, 0, 0] # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, ode_solver=ode_solver, is_cyclic_objective=not loop_from_constraint, is_cyclic_constraint=loop_from_constraint, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, ode_solver, initialize_near_solution): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker_idx": 0, "second_marker_idx": 4, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 5, }, { "type": Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS, "instant": Instant.ALL, "marker_idx": 1, "segment_idx": 2, "axis": (Axe.X), }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) for i in range(1, 8): if i != 3: X_bounds.min[i, [0, -1]] = 0 X_bounds.max[i, [0, -1]] = 0 X_bounds.min[2, -1] = 1.57 X_bounds.max[2, -1] = 1.57 # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): X_init.init[i] = 1.5 for i in range(4, 6): X_init.init[i] = 0.7 for i in range(6, 8): X_init.init[i] = 0.6 # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, 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, final_time, number_shooting_points, marker_ref, excitations_ref, q_ref, state_init, use_residual_torque, activation_driven, kin_data_to_track, nb_threads, use_SX=True, param=False): # --- Options --- # nb_mus = biorbd_model.nbMuscleTotal() activation_min, activation_max, activation_init = 0, 1, 0.3 excitation_min, excitation_max, excitation_init = 0, 1, 0.1 torque_min, torque_max, torque_init = -100, 100, 0 # -- Force iso ipopt pour acados # if param is not True: # fiso = [] # for k in range(nb_mus): # fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx()) # mat_content = sio.loadmat("./results/param_f_iso_flex.mat") # f_iso = mat_content["f_iso"] * mat_content["f_iso_global"] # for k in range(biorbd_model.nbMuscles()): # biorbd_model.muscle(k).characteristics().setForceIsoMax( # f_iso[k] * fiso[k] # ) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=10, target=excitations_ref) objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.01) if use_residual_torque: objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1) if kin_data_to_track == "markers": objective_functions.add( Objective.Lagrange.TRACK_MARKERS, weight=1000, target=marker_ref[:, -biorbd_model.nbMarkers():, :]) elif kin_data_to_track == "q": objective_functions.add( Objective.Lagrange.TRACK_STATE, weight=100, # target=q_ref, # states_idx=range(biorbd_model.nbQ()) ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsTypeList() if use_residual_torque: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) elif activation_driven: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN) else: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # Constraints constraints = () # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) if use_SX: x_bounds[0].min[:, 0] = np.concatenate( (state_init[6:biorbd_model.nbQ() + 6, 0], state_init[biorbd_model.nbQ() + 12:-nb_mus, 0])) x_bounds[0].max[:, 0] = np.concatenate( (state_init[6:biorbd_model.nbQ() + 6, 0], state_init[biorbd_model.nbQ() + 12:-nb_mus, 0])) # Add muscle to the bounds if activation_driven is not True: x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Initial guess x_init = InitialConditionsList() if activation_driven: # state_base = np.ndarray((12, n_shooting_points+1)) # for i in range(n_shooting_points+1): # state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6)))) x_init.add(np.concatenate( (state_init[6:biorbd_model.nbQ() + 6, :], state_init[biorbd_model.nbQ() + 12:-nb_mus, :])), interpolation=InterpolationType.EACH_FRAME) # x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME) else: # x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) x_init.add(state_init[biorbd_model.nbQ():, :], interpolation=InterpolationType.EACH_FRAME) # Add muscle to the bounds u_bounds = BoundsList() u_init = InitialConditionsList() nb_tau = 6 # init_residual_torque = np.concatenate((np.ones((biorbd_model.nbGeneralizedTorque(), n_shooting_points))*0.5, # excitations_ref)) if use_residual_torque: u_bounds.add([ [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(), ]) u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal()) # u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME) else: u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()]) if activation_driven: # u_init.add([activation_init] * biorbd_model.nbMuscleTotal()) u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) else: # u_init.add([excitation_init] * biorbd_model.nbMuscleTotal()) u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) # Get initial isometric forces fiso = [] for k in range(nb_mus): fiso.append( biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx()) # Define the parameter to optimize bound_p_iso = Bounds( # min_bound=np.repeat(0.75, nb_mus), max_bound=np.repeat(3, nb_mus), interpolation=InterpolationType.CONSTANT) min_bound=[0.5] * nb_mus + [0.75], max_bound=[3.5] * nb_mus + [3], interpolation=InterpolationType.CONSTANT) bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus), max_bound=np.repeat(0, nb_mus), interpolation=InterpolationType.CONSTANT) p_iso_init = InitialConditions([1] * nb_mus + [2]) initial_guess_A = InitialConditions([-3] * nb_mus) parameters = ParameterList() parameters.add( "p_iso", # The name of the parameter modify_isometric_force, # The function that modifies the biorbd model p_iso_init, bound_p_iso, # The bounds size=nb_mus + 1, # The number of elements this particular parameter vector has fiso_init=fiso, ) # parameters.add( # "shape_factor", # The name of the parameter # modify_shape_factor, # initial_guess_A, # bound_shape_factor, # The bounds # size=nb_mus, # The number of elements this particular parameter vector has # ) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_threads=nb_threads, use_SX=use_SX, # parameters=parameters )
def prepare_ocp( biorbd_model, final_time, number_shooting_points, marker_ref, excitations_ref, q_ref, state_ekf, use_residual_torque, kin_data_to_track, nb_threads, use_SX=False, ): # --- Options --- # nb_mus = biorbd_model.nbMuscleTotal() activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.1 torque_min, torque_max, torque_init = -100, 100, 0 # shapeFactor = [-1.17807827e-05, -1.12345339e-04, -1.06298720e-05, -4.85475442e-02, -1.90456361e-03, -2.60736365e-01, # -7.85131950e-03, -4.15346233e-02, -2.12925976e-05, # -1.48848665e-01, # -1.38652787e-01, # -2.28073508e-05, # -1.36886153e-05, # -1.75216867e-05, # -1.08256972e-01, # -1.95064804e-01, # -1.37723691e-05, # -2.42525695e-02, # -3.90772988e-04, # -3.13160043e-05] # for k in range(biorbd_model.nbMuscles()): # biorbd.StateDynamicsBuchananDeGroote(biorbd_model.muscle(k).state()).setShapeFactor(shapeFactor[k]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=1, target=excitations_ref) if use_residual_torque: objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10) if kin_data_to_track == "markers": objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=100, target=marker_ref) elif kin_data_to_track == "q": objective_functions.add(Objective.Lagrange.TRACK_STATE, weight=100, target=q_ref, states_idx=range(biorbd_model.nbQ())) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsTypeList() if use_residual_torque: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) else: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # Constraints constraints = () # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # x_bounds[0].min[:, :] = - 2 * np.pi # x_bounds[0].max[:, :] = 20 * np.pi # Add muscle to the bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Initial guess x_init = InitialConditionsList() # x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) x_init.add(state_ekf, interpolation=InterpolationType.EACH_FRAME) # Add muscle to the bounds u_bounds = BoundsList() u_init = InitialConditionsList() init_residual_torque = np.concatenate((np.ones( (biorbd_model.nbGeneralizedTorque(), n_shooting_points)) * 0.5, excitations_ref)) if use_residual_torque: u_bounds.add([ [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(), ]) # u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal()) u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME) else: u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()]) # u_init.add([excitation_init] * biorbd_model.nbMuscleTotal()) u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) # Get initial isometric forces fiso = [] for k in range(nb_mus): fiso.append( biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx()) # Define the parameter to optimize bound_p_iso = Bounds(min_bound=[0.5] * nb_mus + [0.75], max_bound=[3.5] * nb_mus + [3], interpolation=InterpolationType.CONSTANT) bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus), max_bound=np.repeat(0, nb_mus), interpolation=InterpolationType.CONSTANT) bound_A1 = Bounds(min_bound=np.repeat(0.0001, nb_mus), max_bound=np.repeat(0.12, nb_mus), interpolation=InterpolationType.CONSTANT) bound_A2 = Bounds(min_bound=np.repeat(0.01, nb_mus), max_bound=np.repeat(1e11, nb_mus), interpolation=InterpolationType.CONSTANT) p_iso_init = InitialConditions([1] * nb_mus + [2]) initial_guess_A = InitialConditions([-2] * nb_mus) initial_guess_A1 = InitialConditions([0.05] * nb_mus) initial_guess_A2 = InitialConditions([700] * nb_mus) parameters = ParameterList() # parameters.add( # "p_iso", # The name of the parameter # modify_isometric_force, # The function that modifies the biorbd model # p_iso_init, # bound_p_iso, # The bounds # size=nb_mus+1, # The number of elements this particular parameter vector has # fiso_init=fiso, # ) # parameters.add( # "shape_factor", # The name of the parameter # modify_shape_factor, # initial_guess_A, # bound_shape_factor, # The bounds # size=nb_mus, # The number of elements this particular parameter vector has # ) parameters.add( "A1", # The name of the parameter modify_A1, initial_guess_A1, bound_A1, # The bounds size= nb_mus, # The number of elements this particular parameter vector has ) parameters.add( "A2", # The name of the parameter modify_A2, initial_guess_A2, bound_A2, # The bounds size= nb_mus, # The number of elements this particular parameter vector has ) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_threads=nb_threads, use_SX=use_SX, parameters=parameters, )
def prepare_ocp(biorbd_model_path="cube_with_forces.bioMod", ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker_idx": 0, "second_marker_idx": 1, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 2, }, ) # External forces external_forces = [ np.repeat(np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], number_shooting_points, axis=2) ] # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[3:6, [0, -1]] = 0 X_bounds.max[3:6, [0, -1]] = 0 # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, 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( show_online_optim=False, use_symmetry=True, ): # --- Options --- # # Model path biorbd_model = ( biorbd.Model("jumper2contacts.bioMod"), biorbd.Model("jumper1contacts.bioMod"), ) nb_phases = len(biorbd_model) torque_min, torque_max, torque_init = -1000, 1000, 0 # Problem parameters number_shooting_points = [8, 8] phase_time = [0.4, 0.2] if use_symmetry: q_mapping = BidirectionalMapping( Mapping([0, 1, 2, -1, 3, -1, 3, 4, 5, 6, 4, 5, 6], [5]), Mapping([0, 1, 2, 4, 7, 8, 9])) q_mapping = q_mapping, q_mapping tau_mapping = BidirectionalMapping( Mapping([-1, -1, -1, -1, 0, -1, 0, 1, 2, 3, 1, 2, 3], [5]), Mapping([4, 7, 8, 9])) tau_mapping = tau_mapping, tau_mapping else: q_mapping = BidirectionalMapping( Mapping([i for i in range(biorbd_model[0].nbQ())]), Mapping([i for i in range(biorbd_model[0].nbQ())]), ) q_mapping = q_mapping, q_mapping tau_mapping = q_mapping # Add objective functions objective_functions = ( (), ( { "type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1 }, { "type": Objective.Lagrange.MINIMIZE_ALL_CONTROLS, "weight": 1 / 100 }, ), ) # Dynamics problem_type = ( ProblemType.torque_driven_with_contact, ProblemType.torque_driven_with_contact, ) constraints_first_phase = [] constraints_second_phase = [] contact_axes = (1, 2, 4, 5) for i in contact_axes: constraints_first_phase.append({ "type": Constraint.CONTACT_FORCE_GREATER_THAN, "instant": Instant.ALL, "idx": i, "boundary": 0, }) contact_axes = (1, 3) for i in contact_axes: constraints_second_phase.append({ "type": Constraint.CONTACT_FORCE_GREATER_THAN, "instant": Instant.ALL, "idx": i, "boundary": 0, }) constraints_first_phase.append({ "type": Constraint.NON_SLIPPING, "instant": Instant.ALL, "normal_component_idx": (1, 2, 4, 5), "tangential_component_idx": (0, 3), "static_friction_coefficient": 0.5, }) constraints_second_phase.append({ "type": Constraint.NON_SLIPPING, "instant": Instant.ALL, "normal_component_idx": (1, 3), "tangential_component_idx": (0, 2), "static_friction_coefficient": 0.5, }) if not use_symmetry: first_dof = (3, 4, 7, 8, 9) second_dof = (5, 6, 10, 11, 12) coeff = (-1, 1, 1, 1, 1) for i in range(len(first_dof)): constraints_first_phase.append({ "type": Constraint.PROPORTIONAL_STATE, "instant": Instant.ALL, "first_dof": first_dof[i], "second_dof": second_dof[i], "coef": coeff[i], }) for i in range(len(first_dof)): constraints_second_phase.append({ "type": Constraint.PROPORTIONAL_STATE, "instant": Instant.ALL, "first_dof": first_dof[i], "second_dof": second_dof[i], "coef": coeff[i], }) constraints = (constraints_first_phase, constraints_second_phase) # Path constraint if use_symmetry: nb_q = q_mapping[0].reduce.len nb_qdot = nb_q pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47] else: nb_q = q_mapping[0].reduce.len nb_qdot = nb_q pose_at_first_node = [ 0, 0, -0.5336, 0, 1.4, 0, 1.4, 0.8, -0.9, 0.47, 0.8, -0.9, 0.47, ] # Initialize X_bounds X_bounds = [ QAndQDotBounds(biorbd_model[i], all_generalized_mapping=q_mapping[i]) for i in range(nb_phases) ] X_bounds[0].first_node_min = pose_at_first_node + [0] * nb_qdot X_bounds[0].first_node_max = pose_at_first_node + [0] * nb_qdot # Initial guess X_init = [ InitialConditions(pose_at_first_node + [0] * nb_qdot), InitialConditions(pose_at_first_node + [0] * nb_qdot), ] # Define control path constraint U_bounds = [ Bounds(min_bound=[torque_min] * tau_m.reduce.len, max_bound=[torque_max] * tau_m.reduce.len) for tau_m in tau_mapping ] U_init = [ InitialConditions([torque_init] * tau_m.reduce.len) for tau_m in tau_mapping ] # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, phase_time, objective_functions, X_init, U_init, X_bounds, U_bounds, constraints, q_mapping=q_mapping, q_dot_mapping=q_mapping, tau_mapping=tau_mapping, show_online_optim=show_online_optim, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, use_SX=False, nb_threads=1): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -5, 5, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=100) objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # Add muscle to the bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Following values are taken from Belaise's matlab code x_bounds[0].min[:, 0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15, -0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles() x_bounds[0].max[:, 0] = (-0.2, 0.1, -0.25, 0.1, 0, -0, -0.2, 0.05, -0.15, -0.02, 0, 0.28) + (0, ) * biorbd_model.nbMuscles() x_bounds[0].min[:biorbd_model.nbQ() * 2, -1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17, -0.62, 1.4, -0.57) x_bounds[0].max[:biorbd_model.nbQ() * 2, -1] = (-0.03, 0.1, -0.1, 0.2, -0.76, 1., 2., -1.5, -0.17, -0.62, 1.4, -0.57) # Initial guess x_init = InitialConditionsList() x_init.add([1.] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot() + [0] * biorbd_model.nbMuscles()) # Define control path constraint u_bounds = BoundsList() u_init = InitialConditionsList() u_bounds.add([ [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(), ]) u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()) # ------------- # 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(biorbd_model_path="../models/BrasViolon.bioMod"): """ Mix .bioMod and users data to call OptimalControlProgram constructor. :param biorbd_model_path: path to the .bioMod file. :param show_online_optim: bool which active live plot function. :return: OptimalControlProgram object. """ optimal_initial_values = False nb_phases = 2 biorbd_model = [] objective_functions = ObjectiveList() dynamics = DynamicsTypeList() constraints = ConstraintList() x_bounds = BoundsList() u_bounds = BoundsList() x_init = InitialConditionsList() u_init = InitialConditionsList() # --- Options --- # number_shooting_points = [20] * nb_phases final_time = [1] * nb_phases muscle_activated_init, muscle_fatigued_init, muscle_resting_init = 0, 0, 1 torque_min, torque_max, torque_init = -10, 10, 0 muscle_states_min, muscle_states_max = 0, 1 # --- Aliasing --- # model_tp = biorbd.Model(biorbd_model_path) n_q = model_tp.nbQ() n_qdot = model_tp.nbQdot() n_tau = model_tp.nbGeneralizedTorque() n_mus = model_tp.nbMuscles() violon_string = Violin("G") inital_bow_side = Bow("frog") # --- External forces --- # external_forces = [ np.repeat(violon_string.external_force[:, np.newaxis], number_shooting_points[0], axis=1) ] * nb_phases for idx_phase in range(nb_phases): biorbd_model.append(biorbd.Model(biorbd_model_path)) # --- Objective --- # objective_functions.add(Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=1, phase=idx_phase) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1, phase=idx_phase) objective_functions.add( Objective.Lagrange.ALIGN_SEGMENT_WITH_CUSTOM_RT, weight=1, segment_idx=Bow.segment_idx, rt_idx=violon_string.rt_on_string, phase=idx_phase, ) objective_functions.add( Objective.Lagrange.MINIMIZE_TORQUE, controls_idx=[0, 1, 2, 3], weight=10, phase=idx_phase ) # --- Dynamics --- # dynamics.add(xia.xia_model_configuration, dynamic_function=xia.xia_model_dynamic) # --- Constraints --- # if idx_phase == 0: constraints.add( Constraint.ALIGN_MARKERS, instant=Instant.START, min_bound=-0.00001, max_bound=0.00001, first_marker_idx=Bow.frog_marker, second_marker_idx=violon_string.bridge_marker, phase=idx_phase, ) constraints.add( Constraint.ALIGN_MARKERS, instant=Instant.MID, min_bound=-0.00001, max_bound=0.00001, first_marker_idx=Bow.tip_marker, second_marker_idx=violon_string.bridge_marker, phase=idx_phase, ) constraints.add( Constraint.ALIGN_MARKERS, instant=Instant.END, min_bound=-0.00001, max_bound=0.00001, first_marker_idx=Bow.frog_marker, second_marker_idx=violon_string.bridge_marker, phase=idx_phase, ) # constraints.add( # Constraint.ALIGN_SEGMENT_WITH_CUSTOM_RT, # instant=Instant.ALL, # min_bound=-0.00001, # max_bound=0.00001, # segment_idx=Bow.segment_idx, # rt_idx=violon_string.rt_on_string, # phase=idx_phase, # ) # constraints.add( # Constraint.ALIGN_MARKER_WITH_SEGMENT_AXIS, # instant=Instant.ALL, # min_bound=-0.00001, # max_bound=0.00001, # marker_idx=violon_string.bridge_marker, # segment_idx=Bow.segment_idx, # axis=(Axe.Y), # phase=idx_phase, # ) constraints.add( Constraint.ALIGN_MARKERS, instant=Instant.ALL, first_marker_idx=Bow.contact_marker, second_marker_idx=violon_string.bridge_marker, phase=idx_phase, # TODO: add constraint about velocity in a marker of bow (start and end instant) ) # --- Path constraints --- # x_bounds.add(QAndQDotBounds(biorbd_model[0]), phase=idx_phase) # Start and finish with zero velocity if idx_phase == 0: x_bounds[idx_phase][n_q :, 0] = 0 if idx_phase == nb_phases - 1: x_bounds[idx_phase][n_q :, -1] = 0 muscle_states_bounds = Bounds( [muscle_states_min] * n_mus * 3, [muscle_states_max] * n_mus * 3, ) if idx_phase == 0: # fatigued_fibers = activated_fibers = 0 and resting_fibers = 1 at start muscle_states_bounds[:2 * n_mus, 0] = 0 muscle_states_bounds[2 * n_mus:, 0] = 1 x_bounds[idx_phase].concatenate(muscle_states_bounds) u_bounds.add( [[torque_min] * n_tau + [muscle_states_min] * n_mus, [torque_max] * n_tau + [muscle_states_max] * n_mus], phase=idx_phase, ) # --- Initial guess --- # if optimal_initial_values: # TODO: Fix this part (avoid using .bio) raise NotImplementedError("optimal_initial_values = True should be reviewed") if idx_phase == 0: with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file: dict = pickle.load(file) else: with open(f"utils/optimal_init_{number_shooting_points[0]}_nodes_constr_first.bio", "rb") as file: dict = pickle.load(file) x_init.add(dict["states"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase) u_init.add(dict["controls"], interpolation=InterpolationType.EACH_FRAME, phase=idx_phase) else: # TODO: x_init could be a LINEAR from frog to tip x_init.add( violon_string.initial_position()[inital_bow_side.side] + [0] * n_qdot, interpolation=InterpolationType.CONSTANT, phase=idx_phase, ) muscle_states_init = InitialConditions( [muscle_activated_init] * n_mus + [muscle_fatigued_init] * n_mus + [muscle_resting_init] * n_mus, interpolation=InterpolationType.CONSTANT, ) x_init[idx_phase].concatenate(muscle_states_init) u_init.add( [torque_init] * n_tau + [muscle_activated_init] * n_mus, interpolation=InterpolationType.CONSTANT, phase=idx_phase, ) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, external_forces=external_forces, ode_solver=OdeSolver.RK, nb_threads=4, )
def prepare_ocp(model_path, phase_time, number_shooting_points, direction, boundary): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_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 = ({ "type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1 }, ) # Dynamics problem_type = ProblemType.muscle_excitations_and_torque_driven_with_contact # Constraints constraints = ( { "type": Constraint.CONTACT_FORCE_INEQUALITY, "direction": direction, "instant": Instant.ALL, "contact_force_idx": 1, "boundary": boundary, }, { "type": Constraint.CONTACT_FORCE_INEQUALITY, "direction": direction, "instant": Instant.ALL, "contact_force_idx": 2, "boundary": boundary, }, ) # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q nb_mus = biorbd_model.nbMuscleTotal() pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize X_bounds X_bounds = QAndQDotBounds(biorbd_model) X_bounds.concatenate( Bounds([activation_min] * nb_mus, [activation_max] * nb_mus)) X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus # Initial guess X_init = [ InitialConditions(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus) ] # Define control path constraint U_bounds = [ Bounds( min_bound=[torque_min] * tau_mapping.reduce.len + [activation_min] * biorbd_model.nbMuscleTotal(), max_bound=[torque_max] * tau_mapping.reduce.len + [activation_max] * biorbd_model.nbMuscleTotal(), ) ] U_init = [ InitialConditions([torque_init] * tau_mapping.reduce.len + [activation_init] * biorbd_model.nbMuscleTotal()) ] # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, phase_time, X_init, U_init, X_bounds, U_bounds, objective_functions=objective_functions, constraints=constraints, tau_mapping=tau_mapping, )
def prepare_ocp(model_path, phase_time, number_shooting_points, mu): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_init = -500, 500, 0 tau_mapping = BidirectionalMapping(Mapping([-1, -1, -1, 0]), Mapping([3])) # Add objective functions objective_functions = ({"type": Objective.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, "weight": -1},) # Dynamics problem_type = ProblemType.torque_driven_with_contact # Constraints constraints = ( { "type": Constraint.CONTACT_FORCE_INEQUALITY, "direction": "GREATER_THAN", "instant": Instant.ALL, "contact_force_idx": 1, "boundary": 0, }, { "type": Constraint.CONTACT_FORCE_INEQUALITY, "direction": "GREATER_THAN", "instant": Instant.ALL, "contact_force_idx": 2, "boundary": 0, }, { "type": Constraint.NON_SLIPPING, "instant": Instant.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 = QAndQDotBounds(biorbd_model) X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess X_init = InitialConditions(pose_at_first_node + [0] * nb_qdot) # Define control path constraint U_bounds = Bounds(min_bound=[torque_min] * tau_mapping.reduce.len, max_bound=[torque_max] * tau_mapping.reduce.len) U_init = InitialConditions([torque_init] * tau_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, 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, ode_solver, marker_velocity_or_displacement, marker_in_first_coordinates_system, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() biorbd_model.markerNames() # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions if marker_in_first_coordinates_system: coordinates_system_idx = 0 else: coordinates_system_idx = -1 if marker_velocity_or_displacement == "disp": objective_functions = ( { "type": Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, "coordinates_system_idx": coordinates_system_idx, "markers_idx": 6, "weight": 1000, }, { "type": Objective.Lagrange.MINIMIZE_STATE, "states_idx": 6, "weight": -1 }, { "type": Objective.Lagrange.MINIMIZE_STATE, "states_idx": 7, "weight": -1 }, ) elif marker_velocity_or_displacement == "velo": objective_functions = ( { "type": Objective.Lagrange.MINIMIZE_MARKERS_VELOCITY, "markers_idx": 6, "weight": 1000 }, { "type": Objective.Lagrange.MINIMIZE_STATE, "states_idx": 6, "weight": -1 }, { "type": Objective.Lagrange.MINIMIZE_STATE, "states_idx": 7, "weight": -1 }, ) else: raise RuntimeError( "Wrong choice of marker_velocity_or_displacement, actual value is " "{marker_velocity_or_displacement}, should be 'velo' or 'disp'.") # Dynamics problem_type = ProblemType.torque_driven # Path constraint X_bounds = QAndQDotBounds(biorbd_model) for i in range(nq, 2 * nq): X_bounds.min[i, :] = -10 X_bounds.max[i, :] = 10 # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) for i in range(2): X_init.init[i] = 1.5 for i in range(4, 6): X_init.init[i] = 0.7 for i in range(6, 8): X_init.init[i] = 0.6 # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics variable_type = ProblemType.torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2, }, { "type": Constraint.PROPORTIONAL_STATE, "instant": Instant.ALL, "first_dof": 2, "second_dof": 3, "coef": -1, }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) for i in range(4, 8): X_bounds.first_node_min[i] = 0 X_bounds.last_node_min[i] = 0 X_bounds.first_node_max[i] = 0 X_bounds.last_node_max[i] = 0 # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbQ(), [torque_max] * biorbd_model.nbQ(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, number_shooting_points, final_time, objective_functions, X_init, U_init, X_bounds, U_bounds, constraints, ode_solver=ode_solver, show_online_optim=show_online_optim, )
def prepare_ocp( biorbd_model, final_time, number_shooting_points, marker_ref, excitations_ref, q_ref, state_init, use_residual_torque, activation_driven, kin_data_to_track, nb_threads, use_SX=True, ): # --- Options --- # nb_mus = biorbd_model.nbMuscleTotal() activation_min, activation_max, activation_init = 0, 1, 0.3 excitation_min, excitation_max, excitation_init = 0, 1, 0.1 torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=10, target=excitations_ref) objective_functions.add(Objective.Lagrange.MINIMIZE_STATE, weight=0.01) if use_residual_torque: objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1) if kin_data_to_track == "markers": objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=1000, target=marker_ref[:, -biorbd_model.nbMarkers():, :] ) elif kin_data_to_track == "q": objective_functions.add( Objective.Lagrange.TRACK_STATE, weight=100, # target=q_ref, # states_idx=range(biorbd_model.nbQ()) ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsTypeList() if use_residual_torque: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) elif activation_driven: dynamics.add(DynamicsType.MUSCLE_ACTIVATIONS_DRIVEN) else: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # Constraints constraints = () # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) if use_SX: x_bounds[0].min[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0], state_init[biorbd_model.nbQ() + 12:-nb_mus, 0])) x_bounds[0].max[:, 0] = np.concatenate((state_init[6:biorbd_model.nbQ() + 6, 0], state_init[biorbd_model.nbQ() + 12:-nb_mus, 0])) # Add muscle to the bounds if activation_driven is not True: x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles()) ) # Initial guess x_init = InitialConditionsList() if activation_driven: # state_base = np.ndarray((12, n_shooting_points+1)) # for i in range(n_shooting_points+1): # state_base[:, i] = np.concatenate((state_init[:6, 0], np.zeros((6)))) x_init.add(np.concatenate((state_init[6:biorbd_model.nbQ() + 6, :], state_init[biorbd_model.nbQ() + 12:-nb_mus, :])), interpolation=InterpolationType.EACH_FRAME) # x_init.add(state_init[:-nb_mus, :], interpolation=InterpolationType.EACH_FRAME) else: # x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) x_init.add(state_init[biorbd_model.nbQ():, :], interpolation=InterpolationType.EACH_FRAME) # Add muscle to the bounds u_bounds = BoundsList() u_init = InitialConditionsList() nb_tau = 6 # init_residual_torque = np.concatenate((np.ones((biorbd_model.nbGeneralizedTorque(), n_shooting_points))*0.5, # excitations_ref)) if use_residual_torque: u_bounds.add( [ [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(), ] ) u_init.add( [torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal()) # u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME) else: u_bounds.add( [[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()]) if activation_driven: # u_init.add([activation_init] * biorbd_model.nbMuscleTotal()) u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) else: # u_init.add([excitation_init] * biorbd_model.nbMuscleTotal()) u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) # Get initial isometric forces fiso = [] for k in range(nb_mus): fiso.append(biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_threads=nb_threads, use_SX=use_SX, # parameters=parameters )
def prepare_ocp(biorbd_model_path, number_shooting_points, final_time, initial_guess=InterpolationType.CONSTANT): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() nqdot = biorbd_model.nbQdot() ntau = biorbd_model.nbGeneralizedTorque() torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker_idx": 0, "second_marker_idx": 1, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker_idx": 0, "second_marker_idx": 2, }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[1:6, [0, -1]] = 0 X_bounds.max[1:6, [0, -1]] = 0 X_bounds.min[2, -1] = 1.57 X_bounds.max[2, -1] = 1.57 # Define control path constraint U_bounds = Bounds( [torque_min] * ntau, [torque_max] * ntau, ) # Initial guesses if initial_guess == InterpolationType.CONSTANT: x = [0] * (nq + nqdot) u = [torque_init] * ntau elif initial_guess == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [1.5, 0.0, 0.785, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T u = np.array([[1.45, 9.81, 2.28], [0, 9.81, 0], [-1.45, 9.81, -2.28]]).T elif initial_guess == InterpolationType.LINEAR: x = np.array([[1.0, 0.0, 0.0, 0, 0, 0], [2.0, 0.0, 1.57, 0, 0, 0]]).T u = np.array([[1.45, 9.81, 2.28], [-1.45, 9.81, -2.28]]).T elif initial_guess == InterpolationType.EACH_FRAME: x = np.random.random((nq + nqdot, number_shooting_points + 1)) u = np.random.random((ntau, number_shooting_points)) else: raise RuntimeError("Initial guess not implemented yet") X_init = InitialConditions(x, interpolation_type=initial_guess) U_init = InitialConditions(u, interpolation_type=initial_guess) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -30, 30, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10) # Dynamics dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds.min[:, [0, -1]] = 0 x_bounds.max[:, [0, -1]] = 0 x_bounds.min[1, -1] = 3.14 x_bounds.max[1, -1] = 3.14 # Initial guess x_init = InitialConditionsOption([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau]) u_bounds.min[1, :] = 0 u_bounds.max[1, :] = 0 u_init = InitialConditionsOption([tau_init] * n_tau) # Define the parameter to optimize # Give the parameter some min and max bounds parameters = ParameterList() bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialConditions((min_g + max_g) / 2) parameter_objective_functions = ObjectiveOption( my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target_value=target_g) parameters.add( "gravity_z", # The name of the parameter my_parameter_function, # The function that modifies the biorbd model initial_gravity, # The initial guess bound_gravity, # The bounds size=1, # The number of elements this particular parameter vector has penalty_list= parameter_objective_functions, # Objective of constraint for this particular parameter extra_value=1, # You can define as many extra arguments as you want ) return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, )
def prepare_ocp(model_path, phase_time, number_shooting_points, direction, boundary): # --- Options --- # # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_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, weight=-1) # Dynamics dynamics = DynamicsTypeList() dynamics.add( DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( Constraint.CONTACT_FORCE_INEQUALITY, direction=direction, instant=Instant.ALL, contact_force_idx=1, boundary=boundary, ) constraints.add( Constraint.CONTACT_FORCE_INEQUALITY, direction=direction, instant=Instant.ALL, contact_force_idx=2, boundary=boundary, ) # Path constraint nb_q = biorbd_model.nbQ() nb_qdot = nb_q nb_mus = biorbd_model.nbMuscleTotal() 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].concatenate( Bounds([activation_min] * nb_mus, [activation_max] * nb_mus)) x_bounds[0].min[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus x_bounds[0].max[:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus # Initial guess x_init = InitialConditionsList() x_init.add(pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus) # Define control path constraint u_bounds = BoundsList() u_bounds.add([ [torque_min] * tau_mapping.reduce.len + [activation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * tau_mapping.reduce.len + [activation_max] * biorbd_model.nbMuscleTotal(), ]) u_init = InitialConditionsList() u_init.add([torque_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=objective_functions, constraints=constraints, tau_mapping=tau_mapping, )
def prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, excitations_ref, q_ref, kin_data_to_track="markers", show_online_optim=False, ): # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.5 # Add objective functions objective_functions = [ { "type": Objective.Lagrange.TRACK_MUSCLES_CONTROL, "weight": 1, "data_to_track": excitations_ref }, { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 10000 }, ] if kin_data_to_track == "markers": objective_functions.append( { "type": Objective.Lagrange.TRACK_MARKERS, "weight": 100, "data_to_track": markers_ref }, ) elif kin_data_to_track == "q": objective_functions.append( { "type": Objective.Lagrange.TRACK_STATE, "weight": 100, "data_to_track": q_ref, "states_idx": range(biorbd_model.nbQ()), }, ) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics variable_type = ProblemType.muscle_excitations_and_torque_driven # Constraints constraints = () # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.first_node_min += [activation_min] * biorbd_model.nbMuscleTotal() X_bounds.first_node_max += [activation_max] * biorbd_model.nbMuscleTotal() X_bounds.min += [activation_min] * biorbd_model.nbMuscleTotal() X_bounds.max += [activation_max] * biorbd_model.nbMuscleTotal() X_bounds.last_node_min += [activation_min] * biorbd_model.nbMuscleTotal() X_bounds.last_node_max += [activation_max] * biorbd_model.nbMuscleTotal() # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [activation_init] * biorbd_model.nbMuscleTotal()) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(), ) U_init = InitialConditions( [torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, nb_shooting, final_time, objective_functions, X_init, U_init, X_bounds, U_bounds, constraints, show_online_optim=show_online_optim, )
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) torque_min, torque_max, torque_init = -100, 100, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Problem parameters number_shooting_points = 30 final_time = 1.5 # Add objective functions objective_functions = ( { "type": Objective.Lagrange.MINIMIZE_MARKERS_DISPLACEMENT, "weight": 1, "markers_idx": hand_marker_idx }, { "type": Objective.Lagrange.MINIMIZE_MUSCLES_CONTROL, "weight": 1 }, { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 1 }, ) # Dynamics problem_type = ProblemType.muscle_activations_and_torque_driven # Constraints constraints = ( { "type": Constraint.ALIGN_MARKERS, "first_marker_idx": hand_marker_idx, "second_marker_idx": end_crank_idx, "instant": Instant.ALL, }, { "type": Constraint.TRACK_STATE, "instant": Instant.ALL, "states_idx": 0, "data_to_track": np.linspace(0, 2 * np.pi, number_shooting_points + 1), }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) # Initial guess X_init = InitialConditions([0, -0.9, 1.7, 0.9, 2.0, -1.3] + [0] * biorbd_model.nbQdot()) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, final_time, X_init, U_init, X_bounds, U_bounds, objective_functions, constraints, is_cyclic_objective=True, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, ode_solver): # --- Options --- #nq # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() # Problem parameters torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = { "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 } # Dynamics problem_type = ProblemType.torque_driven # Constraints constraints = ({ "type": Constraint.ALIGN_SEGMENT_WITH_CUSTOM_RT, "instant": Instant.ALL, "segment_idx": 2, "rt_idx": 0, }, ) # Path constraint X_bounds = QAndQDotBounds(biorbd_model) X_bounds.min[2, [0, -1]] = [-1.57, 1.57] X_bounds.max[2, [0, -1]] = [-1.57, 1.57] X_bounds.min[nq:, [0, -1]] = 0 X_bounds.max[nq:, [0, -1]] = 0 # Initial guess X_init = InitialConditions([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint U_bounds = Bounds( [torque_min] * biorbd_model.nbGeneralizedTorque(), [torque_max] * biorbd_model.nbGeneralizedTorque(), ) U_init = InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, problem_type, 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, final_time, number_shooting_points, marker_ref, excitations_ref, q_ref, state_ekf, use_residual_torque, kin_data_to_track, nb_threads, use_SX=True, ): # --- Options --- # nb_mus = biorbd_model.nbMuscleTotal() activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.1 torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, weight=0.001, target=excitations_ref) if use_residual_torque: objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=10) if kin_data_to_track == "markers": objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=1, target=marker_ref) elif kin_data_to_track == "q": objective_functions.add(Objective.Lagrange.TRACK_STATE, weight=100, target=q_ref, states_idx=range(biorbd_model.nbQ())) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsTypeList() if use_residual_torque: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) else: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # Add muscle to the bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) x_bounds[0].min[:, 0] = 0 # state_ekf[:, 0] x_bounds[0].max[:, 0] = 0 # state_ekf[:, 0] # Initial guess x_init = InitialConditionsList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) # x_init.add(state_ekf, interpolation=InterpolationType.EACH_FRAME) # Add muscle to the bounds u_bounds = BoundsList() u_init = InitialConditionsList() init_residual_torque = np.concatenate((np.ones( (biorbd_model.nbGeneralizedTorque(), n_shooting_points)) * 0.5, excitations_ref)) if use_residual_torque: u_bounds.add([ [torque_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscleTotal(), [torque_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscleTotal(), ]) # u_init.add([torque_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscleTotal()) u_init.add(init_residual_torque, interpolation=InterpolationType.EACH_FRAME) else: u_bounds.add([[excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()]) u_init.add([0] * biorbd_model.nbMuscleTotal()) # u_init.add(excitations_ref, interpolation=InterpolationType.EACH_FRAME) # Get initial isometric forces fiso = [] for k in range(nb_mus): fiso.append( biorbd_model.muscle(k).characteristics().forceIsoMax().to_mx()) # Define the parameter to optimize bound_p_iso = Bounds(min_bound=np.repeat(0.5, nb_mus + 1), max_bound=np.repeat(3.5, nb_mus + 1), interpolation=InterpolationType.CONSTANT) bound_shape_factor = Bounds(min_bound=np.repeat(-3, nb_mus), max_bound=np.repeat(0, nb_mus), interpolation=InterpolationType.CONSTANT) p_iso_init = InitialConditions(np.repeat(1, nb_mus + 1)) initial_guess_A = InitialConditions([-3] * nb_mus) parameters = ParameterList() parameters.add( "p_iso", # The name of the parameter modify_isometric_force, # The function that modifies the biorbd model p_iso_init, bound_p_iso, # The bounds size=nb_mus + 1, # The number of elements this particular parameter vector has fiso_init=fiso, ) # parameters.add( # "shape_factor", # The name of the parameter # modify_shape_factor, # initial_guess_A, # bound_shape_factor, # The bounds # size=nb_mus, # The number of elements this particular parameter vector has # ) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, nb_threads=nb_threads, use_SX=use_SX, # parameters=parameters, )
def prepare_ocp( model_path, phase_time, number_shooting_points, muscle_activations_ref, contact_forces_ref, ): # Model path biorbd_model = biorbd.Model(model_path) torque_min, torque_max, torque_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 # Add objective functions objective_functions = ( { "type": Objective.Lagrange.TRACK_MUSCLES_CONTROL, "weight": 1, "data_to_track": muscle_activations_ref }, { "type": Objective.Lagrange.TRACK_CONTACT_FORCES, "weight": 1, "data_to_track": contact_forces_ref }, ) # Dynamics problem_type = ProblemType.muscles_activations_and_torque_driven_with_contact # Constraints constraints = () # 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 = QAndQDotBounds(biorbd_model) X_bounds.min[:, 0] = pose_at_first_node + [0] * nb_qdot X_bounds.max[:, 0] = pose_at_first_node + [0] * nb_qdot # Initial guess X_init = [InitialConditions(pose_at_first_node + [0] * nb_qdot)] # Define control path constraint U_bounds = [ Bounds( min_bound=[torque_min] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(), max_bound=[torque_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(), ) ] U_init = [ InitialConditions([torque_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) ] # ------------- # return OptimalControlProgram( biorbd_model, problem_type, number_shooting_points, phase_time, X_init, U_init, X_bounds, U_bounds, objective_functions=objective_functions, constraints=constraints, )
def prepare_ocp(biorbd_model_path="cube.bioMod", show_online_optim=False, ode_solver=OdeSolver.RK, long_optim=False): # --- Options --- # # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: number_shooting_points = (100, 1000) else: number_shooting_points = (20, 30) final_time = (2, 5) torque_min, torque_max, torque_init = -100, 100, 0 # Add objective functions objective_functions = ( ({ "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 }, ), ({ "type": Objective.Lagrange.MINIMIZE_TORQUE, "weight": 100 }, ), ) # Dynamics variable_type = (ProblemType.torque_driven, ProblemType.torque_driven) # Constraints constraints = ( ( { "type": Constraint.ALIGN_MARKERS, "instant": Instant.START, "first_marker": 0, "second_marker": 1, }, { "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 2, }, ), ({ "type": Constraint.ALIGN_MARKERS, "instant": Instant.END, "first_marker": 0, "second_marker": 1, }, ), ) # Path constraint X_bounds = [ QAndQDotBounds(biorbd_model[0]), QAndQDotBounds(biorbd_model[0]) ] for bounds in X_bounds: for i in range(6): if i != 0 and i != 2: bounds.first_node_min[i] = 0 bounds.last_node_min[i] = 0 bounds.first_node_max[i] = 0 bounds.last_node_max[i] = 0 X_bounds[0].first_node_min[2] = 0.0 X_bounds[0].first_node_max[2] = 0.0 X_bounds[1].first_node_min[2] = 0.0 X_bounds[1].first_node_max[2] = 0.0 X_bounds[1].last_node_min[2] = 1.57 X_bounds[1].last_node_max[2] = 1.57 # Initial guess X_init = InitialConditions( [0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint U_bounds = [ Bounds( [torque_min] * biorbd_model[0].nbGeneralizedTorque(), [torque_max] * biorbd_model[0].nbGeneralizedTorque(), ), Bounds( [torque_min] * biorbd_model[0].nbGeneralizedTorque(), [torque_max] * biorbd_model[0].nbGeneralizedTorque(), ), ] U_init = InitialConditions([torque_init] * biorbd_model[0].nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, variable_type, number_shooting_points, final_time, objective_functions, (X_init, X_init), (U_init, U_init), X_bounds, U_bounds, constraints, ode_solver=ode_solver, show_online_optim=show_online_optim, )
def prepare_ocp( biorbd_model, final_time, nb_shooting, markers_ref, excitations_ref, q_ref, use_residual_torque, kin_data_to_track="markers", ): # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 excitation_min, excitation_max, excitation_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.TRACK_MUSCLES_CONTROL, target=excitations_ref) if use_residual_torque: objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE) if kin_data_to_track == "markers": objective_functions.add(Objective.Lagrange.TRACK_MARKERS, weight=100, target=markers_ref) elif kin_data_to_track == "q": objective_functions.add(Objective.Lagrange.TRACK_STATE, weight=100, target=q_ref, states_idx=range(biorbd_model.nbQ())) else: raise RuntimeError("Wrong choice of kin_data_to_track") # Dynamics dynamics = DynamicsTypeList() if use_residual_torque: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN) else: dynamics.add(DynamicsType.MUSCLE_EXCITATIONS_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) # Due to unpredictable movement of the forward dynamics that generated the movement, the bound must be larger x_bounds[0].min[[0, 1], :] = -2 * np.pi x_bounds[0].max[[0, 1], :] = 2 * np.pi # Add muscle to the bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles())) # Initial guess x_init = InitialConditionsList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot()) + [0] * biorbd_model.nbMuscles()) # Define control path constraint u_bounds = BoundsList() u_init = InitialConditionsList() if use_residual_torque: u_bounds.add([ [tau_min] * biorbd_model.nbGeneralizedTorque() + [excitation_min] * biorbd_model.nbMuscles(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [excitation_max] * biorbd_model.nbMuscles(), ]) u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [excitation_init] * biorbd_model.nbMuscles()) else: u_bounds.add([[excitation_min] * biorbd_model.nbMuscles(), [excitation_max] * biorbd_model.nbMuscles()]) u_init.add([excitation_init] * biorbd_model.nbMuscles()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, nb_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, )