===== T3b-rrt - Curvature-constrained local planning in RRT ======
The main task is to implement the Rapidly Exploring Random Trees (RRT).

|**Deadline**  |  7. December 2019, 23:59 PST |
|**Points**  |  3 |
|**Label in BRUTE**  |  t3b-rrt |
|**Files to submit**  |  archive with the ''RRTDubinsPlanner.py'' file |
|**Resources**  |  {{ :courses:b4m36uir:hw:uir-t3b-rrt.zip | T3b-rrt resource package (updated 27.11.2019)}}|

\\

===Assignment===
In ''RRTDubinsPlanner.py'' implement the Rapidly Exploring Random Tree (RRT) randomized sampling-based path planning algorithm according to the description and pseudocode presented in the {{courses:b4m36uir:lectures:b4m36uir-lec07-slides.pdf| Lecture 07}}.
The algorithm should provide a collision free curvature constrained path through an environment represented by a geometrical map. 

The ''RRTDubinslanner.plan'' function has the following prescription
<code python>
import SamplingPlanner as sp

...

class RRTPlanner(sp.SamplingPlanner):
    
    ...

    def plan(self, environment, start, goal):
        """
        Method to plan the path

        Parameters
        ----------
        environment: Environment
            Map of the environment that provides collision checking 
        start: numpy array (4x4)
            start configuration of the robot in SE(3) coordinates
        goal: numpy array (4x4)
            goal configuration of the robot in SE(3) coordinates

        Returns
        -------
        list(numpy array (4x4))
            the path between the start and the goal Pose in SE(3) coordinates
        """

        #TODO: t3b-rrt - implement the curvature constrained RRT planner

        print (start)
        print (goal)

        assert not environment.check_robot_collision(start), 'start collision'
        assert not environment.check_robot_collision(goal), 'goal collision'

        path = [start,goal]

         
        return(path)
</code>

The path is contrained by the minimal turning radius ''RRTPlanner.turning_radius''. 
The ''dubins'' package is to be used to compute the curvature constrained paths.

Although the curvature constrained path is effectively $SE(2)$, i.e., three degrees of freedom, $SE(3)$ is used to represent the individual poses.
The pose $\mathbf{P} \in SE(3)$ is given as $$
\mathbf{P} = \begin{bmatrix}
\mathbf{R} & \mathbf{T}\\
[0, 0, 0] & 1
\end{bmatrix}, 
$$ where $\mathbf{R} \in \mathcal{R}^{3\times3}$ is the rotation matrix for which $\mathbf{R}\cdot\mathbf{R} = \mathbf{I}$ and $\det(\mathbf{R})=1$. $\mathbf{T} \in \mathcal{R}^3$ is the translation vector 
Therefore, the individual poses are the rigid body transformations in the global reference frame. Hence, the position of the robot $r$ is given as the transformation of the robot base pose $\mathbf{r}_b$ in homogeneous coordinates, given as:$$ \begin{bmatrix}
\mathbf{r}\\
1 \end{bmatrix} = \mathbf{P}\cdot\begin{bmatrix}
\mathbf{r}_b\\
1\end{bmatrix},
$$ which can be also written as$$
 \mathbf{r} = \mathbf{R}\cdot\mathbf{r}_b + \mathbf{T}.
$$

The boundaries for individual configuration variables are given during the initialization of the planner in ''SamplingPlanner.limits'' variable as a list of lower-bound and upper-bound limit tuples, i.e. ''list( (lower_bound, upper_bound) )'', for each of the variables $(x,y,z,\phi_x,\phi_y,\phi_z)$.
The $(x,y,z,\phi_x,\phi_y,\phi_z)$ vector can be transformed to and from $SE(3)$ using the ''sp.te_2_se3'' and ''sp.se3_2_te'' functions, respectively.

The individual poses shall not be further than ''SamplingPlanner.max_norm_translation'', which distance should be computed along the curvature constrained path.

The collision checking is performed using the ''environment.check_robot_collision'' function that takes on the input an $SE(3)$ pose matrix. The collision checking function returns ''True'' if there is collision between the robot and the environment and ''False'' if there is no collision.
Moreover, the ''SamplingPlanner.is_valid_edge'' can be used to check whether the connection from $SE(3)$ represented point ''start'' to $SE(3)$ represented point ''end'' is valid both in terms of the collision checker and maximum distance.

\\

=== Dubins library installation ===

<code bash>
pip install dubins
</code>

\\

=== RAPID collision checking library installation notes ===
**On Linux (tested with Ubuntu 14.04, 16.04, 18.04)**
  - Download the resource package and ''make'' the rapid library in ''environment/rapid'' directory

**On MacOS**
  - On line 7 of ''environment/rapid/Makefile'' change ''TARGET=librapid.so'' to ''TARGET=librapid.dylib''
  - On line 11 of ''environment/rapid/Makefile'' change ''-soname'' to ''-install_name''