## 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 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 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

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)

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

pip install dubins

#### RAPID collision checking library installation notes

On Linux (tested with Ubuntu 14.04, 16.04, 18.04)

1. Download the resource package and make the rapid library in environment/rapid directory

On MacOS

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