====== Lab07 - Randomized Sampling-based Motion Planning contd.======

^ 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 [[courses:b4m36uir:tutorials:ompl_tut|Open Motion Planning Library]] |
^ Tasks ([[courses:b4m36uir:internal:instructions:lab07|teacher]]) ^
| Implement the RRT-based planning approach using the OMPL framework |
^ Lab resources ^
| Lab scripts: {{:courses:b4m36uir:labs:simple_ompl.zip| OMPL example script}} |

===== Open Motion Planning Library =====
The [[http://ompl.kavrakilab.org/index.html|Open Motion Planning Library]] contains implementations of many [[http://ompl.kavrakilab.org/planners.html|sampling-based algorithms]] such as PRM, RRT, EST, SBL, KPIECE, SyCLOP, and several variants of these.
Below is a simple annotated example of motion planning in continuous space:
<code python>
import matplotlib.pyplot as plt

from ompl import base as ob
from ompl import geometric as og

def plot_points(points, specs):
	x_val = [x[0] for x in points]
	y_val = [x[1] for x in points]
	plt.plot(x_val, y_val, specs)
	plt.draw()

def isStateValid(state):
	#method for collision detection checking
	return (state[0] >= state[1])

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
	task = og.SimpleSetup(stateSpace)
	
	#set methods for collision detection and resolution of the checking 
	task.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
	task.getSpaceInformation().setStateValidityCheckingResolution(0.001)
	
	#setting start and goal positions
	start_pose = ob.State(task.getStateSpace())   
	goal_pose = ob.State(task.getStateSpace())
	start_pose()[0] = start[0]
	start_pose()[1] = start[1]
	goal_pose()[0] = goal[0]
	goal_pose()[1] = goal[1]
	task.setStartAndGoalStates(start_pose, goal_pose)
	
	#setting particular planner from the supported planners
	info = task.getSpaceInformation()
	planner = og.RRT(info)   # RRT, RRTConnect, FMT, ...
	task.setPlanner(planner)
	
	#find the solution
	solution = task.solve()
	
	#simplify the solution
	if solution:
		task.simplifySolution()
	
	#retrieve found path
	plan = task.getSolutionPath()
	
	#extract path and plot it
	path = []
	for i in range(plan.getStateCount()):
		path.append((plan.getState(i)[0], plan.getState(i)[1]))
	
	plot_points(path,'r')
	plt.show()


if __name__ == "__main__":
	plan([0, 0], [10, 10])
</code>

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, e.g., (( [[https://github.com/jslee02/awesome-collision-detection]] )) (( [[http://gamma.cs.unc.edu/OBB/]] )).\\

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

	#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()[0], vert.getState()[1]))
	
	#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,'g')
	plt.show()
</code>


===== Motion planning benchmarking =====
A set of [[https://parasol.tamu.edu/dsmft/benchmarks/|motion planning benchmarking examples]] available online.