RRT

Introduction

This is a simple implementation of the Rapidly-exploring Random Tree using Python.

It extensively uses the matplotlib library to plot the environment, the obstacles, the path and the nodes. It also requires the scipy library to use the savgol_filter function for smoothing the path.

Implementation

Node

The tree is made up of nodes. Each node has a position, and a parent.

Obstacle

The obstacles are modelled as rectangles. Each obstacle is defined by the bottom-left and top-right corners. The bounding walls are also considered as obstacles.

Environment

The Environment is modelled as a 2D grid, with the bottom-left corner as $(0, 0)$ and the top-right corner as $(50, 50)$. I used three different environments for testing the algorithm.

1. Environment 1: This is a simple environment with only walls and no obstacles between the start and the goal. The start position is $(12, 12)$ and the goal position is $(38, 38)$.

   

2. Environment 2: This is a slightly more complex environment with walls and a few obstacles between the start and the goal. The start position is $(12, 12)$ and the goal position is $(38, 38)$.

   

3. Environment 3: This is a complex environment with walls and is desigened to be a maze with only one path from the start to the goal. The start position is $(12, 12)$ and the goal position is $(36, 38)$.

   

RRT

RRT is a simple path planning algorithm that works by building a tree of nodes. It starts with the start node and keeps adding nodes to the tree, until it reaches the goal. The algorithm works as follows:

1. START with an environment which has a start position, a goal position and some obstacles. Initially, the tree has only the start node.

2. Generate a random node within the environment.

3. Find the nearest node in the tree to the random node.

4. Move from the nearest node towards the random node, but only by a certain distance step_size.

5. Add the new node to the tree.

6. If the new node is close enough to the goal, then END, else goto Step 2.

It uses the term Rapidly-exploring because, initally, when the tree is small and there is a higher chance of the random node being in an empty space. Therefore, it is more likely to explore the empty spaces first. It is also Random, because, well... the nodes are generated randomly. And it is a Tree, because the nodes are connected in a tree structure, that is, each node has only one parent, and there are no loops.

Smoothing

The path generated by the RRT algorithm almost always has a lot of sharp turns. This is because, the nodes are generated randomly, and the path is not very smooth. To make the path smoother, a simple option is to do Polynomial Regression in order to fit a smooth curve to the path.

However, as we can see, Polynomial Regression is not very good at smoothing the path. Therefore, I am going to use Moving Polynomial Regression. In this, I will take a window of points around a point, and fit a polynomial to these points. This way, the path will be much smoother. This is commonly referred to as Savitzky-Golay Filtering.

There are other methods of smoothing, such as Bezier Curves, B-Splines, etc. However, I am going to use Savitky-Golay Filtering, as it is simple and effective.

Spacing

In a normal RRT, the nodes are generated randomly. In order to account for obstacles, we simply implement in such a way that, if there is a collision with an obstacle, then we can discard the node, and try with a new random node.

However, it doesn't take into account the spacing between the obstacle and the bot. Without spacing, RRT can generate a path that could be very close to the obstacle or could pass very close at the corners.

In order to prevent this, we just adding padding around each obstacle. The amount of padding is the spacing parameter. I implemented this in a probabilistic way. The probability of placing a node in a particular position is given by a sigmoid function of the distance of the node from the closest obstacle such that, the probability of placing a node at a distance $= \text{spacing}$ from the obstacle is 0.5, and the probability decreases as we move closer to the obstacle, and increases as we move away from the obstacle. The plots of the probability for Environmen are shown below.

Greed

In a greedy approach to the RRT algorithm, we can add a parameter called goal_bias. If goal_bias $=0$, then the nodes are purely random. If not, there is a chance that the random node is the goal node itself, or a node close to the goal node. This way, the algorithm finds the goal much faster.

If the goal_bias is high, then the algorithm will find the goal much faster in environments where the goal is easy to reach such as Environment 1. However in complex environments such as Environment 3, the algorithm might not find the goal at all. Notice the high concentration of nodes near the goal in the second image.

Code

I implemented the RRT algorithm in Python. The whole code and its explanation is given here:

