Warning

Lab07 - Asymptotically Optimal Randomized Sampling-based Path Planning

Motivations and Goals
Become familiar with advanced methods of randomized sampling-based motion planning
Be able to plan the path using the framework of the Open Motion Planning Library
Implement the RRT-based planning approach using the OMPL framework
Lab resources
OMPL example instance

Open Motion Planning Library

The Open Motion Planning Library contains implementations of many sampling-based algorithms such as PRM, RRT, EST, SBL, KPIECE, SyCLOP, and several variants of these. Details on the installation of the OMPL library are at Open Motion Planning Library tutorial. Below is a simple annotated example of motion planning in continuous space:

#!/usr/bin/env python3

import numpy as np
import matplotlib.pyplot as plt
from ompl import base as ob
from ompl import geometric as og

def plot_points(points, specs='r'):
"""
method to plot the points

Parameters
----------
points: list(float, float)
list of the pint coordinates
"""
x_val = [x for x in points]
y_val = [x for x in points]
plt.plot(x_val, y_val, specs)
plt.draw()

def isStateValid(state):
"""
method for collision detection checking

Parameters
----------
state : list(float)
configuration to be checked for collision

Returns
-------
bool
False if there is collision, True otherwise
"""
return (state >= state)

def plan(start, goal):
#set dimensions of the problem to be solved
stateSpace = ob.RealVectorStateSpace()  #set state space
stateSpace.addDimension(0.0, 10.0) #set width
stateSpace.addDimension(0.0, 10.0) #set height

#create a simple setup object

#set methods for collision detection and resolution of the checking

#setting start and goal positions
start_pose() = start
start_pose() = start
goal_pose() = goal
goal_pose() = goal

#setting particular planner from the supported planners
planner = og.RRT(info)   # RRT, RRTConnect, FMT, ...

#find the solution

#simplify the solution
if solution:

#retrieve found path

#extract path and plot it
path = []
for i in range(plan.getStateCount()):
path.append((plan.getState(i), plan.getState(i)))

plot_points(path,'r')
plt.show()

if __name__ == "__main__":
plan([0, 0], [10, 10])

Besides the setup of the planner, the programmer needs to deliver the validity checking function. In the problem of robotic motion planning this function usually consists of collision checking which can be based on different approaches.

Further information of the planner can be obtained using access through PlannerData object as demonstrated below.

#retrieve planner data
plannerdata = ob.PlannerData(info)
planner.getPlannerData(plannerdata)

#getting vertices
nv = plannerdata.numVertices()
g = []
for i in range(0, nv):
vert = plannerdata.getVertex(i)
g.append((vert.getState(), vert.getState()))

#getting edges
e = []
for i in range(0, nv):
for j in range(0, nv):
if plannerdata.edgeExists(i, j):
e.append((g[i], g[j]))
#show edges
plot_edges(e)
plt.show()

def plot_edges(edges):
"""
method to plot the edges of the randomized sampling-based path planning approach graph

Parameters
----------
edges: list((float,float),(float,float))
list of edges (coordinate1, coordinate2)
"""
ed = np.asarray(edges)
for edge in ed:
plt.plot([edge[0,0], edge[1,0]],[edge[0,1], edge[1,1]],'g',linewidth=0.6)
plt.draw()

Motion planning benchmarking

A set of motion planning benchmarking examples available online.

courses/b4m36uir/labs/lab07.txt · Last modified: 2018/11/19 15:21 by cizekpe6 