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

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.

`pip install dubins`

**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`

courses/b4m36uir/hw/t3b-rrt.txt · Last modified: 2019/12/13 15:14 by pragrmi1