'dynamicrrt'

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py

@@ -13,58 +13,109 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, isinbound
+from rrt_3D.rrt3D import rrt
 
-class dynamic_rrt_3D:
+class dynamic_rrt_3D(rrt):
     
     def __init__(self):
+        # variables in rrt
         self.env = env()
         self.Parent = {}
         self.E = edgeset() # edgeset
         self.V = [] # nodeset
         self.i = 0
-        self.maxiter = 2000 # at least 2000 in this env
+        self.maxiter = 10000 # at least 2000 in this env
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = 2*self.stepsize
         self.Path = []
         self.done = False
+        self.x0 = tuple(self.env.goal)
+        self.xt = tuple(self.env.start)
+        # additional variables
+        self.Flag = {}
+        self.xrobot = tuple(self.env.start)
+        
+        self.V.append(self.x0)
+        self.ind = 0
+        self.fig = plt.figure(figsize=(10, 8))
 
-    def RegrowRRT(self):
+    def RegrowRRT(self, xrobot):
         self.TrimRRT()
-        self.GrowRRT()
+        self.GrowRRT(xrobot = xrobot)
 
     def TrimRRT(self):
-        S = set()
+        S = []
         i = 1
         for qi in self.V:
-            qp = self.Parent(qi)
-            if qp.flag == 'Invalid':
-                qi.flag = 'Invalid'
-            if qi.flag != 'Invalid':
-                S.add(qi)
+            if qi == self.x0:
+                continue
+            qp = self.Parent[qi]
+            if qp == self.x0:
+                continue
+            if self.Flag[qp] == 'Invalid':
+                self.Flag[qi] = 'Invalid'
+                self.E.remove_edge([qi,qp]) # REMOVE edge that parent is invalid
+            if self.Flag[qi] != 'Invalid':
+                S.append(qi)
             i += 1
         self.V, self.E = self.CreateTreeFromNodes(S)
     
-    def InvalidateNodes(self, obstacle):
-        E = self.FindAffectedEdges(obstacle)
+    def InvalidateNodes(self, obstacle, mode):
+        E = self.FindAffectedEdges(obstacle, mode)
         for e in E:
             qe = self.ChildEndpointNode(e)
-            qe.flag = 'Invalid'
+            self.Flag[qe] = 'Invalid'
 
+    def Main(self):
+        qgoal = tuple(self.env.start)
+        qstart = tuple(self.env.goal)
+        self.GrowRRT()
+        self.done = True
+        # visualization(self)
+        self.done = False
+        # change the enviroment
+        new0,old0 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=1, mode='translation')
+        while qgoal != qstart:
+            qgoal = self.Parent[qgoal]
+            # TODO move to qgoal and check for new obs
+            xrobot = qstart
+            # TODO if any new obstacle are observed
+            self.InvalidateNodes(new0, mode = 'translation')
+            for xi in self.Path:
+                if self.Flag[tuple(xi[0])] == 'Invalid':
+                    self.RegrowRRT(tuple(self.env.start))
+        self.done = True
+        self.Path, D = path(self)
+        visualization(self)
+        plt.show()
         
-    def GrowRRT(self):
-        # TODO
-        pass
-            
+    def GrowRRT(self, Reversed = True, xrobot = None):
+        # rrt.run()
+        self.run(Reversed = Reversed, xrobot = xrobot)
+
     def CreateTreeFromNodes(self, S):
-        #TODO
-        pass
+        return S, self.E
+
+    def FindAffectedEdges(self, obstacle, mode):
+        # if the nodes appears inside of the new range an obstacle moved, 
+        # find the edges associated with this node. (the node is the parent)
+        nodes = np.array(self.V)
+        if mode == 'translation':
+            affected_nodes = nodes[isinbound(obstacle, nodes, isarray = True)]
+        elif mode == 'rotation':
+            affected_nodes = nodes[isinbound(obstacle, nodes, mode = 'obb', isarray = True)]
+        if len(affected_nodes) == 0:
+            return []
+        return self.E.get_edge(affected_nodes)
 
