def test_accessors_on_bounds_option_multidimensional(): x_min = [[-100, -50, 0] for i in range(6)] x_max = [[100, 150, 200] for i in range(6)] x_bounds = Bounds( x_min, x_max, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) x_bounds[:3, 0] = 0 x_bounds.min[1:5, 1:] = -10 x_bounds.max[1:5, 1:] = 10 # Check accessor and min/max values to be equal np.testing.assert_almost_equal(x_bounds[:].min, x_bounds.min[:]) np.testing.assert_almost_equal(x_bounds[:].max, x_bounds.max[:]) # Check min and max have the right value np.testing.assert_almost_equal( x_bounds.min[:], np.array([[0, -50, 0], [0, -10, -10], [0, -10, -10], [-100, -10, -10], [-100, -10, -10], [-100, -50, 0]]), ) np.testing.assert_almost_equal( x_bounds.max[:], np.array([[0, 150, 200], [0, 10, 10], [0, 10, 10], [100, 10, 10], [100, 10, 10], [100, 150, 200]]), )
def test_mhe_redim_xbounds_and_init(): root_folder = TestUtils.bioptim_folder() + "/examples/moving_horizon_estimation/" biorbd_model = biorbd.Model(root_folder + "cart_pendulum.bioMod") nq = biorbd_model.nbQ() ntau = biorbd_model.nbGeneralizedTorque() n_cycles = 3 window_len = 5 window_duration = 0.2 x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT) x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1))) mhe = MovingHorizonEstimator( biorbd_model, Dynamics(DynamicsFcn.TORQUE_DRIVEN), window_len, window_duration, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, n_threads=4, ) def update_functions(mhe, t, _): return t < n_cycles mhe.solve(update_functions, Solver.IPOPT)
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_test_ocp(with_muscles=False, with_contact=False, with_actuator=False): bioptim_folder = TestUtils.bioptim_folder() if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator: raise RuntimeError( "With muscles and with contact and with_actuator together is not defined" ) elif with_muscles: biorbd_model = biorbd.Model( bioptim_folder + "/examples/muscle_driven_ocp/models/arm26.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_DRIVEN, with_torque=True) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles() elif with_contact: biorbd_model = biorbd.Model( bioptim_folder + "/examples/muscle_driven_with_contact/models/2segments_4dof_2contacts_1muscle.bioMod" ) dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN, with_contact=True, expand=False) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() elif with_actuator: biorbd_model = biorbd.Model( bioptim_folder + "/examples/torque_driven_ocp/models/cube.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() else: biorbd_model = biorbd.Model( bioptim_folder + "/examples/track/models/cube_and_line.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() x_init = InitialGuess(np.zeros((nx, 1))) u_init = InitialGuess(np.zeros((nu, 1))) x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1))) u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1))) ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init, u_init, x_bounds, u_bounds, use_sx=True) ocp.nlp[0].J = [[]] ocp.nlp[0].g = [[]] return ocp
def test_double_update_bounds_and_init(): bioptim_folder = TestUtils.bioptim_folder() biorbd_model = biorbd.Model(bioptim_folder + "/examples/track/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) x_bounds = Bounds(-2.0 * np.ones((nq * 2, 1)), 2.0 * np.ones((nq * 2, 1))) u_bounds = Bounds(-4.0 * np.ones((nq, 1)), 4.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds=x_bounds) ocp.update_bounds(u_bounds=u_bounds) expected = np.array([[-2] * (nq * 2) * (ns + 1) + [-4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, expected) expected = np.array([[2] * (nq * 2) * (ns + 1) + [4] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, expected) x_init = InitialGuess(0.25 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.25 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.25] * (nq * 2) * (ns + 1) + [-0.25] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, expected) with pytest.raises( RuntimeError, match= "x_init should be built from a InitialGuess or InitialGuessList"): ocp.update_initial_guess(x_bounds, u_bounds) with pytest.raises( RuntimeError, match="x_bounds should be built from a Bounds or BoundsList"): ocp.update_bounds(x_init, u_init)
def test_acados_bounds_not_implemented(failing): if platform == "win32": print("Test for ACADOS on Windows is skipped") return root_folder = TestUtils.bioptim_folder( ) + "/examples/moving_horizon_estimation/" biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod") nq = biorbd_model.nbQ() ntau = biorbd_model.nbGeneralizedTorque() n_cycles = 3 window_len = 5 window_duration = 0.2 x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT) if failing == "u_bounds": x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1))) u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT) elif failing == "x_bounds": x_bounds = Bounds(np.zeros((nq * 2, 1)), np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1))) else: raise ValueError("Wrong value for failing") mhe = MovingHorizonEstimator( biorbd_model, Dynamics(DynamicsFcn.TORQUE_DRIVEN), window_len, window_duration, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, n_threads=4, ) def update_functions(mhe, t, _): return t < n_cycles with pytest.raises( NotImplementedError, match= f"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT for the {failing}", ): mhe.solve(update_functions, Solver.ACADOS)
def test_accessors_on_bounds_option(): x_min = [-100] * 6 x_max = [100] * 6 x_bounds = Bounds(x_min, x_max, interpolation=InterpolationType.CONSTANT) x_bounds[:3] = 0 x_bounds.min[3:] = -10 x_bounds.max[1:3] = 10 # Check accessor and min/max values to be equal np.testing.assert_almost_equal(x_bounds[:].min, x_bounds.min[:]) np.testing.assert_almost_equal(x_bounds[:].max, x_bounds.max[:]) # Check min and max have the right value np.testing.assert_almost_equal(x_bounds.min[:], np.array([[0], [0], [0], [-10], [-10], [-10]])) np.testing.assert_almost_equal(x_bounds.max[:], np.array([[0], [10], [10], [100], [100], [100]]))
def prepare_test_ocp(with_muscles=False, with_contact=False, with_actuator=False): PROJECT_FOLDER = Path(__file__).parent / ".." if with_muscles and with_contact or with_muscles and with_actuator or with_contact and with_actuator: raise RuntimeError( "With muscles and with contact and with_actuator together is not defined" ) elif with_muscles: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/muscle_driven_ocp/arm26.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.MUSCLE_ACTIVATIONS_AND_TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() + biorbd_model.nbMuscles() elif with_contact: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/muscle_driven_with_contact/2segments_4dof_2contacts_1muscle.bioMod" ) dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN_WITH_CONTACT) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() elif with_actuator: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/torque_driven_ocp/cube.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() else: biorbd_model = biorbd.Model( str(PROJECT_FOLDER) + "/examples/align/cube_and_line.bioMod") dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) nx = biorbd_model.nbQ() + biorbd_model.nbQdot() nu = biorbd_model.nbGeneralizedTorque() x_init = InitialGuess(np.zeros((nx, 1))) u_init = InitialGuess(np.zeros((nu, 1))) x_bounds = Bounds(np.zeros((nx, 1)), np.zeros((nx, 1))) u_bounds = Bounds(np.zeros((nu, 1)), np.zeros((nu, 1))) ocp = OptimalControlProgram(biorbd_model, dynamics, 10, 1.0, x_init, u_init, x_bounds, u_bounds) ocp.nlp[0].J = [list()] ocp.nlp[0].g = [list()] ocp.nlp[0].g_bounds = [list()] return ocp
def prepare_ocp(biorbd_model_path, n_shooting, tf, ode_solver=OdeSolver.RK4(), use_sx=True): # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) 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, tf, x_init, u_init, x_bounds, u_bounds, ode_solver=ode_solver, use_sx=use_sx, )
def test_update_bounds_and_init_with_param(): def my_parameter_function(biorbd_model, value, extra_value): biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value)) def my_target_function(ocp, value, target_value): return value + target_value biorbd_model = biorbd.Model(TestUtils.bioptim_folder() + "/examples/track/cube_and_line.bioMod") nq = biorbd_model.nbQ() ns = 10 g_min, g_max, g_init = -10, -6, -8 dynamics = DynamicsList() dynamics.add(DynamicsFcn.TORQUE_DRIVEN) parameters = ParameterList() bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT) initial_gravity = InitialGuess(g_init) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8 ) parameters.add( "gravity_z", my_parameter_function, initial_gravity, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) ocp = OptimalControlProgram(biorbd_model, dynamics, ns, 1.0, parameters=parameters) x_bounds = Bounds(-np.ones((nq * 2, 1)), np.ones((nq * 2, 1))) u_bounds = Bounds(-2.0 * np.ones((nq, 1)), 2.0 * np.ones((nq, 1))) ocp.update_bounds(x_bounds, u_bounds) expected = np.array([[-1] * (nq * 2) * (ns + 1) + [-2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.min, np.append(expected, [g_min])[:, np.newaxis]) expected = np.array([[1] * (nq * 2) * (ns + 1) + [2] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.bounds.max, np.append(expected, [g_max])[:, np.newaxis]) x_init = InitialGuess(0.5 * np.ones((nq * 2, 1))) u_init = InitialGuess(-0.5 * np.ones((nq, 1))) ocp.update_initial_guess(x_init, u_init) expected = np.array([[0.5] * (nq * 2) * (ns + 1) + [-0.5] * nq * ns]).T np.testing.assert_almost_equal(ocp.v.init.init, np.append(expected, [g_init])[:, np.newaxis])
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points, min_g, max_g, target_g): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) tau_min, tau_max, tau_init = -30, 30, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveOption(Objective.Lagrange.MINIMIZE_TORQUE, weight=10) # Dynamics dynamics = DynamicsTypeOption(DynamicsType.TORQUE_DRIVEN) # Path constraint x_bounds = BoundsOption(QAndQDotBounds(biorbd_model)) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess x_init = InitialGuessOption([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = BoundsOption([[tau_min] * n_tau, [tau_max] * n_tau]) u_bounds[1, :] = 0 u_init = InitialGuessOption([tau_init] * n_tau) # Define the parameter to optimize # Give the parameter some min and max bounds parameters = ParameterList() bound_gravity = Bounds(min_bound=min_g, max_bound=max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) parameter_objective_functions = ObjectiveOption( my_target_function, weight=10, quadratic=True, custom_type=Objective.Parameter, target=target_g ) parameters.add( "gravity_z", # The name of the parameter my_parameter_function, # The function that modifies the biorbd model initial_gravity, # The initial guess bound_gravity, # The bounds size=1, # The number of elements this particular parameter vector has penalty_list=parameter_objective_functions, # Objective of constraint for this particular parameter extra_value=1, # You can define as many extra arguments as you want ) return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, )
def 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, 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=100) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Constraints constraints = ConstraintList() constraints.add(custom_func_align_markers, node=Node.START, first_marker_idx=0, second_marker_idx=1) constraints.add(custom_func_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, )
def prepare_ocp(biorbd_model_path: str, final_time: float, n_shooting: int) -> OptimalControlProgram: """ The initialization of an ocp Parameters ---------- biorbd_model_path: str The path to the biorbd model final_time: float The time in second required to perform the task n_shooting: int The number of shooting points to define int the direct multiple shooting program Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Add objective functions objective_functions = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint n_tau = biorbd_model.nbGeneralizedTorque() tau_min, tau_max, tau_init = -100, 100, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, )
def _set_bounds(self): self.x_bounds = QAndQDotBounds(self.model) self.x_bounds[:self.n_q, 0] = self.violin.q(self.bow_starting) self.x_bounds[self.n_q:, 0] = 0 # self.x_bounds.min[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) - 0.01 # self.x_bounds.max[:self.n_q, -1] = np.array(self.violin.q(self.bow_starting)) + 0.01 u_min = [self.tau_min] * self.n_tau + [0] * self.n_mus u_max = [self.tau_max] * self.n_tau + [1] * self.n_mus self.u_bounds = Bounds(u_min, u_max)
def test_mhe_redim_xbounds_not_implemented(): root_folder = TestUtils.bioptim_folder( ) + "/examples/moving_horizon_estimation/" biorbd_model = biorbd.Model(root_folder + "models/cart_pendulum.bioMod") nq = biorbd_model.nbQ() ntau = biorbd_model.nbGeneralizedTorque() n_cycles = 3 window_len = 5 window_duration = 0.2 x_init = InitialGuess(np.zeros((nq * 2, 1)), interpolation=InterpolationType.CONSTANT) u_init = InitialGuess(np.zeros((ntau, 1)), interpolation=InterpolationType.CONSTANT) x_bounds = Bounds( np.zeros((nq * 2, window_len + 1)), np.zeros((nq * 2, window_len + 1)), interpolation=InterpolationType.EACH_FRAME, ) u_bounds = Bounds(np.zeros((ntau, 1)), np.zeros((ntau, 1))) mhe = MovingHorizonEstimator( biorbd_model, Dynamics(DynamicsFcn.TORQUE_DRIVEN), window_len, window_duration, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, n_threads=4, ) def update_functions(mhe, t, _): return t < n_cycles with pytest.raises( NotImplementedError, match="The MHE is not implemented yet for x_bounds not being " "CONSTANT or CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT", ): mhe.solve(update_functions, Solver.IPOPT)
def prepare_nmpc(model_path, cycle_len, cycle_duration, n_cycles_simultaneous, n_cycles_to_advance, max_torque): model = biorbd.Model(model_path) dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) x_bound = QAndQDotBounds(model) x_bound.min[0, :] = -2 * np.pi * n_cycles_simultaneous # Allow the wheel to spin as much as needed x_bound.max[0, :] = 0 u_bound = Bounds([-max_torque] * model.nbQ(), [max_torque] * model.nbQ()) x_init = InitialGuess( np.zeros( model.nbQ() * 2, ) ) u_init = InitialGuess( np.zeros( model.nbQ(), ) ) new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_STATE, key="q") # Rotate the wheel and force the marker of the hand to follow the marker on the wheel wheel_target = np.linspace(-2 * np.pi * n_cycles_simultaneous, 0, cycle_len * n_cycles_simultaneous + 1)[ np.newaxis, : ] constraints = ConstraintList() constraints.add(ConstraintFcn.TRACK_STATE, key="q", index=0, node=Node.ALL, target=wheel_target) constraints.add( ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.ALL, first_marker="wheel", second_marker="COM_hand", axes=[Axis.X, Axis.Y], ) return MyCyclicNMPC( model, dynamics, cycle_len=cycle_len, cycle_duration=cycle_duration, n_cycles_simultaneous=n_cycles_simultaneous, n_cycles_to_advance=n_cycles_to_advance, objective_functions=new_objectives, constraints=constraints, x_init=x_init, u_init=u_init, x_bounds=x_bound, u_bounds=u_bound, )
def prepare_ocp(biorbd_model_path: str, final_time: float, n_shooting: int) -> 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 """ biorbd_model = biorbd.Model(biorbd_model_path) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint torque_min, torque_max, torque_init = -100, 100, 0 n_tau = biorbd_model.nbGeneralizedTorque() u_bounds = Bounds([torque_min] * n_tau, [torque_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([torque_init] * n_tau) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, )
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_ocp( biorbd_model_path, final_time, number_shooting_points, nb_threads, use_SX=False, ode_solver=OdeSolver.RK ): # --- 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 = Objective(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE_DERIVATIVE) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions=objective_functions, nb_threads=nb_threads, use_SX=use_SX, ode_solver=ode_solver, )
def prepare_ocp(biorbd_model_path: str = "models/mass_point.bioMod"): # Model path m = biorbd.Model(biorbd_model_path) m.setGravity(np.array((0, 0, 0))) # Add objective functions (high upward velocity at end point) objective_functions = Objective(ObjectiveFcn.Mayer.MINIMIZE_STATE, key="qdot", index=0, weight=-1) # Dynamics dynamics = Dynamics(custom_configure, dynamic_function=custom_dynamic) # Path constraint x_bounds = QAndQDotBounds(m) x_bounds[:, 0] = [0] * m.nbQ() + [0] * m.nbQdot() x_bounds.min[:, 1] = [-1] * m.nbQ() + [-100] * m.nbQdot() x_bounds.max[:, 1] = [1] * m.nbQ() + [100] * m.nbQdot() x_bounds.min[:, 2] = [-1] * m.nbQ() + [-100] * m.nbQdot() x_bounds.max[:, 2] = [1] * m.nbQ() + [100] * m.nbQdot() # Initial guess x_init = InitialGuess([0] * (m.nbQ() + m.nbQdot())) # Define control path constraint u_bounds = Bounds([-100] * m.nbGeneralizedTorque(), [0] * m.nbGeneralizedTorque()) u_init = InitialGuess([0] * m.nbGeneralizedTorque()) return OptimalControlProgram( m, dynamics, n_shooting=30, phase_time=0.5, x_init=x_init, u_init=u_init, x_bounds=x_bounds, u_bounds=u_bounds, objective_functions=objective_functions, )
def prepare_ocp(biorbd_model_path, nbs, tf, ode_solver=OdeSolver.RK, use_SX=True): # --- Options --- # # Model path biorbd_model = biorbd.Model(biorbd_model_path) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) # 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, nbs, tf, x_init, u_init, x_bounds, u_bounds, ode_solver=ode_solver, use_SX=use_SX, )
def prepare_mhe(biorbd_model, window_len, window_duration, max_torque, x_init, u_init): new_objectives = Objective(ObjectiveFcn.Lagrange.MINIMIZE_MARKERS, node=Node.ALL, weight=1000, list_index=0) return MovingHorizonEstimator( biorbd_model, Dynamics(DynamicsFcn.TORQUE_DRIVEN), window_len, window_duration, objective_functions=new_objectives, x_init=InitialGuess(x_init, interpolation=InterpolationType.EACH_FRAME), u_init=InitialGuess(u_init, interpolation=InterpolationType.EACH_FRAME), x_bounds=QAndQDotBounds(biorbd_model), u_bounds=Bounds([-max_torque, 0.0], [max_torque, 0.0]), n_threads=4, )
def prepare_ocp(biorbd_model_path, final_time, number_shooting_points): # --- Options --- # biorbd_model = biorbd.Model(biorbd_model_path) torque_min, torque_max, torque_init = -100, 100, 0 n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint u_bounds = Bounds([torque_min] * n_tau, [torque_max] * n_tau) u_bounds[n_tau - 1, :] = 0 u_init = InitialGuess([torque_init] * n_tau) # ------------- # return OptimalControlProgram( biorbd_model, dynamics, number_shooting_points, final_time, x_init, u_init, x_bounds, u_bounds, )
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(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: 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 test_add_wrong_param(): g_min, g_max, g_init = -10, -6, -8 def my_parameter_function(biorbd_model, value, extra_value): biorbd_model.setGravity(biorbd.Vector3d(0, 0, value + extra_value)) def my_target_function(ocp, value, target_value): return value + target_value parameters = ParameterList() initial_gravity = InitialGuess(g_init) bounds_gravity = Bounds(g_min, g_max, interpolation=InterpolationType.CONSTANT) parameter_objective_functions = Objective( my_target_function, weight=10, quadratic=True, custom_type=ObjectiveFcn.Parameter, target_value=-8) with pytest.raises( RuntimeError, match= "function, initial_guess, bounds and size are mandatory elements to declare a parameter" ): parameters.add( "gravity_z", [], initial_gravity, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) with pytest.raises( RuntimeError, match= "function, initial_guess, bounds and size are mandatory elements to declare a parameter" ): parameters.add( "gravity_z", my_parameter_function, None, bounds_gravity, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) with pytest.raises( RuntimeError, match= "function, initial_guess, bounds and size are mandatory elements to declare a parameter" ): parameters.add( "gravity_z", my_parameter_function, initial_gravity, None, size=1, penalty_list=parameter_objective_functions, extra_value=1, ) with pytest.raises( RuntimeError, match= "function, initial_guess, bounds and size are mandatory elements to declare a parameter" ): parameters.add( "gravity_z", my_parameter_function, initial_gravity, bounds_gravity, penalty_list=parameter_objective_functions, extra_value=1, )
def prepare_ocp_custom_objectives( biorbd_model_path, ode_solver=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 = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=2) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_COM_POSITION, node=3) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.START, quadratic=True, first_marker="m0", second_marker="m1", weight=1000, ) objective_functions.add( custom_func_track_markers, custom_type=ObjectiveFcn.Mayer, node=Node.END, quadratic=True, first_marker="m0", second_marker="m2", weight=1000, ) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # 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, ode_solver=ode_solver, )
def prepare_ocp_parameters( biorbd_model_path, final_time, n_shooting, optim_gravity, optim_mass, min_g, max_g, target_g, min_m, max_m, target_m, ode_solver=OdeSolver.RK4(), use_sx=False, ) -> 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 optim_gravity: bool If the gravity should be optimized optim_mass: bool If the mass should be optimized min_g: np.ndarray The minimal value for the gravity max_g: np.ndarray The maximal value for the gravity target_g: np.ndarray The target value for the gravity min_m: float The minimal value for the mass max_m: float The maximal value for the mass target_m: float The target value for the mass 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 --- # biorbd_model = biorbd.Model(biorbd_model_path) n_tau = biorbd_model.nbGeneralizedTorque() # Add objective functions objective_functions = ObjectiveList() objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_TORQUE, weight=10) objective_functions.add(ObjectiveFcn.Mayer.MINIMIZE_TIME, weight=10) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN) # Path constraint x_bounds = QAndQDotBounds(biorbd_model) x_bounds[:, [0, -1]] = 0 x_bounds[1, -1] = 3.14 # Initial guess n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() x_init = InitialGuess([0] * (n_q + n_qdot)) # Define control path constraint tau_min, tau_max, tau_init = -300, 300, 0 u_bounds = Bounds([tau_min] * n_tau, [tau_max] * n_tau) u_bounds[1, :] = 0 u_init = InitialGuess([tau_init] * n_tau) # Define the parameter to optimize parameters = ParameterList() if optim_gravity: # Give the parameter some min and max bounds bound_gravity = Bounds(min_g, max_g, interpolation=InterpolationType.CONSTANT) # and an initial condition initial_gravity = InitialGuess((min_g + max_g) / 2) # and an objective function parameter_objective_functions = Objective( my_target_function, weight=1000, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_g) parameters.add( "gravity_xyz", # The name of the parameter my_parameter_function, # The function that modifies the biorbd model initial_gravity, # The initial guess bound_gravity, # The bounds size= 3, # The number of elements this particular parameter vector has penalty_list= parameter_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([1, 1, 10.0]), extra_value=1, # You can define as many extra arguments as you want ) if optim_mass: bound_mass = Bounds(min_m, max_m, interpolation=InterpolationType.CONSTANT) initial_mass = InitialGuess((min_m + max_m) / 2) mass_objective_functions = Objective( my_target_function, weight=100, quadratic=False, custom_type=ObjectiveFcn.Parameter, target=target_m) parameters.add( "mass", # The name of the parameter set_mass, # The function that modifies the biorbd model initial_mass, # The initial guess bound_mass, # The bounds size= 1, # The number of elements this particular parameter vector has penalty_list= mass_objective_functions, # ObjectiveFcn of constraint for this particular parameter scaling=np.array([10.0]), ) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, x_bounds, u_bounds, objective_functions, parameters=parameters, ode_solver=ode_solver, use_sx=use_sx, )