def create_cone(point, radio, angle, opening, resolution=1): # Define the list for the points of the cone-shape polygon p = [] # The fisrt point will be the vertex of the cone p.append(vis.Point(point[0], point[1])) # Define the start and end of the arc start = angle - opening end = angle + opening for i in range(start, end, resolution): # Convert start angle from degrees to radians rad = math.radians(i) # Calculate the off-set of the first point of the arc x = radio * math.cos(rad) y = radio * math.sin(rad) # Add the off-set to the vertex point new_x = point[0] + x new_y = point[1] + y # Add the first point of the arc to the list p.append(vis.Point(new_x, new_y)) # Add the last point of the arc rad = math.radians(end) x = radio * math.cos(rad) y = radio * math.sin(rad) new_x = point[0] + x new_y = point[1] + y p.append(vis.Point(new_x, new_y)) return vis.Polygon(p)
def find_shortest_path(self): base_energy_start = self.aggregated_power_lower[0][1] base_energy_end = self.aggregated_power_lower[-2][1] if self.strategy == 'full-empty': strategy_state = (1, 0) elif self.strategy == 'full-full': strategy_state = (1, 1) elif self.strategy == 'empty-empty': strategy_state = (0, 0) elif isinstance(self.strategy, tuple): strategy_state = self.strategy else: print('This operation strategy is not supported!') self.start_energy = base_energy_start + strategy_state[ 0] * self.battery_capacity * (1 - self.DOD) # TODO this is hard coded self.end_energy = base_energy_end + strategy_state[ 1] * self.battery_capacity start = vis.Point(0, self.start_energy) end = vis.Point(self.time - 1, self.end_energy) start.snap_to_boundary_of(self.env, self.epsilon) start.snap_to_vertices_of(self.env, self.epsilon) vis_poly = vis.Visibility_Polygon(start, self.env, self.epsilon) shortest_path = self.env.shortest_path(start, end, self.epsilon) return shortest_path
def get_instance(): sites = [(600, 550), (300, 400), (50, 100), (800, 100), (950, 980)] objects, elements, corners = [], [], [] wall = [(0, 0), (1000, 0), (1000, 1000), (0, 1000)] objects.append(wall) elements.append(vis.Polygon([vis.Point(x, y) for x, y in wall])) hole1 = [(100, 300), (100, 500), (150, 500), (150, 300)] objects.append(hole1) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole1])) corners += hole1 hole2 = [(300, 700), (300, 800), (550, 800), (550, 700)] objects.append(hole2) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole2])) corners += hole2 hole3 = [(700, 300), (700, 650), (850, 650), (850, 300)] objects.append(hole3) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole3])) corners += hole3 hole4 = [(300, 100), (300, 200), (450, 200), (450, 100)] objects.append(hole4) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole4])) corners += hole4 env = vis.Environment(elements) return sites, objects, env, corners
def visibleVertices(curr_poly_vx, vertex_list_per_poly, orig_poly, j): vs = [v for i, v in orig_poly.vertex_list_per_poly[0]] pts = list(map(lambda x: vis.Point(x[0], x[1]), vs)) wall_x = [pt.x() for pt in pts] wall_y = [pt.y() for pt in pts] wall_x.append(pts[0].x()) wall_y.append(pts[0].y()) def get_vis_form_from_vx_list(vs): poly = list(map(lambda x: vis.Point(x[1][0], x[1][1]), vs)) walls = vis.Polygon(poly) walls.enforce_standard_form() walls.eliminate_redundant_vertices() return walls env_walls = [ get_vis_form_from_vx_list(vxs) for vxs in orig_poly.vertex_list_per_poly ] # point from which to calculate visibility p1 = curr_poly_vx[j][1] vp1 = vis.Point(p1[0], p1[1]) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes env = vis.Environment(env_walls) # Necesary to generate the visibility polygon vp1.snap_to_boundary_of(env, EPSILON) vp1.snap_to_vertices_of(env, EPSILON) isovist = vis.Visibility_Polygon(vp1, env, EPSILON) vvs = [(isovist[i].x(), isovist[i].y()) for i in range(isovist.n())] if DEBUG: print(curr_poly_vx) print("visibility polygon is:", vvs) point_x, point_y = save_print(isovist) point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) plt.plot(wall_x, wall_y, 'black') plt.plot(point_x, point_y, 'r') plt.plot([vp1.x()], [vp1.y()], 'go') plt.savefig("viz_test.png") plt.clf() visibleVertexSet = [] for poly_vx in vertex_list_per_poly: curr_vis_vx = [] for i in range(len(poly_vx)): p = vis.Point(*poly_vx[i][1]) vp = copy(p).projection_onto_boundary_of(isovist) if (vis.distance(vp, p) < EPSILON) and poly_vx[i][0] != curr_poly_vx[j][0]: curr_vis_vx.append(i) if DEBUG: print('vertices', curr_vis_vx, 'visible from', j) visibleVertexSet.append(curr_vis_vx) return visibleVertexSet
def environments(epsilon=0.0000001): wall_choords = [(0, 0), (700, 0), (700, 900), (0, 900)] wall = vis.Polygon([vis.Point(x, y) for x, y in wall_choords]) holes_choords = [[(100, 300), (100, 500), (150, 500), (150, 300)], [(300, 300), (300, 500), (400, 550), (400, 300)], [(90, 700), (250, 750), (220, 600), (150, 600)], [(330, 700), (330, 800), (530, 850), (530, 790)], [(230, 50), (250, 90), (390, 90), (390, 50)]] holes = [] for hc in holes_choords: holes.append(vis.Polygon([vis.Point(x, y) for x, y in hc])) draw_env(wall_choords, holes_choords) env = vis.Environment([wall] + holes) return env, vis.Visibility_Graph(env, epsilon)
def visible_area(self, x: float, y: float): """ Computes the visible area from point (x,y) :param x: x coordinate of view point :param y: y coordinate of view point :return: Visibile area from (x,y) """ # Define the point of the "observer" observer = vis.Point(x, y) # Necessary to generate the visibility polygon observer.snap_to_boundary_of(self.env, self.epsilon) observer.snap_to_vertices_of(self.env, self.epsilon) # Obtain the visibility polygon of the 'observer' in the environment # previously defined isovist = vis.Visibility_Polygon(observer, self.env, self.epsilon) points = [] for i in range(isovist.n()): points.append((isovist[i].x(), isovist[i].y())) poly = Polygon(points) return poly
def __get_vis(self): poly = [] for p in self.path.to_polygons(): poly.append(vis.Polygon([vis.Point(*v) for v in p[:-1][::-1]])) env = vis.Environment(poly) vg = vis.Visibility_Graph(env, self.epsilon) return env, vg
def filter_points(self,points): res_points = [] for px,py in points: vpoint = vis.Point(px,py) if vpoint._in(self.environment,EPSILON): res_points.append((px,py)) return res_points
def find_obstacles(occ_grid, thresh=125, unknown=0): # occ_grid needs to be cv2 object # returns list of obstacles (list of polygons) # threshold is a little low because I was trying to exclude unknown areas as obstacles grid = occ_grid.copy() grid[np.where(grid == unknown)] = 255 thresh_grid = cv2.threshold(grid, thresh, 255, cv2.THRESH_BINARY)[1] thresh_grid = np.array(thresh_grid, dtype=np.uint8) # cnts = cv2.findContours(thresh.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cnts = cv2.findContours(thresh_grid, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) # set contours as visilibity polygons obstacles = [] for c in cnts: points = [] for p in c: points.append(vis.Point(float(p[0][0]), float(p[0][1]))) obstacle = vis.Polygon(points) obstacle.eliminate_redundant_vertices(redundant_eps) obstacles.append(obstacle) return obstacles
def __init__(self,blocker_polygons,width,height): self.environment = vis.Environment() for poly in blocker_polygons: points = [vis.Point(x,y) for x,y in poly] #self.polys.append(vis.Polygon(points)) poly = vis.Polygon(points) if poly.area() > 0: poly.reverse() self.environment.add_hole(poly) self.environment.set_outer_boundary(vis.Polygon([ vis.Point(0,0), vis.Point(width,0), vis.Point(width,height), vis.Point(0,height), ]))
def outer(y_up, y_down, x_left, x_right): ## use arena coordinates #list points ccw a = vis.Point(x_left, y_size - (y_down - 1)) b = vis.Point(x_right, y_size - (y_down - 1)) c = vis.Point(x_right, y_size - (y_up - 1)) d = vis.Point(x_left, y_size - (y_up - 1)) # Values for graph wall_x = [a.x(), b.x(), c.x(), d.x(), a.x()] wall_y = [a.y(), b.y(), c.y(), d.y(), a.y()] # Create the outer boundary polygon walls = vis.Polygon([a, b, c, d]) return wall_x, wall_y, walls
def paint_cpoint_on_img(img, vis_env, obstacles, inspection_points, c_point, scale=50, eps=1e-7, max_wall=200, fov=np.pi / 2): # compute end-point for vertex links_val = np.rint(compute_links(c_point) * scale).astype(int) ee_val = links_val[-1] # get orientation of end effector ee_orientation = c_point.sum() # set visibility triangle x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov) y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov) x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov) y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov) vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2, y2)]) # define observer if is_in_bounds(ee_val): observer = vis.Point(float(ee_val[0]), float(ee_val[1])) observer.snap_to_boundary_of(vis_env, eps) observer.snap_to_vertices_of(vis_env, eps) isovist = vis.Visibility_Polygon(observer, vis_env, eps) # get environment in points point_x, point_y = save_print(isovist) if len(point_x) == 0 or len(point_y) == 0: print('No points for visibility polygon!') return None point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) poly = Polygon([(x, y) for (x, y) in zip(point_x, point_y)]) visilbe_poly = poly.intersection(vis_tri) if type(visilbe_poly) == Polygon and len( list(visilbe_poly.exterior.coords)) > 0: visilbe_poly_pts = np.array(list( visilbe_poly.exterior.coords)).reshape((-1, 1, 2)).astype(int) # draw visilbe polygon of the observer cv2.fillPoly(img, [visilbe_poly_pts], 150) # draw obstacles and inspection points obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x[ 'x'] + x['width'], x['y'] + x['length']), 255, -1), axis=1) inspection_points.apply( lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1) return img
def hole(y_up, y_down, x_left, x_right): ## use arena coordinates #points are listed in cc order #list points cw a = vis.Point(x_left, y_size - (y_down - 1)) b = vis.Point(x_left, y_size - (y_up - 1)) c = vis.Point(x_right, y_size - (y_up - 1)) d = vis.Point(x_right, y_size - (y_down - 1)) # values for graph\ hole_x = [a.x(), b.x(), c.x(), d.x(), a.x()] hole_y = [a.y(), b.y(), c.y(), d.y(), a.y()] # Create the hole polygon hole = vis.Polygon([a, b, c, d]) return hole_x, hole_y, hole
def __add_sites(self, s, nx_vg, pos, env): node = nx_vg.number_of_nodes() pos[node] = tuple(s) isovist = vis.Visibility_Polygon(vis.Point(*s), env, self.epsilon) for i in xrange(isovist.n()): p = isovist[i] p.snap_to_vertices_of(env, self.epsilon) _p = (p.x(), p.y()) for k, v in pos.items(): if _p == v: nx_vg.add_edge(node, k, weight=self.__dist(s, _p)) return node
def construct_environment_demo(power_dataframe, battery_capacity): aggregated_power = power_dataframe.cumsum().Power aggregated_power_lower = [] aggregated_power_higher = [] i = 0 for power in aggregated_power.tolist(): aggregated_power_lower.append([i, power]) aggregated_power_higher.append([i, power + battery_capacity]) i += 1 # Construct the upper hole in a reverse order aggregated_power_higher.reverse() aggregated_power_higher.insert(0, aggregated_power_higher[-1]) aggregated_power_higher.insert( 1, [aggregated_power_higher[0][0], aggregated_power_higher[1][1]]) del aggregated_power_higher[-1] # Construct the lower hole in the reverse order aggregated_power_lower.append( [aggregated_power_lower[-1][0], aggregated_power_lower[0][1]]) # close the hole # aggregated_power_lower.append(aggregated_power_lower[0]) # Construct the wall list wall_list = [] wall_list.append([aggregated_power_lower[0][0], 0]) wall_list.append([aggregated_power_lower[-1][0], 0]) wall_list.append(aggregated_power_higher[2]) wall_list.append(aggregated_power_higher[1]) higher_hole_points = [vis.Point(x, y) for x, y in aggregated_power_higher] lower_hole_points = [vis.Point(x, y) for x, y in aggregated_power_lower] wall_points = [vis.Point(x, y) for x, y in wall_list] higher_hole = vis.Polygon(higher_hole_points) lower_hole = vis.Polygon(lower_hole_points) wall = vis.Polygon(wall_points) return wall, higher_hole, lower_hole
def build_fixed_env(self): # Define the points which will be the outer boundary of the environment # Must be COUNTER-CLOCK-WISE(ccw) # p1 = vis.Point(0-self.epsilon*2.0, 0-self.epsilon*2.0) # p2 = vis.Point(self.workspace[0]+self.epsilon*2.0, 0-self.epsilon*2.0) # p3 = vis.Point(self.workspace[0]+self.epsilon*2.0, self.workspace[1]+self.epsilon*2.0) # p4 = vis.Point(0-self.epsilon*2.0,self.workspace[1]+self.epsilon*2.0) p1 = vis.Point(0 - 5, 0 - 5) p2 = vis.Point(self.workspace[0] + 5, 0 - 5) p3 = vis.Point(self.workspace[0] + 5, self.workspace[1] + 5) p4 = vis.Point(0 - 5, self.workspace[1] + 5) # Load the values of the outer boundary polygon in order to draw it later self.wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] self.wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw) # Create the outer boundary polygon walls = vis.Polygon([p1, p2, p3, p4]) self.holes_array.append(walls) for (obs, boundary) in iter(self.obs.items()): xx, yy = boundary.exterior.coords.xy hole_x = [] hole_y = [] point_array = [] for i in range(len(xx)): point = vis.Point(xx[i], yy[i]) hole_x.append(point.x()) hole_y.append(point.y()) if i != (len(xx) - 1): point_array.append(point) hole = vis.Polygon(point_array) # print('Hole in standard form: ',hole.is_in_standard_form()) self.holes_array.append(hole) self.hole_x_array.append(hole_x) self.hole_y_array.append(hole_y)
def vis_graph(poly): P = poly_list(poly) vis_polygons = [] for p in P: vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p])) env = vis.Environment(vis_polygons) vg = vis.Visibility_Graph(env, 0.000001) edges = [] for i, j in itertools.combinations(range(vg.n()), 2): if vg(i, j): edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]]) plt.gca().add_collection(mc.LineCollection(edges, color='g')) print len(edges)
def __collinear(self, o, p, q): s1 = vis.Line_Segment(vis.Point(*o), vis.Point(*p)) if vis.Point(*q).in_relative_interior_of(s1, self.eps): return True s2 = vis.Line_Segment(vis.Point(*o), vis.Point(*q)) if vis.Point(*p).in_relative_interior_of(s2, self.eps): return True return False
def visibility_polygon(obsv_x, obsv_y, obstacles): # obsv_pt - (px,py), pixel in costmap observer = vis.Point(obsv_x, obsv_y) env = vis.Environment(obstacles) observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) # get visibility polygon isovist = vis.Visibility_Polygon(observer, env, epsilon) if isovist.n() == 0: logger.warn('Visibility polygon is empty for ({:.2f},{:.2f})!'.format( obsv_x, obsv_y)) isovist.eliminate_redundant_vertices(redundant_eps) return isovist # change to isocnt? externally convert to shapely
def vis_graph(poly): P = poly_list(poly) vis_polygons = [] for p in P: vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p])) env = vis.Environment(vis_polygons) vg = vis.Visibility_Graph(env, 0.000001) edges = [] G = nx.Graph() for i, j in itertools.combinations(range(vg.n()), 2): if vg(i, j): edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]]) G.add_edge(i, j, weight=np.linalg.norm( np.array(edges[-1][0]) - np.array(edges[-1][1]))) plt.gca().add_collection(mc.LineCollection(edges, color='g')) s, t = get_st(env) sp = [i for i in nx.shortest_path(G, s, t, 'weight')] sp_edges = [[[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]] for i, j in zip(sp[:-1], sp[1:])] plt.gca().add_collection(mc.LineCollection(sp_edges, color='r', lw=2))
def get_geodesic_target(self, rob_pos, target, node_landmark, avoid_target=[]): holes_array = self.holes_array.copy() hole_x_array = self.hole_x_array.copy() hole_y_array = self.hole_y_array.copy() # start = datetime.datetime.now() # Define the point of the "observer" observer = vis.Point(rob_pos[0], rob_pos[1]) # Now we define some holes for our environment. A hole blocks the # observer vision, it works as an obstacle in his vision sensor. # The newly defined holes in this part are tha landmarks that need # to be avoided. for avoid, dist in avoid_target: avoid_x = node_landmark.landmark[avoid][0][0] avoid_y = node_landmark.landmark[avoid][0][1] dist_buffer = dist + self.epsilon if math.sqrt((rob_pos[0] - avoid_x)**2 + (rob_pos[1] - avoid_y)**2) < dist_buffer + 10: if math.sqrt((target[0] - avoid_x)**2 + (target[1] - avoid_y)**2) < dist_buffer: return rob_pos hole_x = [] hole_y = [] point_array = [] theta = -math.pi for i in range(self.discretization + 1): xx = dist_buffer * math.cos(theta) + avoid_x yy = dist_buffer * math.sin(theta) + avoid_y theta = theta - 2 * math.pi / self.discretization pt = vis.Point(round(xx, 2), round(yy, 2)) hole_x.append(pt.x()) hole_y.append(pt.y()) if i != (self.discretization): point_array.append(pt) hole = vis.Polygon(point_array) # print('Hole in standard form: ',hole.is_in_standard_form()) holes_array.append(hole) hole_x_array.append(hole_x) hole_y_array.append(hole_y) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes env = vis.Environment(holes_array) # Check if the environment is valid # print('Environment is valid : ',env.is_valid(epsilon)) # Define another point, could be used to check if the observer see it, to # check the shortest path from one point to the other, etc. end = vis.Point(target[0], target[1]) # Necesary to generate the visibility polygon observer.snap_to_boundary_of(env, self.epsilon) observer.snap_to_vertices_of(env, self.epsilon) # Obtein the visibility polygon of the 'observer' in the environmente # previously define isovist = vis.Visibility_Polygon(observer, env, self.epsilon) # Uncomment the following line to obtein the visibility polygon # of 'end' in the environmente previously define #polygon_vis = vis.Visibility_Polygon(end, env, epsilon) # Obtein the shortest path from 'observer' to 'end' and 'end_visible' # in the environment previously define shortest_path = env.shortest_path(observer, end, self.epsilon) route = shortest_path.path() path_x = [] path_y = [] # print ('Points of Polygon: ') for i in range(len(route)): x = route[i].x() y = route[i].y() path_x.append(x) path_y.append(y) # Print the length of the path """ print("\nx\nx\nx\n") print("Shortest Path length from observer to end: ", shortest_path.length()) print("\nx\nx\nx\n") # Check if 'observer' can see 'end', i.e., check if 'end' point is in # the visibility polygon of 'observer' print( "Can observer see end? ", end._in(isovist, self.epsilon)) """ # Print the point of the visibility polygon of 'observer' and save them # in two arrays in order to draw the polygon later point_x, point_y = self.save_print(isovist) # Add the first point again because the function to draw, draw a line from # one point to the next one and to close the figure we need the last line # from the last point to the first one point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) if self.plot_images: # Set the title p.title('VisiLibity Test') # Set the labels for the axis p.xlabel('X Position') p.ylabel('Y Position') # Plot the outer boundary with black color p.plot(self.wall_x, self.wall_y, 'black') # Plot the position of the observer with a green dot ('go') p.plot([observer.x()], [observer.y()], 'go') # Plot the position of 'end' with a green dot ('go') p.plot([end.x()], [end.y()], 'go') # Plot the position of 'end_visible' with a green dot ('go') # Plot the visibility polygon of 'observer' p.plot(point_x, point_y) p.plot(path_x, path_y, 'b') for i in range(len(hole_x_array)): p.plot(hole_x_array[i], hole_y_array[i], 'r') # Show the plot p.show() """ print("\nx\nx\nx\n") print("Shortest Path length from observer to end: ", shortest_path.length()) print("\nx\nx\nx\n") print ('Points of Polygon: ') for i in range(len(route)): x = path_x[i] y = path_y[i] print("%f, %f" %(x,y)) print("\nx\nx\nx\n") print("\nx\nx\nx\n") print("\nx\nx\nx\n") print("\nx\nx\nx\n") """ # NBA_time = (datetime.datetime.now() - start).total_seconds() # print('Time for constructing the NBA: {0:.4f} s'.format(NBA_time)) #If start and goal are within threshold distance then return the start point itself if len(path_x) < 2: return path_x[0], path_y[0] return path_x[1], path_y[1]
def save_images_for_path(data_dir, trial_idx, p_zb, scale=50, eps=1e-7): max_wall = 200 fov = np.pi / 2 files_list = {'obstacles': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_obstacles.csv'), 'inspection_points': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_inspection_points.csv'), 'configurations': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_conf'), 'vertex': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_vertex'), 'results':os.path.join(data_dir, f'test_search_p_zb_{trial_idx}_{p_zb}_result')} # check if test files exist broken_files = False for file_path in files_list.values(): if not os.path.isfile(file_path): broken_files = True if broken_files: print('Broken file') sys.exit() # construct image with obstacles img = np.zeros((101,101)) obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int) obstacles_columns = obstacles.columns.to_list() obstacles['p1'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']),axis=1) obstacles['p2'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']),axis=1) obstacles['p3'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']+x['length']),axis=1) obstacles['p4'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']+x['length']),axis=1) obstacles['polygon'] = obstacles.apply(lambda x: vis.Polygon([x['p1'], x['p2'], x['p3'], x['p4']]) ,axis=1) # define bounded environment with obstacles p1 = vis.Point(0,0) p2 = vis.Point(101,0) p3 = vis.Point(101,101) p4 = vis.Point(0,101) walls_poly = vis.Polygon([p1, p2, p3, p4]) vis_env = vis.Environment([walls_poly] + obstacles['polygon'].to_list()) # load inspection points csv inspection_points = (pd.read_csv(files_list['inspection_points']) * scale).round(0).astype(int) # load configuration space csv cspace_df = pd.read_csv(files_list['configurations'], delimiter=' ', header=None).drop(columns=[0,6]) # drop already seen points from inspection points with open(files_list['results'],'r') as f: line = f.readlines()[-2] vertices_idxs = [int(x) for x in line.split(' ')[1:-1]] img_copy = img.copy() for j in range(0,len(vertices_idxs)-1): c_point = cspace_df.iloc[j].to_numpy() # compute end-point for vertex links_val = np.rint(compute_links(c_point) * scale).astype(int) ee_val = links_val[-1] # get orientation of end effector ee_orientation = c_point.sum() # set visibility triangle x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov) y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov) x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov) y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov) vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2,y2)]) # define observer if is_in_bounds(ee_val): observer = vis.Point(float(ee_val[0]), float(ee_val[1])) observer.snap_to_boundary_of(vis_env, eps) observer.snap_to_vertices_of(vis_env, eps) isovist = vis.Visibility_Polygon(observer, vis_env, eps) # get environment in points point_x , point_y = save_print(isovist) if len(point_x ) == 0 or len(point_y) == 0: continue point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) poly = Polygon([(x,y) for (x,y) in zip(point_x, point_y)]) visilbe_poly = poly.intersection(vis_tri) if type(visilbe_poly) == Polygon and len(list(visilbe_poly.exterior.coords)) > 0: visilbe_poly_pts = np.array(list(visilbe_poly.exterior.coords)).reshape((-1,1,2)).astype(int) # draw visilbe polygon of the observer cv2.fillPoly(img_copy, [visilbe_poly_pts], 150) # draw obstacles and inspection points obstacles.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1) inspection_points.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1) # add sample (x,y) cv2.imwrite(f'sample_path_{trial_idx}_{p_zb}_{j}.png', img_copy)
def testVisilibity(): # Define an epsilon value (should be != 0.0) epsilon = 1e-7 # Define the points which will be the outer boundary of the environment # Must be COUNTER-CLOCK-WISE(ccw) p1 = vis.Point(0, 0) p2 = vis.Point(700, 0) p3 = vis.Point(700, 900) p4 = vis.Point(0, 900) # Load the values of the outer boundary polygon in order to draw it later wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw) # Create the outer boundary polygon walls = vis.Polygon([p1, p2, p3, p4]) # Define the point of the "observer" observer = vis.Point(235, 400) # Uncomment the following line in order to create a cone polygon #walls = create_cone((observer.x(), observer.y()), 500, 270, 30, quality= 3) # Walls should be in standard form print('Walls in standard form : ', walls.is_in_standard_form()) # Now we define some holes for our environment. The holes must be inside # our outer boundary polygon. A hole blocks the observer vision, it works as # an obstacle in his vision sensor. # We define some point for a hole. You can add more points in order to get # the shape you want. # The smalles point should be first p2 = vis.Point(100, 300) p3 = vis.Point(100, 500) p4 = vis.Point(150, 500) p1 = vis.Point(150, 300) # Load the values of the hole polygon in order to draw it later hole_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Note: The point of a hole must be in CLOCK-WISE(cw) order. # Create the hole polygon hole = vis.Polygon([p2, p3, p4, p1]) # Check if the hole is in standard form print('Hole in standard form: ', hole.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(300, 300) p2 = vis.Point(300, 500) p3 = vis.Point(400, 550) p4 = vis.Point(400, 300) # Load the values of the hole polygon in order to draw it later hole1_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole1_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole1 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole1.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p2 = vis.Point(90, 700) p3 = vis.Point(250, 750) p4 = vis.Point(220, 600) p1 = vis.Point(150, 600) # Load the values of the hole polygon in order to draw it later hole2_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole2_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole2 = vis.Polygon([p2, p3, p4, p1]) # Check if the hole is in standard form print('Hole in standard form: ', hole2.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(330, 700) p2 = vis.Point(330, 800) p3 = vis.Point(530, 850) p4 = vis.Point(530, 790) # Load the values of the hole polygon in order to draw it later hole3_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole3_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole3 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole3.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(230, 50) p2 = vis.Point(250, 90) p3 = vis.Point(390, 90) p4 = vis.Point(390, 50) # Load the values of the hole polygon in order to draw it later hole4_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole4_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole4 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole4.is_in_standard_form()) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes env = vis.Environment([walls, hole, hole2, hole1, hole3, hole4]) # Check if the environment is valid print('Environment is valid : ', env.is_valid(epsilon)) # Define another point, could be used to check if the observer see it, to # check the shortest path from one point to the other, etc. end = vis.Point(330, 525) # Define another point that the 'observer' will see end_visible = vis.Point(415, 45) # Necesary to generate the visibility polygon observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) # Obtein the visibility polygon of the 'observer' in the environmente # previously define isovist = vis.Visibility_Polygon(observer, env, epsilon) # Uncomment the following line to obtein the visibility polygon # of 'end' in the environmente previously define #polygon_vis = vis.Visibility_Polygon(end, env, epsilon) # Obtein the shortest path from 'observer' to 'end' and 'end_visible' # in the environment previously define shortest_path = env.shortest_path(observer, end, epsilon) shortest_path1 = env.shortest_path(observer, end_visible, epsilon) # Print the length of the path print("Shortest Path length from observer to end: ", shortest_path.length()) print("Shortest Path length from observer to end_visible: ", shortest_path1.length()) logger.debug('shortest_path:\n%s', dir(shortest_path)) logger.debug('shortest_path.set_vertices:\n%s', shortest_path.size()) # logger.debug('shortest_path.path:\n%s',shortest_path) # Check if 'observer' can see 'end', i.e., check if 'end' point is in # the visibility polygon of 'observer' print("Can observer see end? ", end._in(isovist, epsilon)) print("Can observer see end_visible? ", end_visible._in(isovist, epsilon)) # Print the point of the visibility polygon of 'observer' and save them # in two arrays in order to draw the polygon later logger.debug('isovist: %s', isovist.n()) return point_x, point_y = save_print(isovist) # Add the first point again because the function to draw, draw a line from # one point to the next one and to close the figure we need the last line # from the last point to the first one point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) # Set the title p.title('VisiLibity Test') # Set the labels for the axis p.xlabel('X Position') p.ylabel('Y Position') # Plot the outer boundary with black color p.plot(wall_x, wall_y, 'black') # Plot the position of the observer with a green dot ('go') p.plot([observer.x()], [observer.y()], 'go') # Plot the position of 'end' with a green dot ('go') p.plot([end.x()], [end.y()], 'go') # Plot the position of 'end_visible' with a green dot ('go') p.plot([end_visible.x()], [end_visible.y()], 'go') # Plot the visibility polygon of 'observer' p.plot(point_x, point_y) # Plot the hole polygon with red color p.plot(hole_x, hole_y, 'r') # Plot the hole polygon with red color p.plot(hole1_x, hole1_y, 'r') # Plot the hole polygon with red color p.plot(hole2_x, hole2_y, 'r') # Plot the hole polygon with red color p.plot(hole3_x, hole3_y, 'r') # Plot the hole polygon with red color p.plot(hole4_x, hole4_y, 'r') # Example of a cone-shape polygon cone_point = vis.Point(440, 420) # cone=create_cone([cone_point.x(),cone_point.y()],150,0,45,3) # cone_x,cone_y=save_print(cone) # cone_x.append(cone_x[0]) # cone_y.append(cone_y[0]) # p.plot([cone_point.x()],[cone_point.y()],'go') # p.plot(cone_x,cone_y) # Show the plot p.show()
def calculate_solution(): # The Ilya Constant (TM) epsilon = 0.000000001 # Environment plot (500 x 500 should be enough, right?! *gulps with fear*) p1 = vis.Point(50, 50) p2 = vis.Point(-50, 50) p3 = vis.Point(-50, -50) p4 = vis.Point(50, -50) # Set up our walls (yay) wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] walls = vis.Polygon([p1, p2, p3, p4]) #robot1 = vis.Point(-1,-1) #robot2 = vis.Point(4,4) #obstacle = vis.Polygon([vis.Point(1,6),vis.Point(1,1),vis.Point(5,1),vis.Point(5,5),vis.Point(3,5),vis.Point(3,3),vis.Point(4,3),vis.Point(4,2), # vis.Point(2,2),vis.Point(2,6),vis.Point(6,6),vis.Point(6,0),vis.Point(0,0),vis.Point(0,6),vis.Point(1,6)][::-1]) #obstacle_x = [1, 1, 5, 5, 3, 3, 4, 4, 2, 2, 6, 6, 0, 0, 1][::-1] #obstacle_y = [6, 1, 1, 5, 5, 3, 3, 2, 2, 6, 6, 0, 0, 6, 6][::-1] robot1 = vis.Point(0, 1) robot2 = vis.Point(6, 2) obstacle = vis.Polygon([vis.Point(8,1),vis.Point(4,1),vis.Point(4,4),vis.Point(5,2), vis.Point(8,1)]) obstacle_x = [8, 4, 4, 5, 8] obstacle_y = [1, 1, 4, 2, 1] obstacle2 = vis.Polygon([vis.Point(1,2), vis.Point(1,4), vis.Point(3,4), vis.Point(3,2), vis.Point(1,2)][::-1]) obstacle2_x = [1, 1, 3, 3, 1][::-1] obstacle2_y = [2, 4, 4, 2, 2][::-1] env = vis.Environment([walls, obstacle, obstacle2]) robot1.snap_to_boundary_of(env, epsilon) robot1.snap_to_vertices_of(env, epsilon) isovist = vis.Visibility_Polygon(robot1, env, epsilon) shortest_path = env.shortest_path(robot1, robot2, epsilon) point_x, point_y = save_print(isovist) point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) p.title('Shortest (????) Path') p.xlabel('X Position') p.ylabel('Y Position') p.plot(wall_x, wall_y, 'black') p.plot([robot1.x()], [robot1.y()], 'go') p.plot([robot2.x()], [robot2.y()], 'go') p.plot(obstacle_x, obstacle_y, 'r') p.plot(obstacle2_x, obstacle2_y, 'r') print "Shortest Path length from observer to end: ", shortest_path.length() print "Number of options: ", shortest_path.size() polyline = [] for x in range(0, shortest_path.size()): point = shortest_path.getVertex(x) print "(", point.x(), ", ", point.y(), ")" polyline.append(point) p.show()
def make_arena_polygon(): #build center diamond polygon #list points cw p1 = vis.Point(23, ym - 16) p2 = vis.Point(22, ym - 15) p3 = vis.Point(22, ym - 13) p4 = vis.Point(23, ym - 12) p5 = vis.Point(25, ym - 12) p6 = vis.Point(26, ym - 13) p7 = vis.Point(26, ym - 15) p8 = vis.Point(25, ym - 16) # Load the values of the hole polygon in order to draw it later diam_x = [ p2.x(), p3.x(), p4.x(), p5.x(), p6.x(), p7.x(), p8.x(), p1.x(), p2.x() ] diam_y = [ p2.y(), p3.y(), p4.y(), p5.y(), p6.y(), p7.y(), p8.y(), p1.y(), p2.y() ] # Create the hole polygon diam = vis.Polygon([p2, p3, p4, p5, p6, p7, p8, p1]) #build the arena wall_x, wall_y, walls = outer( 2, 28, 1, 47) #y_up, y_down, x_left, x_right, use arena format #format y_up, y_down, x_left, x_right, use arena format huecos_input = [ [ym - 6, ym - 7, 1, 7], # [ym - 12, ym - 14, 10, 14], # [ym - 20, ym - 26, 9, 11], # [ym - 5, ym - 7, 21, 27], # [ym - 19, ym - 21, 21, 27], # [ym - 12, ym - 14, 34, 38], [ym - 19, ym - 20, 41, 47], # [ym, ym - 6, 37, 39], # ] huecos = [] for item in huecos_input: huecos.append(hole(item[0], item[1], item[2], item[3])) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes arena_poly = vis.Environment([ walls, huecos[0][2], huecos[1][2], huecos[2][2], huecos[3][2], huecos[4][2], huecos[5][2], huecos[6][2], huecos[7][2], diam ]) return arena_poly, [[wall_x, wall_y], [huecos[0][0], huecos[0][1]], [huecos[1][0], huecos[1][1]], [huecos[2][0], huecos[2][1]], [huecos[3][0], huecos[3][1]], [huecos[4][0], huecos[4][1]], [huecos[5][0], huecos[5][1]], [huecos[6][0], huecos[6][1]], [huecos[7][0], huecos[7][1]], [diam_x, diam_y]]
def __get_isovist(self, p): v = vis.Point(*p) v.snap_to_boundary_of(self.env, self.eps) v.snap_to_vertices_of(self.env, self.eps) return vis.Visibility_Polygon(v, self.env, self.eps)
def point_to_vis(point): x,y = point return vis.Point(x,y)
def points_to_vis_points(tupled_points): return [vis.Point(x,y) for x,y in tupled_points]
def find_cut_space(P, v): """ Generate the cut space at v using Visilibity library. """ epsilon = 0.0000001 # Using shapely library, compute the cone of bisection # shp_polygon = shapely.geometry.polygon.orient(Polygon(*P)) # shp_cone = shapely.geometry.polygon.orient(Polygon(find_cone_of_bisection(P, v))) shp_polygon = Polygon(*P) #print("Polygon: %s"%shp_polygon) cone_of_bisection = find_cone_of_bisection(P, v) shp_cone = Polygon(find_cone_of_bisection(P, v)) #print("Cone of bisection: %s"%cone_of_bisection) shp_intersection = shp_cone.intersection(shp_polygon) #print("Intersection: %s"%shp_intersection) # import pylab as p # # # Plot the polygon itself # x, y = shp_polygon.exterior.xy # p.plot(x, y) # x, y = shp_cone.exterior.xy # p.plot(x, y) # x, y = shp_intersection.exterior.xy # p.plot(x, y) # p.show() # plot the intersection of the cone with the polygon #intersection_x, intersection_y = shp_intersection.exterior.xy #p.plot(intersection_x, intersection_y) #for interior in shp_intersection.interiors: # interior_x, interior_y = interior.xy # p.plot(interior_x, interior_y) # Plot the reflex vertex #p.plot([observer.x()], [observer.y()], 'go') #p.plot(point_x, point_y) if shp_intersection.geom_type == "MultiPolygon": shp_intersection = shp_intersection[0] #print shp_intersection elif shp_intersection.geom_type == "GeometryCollection": for shape in shp_intersection: if shape.geom_type == "Polygon": shp_intersection = shape break else: #shp_intersection = shapely.geometry.polygon.orient(shp_intersection) shp_intersection = (shp_intersection) #shp_intersection = shapely.geometry.polygon.orient(shp_cone.intersection(shp_polygon)) #Using the visilibity library, define the reflex vertex observer = vis.Point(*v) # Define the walls of intersection in Visilibity domain # To put into standard form, do this exterior_coords = shp_intersection.exterior.coords[:-1] x_min_idx, x_min = min(enumerate(exterior_coords), key=itemgetter(1)) exterior_coords = exterior_coords[7:]+exterior_coords[:7] vis_intersection_wall_points = [] for point in exterior_coords: vis_intersection_wall_points.append(vis.Point(*point)) #print 'Walls in standard form : ',vis.Polygon(vis_intersection_wall_points).is_in_standard_form() #for i in range(len(vis_intersection_wall_points)): #print vis_intersection_wall_points[i].x(), vis_intersection_wall_points[i].y() #print point.x(), point.y() # Define the holes of intersection in Visilibity domain vis_intersection_holes = [] for interior in shp_intersection.interiors: vis_intersection_hole_points = [] for point in list(interior.coords): vis_intersection_hole_points.append(vis.Point(*point)) vis_intersection_holes.append(vis_intersection_hole_points) #print 'Hole in standard form : ',vis.Polygon(vis_intersection_hole_points).is_in_standard_form() # Construct a convinient list env = [] env.append(vis.Polygon(vis_intersection_wall_points)) for hole in vis_intersection_holes: env.append(vis.Polygon(hole)) # Construct the whole envrionemt in Visilibity domain env = vis.Environment(env) # Construct the visible polygon observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) vis_free_space = vis.Visibility_Polygon(observer, env, epsilon) #print vis_free_space.n() def save_print(polygon): end_pos_x = [] end_pos_y = [] for i in range(polygon.n()): x = polygon[i].x() y = polygon[i].y() end_pos_x.append(x) end_pos_y.append(y) return end_pos_x, end_pos_y point_x , point_y = save_print(vis_free_space) point_x.append(vis_free_space[0].x()) point_y.append(vis_free_space[0].y()) ### # At this point, we have visibility polygon. # Now we need to find edges of visbility polygon which are on the boundary shp_visib = shapely.geometry.polygon.orient(Polygon(zip(point_x, point_y)),-1) shp_ls_visib = LineString(shp_visib.exterior.coords[:]) shp_pl_visib = shp_ls_visib.buffer(0.001) shp_ls_exterior = LineString(shp_polygon.exterior) shp_ls_interior = [] for interior in shp_polygon.interiors: shp_ls_interior.append(LineString(interior)) # Start adding cut space on the exterior cut_space = [] common_items = [] #common_items = shp_ls_exterior.intersection(shp_ls_visib) common_items = shp_ls_exterior.intersection(shp_pl_visib) # Filter out very small segments if common_items.geom_type == "MultiLineString": for item in common_items: linestring = item.coords[:] # Examine each edge of the linestring for i in range(len(linestring)-1): edge = linestring[i:i+2] edge_ls = LineString(edge) if edge_ls.length > 0.02: cut_space.append(edge) elif common_items.geom_type == "LineString": # Examine each edge of the linestring linestring = common_items.coords[:] for i in range(len(linestring)-1): edge = linestring[i:i+2] edge_ls = LineString(edge) if edge_ls.length > 0.02: cut_space.append(edge) #print cut_space # Start adding cut space on the holes for interior in shp_polygon.interiors: common_items = interior.intersection(shp_ls_visib) if common_items.geom_type == "GeometryCollection": # print common_items for item in common_items: if item.geom_type == "LineString": cut_space.append(item.coords[:]) elif common_items.geom_type == "LineString": cut_space.append(common_items.coords[:]) #Point, LineString, GeometryCollection #print cut_space # PLOTTING # import pylab as p # # # Plot the polygon itself # x, y = shp_polygon.exterior.xy # p.plot(x, y) # # # plot the intersection of the cone with the polygon # intersection_x, intersection_y = shp_intersection.exterior.xy # p.plot(intersection_x, intersection_y) # # #for interior in shp_intersection.interiors: # # interior_x, interior_y = interior.xy # # p.plot(interior_x, interior_y) # # # Plot the reflex vertex # p.plot([observer.x()], [observer.y()], 'go') # # p.plot(point_x, point_y) # # p.show() #print cut_space return cut_space
def a_point(x, y): #input arena coordinates return vis.Point(x, y_size - (y - 1))