def plan(self):
start = ob.State(self.space)
(start[0], start[1]) = self.start
goal = ob.State(self.space)
(goal[0], goal[1]) = self.goal
def isStateValid(spaceInformation, state):
# perform collision checking or check if other constraints are satisfied
# state_x, state_y = state[0],state[1]
return spaceInformation.satisfiesBounds(state) and \
all([not circle.isInObs(State2D(state)) for circle in self.obs])
def propagate(start, control, duration, state):
for i in range(2):
state[i] = start[i] + duration * control[i]
self.setup.setStateValidityChecker(
ob.StateValidityCheckerFn(
partial(isStateValid, self.setup.getSpaceInformation())))
self.setup.setStatePropagator(oc.StatePropagatorFn(propagate))
tol = Config.distance_to_goal_tolerance
self.setup.setStartAndGoalStates(start, goal, tol)
si = self.setup.getSpaceInformation()
# planner = oc.KPIECE1(si)
planner = oc.RRT(si)
self.setup.setPlanner(planner)
step_size = Config.propagation_step_size
si.setPropagationStepSize(step_size)
solution_time = Config.solution_time
return self.setup.solve(solution_time)
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
partial(isStateValid, ss.getSpaceInformation())))
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
#planner = oc.RRT(si)
#planner = oc.EST(si)
#planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
planner = oc.SyclopEST(si, decomp)
#planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan():
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ss.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
cspace = ss.getControlSpace()
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, -3.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
#si = ss.getSpaceInformation()
information = ss.getSpaceInformation()
information.setStatePropagator(oc.StatePropagatorFn(propagate))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
def configure(ss):
# create a start state
cspace = ss.getControlSpace()
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, 0.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 0.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
si = ss.getSpaceInformation()
si.setStatePropagator(oc.StatePropagatorFn(propagate))
def plan(options, params):
print(params)
""" Entry point for the planning """
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh(params["robot_mesh"])
ss.setRobotMesh(params["env_mesh"])
# setting the real vector state space bounds
se2bounds = ob.RealVectorBounds(2)
se2bounds.setLow(params["lower_bound"])
se2bounds.setHigh(params["upper_bound"])
ss.getStateSpace().setBounds(se2bounds)
# set the bounds for the control space
cspace = ss.getControlSpace()
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, params["lower_bound_cspace"])
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(params["start_x"])
start().setY(params["start_y"])
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(params["goal_x"])
goal().setY(params["goal_y"])
goal().setYaw(0.0)
# space information
si = ss.getSpaceInformation()
si.setStatePropagator(oc.StatePropagatorFn(propagate))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
if not options.bench:
planOnce(ss)
else:
benchmark(ss)
def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
#self.bounds.setLow(low)
#self.bounds.setHigh(high)
self.bounds.setLow(0, ox)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setLow(1, oy)
self.bounds.setHigh(1, oy + size_y)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
def __init__(self, robot_env: DifferentialDriveEnv):
self.robot_env = robot_env
bounds = self.robot_env.get_bounds()
self.state_bounds = bounds['state_bounds']
self.control_bounds = bounds['control_bounds']
# add the state space
space = ob.RealVectorStateSpace(len(self.state_bounds))
bounds = ob.RealVectorBounds(len(self.state_bounds))
# add state bounds
for k, v in enumerate(self.state_bounds):
bounds.setLow(k, float(v[0]))
bounds.setHigh(k, float(v[1]))
space.setBounds(bounds)
self.space = space
# add the control space
cspace = oc.RealVectorControlSpace(space, len(self.control_bounds))
# set the bounds for the control space
cbounds = ob.RealVectorBounds(len(self.control_bounds))
for k, v in enumerate(self.control_bounds):
cbounds.setLow(k, float(v[0]))
cbounds.setHigh(k, float(v[1]))
cspace.setBounds(cbounds)
self.cspace = cspace
# define a simple setup class
self.ss = oc.SimpleSetup(cspace)
self.ss.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
# set the planner
si = self.ss.getSpaceInformation()
planner = oc.SST(si)
self.ss.setPlanner(planner)
si.setPropagationStepSize(.1)
self.start = None
self.goal = None
self.planning_time = 0.0
self.path = None
def plan():
# construct the state space we are planning in
ripl = app.SE2RigidBodyPlanning()
ripl.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ripl.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
space = ripl.getStateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 3.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
# partial(isStateValid, ss.getSpaceInformation())))
sp_info = ss.getSpaceInformation()
extractor = ripl.getGeometricStateExtractor()
enabled = ripl.isSelfCollisionEnabled()
checker = ripl.allocStateValidityChecker(sp_info, extractor, enabled)
ss.setStateValidityChecker(checker)
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
# planner = oc.PDST(si)
# planner = oc.RRT(si)
# planner = oc.EST(si)
planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
# planner = oc.SyclopEST(si, decomp)
# planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
def plan(run_time, planner_type, objective_type, wind_direction, dimensions,
obstacles):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, dimensions[0])
bounds.setLow(1, dimensions[1])
bounds.setHigh(0, dimensions[2])
bounds.setHigh(1, dimensions[3])
# Set the bounds of space to be in [0,1].
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 1)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(1)
cbounds.setLow(-math.pi / 3)
cbounds.setHigh(math.pi / 3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
propagator = get_state_propagator(wind_direction)
ss.setStatePropagator(oc.StatePropagatorFn(propagator))
# Construct a space information instance for this state space
si = ss.getSpaceInformation()
si.setPropagationStepSize(1)
# Set the object used to check which states in the space are valid
validity_checker = ValidityChecker(si, obstacles)
ss.setStateValidityChecker(validity_checker)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = dimensions[0]
start[1] = dimensions[1]
start[2] = math.pi / 4
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = dimensions[2]
goal[1] = dimensions[3]
goal[2] = math.pi / 4
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
objective = allocate_objective(si, objective_type)
ss.setOptimizationObjective(objective)
# Create a decomposition of the state space
decomp = MyDecomposition(256, bounds)
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizing_planner = allocate_planner(si, planner_type, decomp)
ss.setPlanner(optimizing_planner)
# attempt to solve the planning problem in the given runtime
solved = ss.solve(run_time)
if solved:
# Output the length of the path found
print('{0} found solution of path length {1:.4f} with an optimization '
'objective value of {2:.4f}'.format(
ss.getPlanner().getName(),
ss.getSolutionPath().length(), 0.1))
print(ss.getSolutionPath().printAsMatrix())
print(ss.haveExactSolutionPath())
plot_path(ss.getSolutionPath(), dimensions, obstacles)
else:
print("No solution found.")
def __init__(self,
map_,
pose,
target,
max_time=120,
allow_moving_backward=True,
omega_max=0.5,
vmax=0.08,
objective='duration',
planner=oc.SST,
threshold=0.9,
tolerance=0.3,
control_duration=1,
k=1.0,
min_y=None,
max_y=None):
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(map_.size)
if min_y is not None:
bounds.setLow(min_y)
if max_y is not None:
bounds.setHigh(max_y)
space.setBounds(bounds)
cspace = oc.RealVectorControlSpace(space, 2)
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-omega_max)
cbounds.setHigh(omega_max)
cspace.setBounds(cbounds)
ss = oc.SimpleSetup(cspace)
si = ss.getSpaceInformation()
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(partial(valid, si)))
self.propagation_data = {'t': 0, 'nt': 0}
ss.setStatePropagator(
oc.StatePropagatorFn(
partial(propagate, self.propagation_data, map_, threshold,
allow_moving_backward)))
start = ob.State(space)
state_from_tuple(start(), pose)
goal = ob.State(space)
state_from_tuple(goal(), target)
ss.setStartAndGoalStates(start, goal, tolerance)
p = planner(si)
ss.setPlanner(p)
si.setPropagationStepSize(control_duration)
si.setMinMaxControlDuration(control_duration, control_duration)
if objective == 'duration':
do = DurationObjective(si)
elif objective == 'balanced':
do = getBalancedObjective1(si, k=k)
else:
do = ob.PathLengthOptimizationObjective(si)
ss.setOptimizationObjective(do)
ss.setup()
self.ss = ss
self.map = map_
self.s = pose
self.t = target
self.objective = objective
self.planner_name = 'SST'
self.planner_options = {}
self.threshold = threshold
self.tolerance = tolerance
self.comp_duration = 0
self.max_time = max_time
self.allow_moving_backward = allow_moving_backward
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