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="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, 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, 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 test_initial_condition_constant(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random(nb_elements, ) init = InitialConditions(init_val, interpolation=InterpolationType.CONSTANT) init.check_and_adjust_dimensions(nb_elements, nb_shoot) expected_val = init_val for i in range(nb_shoot): np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
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 test_initial_condition_linear(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random((nb_elements, 2)) init = InitialConditions(init_val, interpolation=InterpolationType.LINEAR) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_condition_each_frame(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random((nb_elements, nb_shoot + 1)) init = InitialConditions(init_val, interpolation=InterpolationType.EACH_FRAME) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): expected_val = init_val[:, i] np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
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 test_initial_condition_constant_with_first_and_last_different(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random((nb_elements, 3)) init = InitialConditions( init_val, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): if i == 0: expected_val = init_val[:, 0] elif i == nb_shoot: expected_val = init_val[:, 2] else: expected_val = init_val[:, 1] np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_simulate_from_initial_single_shoot(): # Load pendulum PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "pendulum", str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.py") pendulum = importlib.util.module_from_spec(spec) spec.loader.exec_module(pendulum) ocp = pendulum.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/getting_started/pendulum.bioMod", final_time=2, number_shooting_points=10, nb_threads=4, ) X = InitialConditions([-1, -2, 1, 0.5]) U = InitialConditions(np.array([[-0.1, 0], [1, 2]]).T, interpolation=InterpolationType.LINEAR) sol_simulate_single_shooting = Simulate.from_controls_and_initial_states( ocp, X, U, single_shoot=True) # Check some of the results states, controls = Data.get_data(ocp, sol_simulate_single_shooting["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0))) np.testing.assert_almost_equal(q[:, -1], np.array( (-0.59371229, 2.09731719))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5))) np.testing.assert_almost_equal(qdot[:, -1], np.array((1.38153013, -0.60425128))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0))) np.testing.assert_almost_equal(tau[:, -1], np.array((1.0, 2.0)))
def test_initial_condition_custom(): nb_elements = 6 nb_shoot = 10 def custom_bound_func(current_shooting, val, total_shooting): # Linear interpolation created with custom bound function return val[:, 0] + (val[:, 1] - val[:, 0]) * current_shooting / total_shooting init_val = np.random.random((nb_elements, 2)) init = InitialConditions( custom_bound_func, interpolation=InterpolationType.CUSTOM, val=init_val, total_shooting=nb_shoot, ) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_condition_spline(): nb_shoot = 10 spline_time = np.hstack((0.0, 1.0, 2.2, 6.0)) init_val = np.array([ [0.5, 0.6, 0.2, 0.8], [0.4, 0.6, 0.8, 0.2], [0.0, 0.3, 0.2, 0.5], [0.5, 0.2, 0.9, 0.4], [0.5, 0.6, 0.2, 0.8], [0.5, 0.6, 0.2, 0.8], ]) nb_elements = init_val.shape[0] # Raise if time is not sent with pytest.raises(RuntimeError): InitialConditions(init_val, interpolation=InterpolationType.SPLINE) init = InitialConditions(init_val, t=spline_time, interpolation=InterpolationType.SPLINE) init.check_and_adjust_dimensions(nb_elements, nb_shoot) time_to_test = [0, nb_shoot // 3, nb_shoot // 2, nb_shoot] expected_matrix = np.array([ [0.5, 0.4, 0.0, 0.5, 0.5, 0.5], [ 0.33333333, 0.73333333, 0.23333333, 0.66666667, 0.33333333, 0.33333333 ], [ 0.32631579, 0.67368421, 0.26315789, 0.79473684, 0.32631579, 0.32631579 ], [0.8, 0.2, 0.5, 0.4, 0.8, 0.8], ]).T for i, t in enumerate(time_to_test): expected_val = expected_matrix[:, i] np.testing.assert_almost_equal(init.init.evaluate_at(t), expected_val)
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, 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( 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( 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, 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="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_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_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, 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, 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(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(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(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(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, 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="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_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, )