Imports

I used matplotlib to plot the environment and the path. I also used numpy for some mathematical operations. The random library is used to generate the random numbers. I also used copy to make deep copies of the environment. The math library is used for mathematical operations, such as sqrt and exp

Node

I defined a class Node, which stores the position of the node and the parent of the node, which is later used to construct the path. It has a __repr__ method, that returns a string representation of the node in the form (x, y).

It also has a distance method, that calculates the Euclidean distance between two nodes.

Obstacle

The next class is Obstacle. It stores the bottom-left and top-right corners, width, height of the obstacle. It also has an attribute is_wall in order to denote if the obstacle is a wall or not.

The functions of the Obstacle class are:

• __contains__: checks if a Node is within the obstacle. I used it so that I can use the in operator to check if a node is within the obstacle.

• distance: calculates the distance of a node from the obstacle. The distance is the distance from the node to the closest point on the obstacle. It does this by finding which side of the obstacle the node is on, and then calculating the distance accordingly. If the node is to the right, left, above or below the obstacle, then the distance is the perpendicular distance from the node to the closest side of the obstacle. If the node is to the top-right, top-left, bottom-right or bottom-left of the obstacle, then the distance is the distance from the node to the closest corner of the obstacle.

Environment

Then I defined a class Environment, that stores environment-specific information such as obstacles, start and goal positions. It also stores the goal-radius, which is the radius around the goal node, within which the goal is considered to be reached.

The functions of the Environment class are:

• add_obstacle: adds an obstacle to the environment.

• random_node: generates a random node in the environment. This random could be anywhere in the environment, even within the obstacles.

• is_valid_node: returns a boolean value, indicating if the node is within the environment and not within any obstacle.

• prob: returns the probability of placing a node in that particular position. The probability is 0 inside obstacles, and there is a gradient from 0 to 1 as we move away from the obstacles. The gradient is a sigmoid function of the distance of the node from the closest obstacle given by Obstacle.distance. If $r$ is this distance, then the probability is given by

  $$P(r) = \frac{1}{1 + e^{-k(r - s)}}$$

  Here $k$ is a constant that determines the steepness of the gradient. For large $k$, the gradient is much more sharp. $s$ is the spacing parameter. It is such that, the probability of finding a node at a distance $= s$ from the obstacle is 0.5. This is how I implemented the spacing parameter.

  

  

• is_suitable_node: returns the probability of placing a node in that particular position using the prob method. It also checks if the line between the nearest node and the new node is valid using the is_valid_line method.

• is_valid_line: returns a boolean value, indicating if the line between two nodes is valid. This is done by dividing the line into small segments and checking if each point is valid.

RRT

Finally, we have the RRT class, which uses the functions of all the previously mentioned classes to implement the RRT algorithm. I store a deepcopy of the environment, so that I can modify the environment in the RRT class, without affecting the original environment. I also store a list of nodes, and other parameters such as goal_bias, step_size, steepness. Finally, I also store the target node (the node that is close to the goal) and the path.

The functions of the RRT class are:

• reset: It is a simple function that resets the nodes, the target node and the path and the length of the path.

• plan: It runs a loop for max_iter number of times. In each iteration, it calls the new function which returns a new node to be placed. This node is appended to nodes. If this node is within the goal-radius, then the target node is set to this node, and the loop is broken. After the loop, the path is generated by calling the construct function.

• new: This function generates a random node using Environment.random_node. It then finds the nearest node to this random node by calling the nearest function. It generates the new node by moving from the nearest node towards the random node by a distance step_size. The probability of placing this node is calculated using the Environment.is_suitable_node function. Depending on this probability, the node is either placed or discarded. If the node is discarded, a new node is generated, and the process is repeated. If the node is placed, then it is given to the plan function.

• nearest: This function finds the nearest node in the tree to the given node. It does this by iterating through all the nodes and finding the node with the minimum distance, and returns this node.

• goal_reached: This function checks if the target node is within the goal-radius, and returns the boolean value.

