def smoothPath(path):
# smooth the path
for i in range(200):
print i
n = len(path)
A = randrange(0,n)
B = randrange(0,n)
while(abs(A-B)<=1):
B = randrange(0,n)
t = A
A = min(A,B)
B = max(B,t)
a = path[A]
b = path[B]
smoothPath = us.stepNodesQ(a,b,step)
flag = False
# print len(smoothPath)
for smoothNode in smoothPath:
# print smoothNode
robot.SetActiveDOFValues(smoothNode)
if env.CheckCollision(robot) == True:
flag = True
break
if flag == False:
for k in range(A+1,B):
del path[A+1]
# print 'smoothing'
k = A+1
for smoothNode in smoothPath:
path.insert(k,smoothNode)
k = k + 1
return path
def RRT(startconfig,goalconfig,goalbias = 0.05, step = 0.5, workspaceBound = [-4.5,4.5,-2.2,2.2,0.21,1.54]):
# return a unsmoothed path from start to goal
# take congiguration as input [translation,quaternion]
# [x,q]
# all data are stored in list except diction CTree
# build RRT tree in cSpace
# RRT tree initialize
cTree = {tuple(startconfig):tuple(startconfig)} #\
iterTime = 0 # search and build tree
iterout = 0
cTreeSize = 1
newAddedNode = startconfig
while iterTime < 100000:
# time.sleep(0.4)
if iterTime % 1000 ==0:
print iterTime
iterTime = iterTime + 1
# test bias
chance = random.uniform(0,1)
if chance < goalbias: # try to connect the goal
# print "try goal"
newnode = goalconfig
# nnNode = us.nnNodeCQ(cTree,newnode)
nnNode = newAddedNode
if us.distXQ(nnNode,newnode) <= step: # goal is within one step length!
cTree[tuple(goalconfig)] = tuple(nnNode)
print "find path, it is just near!"
break
else: # goal is far than one step
nnPath = us.stepNodesQ(nnNode,newnode,step)
collision = False
for pathNode in nnPath: # check the path to goal
robot.SetActiveDOFValues(pathNode)
if env.CheckCollision(robot) == True:
collision = True
break
if collision == False: # the path is clear!
cTree[tuple(nnPath[0])] = tuple(nnNode)
for i in range(1,len(nnPath)):
cTree[tuple(nnPath[i])] = tuple(nnPath[i-1])
# cTree[tuple(goalconfig)] = tuple(nnPath[len(nnPath)-1])
# print nnPath[i-1]
print "find path, by setp nodes!"
break
else: # add new node to the RRT tree
newnode = us.sampleCQ();
# robot.SetActiveDOFValues(newnode)
# time.sleep(0.2)
nnNode = us.nnNodeCQ(cTree,newnode)
steerNode = us.step1NodeQ(nnNode,newnode,step)
robot.SetActiveDOFValues(steerNode)
# time.sleep(0.4)
if env.CheckCollision(robot) == False:
cTree[tuple(steerNode)] = tuple(nnNode)
newAddedNode = steerNode
cTreeSize += 1
print "add new node cTree size:",cTreeSize
robot.SetActiveDOFValues(startconfig) # return the robot the start
###################
# Smooth the path #
###################
if tuple(goalconfig) not in cTree: # have not found the path to the goal
raw_input("T_T")
else:
print "find goal in the outcome path!"
path = us.getpath(cTree,goalconfig)
return path
# Npath.append(ctotal)
raw_input("enter_to draw path")
######################################
# suppose have n narrow passage #
# Npath = [[path1],[path2],,,[pathn]]#
######################################
PathFinal = []
stepFinal = 0.02
beginNode = startconfig
for i in range(len(Npath)):
Npahti = Npath[i]
NpathiBegin = Npahti[0]
NpahtiEnd = Npahti[-1]
PathFinal = PathFinal + us.stepNodesQ(beginNode,NpathiBegin,stepFinal) + Npahti
beginNode = NpahtiEnd
PathFinal = PathFinal + us.stepNodesQ(beginNode,goalconfig,stepFinal)
print PathFinal
print type(PathFinal)
handles = []
plotPath(PathFinal,handles)
raw_input("enter_to draw path by quadcopters")
numTotal = len(PathFinal)
for num in range(0,20):
numNode = PathFinal[int(np.ceil(numTotal*num/20))]
robotAni[num].SetActiveDOFValues(numNode)