This assignment was part of the 16-782 Planning and Decision-Making in Robotics course at Carnegie Mellon University. The goal was to develop various sampling-based planners for a robotic arm to move from its starting configuration to a goal configuration where the planner returns a set of sequential angles correlating to moving between the two configurations collision-free.
Three planning algorithms were used:
- Rapidly Exploring Random Tree (RRT)
- RRT-Connect
- RRT*
Results are presented by showing a comparison across the three algorithms of the following:
- Average planning times
- Success rates for generating solutions in under 5 seconds
- Average number of vertices generated
Below each algorithm description is a GIF showing the path generated by the corresponding algorithm from the start configuration to the end configuration. The planner is written in C++ and executed through the Mex function of MATLAB.
RRT
Rapidly Exploring Random Tree is an algorithm that employs sampling and searching where a search tree is incrementally constructed. A random sequence of samples is used as a guide in the incremental construction of a tree and, in the limit, the tree densely covers the space. Essentially, it constructs a tree from a starting configuration and randomly searches for a path to a goal configuration until a solution is found and the path returned.
I created a class Tree which I used to generate the RRT tree and keep track of the vertices and edges as it expands. It implements a deque for tracking the vertices, a map for the neighbors and a map for the edges; it includes functions such as add_vertex(), add_edge(), list_vertices(), num_neighbors(), return_parent(), etc.
RRT-Connect
RRT-Connect is based on the RRT algorithm but instead of growing in one direction, it incorporates bidirectional growth of the tree. It grows from the starting configuration and from the goal configuration at the same time, and the expansion ceases when the two trees meet.
In implementing RRT-Connect I used the Tree class generated for RRT with the Extend() and Connect() functions modified.
RRT*
RRT* is a combination of RRT and A* search where the algorithm continues to search and grow the tree even after a solution is found. As the amount of nodes grows, even if a solution if found, the algorithm will return the optimal solution. The cost at each node is updated if a better path is found between the start configuration and that node.
Implementation of RRT* was essentially the same as RRT in finding the vertices and finding a solution. The difference is that in this algorithm we don’t break out and return once we arrive at a solution, but rather continue through K iterations until we find the optimal solution. I created a new class Tree_star which builds off of Tree, differing mainly in that it includes a map holding the cost of each vertex, which is the distance between the vertex and the start configuration.
To test and compare the algorithms I wrote a script to iterate through each algorithm 20 times and generate the metrics for arriving at a solution with said algorithm.
