Warning

## Task07 - Asymptotically optimal randomized sampling-based path planning

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

 Deadline 01. December 2018, 23:59 PST Points 4 Label in BRUTE Task07 Files to submit archive with samplingplanner directory Minimal content of the archive: samplingplanner/OMPLPlanner.py Resources Task07 resource files The resource package has been updated on 26. November 2018. The new version contains OMPL bindings directly. Additional environments to test the planner
The assignment and deadline for this task are postponed for one week to Lab08 and 01.12. respectively

#### Assignment

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.

#### Approach

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().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

#### OMPL troubleshooting

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.

Run the ompl examples in python2

#### Evaluation

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()

#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)