def test_acados_one_mayer(cost_type): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "cube", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) cube = importlib.util.module_from_spec(spec) spec.loader.exec_module(cube) ocp = cube.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=10, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, index=[0], target=np.array([[1.0]]).T) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod") q = np.array(sol["qqdot"])[:model.nbQ()] np.testing.assert_almost_equal(q[0, -1], 1.0) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "cube", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) cube = importlib.util.module_from_spec(spec) spec.loader.exec_module(cube) nbs = 10 target = np.expand_dims(np.arange(0, nbs + 1), axis=0) target[0, -1] = nbs - 2 ocp = cube.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=nbs, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, weight=10, index=[0], target=target) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value model = biorbd.Model(str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod") q = np.array(sol["qqdot"])[:model.nbQ()] np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_pendulum_max_time_lagrange_constrained(ode_solver): # Load pendulum_min_time_Lagrange biorbd_model_path = (TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/pendulum.bioMod", ) # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path[0]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weigth=-1, max_bound=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # ------------- # with pytest.raises( RuntimeError, match= "ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have max_bound. Please either use MAYER or constraint", ): OptimalControlProgram(biorbd_model, dynamics, 10, 2, objective_functions=objective_functions)
def test_acados_one_lagrange(cost_type): bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") n_shooting = 10 target = np.expand_dims(np.arange(0, n_shooting + 1), axis=0) target[0, -1] = n_shooting - 2 ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod", n_shooting=n_shooting, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_STATE, weight=10, index=[0], target=target) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, ode_solver=OdeSolver.RK): # --- Options --- #nq # Model path biorbd_model = biorbd.Model(biorbd_model_path) nq = biorbd_model.nbQ() # 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 constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_SEGMENT_WITH_CUSTOM_RT, node=Node.ALL, segment_idx=2, rt_idx=0) # Path constraint 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 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 test_acados_one_mayer(cost_type): bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod", n_shooting=10, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, index=[0], target=np.array([[1.0]]).T) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, -1], 1.0) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_mayer_neg_two_objectives(): ( biorbd_model, number_shooting_points, final_time, time_min, time_max, torque_min, torque_max, torque_init, dynamics, x_bounds, x_init, u_bounds, u_init, ) = partial_ocp_parameters(nb_phases=1) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, phase=0) with pytest.raises(RuntimeError, match="Time constraint/objective cannot declare more than once"): OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, )
def test_pendulum_max_time_lagrange_constrained(ode_solver): # Load pendulum_min_time_Lagrange biorbd_model_path = (TestUtils.bioptim_folder() + "/examples/optimal_time_ocp/models/pendulum.bioMod", ) # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path[0]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, weight=-1, max_bound=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # ------------- # with pytest.raises( TypeError, match=re.escape( "minimize_time() got an unexpected keyword argument 'max_bound'" )): OptimalControlProgram(biorbd_model, dynamics, 10, 2, objective_functions=objective_functions)
def test_acados_one_mayer(cost_type): if platform == "win32": return from bioptim.examples.acados import cube as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", n_shooting=10, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0], target=np.array([[1.0]]).T) ocp.update_objectives(objective_functions) solver = Solver.ACADOS() solver.set_cost_type(cost_type) sol = ocp.solve(solver=solver) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, -1], 1.0) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_one_parameter(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.getting_started import custom_parameters as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/pendulum.bioMod", final_time=1, n_shooting=100, optim_gravity=True, optim_mass=False, min_g=np.array([-1, -1, -10]), max_g=np.array([1, 1, -5]), min_m=10, max_m=30, target_g=np.array([0, 0, -9.81]), target_m=20, use_sx=True, ) model = ocp.nlp[0].model objectives = ObjectiveList() objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="q", target=np.array([[0, 3.14]]).T, weight=100000) objectives.add(ObjectiveFcn.Mayer.TRACK_STATE, key="qdot", target=np.array([[0, 0]]).T, weight=100) objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", index=1, weight=10, multi_thread=False) objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="qdot", weight=0.000000010, multi_thread=False) ocp.update_objectives(objectives) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[[0, 1, 2, 3], 0] = 0 u_bounds = Bounds([-300] * model.nbQ(), [300] * model.nbQ()) ocp.update_bounds(x_bounds, u_bounds) solver = Solver.ACADOS() solver.set_nlp_solver_tol_eq(1e-3) sol = ocp.solve(solver=solver) # Check some of the results q, qdot, tau, gravity = sol.states["q"], sol.states["qdot"], sol.controls["tau"], sol.parameters["gravity_xyz"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((0, 3.14)), decimal=6) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0)), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0)), decimal=6) # parameters np.testing.assert_almost_equal(gravity[-1, :], np.array([-9.81]), decimal=6) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def prepare_ocp( biorbd_model_path, final_time, number_shooting_points, ode_solver=OdeSolver.RK, weight=1, min_time=0, max_time=np.inf, ): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() # A weight of -1 will maximize time objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=weight, min_bound=min_time, max_bound=max_time) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp( biorbd_model_path, phase_time, n_shooting, muscle_activations_ref, contact_forces_ref, ode_solver=OdeSolver.RK4() ): # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -500, 500, 0 activation_min, activation_max, activation_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MUSCLES_CONTROL, target=muscle_activations_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_CONTACT_FORCES, target=contact_forces_ref) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=0.001) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, weight=0.001) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Path constraint n_q = biorbd_model.nbQ() n_qdot = n_q pose_at_first_node = [0, 0, -0.75, 0.75] # Initialize x_bounds x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = pose_at_first_node + [0] * n_qdot # Initial guess x_init = InitialGuessList() x_init.add(pose_at_first_node + [0] * n_qdot) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [activation_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [activation_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [activation_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, phase_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, ode_solver=ode_solver, )
def __init__( self, model_path: str, violin: Violin, bow: Bow, n_cycles: int, bow_starting: BowPosition.TIP, init_file: str = None, use_muscles: bool = True, time_per_cycle: float = 1, n_shooting_per_cycle: int = 30, solver: Solver = Solver.IPOPT, n_threads: int = 8, ): self.model_path = model_path self.model = biorbd.Model(self.model_path) self.n_q = self.model.nbQ() self.n_tau = self.model.nbGeneralizedTorque() self.use_muscles = use_muscles self.n_mus = self.model.nbMuscles() if self.use_muscles else 0 self.violin = violin self.bow = bow self.bow_starting = bow_starting self.n_cycles = n_cycles self.n_shooting_per_cycle = n_shooting_per_cycle self.n_shooting = self.n_shooting_per_cycle * self.n_cycles self.time_per_cycle = time_per_cycle self.time = self.time_per_cycle * self.n_cycles self.solver = solver self.n_threads = n_threads if self.use_muscles: self.dynamics = Dynamics( DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) else: self.dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) self.x_bounds = Bounds() self.u_bounds = Bounds() self._set_bounds() self.x_init = InitialGuess() self.u_init = InitialGuess() self._set_initial_guess(init_file) self.objective_functions = ObjectiveList() self._set_generic_objective_functions() self.constraints = ConstraintList() self._set_generic_constraints() self._set_generic_ocp() if use_muscles: online_muscle_torque(self.ocp)
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 # 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) constraints.add(Constraint.PROPORTIONAL_STATE, node=Node.ALL, first_dof=2, second_dof=3, coef=-1) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][4:8, [0, -1]] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * biorbd_model.nbQ(), [tau_max] * biorbd_model.nbQ()]) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbQ()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, 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, n_shooting, x_warm=None, use_sx=False, n_threads=1): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -50, 50, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=10) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=10) objective_functions.add( ObjectiveFcn.Mayer.SUPERIMPOSE_MARKERS, weight=100000, first_marker="target", second_marker="COM_hand" ) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_residual_torque=True) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (1.0, 1.0, 0, 0) # Initial guess if x_warm is None: x_init = InitialGuess([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) else: x_init = InitialGuess(x_warm, interpolation=InterpolationType.EACH_FRAME) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, n_threads=n_threads, )
def define_objective( q: np.array, iteration: int, rt_ratio: int, ns_mhe: int, biorbd_model: biorbd.Model, use_noise=True ): """ Define the objective function for the ocp Parameters ---------- q: np.array State to track iteration: int Current iteration rt_ratio: int Value of the reference data ratio to send to the estimator ns_mhe: int Size of the windows biorbd_model: biorbd.Model biorbd model build with the bioMod use_noise: bool True if noisy reference data Returns --------- The objective function """ objectives = ObjectiveList() if use_noise is not True: weight = {"track_state": 1000000, "min_act": 1000, "min_dq": 10, "min_q": 10} else: weight = {"track_state": 1000, "min_act": 100, "min_dq": 100, "min_q": 10} objectives.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL, weight=weight["min_act"]) objectives.add( ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=weight["min_dq"], index=np.array(range(biorbd_model.nbQ(), biorbd_model.nbQ() * 2)), ) objectives.add( ObjectiveFcn.Lagrange.MINIMIZE_STATE, weight=weight["min_q"], index=np.array(range(biorbd_model.nbQ())), ) q = q[:, ::rt_ratio] objectives.add( ObjectiveFcn.Lagrange.TRACK_STATE, weight=weight["track_state"], target=q[:, iteration : (ns_mhe + 1 + iteration)], index=range(biorbd_model.nbQ()), ) return objectives
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, ode_solver=OdeSolver.RK): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -1, 1, 0 muscle_min, muscle_max, muscle_init = 0, 1, 0.5 # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_MUSCLES_CONTROL) objective_functions.add(ObjectiveFcn.Mayer.ALIGN_MARKERS, first_marker_idx=0, second_marker_idx=5) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN_WITH_CONTACT) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = (0, 0.07, 1.4, 0, 0, 0) # Initial guess x_init = InitialGuessList() x_init.add([1.57] * biorbd_model.nbQ() + [0] * biorbd_model.nbQdot()) # Define control path constraint u_bounds = BoundsList() u_bounds.add( [tau_min] * biorbd_model.nbGeneralizedTorque() + [muscle_min] * biorbd_model.nbMuscleTotal(), [tau_max] * biorbd_model.nbGeneralizedTorque() + [muscle_max] * biorbd_model.nbMuscleTotal(), ) u_init = InitialGuessList() u_init.add([tau_init] * biorbd_model.nbGeneralizedTorque() + [muscle_init] * biorbd_model.nbMuscleTotal()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() objective_functions.add(Objective.Mayer.MINIMIZE_TIME) # Dynamics dynamics = DynamicsTypeList() dynamics.add(DynamicsType.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(QAndQDotBounds(biorbd_model)) x_bounds[0][:, [0, -1]] = 0 x_bounds[0][n_q - 1, -1] = 3.14 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([[tau_min] * n_tau, [tau_max] * n_tau]) u_bounds[0][n_tau - 1, :] = 0 u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, )
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_idx=0, second_marker_idx=1) 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 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 = ObjectiveList() objective_functions.add(Objective.Mayer.MINIMIZE_STATE, weight=1000, states_idx=[0, 1], target=np.array([[1., 2.]]).T) objective_functions.add(Objective.Mayer.MINIMIZE_STATE, weight=10000, states_idx=[2], target=np.array([[3.]])) objective_functions.add(Objective.Lagrange.MINIMIZE_TORQUE, weight=1,) # Dynamics dynamics = DynamicsTypeList() if problem_type_custom: dynamics.add(custom_configure, dynamic_function=custom_dynamic) else: dynamics.add(DynamicsType.TORQUE_DRIVEN, dynamic_function=custom_dynamic) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds[:, 0] = 0 # Initial guess x_init = InitialGuessOption([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Define control path constraint u_bounds = BoundsOption( [[tau_min] * biorbd_model.nbGeneralizedTorque(), [tau_max] * biorbd_model.nbGeneralizedTorque()] ) u_init = InitialGuessOption([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, ode_solver=ode_solver, use_SX=use_SX )
def test_acados_control_lagrange_and_state_mayer(cost_type): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.acados import cube as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) n_shooting = 10 target = np.array([[2]]) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", n_shooting=n_shooting, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", multi_thread=False) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0], target=target, weight=1000, multi_thread=False ) ocp.update_objectives(objective_functions) solver = Solver.ACADOS() solver.set_cost_type(cost_type) sol = ocp.solve(solver=solver) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, -1], target.squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_one_lagrange(cost_type): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.acados import cube as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) n_shooting = 10 target = np.expand_dims(np.arange(0, n_shooting + 1), axis=0) target[0, -1] = n_shooting - 2 ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", n_shooting=n_shooting, tf=2, ) objective_functions = ObjectiveList() objective_functions.add( ObjectiveFcn.Lagrange.TRACK_STATE, key="q", node=Node.ALL, weight=10, index=[0], target=target, multi_thread=False, ) ocp.update_objectives(objective_functions) solver = Solver.ACADOS() solver.set_cost_type(cost_type) sol = ocp.solve(solver=solver) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, :], target[0, :].squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_control_lagrange_and_state_mayer(cost_type): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") n_shooting = 10 target = np.array([[2]]) ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/cube.bioMod", n_shooting=n_shooting, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_ALL_CONTROLS, ) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, index=[0], target=target, weight=1000) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, -1], target.squeeze()) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_several_mayer(cost_type): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/models/cube.bioMod", n_shooting=10, tf=2, ) objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[0, 1], target=np.array([[1.0, 2.0]]).T) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", index=[2], target=np.array([[3.0]])) ocp.update_objectives(objective_functions) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"cost_type": cost_type}) # Check end state value q = sol.states["q"] np.testing.assert_almost_equal(q[0, -1], 1.0) np.testing.assert_almost_equal(q[1, -1], 2.0) np.testing.assert_almost_equal(q[2, -1], 3.0) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_pendulum_min_time_lagrange_constrained(ode_solver): # Load pendulum_min_time_Lagrange PROJECT_FOLDER = Path(__file__).parent / ".." biorbd_model_path = (str(PROJECT_FOLDER) + "/examples/optimal_time_ocp/pendulum.bioMod",) # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path[0]) # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TIME, min_bound=1) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # ------------- # with pytest.raises( RuntimeError, match="ObjectiveFcn.Lagrange.MINIMIZE_TIME cannot have min_bound. Please either use MAYER or constraint", ): OptimalControlProgram(biorbd_model, dynamics, 10, 2, objective_functions=objective_functions)
def test_acados_one_end_constraints(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return bioptim_folder = TestUtils.bioptim_folder() cube = TestUtils.load_module(bioptim_folder + "/examples/acados/cube.py") ocp = cube.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/acados/models/cube.bioMod", n_shooting=10, tf=2, ) model = ocp.nlp[0].model objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE, index=0, key="q", weight=100, target=np.array([[1]]), multi_thread=False) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=100, multi_thread=False) ocp.update_objectives(objective_functions) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[1:6, [0, -1]] = 0 x_bounds[0, 0] = 0 ocp.update_bounds(x_bounds=x_bounds) 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"] # final position np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((-2.72727272, 9.81, 0)), decimal=6)
def prepare_ocp(biorbd_model, final_time, number_shooting_points, x0, use_sx=False, n_threads=8): # --- Options --- # # Model path activation_min, activation_max, activation_init = 0, 1, 0.1 excitation_min, excitation_max, excitation_init = 0, 1, 0.2 # Add objective functions objective_functions = ObjectiveList() # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_EXCITATIONS_DRIVEN) # State path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # add muscle activation bounds x_bounds[0].concatenate( Bounds([activation_min] * biorbd_model.nbMuscles(), [activation_max] * biorbd_model.nbMuscles()) ) # Control path constraint u_bounds = BoundsList() u_bounds.add([excitation_min] * biorbd_model.nbMuscleTotal(), [excitation_max] * biorbd_model.nbMuscleTotal()) # Initial guesses x_init = InitialGuess( np.tile(np.concatenate((x0, [activation_init] * biorbd_model.nbMuscles())), (number_shooting_points + 1, 1)).T, interpolation=InterpolationType.EACH_FRAME, ) u0 = np.array([excitation_init] * biorbd_model.nbMuscles()) u_init = InitialGuess(np.tile(u0, (number_shooting_points, 1)).T, interpolation=InterpolationType.EACH_FRAME) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=use_sx, n_threads=n_threads, )
def prepare_short_ocp(biorbd_model: biorbd.Model, final_time: float, n_shooting: int): """ Prepare to build a blank short ocp to use single shooting bioptim function Parameters ---------- biorbd_model: biorbd.Model biorbd model build with the bioMod final_time: float The time at the final node n_shooting: int The number of shooting points Returns ------- The blank OptimalControlProgram """ # Add objective functions objective_functions = ObjectiveList() # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([0] * biorbd_model.nbMuscles(), [1] * biorbd_model.nbMuscles()) x_init = InitialGuess([0] * biorbd_model.nbQ() * 2) u_init = InitialGuess([0] * biorbd_model.nbMuscles()) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, use_sx=True, )
def prepare_ocp(biorbd_model, final_time, number_shooting_points, markers_ref, tau_ref, ode_solver=OdeSolver.RK): # --- Options --- # tau_min, tau_max, tau_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.TRACK_MARKERS, axis_to_track=[Axe.Y, Axe.Z], weight=100, target=markers_ref) objective_functions.add(ObjectiveFcn.Lagrange.TRACK_TORQUE, target=tau_ref) # Dynamics dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsList() x_bounds.add(bounds=QAndQDotBounds(biorbd_model)) x_bounds[0][:, 0] = 0 # Initial guess x_init = InitialGuessList() x_init.add([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsList() u_bounds.add([tau_min] * n_tau, [tau_max] * n_tau) u_init = InitialGuessList() u_init.add([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, ode_solver=ode_solver, )
def test_acados_one_end_constraints(): PROJECT_FOLDER = Path(__file__).parent / ".." spec = importlib.util.spec_from_file_location( "constraint", str(PROJECT_FOLDER) + "/examples/acados/cube.py", ) constraint = importlib.util.module_from_spec(spec) spec.loader.exec_module(constraint) ocp = constraint.prepare_ocp( biorbd_model_path=str(PROJECT_FOLDER) + "/examples/acados/cube.bioMod", nbs=10, tf=2, ) model = ocp.nlp[0].model objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Mayer.TRACK_STATE, index=0, weight=100, target=np.array([[1]])) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) ocp.update_objectives(objective_functions) # Path constraint x_bounds = QAndQDotBounds(model) x_bounds[1:6, [0, -1]] = 0 x_bounds[0, 0] = 0 ocp.update_bounds(x_bounds=x_bounds) constraints = ConstraintList() constraints.add(ConstraintFcn.ALIGN_MARKERS, node=Node.END, first_marker_idx=0, second_marker_idx=2) ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS, solver_options={"print_level": 0}) # Check some of the results states, controls = Data.get_data(ocp, sol["x"]) q, qdot, tau = states["q"], states["q_dot"], controls["tau"] # final position np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 0)), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((2.72727272, 9.81, 0)), decimal=6) np.testing.assert_almost_equal(tau[:, -1], np.array((-2.72727272, 9.81, 0)), decimal=6)