Motion planning through graphs [Part 2]
In part 1, I provided some background information on graphs as a basic data structure, some useful applications of it, and I introduced the concept of RRTs. If all of this is new to you, it’s especially important to take a look at part 1 and understand what you are doing and why it’s useful.
I will also be going over run time analysis in this post. For those who are unfamiliar, run time analysis (also known as big-O complexity) can be thought of as a way to determine how long a program takes to run based on the size of the input. So if the input is x (x > 0), the run time can be estimated by f(x). For a problem, the optimal algorithm is said to be the one that has the lowest complexity. This does not necessarily have to be O(1) or O(n).
In this segment, we will be using Python 2.7. This is not a tutorial in Python. Okay, let’s begin by creating a file called rrt.py and add in the functions as we go over them!
So far, we know that an RRT is a unique type of graph. Whereas in traditional graphs, we try to connect a vertex to a vertex, in an RRT, we try to connect a vertex to the nearest point on the tree. This nearest point can be a vertex, but it can also be some point along an edge. This property of an RRT is what gives it a recursive “T” shape that is so distinctive.
Plan the plan
A probabilistic roadmap algorithm like RRT has several components, which I have listed below. For the purpose of simplicity, we will not deal with obstacles and collision detection. We will build this tree component by component and see it come to life!
- Randomly generate vertices
- Building the graph
- Finding the nearest vertex on the tree
- Finding the closest point on an edge
- Connecting a new vertex to the closest point on the tree
- Randomly generate vertices
Python has a handy library that can randomly generate a decimal number between 0 and 1. We will use this.
If you call this function now, it will return an (x, y) point in the square region defined by (0, 0), (10, 10).
2. Building the graph
As mentioned in part 1, we will be using an adjacency list to represent our graph. In Python, this can be accomplished through the use of dictionaries and lists. Once we have generated a vertex, we can just store it in our dictionaries like so:
points = dict()
adjList = dict()
count = 0# assign the random point to the index 0 of the dictionary
# here, the key is 0, and the value is the random point
points[count] = generateVertex()# set the adjacency list of this key to be empty initially
adjList[count] = []
3. Finding the nearest vertex on the tree
We need to do some math here. Since our graph is actually made up of (x, y) coordinates, we will be revisiting some basic geometry to find the nearest vertex on the tree. Then, we need to check all the edges that originate from that point, and find our closest point on the tree.
In addition to the adjacency list representation, we will maintain a list of all the vertices. The purpose of this is to find the nearest vertex conveniently. In the function below, arr is the array of vertices, and point is the point we want to add to our tree.
If the array has n elements, we calculate the distance between the point and the vertex at each index in the array and return the point with minimum distance. This would be our nearest neighbor. The run time of this algorithm would be O(n).
A short note to anyone who has learnt about big-O: we could do better, in terms of run time, by using kd-trees. However, that would require a lot more code, and the primary purpose of this article is to simplify things. So O(n) is okay.
4. Finding the closest point on an edge
Similar to before, we just have to perform some simple algebra to compute the shortest distance from a point to a line segment.
This piece of code is just trying to accomplish the same thing as a regular math equation.
In this function, I find the nearest point on a line segment (segment) from a point (point). Since a line segment is made from 2 points, (x1, y1) would be our vertex, and (x2, y2) would be a neighbor of the vertex. The error return value is (-1, -1). You can infer that we need to call this function for every neighbor of the nearest neighbor we found from the previous part.
5. Connecting a new vertex to the closest point on the tree
Now that we have our closest point on the tree, it’s finally time to connect it! Remember that a vertex could also be the closest point on the tree, so we need to account for that. If the closest point is not already a vertex, we need to create a new vertex, and update the adjacency lists accordingly. Using the animation from the previous part, we can see a visual representation of what changes need to be done.
A simple code snippet that shows how to update a value in a dictionary:
# some keys that we want to use
key = 0
removeKey = 2
newKey = 3arr = dict[key]# remove the vertices that will no longer be neighbors
arr.remove(removeKey)# add a new vertex that is a neighbor
arr.append(newKey)# ensure that arr is a list that does not have duplicate entries
# assign the key an updated list as a value
dict[key] = list(set(arr))
That’s it, really!
In the next and final part, we will put all these components together and write a simple graph search algorithm that builds a path.