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).

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].

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].

Returns:

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*).

Returns:

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.

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.

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)