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, shape, calc_time, resolution, distance):
ou.setLogLevel(ou.LOG_WARN)
self._calc_time = calc_time
self._space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0.0)
bounds.setHigh(0, shape[0] - 1)
bounds.setHigh(1, shape[1] - 1)
self._space.setBounds(bounds)
self._si = ob.SpaceInformation(self._space)
self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si)
self._si.setStateValidityChecker(self._validity_checker)
self._si.setStateValidityCheckingResolution(resolution)
self._si.setup()
self._start = ob.State(self._space)
self._goal = ob.State(self._space)
self._pdef = ob.ProblemDefinition(self._si)
objective = ob.MultiOptimizationObjective(self._si)
objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5)
# ## setting length objective
length_objective = ob.PathLengthOptimizationObjective(self._si)
objective.addObjective(length_objective, 0.1)
# objective.setCostThreshold(1000.0)
self._pdef.setOptimizationObjective(objective)
self._planner = og.RRTstar(self._si)
self._planner.setRange(distance)
self._planner.setup()
def test_optimal(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
objective = ob.MultiOptimizationObjective(si)
objective.addObjective(MapCostObjective(si), 0.9)
objective.addObjective(ob.PathLengthOptimizationObjective(si), 0.1)
pdef.setOptimizationObjective(objective)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
states = pdef.getSolutionPath().getStates()
cost = pdef.getSolutionPath().cost(objective).value()
states_np = np.array([(state[0], state[1]) for state in states])
print(states)
print(cost)
print(states_np)
def __init__(self, lower_bound, upper_bound):
"""Initialize the object."""
# Create an R2 state space
self.space = ob.RealVectorStateSpace(2)
# Set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(lower_bound)
bounds.setHigh(upper_bound)
self.space.setBounds(bounds)
# Create a new space information
self.si = ob.SpaceInformation(self.space)
# Set the state validity checker
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_state_valid))
self.si.setup()
# Set the problem definition
self.pdef = ob.ProblemDefinition(self.si)
# Setup an optimizing planner with RRT*
self.optimizingPlanner = og.RRTstar(self.si)
# Empty obstacles list
self.obstacles = []
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -40)
bounds.setHigh(0, 40)
bounds.setLow(1, 0)
bounds.setHigh(1, 90)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(0)
start().setY(0)
start().setYaw(0)
# start().setZ(-9)
# start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-25)
goal().setY(60)
goal().setYaw(0)
# 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.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
# To change time change value given to ss.solve
solved = ss.solve(1.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()
path = get_path(ss)
draw_graph(path)
plt.show()
def setPlanner_3d(self):
self.si = self.ss.getSpaceInformation()
if self.plannerType.lower() == "bitstar":
planner = og.BITstar(self.si)
elif self.plannerType.lower() == "fmtstar":
planner = og.FMT(self.si)
elif self.plannerType.lower() == "informedrrtstar":
planner = og.InformedRRTstar(self.si)
elif self.plannerType.lower() == "prmstar":
planner = og.PRMstar(self.si)
elif self.plannerType.lower() == "rrtstar":
planner = og.RRTstar(self.si)
elif self.plannerType.lower() == "sorrtstar":
planner = og.SORRTstar(self.si)
else:
print("Planner-type is not implemented in allocation function.")
planner = og.RRTstar(self.si)
self.ss.setPlanner(planner)
def plan():
# create an Vector state space
space = ob.RealVectorStateSpace(SPACE_DIM)
# # set lower and upper bounds
bounds = ob.RealVectorBounds(SPACE_DIM)
for i in range(SPACE_DIM - 3):
bounds.setLow(i, min_bound)
bounds.setHigh(i, max_bound)
for i in range(SPACE_DIM - 3):
bounds.setLow(i + 3, -np.pi)
bounds.setHigh(i + 3, np.pi)
space.setBounds(bounds)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# start.random()
start_state = start.get()
for i in range(SPACE_DIM):
start_state[i] = start_s[i]
# print(start_state[i])
goal = ob.State(space)
# goal.random()
goal_state = goal.get()
for i in range(SPACE_DIM):
goal_state[i] = goal_s[i]
# print(goal_state[i])
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(4.0)
if solved:
# パスの簡単化を実施
ss.simplifySolution()
# 結果を取得
path_result = ss.getSolutionPath()
print(path_result)
si = ss.getSpaceInformation()
pdata = ob.PlannerData(si)
ss.getPlannerData(pdata)
space = path_result.getSpaceInformation().getStateSpace()
plot_result(space, path_result, pdata)
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
else:
OMPL_ERROR("Planner-type is not implemented in allocation function.")
def allocatePlanner(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() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def test_rigid(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
cost = pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()
print(cost)
def plan_rrt(self, name, init, goal):
init = self.createState(*tuple(init))
goal = self.createState(*tuple(goal))
self.name_target = name
planner = og.RRTstar(self.si)
ss = og.SimpleSetup(self.si)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
ss.setPlanner(planner)
ss.setStartAndGoalStates(init, goal)
solved = ss.solve(1.0)
if solved:
ss.simplifySolution()
path = ss.getSolutionPath()
path_res = []
for state in path.getStates():
path_res.append((state.getX(),state.getY(),state.getYaw()))
return path_res
else:
return None
def __init__(self, ndof, iksolver, step_size=0.05):
self.ndof = ndof
self.iksolver = iksolver
self.state_space = MyStateSpace(ndof)
self.control_space = MyControlSpace(self.state_space, ndof)
self.simple_setup = oc.SimpleSetup(self.control_space)
si = self.simple_setup.getSpaceInformation()
si.setPropagationStepSize(step_size)
si.setMinMaxControlDuration(1, 1)
si.setDirectedControlSamplerAllocator(
oc.DirectedControlSamplerAllocator(
directedControlSamplerAllocator))
vc = MyStateValidityChecker(self.simple_setup.getSpaceInformation(),
self.iksolver, ndof)
self.simple_setup.setStateValidityChecker(vc)
ob = MyOptimizationObjective(self.simple_setup.getSpaceInformation())
self.simple_setup.setOptimizationObjective(ob)
propagator = MyStatePropagator(self.simple_setup.getSpaceInformation(),
ndof)
self.simple_setup.setStatePropagator(propagator)
# self.planner = oc.KPIECE1(self.simple_setup.getSpaceInformation())
# self.planner.setup()
# ========= RRT planner ============
self.planner = og.RRTstar(self.simple_setup.getSpaceInformation())
p_goal = 0.0
self.planner.setGoalBias(p_goal)
self.planner.setRange(step_size)
IPython.embed()
# =========== PRM planner =============
# self.planner = og.PRM(self.simple_setup.getSpaceInformation())
self.simple_setup.setPlanner(self.planner)
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 plan(self, controlled_object, x1, y1, v1, a1):
a1 = (a1 + 2 * np.pi) % (2 * np.pi)
car = deepcopy(controlled_object)
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(0, 0)
bounds.setLow(1, 0)
bounds.setLow(2, 0)
bounds.setHigh(0, 1000)
bounds.setHigh(1, 1000)
bounds.setHigh(2, 2 * np.pi)
space.setBounds(bounds)
si = ob.SpaceInformation(space)
validityChecker = ValidityChecker(si, self.state, car)
si.setStateValidityChecker(validityChecker)
#si.setMotionValidator(MotionValidityChecker(si, state, agent_num))
si.setup()
start = ob.State(space)
start[0] = car.x
start[1] = car.y
start[2] = car.angle
goal = ob.State(space)
goal[0] = x1
goal[1] = y1
goal[2] = a1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si))
def objStateCost(state):
return ob.Cost(0)
def objMotionCost(state1, state2):
x, y, a = state1[0], state1[1], state1[2]
xt, yt, at = state2[0], state2[1], state2[2]
yd, xd = yt - y, xt - x
ax = np.arctan(yd / (xd + 0.001))
ax = ax % (np.pi)
if yd < 0:
ax = ax + np.pi
ad = min(np.abs(at - ax), np.abs((ax - at)))
return 10 * ad
pathObj = ob.OptimizationObjective(si)
pathObj.stateCost = objStateCost
pathObj.motionCost = objMotionCost
pathObj.setCostThreshold(1)
#pdef.setOptimizationObjective(pathObj)
optimizingPlanner = og.RRTstar(si)
optimizingPlanner.setRange(self.inter_point_d)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(self.planning_time)
sol = pdef.getSolutionPath()
if not sol:
return []
sol = sol.printAsMatrix()
s = [[float(j) for j in i.split(" ")[:-1]]
for i in sol.splitlines()][:-1]
v0 = car.vel
num_points = len(s)
for i in range(num_points):
[i].append(v0 * (1 - float(i) / float(num_points)) + v1 *
(float(i) / float(num_points)))
return s
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):
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.RRTstar(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")
def plan(fname=None):
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# 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
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Since we want to find an optimal plan, we need to define what
# is optimal with an OptimizationObjective structure. Un-comment
# exactly one of the following 6 lines to see some examples of
# optimization objectives.
pdef.setOptimizationObjective(getPathLengthObjective(si))
# pdef.setOptimizationObjective(getThresholdPathLengthObj(si))
# pdef.setOptimizationObjective(getClearanceObjective(si))
# pdef.setOptimizationObjective(getBalancedObjective1(si))
# pdef.setOptimizationObjective(getBalancedObjective2(si))
# pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si))
# Construct our optimal planner using the RRTstar algorithm.
optimizingPlanner = og.RRTstar(si)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem within one second of
# planning time
solved = optimizingPlanner.solve(10.0)
if solved:
# Output the length of the path found
print("Found solution of path length %g" %
pdef.getSolutionPath().length())
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
else:
print("No solution found.")
