Path Planning

RRT represents a class of methods (Rapidly exploring Random Trees) to finding an optimal path within a given free space from one point to another. It is often used in planning robotic motion constrained motion problems due to its flexibility.

I started the project as an application to a club, but worked on it after as a passion project, and it provided a nice introduction to researching and implementing algorithms.

The core idea is to sample the free space and attempt to grow the tree from the root (starting point) to the sampled point by a chosen step size. Over many iterations, the tree will flood the free space and a solution will be found.

The algorithm makes heavy use of finding the nearest neighbor to the sampled point, and so spatial indexing is often used as a speed up. There are many trees that can be used for indexing, but I chose to implement RTrees due to its flexibility, mainly, its ability to handle objects outside of just points. This becomes handy when dealing with meshes for fast obstacle collision detection.

The simple implementation is not very effective however, as the path is not optimal in terms of length. It is also not effective in maneuvering small spaces:

As it fails to find the optimal path in a maze. One of these problems can be tackled with rrt*, a variant which adjusts the wiring of the new node to the tree. Rather than selecting the closest neighbor to the sampled node, we select the tree node in a neighborhood of the nearest neighbor. The cost function will determing the parent node, and the metric used is the distance from the sampled node to the root of the tree,

We can see that the distance from the root to the leaf nodes are shortened substantially.

There are many other variants, and here is a list and some details of the ones I implemented:

RRT Connect

A bidirectional form of rrt where two trees are grown, one from the start and one from the goal point.

Informed RRT*

After the initial RRT* solution has been found, sampling is restrained to an ellipsoid, for faster convergence to the solution.

Quick RRT

Similar to RRT* but we also consider the ancestors in the rewiring method of RRT*. By doing this, we decrease the depth of the tree, making the path more linear.

EP RRT

Similar to Informed RRT* but we sample within a given radius to our solution path for faster convergence to the solution.

BTO RRT

This makes two modifications. One is the connect heuristic, branch from RRT Connect. Rather than having both trees grow to each other's roots, one tree grows to the other's root, while the other grows towards the most recently appended node, which acts sort of like a flag.

The second modification is the post processing of the path. The path is downsampled to get a tighter solution, and interpolated with a parametric spline for path smoothing. We then clamp our path with keypoints to ensure that no collision is added during the interpolation.

Overall, it was a very fun and rewarding project to work on. Here are some cool animations of visualizing the solution path.

I also built a GUI for visualization of the solution path, obstacles, and underlying data structures, which can be found on Github.

Rapidly Exploring Random Trees

Path Planning