def plan(samplerIndex):
# construct the state space we are planning in
space = ob.RealVectorStateSpace(3)
# set the bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# set state validity checking for this space
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a start state
start = ob.State(space)
start[0] = 0
start[1] = 0
start[2] = 0
# create a goal state
goal = ob.State(space)
goal[0] = 0
goal[1] = 0
goal[2] = 1
# set the start and goal states;
ss.setStartAndGoalStates(start, goal)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
if samplerIndex == 1:
# use obstacle-based sampling
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocOBValidStateSampler))
elif samplerIndex == 2:
# use my sampler
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
# create a planner for the defined space
planner = og.PRM(si)
ss.setPlanner(planner)
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(10.0)
if solved:
print("Found solution:")
# print the path to screen
print(ss.getSolutionPath())
else:
print("No solution found")
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 newplanner(self, si):
planner = og.PRM(si)
return planner
start = ob.State(space)
start()[0] = 0
start()[1] = 0
goal = ob.State(space)
goal()[0] = 2
goal()[1] = 1.5
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
planner = og.RRTstar(si)
planner = og.PRM(si)
ss.setPlanner(planner)
solved = ss.solve(0.05)
c.draw()
if solved:
ss.simplifySolution()
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
pd.computeEdgeWeights()
Vn = pd.numVertices()
for n in range(0, Vn):
v = pd.getVertex(n)
def plan(grid):
#agent and goal are represented by a point(x,y) and radius
global x
global time
x = grid
agent: Agent = grid.agent
goal: Goal = grid.goal
# Construct the robot state space in which we're planning. R2
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be inside Map
bounds = ob.RealVectorBounds(2)
# projection
pj = ProjectionEvaluator(space)
print('pj=', pj)
pj.setCellSizes(list2vec([1.0, 1.0]))
space.registerDefaultProjection(pj)
# Construct the robot state space in which we're planning.
bounds.setLow(0, 0) #set min x to _ 0
bounds.setHigh(0, grid.size.width) #set max x to _ x width
bounds.setLow(1, 0) #set min y to _ 0
bounds.setHigh(1, grid.size.height) #set max y to _ y height
space.setBounds(bounds)
print("bounds=", bounds.getVolume())
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set robot's starting state to agent's position (x,y) -> e.g. (0,0)
start = ob.State(space)
start[0] = float(agent.position.x)
start[1] = float(agent.position.y)
print(start[0], start[1])
# Set robot's goal state (x,y) -> e.g. (1.0,0.0)
goal = ob.State(space)
goal[0] = float(grid.goal.position.x)
goal[1] = float(grid.goal.position.y)
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
#pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# ******create a planner for the defined space
planner = og.PRM(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
print(planner)
#print('checking projection',planner.getProjectionEvaluator())
print("__________________________________________________________")
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print("space settings\n")
print(si.settings())
print("****************************************************************")
print("problem settings\n")
# print the problem settings
print(pdef)
# attempt to solve the problem within ten second of planning time
solved = planner.solve(time)
# For troubleshooting
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n%s" % path)
#return trace for _find_path_internal method
print(
"$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
)
if path is None:
return None
x = pdef.getSolutionPath().printAsMatrix()
lst = x.split()
lst = [int(round(float(x), 0)) for x in lst]
print(x)
print(lst)
trace = []
for i in range(0, len(lst), 2):
trace.append(Point(lst[i], lst[i + 1]))
print(trace)
return trace
else:
print("No solution found")
return None
