'update'

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py

@@ -0,0 +1,70 @@
+"""
+This is dynamic rrt code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from collections import defaultdict
+import time
+import matplotlib.pyplot as plt
+
+import os
+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
+
+class dynamic_rrt_3D:
+    
+    def __init__(self):
+        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.stepsize = 0.5
+        self.gamma = 500
+        self.eta = 2*self.stepsize
+        self.Path = []
+        self.done = False
+
+    def RegrowRRT(self):
+        self.TrimRRT()
+        self.GrowRRT()
+
+    def TrimRRT(self):
+        S = set()
+        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)
+            i += 1
+        self.V, self.E = self.CreateTreeFromNodes(S)
+    
+    def InvalidateNodes(self, obstacle):
+        E = self.FindAffectedEdges(obstacle)
+        for e in E:
+            qe = self.ChildEndpointNode(e)
+            qe.flag = 'Invalid'
+
+        
+    def GrowRRT(self):
+        # TODO
+        pass
+            
+    def CreateTreeFromNodes(self, S):
+        #TODO
+        pass
+
+    def FindAffectedEdges(self, obstacle):
+        #TODO
+        pass
+
+    def ChildEndpointNode(self):
+        #TODO
+        pass

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -29,6 +29,7 @@ class rrtstar():
         self.stepsize = 0.5
         self.Path = []
         self.done = False
+        self.x0 = tuple(self.env.start)
 
     def wireup(self, x, y):
         self.E.add_edge([x, y])  # add edge

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -23,12 +23,13 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 2000 # at least 2000 in this env
+        self.maxiter = 12000 # 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)
 
     def wireup(self,x,y):
         self.E.add_edge([x,y]) # add edge
@@ -52,7 +53,7 @@ class rrtstar():
     def run(self):
         self.V.append(tuple(self.env.start))
         self.ind = 0
-        xnew = tuple(self.env.start)
+        xnew = self.x0
         print('start rrt*... ')
         self.fig = plt.figure(figsize = (10,8))
         while self.ind < self.maxiter:

Sampling-based Planning/rrt_3D/utils3D.py

@@ -37,14 +37,14 @@ def getDist(pos1, pos2):
 '''
 
 
-def sampleFree(initparams):
+def sampleFree(initparams, bias = 0.1):
     '''biased sampling'''
     x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])
     i = np.random.random()
     if isinside(initparams, x):
         return sampleFree(initparams)
     else:
-        if i < 0.1:
+        if i < bias:
             return initparams.env.goal + 1
         else:
             return x
@@ -172,10 +172,9 @@ def steer(initparams, x, y):
 
 def cost(initparams, x):
     '''here use the additive recursive cost function'''
-    if x == tuple(initparams.env.start):
+    if x == initparams.x0:
         return 0
-    xparent = initparams.Parent[x]
-    return cost(initparams, xparent) + getDist(x, xparent)
+    return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x])
 
 
 def path(initparams, Path=[], dist=0):

Search-based Planning/Search_3D/Anytime_Dstar3D.py

@@ -91,28 +91,30 @@ class Anytime_Dstar(object):
         # scan graph for changed cost, if cost is changed update it
         CHANGED = set()
         for xi in self.CLOSED:
-            if xi in self.CHILDREN:
-                oldchildren = self.CHILDREN[xi]  # A
-                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
-                    newchildren = set(children(self, xi))  # B
-                    removed = oldchildren.difference(newchildren)
-                    intersection = oldchildren.intersection(newchildren)
-                    added = newchildren.difference(oldchildren)
-                    self.CHILDREN[xi] = newchildren
-                    for xj in removed:
-                        self.COST[xi][xj] = cost(self, xi, xj)
-                    for xj in intersection.union(added):
-                        self.COST[xi][xj] = cost(self, xi, xj)
-                    CHANGED.add(xi)
-            else:
-                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
-                    CHANGED.add(xi)
-                    children_added = set(children(self, xi))
-                    self.CHILDREN[xi] = children_added
-                    for xj in children_added:
-                        self.COST[xi][xj] = cost(self, xi, xj)
+            if isinbound(old, xi, mode) or isinbound(new, xi, mode):
+                newchildren = set(children(self, xi))  # B
+                self.CHILDREN[xi] = newchildren
+                for xj in newchildren:
+                    self.COST[xi][xj] = cost(self, xi, xj)
+                CHANGED.add(xi)
         return CHANGED
 
+    # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False):
+    #     # TODO scan graph for changed cost, if cost is changed update it
+    #     # make the graph cost via vectorization
+    #     CHANGED = set()
+    #     Allnodes = np.array(list(self.CLOSED))
+    #     isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \
+    #                 isinbound(new, Allnodes, mode = mode, isarray = True)
+    #     Changednodes = Allnodes[isChanged]
+    #     for xi in Changednodes:
+    #         xi = tuple(xi)
+    #         CHANGED.add(xi)
+    #         self.CHILDREN[xi] = set(children(self, xi))
+    #         for xj in self.CHILDREN:
+    #             self.COST[xi][xj] = cost(self, xi, xj)
+        
+
     # --------------main functions for Anytime D star
 
     def key(self, s, epsilon=1):
@@ -228,5 +230,5 @@ class Anytime_Dstar(object):
 
 
 if __name__ == '__main__':
-    AD = Anytime_Dstar(resolution=0.5)
+    AD = Anytime_Dstar(resolution=1)
     AD.Main()

Search-based Planning/Search_3D/DstarLite3D.py

@@ -50,30 +50,16 @@ class D_star_Lite(object):
         self.Path = []
         self.done = False
 
-    def updatecost(self,range_changed=None, new=None, old=None, mode=False):
+    def updatecost(self, range_changed=None, new=None, old=None, mode=False):
         # scan graph for changed cost, if cost is changed update it
         CHANGED = set()
         for xi in self.CLOSED:
-            if xi in self.CHILDREN:
-                oldchildren = self.CHILDREN[xi]# A
-                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
-                    newchildren = set(children(self,xi))# B
-                    removed = oldchildren.difference(newchildren)
-                    intersection = oldchildren.intersection(newchildren)
-                    added = newchildren.difference(oldchildren)
-                    self.CHILDREN[xi] = newchildren
-                    for xj in removed:
-                        self.COST[xi][xj] = cost(self, xi, xj)
-                    for xj in intersection.union(added):
-                        self.COST[xi][xj] = cost(self, xi, xj)
-                    CHANGED.add(xi)
-            else: 
-                if isinbound(old, xi, mode) or isinbound(new, xi, mode):
-                    CHANGED.add(xi)
-                    children_added = set(children(self,xi))
-                    self.CHILDREN[xi] = children_added
-                    for xj in children_added:
-                        self.COST[xi][xj] = cost(self, xi, xj)
+            if isinbound(old, xi, mode) or isinbound(new, xi, mode):
+                newchildren = set(children(self, xi))  # B
+                self.CHILDREN[xi] = newchildren
+                for xj in newchildren:
+                    self.COST[xi][xj] = cost(self, xi, xj)
+                CHANGED.add(xi)
         return CHANGED
 
     def getcost(self, xi, xj):

Search-based Planning/Search_3D/utils3D.py

@@ -39,23 +39,32 @@ def heuristic_fun(initparams, k, t=None):
         t = initparams.goal
     return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
 
-def isinbound(i, x, mode=False, factor = 0):
+def isinbound(i, x, mode = False, factor = 0, isarray = False):
     if mode == 'obb':
-        return isinobb(i, x)
-    if 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:
-        return True
-    return 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) 
+        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
 
 def isinball(i, x, factor = 0):
     if getDist(i[0:3], x) <= i[3] + factor:
         return True
     return False
 
-def isinobb(i, x):
+def isinobb(i, x, isarray = False):
     # transform the point from {W} to {body}
-    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)
+    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 OBB2AABB(obb):
     # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
@@ -244,7 +253,6 @@ def StateSpace(env, factor=0):
                 Space.add((x, y, z))
     return Space
 
-
 def g_Space(initparams):
     '''This function is used to get nodes and discretize the space.
        State space is by x*y*z,3 where each 3 is a point in 3D.'''
@@ -254,7 +262,6 @@ def g_Space(initparams):
         g[v] = np.inf  # this hashmap initialize all g values at inf
     return g
 
-
 def isCollide(initparams, x, child, dist):
     '''see if line intersects obstacle'''
     '''specified for expansion in A* 3D lookup table'''
@@ -277,7 +284,6 @@ def isCollide(initparams, x, child, dist):
             return True, dist
     return False, dist
 
-
 def children(initparams, x, settings = 0):
     # get the neighbor of a specific state
     allchild = []
@@ -299,7 +305,6 @@ def children(initparams, x, settings = 0):
     if settings == 1:
         return allcost
 
-
 def obstacleFree(initparams, x):
     for i in initparams.env.blocks:
         if isinbound(i, x):
@@ -345,11 +350,9 @@ if __name__ == "__main__":
     obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45))
     # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])
     p0, p1 = [2.9,2.5,1],[1.9,2.5,1]
-    dist = getDist(p0,p1)
+    pts = np.array([[1,2,3],[4,5,6],[7,8,9],[2,2,2],[1,1,1],[3,3,3]])
     start = time.time()
-    for i in range(3000*27):
-        lineAABB(p0,p1,dist,obb1)
-        #lineOBB(p0,p1,dist,obb1)
+    isinbound(obb1, pts, mode='obb', factor = 0, isarray = True)
     print(time.time() - start)