def test_new_gen_env(): from env_gen import load_folder_and_plot from obstacle_environment import ObstacleEnvironment2D from mip_planner import MIPPlanner, TESTA, TESTB from core.dynamics.linear_system_dynamics import LinearSystemDynamics import matplotlib.pyplot as plt START = (15, 10, 0, 0) END = (90, 90, 0, 0) OBSTACLES = None x_b = (0, 100) y_b = (0, 100) fig = plt.figure() ax = fig.add_subplot(111) a, b = load_folder_and_plot(ax, (START[0], START[1]), (END[0], END[1]), "../envs/2", x_b, y_b) # read and plot START_ENV = ObstacleEnvironment2D(START, END, OBSTACLES, a, b) planner = MIPPlanner(START_ENV, LinearSystemDynamics(TESTA, TESTB), T=50, h_k=0.01, presolve=0) planner.set_optim_timelimit(180) wp, _, _, _, _, _ = planner.optimize_path() START_ENV.plot_path(wp, ax) ax.set_xlim(-50, 200) ax.set_ylim(-50, 200) fig.show()
fig = plt.figure() ax = fig.add_subplot(111) a, b = load_folder_and_plot(ax, (START[0], START[1]), (END[0], END[1]), "envs/10", x_b, y_b) # read and plot paths = generate_path_list(a, b, START, END) print(paths) # pick a path p = paths[np.random.randint(len(paths))] print(p) START_ENV = ObstacleEnvironment2D(START, END, OBSTACLES, a, b) planner = MIPPlanner(START_ENV, LinearSystemDynamics(TESTA, TESTB), T=t, h_k=0.001, presolve=2) planner.set_optim_timelimit(10) last_inactive_set = np.zeros((t, len(a)), dtype=bool) for row in range(0, int(t / time_steps)): last_inactive_set[row][p[0]] = True last_inactive_set[180 + row][p[-1]] = True curr_reg = 0 wp = None ob = None runtime = 0 for step in range(0, time_steps): print("STEP " + str(step)) active_set = np.zeros((t, len(a)), dtype=bool) inactive_set = last_inactive_set.copy() for row in range(0, int(t / time_steps)): active_set[20 * step + row] = True for step2 in range(step + 1, time_steps - 1):