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
else: # add new node to the RRT tree
newnode = us.sampleCE();
nnNode = KDtree.search_nn(newnode)[0].data
steerNode = us.step1Node(nnNode,newnode,step)
robot.SetActiveDOFValues(steerNode)
if env.CheckCollision(robot) == False:
KDtree.add(steerNode)
cTree[tuple(steerNode)] = tuple(nnNode)
robot.SetActiveDOFValues(startconfig) # return the robot the start
if tuple(goalconfig) not in cTree: # have not found the path to the goal
raw_input("T_T")
else:
print "find path!"
path = us.getpath(cTree,goalconfig)
# print path
color = array(((0,0,1)))
for i in path:
handles.append(env.plot3(points=dot(array([i[0],i[1],i[2]]),1),
pointsize=0.03,
colors=color,
drawstyle=1))
raw_input("Press enter to move robot...")