1
$\begingroup$

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:

BIRRT pseudocode

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
$\endgroup$
  • 1
    $\begingroup$ I'm currently implementing a graph to store the BiRRT, with each vertex being labelled with its position in the array. I will post my results when finished in the hopes somebody can use this. $\endgroup$ – Iche Dec 20 '15 at 23:31
0
$\begingroup$

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...

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.