def test_acados_custom_dynamics(problem_type_custom): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.getting_started import custom_dynamics as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube.bioMod", problem_type_custom=problem_type_custom, ode_solver=OdeSolver.RK4(), use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m2") ocp.update_constraints(constraints) sol = ocp.solve(solver=Solver.ACADOS()) # Check some of the results q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] # initial and final position np.testing.assert_almost_equal(q[:, 0], np.array((2, 0, 0)), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57))) # initial and final velocities np.testing.assert_almost_equal(qdot[:, 0], np.array((0, 0, 0))) np.testing.assert_almost_equal(qdot[:, -1], np.array((0, 0, 0))) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0, 9.81, 2.27903226))) np.testing.assert_almost_equal(tau[:, -2], np.array((0, 9.81, -2.27903226)))
def 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_options(cost_type): if platform == "win32" or platform == "darwin": print("Tests for ACADOS options on Windows and Mac are skipped") return from bioptim.examples.acados import pendulum 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=0.6, n_shooting=200, ) tols = [1e-1, 1e1] iter = [] for tol in tols: solver = Solver.ACADOS() solver.set_cost_type(cost_type) solver.set_nlp_solver_tol_stat(tol) sol = ocp.solve(solver=solver) iter += [sol.iterations] # Check that tol impacted convergence for i in range(len(tols) - 1): np.testing.assert_array_less(iter[i + 1], iter[i]) # 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 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, 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) 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) 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_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 main(): """ If pendulum is run as a script, it will perform the optimization using ACADOS and animates it """ ocp = prepare_ocp(biorbd_model_path="models/pendulum.bioMod", final_time=1, n_shooting=100) # --- Solve the program --- # sol = ocp.solve(solver=Solver.ACADOS()) # --- Show results --- # sol.print_cost() sol.graphs() sol.animate()
def test_acados_one_end_constraints(): 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__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/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 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_acados_no_obj(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, ) solver = Solver.ACADOS() solver.set_cost_type(cost_type) sol = ocp.solve(solver=solver) # Clean test folder os.remove(f"./acados_ocp.json") shutil.rmtree(f"./c_generated_code/")
def test_acados_fail_external(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.acados import pendulum 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=2, ) solver = Solver.ACADOS() solver.set_cost_type("EXTERNAL") with pytest.raises(RuntimeError, match="EXTERNAL is not interfaced yet, please use NONLINEAR_LS"): sol = ocp.solve(solver=solver)
def test_acados_constraints_end_all(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.track import track_marker_on_segment as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add(ConstraintFcn.SUPERIMPOSE_MARKERS, node=Node.END, first_marker="m0", second_marker="m5") constraints.add( ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL_SHOOTING, marker="m1", segment="seg_rt", axis=Axis.X ) 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[:, 0], np.array([2.01701330, 0, 0, 3.20057865e-01]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2, 0, 1.57, 7.85398168e-01)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0.04648408, 9.88616194, 2.24285498, 0.864213)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((0.19389194, 9.99905781, -2.37713652, -0.19858311)), decimal=6)
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_constraints_all(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.track import track_marker_on_segment as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/cube_and_line.bioMod", n_shooting=30, final_time=2, initialize_near_solution=True, constr=False, use_sx=True, ) constraints = ConstraintList() constraints.add( ConstraintFcn.TRACK_MARKER_WITH_SEGMENT_AXIS, node=Node.ALL, marker="m1", segment="seg_rt", axis=Axis.X ) 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[:, 0], np.array([2.28988221, 0, 0, 2.95087911e-01]), decimal=6) np.testing.assert_almost_equal(q[:, -1], np.array((2.28215749, 0, 1.57, 6.62470772e-01)), decimal=6) np.testing.assert_almost_equal(qdot[:, 0], np.array([0, 0, 0, 0]), decimal=6) np.testing.assert_almost_equal(qdot[:, -1], np.array([0, 0, 0, 0]), decimal=6) # initial and final controls np.testing.assert_almost_equal(tau[:, 0], np.array((0.04483914, 9.90739842, 2.24951691, 0.78496612)), decimal=6) np.testing.assert_almost_equal(tau[:, -2], np.array((0.15945561, 10.03978178, -2.36075327, 0.07267697)), decimal=6)
def test_acados_fail_lls(): if platform == "win32": print("Test for ACADOS on Windows is skipped") return from bioptim.examples.acados import static_arm as ocp_module bioptim_folder = os.path.dirname(ocp_module.__file__) ocp = ocp_module.prepare_ocp( biorbd_model_path=bioptim_folder + "/models/arm26.bioMod", final_time=1, n_shooting=2, use_sx=True, ) solver = Solver.ACADOS() solver.set_cost_type("LINEAR_LS") with pytest.raises( RuntimeError, match="SUPERIMPOSE_MARKERS is an incompatible objective term with LINEAR_LS cost type" ): sol = ocp.solve(solver=solver)
def main(): # Options warm_start_ipopt_from_acados_solution = False # --- Solve the program using ACADOS --- # ocp_acados = prepare_ocp(biorbd_model_path="models/arm26.bioMod", final_time=2, n_shooting=51, use_sx=True) solver_acados = Solver.ACADOS() solver_acados.set_convergence_tolerance(1e-3) sol_acados = ocp_acados.solve(solver=solver_acados) # --- Solve the program using IPOPT --- # x_warm = sol_acados[ "qqdot"] if warm_start_ipopt_from_acados_solution else None ocp_ipopt = prepare_ocp( biorbd_model_path="models/arm26.bioMod", final_time=2, x_warm=x_warm, n_shooting=51, use_sx=False, n_threads=6, ) solver_ipopt = Solver.IPOPT() solver_ipopt.set_linear_solver("ma57") solver_ipopt.set_dual_inf_tol(1e-3) solver_ipopt.set_constraint_tolerance(1e-3) solver_ipopt.set_convergence_tolerance(1e-3) solver_ipopt.set_maximum_iterations(100) solver_ipopt.set_hessian_approximation("exact") sol_ipopt = ocp_ipopt.solve(solver=solver_ipopt) # --- Show results --- # print("\n\n") print("Results using ACADOS") print(f"Final objective: {np.nansum(sol_acados.cost)}") sol_acados.print_cost() print(f"Time to solve: {sol_acados.real_time_to_optimize}sec") print(f"") print( f"Results using Ipopt{'' if warm_start_ipopt_from_acados_solution else ' not'} " f"warm started from ACADOS solution") print(f"Final objective : {np.nansum(sol_ipopt.cost)}") sol_ipopt.print_cost() print(f"Time to solve: {sol_ipopt.real_time_to_optimize}sec") print(f"") visualizer = sol_acados.animate(show_now=False) visualizer.extend(sol_ipopt.animate(show_now=False)) # Update biorbd-viz by hand so they can be visualized simultaneously should_continue = True while should_continue: for i, b in enumerate(visualizer): if b.vtk_window.is_active: b.update() else: should_continue = False
def main(): model_path = "models/cube.bioMod" ns = 30 tf = 2 ocp = prepare_ocp(biorbd_model_path=model_path, n_shooting=ns, tf=tf) # --- Add objective functions --- # objective_functions = ObjectiveList() objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", weight=1000, index=[0, 1], target=np.array([[1.0, 2.0]]).T, multi_thread=False, ) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", weight=10000, index=[2], target=np.array([[3.0]]), multi_thread=False, ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=1, multi_thread=False) ocp.update_objectives(objective_functions) # --- Solve the program --- # solver = Solver.ACADOS() sol = ocp.solve(solver) sol.graphs() objective_functions = ObjectiveList() objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", weight=1, index=[0, 1], target=np.array([[1.0, 2.0]]).T, multi_thread=False, ) objective_functions.add( ObjectiveFcn.Mayer.MINIMIZE_STATE, key="q", weight=10000, index=[2], target=np.array([[3.0]]), multi_thread=False, ) objective_functions.add(ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="tau", weight=10, multi_thread=False) ocp.update_objectives(objective_functions) solver.set_nlp_solver_tol_stat(1e-2) sol = ocp.solve(solver) # --- Show results --- # sol.graphs() sol.animate()
def main(): biorbd_model_path = "models/cart_pendulum.bioMod" biorbd_model = biorbd.Model(biorbd_model_path) solver = Solver.ACADOS() # or Solver.IPOPT() final_time = 5 n_shoot_per_second = 100 window_len = 10 window_duration = 1 / n_shoot_per_second * window_len n_frames_total = final_time * n_shoot_per_second - window_len - 1 x0 = np.array([0, np.pi / 2, 0, 0]) noise_std = 0.05 # STD of noise added to measurements torque_max = 2 # Max torque applied to the model states, markers, markers_noised, controls = generate_data( biorbd_model, final_time, x0, torque_max, n_shoot_per_second * final_time, noise_std, show_plots=False) x_init = np.zeros((biorbd_model.nbQ() * 2, window_len + 1)) u_init = np.zeros((biorbd_model.nbQ(), window_len)) torque_max = 5 # Give a bit of slack on the max torque biorbd_model = biorbd.Model(biorbd_model_path) mhe = prepare_mhe( biorbd_model, window_len=window_len, window_duration=window_duration, max_torque=torque_max, x_init=x_init, u_init=u_init, ) def update_functions(mhe, t, _): def target(i: int): return markers_noised[:, :, i:i + window_len + 1] mhe.update_objectives_target(target=target(t), list_index=0) return t < n_frames_total # True if there are still some frames to reconstruct # Solve the program sol = mhe.solve(update_functions, **get_solver_options(solver)) print("ACADOS with Bioptim") print(f"Window size of MHE : {window_duration} s.") print(f"New measurement every : {1 / n_shoot_per_second} s.") print( f"Average time per iteration of MHE : {sol.solver_time_to_optimize / (n_frames_total - 1)} s." ) print( f"Average real time per iteration of MHE : {sol.real_time_to_optimize / (n_frames_total - 1)} s." ) print( f"Norm of the error on state = {np.linalg.norm(states[:, :n_frames_total] - sol.states['all'])}" ) markers_estimated = states_to_markers(biorbd_model, sol.states["all"]) plt.plot( markers_noised[1, :, :n_frames_total].T, markers_noised[2, :, :n_frames_total].T, "x", label="Noised markers trajectory", ) plt.gca().set_prop_cycle(None) plt.plot(markers[1, :, :n_frames_total].T, markers[2, :, :n_frames_total].T, label="True markers trajectory") plt.gca().set_prop_cycle(None) plt.plot(markers_estimated[1, :, :].T, markers_estimated[2, :, :].T, "o", label="Estimated marker trajectory") plt.legend() plt.figure() plt.plot(sol.states["all"].T, "--", label="States estimate") plt.plot(states[:, :n_frames_total].T, label="State truth") plt.legend() plt.show() sol.animate()