The main task is to learn the basic principles of randomized sampling-based planning. The principles are demonstrated on the multi-query planning algorithm - the Simplified Probabilistic Roadmap (sPRM).

1. Compile rapid library by make in environment/rapid library.
2. Install Dijkstar package to python3.

pip3 install Dijkstar # or our favorite way to install the package

In class PRMPlanner.py implement the Simplified Probabilistic Roadmap (sPRM) randomized sampling-based path planning algorithm according to the description and pseudocode presented in the Lecture 08.

The planner is invoked using the method plan and shall provide a collision free path through an environment represented by a geometrical map. The input parameters of the function plan are:

1. environment - Environment - geometric representation of the environment and robot together with the wrapper for RAPID collision checking library
   • represents robot and environment
   • check for collision between the robot and the environment using function environment.check_robot_collision(P) where P is Pose message. Returns True if the Robot at pose P collides with the obstacle.
   • contains environment dimensions in environment.limits_x, environment.limits_y, environment.limits_z float constants
2. start - Pose message - the start pose of the robot.
3. goal - Pose message - the desired goal pose of the robot.
4. space - Type of configuration space (“R2”, “SE(2)”, “R3”, “SE(3)”).
5. number_of_samples - Number of samples to be generated.
6. neighborhood_radius - Neighborhood radius to connect samples [s].
7. collision_step - Minimum step to check collisions [s].

The function returns:

1. the Path message with the found path or None when the path cannot be found. The individual poses of the path shall not be further than collision_step.
2. the NavGraph message representing the underlying navigation graph constructed by the method from which the resulting path is reconstructed.

For easier implementation, several functions are already implemented in the PRMPlanner class. You may utilize them in your solution. The detailed description of the recommended approach is in the next section.

After downloading the resource pack, please build the RAPID collision checking library in the environment/rapid directory using the provided Makefile. The Environment class provides the wrapper for the RAPID library which is used for checking the sampled robot poses for collision with the obstacles in the environment.

An example of the running sPRM can be seen in the following images. From left to right: Environment with robot and obstacles. Constructed navigation graph. Final path and its execution.

The sPRM algorithm is demonstrated in the continuous geometric space, which is a different representation of the environment in comparison to the OccupancyGrid message, i.e., the probabilistic occupancy grid map, used in the previous tasks. In the geometric space we need a way to evaluate the robot interference with the environment using collision checking. There are multiple collision checking libraries available, (e.g. , Coldet, BULLET, Nvidia PhysX, FCL, RAPID , etc. They all differ in the performance, collision checking approach and precision. ). In UIR we are using the RAPID collision checking library which is sufficiently precise, fast and easy to install. The provided Environment class within the resource pack contains the representation of the planning scenarios and wrapper for the RAPID library.

Gimbal lock

Slerp - spherical linear interpolation