===== T3a-sampl - Randomized sampling-based algorithms - Probabilistic Roadmap ======
The main task is to implement the Probabilistic Roadmap (PRM).

|**Deadline**  |  7. December 2019, 23:59 PST |
|**Points**  |  3 |
|**Label in BRUTE**  |  t3a-sampl |
|**Files to submit**  |  archive with the ''PRMPlanner.py'' file |
|**Resources**  |  {{ :courses:b4m36uir:hw:uir-t3a-sampl.zip | T3a-sampl resource package}}|

\\

===Assignment===
In ''PRMPlanner.py'' implement the Probabilistic Roadmap (PRM) 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 algorithms shall provide a collision free path through an environment represented by a geometrical map. 

The ''PRMPlanner.plan'' function has the following prescription
<code python>
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)
</code>

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'', 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.

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.

\\

=== 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''