===== 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** | {{ :courses:b4m36uir:hw:task07.zip |Task07 resource files}}|
| | The resource package has been updated on 26. November 2018. The new version contains OMPL bindings directly.|
| | {{ :courses:b4m36uir:hw:environments.zip | 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 [[courses:b4m36uir:hw:task05|Task05 - Randomized sampling-based algorithms]] to planning using the [[courses:b4m36uir:labs:lab07|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 [[courses:b4m36uir:hw:task05|Task05 - Randomized sampling-based algorithms]].
=== Approach ===
Adjust the provided minimum example of the [[courses:b4m36uir:labs:lab07|Open Motion Planning Library (OMPL)]] path planning and fuse it into your code from [[courses:b4m36uir:hw:task05|Task05 - Randomized sampling-based algorithms]].
Two adjustments has to be made in the code:
- 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'')
- 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)''
- As a state space, you can use either RealVectorStateSpace() or SE3StateSpace(), think about the pros and cons of using either of the state spaces.
- 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.
- 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
=== OMPL troubleshooting ===
It is possible to install the OMPL from sources or using the minimum installation as described in the tutorial [[courses:b4m36uir:tutorials:ompl_tut|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()
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)