def plan(self, start_row, start_col, goal_row, goal_col):
start = ob.State(self.ss_.getStateSpace())
start()[0] = start_row
start()[1] = start_col
# start().setX(start_row)
# start().setY(start_col)
goal = ob.State(self.ss_.getStateSpace())
goal()[0] = goal_row
goal()[1] = goal_col
self.ss_.setStartAndGoalStates(start, goal)
self.ss_.setup()
solved = self.ss_.solve()
if solved:
print('Found %d solutions' %
self.ss_.getProblemDefinition().getSolutionCount())
if (self.ss_.haveSolutionPath()):
self.ss_.simplifySolution()
p = self.ss_.getSolutionPath()
ps = og.PathSimplifier(self.ss_.getSpaceInformation())
ps.simplifyMax(p)
ps.smoothBSpline(p)
# self.ss_.getPathSimplifier().simplifyMax(p)
# self.ss_.getPathSimplifier().smoothBSpline(p)
return True
return False
def plan(self, start_row, start_col, goal_row, goal_col):
if not self.ss_:
return False
start = ob.State(self.ss_.getStateSpace())
start()[0] = start_row
start()[1] = start_col
goal = ob.State(self.ss_.getStateSpace())
goal()[0] = goal_row
goal()[1] = goal_col
self.ss_.setStartAndGoalStates(start, goal)
# generate a few solutions; all will be added to the goal
for _ in range(10):
if self.ss_.getPlanner():
self.ss_.getPlanner().clear()
self.ss_.solve()
ns = self.ss_.getProblemDefinition().getSolutionCount()
print("Found %d solutions" % ns)
if self.ss_.haveSolutionPath():
self.ss_.simplifySolution()
p = self.ss_.getSolutionPath()
ps = og.PathSimplifier(self.ss_.getSpaceInformation())
ps.simplifyMax(p)
ps.smoothBSpline(p)
return True
return False
def execute(self, env, time, pathLength, show=False):
result = True
# instantiate space information
si = mySpaceInformation(env)
# instantiate problem definition
pdef = ob.ProblemDefinition(si)
# instantiate motion planner
planner = self.newplanner(si)
planner.setProblemDefinition(pdef)
planner.setup()
# the initial state
state = ob.State(si)
state()[0] = env.start[0]
state()[1] = env.start[1]
pdef.addStartState(state)
goal = ob.GoalState(si)
gstate = ob.State(si)
gstate()[0] = env.goal[0]
gstate()[1] = env.goal[1]
goal.setState(gstate)
goal.threshold = 1e-3
pdef.setGoal(goal)
startTime = clock()
if planner.solve(SOLUTION_TIME):
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Found solution in %f seconds!' % elapsed)
path = pdef.getSolutionPath()
sm = og.PathSimplifier(si)
startTime = clock()
sm.reduceVertices(path)
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Simplified solution in %f seconds!' % elapsed)
path.interpolate(100)
pathLength = pathLength + path.length()
if show:
print(env, '\n')
temp = copy.deepcopy(env)
for i in range(len(path.states)):
x = int(path.states[i][0])
y = int(path.states[i][1])
if temp.grid[x][y] in [0, 2]:
temp.grid[x][y] = 2
else:
temp.grid[x][y] = 3
print(temp, '\n')
else:
result = False
return (result, time, pathLength)
def path_optimization(self, path, space_info):
"""
Function of optimizing the path and splitting it into equal segments
"""
ps = og.PathSimplifier(space_info)
shorteningPath = ps.shortcutPath(path)
reduceVertices = ps.reduceVertices(path)
path.interpolate(int(path.length() * 10))
return path
def path_optimization(self, path, space_info):
"""
Function of optimizing the path and splitting it into equal segments
"""
ps = og.PathSimplifier(space_info)
shorteningPath = ps.shortcutPath(path)
reduceVertices = ps.reduceVertices(path)
path.interpolate(int(path.length() / self.planner_range))
path = self.convert_path_to_point2d(path.getStates())
return path
def plan(mapfile,start_node, goal_node,runTime, plannerType, objectiveType, d, fname):
boundary, blocks = load_map(mapfile)
boundary[:,:3] = boundary[:,:3] +0.0005
boundary[:,3:] = boundary[:,3:] -0.0005
alpha = 1.05
blocks = blocks*alpha
# Construct the robot state space in which we're planning. We're
space = ob.RealVectorStateSpace(3)
sbounds = ob.RealVectorBounds(3)
sbounds.low[0] =float(boundary[0,0])
sbounds.high[0] =float(boundary[0,3])
sbounds.low[1] = float(boundary[0,1])
sbounds.high[1] = float(boundary[0,4])
sbounds.low[2] = float(boundary[0,2])
sbounds.high[2] = float(boundary[0,5])
space.setBounds(sbounds)
# 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, boundary, blocks)
si.setStateValidityChecker(validityChecker)
mv = MyMotionValidator(si, boundary, blocks)
si.setMotionValidator(mv)
si.setup()
# Set our robot's starting state
start = ob.State(space)
start[0] = start_node[0]
start[1] = start_node[1]
start[2] = start_node[2]
# Set our robot's goal state
goal = ob.State(space)
goal[0] = goal_node[0]
goal[1] = goal_node[1]
goal[2] = goal_node[2]
# 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))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si,d, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = "Approximate solution"
# attempt to solve the planning problem in the given runtime
t1 = tic()
while(str(solved) == 'Approximate solution'):
solved = optimizingPlanner.solve(runTime)
# print(pdef.getSolutionPath().printAsMatrix())
t2 = toc(t1)
p = pdef.getSolutionPath()
ps = og.PathSimplifier(si)
ps.simplifyMax(p)
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( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
env = mapfile.split(".")[1].split("/")[-1]
fname = fname + "{}_{}_{}_{}.txt".format(env, d,np.round(pdef.getSolutionPath().length(),2),np.round(t2,2))
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
path = np.genfromtxt(fname)
print("The found path : ")
print(path)
pathlength = np.sum(np.sqrt(np.sum(np.diff(path,axis=0)**2,axis=1)))
print("pathlength : {}".format(pathlength))
fig, ax, hb, hs, hg = draw_map(boundary, blocks, start_node, goal_node)
ax.plot(path[:,0],path[:,1],path[:,2],'r-')
plt.show()