-    def FindAffectedEdges(self, obstacle):
-        #TODO
-        pass
+    def ChildEndpointNode(self, e):
+        # if self.E.isEndNode(e[1]):
+        return e[1]
+        #else: 
+        #    return self.ChildEndpointNode(e[1])
 
-    def ChildEndpointNode(self):
-        #TODO
-        pass
+if __name__ == '__main__':
+    rrt = dynamic_rrt_3D()
+    rrt.Main()

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -17,10 +17,9 @@ from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
-class rrtstar():
+class rrt():
     def __init__(self):
         self.env = env()
-        # self.Parent = defaultdict(lambda: defaultdict(dict))
         self.Parent = {}
         self.V = []
         self.E = edgeset()
@@ -30,42 +29,59 @@ class rrtstar():
         self.Path = []
         self.done = False
         self.x0 = tuple(self.env.start)
+        self.xt = tuple(self.env.goal)
+        self.Flag = None
+
+        
+        self.ind = 0
+        self.fig = plt.figure(figsize=(10, 8))
 
     def wireup(self, x, y):
         self.E.add_edge([x, y])  # add edge
         self.Parent[x] = y
 
-    def run(self):
-        self.V.append(tuple(self.env.start))
-        self.ind = 0
-        self.fig = plt.figure(figsize=(10, 8))
-        xnew = self.env.start
-        while self.ind < self.maxiter and getDist(xnew, self.env.goal) > self.stepsize:
+    def run(self, Reversed = True, xrobot = None):
+        if Reversed:
+            if xrobot is None:
+                self.x0 = tuple(self.env.goal)
+                self.xt = tuple(self.env.start)
+            else:
+                self.x0 = tuple(self.env.goal)
+                self.xt = xrobot
+            xnew = self.env.goal
+        else:
+            xnew = self.env.start
+        self.V.append(self.x0)
+        while self.ind < self.maxiter:
             xrand = sampleFree(self)
             xnearest = nearest(self, xrand)
             xnew = steer(self, xnearest, xrand)
             collide, _ = isCollide(self, xnearest, xnew)
             if not collide:
                 self.V.append(xnew)  # add point
+                if self.Flag is not None:
+                    self.Flag[xnew] = 'Valid'
                 self.wireup(xnew, xnearest)
 
-                if getDist(xnew, self.env.goal) <= self.stepsize:
-                    goal = tuple(self.env.goal)
-                    self.wireup(goal, xnew)
+                if getDist(xnew, self.xt) <= self.stepsize:
+                    self.wireup(self.xt, xnew)
                     self.Path, D = path(self)
                     print('Total distance = ' + str(D))
+                    if self.Flag is not None:
+                        self.Flag[self.xt] = 'Valid'
+                    break
                 # visualization(self)
                 self.i += 1
             self.ind += 1
             # if the goal is really reached
             
-        self.done = True
-        visualization(self)
-        plt.show()
+        # self.done = True
+        # visualization(self)
+        # plt.show()
 
 
 if __name__ == '__main__':
-    p = rrtstar()
+    p = rrt()
     starttime = time.time()
     p.run()
     print('time used = ' + str(time.time() - starttime))

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -23,14 +23,17 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 12000 # at least 2000 in this env
+        self.maxiter = 5000 # at least 2000 in this env
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = 2*self.stepsize
         self.Path = []
         self.done = False
         self.x0 = tuple(self.env.start)
+        self.xt = tuple(self.env.goal)
 
+        self.V.append(self.x0)
+        self.ind = 0
     def wireup(self,x,y):
         self.E.add_edge([x,y]) # add edge
         self.Parent[x] = y
@@ -42,7 +45,7 @@ class rrtstar():
 
     def reached(self):
         self.done = True
-        goal = tuple(self.env.goal)
+        goal = self.xt
         xn = near(self,self.env.goal)
         c = [cost(self,tuple(x)) for x in xn]
         xncmin = xn[np.argmin(c)]
