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)