Merge branch 'master' of https://github.com/zhm-real/PathPlanning

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py

@@ -0,0 +1,121 @@
+"""
+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, isinbound
+from rrt_3D.rrt3D import rrt
+
+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 = 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, xrobot):
+        self.TrimRRT()
+        self.GrowRRT(xrobot = xrobot)
+
+    def TrimRRT(self):
+        S = []
+        i = 1
+        for qi in self.V:
+            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, mode):
+        E = self.FindAffectedEdges(obstacle, mode)
+        for e in E:
+            qe = self.ChildEndpointNode(e)
+            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, Reversed = True, xrobot = None):
+        # rrt.run()
+        self.run(Reversed = Reversed, xrobot = xrobot)
+
+    def CreateTreeFromNodes(self, S):
+        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 ChildEndpointNode(self, e):
+        # if self.E.isEndNode(e[1]):
+        return e[1]
+        #else: 
+        #    return self.ChildEndpointNode(e[1])
+
+if __name__ == '__main__':
+    rrt = dynamic_rrt_3D()
+    rrt.Main()

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -14,52 +14,74 @@ 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, \
-    hash3D, dehash
+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()
         self.i = 0
         self.maxiter = 10000
-        self.stepsize = 1.0
+        self.stepsize = 0.5
         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[hash3D(x)] = y
+        self.Parent[x] = y
 
-    def run(self):
-        self.V.append(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) > 1:
+    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)
-            if not isCollide(self, xnearest, xnew):
+            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)
-                visualization(self)
+
+                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 getDist(xnew, self.env.goal) <= 1:
-                self.wireup(self.env.goal, xnew)
-                self.Path, D = path(self)
-                print('Total distance = ' + str(D))
-        self.done = True
-        visualization(self)
-        plt.show()
+            # if the goal is really reached
+            
+        # 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

@@ -13,8 +13,7 @@ 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, \
-    hash3D, dehash
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
 
 
 class rrtstar():
@@ -24,70 +23,79 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 10000  # at least 4000 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.eta = 2*self.stepsize
         self.Path = []
         self.done = False
+        self.x0 = tuple(self.env.start)
+        self.xt = tuple(self.env.goal)
 
-    def wireup(self, x, y):
-        self.E.add_edge([x, y])  # add edge
-        self.Parent[hash3D(x)] = y
+        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
 
-    def removewire(self, xnear):
-        xparent = self.Parent[hash3D(xnear)]
-        a = [xnear, xparent]
-        self.E.remove_edge(a)  # remove and replace old the connection
+    def removewire(self,xnear):
+        xparent = self.Parent[xnear]
+        a = [xnear,xparent]
+        self.E.remove_edge(a) # remove and replace old the connection
 
     def reached(self):
         self.done = True
-        xn = near(self, self.env.goal)
-        c = [cost(self, x) for x in xn]
+        goal = self.xt
+        xn = near(self,self.env.goal)
+        c = [cost(self,tuple(x)) for x in xn]
         xncmin = xn[np.argmin(c)]
-        self.wireup(self.env.goal, xncmin)
-        self.V.append(self.env.goal)
-        self.Path, self.D = path(self)
+        self.wireup(goal , tuple(xncmin))
+        self.V.append(goal)
+        self.Path,self.D = path(self)
 
     def run(self):
-        self.V.append(self.env.start)
-        self.ind = 0
-        xnew = self.env.start
+        xnew = self.x0
         print('start rrt*... ')
-        self.fig = plt.figure(figsize=(10, 8))
+        self.fig = plt.figure(figsize = (10,8))
         while self.ind < self.maxiter:
-            xrand = sampleFree(self)
-            xnearest = nearest(self, xrand)
-            xnew = steer(self, xnearest, xrand)
-            if not isCollide(self, xnearest, xnew):
-                Xnear = near(self, xnew)
-                self.V.append(xnew)  # add point
-                visualization(self)
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            collide, _ = isCollide(self,xnearest,xnew)
+            if not collide:
+                Xnear = near(self,xnew)
+                self.V.append(xnew) # add point
+                # visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path
                 for xnear in Xnear:
+                    xnear = tuple(xnear)
                     c1 = cost(self, xnear) + getDist(xnew, xnear)
-                    if not isCollide(self, xnew, xnear) and c1 < cmin:
+                    collide, _ = isCollide(self, xnew, xnear)
+                    if not collide and c1 < cmin:
                         xmin, cmin = xnear, c1
                 self.wireup(xnew, xmin)
                 # rewire
                 for xnear in Xnear:
+                    xnear = tuple(xnear)
                     c2 = cost(self, xnew) + getDist(xnew, xnear)
-                    if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear):
+                    collide, _ = isCollide(self, xnew, xnear)
+                    if not collide and c2 < cost(self, xnear):
                         self.removewire(xnear)
                         self.wireup(xnear, xnew)
                 self.i += 1
             self.ind += 1
         # max sample reached
         self.reached()
-        print('time used = ' + str(time.time() - starttime))
-        print('Total distance = ' + str(self.D))
+        print('time used = ' + str(time.time()-starttime))
+        print('Total distance = '+str(self.D))
         visualization(self)
         plt.show()
-
+        
 
 if __name__ == '__main__':
     p = rrtstar()
     starttime = time.time()
     p.run()
+    

Sampling-based Planning/rrt_3D/utils3D.py

@@ -37,77 +37,152 @@ 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:
-            return initparams.env.goal + 1
+        if i < bias:
+            return np.array(initparams.xt) + 1
         else:
-            return np.array(x)
-        return np.array(x)
-
+            return x
+        return x
 
+# ---------------------- Collision checking algorithms
 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, 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 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 isinball(i, x, factor = 0):
+    if getDist(i[0:3], x) <= i[3] + factor:
         return True
     return False
 
+def lineSphere(p0, p1, ball):
+    # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf
+    c, r = ball[0:3], ball[-1]
+    line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]]
+    d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]]
+    t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * (
+                line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2])
+    if t <= 0:
+        if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True
+    elif t >= 1:
+        d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]]
+        if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True
+    elif 0 < t < 1:
+        x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]]
+        k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]]
+        if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True
+    return False
 
-def isCollide(initparams, x, y):
+def lineAABB(p0, p1, dist, aabb):
+    # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
+    # aabb should have the attributes of P, E as center point and extents
+    mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2]  # mid point
+    I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist]  # unit direction
+    hl = dist / 2  # radius
+    T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]]
+    # do any of the principal axis form a separting axis?
+    if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False
+    if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False
+    if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False
+    # I.cross(x axis) ?
+    r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])
+    if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
+    # I.cross(y axis) ?
+    r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0])
+    if abs(T[2] * I[0] - T[0] * I[2]) > r: return False
+    # I.cross(z axis) ?
+    r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0])
+    if abs(T[0] * I[1] - T[1] * I[0]) > r: return False
+
+    return True
+
+def lineOBB(p0, p1, dist, obb):
+    # transform points to obb frame
+    res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T 
+    # record old position and set the position to origin
+    oldP, obb.P= obb.P, [0,0,0] 
+    # calculate segment-AABB testing
+    ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb)
+    # reset the position
+    obb.P = oldP 
+    return ans
+
+def isCollide(initparams, x, child, dist=None):
     '''see if line intersects obstacle'''
-    ray = getRay(x, y)
-    dist = getDist(x, y)
-    if not isinbound(initparams.env.boundary, y):
-        return True
-    for i in getAABB(initparams.env.blocks):
-        shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
-        if shot is not None:
-            dist_wall = getDist(x, shot)
-            if dist_wall <= dist:  # collide
-                return True
+    '''specified for expansion in A* 3D lookup table'''
+    if dist==None:
+        dist = getDist(x, child)
+    # check in bound
+    if not isinbound(initparams.env.boundary, child): 
+        return True, dist
+    # check collision in AABB
+    for i in range(len(initparams.env.AABB)):
+        if lineAABB(x, child, dist, initparams.env.AABB[i]): 
+            return True, dist
+    # check collision in ball
     for i in initparams.env.balls:
-        shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
-        if shot != []:
-            dists_ball = [getDist(x, j) for j in shot]
-            if all(dists_ball <= dist):  # collide
-                return True
-    return False
-
-
+        if lineSphere(x, child, i): 
+            return True, dist
+    # check collision with obb
+    for i in initparams.env.OBB:
+        if lineOBB(x, child, dist, i):
+            return True, dist
+    return False, dist
+
+# ---------------------- leaf node extending algorithms
 def nearest(initparams, x):
     V = np.array(initparams.V)
     if initparams.i == 0:
         return initparams.V[0]
     xr = repmat(x, len(V), 1)
     dists = np.linalg.norm(xr - V, axis=1)
-    return initparams.V[np.argmin(dists)]
-
-
-def steer(initparams, x, y):
-    direc = (y - x) / np.linalg.norm(y - x)
-    xnew = x + initparams.stepsize * direc
-    return xnew
-
+    return tuple(initparams.V[np.argmin(dists)])
 
 def near(initparams, x):
+    x = np.array(x)
+    V = np.array(initparams.V)
     cardV = len(initparams.V)
     eta = initparams.eta
     gamma = initparams.gamma
     r = min(gamma * (np.log(cardV) / cardV), eta)
-    if initparams.done: r = 1
-    V = np.array(initparams.V)
+    if initparams.done: 
+        r = 1
     if initparams.i == 0:
         return [initparams.V[0]]
     xr = repmat(x, len(V), 1)
@@ -115,52 +190,80 @@ def near(initparams, x):
     nearpoints = V[inside]
     return np.array(nearpoints)
 
+def steer(initparams, x, y):
+    dist, step = getDist(y, x), initparams.stepsize
+    increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)
+    xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
+    # direc = (y - x) / np.linalg.norm(y - x)
+    # xnew = x + initparams.stepsize * direc
+    return xnew
 
 def cost(initparams, x):
     '''here use the additive recursive cost function'''
-    if all(x == initparams.env.start):
+    if x == initparams.x0:
         return 0
-    xparent = initparams.Parent[hash3D(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):
-    x = initparams.env.goal
-    while not all(x == initparams.env.start):
-        x2 = initparams.Parent[hash3D(x)]
+    x = initparams.xt
+    while x != initparams.x0:
+        x2 = initparams.Parent[x]
         Path.append(np.array([x, x2]))
         dist += getDist(x, x2)
         x = x2
     return Path, dist
 
-
-def hash3D(x):
-    return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2])
-
-
-def dehash(x):
-    return np.array([float(i) for i in x.split(' ')])
-
-
 class edgeset(object):
     def __init__(self):
         self.E = {}
 
     def add_edge(self, edge):
-        x, y = hash3D(edge[0]), hash3D(edge[1])
+        x, y = edge[0], edge[1]
         if x in self.E:
-            self.E[x].append(y)
+            self.E[x].add(y)
         else:
-            self.E[x] = [y]
+            self.E[x] = set()
+            self.E[x].add(y)
 
     def remove_edge(self, edge):
         x, y = edge[0], edge[1]
-        self.E[hash3D(x)].remove(hash3D(y))
+        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((dehash(v), dehash(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/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):
@@ -197,7 +199,7 @@ class Anytime_Dstar(object):
             self.INCONS = set()
             self.CLOSED = set()
             self.ComputeorImprovePath()
-            # TODO publish current epsilon sub optimal solution
+            # publish current epsilon sub optimal solution
             self.Path = self.path()
             # if epsilon == 1:
             # wait for change to occur
@@ -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,29 +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)
-                    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[:,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
 
 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)