class RRT: """Naive 2-D RRT that doesn't incorporate dynamics into the system""" def __init__(self, start, goal): self.start = start self.goal = goal # distance within which tree has 'reached' the goal self.eps = 5 # extension distance self.ext = 10 self.tree = KDTree(len(self.start), [self.start]) # edges self.connections = dict() # run the algorithm and return a path from start to goal def run(self, world): i = 0 while i < 2500: i += 1 s1 = self.sample(world) s = self.tree.kNearestNeighbors(s1, 1, [])[0][0] s1e = self.extend(s, s1, world) if not s1e: continue else: self.tree.insert(s1e) try: self.connections[s].add(s1e) except: self.connections[s] = set([s1e]) try: self.connections[s1e].add(s) except: self.connections[s1e] = set([s]) if dist(s1e, self.goal) < self.eps: return AStarSearch(self.connections, self.start, self.goal, self.eps) return [] # sample world uniformly def sample(self, world): if random.random() < 0.05: return self.goal point = (random.random() * world.displayWidth, random.random() * world.displayHeight) for obstacle in world.obstacleBeliefs: # make sure the point is at least carsize / 2 away from the nearest obstacle if obstacle.colliderect( (point[0] - world.cars[0].size[0] / 2., point[1] - world.cars[0].size[1] / 2., world.cars[0].size[0], world.cars[0].size[1])): return self.sample(world) return point def extend(self, p1, p2, world): pointDir = (p2[0] - p1[0], p2[1] - p1[1]) magDir = dist(pointDir, (0, 0)) newPoint = (p1[0] + self.ext * pointDir[0] / magDir, p1[1] + self.ext * pointDir[1] / magDir) for obstacle in world.obstacleBeliefs: if obstacle.colliderect( (newPoint[0] - world.cars[0].size[0] / 2., newPoint[1] - world.cars[0].size[1] / 2., world.cars[0].size[0], world.cars[0].size[1])): return None return newPoint
class PRM: def __init__(self, world): self.points = KDTree(2, [world.kdtreeStart]) # number of samples in the prm self.size = 2500 # number of connections per sample self.connsPerSample = 6 self.carSize = world.carSize self.getPoints(world) # edges of the prm self.connections = self.getConnections(world) # uniformly sample floating point coordinates from the world def sample(self, world): point = (random.random() * world.displayWidth, random.random() * world.displayHeight) for obstacle in world.obstacleBeliefs: if obstacle.colliderect((point[0] - self.carSize[0] / 2., point[1] - self.carSize[1] / 2., self.carSize[0], self.carSize[1])): return self.sample(world) return point # uniformly sample integer coordinates from the world def sampleInt(self, world): point = (random.randint(0, world.displayWidth), random.randint(0, world.displayHeight)) for obstacle in world.obstacleBeliefs: if obstacle.colliderect((point[0] - self.carSize[0] / 2., point[1] - self.carSize[1] / 2., self.carSize[0], self.carSize[1])): return self.sampleInt(world) return point # fill the KD tree def getPoints(self, world): for _ in range(self.size - 1): self.points.insert(self.sample(world)) # ignore this function def getPaths(self, world): paths = Paths() for p1 in self.connections: for p2 in self.connections[p1]: print p1, p2 if p1 == p2: continue try: _ = paths[p1, p2] except: paths[p1, p2] = RRT(p1, p2).run(world) return paths # generate a path (a list of points along the prm) from p1 to p2 def getPath(self, p1, p2, world): if not self.points.contains(p1): self.insertConnection(p1, world) if not self.points.contains(p2): self.insertConnection(p2, world) path = AStarSearch(self.connections, p1, p2, 5, self.size) # use RRT if no path exists along prm if not path: path = RRT(p1, p2).run(world) return path # insert a point into the prm def insertConnection(self, point, world): nns = self.points.kNearestNeighbors(point, self.connsPerSample, []) self.connections[point] = [] for p in nns: collided = False for obstacle in world.obstacleBeliefs: if obstacle.collideline((point[0], point[1], p[0][0], p[0][1]), 10): collided = True if not collided: self.connections[point].append(p[0]) self.connections[p[0]].append(point) self.points.insert(point) # form the prm from the set of sampled points def getConnections(self, world): connections = dict() pointsList = self.points.list() for point in pointsList: nns = self.points.kNearestNeighbors(point, self.connsPerSample + 1, []) connections[point] = [] for p in nns: collided = False for obstacle in world.obstacleBeliefs: if obstacle.collideline( (point[0], point[1], p[0][0], p[0][1]), 10): collided = True if not collided: connections[point].append(p[0]) connections[point].remove(point) return connections