BiRRT: Getting path from an array of 7 DOF angle configurations

Question

I've kind of finished implementing a BiRRT for a 7 DOF arm, using a KD-tree from numpy.spatial in order to get nearest queries. A picture is below:

I'm currently having trouble with the fact that it is impossible to retrieve a path from the start node to a particular node using a KD-tree, and while I do have an array of of all the nodes, and there are edges that can be calculated between subsets of the array, but the edges are not in any useful order. Can anyone give me some tips on how I'd retrieve a path from the starting node in the first array, and the ending node in the second array? Are there any useful data structures that would let me do this? Below is my code

def makeLine(distance, q_near, xrand, nodes):
    num = int((distance)/0.01)

    for i in range(1, num+1):
        qnext = (xrand - q_near)/distance * 0.01 * i + q_near
        #check for collision at qnext, if no collision detected:
            nodes = numpy.append(nodes, qnext)
        #else if there is collision, return 0, nodes, ((xrand - q_near)/distance*0.01*(i-1)+q_near
    return 1, nodes, qnext

def BIRRT(start, goal):

    startNode = numpy.array([start])
    goalNode = numpy.array([goal])
    limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
    for i in range(1, 10000):
        xrand = numpy.array([])
        for k in range(0, len(limits)):
            xrand = numpy.append(xrand, random.uniform(limits[k,:][0], limits[k,:][1]))

        kdTree = scipy.spatial.KDTree(startNode[:, 0:7])
        distance, index = kdTree.query(xrand)
        q_near = kdTree.data[index]     


        success, startNode, qFinal = makeLine(distance, q_near, xrand, startNode)

        kdTree2 = scipy.spatial.KDTree(goalNode[:, 0:7])
        distance2, index2 = kdTree2.query(qFinal)
        q_near2 = kdTree2.data[index2]

        success, startNode, qFinal2 = makeLine(distance2, qFinal, q_near2, startNode)

        if success:
            return 1, startNode, goalNode, 1, qFinal, qFinal2

        xrand = numpy.array([])
        for k in range(0, len(limits)):
            xrand = numpy.append(xrand, random.uniform(limits[k,:][0], limits[k,:][1]))

        kdTree = scipy.spatial.KDTree(goalNode[:, 0:7])
        distance, index = kdTree.query(xrand)
        q_near = kdTree.data[index]     

        success, goalNode, qFinal = makeLine(distance, q_near, xrand, goalNode)

        kdTree2 = scipy.spatial.KDTree(startNode[:, 0:7])
        distance2, index2 = kdTree2.query(qFinal)
        q_near2 = kdTree2.data[index2]

        success, goalNode, qFinal2 = makeLine(distance2, qFinal, q_near2, goalNode)

        if success:
            return 1, startNode, goalNode, 2, qFinal, qFinal2
    return 0

Answer 1

I got a working solution using both a KD-tree and a Graph (iGraph) provided by Python. Below is the working code:

def makeLine(distance, q_near, index, xrand, nodes, graph, elem):
    num = int((distance)/0.01)

    for i in range(1, num+1):
        qnext = (xrand - q_near)/distance * 0.01 * i + q_near
        #check for collision at qnext, if no collision detected:
        nodes = numpy.vstack([nodes, qnext])
        graph.add_vertex(len(nodes)-1)
        if i == 1 and elem == 1:
            graph.add_edge(index, len(nodes)-1)
        else:
            graph.add_edge(len(nodes)-2, len(nodes)-1)
        #else if there is collision, return 0, nodes, ((xrand - q_near)/distance*0.01*(i-1)+q_near
    return 1, nodes, qnext, graph

def BIRRT(start, goal):
    gStart = Graph()
    gGoal = Graph()
    gStart.add_vertex(0)
    gGoal.add_vertex(0)
    startNode = start 
    goalNode = goal 
    limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
    for i in range(1, 10000):
        xrand = numpy.array([])
        for k in range(0, len(limits)):
            xrand = numpy.append(xrand, random.uniform(limits[k,:][0], limits[k,:][1]))

        kdTree = scipy.spatial.KDTree(startNode[:, 0:7])
        distance, index = kdTree.query(xrand)
        q_near = kdTree.data[index]     
        index = numpy.where(numpy.all(startNode==q_near, axis=1))[0][0]

        success, startNode, qFinal, gStart = makeLine(distance, q_near, index, xrand, startNode, gStart, 1)

        kdTree2 = scipy.spatial.KDTree(goalNode[:, 0:7])
        distance2, index2 = kdTree2.query(qFinal)
        q_near2 = kdTree2.data[index2]
        index = numpy.where(numpy.all(goalNode==q_near2, axis=1))[0][0]
        success, startNode, qFinal2, gStart = makeLine(distance2, qFinal, q_near2, startNode, gStart, 0)

        if success:
            NodePath = numpy.array(start)
            graphPath = []
            graphPath.append(gStart.get_all_shortest_paths(0, numpy.where(numpy.all(startNode==qFinal2, axis=1))[0][0])[0])
            k = len(graphPath)
            graphPath.append(gGoal.get_all_shortest_paths(i, 0)[0]) 
            for i in range(0, len(graphPath)-1):
                if i < k:
                    NodePath = numpy.vstack((NodePath, startNode[graphPath[i]]))
                else:
                    NodePath = numpy.vstack((Nodepath, goalNode[graphPath[i]]))
            NodePath = numpy.vstack((NodePath, goal))
            return 1, NodePath

        xrand = numpy.array([])
        for k in range(0, len(limits)):
            xrand = numpy.append(xrand, random.uniform(limits[k,:][0], limits[k,:][1]))

        kdTree = scipy.spatial.KDTree(goalNode[:, 0:7])
        distance, index = kdTree.query(xrand)
        q_near = kdTree.data[index]     
        index = numpy.where(numpy.all(goalNode==q_near, axis=1))[0][0]
        success, goalNode, qFinal, gGoal = makeLine(distance, q_near, index, xrand, goalNode, gGoal, 1)

        kdTree2 = scipy.spatial.KDTree(startNode[:, 0:7])
        distance2, index2 = kdTree2.query(qFinal)
        q_near2 = kdTree2.data[index2]
        index = numpy.where(numpy.all(startNode==q_near2, axis=1))[0][0]
        success, goalNode, qFinal2, gGoal = makeLine(distance2, qFinal, index, q_near2, goalNode, gGoal, 0)

        if success:
            NodePath = numpy.array(start)
            graphPath = []
            graphPath.append(gStart.get_all_shortest_paths(0, i)[0])
            k = len(graphPath)
            graphPath.append(gGoal.get_all_shortest_paths(numpy.where(numpy.all(goalNode==qFinal2, axis=1))[0][0], 0)[0])

            for i in range(0, len(graphPath)-1):
                if i < k:
                    NodePath = numpy.vstack(NodePath, startNode[graphPath[i]])
                else:
                    NodePath = numpy.vstack(NodePath, goalNode[graphPath[i]])
            NodePath = numpy.vstack((NodePath, goal))
            return 1, NodePath
    return 0        

It's pretty messy, yes but it works for now. Now I need to figure out how to determine if a joint angle configuration will cause a collision in Gazebo...