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 test_simulate_from_initial_single_shoot(): # Load pendulum bioptim_folder = TestUtils.bioptim_folder() pendulum = TestUtils.load_module(bioptim_folder + "/examples/getting_started/example_save_and_load.py") ocp = pendulum.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/getting_started/pendulum.bioMod", final_time=2, n_shooting=10, n_threads=4, ) X = InitialGuess([-1, -2, 1, 0.5]) U = InitialGuess(np.array([[-0.1, 0], [1, 2]]).T, interpolation=InterpolationType.LINEAR) sol = Solution(ocp, [X, U]) controls = sol.controls sol = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS, keep_intermediate_points=True) # Check some of the results states = sol.states q, qdot, tau = states["q"], states["qdot"], controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((-1.0, -2.0))) np.testing.assert_almost_equal(q[:, -1], np.array((-0.70545232, 2.02188073))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((1.0, 0.5))) np.testing.assert_almost_equal(qdot[:, -1], np.array((1.21773723, -0.77896332))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((-0.1, 0.0))) np.testing.assert_almost_equal(tau[:, -2], np.array((0.89, 1.8)))
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_initial_guess_spline(): nb_shoot = 10 spline_time = np.hstack((0.0, 1.0, 2.2, 6.0)) init_val = np.array( [ [0.5, 0.6, 0.2, 0.8], [0.4, 0.6, 0.8, 0.2], [0.0, 0.3, 0.2, 0.5], [0.5, 0.2, 0.9, 0.4], [0.5, 0.6, 0.2, 0.8], [0.5, 0.6, 0.2, 0.8], ] ) nb_elements = init_val.shape[0] # Raise if time is not sent with pytest.raises(RuntimeError): InitialGuess(init_val, interpolation=InterpolationType.SPLINE) init = InitialGuess(init_val, t=spline_time, interpolation=InterpolationType.SPLINE) init.check_and_adjust_dimensions(nb_elements, nb_shoot) time_to_test = [0, nb_shoot // 3, nb_shoot // 2, nb_shoot] expected_matrix = np.array( [ [0.5, 0.4, 0.0, 0.5, 0.5, 0.5], [0.33333333, 0.73333333, 0.23333333, 0.66666667, 0.33333333, 0.33333333], [0.32631579, 0.67368421, 0.26315789, 0.79473684, 0.32631579, 0.32631579], [0.8, 0.2, 0.5, 0.4, 0.8, 0.8], ] ).T for i, t in enumerate(time_to_test): expected_val = expected_matrix[:, i] np.testing.assert_almost_equal(init.init.evaluate_at(t), expected_val)
def prepare_ocp(biorbd_model_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 __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: 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 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_initial_guess_constant(): n_elements = 6 n_shoot = 10 init_val = np.random.random(n_elements, ) init = InitialGuess(init_val, interpolation=InterpolationType.CONSTANT) init.check_and_adjust_dimensions(n_elements, n_shoot) expected_val = init_val for i in range(n_shoot): np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
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 test_initial_guess_each_frame(): n_elements = 6 n_shoot = 10 init_val = np.random.random((n_elements, n_shoot + 1)) init = InitialGuess(init_val, interpolation=InterpolationType.EACH_FRAME) init.check_and_adjust_dimensions(n_elements, n_shoot) for i in range(n_shoot + 1): expected_val = init_val[:, i] np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def test_initial_guess_linear(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random((nb_elements, 2)) init = InitialGuess(init_val, interpolation=InterpolationType.LINEAR) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def main(): # --- Load pendulum --- # ocp = pendulum.prepare_ocp( biorbd_model_path="models/pendulum.bioMod", final_time=2, n_shooting=10, ) # Simulation the Initial Guess # Interpolation: Constant X = InitialGuess([0, 0, 0, 0]) U = InitialGuess([-1, 1]) sol_from_initial_guess = Solution(ocp, [X, U]) s = sol_from_initial_guess.integrate( shooting_type=Shooting.SINGLE_CONTINUOUS) print( f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}" ) # Uncomment the next line to animate the integration # s.animate() # Interpolation: Each frame (for instance, values from a previous optimization or from measured data) U = np.random.rand(2, 11) U = InitialGuess(U, interpolation=InterpolationType.EACH_FRAME) sol_from_initial_guess = Solution(ocp, [X, U]) s = sol_from_initial_guess.integrate( shooting_type=Shooting.SINGLE_CONTINUOUS) print( f"Final position of q from single shooting of initial guess = {s.states['q'][:, -1]}" ) # Uncomment the next line to animate the integration # s.animate() # Uncomment the following lines to graph the solution from initial guesses # sol_from_initial_guess.graphs(shooting_type=Shooting.SINGLE_CONTINUOUS) # sol_from_initial_guess.graphs(shooting_type=Shooting.MULTIPLE) # Simulation of the solution. It is not the graph of the solution, # it is the graph of a Runge Kutta from the solution sol = ocp.solve() s_single = sol.integrate(shooting_type=Shooting.SINGLE_CONTINUOUS) # Uncomment the next line to animate the integration # s_single.animate() print( f"Final position of q from single shooting of the solution = {s_single.states['q'][:, -1]}" ) s_multiple = sol.integrate(shooting_type=Shooting.MULTIPLE, keep_intermediate_points=True) print( f"Final position of q from multiple shooting of the solution = {s_multiple.states['q'][:, -1]}" )
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 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 test_initial_guess_update(): # Load pendulum bioptim_folder = TestUtils.bioptim_folder() pendulum = TestUtils.load_module( bioptim_folder + "/examples/optimal_time_ocp/pendulum_min_time_Mayer.py") ocp = pendulum.prepare_ocp( biorbd_model_path=bioptim_folder + "/examples/optimal_time_ocp/models/pendulum.bioMod", final_time=2, n_shooting=10, ) np.testing.assert_almost_equal(ocp.nlp[0].x_init.init, np.zeros((4, 1))) np.testing.assert_almost_equal(ocp.nlp[0].u_init.init, np.zeros((2, 1))) idx = ocp.v.parameters_in_list.index("time") np.testing.assert_almost_equal( ocp.v.parameters_in_list[idx].initial_guess.init[0, 0], 2) np.testing.assert_almost_equal( ocp.v.init.init, np.concatenate((np.zeros( (4 * 11 + 2 * 10, 1)), [[2]]))) wrong_new_x_init = InitialGuess([1] * 6) new_x_init = InitialGuess([1] * 4) wrong_new_u_init = InitialGuess([3] * 4) new_u_init = InitialGuess([3] * 2) new_time_init = InitialGuess([4]) # No name for the param with pytest.raises( ValueError, match="update_initial_guess must specify a name for the parameters" ): ocp.update_initial_guess(new_x_init, new_u_init, new_time_init) new_time_init.name = "dumb name" with pytest.raises( ValueError, match="update_initial_guess cannot declare new parameters"): ocp.update_initial_guess(new_x_init, new_u_init, new_time_init) new_time_init.name = "time" with pytest.raises(RuntimeError): ocp.update_initial_guess(new_x_init, wrong_new_u_init, new_time_init) with pytest.raises(RuntimeError): ocp.update_initial_guess(wrong_new_x_init, wrong_new_u_init, new_time_init) ocp.update_initial_guess(new_x_init, new_u_init, new_time_init) np.testing.assert_almost_equal(ocp.nlp[0].x_init.init, np.ones((4, 1))) np.testing.assert_almost_equal(ocp.nlp[0].u_init.init, np.ones((2, 1)) * 3) idx = ocp.v.parameters_in_list.index("time") np.testing.assert_almost_equal( ocp.v.parameters_in_list[idx].initial_guess.init[0, 0], 4) np.testing.assert_almost_equal( ocp.v.init.init, np.array([[1, 1, 1, 1] * 11 + [3, 3] * 10 + [4]]).T)
def _set_initial_guess(self, init_file): if init_file is None: x_init = np.zeros((self.n_q * 2, 1)) x_init[:self.n_q, 0] = self.violin.q(self.bow_starting) u_init = np.zeros((self.n_tau + self.n_mus, 1)) self.x_init = InitialGuess(x_init) self.u_init = InitialGuess(u_init) else: _, sol = ViolinOcp.load(init_file) self.x_init = InitialGuess( sol.states["all"], interpolation=InterpolationType.EACH_FRAME) self.u_init = InitialGuess( sol.controls["all"][:, :-1], interpolation=InterpolationType.EACH_FRAME)
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: 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 test_initial_guess_constant_with_first_and_last_different(): nb_elements = 6 nb_shoot = 10 init_val = np.random.random((nb_elements, 3)) init = InitialGuess(init_val, interpolation=InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): if i == 0: expected_val = init_val[:, 0] elif i == nb_shoot: expected_val = init_val[:, 2] else: expected_val = init_val[:, 1] np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def 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 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_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_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 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_single_shooting( biorbd_model_path: str, n_shooting: int, final_time: float, ode_solver: OdeSolver, n_threads: int = 1, use_sx: bool = False, ) -> OptimalControlProgram: """ Prepare the ss Returns ------- The OptimalControlProgram ready to be solved """ biorbd_model = biorbd.Model(biorbd_model_path) # Dynamics dynamics = Dynamics(DynamicsFcn.TORQUE_DRIVEN, implicit_dynamics=False, implicit_soft_contacts=False) # Initial guess x_init = InitialGuess([0] * (biorbd_model.nbQ() + biorbd_model.nbQdot())) # Problem parameters tau_min, tau_max, tau_init = -100, 100, 0 u_init = InitialGuess([tau_init] * biorbd_model.nbGeneralizedTorque()) return OptimalControlProgram( biorbd_model, dynamics, n_shooting, final_time, x_init, u_init, ode_solver=ode_solver, use_sx=use_sx, n_threads=n_threads, )
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 test_initial_guess_custom(): nb_elements = 6 nb_shoot = 10 def custom_bound_func(current_shooting, val, total_shooting): # Linear interpolation created with custom bound function return val[:, 0] + (val[:, 1] - val[:, 0]) * current_shooting / total_shooting init_val = np.random.random((nb_elements, 2)) init = InitialGuess( custom_bound_func, interpolation=InterpolationType.CUSTOM, val=init_val, total_shooting=nb_shoot, ) init.check_and_adjust_dimensions(nb_elements, nb_shoot) for i in range(nb_shoot + 1): expected_val = init_val[:, 0] + (init_val[:, 1] - init_val[:, 0]) * i / nb_shoot np.testing.assert_almost_equal(init.init.evaluate_at(i), expected_val)
def 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, )