In [1]:

    

%matplotlib inline
import matplotlib
import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.append('../../rai/rai/ry')
import libry as lry
import time
import copy
import random


    

In [2]:

    

class RRT:
    def __init__(self, q0, stepsize, K):
        self._stepsize = stepsize
        self.parent = []
        self.ann = []  # one could use a spatial data structure for speedup
        # add q0 to tree
        self.ann.append(q0)
        self.parent.append(0)  # q0 has itself as parent 
        self.lines = []
        self.F = K.feature(lry.FS.position, ["peg"])
            
    def getProposalTowards(self, q):
        array = np.array(self.ann)
        self.nearest = (np.linalg.norm(array - q, axis=1)).argmin()
        diff = q - self.ann[self.nearest]
        dist = np.linalg.norm(diff)
        qr = self.ann[self.nearest] + self._stepsize/dist * diff
        return dist, qr

    def add(self, q):
        self.ann.append(copy.deepcopy(q))
        self.parent.append(self.nearest)

    def addLineDraw(self, q, K):
        # We can't draw the edge in the 7-dim joint space!
        # But we can draw a projected edge in 3D endeffector position space
        K.setJointState(self.ann[self.nearest])
        y_from, J = self.F.eval(K)
        K.setJointState(q)
        y_to, J = self.F.eval(K)
        self.lines.append(y_from)
        self.lines.append(y_to)
    
    def getNearest(self):
        return self.nearest
    
    def getParent(self, i):
        return self.parent[i]
    
    def getNode(self, i):
        return self.ann[i]
    
    def getRandomNode(self):
        return random.choice(self.ann)
    
    def getNumberNodes(self):
        return len(self.ann)


    

In [3]:

    

K = lry.Config()
D = K.view()
K.clear();
K.addFile('pegInAHole.g')
q0 = K.getJointState()
qT = np.array([0.945499, 0.431195, -1.97155, 0.623969, 2.22355, -0.665206, -1.48356])
q = q0


    

In [4]:

    

K.setJointState(qT)  # this shows the goal position


    

In [5]:

    

K.setJointState(q0)  # this shows the start position


    

In [6]:

    

K.setJointState(q0) # start at q0

stepsize = .1

rrt0 = RRT(q0, stepsize, K)

K.addObject(name="lines0", shape=lry.ST.mesh)  # Add a mesh for line drawing to the world
lineframe = K.frame("lines0")

success = False
coll = K.feature(lry.FS.accumulatedCollisions, [])    

for i in range(0, 10000):
    
    # let rrt0 grow in random direction
    q = np.random.uniform(-2*np.pi, 2*np.pi, len(q))

    # compute q_new
    dist, q = rrt0.getProposalTowards(q)
    K.setJointState(q)

    # check if q is collision free
    K.computeCollisions()
    y_col = coll.eval(K)[0]    
    if y_col <= 1e-10:
        rrt0.add(q)
        rrt0.addLineDraw(q, K)

    # some output
    if i%100 == 0:
        K.setJointState(q);
        lineframe.setMeshAsLines(np.array(rrt0.lines).flatten())  # update the mesh with lines (sets all the lines)
        time.sleep(1)  # uncomment if you do not want to pause
        print("\rRRT samples=" + str(i) + " tree sizes = " + str(rrt0.getNumberNodes()))


    

    




---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
<ipython-input-6-0e5fad1710b9> in <module>
     21 
     22     # check if q is collision free
---> 23     K.computeCollisions()
     24     y_col = coll.eval(K)[0]
     25     if y_col <= 1e-10:

RuntimeError: kin_swift.cpp:SwiftInterface:82(-2) CHECK failed: 's->mesh().V.d0' the mesh must have been created earlier -- has size zero!

In [ ]:

    




    

In [ ]: