Esempio n. 1
0
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...")