def octagon_rs(L):
oct_verts = np.array(mk_regpoly(8, 0.8 * L, offset=np.pi / 8.))
octagon = Simple_Polygon("octagon", oct_verts)
wire_verts = np.array(mk_regpoly(4, 0.4 * L, offset=np.pi / 4))
r1 = [
wire_verts[0],
midpoint(oct_verts[0], oct_verts[1]), oct_verts[1], oct_verts[2],
midpoint(oct_verts[2], oct_verts[3]), wire_verts[1]
]
r2 = [
wire_verts[1],
midpoint(oct_verts[2], oct_verts[3]), oct_verts[3], oct_verts[4],
midpoint(oct_verts[4], oct_verts[5]), wire_verts[2]
]
r3 = [
wire_verts[2],
midpoint(oct_verts[4], oct_verts[5]), oct_verts[5], oct_verts[6],
midpoint(oct_verts[6], oct_verts[7]), wire_verts[3]
]
r4 = [
wire_verts[3],
midpoint(oct_verts[6], oct_verts[7]), oct_verts[7], oct_verts[0],
midpoint(oct_verts[0], oct_verts[1]), wire_verts[0]
]
rs = [wire_verts, r1, r2, r3, r4]
rs_as_obs = [mk_obstacle(r) for r in rs]
regions = [
Simple_Polygon("r" + str(i), np.array(vs)) for i, vs in enumerate(rs)
]
return regions
def compute_inserted_polygon(self, polygon, sequence_info):
''' Compute the polygon with all new vertices from the partial local sequence inserted
'''
t_pts_grouped = {i: [] for i in range(polygon.size)}
for i in range(len(sequence_info)):
for (pt, edge, _) in sequence_info[i]:
t_pts_grouped[edge].append(pt)
def get_new_vertices_for_single_polygon(curr_polygon_vx):
# sort transition points along edge
new_poly_grouped = {}
for i in range(len(curr_polygon_vx)):
new_poly_grouped[i] = sort_by_distance(
curr_polygon_vx[i][1],
np.array(t_pts_grouped[curr_polygon_vx[i][0]]))
if DEBUG:
print('Inserted verts', new_poly_grouped[i], 'on edge', i)
inserted_polygon = []
new_vertices = []
for i in range(len(curr_polygon_vx)):
new_vertices.extend(new_poly_grouped[i])
return new_vertices
new_vertices = get_new_vertices_for_single_polygon(
polygon.outer_boundary_vertices)
new_holes = []
for hole in polygon.holes:
new_holes.append(get_new_vertices_for_single_polygon(hole))
inserted_polygon = Simple_Polygon('inserted_' + polygon.name,
np.array(new_vertices), new_holes)
return inserted_polygon
def mk_bounding_box(poly):
vs = poly.vertex_list_per_poly[0]
xs = np.sort([v[0] for i, v in vs])
ys = np.sort([v[1] for i, v in vs])
min_x = xs[0]
max_x = xs[-1]
min_y = ys[0]
max_y = ys[-1]
bb_verts = np.array([(min_x, min_y), (max_x, min_y), (max_x, max_y),
(min_x, max_y)])
bb = Simple_Polygon("bb" + poly.name, bb_verts)
return min_x, max_x, min_y, max_y, bb
def square(L):
return Simple_Polygon("square",
np.array([[0.0, 0.0], [L, 0.0], [L, L], [0.0, L]]))
def spike_annulus(L):
return Simple_Polygon("spk_ring", np.array(mk_spiky_circle(8, 0.5 * L)),
[np.array(mk_obstacle(mk_regpoly(4, 0.4 * L)))])
def spikes(L):
return Simple_Polygon("spikes", np.array(mk_spiky_circle(8, 0.5 * L)))
def square(L):
return Simple_Polygon("square",
np.array([[0.0, 0.0], [L, 0.0], [L, L], [0.0, L]]))
def spikes(L):
return Simple_Polygon("spikes", np.array(mk_spiky_circle(8, 0.5 * L)))
def spike_annulus(L):
return Simple_Polygon("spk_ring", np.array(mk_spiky_circle(8, 0.5 * L)),
[np.array(mk_obstacle(mk_regpoly(4, 0.4 * L)))])
square_hole = Simple_Polygon("sqh", simple_holes[0], simple_holes[1])
l_poly = [
np.array([(0, 0), (60, 0), (60, 23), (26, 23), (26, 46), (0, 46)],
dtype=np.float)
]
# octagon divided into five regions
def octagon_rs(L):
oct_verts = np.array(mk_regpoly(8, 0.8 * L, offset=np.pi / 8.))
octagon = Simple_Polygon("octagon", oct_verts)
wire_verts = np.array(mk_regpoly(4, 0.4 * L, offset=np.pi / 4))
r1 = [
wire_verts[0],
#! /usr/bin/env python3
import sys
sys.path.append('../src')
from helper.visualization_helper import *
from environments.maps import simple_bit
from simple_polygon import Simple_Polygon
from bounce_visibility_diagram import Bounce_Visibility_Diagram
from bounce_graph import Bounce_Graph
from navigation import Navigation
if __name__ == '__main__':
poly = Simple_Polygon("sp", pinched_square[0])
poly_vx = poly.complete_vertex_list
poly_name = "sp"
# generate visibility discretization and visualize
pls = Partial_Local_Sequence(poly)
ins_vs = np.array(pls.inserted_polygon.complete_vertex_list)
visualize_polygon(ins_vs, "test")
bvd = Bounce_Visibility_Diagram(pls)
# compute transition graphs between segments on boundary
bounce_graph = Bounce_Graph(bvd)
visualize_graph(bounce_graph.visibility_graph, "bounce_visibility_graph")
visualize_graph(bounce_graph.safe_action_graph, "bounce_safe_action_graph")
# perform navigation
start = (0.01, 0.02)
goal = (0.51, 0.66)
nav_task = Navigation(start, goal, bounce_graph)
path = nav_task.navigate()
def sweep_envs():
fracs = np.zeros(len(range(2, 6)) * len(range(5, 20, 3)))
params = np.zeros((len(range(2, 6)) * len(range(5, 20, 3)), 2))
for iteration in range(10):
print("Iteration", iteration)
polys = []
for num_rooms in range(2, 6):
for rvx in range(5, 20, 3):
corridors, rooms, tree_map = random_tree(num_rooms)
hobbit_hole = genTreeEnv(num_rooms, rvx, rooms, corridors,
tree_map,
Hallway_Type.RANDOM_DIRECTION)
polys.append((num_rooms, rvx, hobbit_hole))
for i, (num_rooms, rvx, poly) in enumerate(polys):
print("polygon", i)
p = Simple_Polygon("sp", poly)
poly_vx = p.complete_vertex_list
# generate visibility discretization and visualize
pls = Partial_Local_Sequence(p)
ins_vs = np.array(pls.inserted_polygon.complete_vertex_list)
#visualize_polygon(ins_vs, "poly"+str(i))
bvd = Bounce_Visibility_Diagram(pls)
# compute transition graphs between segments on boundary
bounce_graph = Bounce_Graph(bvd)
#visualize_graph(bounce_graph.visibility_graph, "bg"+str(i))
#visualize_graph(bounce_graph.safe_action_graph, "safe_bg"+str(i))
unreach_segs = containsUnreachableSegs(
p, bounce_graph.safe_action_graph)
#if unreach_segs != []:
# print("Visualizing Unreachable Set")
# visualize_subset_polygon(ins_vs, "reachable_boundary"+str(i), unreach_segs)
frac = edgeFrac(pls.inserted_polygon, unreach_segs)
fracs[i] += frac
params[i][0] = num_rooms
params[i][1] = rvx
LOG = True
if LOG:
with open("hobbit_hole_results.txt", 'a+') as f:
f.write("Polygon " + str(i) + '\n')
f.write("num_rooms: " + str(num_rooms) + '\n')
f.write("num vxs per room: " + str(rvx) + '\n')
f.write("fraction unreachable: " + str(frac) + '\n')
f.write('\n')
fracs = fracs / 24.
print(fracs)
print(params)
data = fracs.reshape((len(range(2, 6)), len(range(5, 20, 3))))
plt.imshow(data, cmap='hot', interpolation='nearest')
plt.savefig('hobbit_heatmap.png', dpi=300, bbox_inches='tight')
map = poly.unit_interval_mapping[0]
frac = 0.
for i in edges:
s = map[i + 1] - map[i]
frac += s
return frac
num_rooms = 2
rvx = 10
corridors, rooms, tree_map = random_tree(num_rooms)
poly = genTreeEnv(num_rooms, rvx, rooms, corridors, tree_map,
Hallway_Type.RANDOM_DIRECTION)
p = Simple_Polygon("sp", poly)
poly_vx = p.complete_vertex_list
# generate visibility discretization and visualize
pls = Partial_Local_Sequence(p)
ins_vs = np.array(pls.inserted_polygon.complete_vertex_list)
print("visualizing polygon")
visualize_polygon(ins_vs, "hobbit_inserted")
bvd = Bounce_Visibility_Diagram(pls)
# compute transition graphs between segments on boundary
bounce_graph = Bounce_Graph(bvd)
print("visualizing graphs")
visualize_graph(bounce_graph.visibility_graph, "bg_hobbit")
visualize_graph(bounce_graph.safe_action_graph, "safe_bg_hobbit")
unreach_segs = containsUnreachableSegs(p, bounce_graph.safe_action_graph)
def simple_nonconv_p():
return Simple_Polygon("simple_nonconv", simple_nonconv[0])
def shelfp():
return Simple_Polygon("shelf", shelf[0])
def hallway_obs():
return Simple_Polygon("hallway", rectangle_hole[0], rectangle_hole[1])
def square_hole():
return Simple_Polygon("sqh", simple_holes[0], simple_holes[1])
def octagon(L):
return Simple_Polygon("octagon", np.array(mk_regpoly(8, L)))
