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)))