def generatepath(source_loc: Tuple[float, float], target_loc: Tuple[float, float], other_rects: List[List[Dict[str, float]]], agent_width: float = PERFORMER_WIDTH) \ -> Optional[List[Tuple[float, float]]]: """Boundary has to be CCW, Holes CW""" # dilate all the polygons based on the agent width dilation = agent_width / 2.0 polygons = _dilate_polygons(other_rects, dilation, Point(*source_loc), Point(*target_loc)) if polygons is None: return None # they may intersect now, so unify any that do poly_coords = _unify_and_clean_polygons(polygons) environment = Environment() boundary_coordinates = [(MAX_X - dilation, MAX_Z - dilation), (MIN_X + dilation, MAX_Z - dilation), (MIN_X + dilation, MIN_Z + dilation), (MAX_X - dilation, MIN_Z + dilation)] try: environment.store(boundary_coordinates, poly_coords, validate=True) environment.prepare() path, length = environment.find_shortest_path(source_loc, target_loc) # Work around a bug in the DirectedHeuristicGraph class # defined by the extremitypathfinder module. It causes the # graph to keep growing across successive Environments. See # https://github.com/MrMinimal64/extremitypathfinder/issues/10 environment.graph.all_nodes.clear() except TypeError as e: # We sometimes get "TypeError: unsupported operand type(s) for # +: 'NoneType' and 'float'" and "'<' not supported between # instances of 'generator' and 'generator'", but I don't know # why. logging.info(f'path finding failed (maybe impossible?): {e}') return None except Exception: # We shouldn't get other exceptions from the path finder, but # just in case... logging.warning('unexpected error in path finding') traceback.print_exc() logObject = { 'boundary': boundary_coordinates, 'holes': poly_coords, 'start': source_loc, 'target': target_loc } logging.warning(logObject) return None return path
class GlobalLocalPlanner(): def __init__(self): self.environment = Environment() # counter clockwise vertex numbering! boundary_coordinates = [(0 + ROBOT_SIZE, 0 + ROBOT_SIZE), (3.25 - ROBOT_SIZE, 0 + ROBOT_SIZE), (3.25 - ROBOT_SIZE, 1.0 + ROBOT_SIZE), (3.5 + ROBOT_SIZE, 1.0 + ROBOT_SIZE), (3.5 + ROBOT_SIZE, 0 + ROBOT_SIZE), (8.0 - ROBOT_SIZE, 0 + ROBOT_SIZE), (8.0 - ROBOT_SIZE, 5.0 - ROBOT_SIZE), (8.0 - 3.25 + ROBOT_SIZE, 5.0 - ROBOT_SIZE), (8.0 - 3.25 + ROBOT_SIZE, 4.0 - ROBOT_SIZE), (8.0 - 3.5 - ROBOT_SIZE, 4.0 - ROBOT_SIZE), (8.0 - 3.5 - ROBOT_SIZE, 5.0 - ROBOT_SIZE), (0 + ROBOT_SIZE, 5.0 - ROBOT_SIZE)] # clockwise numbering! list_of_holes = [] for (x, y), (w, h) in zip(BORDER_POS[:], BORDER_BOX[:]): #x, y, w, h = x*10, y*10, w*10, h*10 list_of_holes.append([ ((x - w - ROBOT_SIZE), (y - h - ROBOT_SIZE)), ((x - w - ROBOT_SIZE), (y + h + ROBOT_SIZE)), ((x + w + ROBOT_SIZE), (y + h + ROBOT_SIZE)), ((x + w + ROBOT_SIZE), (y - h - ROBOT_SIZE)), ]) self.environment.store(boundary_coordinates, list_of_holes, validate=False) self.environment.prepare() self.done = True self.dynamic = DynamicWindow() def plot(self): draw_prepared_map(self.environment) def findPath(self, start, goal): path, length = self.environment.find_shortest_path(start, goal) return path def setGoal(self, start, goal, angle=0): #print(start, goal) self.path = self.findPath(start, goal) self.index = 1 self.angle_path = [] if len(self.path) > 1: self.next_target = self.path[1] #print("new target: {}".format(self.next_target)) for i, j in zip(self.path[:-1], self.path[1:]): self.angle_path.append(math.atan2(j[1] - i[1], j[0] - i[0])) self.angle_path.append(angle) self.next_angle = self.angle_path[0] self.done = False else: self.done = True def moveTo(self, pos, vel, angle, angular, action): if self.done: return action if self.distance(pos, self.next_target) < 0.4: self.index += 1 if self.index < len(self.path): self.next_target = self.path[self.index] self.next_angle = self.angle_path[self.index - 1] print("new target: {}".format(self.next_target)) else: self.done = True action[0], action[1], action[2] = 0, 0, 0 return action #tic = time.time() tmp_angle = math.atan2(self.next_target[1] - pos[1], self.next_target[0] - pos[0]) tmp_length = (self.next_target[0]**2 + self.next_target[1]**2) tmp_target = np.array([math.cos(tmp_angle), math.sin(tmp_angle)]) * min(5, tmp_length) action = self.dynamic.moveTo(action, pos, vel, angle, angular, tmp_target, self.next_angle) #print(time.time() - tic) ''' u = np.array([ self.next_target[0]-pos[0], self.next_target[1]-pos[1]]) target_angle = math.atan2(u[1], u[0]) u = u.reshape([2, 1]) mat_angle = np.array([ [math.cos(angle), math.sin(angle)], [math.sin(angle), -math.cos(angle)]]) v = np.matmul(np.linalg.inv(mat_angle), u) / 10 #print(u, v, angle) v /= v.max() action[0] = v[0][0] action[1] = target_angle - angle action[2] = v[1][0] ''' return action def distance(self, p1, p2): return ((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)**0.5
def find_path(thymioCoord, goal, obsListOrig, plotFlag=True): """Given an inital position, a goal and a set of obstacle, will create a Visibility Graph of the map, then perform an A* search on the given graph, and return the shortest path Librairy used for visiblity Graph generation and Graph search:https://github.com/MrMinimal64/extremitypathfinder """ obsList = obsListOrig.copy() MARGE_BORD = 5 EXTREME_DIST = 400 # if obstacle are near the map border, make them bigger so we are surefound path dosn't go trough the small gap for iObs in range(0, len(obsList)): for iVert in range(0, len(obsList[iObs].vertexExpanded)): listV = list(obsList[iObs].vertexExpanded[iVert]) if listV[0] > MAP_MAX_X_AXIS - MARGE_BORD: listV[0] = EXTREME_DIST obsList[iObs].vertexExpanded[iVert] = listV if listV[0] < 0 + MARGE_BORD: listV[0] = -EXTREME_DIST obsList[iObs].vertexExpanded[iVert] = listV if listV[1] > MAP_MAX_Y_AXIS - MARGE_BORD: listV[1] = EXTREME_DIST obsList[iObs].vertexExpanded[iVert] = listV if listV[1] < 0 + MARGE_BORD: listV[1] = -EXTREME_DIST obsList[iObs].vertexExpanded[iVert] = listV if plotFlag: for obs in obsList: unzippedList = list(zip(*obs.vertex)) unzippedList = [list(elem) for elem in unzippedList] unzippedList[0].append(unzippedList[0][0]) unzippedList[1].append(unzippedList[1][0]) plt.plot(unzippedList[0], unzippedList[1]) if obs.expandEnabled: unzippedList = list(zip(*obs.vertexExpanded)) unzippedList = [list(elem) for elem in unzippedList] unzippedList[0].append(unzippedList[0][0]) unzippedList[1].append(unzippedList[1][0]) plt.plot(unzippedList[0], unzippedList[1], '--') plt.plot(thymioCoord[0], thymioCoord[1], 'o', color='green') plt.plot(goal[0], goal[1], 'o', color='red') map = Environment() # boundary of the map, anti clockwise vertex numbering! boundary_coordinates = [(0.0, 0.0), (MAP_MAX_X_AXIS, 0.0), (MAP_MAX_X_AXIS, MAP_MAX_Y_AXIS), (0.0, MAP_MAX_Y_AXIS)] list_of_obstacle = [] for obs in obsList: if obs.expandEnabled: list_of_obstacle.append(obs.vertexExpanded) map.store(boundary_coordinates, list_of_obstacle, validate=False) map.prepare() path, length = map.find_shortest_path(thymioCoord, goal) if plotFlag: unzippedPath = list(zip(*path)) plt.plot(unzippedPath[0], unzippedPath[1], '--', color='black') plt.xlim(0, MAP_MAX_X_AXIS) plt.ylim(0, MAP_MAX_Y_AXIS) plt.gca().set_aspect('equal', adjustable='box') plt.show() return path
# counter clockwise vertex numbering! boundary_coordinates = [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)] # clockwise numbering! list_of_holes = [[ (3.0, 7.0), (5.0, 9.0), (4.5, 7.0), (5.0, 4.0), ]] # environment.store(boundary_coordinates, list_of_holes, validate=True, export_plots=True) environment.store(boundary_coordinates, list_of_holes, validate=False) environment.prepare() # environment.export_pickle() # from extremitypathfinder.extremitypathfinder import load_pickle # environment = load_pickle() start_coordinates = (4.5, 1.0) goal_coordinates = (4.0, 8.5) path, length = environment.find_shortest_path(start_coordinates, goal_coordinates) print(path, length) # grid world size_x, size_y = 19, 10 obstacle_iter = [
def test_fct(self): environment = PolygonEnvironment() size_x, size_y = 19, 10 obstacle_iter = [ # (x,y), # obstacles changing boundary (0, 1), (1, 1), (2, 1), (3, 1), (17, 9), (17, 8), (17, 7), (17, 5), (17, 4), (17, 3), (17, 2), (17, 1), (17, 0), # hole 1 (5, 5), (5, 6), (6, 6), (6, 7), (7, 7), # hole 2 (7, 5), ] environment.store_grid_world(size_x, size_y, obstacle_iter, simplify=False, validate=False) assert len( environment.all_extremities) == 17 # should detect all extremities environment.prepare() assert len(environment.graph.all_nodes ) == 16 # identical nodes should get joined in the graph! # test if points outside the map are being rejected for start_coordinates, goal_coordinates in [ # outside of map region ((-1, 5.0), (17, 0.5)), ((17, 0.5), (-1, 5.0)), ((20, 5.0), (17, 0.5)), ((17, 0.5), (20, 5.0)), ((1, -5.0), (17, 0.5)), ((17, 0.5), (1, -5.0)), ((1, 11.0), (17, 0.5)), ((17, 0.5), (1, 11.0)), # outside boundary polygon ((17.5, 5.0), (17, 0.5)), ((17, 0.5), (17.5, 5.0)), ((1, 1.5), (17, 0.5)), ((17, 0.5), (1, 1.5)), # inside hole ((6.5, 6.5), (17, 0.5)), ((17, 0.5), (6.5, 6.5)), ]: with pytest.raises(ValueError): environment.find_shortest_path(start_coordinates, goal_coordinates) for ((start_coordinates, goal_coordinates), expected_output) in [ # ((start,goal),(path,distance)) # identical nodes (((15, 5), (15, 5)), ([(15, 5), (15, 5)], 0.0)), # directly reachable (((15, 5), (15, 6)), ([(15, 5), (15, 6)], 1.0)), (((15, 6), (15, 5)), ([(15, 6), (15, 5)], 1.0)), (((15, 5), (16, 6)), ([(15, 5), (16, 6)], sqrt(2))), (((16, 6), (15, 5)), ([(16, 6), (15, 5)], sqrt(2))), # points on the polygon edges (vertices) should be accepted! # on edge (((15, 0), (15, 6)), ([(15, 0), (15, 6)], 6.0)), (((15, 6), (15, 0)), ([(15, 6), (15, 0)], 6.0)), (((17, 5), (16, 5)), ([(17, 5), (16, 5)], 1.0)), (((16, 5), (17, 5)), ([(16, 5), (17, 5)], 1.0)), # on edge of hole (((7, 8), (7, 9)), ([(7, 8), (7, 9)], 1.0)), (((7, 9), (7, 8)), ([(7, 9), (7, 8)], 1.0)), # on vertex (((4, 2), (4, 3)), ([(4, 2), (4, 3)], 1.0)), (((4, 3), (4, 2)), ([(4, 3), (4, 2)], 1.0)), # on vertex of hole (((6, 8), (6, 9)), ([(6, 8), (6, 9)], 1.0)), (((6, 9), (6, 8)), ([(6, 9), (6, 8)], 1.0)), # on two vertices # coinciding with edge (direct neighbour) (((4, 2), (4, 1)), ([(4, 2), (4, 1)], 1.0)), (((5, 5), (5, 7)), ([(5, 5), (5, 7)], 2.0)), # should have direct connection to all visible extremities! connected in graph (((6, 8), (5, 7)), ([(6, 8), (5, 7)], sqrt(2))), (((4, 1), (5, 7)), ([(4, 1), (5, 7)], sqrt(1**2 + 6**2))), # should have direct connection to all visible extremities! even if not connected in graph! (((4, 2), (5, 7)), ([(4, 2), (5, 7)], sqrt(1**2 + 5**2))), # mix of edges and vertices, directly visible (((2, 2), (5, 7)), ([(2, 2), (5, 7)], sqrt(3**2 + 5**2))), # also regular points should have direct connection to all visible extremities! (((10, 3), (17, 6)), ([(10, 3), (17, 6)], sqrt(7**2 + 3**2))), (((10, 3), (8, 8)), ([(10, 3), (8, 8)], sqrt(2**2 + 5**2))), # even if the query point lies in front of an extremity! (test if new query vertices are being created!) (((10, 3), (8, 5)), ([(10, 3), (8, 5)], sqrt(2**2 + 2**2))), # using a* graph search: # directly reachable through a single vertex (does not change distance!) (((5, 1), (3, 3)), ([(5, 1), (4, 2), (3, 3)], sqrt(2**2 + 2**2))), # If two Polygons have vertices with identical coordinates (this is allowed), # paths through these vertices are theoretically possible! (((6.5, 5.5), (7.5, 6.5)), ([(6.5, 5.5), (7, 6), (7.5, 6.5)], sqrt(1**2 + 1**2))), # distance should stay the same even if multiple extremities lie on direct path # test if path is skipping passed extremities (((8, 4), (8, 8)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8)], 4)), (((8, 4), (8, 9)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9)], 5)), # regular examples (((0.5, 6), (18.5, 0.5)), ([(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (17, 6), (18, 6), (18.5, 0.5)], 23.18783787537749)), (((0.5, 6), (9, 6)), ([(0.5, 6.0), (5, 5), (6, 5), (7, 6), (8, 6), (9, 6)], 9.023985791019538)), (((0.5, 6), (18.5, 9)), ([(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (18, 7), (18.5, 9.0)], 19.869364068640845)), # 19.86936406864084 (((6.9, 4), (7, 9)), ([(6.9, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.830925564196269)), (((6.5, 4), (7, 9)), ([(6.5, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.889979937555021)), ]: # test if path and distance are correct # print(input, expected_output, fct(input)) actual_output = environment.find_shortest_path( start_coordinates, goal_coordinates) if actual_output != expected_output: print('input: {} expected: {} got: {}'.format( (start_coordinates, goal_coordinates), expected_output, actual_output)) assert actual_output == expected_output # when the deep copy mechanism works correctly # even after many queries the internal graph should have the same structure as before # otherwise the temporarily added vertices during a query stay stored assert len(environment.graph.all_nodes) == 16
class PathPreProcessor: def __init__(self, config, plotting = False): self.config = config self.inflator = pyclipper.PyclipperOffset() # Define environment based on wheter or not to plot if plotting: self.env = PlottingEnvironment(plotting_dir='plotted_paths') else: self.env = PolygonEnvironment() def prepare(self, graph_map): self.dyn_obs_list = graph_map.dyn_obs_list.copy() # Obstacles self.original_obstacle_list = graph_map.obstacle_list.copy() # Pre-proccess obstacles self.processed_obstacle_list = self.preprocess_obstacles(graph_map.obstacle_list) # Boundary self.original_boundary_coordinates = graph_map.boundary_coordinates.copy() # Pre-process boundaries self.processed_boundary_coordinates = self.preprocess_obstacle( pyclipper.scale_to_clipper(graph_map.boundary_coordinates), pyclipper.scale_to_clipper(-self.config.vehicle_width)) # Give obstacles and boundaries to environment self.env.store(self.processed_boundary_coordinates, self.processed_obstacle_list) # Prepare the visibility graph self.env.prepare() def get_initial_guess(self, start_pos, end_pos): """Method that generates the initially guessed path based on obstacles and boundaries specified during creation Args: start_pos (tuple): tuple containing x and y position for starting position end_pos (tuple): tuple containing x and y position for end position Returns: path ([tuples]) : a list of coordinates that together becomes the inital path. These points lies on extremities of the padded obstacles vertices ([tuples]) : a list of the vertices on the original (unpadded) obstacles corresponding to the vertices in path """ path, distance = self.env.find_shortest_path(start_pos, end_pos) vertices = self.find_original_vertices(path) self.path = path self.vert = vertices self.vert_copy = vertices.copy() return path, vertices def preprocess_obstacle(self, obstacle, inflation, boundary = False): self.inflator.Clear() self.inflator.AddPath(obstacle, pyclipper.JT_MITER, pyclipper.ET_CLOSEDPOLYGON) inflated_obstacle = pyclipper.scale_from_clipper(self.inflator.Execute(inflation))[0] return inflated_obstacle def preprocess_obstacles(self, obstacle_list): inflation = pyclipper.scale_to_clipper(self.config.vehicle_width) inflated_obstacles = [] for obs in obstacle_list: obstacle = pyclipper.scale_to_clipper(obs) inflated_obstacle = self.preprocess_obstacle(obstacle, inflation, boundary=False) inflated_obstacle.reverse() # obstacles are ordered clockwise inflated_obstacles.append(inflated_obstacle) return inflated_obstacles @staticmethod def dist_between_points(p1, p2): return np.linalg.norm((np.asarray(p1) - np.asarray(p2)), ord = 2) def get_closest_vert(self, vert, list_to_compare): #best_vert = () #best_dist = 10e3 #best_idx = 0 distances = map(lambda x: self.dist_between_points(vert, x), list_to_compare) best_idx = np.argmin(list(distances)) '''for idx, ref_vert in enumerate(list_to_compare): dist = self.dist_between_points(vert, ref_vert) if dist < best_dist: best_dist = dist best_idx = idx best_vert = ref_vert''' return list_to_compare[best_idx], best_idx def find_original_vertices(self, path): vertices = [] if not (len(path) > 2): print('[INFO] Path is only one line. No vertices to find.') return vertices all_vert = self.original_obstacle_list + [self.original_boundary_coordinates] all_vert_flat = [item for sublist in all_vert for item in sublist] for vert in path[1:-1]: # dont use start and final positions as contstraints closest_vert, _ = self.get_closest_vert(vert, all_vert_flat) vertices.append(closest_vert) return vertices def find_closest_vertices(self, current_pos, n_vertices = 10, N_STEPS_LOOK_BACK = 2): if n_vertices >= len(self.vert): return self.vert _, idx = self.get_closest_vert(current_pos, self.vert) lb = max(0, idx - N_STEPS_LOOK_BACK) # look two objects behind ub = min(len(self.vert), n_vertices - N_STEPS_LOOK_BACK) return self.vert[lb:ub] ###### If one wants to use euclidian distance instead. # self.vert_copy.sort(key = lambda x: self.dist_between_points(current_pos, x)) # return self.vert.copy()[:n_vertices] ###### @staticmethod def generate_obstacle(p1, p2, freq, time): p1 = np.array(p1) p2 = np.array(p2) time = np.array(time) t = abs(np.sin(freq * time)) if type(t) == np.ndarray: t = np.expand_dims(t,1) p3 = t*p1 + (1-t)*p2 return p3 @staticmethod def rotate(origin, point, angle): ox, oy = origin px, py = point qx = math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy) qy = math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy) return np.array([qx, qy]) def rotate_and_add(self, p1,p2, angle, addition): rot = self.rotate(p1, p2, angle) rot[1] += addition rot = self.rotate(np.array([0,0]), rot, -angle) return rot + p1 def generate_sinus_obstacle(self, p1, p2, freq, time, ampl=1.5): p1 = np.array(p1) p2 = np.array(p2) dp = p2 - p1 angle = np.arctan2(dp[1],dp[0]) time = np.array(time) t = abs(np.sin(freq * time)) if type(t) == np.ndarray: t = np.expand_dims(t,1) p3 = t*p1 + (1-t)*p2 add = ampl*np.cos(10*freq*time) return self.rotate_and_add(p1, p3, angle, add) def get_dyn_obstacle(self, t, horizon, sinus_object=False): # TODO: allow simulation to start from previously calculated positionss if len(self.dyn_obs_list) == 0: return [] time = np.linspace(t, t+horizon*self.config.ts, horizon) obs_list = [] for i, obs in enumerate(self.dyn_obs_list): p1, p2, freq, x_radius, y_radius, angle = obs x_radius = x_radius+self.config.vehicle_width/2+self.config.vehicle_margin y_radius = y_radius+self.config.vehicle_width/2+self.config.vehicle_margin if sinus_object and i == 2: q = [(*self.generate_sinus_obstacle(p1, p2, freq, t), x_radius, y_radius, angle) for t in time] obs_list.append(q) else: obs_list.append([(*self.generate_obstacle(p1, p2, freq, t), x_radius, y_radius, angle) for t in time]) return obs_list def plot_all(self, ax): plot_boundaries(self.original_boundary_coordinates, ax, c='k', label = 'Original Boundary') plot_boundaries(self.processed_boundary_coordinates, ax, c='g', label = 'Padded Boundary') plot_obstacles(self.original_obstacle_list, c='r', ax = ax, label = 'Original Obstacles') plot_obstacles(self.processed_obstacle_list,c='y', ax = ax, label = 'Padded Obstacles') plot_path(self.path, c='--k', ax = ax) plot_vertices(self.vert, radius = self.config.vehicle_width, ax = ax)