def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
elif plannerType.lower() == "rrtxstatic":
return og.RRTXstatic(si)
elif plannerType.lower() == "rrtsharp":
return og.RRTsharp(si)
# multi-query
elif plannerType.lower() == "lazyprmstar":
return og.LazyPRMstar(si)
# single-query
elif plannerType.lower() == "rrtconnect":
return og.RRTConnect(si)
elif plannerType.lower() == "lbtrrt":
return og.LBTRRT(si)
elif plannerType.lower() == "lazylbtrrt":
return og.LazyLBTRRT(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def __init__(self):
img = Image.open('/home/alphonsus/ompl_code/test_codes/rrt_2d/rrt.png')
self.env = np.array(img, dtype='uint8')
self.maxWidth = self.env.shape[0] - 1
self.maxHeight = self.env.shape[1] - 1
# bounds = ob.RealVectorBounds(2)
# bounds.setLow(0,0); bounds.setHigh(0,self.env.shape[0])
# bounds.setLow(1,0); bounds.setHigh(1,self.env.shape[1])
# self.space = ob.SE2StateSpace()
self.space = ob.RealVectorStateSpace()
self.space.addDimension(0.0, self.env.shape[0])
self.space.addDimension(0.0, self.env.shape[1])
# self.space.setBounds(bounds)
self.ss_ = og.SimpleSetup(self.space)
self.ss_.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
self.space.setup()
self.ss_.getSpaceInformation().setStateValidityCheckingResolution(
1.0 / self.space.getMaximumExtent())
self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation()))
self.ss_.setup()
def plan_path(self):
self.problem_definition.clearSolutionPaths()
path_collides = True
if self.use_linear_path:
path = self.linear_path(self.start_state, self.goal_state)
path_collides = self.path_collides(path)
if path_collides:
if not self.motion_validator.isValid(
self.start_state) or not self.motion_validator.isValid(
self.goal_state):
logging.warn(
"PathPlanner: Start or goal state not valid. Skipping")
return [], [], []
self.problem_definition.addStartState(self.start_state)
self.problem_definition.setStartAndGoalStates(
self.start_state, self.goal_state)
self.planner = og.RRTConnect(self.si)
self.planner.setRange(
np.sqrt(self.si.getStateSpace().getDimension() *
np.square(self.delta_t * self.max_velocity)))
self.planner.setProblemDefinition(self.problem_definition)
self.planner.setup()
start_time = time.time()
while not self.problem_definition.hasSolution():
self.planner.solve(1.0)
delta = time.time() - start_time
logging.info("PathPlanner: Time spent on planning so far: " +
str(delta))
if delta > 3.0:
xs = []
us = []
zs = []
return xs, us, zs
path = []
if self.problem_definition.hasSolution():
logging.info("PathPlanner: Solution found after " +
str(time.time() - start_time) + " seconds")
solution_path = self.problem_definition.getSolutionPath()
states = solution_path.getStates()
path = [
np.array(
[state[i] for i in xrange(self.space.getDimension())])
for state in states
]
#print "path " + str(path)
else:
print "no solution"
return self._augment_path(path)
def plan():
# create an Vector state space
space = ob.RealVectorStateSpace(SPACE_DIM)
# print(space)
# # set lower and upper bounds
bounds = ob.RealVectorBounds(SPACE_DIM)
for i in range(SPACE_DIM):
bounds.setLow(i, -1)
bounds.setHigh(i, 1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTConnect(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# we can pick a random start state...
start.random()
start_state = start.get()
for i in range(SPACE_DIM):
# start_state[i] = 0.1
print(start_state[i])
goal = ob.State(space)
# we can pick a random goal state...
goal.random()
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print(ss.getSolutionPath())
def plan_path(self):
self.problem_definition.clearSolutionPaths()
path_collides = True
if (not self.motion_validator.isValid(self.start_state)
or not self.problem_definition.getGoal().canSample()):
return [], [], []
if self.use_linear_path:
path = self.linear_path(
self.start_state,
self.problem_definition.getGoal().getRandomGoalState())
path_collides = self.path_collides(path)
if path_collides:
self.problem_definition.addStartState(self.start_state)
self.planner = og.RRTConnect(self.si)
self.planner.setRange(
np.sqrt(self.si.getStateSpace().getDimension() *
np.square(self.delta_t * self.max_velocity)))
self.planner.setProblemDefinition(self.problem_definition)
self.planner.setup()
start_time = time.time()
while not self.problem_definition.hasSolution():
self.planner.solve(1.0)
delta = time.time() - start_time
if delta > 3.0:
#logging.warn("PathPlanner: Maximum allowed planning time exceeded. Aborting")
xs = []
us = []
zs = []
return xs, us, zs
path = []
if self.problem_definition.hasSolution():
logging.info("PathPlanner: Solution found after " +
str(time.time() - start_time) + " s")
solution_path = self.problem_definition.getSolutionPath()
states = solution_path.getStates()
path = [
np.array(
[state[i] for i in xrange(self.space.getDimension())])
for state in states
]
return self._augment_path(path)
def planTheHardWay():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# construct an instance of space information from this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a random start state
start = ob.State(space)
start.random()
# create a random goal state
goal = ob.State(space)
goal.random()
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.RRTConnect(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print(si.settings())
# print the problem settings
print(pdef)
# attempt to solve the problem within one second of planning time
solved = planner.solve(1.0)
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)
else:
print("No solution found")
def plan():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTConnect(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# we can pick a random start state...
start.random()
# ... or set specific values
start().setX(.5)
goal = ob.State(space)
# we can pick a random goal state...
goal.random()
# ... or set specific values
goal().setX(-.5)
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print(ss.getSolutionPath())
def choose_planner(self, si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtconnect":
return og.RRTConnect(si)
elif plannerType.lower() == "rrtsharp":
return og.RRTsharp(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
else:
OMPL_ERROR("Planner-type is not implemented in allocation function.")
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
planner = og.BITstar(si)
planner.setPruning(False)
planner.setSamplesPerBatch(200)
planner.setRewireFactor(20.)
return planner
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
elif plannerType.lower() == 'rrtconnect':
return og.RRTConnect(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def newplanner(self, si):
planner = og.RRTConnect(si)
planner.setRange(10.0)
return planner
goal1 = goal()[0]
goal1.setX(200.49)
goal1.setY(-40.62)
goal1.setZ(70.57)
goal1.rotation().setIdentity()
# set goal for robot 2
goal2 = goal()[1]
goal2.setX(-4.96)
goal2.setY(-40.62)
goal2.setZ(70.57)
goal2.rotation().setIdentity()
# set the start & goal states
setup.setStartAndGoalStates(start, goal)
# setting collision checking resolution to 1% of the space extent
setup.getSpaceInformation().setStateValidityCheckingResolution(0.01)
# use RRTConnect for planning
setup.setPlanner(og.RRTConnect(setup.getSpaceInformation()))
# we call setup just so print() can show more information
setup.setup()
print(setup)
# try to solve the problem
if setup.solve(60):
# simplify & print the solution
setup.simplifySolution()
print(setup.getSolutionPath().printAsMatrix())
def get_planner(self, si):
return og.RRTConnect(si)
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.RRTConnect(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")
