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
