Back to Home

Manipulator RRT Motion Planner

View Code

Overview

An implementation of the Rapidly-Exploring Random Tree algorithm, written for the 6DOF-UR5 manipulator. The core functionality of the algorithm, such as reconstructing a path from a network of connected nodes, was written from scratch.

Implementation Details

  1. Initialize two trees: One whose root at the starting configuration (q_init), and one whose root is the goal configuration (q_goal). For the rest of this description, we will refer to these trees as startTree and goalTree, respectively.
  2. We start by growing startTree. Sample a random collision-free point (q_rand) in the configuration space. But, there is a goal bias. There is a 5% chance that we just make the goal configuration our sampled point. This occasionally gives the tree some direction towards the goal, preventing it from growing way off course.
  3. Find the nearest node (q_near) in startTree to this point, using Euclidean Metric in the dimension of the configuration space. (For example, if each configuration has 6 elements, which is standard for a 6-DOF robot, we use the squared distance between the 6-dimensional configs).
  4. Sample a random collision-free point (q_rand) in the configuration space, then find the nearest node (q_near) in startTree to this point. (Euclidean Metric, or simply squared distance between the 6-dimensional configs).
  5. Generate a new configuration (q_new), which is on the straight-line path between q_rand and q_near by some proportion. For example, suppose the distance between q_rand and q_near is L, the distance between q_new and q_near could be 0.3*.
  6. If there is a collision-free path from q_near to q_new, add q_new to startTree.
  7. Then, we check if there is a collision-free path from q_new to any node in goalTree. If there is, we call this node otherNode. Then, we find a path from q_init to q_new, and the path from otherNode to q_goal. Then we can combine the paths, to get a succesful solution.
  8. If step 6 or step 7 fails (Either there is no collision-free path from q_near to q_new, or there is no collision-free path from q_new to any node in goalTree), we repeat from step 2. But this time, we are growing goalTree. So essentially in ever other iteration, q_init is treated as the goal, and q_goal is treated as the start. This allows for a faster convergence to a solution.
  9. These repeat until a valid solution is found, or the maximum allowable iterations is reached.

Planning Demonstration