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
if __name__ == "__main__": environment = Environment() # 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
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
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)