Warning

This page is located in archive.
Go to the latest version of this course pages.
Go the latest version of this page.

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