def test_mayer_neg_monophase_time_constraint(): ( biorbd_model, n_shooting, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(n_phases=1) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0]) with pytest.raises(RuntimeError, match="Time constraint/objective cannot declare more than once"): OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def test_mayer_multiphase_time_constraint(): ( biorbd_model, n_shooting, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(n_phases=3) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=2) constraints.add(ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=2) OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def test_acados_custom_dynamics(problem_type_custom): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module( bioptim_folder + "/examples/getting_started/custom_dynamics.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/getting_started/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -2], np.array( (0, 9.81, -2.27903226)))
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound, mu, ode_solver=OdeSolver.RK4()): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 tau_mapping = BiMapping([None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=max_bound, node=Node.ALL, contact_force_idx=2, ) constraints.add( ConstraintFcn.NON_SLIPPING, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=mu, ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * tau_mapping.to_first.len, [tau_max] * tau_mapping.to_first.len) u_init = InitialGuessList() u_init.add([tau_init] * tau_mapping.to_first.len) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, tau_mapping=tau_mapping, ode_solver=ode_solver, )
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: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeList() if use_actuators: dynamics.add(DynamicsType.TORQUE_ACTIVATIONS_DRIVEN) else: dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 x_bounds[0][2, [0, -1]] = [0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, 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, initialize_near_solution, ode_solver=OdeSolver.RK, constr=True, use_SX=False, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr is True: constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=4) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=5) constraints.add( ConstraintFcn.ALIGN_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker_idx=1, segment_idx=2, axis=(Axe.X) ) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_SX=use_SX, )
def prepare_ocp( biorbd_model_path: str, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(custom_func_track_markers, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(custom_func_track_markers, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path, phase_time, n_shooting, min_bound, max_bound): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 dof_mapping = BiMappingList() dof_mapping.add("tau", [None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True, with_contact=True) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=max_bound, node=Node.ALL_SHOOTING, contact_index=1, ) constraints.add( ConstraintFcn.TRACK_CONTACT_FORCES, min_bound=min_bound, max_bound=max_bound, node=Node.ALL_SHOOTING, contact_index=2, ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * len(dof_mapping["tau"].to_first) + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * len(dof_mapping["tau"].to_first) + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mapping["tau"].to_first) + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints=constraints, variable_mappings=dof_mapping, )
idx=3 ) # permet de réduire le nombre d'itérations avant la convergence new_objectives.add( Objective.Lagrange.MINIMIZE_TORQUE_DERIVATIVE, instant=Instant.ALL, state_idx=bow.hair_idx, # weight=1, idx=4 ) # rajoute des itérations et ne semble riuen changer au mouvement... ocp.update_objectives(new_objectives) new_constraints = ConstraintList() for j in range(1, 5): new_constraints.add(Constraint.ALIGN_MARKERS, instant=j, min_bound=0, max_bound=0, first_marker_idx=Bow.contact_marker, second_marker_idx=violin.bridge_marker) for j in range(5, nb_shooting_pts_window + 1): new_constraints.add( Constraint.ALIGN_MARKERS, instant=j, # min_bound=-1, #-10**(j-14) donne 25 itérations # max_bound=1, # (j-4)/10 donne 21 itérations first_marker_idx=Bow.contact_marker, second_marker_idx=violin.bridge_marker) ocp.update_constraints(new_constraints) sol = ocp.solve( show_online_optim=False,
def prepare_ocp(biorbd_model_path="cube.bioMod", ode_solver=OdeSolver.RK, long_optim=False): # --- Options --- # # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: number_shooting_points = (100, 300, 100) else: number_shooting_points = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) dynamics.add(DynamicsType.TORQUE_DRIVEN) dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=0) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1, phase=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2, phase=2) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model[0])) x_bounds.add(QAndQDotBounds(biorbd_model[0])) x_bounds.add(QAndQDotBounds(biorbd_model[0])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_bounds.add([[tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4(), long_optim: bool = False ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solve to use long_optim: bool If the solver should solve the precise optimization (500 shooting points) or the approximate (50 points) Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters if long_optim: n_shooting = (100, 300, 100) else: n_shooting = (20, 30, 20) final_time = (2, 5, 4) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, quadratic=True, ) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp(model_path, phase_time, ns, time_min, time_max, init): # --- Options --- # # Model path biorbd_model = [biorbd.Model(elt) for elt in model_path] nb_phases = len(biorbd_model) 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, 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, tau_mapping, tau_mapping, tau_mapping nq = len(q_mapping[0].reduce.map_idx) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-100, phase=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) # --- Constraints --- # constraints = ConstraintList() # Positivity constraints of the normal component of the reaction forces contact_axes = (1, 2, 4, 5) for i in contact_axes: constraints.add(ConstraintFcn.CONTACT_FORCE, phase=0, node=Node.ALL, contact_force_idx=i, max_bound=np.inf) constraints.add(ConstraintFcn.CONTACT_FORCE, phase=4, node=Node.ALL, contact_force_idx=i, max_bound=np.inf) contact_axes = (1, 3) for i in contact_axes: constraints.add(ConstraintFcn.CONTACT_FORCE, phase=1, node=Node.ALL, contact_force_idx=i, max_bound=np.inf) constraints.add(ConstraintFcn.CONTACT_FORCE, phase=3, node=Node.ALL, contact_force_idx=i, max_bound=np.inf) # Non-slipping constraints # N.B.: Application on only one of the two feet is sufficient, as the slippage cannot occurs on only one foot. constraints.add( ConstraintFcn.NON_SLIPPING, phase=0, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=0.5, ) constraints.add( ConstraintFcn.NON_SLIPPING, phase=1, node=Node.ALL, normal_component_idx=1, tangential_component_idx=0, static_friction_coefficient=0.5, ) constraints.add( ConstraintFcn.NON_SLIPPING, phase=3, node=Node.ALL, normal_component_idx=1, tangential_component_idx=0, static_friction_coefficient=0.5, ) constraints.add( ConstraintFcn.NON_SLIPPING, phase=4, node=Node.ALL, normal_component_idx=(1, 2), tangential_component_idx=0, static_friction_coefficient=0.5, ) # Custom constraints for contact forces at transitions constraints.add(utils.toe_on_floor, phase=3, node=Node.START, min_bound=-0.0001, max_bound=0.0001) constraints.add(utils.heel_on_floor, phase=4, node=Node.START, min_bound=-0.0001, max_bound=0.0001) # Custom constraints for positivity of CoM_dot on z axis just before the take-off constraints.add(utils.com_dot_z, phase=1, node=Node.END, min_bound=0, max_bound=np.inf) # Constraint arm positivity constraints.add(ConstraintFcn.TRACK_STATE, phase=1, node=Node.END, index=3, min_bound=1.0, max_bound=np.inf) # Constraint foot positivity constraints.add(utils.heel_on_floor, phase=1, node=Node.ALL, min_bound=-0.0001, max_bound=np.inf) constraints.add(utils.heel_on_floor, phase=2, node=Node.ALL, min_bound=-0.0001, max_bound=np.inf) constraints.add(utils.toe_on_floor, phase=2, node=Node.ALL, min_bound=-0.0001, max_bound=np.inf) constraints.add(utils.heel_on_floor, phase=3, node=Node.ALL, min_bound=-0.0001, max_bound=np.inf) # Torque constraint + minimize_state for i in range(nb_phases): constraints.add(utils.tau_actuator_constraints, phase=i, node=Node.ALL, minimal_tau=20) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=0.0001, phase=i, min_bound=time_min[i], max_bound=time_max[i]) # State transitions state_transitions = StateTransitionList() state_transitions.add(StateTransition.IMPACT, phase_pre_idx=2) state_transitions.add(StateTransition.IMPACT, phase_pre_idx=3) # Path constraint nb_q = q_mapping[0].reduce.len nb_q_dot = nb_q pose_at_first_node = [0, 0, -0.5336, 1.4, 0.8, -0.9, 0.47] # Initialize x_bounds (Interpolation type is CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) x_bounds = BoundsList() for i in range(nb_phases): x_bounds.add(bounds=QAndQDotBounds( biorbd_model[i], all_generalized_mapping=q_mapping[i])) x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_q_dot x_bounds[3].min[13, 0] = -1000 x_bounds[3].max[13, 0] = 1000 x_bounds[4].min[13, 0] = -1000 x_bounds[4].max[13, 0] = 1000 x_bounds[4][2:, -1] = pose_at_first_node[2:] + [0] * nb_q_dot # Initial guess for states (Interpolation type is CONSTANT) x_init = InitialGuessList() for i in range(nb_phases): x_init.add(pose_at_first_node + [0] * nb_q_dot) # Define control path constraint u_bounds = BoundsList() for i in range(nb_phases): u_bounds.add([-500] * tau_mapping[i].reduce.len, [500] * tau_mapping[i].reduce.len) # Define initial guess for controls u_init = InitialGuessList() for i in range(nb_phases): if init is not None: u_init.add(init) else: u_init.add([0] * tau_mapping[i].reduce.len) # ------------- # ocp = OptimalControlProgram( biorbd_model, dynamics, ns, phase_time, x_init=x_init, x_bounds=x_bounds, u_init=u_init, u_bounds=u_bounds, objective_functions=objective_functions, constraints=constraints, q_mapping=q_mapping, q_dot_mapping=q_mapping, tau_mapping=tau_mapping, state_transitions=state_transitions, nb_threads=2, use_SX=False, ) return utils.add_custom_plots(ocp, nb_phases, x_bounds, nq, minimal_tau=20)
def prepare_ocp( biorbd_model_path, number_shooting_points, final_time, interpolation_type=InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT, ): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() nqdot = biorbd_model.nbQdot() ntau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraints if interpolation_type == InterpolationType.CONSTANT: x_min = [-100] * (nq + nqdot) x_max = [100] * (nq + nqdot) x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT) u_min = [tau_min] * ntau u_max = [tau_max] * ntau u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT) elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: x_min = np.random.random((6, 3)) * (-10) - 5 x_max = np.random.random((6, 3)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) u_min = np.random.random((3, 3)) * tau_min + tau_min / 2 u_max = np.random.random((3, 3)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) elif interpolation_type == InterpolationType.LINEAR: x_min = np.random.random((6, 2)) * (-10) - 5 x_max = np.random.random((6, 2)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR) u_min = np.random.random((3, 2)) * tau_min + tau_min / 2 u_max = np.random.random((3, 2)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR) elif interpolation_type == InterpolationType.EACH_FRAME: x_min = np.random.random( (nq + nqdot, number_shooting_points + 1)) * (-10) - 5 x_max = np.random.random( (nq + nqdot, number_shooting_points + 1)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.EACH_FRAME) u_min = np.random.random( (ntau, number_shooting_points)) * tau_min + tau_min / 2 u_max = np.random.random( (ntau, number_shooting_points)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.EACH_FRAME) elif interpolation_type == InterpolationType.SPLINE: spline_time = np.hstack( (0, np.sort(np.random.random((3, )) * final_time), final_time)) x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5 x_max = np.random.random((nq + nqdot, 5)) * 10 + 5 u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2 u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.SPLINE, t=spline_time) u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.SPLINE, t=spline_time) elif interpolation_type == InterpolationType.CUSTOM: # The custom functions refer to the ones at the beginning of the file. # For this particular instance, they emulate a Linear interpolation extra_params_x = { "n_elements": nq + nqdot, "nb_shooting": number_shooting_points } extra_params_u = { "n_elements": ntau, "nb_shooting": number_shooting_points } x_bounds = Bounds(custom_x_bounds_min, custom_x_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_x) u_bounds = Bounds(custom_u_bounds_min, custom_u_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_u) else: raise NotImplementedError("Not implemented yet") # Initial guess x_init = InitialGuess([0] * (nq + nqdot)) u_init = InitialGuess([tau_init] * ntau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path, problem_type_custom=True, ode_solver=OdeSolver.RK, use_SX=False): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() if problem_type_custom: dynamics.add(custom_configure, dynamic_function=custom_dynamic) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_SX=use_SX, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the model final_time: float The time of the final node n_shooting: int The number of shooting points ode_solver: The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment="seg_rt", rt=0) # Path constraint nq = biorbd_model.nbQ() x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][2, [0, -1]] = [-1.57, 1.57] x_bounds[0][nq:, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, actuator_type: int = None, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod n_shooting: int The number of shooting points final_time: float The time at final node actuator_type: int The type of actuator to use: 1 (torque activations) or 2 (torque max constraints) ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters if actuator_type and actuator_type == 1: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100) # Dynamics dynamics = DynamicsList() if actuator_type: if actuator_type == 1: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN) elif actuator_type == 2: dynamics.add(DynamicsFcn.TORQUE_DRIVEN) else: raise ValueError( "actuator_type is 1 (torque activations) or 2 (torque max constraints)" ) else: expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") if actuator_type == 2: constraints.add(ConstraintFcn.TORQUE_MAX_FROM_Q_AND_QDOT, node=Node.ALL_SHOOTING, min_torque=7.5) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 x_bounds[0][2, [0, -1]] = [0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str = "models/cube.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ # Model path biorbd_model = ( biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), ) # Problem parameters n_shooting = (20, 20, 20, 20) final_time = (2, 5, 4, 2) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, phase=3) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=3) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds[0][[1, 3, 4, 5], 0] = 0 x_bounds[-1][[1, 3, 4, 5], -1] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) """ By default, all phase transitions (here phase 0 to phase 1, phase 1 to phase 2 and phase 2 to phase 3) are continuous. In the event that one (or more) phase transition(s) is desired to be discontinuous, as for example IMPACT or CUSTOM can be used as below. "phase_pre_idx" corresponds to the index of the phase preceding the transition. IMPACT will cause an impact related discontinuity when defining one or more contact points in the model. CUSTOM will allow to call the custom function previously presented in order to have its own phase transition. Finally, if you want a phase transition (continuous or not) between the last and the first phase (cyclicity) you can use the dedicated PhaseTransitionFcn.Cyclic or use a continuous set at the last phase_pre_idx. If for some reason, you don't want the phase transition to be hard constraint, you can specify a weight higher than zero. It will thereafter be treated as a Mayer objective function with the specified weight. """ phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=0, states_mapping=BiMapping(range(3), range(3))) phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(custom_phase_transition, phase_pre_idx=2, coef=0.5) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, phase_transitions=phase_transitions, )
def prepare_ocp( biorbd_model_path: str = "cubeSym.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str Path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, index=[0, 1, 3], key="tau", weight=100) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True dynamics.add(DynamicsFcn.TORQUE_DRIVEN, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") constraints.add(ConstraintFcn.PROPORTIONAL_CONTROL, key="tau", node=Node.ALL_SHOOTING, first_dof=3, second_dof=4, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][2, :] = 0 # Third dof is set to zero x_bounds[0][biorbd_model.nbQ():, [0, -1]] = 0 # Velocity is 0 at start and end # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()) u_bounds[0][2, :] = 0 # Third dof is left uncontrolled u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, interpolation_type: InterpolationType = InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT, ) -> OptimalControlProgram: """ Prepare the ocp for the specified interpolation type Parameters ---------- biorbd_model_path: str The path to the biorbd model n_shooting: int The number of shooting point final_time: float The movement time interpolation_type: InterpolationType The requested InterpolationType Returns ------- The OCP fully prepared and ready to be solved """ # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() nqdot = biorbd_model.nbQdot() ntau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau") # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraints if interpolation_type == InterpolationType.CONSTANT: x_min = [-100] * (nq + nqdot) x_max = [100] * (nq + nqdot) x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT) u_min = [tau_min] * ntau u_max = [tau_max] * ntau u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.CONSTANT) elif interpolation_type == InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: x_min = np.random.random((6, 3)) * (-10) - 5 x_max = np.random.random((6, 3)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) u_min = np.random.random((3, 3)) * tau_min + tau_min / 2 u_max = np.random.random((3, 3)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType. CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) elif interpolation_type == InterpolationType.LINEAR: x_min = np.random.random((6, 2)) * (-10) - 5 x_max = np.random.random((6, 2)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.LINEAR) u_min = np.random.random((3, 2)) * tau_min + tau_min / 2 u_max = np.random.random((3, 2)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.LINEAR) elif interpolation_type == InterpolationType.EACH_FRAME: x_min = np.random.random((nq + nqdot, n_shooting + 1)) * (-10) - 5 x_max = np.random.random((nq + nqdot, n_shooting + 1)) * 10 + 5 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.EACH_FRAME) u_min = np.random.random((ntau, n_shooting)) * tau_min + tau_min / 2 u_max = np.random.random((ntau, n_shooting)) * tau_max + tau_max / 2 u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.EACH_FRAME) elif interpolation_type == InterpolationType.SPLINE: spline_time = np.hstack( (0, np.sort(np.random.random((3, )) * final_time), final_time)) x_min = np.random.random((nq + nqdot, 5)) * (-10) - 5 x_max = np.random.random((nq + nqdot, 5)) * 10 + 5 u_min = np.random.random((ntau, 5)) * tau_min + tau_min / 2 u_max = np.random.random((ntau, 5)) * tau_max + tau_max / 2 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.SPLINE, t=spline_time) u_bounds = Bounds(u_min, u_max, interpolation=InterpolationType.SPLINE, t=spline_time) elif interpolation_type == InterpolationType.CUSTOM: # The custom functions refer to the ones at the beginning of the file. # For this particular instance, they emulate a Linear interpolation extra_params_x = {"n_elements": nq + nqdot, "n_shooting": n_shooting} extra_params_u = {"n_elements": ntau, "n_shooting": n_shooting} x_bounds = Bounds(custom_x_bounds_min, custom_x_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_x) u_bounds = Bounds(custom_u_bounds_min, custom_u_bounds_max, interpolation=InterpolationType.CUSTOM, **extra_params_u) else: raise NotImplementedError("Not implemented yet") # Initial guess x_init = InitialGuess([0] * (nq + nqdot)) u_init = InitialGuess([tau_init] * ntau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, )
def prepare_ocp(biorbd_model_path="cubeSym.bioMod", ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters number_shooting_points = 30 final_time = 2 tau_min, tau_max, tau_init = -100, 100, 0 all_generalized_mapping = BidirectionalMapping(Mapping([0, 1, 2, 2], [3]), Mapping([0, 1, 2])) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model, all_generalized_mapping)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * all_generalized_mapping.reduce.len * 2) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * all_generalized_mapping.reduce.len, [tau_max] * all_generalized_mapping.reduce.len) u_init = InitialGuessList() u_init.add([tau_init] * all_generalized_mapping.reduce.len) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, all_generalized_mapping=all_generalized_mapping, )
def prepare_ocp( biorbd_model_path: str, final_time: float, n_shooting: int, initialize_near_solution: bool, ode_solver: OdeSolver = OdeSolver.RK4(), constr: bool = True, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file final_time: float The time at the final node n_shooting: int The number of shooting points initialize_near_solution: bool If the initial guess should be almost the solution (this is merely to reduce the time of the tests) ode_solver: OdeSolver The ode solver to use constr: bool If the constraint should be applied (this is merely to reduce the time of the tests) use_sx: bool If SX CasADi variables should be used Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints if constr: constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m4") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m5") constraints.add(ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X) else: constraints = ConstraintList() # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) for i in range(1, 8): if i != 3: x_bounds[0][i, [0, -1]] = 0 x_bounds[0][2, -1] = 1.57 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) if initialize_near_solution: for i in range(2): x_init[0].init[i] = 1.5 for i in range(4, 6): x_init[0].init[i] = 0.7 for i in range(6, 8): x_init[0].init[i] = 0.6 # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp( biorbd_model_path: str = "cube_with_forces.bioMod", ode_solver: OdeSolver = OdeSolver.RK4() ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod ode_solver: OdeSolver The ode solver to use Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # External forces. external_forces is of len 1 because there is only one phase. # The array inside it is 6x2x30 since there is [Mx, My, Mz, Fx, Fy, Fz] for the two externalforceindex for each node external_forces = [ np.repeat(np.array([[0, 0, 0, 0, 0, -2], [0, 0, 0, 0, 0, 5]]).T[:, :, np.newaxis], n_shooting, axis=2) ] # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds.add([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, external_forces=external_forces, ode_solver=ode_solver, )
def prepare_ocp_phase_transitions( biorbd_model_path: str, with_constraints: bool, with_mayer: bool, with_lagrange: bool, ) -> OptimalControlProgram: """ Parameters ---------- biorbd_model_path: str The path to the bioMod Returns ------- The ocp ready to be solved """ # Model path biorbd_model = ( biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), ) # Problem parameters n_shooting = (20, 20, 20, 20) final_time = (2, 5, 4, 2) tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() if with_lagrange: objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=3) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, weight=0, phase=3, axis=None) if with_mayer: objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, phase=0, node=1) objective_functions.add( minimize_difference, custom_type=ObjectiveFcn.Mayer, node=Node.TRANSITION, weight=100, phase=1, get_all_nodes_at_once=True, quadratic=True, ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() if with_constraints: constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=2, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add(custom_func_track_markers, node=Node.ALL, first_marker="m0", second_marker="m1", phase=3) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) x_bounds[0][[1, 3, 4, 5], 0] = 0 x_bounds[-1][[1, 3, 4, 5], -1] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) # Define phase transitions phase_transitions = PhaseTransitionList() phase_transitions.add(PhaseTransitionFcn.IMPACT, phase_pre_idx=1) phase_transitions.add(PhaseTransitionFcn.CONTINUOUS, phase_pre_idx=2, idx_1=1, idx_2=3) phase_transitions.add(PhaseTransitionFcn.CYCLIC) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, phase_transitions=phase_transitions, )
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 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.MID, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2) constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[2:6, -1] = [1.57, 0, 0, 0] # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # # A state transition loop constraint is treated as # hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or # as a soft penalty (objective) otherwise state_transitions = StateTransitionList() if loop_from_constraint: state_transitions.add(StateTransitionFcn.CYCLIC, weight=0) else: state_transitions.add(StateTransitionFcn.CYCLIC, weight=10000) return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, state_transitions=state_transitions, )
def prepare_ocp( biorbd_model_path: str, phase_time: float, n_shooting: int, use_actuators: bool = False, ode_solver: OdeSolver = OdeSolver.RK4(), objective_name: str = "MINIMIZE_PREDICTED_COM_HEIGHT", com_constraints: bool = False, ) -> OptimalControlProgram: """ Prepare the ocp Parameters ---------- biorbd_model_path: str The path to the bioMod file phase_time: float The time at the final node n_shooting: int The number of shooting points use_actuators: bool If torque or torque activation should be used for the dynamics ode_solver: OdeSolver The ode solver to use objective_name: str The objective function to run ('MINIMIZE_PREDICTED_COM_HEIGHT', 'MINIMIZE_COM_POSITION' or 'MINIMIZE_COM_VELOCITY') com_constraints: bool If a constraint on the COM should be applied Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) if use_actuators: tau_min, tau_max, tau_init = -1, 1, 0 else: tau_min, tau_max, tau_init = -500, 500, 0 dof_mapping = BiMappingList() dof_mapping.add("tau", [None, None, None, 0], [3]) # Add objective functions objective_functions = ObjectiveList() if objective_name == "MINIMIZE_PREDICTED_COM_HEIGHT": objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) elif objective_name == "MINIMIZE_COM_POSITION": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_POSITION, node=Node.ALL, axes=Axis.Z, weight=-1) elif objective_name == "MINIMIZE_COM_VELOCITY": objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_COM_VELOCITY, node=Node.ALL, axes=Axis.Z, weight=-1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1 / 100) # Dynamics dynamics = DynamicsList() if use_actuators: dynamics.add(DynamicsFcn.TORQUE_ACTIVATIONS_DRIVEN, with_contact=True) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True) # Constraints constraints = ConstraintList() if com_constraints: constraints.add( ConstraintFcn.TRACK_COM_VELOCITY, node=Node.ALL, min_bound=np.array([-100, -100, -100]), max_bound=np.array([100, 100, 100]), ) constraints.add( ConstraintFcn.TRACK_COM_POSITION, node=Node.ALL, min_bound=np.array([-1, -1, -1]), max_bound=np.array([1, 1, 1]), ) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.5, 0.5] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * len(dof_mapping["tau"].to_first), [tau_max] * len(dof_mapping["tau"].to_first)) u_init = InitialGuessList() u_init.add([tau_init] * len(dof_mapping["tau"].to_first)) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints=constraints, variable_mappings=dof_mapping, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, problem_type_custom: bool = True, ode_solver: OdeSolver = OdeSolver.RK4(), use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model problem_type_custom: bool If the preparation should be done using the user-defined dynamic function or the normal TORQUE_DRIVEN. They should return the same results ode_solver: OdeSolver The type of ode solver used use_sx: bool If the program should be constructed using SX instead of MX (longer to create the CasADi graph, faster to solve) Returns ------- The ocp ready to be solved """ # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters n_shooting = 30 final_time = 2 # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) # Dynamics dynamics = DynamicsList() expand = False if isinstance(ode_solver, OdeSolver.IRK) else True if problem_type_custom: dynamics.add(custom_configure, dynamic_function=custom_dynamic, my_additional_factor=1, expand=expand) else: dynamics.add(DynamicsFcn.TORQUE_DRIVEN, dynamic_function=custom_dynamic, expand=expand) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1") constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[1:6, [0, -1]] = 0 x_bounds[2, -1] = 1.57 # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, use_sx=use_sx, )
def prepare_ocp(phase_time_constraint, use_parameter): # --- Inputs --- # final_time = (2, 5, 4) time_min = [1, 3, 0.1] time_max = [2, 4, 0.8] ns = (20, 30, 20) biorbd_model_path = TestUtils.bioptim_folder( ) + "/examples/optimal_time_ocp/cube.bioMod" ode_solver = OdeSolver.RK4() # --- Options --- # n_phases = len(ns) # Model path biorbd_model = (biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path), biorbd.Model(biorbd_model_path)) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=0) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=1) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100, phase=2) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=0) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=1) dynamics.add(DynamicsFcn.TORQUE_DRIVEN, phase=2) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.START, first_marker="m0", second_marker="m1", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=0) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m1", phase=1) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2", phase=2) constraints.add( ConstraintFcn.TIME_CONSTRAINT, node=Node.END, minimum=time_min[0], maximum=time_max[0], phase=phase_time_constraint, ) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 0 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 1 x_bounds.add(bounds=QAndQDotBounds(biorbd_model[0])) # Phase 2 for bounds in x_bounds: for i in [1, 3, 4, 5]: bounds[i, [0, -1]] = 0 x_bounds[0][2, 0] = 0.0 x_bounds[2][2, [0, -1]] = [0.0, 1.57] # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) x_init.add([0] * (biorbd_model[0].nbQ() + biorbd_model[0].nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_bounds.add([tau_min] * biorbd_model[0].nbGeneralizedTorque(), [tau_max] * biorbd_model[0].nbGeneralizedTorque()) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) u_init.add([tau_init] * biorbd_model[0].nbGeneralizedTorque()) parameters = ParameterList() if use_parameter: def my_target_function(ocp, value, target_value): return value - target_value def my_parameter_function(biorbd_model, value, extra_value): new_gravity = MX.zeros(3, 1) new_gravity[2] = value + extra_value biorbd_model.setGravity(new_gravity) min_g = -10 max_g = -6 target_g = -8 bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=target_g) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bound_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) # ------------- # return OptimalControlProgram( biorbd_model[:n_phases], dynamics, ns, final_time[:n_phases], x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, parameters=parameters, )
def prepare_ocp(biorbd_model_path="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 tau_min, tau_max, tau_init = -100, 100, 0 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(Constraint.ALIGN_MARKERS, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(Constraint.ALIGN_MARKERS, node=Node.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 = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][3:6, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, constraints=constraints, external_forces=external_forces, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path: str, n_shooting: int, final_time: float, loop_from_constraint: bool, ode_solver: OdeSolver = OdeSolver.RK4(), ) -> OptimalControlProgram: """ Prepare the program Parameters ---------- biorbd_model_path: str The path of the biorbd model final_time: float The time at the final node n_shooting: int The number of shooting points loop_from_constraint: bool If the looping cost should be a constraint [True] or an objective [False] ode_solver: OdeSolver The type of ode solver used Returns ------- The ocp ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.MID, first_marker_idx=0, second_marker_idx=2) constraints.add(ConstraintFcn.TRACK_STATE, node=Node.MID, index=2) constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=1) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) # First node is free but mid and last are constrained to be exactly at a certain point. # The cyclic penalty ensures that the first node and the last node are the same. x_bounds[2:6, -1] = [1.57, 0, 0, 0] # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()) u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) # ------------- # # A phase transition loop constraint is treated as # hard penalty (constraint) if weight is <= 0 [or if no weight is provided], or # as a soft penalty (objective) otherwise phase_transitions = PhaseTransitionList() if loop_from_constraint: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=0) else: phase_transitions.add(PhaseTransitionFcn.CYCLIC, weight=10000) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, constraints, ode_solver=ode_solver, phase_transitions=phase_transitions, )
def prepare_ocp(model_path, phase_time, number_shooting_points, min_bound, ode_solver=OdeSolver.RK): # --- 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(ObjectiveFcn.Mayer.MINIMIZE_PREDICTED_COM_HEIGHT, weight=-1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Constraints constraints = ConstraintList() constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=np.inf, node=Node.ALL, contact_force_idx=1, ) constraints.add( ConstraintFcn.CONTACT_FORCE, min_bound=min_bound, max_bound=np.inf, node=Node.ALL, contact_force_idx=2, ) # 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(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0].concatenate( Bounds([activation_min] * nb_mus, [activation_max] * nb_mus)) x_bounds[0][:, 0] = pose_at_first_node + [0] * nb_qdot + [0.5] * nb_mus # Initial guess x_init = InitialGuessList() 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 = InitialGuessList() 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, ode_solver=ode_solver, )