@@ -51,8 +54,6 @@ class rrtstar():
         self.Path,self.D = path(self)
 
     def run(self):
-        self.V.append(tuple(self.env.start))
-        self.ind = 0
         xnew = self.x0
         print('start rrt*... ')
         self.fig = plt.figure(figsize = (10,8))

Sampling-based Planning/rrt_3D/utils3D.py

@@ -45,7 +45,7 @@ def sampleFree(initparams, bias = 0.1):
         return sampleFree(initparams)
     else:
         if i < bias:
-            return initparams.env.goal + 1
+            return np.array(initparams.xt) + 1
         else:
             return x
         return x
@@ -54,12 +54,40 @@ def sampleFree(initparams, bias = 0.1):
 def isinside(initparams, x):
     '''see if inside obstacle'''
     for i in initparams.env.blocks:
-        if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+        if isinbound(i, x):
+            return True
+    for i in initparams.env.OBB:
+        if isinbound(i, x, mode = 'obb'):
+            return True
+    for i in initparams.env.balls:
+        if isinball(i, x):
             return True
     return False
 
-def isinbound(i, x):
-    if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+def isinbound(i, x, mode = False, factor = 0, isarray = False):
+    if mode == 'obb':
+        return isinobb(i, x, isarray)
+    if isarray:
+        compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) 
+        compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) 
+        compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) 
+        return compx & compy & compz
+    else:    
+        return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5]
+
+def isinobb(i, x, isarray = False):
+    # transform the point from {W} to {body}
+    if isarray:
+        pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3]
+        block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
+        return isinbound(block, pts, isarray = isarray)
+    else:
+        pt = i.T@np.append(x,1)
+        block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]
+        return isinbound(block, pt)
+
+def isinball(i, x, factor = 0):
+    if getDist(i[0:3], x) <= i[3] + factor:
         return True
     return False
 
@@ -178,8 +206,8 @@ def cost(initparams, x):
 
 
 def path(initparams, Path=[], dist=0):
-    x = tuple(initparams.env.goal)
-    while x != tuple(initparams.env.start):
+    x = initparams.xt
+    while x != initparams.x0:
         x2 = initparams.Parent[x]
         Path.append(np.array([x, x2]))
         dist += getDist(x, x2)
@@ -202,10 +230,40 @@ class edgeset(object):
         x, y = edge[0], edge[1]
         self.E[x].remove(y)
 
-    def get_edge(self):
+    def get_edge(self, nodes = None):
         edges = []
-        for v in self.E:
-            for n in self.E[v]:
-                # if (n,v) not in edges:
-                edges.append((v, n))
+        if nodes is None:
+            for v in self.E:
+                for n in self.E[v]:
+                    # if (n,v) not in edges:
+                    edges.append((v, n))
+        else: 
+            for v in nodes:
+                for n in self.E[tuple(v)]:
+                    edges.append((v, n))
         return edges
+
+    def isEndNode(self, node):
+        return node not in self.E
+
+
+class Node:
+    def __init__(self, data):
+        self.data = data
+        self.sibling = None
+        self.child = None
+
+class Tree:
+    def __init__(self, start):
+        self.root = Node(start)
+        self.ind = 0
+        self.index = {start:self.ind}
+
+    def add_edge(self, edge):
+        # y exists in the tree while x does not
+        x, y = edge[0], edge[1]
+
+        
+        
+
+

Search-based Planning/Search_3D/utils3D.py

@@ -44,8 +44,8 @@ def isinbound(i, x, mode = False, factor = 0, isarray = False):
         return isinobb(i, x, isarray)
     if isarray:
         compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) 
-        compy = (i[1] - factor <= x[:,1]) & (x[:,0] < i[4] + factor) 
-        compz = (i[2] - factor <= x[:,2]) & (x[:,0] < i[5] + factor) 
+        compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) 
+        compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) 
         return compx & compy & compz
     else:    
         return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor