T3b-rrt - Randomized sampling-based algorithms - Rapidly-exploring Random Trees (RRT)

The main task is to learn the basic principles of randomized sampling-based planning. The principles are demonstrated on the multi-query planning algorithm - Rapidly-exploring Random Trees (RRT).

Deadline 20. December 2020, 23:59 PST
Points 7
Label in BRUTE t3b-rrt
Files to submit archive with the RRTPlanner.py file
Resources T3b-rrt resource package


  1. Compile rapid library by make in environment/rapid library.


In class RRTPlanner.py implement RRT/RRT* algorithm according to the description and pseudo-code from [1].

Source code

Initialization of RRTPlanner class by method init sets the environment by arguments:

  1. environment - Environment - geometric representation of the environment and robot together with the wrapper for RAPID collision checking library,
    • represents robot and environment,
    • check for collision between the robot and the environment using function environment.check_robot_collision(P) where P is Pose message. Returns True if the Robot at pose P collides with the obstacle,
    • contains environment dimensions in environment.limits_x, environment.limits_y, environment.limits_z float constants.
  2. translate_speed: Speed of translation motion [m/s].
  3. rotate_speedAngular speed of rotation motion [rad/s].

The RRT/RRT* algorithm itself is divided into two methods expand_tree and query. Thus, we implement multi-query variant of the RRT/RRT* algorithm.

First method expand_tree is called to build the tree and return it as a NavGraph structure. The input parameters are:

  1. start - Pose message - the start pose of the robot.
  2. space - Type of configuration space (“R2”, “SE(2)”, “R3”, “SE(3)”).
  3. number_of_samples - Number of samples to be generated.
  4. neighborhood_radius - Neighborhood radius to connect samples [s].
  5. collision_step - Minimum step to check collisions [s].
  6. isrrtstar - Determine variant of the algorithm (False → RRT, True → RRT*).
  7. steer_step - Step utilized of steering function [s].


  1. the NavGraph message representing the underlying navigation graph constructed by the method from which the resulting path is reconstructed.

The second method query retrieves a path from the expanded tree. The input parameters of the function are:

  1. goal - Pose message - the desired goal pose of the robot.
  2. neighborhood_radius - Neighborhood radius to connect samples [s].
  3. collision_step - Minimum step to check collisions [s].
  4. isrrtstar - Determine variant of the algorithm (False → RRT, True → RRT*).


  1. the Path message with the found path or None when the path cannot be found. The individual poses of the path shall not be further than collision_step.

Implementation notes

  1. You can use provided Steer function.
  2. Use tree structure for RRT/RRT* instead of a general graph.
  3. There are two ways how to store cost in tree nodes:
    • Store the total cost.
    • Store cost of the last connection.
  4. Use neighborhood_radius in Near function.

Example output

RRT algorithm in “R2” space with 300 (left) and 1000 (right) samples.

RRT* algorithm in “R2” space with 300 (left) and 1000 (right) samples.


[1] Karaman, Sertac, and Emilio Frazzoli. “Sampling-based algorithms for optimal motion planning.” The international journal of robotics research 30.7 (2011): 846-894. Journal version Arxiv version

[2] Demonstration in Wolfram

[3] RRT* algorithm illustrative example (youtube)

courses/b4m36uir/hw/t3b-rrt.txt · Last modified: 2020/11/30 23:34 by vanapet1