The main task is to obtain experience with the implementation of the stateoftheart asymptotically optimal randomized samplingbased 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 
Extend the previous work on Task05  Randomized samplingbased 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 samplingbased algorithms.
Adjust the provided minimum example of the Open Motion Planning Library (OMPL) path planning and fuse it into your code from Task05  Randomized samplingbased algorithms. Two adjustments has to be made in the code:
task.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
environment
)
task.getSpaceInformation().setStateValidityCheckingResolution(resolution)
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]**2q[2]**2q[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]**2q[1]**2+q[2]**2q[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]**2q[1]**2q[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 qNotice, 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.
#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###/distpackages/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: utf8 * 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)