Click here for the parent post about Formula Electric
Result Link to heading
Overview Link to heading
Our implimentation is heavily based on this paper
We still have optimizations for RRT*, like
- limiting the space we randomly sample points from to in front of the car
- punishing steep angle changes
Here is a good video explaining RRT*:
- The main program is RRT.py
- RRT.py has an infinite loop which instantiaites and calls other classes.
- RRT.py uses pygame, TreePath, and Obstacle datastructures to do and display the RRT algorithm.
- treepath.py is the main data structure representing the treelike path of rrt.
- Obstacle.py has the Obstacle class that uses the Point data structure to represent obstacles.
Classes Link to heading
- point.py
- Obstacle.py
- Tests.py
- treepath.py
- RRT.py
Deep dive into each Class Link to heading
point.py Link to heading
Super simple class that represents a point (for obstacle class)
It is easier to read code when we write point.x instead of point[0]
dependencies Link to heading
None
methods Link to heading
makes a point
def __init__(self, x, y):
self.x = x
self.y = y
attributes Link to heading
self.x # x coordinate of point
self.y # y coordinate of point
Obstacle.py Link to heading
Represent an obstacle in 2 dimentions. We want to feed in a list of Point objects and create an obstacle. This obstacle should be able to draw itself and handle collisions with paths.
This code defines an obstacle class used in a RRT simulation that checks if a line segment from node N to node K intersects with the obstacle edges. The class has a constructor that initializes the obstacle with a list of points and a method that draws the obstacle using the pygame library. The method for checking if a line segment intersects with the obstacle edges uses the shapely library to create line segment data structures and checks if they intersect with any of the obstacle edges. If an intersection is found, the method returns True. If no intersection is found, the method returns False.
dependencies Link to heading
import pygame
from shapely.geometry import LineString
import numpy as np
methods Link to heading
Take in a list of Point objects
# initialize obstacle with its edge points (use list of Point data structure)
def __init__(self, points):
self.points = points
Checks if path from node N to node K intersects itself
# checks if a line from node N to node K intersects with it
def instersects(self, N, K):
# make line segment data structure
pathline = LineString([(N.xpos, N.ypos), (K.xpos, K.ypos)])
# check if pathline intersects with any of the obstacle edges
for i in range(len(self.points)):
tempLine = LineString([(self.points[i].x, self.points[i].y), (self.points[(i+1)%len(self.points)].x, self.points[(i+1)%len(self.points)].y)])
if pathline.intersects(tempLine):
return True
# if no intersection with any edge is found, return false
return False
Draw itself
# draw the obstacle in pygame
def draw(self, surface):
drawPoints = []
for i in range(len(self.points)):
drawPoints.append((self.points[i].x, self.points[i].y))
pygame.draw.polygon(surface, obstacleColor, drawPoints)
treepath.py Link to heading
Main datastructure. It is a node with position attributes, a parent attribute, and a list of children. A root node is initialized and the successive nodes are added as children to the tree.
This code defines a class TreePath that is used for constructing a tree and finding the shortest path between nodes in the tree. Each instance of the class represents a node in the tree, which is specified by its x and y coordinates, and its parent node. The class includes methods for adding child nodes to a given parent node, finding the nearest node to a given node, and checking if there are obstacles between a given node and the root node of the tree. There is also a method for finding all nodes within a given distance of a specified node. Additionally, there are methods for steering the distance of a given node, calculating the distance between two nodes, and calculating the cost of a path from a node to the root of the tree.
dependencies Link to heading
import numpy as np
import pygame
methods Link to heading
Initialize attributes
def __init__(self, xpos, ypos, parent = None):
self.xpos = xpos
self.ypos = ypos
self.parent = parent
self.children = []
self.path = [self]
nodeList.append(self)
gets the distance between two nodes (static)
def getDistance(leaf1, leaf2):
x = abs(leaf1.xpos - leaf2.xpos)
y = abs(leaf1.ypos - leaf2.ypos)
return np.sqrt(x*x + y*y)
gets distance from self to a node
def getDistanceTo(self, leaf1):
x = abs(leaf1.xpos - self.xpos)
y = abs(leaf1.ypos - self.ypos)
return np.sqrt(x*x + y*y)
Creates a node at specified position and adds it as a child.
def addChild_byPosition(self, xpos, ypos):
Adds a specified node as a child to self
def addChild_byPointer(self, tree):
removes a specified child
def removeChild(self, tree):
modifies the coordinates of x_new to be exactly steerDistance away from x_nearest
def distanceSteer(self, x_nearest, x_new):
dist = x_nearest.getDistanceTo(x_new)
x_new.xpos = x_nearest.xpos + steerDistance* (x_new.xpos - x_nearest.xpos)/dist
x_new.ypos = x_nearest.ypos + steerDistance* (x_new.ypos - x_nearest.ypos)/dist
same as above but makes sure x_new is not going “backwards” with respect to the path.\
def direction_distance_steer(self, x_nearest, x_new):
# distance constraint
dist = x_nearest.getDistanceTo(x_new)
x_new.xpos = x_nearest.xpos + steerDistance* (x_new.xpos - x_nearest.xpos)/dist
x_new.ypos = x_nearest.ypos + steerDistance* (x_new.ypos - x_nearest.ypos)/dist
# dot product for direction correcting
if x_nearest.parent != None:
if (x_new.xpos - x_nearest.xpos) * (x_nearest.parent.xpos - x_nearest.xpos) + (x_new.ypos - x_nearest.ypos) * (x_nearest.parent.ypos - x_nearest.ypos) > 0:
x_new.xpos *= -1
x_new.ypos *= -1
#return node close to node closest to K
gets the nearest node to K out of all children
def nearest(self, K):
gets the nearest node to K out of all children and its distance
def nearestHelper(self, K):
Removes a specified child node
def removeChild(self, child):
self.children.remove(child)
#return node and distance of node closest to K recursive
Checks if the path from self to node K intersects any obstacles
def ObstacleFree(self, K, obstacles):
#iterates through a list of obstacle objects
for obstacle in obstacles:
if obstacle.instersects(self, K):
return False
return True
# takes in radius and node, returns all nodes within distance r to node
Gets all nodes within radius of N.
def Near(self, radius, N):
returnArray = []
#recursive call either including itself or not
if (self.getDistance(N) <= radius):
returnArray.append(self)
for i in self.children:
returnArray.extend(i.Near(radius, N))
return returnArray
returns the cost (distance it takes from the root node) to target
def cost(target):
if target.parent == None:
return 0
return TreePath.getDistance(target, target.parent) + TreePath.cost(target.parent)
#gets total euclidian distance required to travel along certain path
#input is list of nodes in order, output is distance
returns the cost (distance) to traverse the list of nodes
def c(self, pathlist):
if len(pathlist) == 0:
return 0
else:
totalDist = 0
for i in range(len(pathlist) - 1):
totalDist += TreePath.getDistance(pathlist[i-1], pathlist[i])
return totalDist
get a new random node
def sample_free(xdimension, ydimension):
newx = np.random(0, xdimension)
newy = np.random(0, ydimension)
return TreePath(newx, newy)
draw the tree
def draw(self,surface):
pygame.draw.circle(surface, nodeColor, (self.xpos, self.ypos), nodeRadius)
# print("drawing childern: ", self, self.children)
for a in self.children:
pygame.draw.line(surface, edgeColor, (self.xpos, self.ypos), (a.xpos, a.ypos))
a.draw(surface)
draw the path from the root node to node
def drawPathTo(surface, node):
if node.parent == None:
return
pygame.draw.line(surface, FoundPathColor, (node.xpos, node.ypos), (node.parent.xpos, node.parent.ypos), width=foundPathWidth)
TreePath.drawPathTo(surface, node.parent)
attributes Link to heading
self.xpos = xpos
self.ypos = ypos
self.parent = parent
self.children = []
self.path = [self]
nodeList.append(self)
RRT.py Link to heading
Main program we run that uses pygame to display the RRt algorithm.
We create an obstacles list Obstacles, and set various settings such as the goal node location, optimization steps.
The infinite loop displays the algorithm every iteration and waits 1 second when the algorithm is completed.
dependencies Link to heading
import sys
sys.path.append('../')
from penisC.Tree.treepath import TreePath
from penisC.Tree.Obstacle import Obstacle
from penisC.Tree.point import Point
import pygame
import numpy as np
from numpy import random
methods Link to heading
return a random node from the search space (the screen)
# return a new node in a random place on the screen
def SAMPLE_FREE(width, height):
x = random.rand() * width
y = random.rand() * height
return TreePath(x, y)
attributes Link to heading
Dimentions of the game area:
width = 700
height = 700
dim = [width, height]
# Set up the drawing window
screen = pygame.display.set_mode(dim)
List of Obstacles
Obstacles = [Obstacle([Point(0,0), Point(100,0), Point(100,100), Point(0,100)]),
Obstacle([Point(200,200), Point(200,270), Point(150,360), Point(10,400)]),
Obstacle([Point(500,200), Point(500,270), Point(600,360), Point(400,400)]),
Obstacle([Point(300, 0), Point(400, 0), Point(300, 600), Point(400,600)])
]
Initialize Goals with goal node radius 50 and location 600, 100:
goalTolerance = 50
goalNode = TreePath(600, 100)
used to exit out of window
running = True
Instantiaite the root node of the rrt tree. Set the root node to location 100, 200.
rrt = TreePath(100,200)
found does nothing in practice, but turns to true when a path to the goal is found. optimization steps is the amount of additional RRT iterations we do after we find a path to the goal (to hopefully optimize our path).
found = False
optimizationSteps = 400
x_rand is the new node initialized as a random node from the sample space. x_nearest is the nearest node in the rrt tree to x_rand.
x_rand = SAMPLE_FREE(width, height)
x_nearest = rrt.nearest(x_rand)
near Nodes is a list of nodes that are within 100 units of x_rand. Used to optimize the treepath.
nearNodes = rrt.Near(100, x_rand)
Tests.py Link to heading
This is where we have the unit tests for all other classes.
This code defines a set of unit tests for a class called TreePath which represents a node in a tree structure. The TreePath class is imported from the treepath module, along with other classes Obstacle and Point. The unittest module is also imported.
There are several tests defined in the TestTree class, which is a subclass of unittest.TestCase. The setUp() method, which is typically used to set up any state needed for the tests, is not used in this case.
test_TreeInitialization(): This test checks if creating nodes creates nodes with the proper initialization conditions. Three TreePath objects are created with different coordinates and their xpos, ypos, and parent attributes are checked. This test also checks that different initializations should result in different objects, but two objects with the same coordinates should not be equal.
test_TreeStructure(): This test checks if adding child by pointer correctly creates nodes and the parent nodes have children lists that include their children. Several TreePath objects are created, and a tree structure is built by adding children using the addChild_byPointer() method. The test checks the correct parent-child relationships and the correct children lists.
test_RemoveChild(): This test checks if removing children works as expected. A tree structure is built, and two children are removed from a parent node using the removeChild() method. The test checks that the children are correctly removed and the tree structure is updated accordingly.
test_sanity(): This is a simple test that checks if 1 > 0, which should always be true. This test is used to ensure that the testing framework is working correctly.
test_getDistanceTo(): This test checks if the getDistanceTo() method returns the correct Euclidean distance between two TreePath objects. Two objects are created with different coordinates, and the distance between them is computed using the getDistanceTo() method. The result is checked against a known value.
test_getDistance(): This test checks if the getDistance() method returns the correct Euclidean distance between two TreePath objects. Two objects are created with different coordinates, and the distance between them is computed using the getDistance() method. The result is checked against a known value.
test_addChild_byPosition(): This test checks if the addChild_byPosition() method correctly adds a child node to a parent node using the xpos and ypos arguments. The test checks that the parent-child relationships and the list of nodes are correctly updated.
test_addChild_byPointer(): This test checks if the addChild_byPointer() method correctly adds a child node to a parent node using the child node object. The test checks that the parent-child relationships and the list of nodes are correctly updated.
Overall, these tests provide good coverage of the TreePath class and its methods. The tests ensure that the expected behavior is maintained even as the class is modified or updated.
dependencies Link to heading
import treepath
from treepath import TreePath
from obstacle import Obstacle
from point import Point
import unittest
methods Link to heading
Tests if the constructor of treepath is working correctly
def test_TreeInitialization(self):
t1 = TreePath(0,0)
self.assertEqual(t1.xpos, 0, 'The xpos is wrong')
self.assertEqual(t1.ypos, 0, 'The ypos is wrong')
self.assertEqual(t1.parent, None, 'The parent is wrong')
t2 = TreePath(-5,7.9)
self.assertEqual(t2.xpos, -5, 'The xpos is wrong')
self.assertEqual(t2.ypos, 7.9, 'The ypos is wrong')
self.assertEqual(t2.parent, None, 'The parent is wrong')
self.assertNotEqual(t2, t1, 'different initializations are equal')
t3 = TreePath(-5,7.9)
self.assertNotEqual(t2, t3, 'different initializations, with same coordinate are equal when they shouldnt be')
Tests if adding children is working properly
def test_TreeStructure(self):
n1 = TreePath(0,0)
n2 = TreePath(2,2)
n3 = TreePath(9,9)
n4 = TreePath(10,10)
n5 = TreePath(10,10) # try out duplicates
n6 = TreePath(10,15)
n1.addChild_byPointer(n2)
n1.addChild_byPointer(n3)
n2.addChild_byPointer(n4)
n2.addChild_byPointer(n5)
n2.addChild_byPointer(n6)
# structure:
# n1
# / \
# n2 n3
# / | \
# n4 n5 n6
self.assertEqual(n1.children, [n2, n3])
self.assertEqual(n2.children, [n4, n5, n6])
self.assertEqual(n4.children, [])
self.assertEqual(n2.parent, n1)
self.assertEqual(n5.parent, n2)
Tests if removing children is working properly.
def test_RemoveChild(self):
n1 = TreePath(0,0)
n2 = TreePath(2,2)
n3 = TreePath(9,9)
n4 = TreePath(10,10)
n5 = TreePath(10,10) # try out duplicates
n6 = TreePath(10,15)
n1.addChild_byPointer(n2)
n1.addChild_byPointer(n3)
n2.addChild_byPointer(n4)
n2.addChild_byPointer(n5)
n2.addChild_byPointer(n6)
# structure:
# n1
# / \
# n2 n3
# / | \
# n4 n5 n6
n2.removeChild(n5)
n2.removeChild(n4)
# structure:
# n1
# / \
# n2 n3
# /
# n6
self.assertEqual(n2.children, [n6])
self.assertEqual(n1.children, [n2, n3])
n1.removeChild(n2)
# structure:
# n1
# /
# n3
self.assertEqual(n1.children, [n3])
Tests sanity
def test_sanity(self):
assert 1 > 0
Tests get distance function
def test_getDistanceTo(self):
x1 = 1.2
y1 = 5.3
x2 = -2.3
y2 = 100.5
t1 = TreePath(x1,y1)
t2 = TreePath(x2, y2)
self.assertAlmostEqual(t1.getDistanceTo(t2), 95.2643165094)
Tests get distance function
def test_getDistance(self):
x1 = 1.2
y1 = 5.3
x2 = -2.3
y2 = 100.5
t1 = TreePath(x1, y1)
t2 = TreePath(x2, y2)
self.assertAlmostEqual(TreePath.getDistance(t1, t2), 95.2643165094)
Tests adding children by specifying the position where the new child should be created.
def test_addChild_byPosition(self):
treepath.nodeList = []
t = TreePath(1,2)
t.addChild_byPosition(3,45)
self.assertEqual(t.children[0].parent, t)
self.assertEqual(treepath.nodeList[0], t)
self.assertEqual(treepath.nodeList[1], t.children[0])
t.addChild_byPosition(3,22)
t.children[0].addChild_byPosition(23,4)
self.assertEqual(t.children[0].children[0], treepath.nodeList[3])
Tests adding children.
def test_addChild_byPointer(self):
treepath.nodeList = []
t = TreePath(1,2)
c1 = TreePath(3,4)
c2 = TreePath(5,6)
t.addChild_byPointer(c1)
c1.addChild_byPointer(c2)
self.assertEqual(t.children, [c1])
self.assertEqual(c1.children, [c2])
t.addChild_byPosition(3,22)
self.assertEqual(len(t.children), 2)
Tests the nearest function that returns the nearest node to a given node.
def test_nearest(self):
n1 = TreePath(0,0)
n2 = TreePath(2,2)
n3 = TreePath(9,9)
target = TreePath(10,10)
n1.addChild_byPosition(2,3)
n1.addChild_byPosition(3,3)
n1.addChild_byPosition(3,2)
n1.addChild_byPosition(3,5)
n1.addChild_byPointer(n2)
n2.addChild_byPosition(5,5)
n2.addChild_byPosition(6,6)
n2.addChild_byPointer(n3)
self.assertEqual(n1.nearest(target), n3)
Tests Obstacle free.
def test_ObstacleFree(self):
n1 = TreePath(-1,-1)
n2 = TreePath(10,0)
n3 = TreePath(10, 10)
n4 = TreePath(0, 10)
n5 = TreePath(-10,-10)
obstacles = [Obstacle([Point(0,0), Point(1,0), Point(1,1), Point(0,1)])]
# on the board should return false?
self.assertEqual(n1.ObstacleFree(n2, obstacles), True)
self.assertEqual(n1.ObstacleFree(n3, obstacles), False)
self.assertEqual(n1.ObstacleFree(n4, obstacles), True)
self.assertEqual(n1.ObstacleFree(n5, obstacles), True)
# same both ways
self.assertEqual(n2.ObstacleFree(n1, obstacles), True)
self.assertEqual(n3.ObstacleFree(n1, obstacles), False)
self.assertEqual(n4.ObstacleFree(n1, obstacles), True)
self.assertEqual(n5.ObstacleFree(n1, obstacles), True)
# returns itself as well if th enode is already added to tree
Tests Near which returns nodes within a given radius to a node.
def test_Near(self):
t1 = TreePath(0,0)
n1 =TreePath(3,3)
t1.addChild_byPointer(n1)
t1.addChild_byPosition(10,10)
n2 = TreePath(3,4)
# t1.addChild_byPointer(n2)
self.assertEqual(t1.Near(2, n2), [n1], 'Near is wrong. Case: node not already in tree')
t1.addChild_byPointer(n2)
self.assertIn(t1.Near(2, n2), [[n1, n2], [n2, n1]], 'Near is wrong. Case: node already in tree')
tests the obstacles class and instersections of paths.
def test_Obstacle(self):
# make a square of length 1
o = Obstacle([Point(0,0), Point(1,0), Point(1,1), Point(0,1)])
n1 = TreePath(0,0)
n2 = TreePath(10,0)
n3 = TreePath(10, 10)
n4 = TreePath(0, 10)
n5 = TreePath(-10,-10)
o = Obstacle([Point(0,0), Point(1,0), Point(1,1), Point(0,1)])
self.assertEqual(o.instersects(n1, n3), True)
self.assertEqual(o.instersects(n3, n5), True)
self.assertEqual(o.instersects(n1, n2), True) # on the board should return true?
self.assertEqual(o.instersects(n4, n3), False)
# both ways
self.assertEqual(o.instersects(n3, n1), True)
self.assertEqual(o.instersects(n5, n3), True)
self.assertEqual(o.instersects(n2, n1), True) # on the board should return true?
self.assertEqual(o.instersects(n3, n4), False)
Tests if nodeList is being updated every time we make a new node.
def test_randomTest(self):
treepath.nodeList = []
t = TreePath(0,0)
x = t
for i in range(100):
x.addChild_byPosition(0,0)
x = x.children[0]
self.assertEqual(len(treepath.nodeList), 101, 'the list does not contain them all.')
Tests the cost function (the distance it takes to get to it from the root node)
def test_Cost(self):
treepath.nodeList = []
t = TreePath(0,0)
for i in range(10):
t.addChild_byPosition(1,i)
t1 = t.children[0]
for j in range(10):
t1.addChild_byPosition(2,i)
t2 = t1.children[0]
# . .
# . .
# t? t?
# / /
# t - t1 - t2
self.assertAlmostEqual(2.0, t2.xpos)
self.assertAlmostEqual(0.0, t2.ypos)
self.assertAlmostEqual(1.0, TreePath.cost(t1))
self.assertAlmostEqual(2.0, TreePath.cost(t2))
Tests the cost of traversing a given list of nodes (total distance)
def test_C(self):
t = TreePath(0,0)
t.addChild_byPosition(10,10)
t.addChild_byPosition(20,20)
t.addChild_byPosition(15,15)
self.assertAlmostEqual(t.c([t.children]), 35.35535)
Not implimented
def test_Nearest(self):
pass