def test_mip_initalization_default(): planner = MIPPlanner(DEFAULT_ENV, LinearSystemDynamics(TESTA, TESTB), T=200, h_k=0.001) n_regions = len(DEFAULT_ENV.get_cvx_ineqs()[0]) n_timesteps = planner.T # active_set = np.zeros((n_timesteps, n_regions), dtype=bool) # active_set[:100, 0] = True # active_set[100:, 2] = True # active_set[120:, 3] = True # active_set_vals = active_set.copy() active_set = None active_set_vals = None x, u, obj, active_set, inactive_set_val, rt = planner.optimize_path( active_set, active_set_vals) x_inited, u_inited, obj_inited, as_inited,\ is_val_inited, rt_inited = \ planner.optimize_path(np.zeros_like(active_set, dtype=bool), inactive_set_val, initial_soln=(x, u)) # ax = START_ENV.plot_path(x) # START_ENV.plot_path(x_inited, ax) # plt.show() np.testing.assert_allclose(x, x_inited) np.testing.assert_allclose(u, u_inited) np.testing.assert_allclose(inactive_set_val, is_val_inited) np.testing.assert_allclose([obj], [obj_inited]) assert rt_inited < rt
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()
def test_mip_planner(): planner = MIPPlanner(DEFAULT_ENV, LinearSystemDynamics(TESTA, TESTB), T=200, h_k=0.001) x, u, _, _, _, _ = planner.optimize_path() np.testing.assert_allclose(x[0], DEFAULT_ENV.start) np.testing.assert_allclose(x[-1], DEFAULT_ENV.end) assert x[70][1] < 5 # bottom right corner
def test_mip_discrete_regions(): planner = MIPPlanner(DEFAULT_ENV, LinearSystemDynamics(TESTA, TESTB), T=200, h_k=0.001) n_regions = len(DEFAULT_ENV.get_cvx_ineqs()[0]) n_timesteps = planner.T active_set = np.zeros((n_timesteps, n_regions), dtype=bool) active_set[:100, 0] = True active_set[100:, 2] = True active_set[120:, 3] = True active_set_vals = active_set x, u, _, _, _, _ = planner.optimize_path(active_set, active_set_vals) np.testing.assert_allclose(x[0], DEFAULT_ENV.start) np.testing.assert_allclose(x[-1], DEFAULT_ENV.end) assert x[90][0] < 10 and x[90][1] > 80 # top left corner
help='render the environment') parser.add_argument('--log-interval', type=int, default=10, metavar='N', help='interval between training status logs (default: 10)') args = parser.parse_args() torch.autograd.set_detect_anomaly(True) T = 200 #number of timesteps NSection = 10 obs_env = DEFAULT_ENV dyn = DEFAULT_DYN NCVX = obs_env.get_num_cvx() planner = MIPPlanner(obs_env, dyn, T, h_k=1e-3, presolve=0) env = GymOptEnv(planner, T, obs_env.get_num_cvx(), args.seed) env.seed(args.seed) torch.manual_seed(args.seed) class RandomPolicy(nn.Module): def __init__(self, ncvx): super(RandomPolicy, self).__init__() self.delta_t = T // NSection self.delta_t_i = 1 self.ncvx = ncvx #AB to H self.Asl1 = nn.Linear(self.ncvx * 4, 128)
time_steps = 10 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/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()
sol[v] = 0 return sol, init_obj START = (10, 10, 50, 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/1", x_b, y_b) # read and plot START_ENV = ObstacleEnvironment2D(START, END, OBSTACLES, a, b) planner = MIPPlanner(START_ENV, LinearSystemDynamics(TESTA, TESTB), T=200, h_k=0.001) start_time = time.time() t, _, _, _, _ = LNS(planner, 5, 10, time_limit=1, verbose=True) end_time = time.time() total_t = end_time - start_time print('solver time: ', t) print('total time: ', total_t) status = P.optimize(max_seconds=total_t * 10) print(status) print(P.objective_value)