• construct: This function constructs the path from the start node to the target node. It does this by starting from the target node, and moving to its parent, and then moving to the parent of the parent, and so on, until the start node is reached. Finally, it reverses the path, so that the start node is the first element of the path, and returns it.

• plot: This function is used to plot the environment, the obstacles, the path and the nodes, using matplotlib.

• smoothen_path: This function uses scipy.signal._savitzky_golay.savgol_filter to smoothen the path. It takes a window size of 7 and a polynomial degree of 2. It returns the smoothened path and the length of the path.

• plot_prob: This function is used to plot the probability of placing a node at each point in the environment. It uses the Environment.is_suitable_node function to calculate the probability.

Usage

Now that we have defined all the classes and functions, we can use them to run the RRT algorithm. Here, I have given an example of how to use it to implement the RRT algorithm to find a path in Environment 2.

Firstly, I defined a function that returns an environment from Environment 1, Environment 2 or Environment 3 based on the parameter.

Now, I can use this function to create an environment, and then use the RRT class to find a path in this environment.

In order to get some insights into the performance of each variant, I defined a function test, which runs the RRT algorithm for each variant, and stores the data as a csv file.

Results

By using the test function, I ran the RRT algorithm for each variant 1000 times, and stored the data in a csv file. I then used this data to plot the performance of each variant. In order to compare all of them, I used the same parameters for each variant, except for the goal bias and the spacing. The parameters used were:

• Normal: goal_bias = $0$, spacing = $0$

• Spaced: goal_bias = $0$, spacing = $1$

• Greedy: goal_bias = $0.37$, spacing = $0$

• Speedy: goal_bias = $0.37$, spacing = $1$

Speedy is the combination of Greedy and Spaced. Literally, it is Spaced + Greedy.

The values of spacing was chosen as $0.5$ for Environment 3, as the obstacles were much closer to each other.

Environments

Before we look at the results, let's look at the environments and the smooth paths for each of them.

Path Length

I am going to be using Path Length in order to compare each variant, as it is a better indicator than the number of nodes in the path.

First, we have the frequency of the path length for each variant in each environment.

We can also plot the average path length for each variant in each environment.

This can be better visualized using a violin plot as shown below:

Conclusions

• We can observe that the Greedy variant has the best performance in environments with fewer obstacles such as Environment 1 and Environment 2.

• In environments with more obstacles such as Environment 3, the Spaced variant performs the best. This is because the obstacles are much closer to each other, and the Spacing helps in avoiding these obstacles.

• Overall the Speedy variant performs the best, as it has both Goal Bias and Spacing.

• As expected, the Normal variant performs the worst in all environments.

The Effect of Spacing

For part (c) of the Task, We are asked to check our implementation of Spacing for an environment as shown here:

Without any spacing and without any goal bias, the path found by the RRT algorithm is as shown below:

Observe the path, it goes very close to the obstacles. This is not ideal and it can be fixed by using the Spacing parameter. I used a Spacing of $3$ for this environment, and the path found by the RRT algorithm is as shown below:

We can clearly notice, that the Spaced variants have much better paths. It doesn't try to go through the small gaps between the obstacles and the wall.

Time Complexity

One interesting thing to note is the time taken by each variant. I am going to plot the Time Taken for the RRT algorithm for each variant vs The total number of obstacles in the environment.

Conclusions

• There is a direct correlation between the Total Number of Obstacles and the Time Taken by the RRT algorithm. The relationship is exponential, and the shape of the curve remains the same for all variants for each environment.

• For environments with fewer obstacles, the Greedy variants take significantly less time than the non-Greedy variants.

• However, in Environment 3, all the variants take a similar amount of time, as the obstacles are much closer to each other.

• We can see, for a given Number of Nodes, the Speedy variant takes the most time, as it has to check for both Spacing and Goal Bias. The Normal variant takes the least time, as it doesn't have to check for either.

• One interesting thing to note is that the Spaced variant takes more time than the Greedy variant in Environment 2. This makes sense considering that, the program generates a lot more random nodes, as a lot of them are discarded due to the Spacing. Meanwhile, for Greedy, the program has a chance to choose a closer node, but it doesn't have to generate as many random nodes.