Search
The main task is to implement the Probabilistic Roadmap (PRM).
PRMPlanner.py
In PRMPlanner.py implement the Probabilistic Roadmap (PRM) randomized sampling-based path planning algorithm according to the description and pseudocode presented in the Lecture 07. The algorithms shall provide a collision free path through an environment represented by a geometrical map.
The PRMPlanner.plan function has the following prescription
PRMPlanner.plan
import SamplingPlanner as sp ... class PRMPlanner(sp.SamplingPlanner): ... def plan(self, environment, start, goal, n_samples = 300): """ 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 n_samples: int number of C_free samples Returns ------- list(numpy array (4x4)) the path between the start and the goal Pose in SE(3) coordinates """ #TODO: t3a-sampl - implement the PRM planner path = [] 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 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.
SamplingPlanner.limits
list( (lower_bound, upper_bound) )
sp.te_2_se3
sp.se3_2_te
The individual poses shall not be further than SamplingPlanner.max_norm_translation, and two consecutive path points' orientations shall not change more than SamplingPlanner.max_norm_rotation. The norms of translation and rotation may be computed using sp.norm_translation and sp.norm_rotation_R, respectively. Note, the configuration space sampling is not affected by this requirement. Individual random samples may be arbitrarily far away; however, their connection shall adhere to the given constraint on the maximum distance and rotation to ensure sufficient sampling of the configuration space and smooth motion of the robot.
SamplingPlanner.max_norm_translation
SamplingPlanner.max_norm_rotation
sp.norm_translation
sp.norm_rotation_R
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 function 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 distances.
environment.check_robot_collision
True
False
SamplingPlanner.is_valid_edge
start
end
On Linux (tested with Ubuntu 14.04, 16.04, 18.04)
make
environment/rapid
On MacOS
environment/rapid/Makefile
TARGET=librapid.so
TARGET=librapid.dylib
-soname
-install_name