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
raw_input("enter_to draw path")
######################################
# suppose have n narrow passage #
# Npath = [[path1],[path2],,,[pathn]]#
######################################
PathFinal = []
pathSegment = []
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
pathSegment = pathSegment + [
us.stepNodesQ(beginNode, NpathiBegin, stepFinal), Npahti
]
beginNode = NpahtiEnd
PathFinal = PathFinal + us.stepNodesQ(beginNode, goalconfig, stepFinal)
pathSegment = pathSegment + [
us.stepNodesQ(beginNode, goalconfig, stepFinal)
]
print PathFinal
print type(PathFinal)
handles = []
# plotPath(PathFinal,handles)
print[
len(pathSegment[0]),
for ct in ctotal:
robot.SetActiveDOFValues(ct)
time.sleep(delta_t)
#####################################
######### end of local planner ######
#####################################
# fetch 10 pose from the trajectory and draw a static path
numTotal = len(ctotal)
for num in range(0, 10):
numNode = ctotal[int(np.ceil(numTotal * num / 10))]
robotAni[num].SetActiveDOFValues(numNode)
raw_input("Press enter to next...")
stc = us.stepNodesQ(startconfig, ctotal[0], 0.05)
ctg = us.stepNodesQ(ctotal[-1], goalconfig, 0.05)
complete = stc + ctotal + ctg
numTotal = len(complete)
for num in range(0, 20):
numNode = complete[int(np.ceil(numTotal * num / 20))]
robotAni[num].SetActiveDOFValues(numNode)
raw_input("Press enter to next...")
raw_input("Press enter to next...")
#sample
path_length = len(path)
# failureTIme = zeros(path)
StateTree = []
# 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!"
iterTime = iterTime + 1
iterout = iterout + 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!"
