def setSpace_3d(self):
# construct the state space we are planning in
self.space = ob.SE3StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(3)
if not self.costMap:
self.bounds.setLow(0)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
oz = self.costMap.getOriginZ()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
size_z = self.costMap.getSizeInMetersZ()
print('o', ox, oy, oz)
print('size', size_x, size_y, size_z)
low = min(ox, oy, oz)
high = max(ox + size_x, oy + size_y, oz + size_z)
print("low", low)
print("high", high)
self.bounds.setLow(0, ox)
self.bounds.setLow(1, oy)
self.bounds.setLow(2, oz)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setHigh(1, oy + size_y)
self.bounds.setHigh(2, oz + size_z)
self.space.setBounds(self.bounds)
# define a simple setup class
self.ss = og.SimpleSetup(self.space)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
def plan():
# construct the state space we are planning in
space = ob.SE3StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-10)
bounds.setHigh(10)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(-9)
start().setY(-9)
start().setZ(-9)
start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-9)
goal().setY(9)
goal().setZ(-9)
goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.PRM(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool:
useGraphTool(pd)
def setup(problem):
OMPL_INFORM("Robot type is: %s" % str(problem["robot.type"]))
ompl_setup = eval("oa.%s()" % problem["robot.type"])
problem["is3D"] = isinstance(ompl_setup.getGeometricComponentStateSpace(), ob.SE3StateSpace)
if str(ompl_setup.getAppType()) == "GEOMETRIC":
problem["isGeometric"] = True
else:
problem["isGeometric"] = False
ompl_setup.setEnvironmentMesh(str(problem['env_loc']))
ompl_setup.setRobotMesh(str(problem['robot_loc']))
if problem["is3D"]:
# Set the dimensions of the bounding box
bounds = ob.RealVectorBounds(3)
bounds.low[0] = float(problem['volume.min.x'])
bounds.low[1] = float(problem['volume.min.y'])
bounds.low[2] = float(problem['volume.min.z'])
bounds.high[0] = float(problem['volume.max.x'])
bounds.high[1] = float(problem['volume.max.y'])
bounds.high[2] = float(problem['volume.max.z'])
ompl_setup.getGeometricComponentStateSpace().setBounds(bounds)
space = ob.SE3StateSpace()
# Set the start state
start = ob.State(space)
start().setXYZ(float(problem['start.x']), float(problem['start.y']), \
float(problem['start.z']))
# Set the start rotation
start().rotation().x = float(problem['start.q.x'])
start().rotation().y = float(problem['start.q.y'])
start().rotation().z = float(problem['start.q.z'])
start().rotation().w = float(problem['start.q.w'])
# Set the goal state
goal = ob.State(space)
goal().setXYZ(float(problem['goal.x']), float(problem['goal.y']), float(problem['goal.z']))
# Set the goal rotation
goal().rotation().x = float(problem['goal.q.x'])
goal().rotation().y = float(problem['goal.q.y'])
goal().rotation().z = float(problem['goal.q.z'])
goal().rotation().w = float(problem['goal.q.w'])
start = ompl_setup.getFullStateFromGeometricComponent(start)
goal = ompl_setup.getFullStateFromGeometricComponent(goal)
ompl_setup.setStartAndGoalStates(start, goal)
else:
# Set the dimensions of the bounding box
bounds = ob.RealVectorBounds(2)
bounds.low[0] = float(problem['volume.min.x'])
bounds.low[1] = float(problem['volume.min.y'])
bounds.high[0] = float(problem['volume.max.x'])
bounds.high[1] = float(problem['volume.max.y'])
ompl_setup.getGeometricComponentStateSpace().setBounds(bounds)
space = ob.SE2StateSpace()
start = ob.State(space)
start().setX(float(problem['start.x']))
start().setY(float(problem['start.y']))
start().setYaw(float(problem['start.yaw']))
goal = ob.State(space)
goal().setX(float(problem['goal.x']))
goal().setY(float(problem['goal.y']))
goal().setYaw(float(problem['goal.yaw']))
start = ompl_setup.getFullStateFromGeometricComponent(start)
goal = ompl_setup.getFullStateFromGeometricComponent(goal)
ompl_setup.setStartAndGoalStates(start, goal)
return ompl_setup
def get_trajectory(constraints,
start_state,
goal_state,
start_rotation=[0, 0, 0, 1],
goal_rotation=[0, 0, 0, 1],
min_bounds=-5,
max_bounds=5,
t=1.0):
"""
Takes in a list of constraints, a start, a goal, bounds, and a maximum computation time, and returns a list of points that form a trajectory to the goal, or None if no such trajectory is found.
"""
# Return a function that represents spatial constraints
fn = valid_fn(constraints)
space = ob.SE3StateSpace()
bounds = ob.RealVectorBounds(3)
# Set limits for the x, y, z axes
bounds.setLow(min_bounds)
bounds.setHigh(max_bounds)
space.setBounds(bounds)
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(fn))
# Create the start state
start = ob.State(space)
start.random()
start[0] = start_state[0]
start[1] = start_state[1]
start[2] = start_state[2]
start[3] = start_rotation[0]
start[4] = start_rotation[1]
start[5] = start_rotation[2]
start[6] = start_rotation[3]
# Create the goal state
goal = ob.State(space)
goal.random()
goal[0] = goal_state[0]
goal[1] = goal_state[1]
goal[2] = goal_state[2]
goal[3] = goal_rotation[0]
goal[4] = goal_rotation[1]
goal[5] = goal_rotation[2]
goal[6] = goal_rotation[3]
# Compute a trajectory using the default parameters
ss.setStartAndGoalStates(start, goal)
solved = ss.solve(t)
if solved:
ss.simplifySolution()
path = ss.getSolutionPath()
length = path.getStateCount()
lst = []
print "Solution found with length ", length
for i in range(length):
state = path.getState(i)
rotation = path.getState(i).rotation()
lst.append([
state.getX(),
state.getY(),
state.getZ(), rotation.x, rotation.y, rotation.z, rotation.w
])
return np.matrix(lst)
print "No solution found"
return None
def plan(self,goalPoints,current_region,next_region,samplerIndex):
"""
goal points: array that contains the coordinates of all the possible goal states
current_reg: name of the current region (p1 etc)
next_reg : name of the next region (p1 etc)
"""
# construct the state space we are planning in
if self.Space_Dimension == 2:
space = ob.SE2StateSpace()
else:
space = ob.SE3StateSpace()
# set the bounds
bounds = ob.RealVectorBounds(self.Space_Dimension)
BoundaryMaxMin = self.all.boundingBox() #0-3:xmin, xmax, ymin and ymax
bounds.setLow(0,BoundaryMaxMin[0]) # 0 stands for x axis
bounds.setHigh(0,BoundaryMaxMin[1])
bounds.setLow(1,BoundaryMaxMin[2]) # 1 stands for y axis
bounds.setHigh(1,BoundaryMaxMin[3])
if self.Space_Dimension == 3:
bounds.setLow(2,0) # 2 stands for z axis
bounds.setHigh(2,self.maxHeight)
space.setBounds(bounds)
if self.system_print == True:
print "The bounding box of the boundary is: " + str(self.all.boundingBox() )
print "The volume of the bounding box is: " + str(bounds.getVolume())
if self.Geometric_Control == 'C':
# create a control space
cspace = oc.RealVectorControlSpace(space, self.Space_Dimension)
# set the bounds for the control space
# cbounds[0] for velocity
# cbounds[1] for angular velocity
cbounds = ob.RealVectorBounds(self.Space_Dimension)
cbounds.setLow(0,0)
cbounds.setHigh(0,max((BoundaryMaxMin[1]-BoundaryMaxMin[0]),(BoundaryMaxMin[3]-BoundaryMaxMin[2]))/100)
cbounds.setLow(1,-pi/5)
cbounds.setHigh(1,pi/5)
cspace.setBounds(cbounds)
if self.system_print == True:
print cspace.settings()
if self.Geometric_Control == 'G':
# define a simple setup class
ss = og.SimpleSetup(space)
else:
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# set state validity checking for this space
ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
# create a start state
start = ob.State(space)
pose = self.pose_handler.getPose() #x,y,w,(z if using ROS quadrotor)
start().setX(pose[0])
start().setY(pose[1])
if self.Space_Dimension == 2:
while pose[2] > pi or pose[2] < -pi:
if pose[2]> pi:
pose[2] = pose[2] - pi
else:
pose[2] = pose[2] + pi
start().setYaw(pose[2]) #
else:
start().setZ(pose[3])
start().rotation().setIdentity()
if self.system_print is True and self.Space_Dimension == 3:
print "start:" + str(start().getX())+","+str(start().getY()) +"," + str(start().getZ())
print goalPoints
# create goal states
goalStates = ob.GoalStates(ss.getSpaceInformation())
for i in range(shape(goalPoints)[1]):
goal = ob.State(space)
goal().setX(goalPoints[0,i])
goal().setY(goalPoints[1,i])
if self.Space_Dimension == 3:
if self.system_print is True:
print current_region,next_region
print self.map['height'][current_region],self.map['height'][next_region]
z_goalPoint = min(self.map['height'][current_region]/2,self.map['height'][next_region]/2)
goal().setZ(z_goalPoint)
goal().rotation().setIdentity()
else:
goal().setYaw(0.0)
if self.plotting == True:
if self.Space_Dimension == 3:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],[z_goalPoint],'ro')
else:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],'ro')
self.setPlotLimitXYZ()
self.ax.get_figure().canvas.draw()
goalStates.addState(goal)
if self.system_print is True:
print goalStates
# set the start and goal states;
ss.setGoal(goalStates)
ss.setStartState(start)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
# set planner
planner_prep = self.planner_dictionary[self.Geometric_Control][self.planner]
planner = planner_prep(si)
ss.setPlanner(planner)
if self.Geometric_Control == 'G':
if not self.planner == 'PRM':
planner.setRange(self.radius*2)
if self.system_print is True:
print "planner.getRange():" + str(planner.getRange())
#if not self.planner == 'RRTConnect':
# planner.setGoalBias(0.5)
else:
# (optionally) set propagation step size
si.setPropagationStepSize(1) #actually is the duration in propagate
si.setMinMaxControlDuration(3,3) # is the no of steps taken with the same velocity and omega
if self.system_print is True:
print "radius: " +str(self.radius)
print "si.getPropagationStepSize():" + str(si.getPropagationStepSize())
planner.setGoalBias(0.5)
ss.setup()
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(1000.0) #10
if (solved):
print("Found solution:")
# print the path to screen
print >>sys.__stdout__,(ss.getSolutionPath())
else:
print("No solution found")
if self.plotting == True :
self.ax.set_title('Map with Geo/Control: '+str(self.Geometric_Control) + ",Planner:" +str(self.planner), fontsize=12)
self.ax.set_xlabel('x')
self.ax.set_ylabel('y')
for i in range(ss.getSolutionPath().getStateCount()-1):
if self.Space_Dimension == 3:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),((ss.getSolutionPath().getState(i)).getZ(),(ss.getSolutionPath().getState(i+1)).getZ()),'b')
ro=Polygon.Shapes.Circle (self.radius,((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i)).getY()))
self.plotPoly(ro,'r')
else:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),0,'b')
self.setPlotLimitXYZ()
#self.ax.get_figure().canvas.draw()
return ss