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

|**Due date**  |  December 17, 2023, 23:59 PST |
|**Deadline**  |  January 13, 2024, 23:59 PST |
|**Points**  |  7 |
|**Label in BRUTE**  |  t3b-rrt |
|**Files to submit**  |  archive with the ''RRTPlanner.py'' file |
|**Resources**  | {{:courses:uir:hw:b4m36uir_23_t3_resource_pack.zip | B4M36UIR_t3_resource_pack}} |

----

====Installation====
  - Compile rapid library by ''make'' in ''environment/rapid'' library.

/*<note warning>This page is being prepared and not finished yet!</note>*/

====Assignment====

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


{{:courses:uir:hw:rrt.png?650|}}

{{:courses:uir:hw:rrtstar.png?650|}}

====Source code====

**Initialization** of ''RRTPlanner'' class by method ''__init__'' sets the environment by arguments:
  - ''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.
  - ''translate_speed'': Speed of translation motion [m/s].
  - ''rotate_speed''Angular 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: 
  - ''start'' - [[courses:uir:hw:support:messages|Pose message]] - the start pose of the robot.
  - ''space'' - Type of configuration space ("R2", "SE(2)", "R3", "SE(3)").
  - ''number_of_samples'' - Number of samples to be generated.
  - ''neighborhood_radius'' - Neighborhood radius to connect samples [s].
  - ''collision_step'' - Minimum step to check collisions [s].
  - ''isrrtstar'' - Determine variant of the algorithm (''False'' -> RRT, ''True'' -> RRT*).
  - ''steer_step'' - Step utilized of steering function [s].
  
Returns:
  - the [[courses:uir:hw:support:messages|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: 
  - ''goal'' - [[courses:uir:hw:support:messages|Pose message]] - the desired goal pose of the robot.
  - ''neighborhood_radius'' - Neighborhood radius to connect samples [s].
  - ''collision_step'' - Minimum step to check collisions [s].
  - ''isrrtstar'' - Determine variant of the algorithm (''False'' -> RRT, ''True'' -> RRT*).

Returns:
  - the [[courses:uir:hw:support:messages|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====
  - You can use provided ''Steer'' function.
  - Use tree structure for RRT/RRT* instead of a general graph.
  - There are two ways how to store cost in tree nodes:
    * Store the total cost. 
    * Store cost of the last connection.
  - Use ''neighborhood_radius'' in ''Near'' function.

/*
An example of the running sPRM can be seen in the following images. From left to right: Environment with robot and obstacles. Constructed navigation graph. Final path and its execution.
*/

====Example output====

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

{{:courses:uir:hw:simple_test_1_reference.png?400|}}
{{:courses:uir:hw:simple_test_2_reference.png?400|}}

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

{{:courses:uir:hw:simple_test_3_reference.png?400|}}
{{:courses:uir:hw:simple_test_4_reference.png?400|}}

====References====

[1] Karaman, Sertac, and Emilio Frazzoli. "Sampling-based algorithms for optimal motion planning." The international journal of robotics research 30.7 (2011): 846-894. [[https://journals.sagepub.com/doi/abs/10.1177/0278364911406761|Journal version]] [[https://arxiv.org/abs/1105.1186|Arxiv version]]

[2] [[https://demonstrations.wolfram.com/RapidlyExploringRandomTreeRRTAndRRT | Demonstration in Wolfram]]

[3] [[https://www.youtube.com/watch?v=YKiQTJpPFkA| RRT* algorithm illustrative example (youtube)]]