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()
for k, v in dict.items():
for v_i in v:
G.add_edge(k, v_i)
return G
def generate_path_list(a,b, start,end):
G = generate_graph(a,b)
start_node = point_in_which_regions(start, a, b)
end_node = point_in_which_regions(end, a, b)
assert len(start_node) > 0
assert len(end_node) > 0
#lets assume start and end only belong in one region for now
#TODO: add more regions if we can handle it
return [p for p in all_simple_paths(G, start_node[0], end_node[0])]
if __name__ == "__main__":
s = (10, 10)
e = (90, 90)
x_b = (0, 100)
y_b = (0, 100)
fig = plt.figure()
ax = fig.add_subplot(111)
a, b = load_folder_and_plot(ax, s, e, "envs/2", x_b, y_b) # read and plot
fig.show()
fig = plt.figure(2)
G = generate_graph(a,b)
start_node = point_in_which_regions(s, a, b)
end_node = point_in_which_regions(e, a, b)
networkx.draw(G)
fig.show()
print([p for p in all_simple_paths(G, start_node[0], end_node[0])])
import numpy as np
import reinforce as rf
np.random.seed(1)
START = (10, 10, 0, 0)
END = (90, 90, 0, 0)
OBSTACLES = None
t = 200
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