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.space.setBounds(self.bounds)
# define a simple setup class
self.ss = og.SimpleSetup(self.space)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
def setup_ompl(self):
print "set space"
self.space = ob.RealVectorStateSpace(dim=self.space_dimension)
bounds = ob.RealVectorBounds(self.space_dimension)
print "set bounds"
for i in xrange(self.space_dimension):
bounds.setLow(i, self.joint_constraints[0])
bounds.setHigh(i, self.joint_constraints[1])
print "set bounds 2"
self.space.setBounds(bounds)
print "set si"
self.si = ob.SpaceInformation(self.space)
print "set motion validator"
self.motion_validator = MotionValidator(self.si)
self.motion_validator.set_link_dimensions(self.link_dimensions)
self.motion_validator.set_workspace_dimension(self.workspace_dimension)
self.motion_validator.set_max_distance(self.max_velocity, self.delta_t)
print "set state validiy checker"
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.motion_validator.isValid))
print "set motion validator"
self.si.setMotionValidator(self.motion_validator)
print "si.setup"
self.si.setup()
self.problem_definition = ob.ProblemDefinition(self.si)
def plan():
# create an R^3 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
start()[0] = -1.
start()[1] = -1.
start()[2] = -1.
goal = ob.State(space)
goal()[0] = 1.
goal()[1] = 1.
goal()[2] = 1.
ss.setStartAndGoalStates(start, goal, .05)
# set the planner
planner = RandomWalkPlanner(ss.getSpaceInformation())
ss.setPlanner(planner)
result = ss.solve(10.0)
if result:
if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
print("Solution is approximate")
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print(ss.getSolutionPath())
def __init__(self, env):
self.space = ob.CompoundStateSpace()
self.setup = og.SimpleSetup(self.space)
bounds = ob.RealVectorBounds(1)
bounds.setLow(0)
bounds.setHigh(float(env.width) - 0.000000001)
self.m1 = mySpace1()
self.m1.setBounds(bounds)
bounds.setHigh(float(env.height) - 0.000000001)
self.m2 = mySpace1()
self.m2.setBounds(bounds)
self.space.addSubspace(self.m1, 1.0)
self.space.addSubspace(self.m2, 1.0)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
self.setup.setStateValidityChecker(isValidFn)
state = ob.CompoundState(self.space)
state()[0][0] = env.start[0]
state()[1][0] = env.start[1]
self.start = ob.State(state)
gstate = ob.CompoundState(self.space)
gstate()[0][0] = env.goal[0]
gstate()[1][0] = env.goal[1]
self.goal = ob.State(gstate)
self.setup.setStartAndGoalStates(self.start, self.goal)
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 __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():
# 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)
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)
# this will automatically choose a default planner with
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print ss.getSolutionPath()
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(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
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 planBITstar(q_st, q_g, res):
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(0)
bounds.setHigh(res)
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 start state
start = ob.State(space)
start[0] = q_st[0]
start[1] = q_st[1]
# create goal state
goal = ob.State(space)
goal[0] = q_g[0]
goal[1] = q_g[1]
# 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.BITstar(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# 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")
path_arr = []
print(path.getStateCount())
for i in range(path.getStateCount()):
path_arr.append([])
path_arr[i].append(path.getState(i)[0])
path_arr[i].append(path.getState(i)[1])
return path_arr
else:
print("No solution found")
return []
def _get_path(
self,
is_state_valid: Callable[[np.ndarray], bool],
start_js: np.ndarray,
robot_targ: RobotTarget,
use_sim: MpSim,
mp_space: MpSpace,
timeout: int,
):
"""
Does the low-level path planning with OMPL.
"""
if not self._should_render:
ou.setLogLevel(ou.LOG_ERROR)
dim = mp_space.get_state_dim()
space = ob.RealVectorStateSpace(dim)
bounds = ob.RealVectorBounds(dim)
lims = mp_space.get_state_lims()
for i, lim in enumerate(lims):
bounds.setLow(i, lim[0])
bounds.setHigh(i, lim[1])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid))
si.setup()
pdef = ob.ProblemDefinition(si)
mp_space.set_problem(pdef, space, si, start_js, robot_targ)
planner = mp_space.get_planner(si)
planner.setProblemDefinition(pdef)
planner.setup()
if mp_space.get_range() is not None:
planner.setRange(mp_space.get_range())
solved = planner.solve(timeout)
if not solved:
self._log("Could not find plan")
return None
objective = pdef.getOptimizationObjective()
if objective is not None:
cost = (pdef.getSolutionPath().cost(
pdef.getOptimizationObjective()).value())
else:
cost = np.inf
self._log("Got a path of length %.2f and cost %.2f" %
(pdef.getSolutionPath().length(), cost))
path = pdef.getSolutionPath()
joint_plan = mp_space.convert_sol(path)
self._is_approx_sol = pdef.hasApproximateSolution()
return joint_plan
def space_info_creation(self, space):
"""
Function of creating space information that includes valid and invalid states
"""
space_info = ob.SpaceInformation(space)
space_info.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
space_info.setup()
return space_info
def plan(runTime, plannerType, objectiveType, plotData, fname, pdfname):
space = ob.RealVectorStateSpace(2)
space.setBounds(0.0, 1.0)
ss = og.SimpleSetup(space)
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
ss.setOptimizationObjective(allocateObjective(si, objectiveType))
ss.setPlanner(allocatePlanner(si, plannerType))
ss.setup()
# attempt to solve the problem
solved = ss.solve(runTime)
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.getSolutionPlannerName(), \
ss.getSolutionPath().length(), \
ss.getSolutionPath().cost(ss.getOptimizationObjective()).value()))
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(si)
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool and plotData:
useGraphTool(pd)
# 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(ss.getSolutionPath().printAsMatrix())
if pdfname:
pds = ob.PlannerDataStorage()
pds.store(pd, pdfname)
else:
print("No solution found.")
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 space_info_creation(self, space, obstacle_list):
"""
Function of creating space information that includes valid and invalid states
"""
space_info = ob.SpaceInformation(space)
isValidFn = ob.StateValidityCheckerFn(partial(self.isStateValid, obstacle_list))
space_info.setStateValidityChecker(isValidFn)
#space_info.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
space_info.setup()
return space_info
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 plan():
# construct the state space we are planning in
# The state space of the car is SE(2) (x and y position with one angle for orientation).
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
# ?? for this simple car model consists of the velocity and steering angle, both real valued.
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
global ss
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
#odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
odeSolver = oc.ODEErrorSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# 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)
# attempt to solve the problem
solved = ss.solve(120.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
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)
validityChecker = ob.StateValidityCheckerFn(
partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# 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)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" %
ss.getSolutionPath().asGeometric().printAsMatrix())
def __init__(self, ppm_file):
self.ppm_ = ou.PPM()
self.ppm_.loadFile(ppm_file)
space = ob.RealVectorStateSpace()
space.addDimension(0.0, self.ppm_.getWidth())
space.addDimension(0.0, self.ppm_.getHeight())
self.maxWidth_ = self.ppm_.getWidth() - 1
self.maxHeight_ = self.ppm_.getHeight() - 1
self.ss_ = og.SimpleSetup(space)
# set state validity checking for this space
self.ss_.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(Plane2DEnvironment.isStateValid, self)))
space.setup()
self.ss_.getSpaceInformation().setStateValidityCheckingResolution( \
1.0 / space.getMaximumExtent())
def plan(self, start, goal, plan_time=20.0, simplify=True):
# create a simple setup object
ss = og.SimpleSetup(self.space)
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_state_valid))
ss.setStartAndGoalStates(self._get_state(start), self._get_state(goal))
# this will automatically choose a default planner with
# default parameters
solved = ss.solve(plan_time)
if solved:
# try to shorten the path
if simplify:
ss.simplifySolution()
path = ss.getSolutionPath()
return [(s.getX(), s.getY()) for s in path.getStates()]
return None
def plan():
N = 10
# create an R^2 state space
space = ob.RealVectorStateSpace(2)
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(N)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
start[0] = 0 # random.randint(0, int(N / 2))
start[1] = 0 # random.randint(0, int(N / 2))
goal = ob.State(space)
goal[0] = N # random.randint(int(N / 2), N)
goal[1] = N # random.randint(int(N / 2), N)
ss.setStartAndGoalStates(start, goal)
planner = Astar(ss.getSpaceInformation())
ss.setPlanner(planner)
result = ss.solve(.01)
if result:
if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
print("Solution is approximate")
matrix = ss.getSolutionPath().printAsMatrix()
print(matrix)
verts = []
for line in matrix.split("\n"):
x = []
for item in line.split():
x.append(float(item))
if len(x) is not 0:
verts.append(list(x))
# print(verts)
plt.axis([0, N, 0, N])
x = []
y = []
for i in range(0, len(verts)):
x.append(verts[i][0])
y.append(verts[i][1])
# plt.plot(verts[i][0], verts[i][1], 'r*-')
plt.plot(x, y, 'ro-')
plt.show()
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(space, planner, runTime, start, goal):
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
ss.setStartAndGoalStates(start, goal)
if planner == 'RRT':
ss.setPlanner(og.RRT(ss.getSpaceInformation()))
elif planner == 'Astar':
ss.setPlanner(Astar.Astar(ss.getSpaceInformation()))
else:
print('Bad planner')
solved = ss.solve(runTime)
if solved:
path = ss.getSolutionPath()
path.interpolate(1000)
return path.printAsMatrix()
# return ss.getSolutionPath().printAsMatrix()
else:
print("No solution found.")
return None
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 setSpace(self):
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.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)
validityChecker = ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation()))
self.ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(self.kinematicCarODE)
odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
self.ss.setStatePropagator(propagator)
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, env):
self.sMan = mySpace()
super(mySpaceInformation, self).__init__(self.sMan)
sbounds = ob.RealVectorBounds(2)
# dimension 0 (x) spans between [0, width)
# dimension 1 (y) spans between [0, height)
# since sampling is continuous and we round down, we allow values until
# just under the max limit
# the resolution is 1.0 since we check cells only
sbounds.low[0] = 0.0
sbounds.high[0] = float(env.width) - 0.000000001
sbounds.low[1] = 0.0
sbounds.high[1] = float(env.height) - 0.000000001
self.sMan.setBounds(sbounds)
self.setStateValidityCheckingResolution(0.5)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
self.setStateValidityChecker(isValidFn)
self.setup()