The main task is to obtain experience with the implementation of the state-of-the-art asymptotically optimal randomized sampling-based motion planning algorithms.

Extend the previous work on Task05 - Randomized sampling-based algorithms to planning using the Open Motion Planning Library (OMPL). Use the Informed RRT* OMPL implementation to provide a collision free path through the environment represented by a geometrical map.

In file OMPLPlanner.py implement the Informed RRT* algorithm using the OMPL library.

The implementation requirements are similar to the ones of the Task05 - Randomized sampling-based algorithms.

Adjust the provided minimum example of the Open Motion Planning Library (OMPL) path planning and fuse it into your code from Task05 - Randomized sampling-based algorithms. Two adjustments has to be made in the code:

1. You need to provide the planner with a callback function that will be used for collision checking:
   task.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
   This function basically does only proper interfacing of the environment)
2. Don't forget that you need to appropriately set the sampling resolution for the collision checking based on the given constraints
   task.getSpaceInformation().setStateValidityCheckingResolution(resolution)
3. As a state space, you can use either RealVectorStateSpace() or SE3StateSpace(), think about the pros and cons of using either of the state spaces.
4. If you use SE3StateSpace(), note the robot configuration in OMPL is represented as follows. For the translation, $(x,y,z)$ coordinates are used. For the orientation, quaternion $(qx,qy,qz,qw)$ is used. Extracting Euler angles from quaternion is troublesome because it is necessary to fix the order of rotations. However, a rotation matrix can be easily obtained using the following code as well as the quaternion from rotation matrix to initilaize the setup

   def q2r(q):
       """
       method to convert quaternion into rotation matrix
    
       Parameters
       ----------
       q: 4*(float)
           quaternion in form (qx,qy,qz,qw)
    
       Returns
       -------
       np.array(3x3)
           R .. rotation matrix
       """
       q = q[[3, 0, 1, 2]]
    
       R = [[q[0]**2+q[1]**2-q[2]**2-q[3]**2, 2*(q[1]*q[2]-q[0]*q[3]), 2*(q[1]*q[3]+q[0]*q[2])],
            [2*(q[1]*q[2]+q[0]*q[3]), q[0]**2-q[1]**2+q[2]**2-q[3]**2, 2*(q[2]*q[3]-q[0]*q[1])],
            [2*(q[1]*q[3]-q[0]*q[2]), 2*(q[2]*q[3]+q[0]*q[1]), q[0]**2-q[1]**2-q[2]**2+q[3]**2]]
    
       return np.asarray(R)
    
   def r2q(R):
       """
       method to convert rotation matrix into quaternion
    
       Parameters
       ----------
       R: np.array(3,3)
           rotation matrix
    
       Returns
       -------
       np.array(4)
           quaternion in form (qx,qy,qz,qw)
       """
       q = np.array([R.trace() + 1, R[2,1]-R[1,2],R[0,2]-R[2,0],R[1,0]-R[0,1]])
       q = 1.0/(2.0*math.sqrt(R.trace() + 1.0000001))*q
       q = q[[1, 2, 3, 0]]
       #normalize the quaternion - |q| = 1
       q = q/np.linalg.norm(q)
    
       return q
    

   Notice, that you don't need the state space representation in any step of the planning, i.e., you don't have to extract the Euler angles at any point. The rotation matrix is sufficient representation in the given assignment.
5. In SE(3) state space the start and goal pose initialization is done using the following code which shows how to access the translational and rotational parts of the configuration:

       #setting start and goal positions
       start_pose = ob.State(task.getStateSpace())   
       goal_pose = ob.State(task.getStateSpace())
       start_pose().setX = start[0]
       start_pose().setY = start[1]
       start_pose().setZ = start[2]
    
       q = ... # construct the normalized quaternion
    
       start_pose().rotation().x = q[0]
       start_pose().rotation().y = q[1]
       start_pose().rotation().z = q[2]
       start_pose().rotation().w = q[3]
    
       # ... set the goal position

It is possible to install the OMPL from sources or using the minimum installation as described in the tutorial Open Motion Planning Library. If running of the task7_eval.py script does fail on segmentation fault or import error, you should copy the full content of the /usr/local/lib/python###/dist-packages/ompl directory into the local ompl directory of the samplingplanner. Using ldd command on the library file you can check if you have missing dependencies.

The simplified evaluation script for testing of the implementation is following

#!/usr/bin/env python2
# -*- coding: utf-8 -*-
 
import sys
import math
import time
import numpy as np
import matplotlib.pyplot as plt
from collections import deque
 
sys.path.append('environment')
sys.path.append('samplingplanner')
 
import Environment as env
import OMPLPlanner as op
 
 
if __name__ == "__main__":
    #define the planning scenarios
    #scenario name
    #start configuration
    #goal configuration
    #limits for individual DOFs
    scenarios = [("environments/simple_test", (2,2,0,0,0,0), (-2,-2,0,0,0,math.pi/2), [(-3,3), (-3,3), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/squares", (2,-3,0,0,0,0), (4,8,0,0,0,0), [(0,5), (-5,10), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/maze1", (2,2,0,0,0,math.pi/4), (35,35,0,0,0,math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/maze2", (2,2,0,0,0,math.pi/4), (35,26,0,0,0,3*math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/maze3", (2,2,0,0,0,0), (25,36,0,0,0,0), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/maze4", (2,2,0,0,0,0), (27,36,0,0,0,math.pi/2), [(0,40), (0,40), (0,0), (0,0), (0,0), (0,2*math.pi)]),
                 ("environments/cubes", (-1,-1,1,0,0,0), (6,6,-6,math.pi,0,math.pi/2), [(-2,7), (-2,7), (-7,2), (0,2*math.pi), (0,2*math.pi), (0,2*math.pi)]),
                 ("environments/cubes2", (2,2,2,0,0,0), (19,19,19,0,0,0), [(-10,30), (-10,30), (-10,30), (0,2*math.pi), (0,2*math.pi), (0,2*math.pi)]),
                 ("environments/alpha_puzzle", (0,5,0,0,0,0),(25,25,25,0,0,0), [(-40,70),(-40,70),(-40,70),(0,2*math.pi),(0,2*math.pi),(0,2*math.pi)])]
 
    #enable dynamic drawing in matplotlib
    plt.ion()
 
    ########################################
    ## EVALUATION OF THE OMPL PLANNER
    ########################################
    for scenario in scenarios:
        name = scenario[0]
        start = np.asarray(scenario[1])
        goal = np.asarray(scenario[2])
        limits = scenario[3]
 
        print("processing scenario: " + name)
 
        #initiate environment and robot meshes
        environment = env.Environment()
        environment.load_environment(name)
 
        #instantiate the planner
        planner = op.OMPLPlanner(limits)
 
        #plan the path through the environment
        path = planner.plan(environment, start, goal)
 
        #plot the path step by step
        ax = None
        for Pose in path:
            ax = environment.plot_environment(Pose, ax=ax, limits=limits)
            plt.pause(0.01)