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

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,:], limits[k,:]))

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,:], limits[k,:]))

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

• 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.
– Iche
Dec 20, 2015 at 23:31

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])
if i == 1 and elem == 1:
else:
#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()
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,:], limits[k,:]))

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

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))
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))))
k = len(graphPath)
graphPath.append(gGoal.get_all_shortest_paths(i, 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,:], limits[k,:]))

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))
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))
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))
k = len(graphPath)
graphPath.append(gGoal.get_all_shortest_paths(numpy.where(numpy.all(goalNode==qFinal2, axis=1)), 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...