'changed'

Search-based Planning/Search_3D/Astar3D.py

@@ -12,7 +12,7 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
     cost
 from Search_3D.plot_util3D import visualization
 import queue
@@ -27,21 +27,21 @@ class Weighted_A_star(object):
                                   [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
 
         self.env = env(resolution=resolution)
-        self.Space = g_Space(self)  # key is the point, store g value
-        self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal)
+        self.g = g_Space(self)  # key is the point, store g value
+        self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
         # self.AABB = getAABB(self.env.blocks)
-        self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
+        self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
 
-        self.h = Heuristic(self.Space, self.goal)
+        self.h = Heuristic(self.g, self.goal)
         self.Parent = {}
         self.CLOSED = set()
         self.V = []
         self.done = False
         self.Path = []
         self.ind = 0
-        self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
+        self.x0, self.xt = self.start, self.goal
         self.OPEN = queue.QueuePrior()  # store [point,priority]
-        self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
         self.lastpoint = self.x0
 
     def children(self, x):
@@ -54,29 +54,27 @@ class Weighted_A_star(object):
 
     def run(self, N=None):
         xt = self.xt
-        strxi = self.x0
+        xi = self.x0
         while xt not in self.CLOSED and self.OPEN:  # while xt not reached and open is not empty
-            strxi = self.OPEN.get()
-            xi = dehash(strxi)
-            if strxi not in self.CLOSED:
-                self.V.append(xi)
-            self.CLOSED.add(strxi)  # add the point in CLOSED set
+            xi = self.OPEN.get()
+            if xi not in self.CLOSED:
+                self.V.append(np.array(xi))
+            self.CLOSED.add(xi)  # add the point in CLOSED set
             visualization(self)
             allchild = self.children(xi)
             for xj in allchild:
-                strxj = hash3D(xj)
-                if strxj not in self.CLOSED:
-                    gi, gj = self.Space[strxi], self.Space[strxj]
+                if xj not in self.CLOSED:
+                    gi, gj = self.g[xi], self.g[xj]
                     a = gi + cost(xi, xj)
                     if a < gj:
-                        self.Space[strxj] = a
-                        self.Parent[strxj] = xi
-                        if (a, strxj) in self.OPEN.enumerate():
+                        self.g[xj] = a
+                        self.Parent[xj] = xi
+                        if (a, xj) in self.OPEN.enumerate():
                             # update priority of xj
-                            self.OPEN.put(strxj, a + 1 * self.h[strxj])
+                            self.OPEN.put(xj, a + 1 * self.h[xj])
                         else:
                             # add xj in to OPEN set
-                            self.OPEN.put(strxj, a + 1 * self.h[strxj])
+                            self.OPEN.put(xj, a + 1 * self.h[xj])
             # For specified expanded nodes, used primarily in LRTA*
             if N:
                 if len(self.CLOSED) % N == 0:
@@ -84,7 +82,7 @@ class Weighted_A_star(object):
             if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V)))
             self.ind += 1
 
-        self.lastpoint = strxi
+        self.lastpoint = xi
         # if the path finding is finished
         if xt in self.CLOSED:
             self.done = True
@@ -98,22 +96,21 @@ class Weighted_A_star(object):
 
     def path(self):
         path = []
-        strx = self.lastpoint
-        # strstart = hash3D(getNearest(self.Space, self.env.start))
-        strstart = self.x0
-        while strx != strstart:
-            path.append([dehash(strx), self.Parent[strx]])
-            strx = hash3D(self.Parent[strx])
+        x = self.lastpoint
+        start = self.x0
+        while x != start:
+            path.append([x, self.Parent[x]])
+            x = self.Parent[x]
         # path = np.flip(path, axis=0)
         return path
 
     # utility used in LRTA*
     def reset(self, xj):
-        self.Space = g_Space(self)  # key is the point, store g value
+        self.g = g_Space(self)  # key is the point, store g value
         self.start = xj
-        self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
-        self.x0 = hash3D(xj)
-        self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
+        self.x0 = xj
+        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
         self.CLOSED = set()
 
         # self.h = h(self.Space, self.goal)

Search-based Planning/Search_3D/Dstar3D.py

@@ -12,14 +12,6 @@ from Search_3D.utils3D import StateSpace, getDist, getRay, isinbound, isinball
 import pyrr
 
 
-def getNearest(Space,pt):
-    '''get the nearest point on the grid'''
-    mindis,minpt = 1000,None
-    for pts in Space: 
-        dis = getDist(pts,pt)
-        if dis < mindis:
-            mindis,minpt = dis,pts
-    return minpt
 
 def isCollide(initparams, x, child):
     '''see if line intersects obstacle'''

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -25,9 +25,9 @@ class Lifelong_Astar(object):
         self.env = env(resolution=resolution)
         self.g = g_Space(self)
         self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
-        self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
+        self.x0, self.xt = self.start, self.goal
         self.v = g_Space(self) # rhs(.) = g(.) = inf
-        self.v[hash3D(self.start)] = 0 # rhs(x0) = 0
+        self.v[self.start] = 0 # rhs(x0) = 0
         self.h = Heuristic(self.g, self.goal)
         
         self.OPEN = queue.QueuePrior()  # store [point,priority]
@@ -50,26 +50,25 @@ class Lifelong_Astar(object):
 
     def costset(self):
         NodeToChange = set()
-        for strxi in self.CHILDREN.keys():
-            children = self.CHILDREN[strxi]
-            xi = dehash(strxi)
+        for xi in self.CHILDREN.keys():
+            children = self.CHILDREN[xi]
             toUpdate = [self.cost(xj,xi) for xj in children]
-            if strxi in self.COST:
+            if xi in self.COST:
                 # if the old cost not equal to new cost
-                diff = np.not_equal(self.COST[strxi],toUpdate)
+                diff = np.not_equal(self.COST[xi],toUpdate)
                 cd = np.array(children)[diff]
                 for i in cd:
-                    NodeToChange.add(hash3D(i))
-                self.COST[strxi] = toUpdate
+                    NodeToChange.add(tuple(i))
+                self.COST[xi] = toUpdate
             else:
-                self.COST[strxi] = toUpdate
+                self.COST[xi] = toUpdate
         return NodeToChange
 
-    def getCOSTset(self,strxi,xj):
-        ind, children = 0, self.CHILDREN[strxi]
+    def getCOSTset(self,xi,xj):
+        ind, children = 0, self.CHILDREN[xi]
         for i in children:
-            if all(i == xj):
-                return self.COST[strxi][ind]
+            if i == xj:
+                return self.COST[xi][ind]
             ind += 1
             
 
@@ -77,15 +76,14 @@ class Lifelong_Astar(object):
         allchild = []
         resolution = self.env.resolution
         for direc in self.Alldirec:
-            child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+            child = tuple(map(np.add,x,np.multiply(direc,resolution)))
             if isinbound(self.env.boundary,child):
                 allchild.append(child)
         return allchild
 
     def getCHILDRENset(self):
-        for strxi in self.g.keys():
-            xi = dehash(strxi)
-            self.CHILDREN[strxi] = self.children(xi)
+        for xi in self.g.keys():
+            self.CHILDREN[xi] = self.children(xi)
         
     def isCollide(self, x, child):
         ray , dist = getRay(x, child) ,  getDist(x, child)
@@ -112,56 +110,54 @@ class Lifelong_Astar(object):
         if collide: return np.inf
         else: return dist
             
-    def key(self,strxi,epsilion = 1):
-        return [min(self.g[strxi],self.v[strxi]) + epsilion*self.h[strxi],min(self.g[strxi],self.v[strxi])]
+    def key(self,xi,epsilion = 1):
+        return [min(self.g[xi],self.v[xi]) + epsilion*self.h[xi],min(self.g[xi],self.v[xi])]
 
     def path(self):
         path = []
-        strx = self.xt
-        strstart = self.x0
+        x = self.xt
+        start = self.x0
         ind = 0
-        while strx != strstart:
-            j = dehash(strx)
-            nei = self.CHILDREN[strx]
-            gset = [self.g[hash3D(xi)] for xi in nei]
+        while x != start:
+            j = x
+            nei = self.CHILDREN[x]
+            gset = [self.g[xi] for xi in nei]
             # collision check and make g cost inf
             for i in range(len(nei)):
                 if self.isCollide(nei[i],j)[0]:
                     gset[i] = np.inf
             parent = nei[np.argmin(gset)]
-            path.append([dehash(strx), parent])
-            strx = hash3D(parent)
+            path.append([x, parent])
+            x = parent
             if ind > 100:
                 break
             ind += 1
         return path
 
     #------------------Lifelong Plannning A* 
-    def UpdateMembership(self,strxi, xi, xparent=None):
-        if strxi != self.x0:
-            self.v[strxi] = min([self.g[hash3D(j)] + self.getCOSTset(strxi,j) for j in self.CHILDREN[strxi]])
-        self.OPEN.check_remove(strxi)
-        if self.g[strxi] != self.v[strxi]:
-            self.OPEN.put(strxi,self.key(strxi))
+    def UpdateMembership(self, xi, xparent=None):
+        if xi != self.x0:
+            self.v[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]])
+        self.OPEN.check_remove(xi)
+        if self.g[xi] != self.v[xi]:
+            self.OPEN.put(xi,self.key(xi))
     
     def ComputePath(self):
         print('computing path ...')
         while self.key(self.xt) > self.OPEN.top_key() or self.v[self.xt] != self.g[self.xt]:
-            strxi = self.OPEN.get()
-            xi = dehash(strxi)
+            xi = self.OPEN.get()
             # if g > rhs, overconsistent
-            if self.g[strxi] > self.v[strxi]: 
-                self.g[strxi] = self.v[strxi]
+            if self.g[xi] > self.v[xi]: 
+                self.g[xi] = self.v[xi]
                 # add xi to expanded node set
-                if strxi not in self.CLOSED:
+                if xi not in self.CLOSED:
                     self.V.append(xi)
-                self.CLOSED.add(strxi)
+                self.CLOSED.add(xi)
             else: # underconsistent and consistent
-                self.g[strxi] = np.inf
-                self.UpdateMembership(strxi, xi)
-            for xj in self.CHILDREN[strxi]:
-                strxj = hash3D(xj)
-                self.UpdateMembership(strxj, xj)
+                self.g[xi] = np.inf
+                self.UpdateMembership(xi)
+            for xj in self.CHILDREN[xi]:
+                self.UpdateMembership(xj)
 
             # visualization(self)
             self.ind += 1
@@ -176,9 +172,8 @@ class Lifelong_Astar(object):
         self.Path = []
         self.CLOSED = set()
         N = self.costset()
-        for strxi in N:
-            xi = dehash(strxi)
-            self.UpdateMembership(strxi,xi)
+        for xi in N:
+            self.UpdateMembership(xi)
 
 if __name__ == '__main__':
     sta = time.time()

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -26,40 +26,37 @@ class LRT_A_star2:
 
     def updateHeuristic(self):
         # Initialize hvalues at infinity
-        for strxi in self.Astar.CLOSED:
-            self.Astar.h[strxi] = np.inf
+        for xi in self.Astar.CLOSED:
+            self.Astar.h[xi] = np.inf
         Diff = True
         while Diff:  # repeat DP until converge
             hvals, lasthvals = [], []
-            for strxi in self.Astar.CLOSED:
-                xi = dehash(strxi)
-                lasthvals.append(self.Astar.h[strxi])
+            for xi in self.Astar.CLOSED:
+                lasthvals.append(self.Astar.h[xi])
                 # update h values if they are smaller
                 Children = self.Astar.children(xi)
-                minfval = min([cost(xi, xj, settings=0) + self.Astar.h[hash3D(xj)] for xj in Children])
+                minfval = min([cost(xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
                 # h(s) = h(s') if h(s) > c(s,s') + h(s') 
-                if self.Astar.h[strxi] >= minfval:
-                    self.Astar.h[strxi] = minfval
-                hvals.append(self.Astar.h[strxi])
+                if self.Astar.h[xi] >= minfval:
+                    self.Astar.h[xi] = minfval
+                hvals.append(self.Astar.h[xi])
             if lasthvals == hvals: Diff = False
 
     def move(self):
-        strst = self.Astar.x0
-        st = self.Astar.start
+        st = self.Astar.x0
         ind = 0
         # find the lowest path down hill
-        while strst in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in OPEN, stop
-            # strChildren = self.children(st)
-            strChildren = [hash3D(i) for i in self.Astar.children(st)]
+        while st in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in OPEN, stop
+            Children = [i for i in self.Astar.children(st)]
             minh, minchild = np.inf, None
-            for child in strChildren:
+            for child in Children:
                 h = self.Astar.h[child]
                 if h <= minh:
-                    minh, minchild = h, dehash(child)
+                    minh, minchild = h, child
             self.path.append([st, minchild])
-            strst, st = hash3D(minchild), minchild
-            for (_, strp) in self.Astar.OPEN.enumerate():
-                if strp == strst:
+            st = minchild
+            for (_, p) in self.Astar.OPEN.enumerate():
+                if p == st:
                     break
             ind += 1
             if ind > 1000:

Search-based Planning/Search_3D/RTA_Astar3D.py

@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -23,43 +23,40 @@ class RTA_A_star:
         self.N = N # node to expand 
         self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star
         self.path = [] # empty path
-        self.strst = []
+        self.st = []
         self.localhvals = []
 
     def updateHeuristic(self):
         # Initialize hvalues at infinity
         self.localhvals = []
         nodeset, vals = [], []
-        for (_,strxi) in self.Astar.OPEN.enumerate():
-            nodeset.append(strxi)
-            vals.append(self.Astar.Space[strxi] + self.Astar.h[strxi])
-        strj, fj = nodeset[np.argmin(vals)], min(vals)
-        self.strst = strj
+        for (_,xi) in self.Astar.OPEN.enumerate():
+            nodeset.append(xi)
+            vals.append(self.Astar.g[xi] + self.Astar.h[xi])
+        j, fj = nodeset[np.argmin(vals)], min(vals)
+        self.st = j
         # single pass update of hvals
-        for strxi in self.Astar.CLOSED:
-            # xi = dehash(strxi)
-            self.Astar.h[strxi] = fj - self.Astar.Space[strxi]
-            self.localhvals.append(self.Astar.h[strxi])
+        for xi in self.Astar.CLOSED:
+            self.Astar.h[xi] = fj - self.Astar.g[xi]
+            self.localhvals.append(self.Astar.h[xi])
         
     def move(self):
-        strst, localhvals = self.strst, self.localhvals
+        st, localhvals = self.st, self.localhvals
         maxhval = max(localhvals)
-        st = dehash(strst)
-        sthval = self.Astar.h[strst]
+        sthval = self.Astar.h[st]
         # find the lowest path up hill
         while sthval < maxhval:
             parentsvals , parents = [] , []
             # find the max child
             for xi in self.Astar.children(st):
-                strxi = hash3D(xi)
-                if strxi in self.Astar.CLOSED:
+                if xi in self.Astar.CLOSED:
                     parents.append(xi)
-                    parentsvals.append(self.Astar.h[strxi])
+                    parentsvals.append(self.Astar.h[xi])
             lastst = st            
-            st, strst = parents[np.argmax(parentsvals)], hash3D(st)
+            st = parents[np.argmax(parentsvals)]
             self.path.append([st,lastst]) # add to path
-            sthval = self.Astar.h[strst]
-        self.Astar.reset(dehash(self.strst))
+            sthval = self.Astar.h[st]
+        self.Astar.reset(self.st)
 
     def run(self):
         while True:
@@ -74,5 +71,5 @@ class RTA_A_star:
 
 
 if __name__ == '__main__':
-    T = RTA_A_star(resolution=0.5, N=100)
+    T = RTA_A_star(resolution=1, N=100)
     T.run()

Search-based Planning/Search_3D/bidirectional_Astar3D.py

@@ -13,7 +13,7 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, cost
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -25,14 +25,14 @@ class Weighted_A_star(object):
                       [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
                       [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
         self.env = env(resolution = resolution)
-        self.Space = g_Space(self) # key is the point, store g value
-        self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
-        self.Space[hash3D(self.start)] = 0 # set g(x0) = 0
-        self.Space[hash3D(self.goal)] = 0 # set g(x0) = 0
+        self.g = g_Space(self) # key is the point, store g value
+        self.start, self.goal = getNearest(self.g,self.env.start), getNearest(self.g,self.env.goal)
+        self.g[self.start] = 0 # set g(x0) = 0
+        self.g[self.goal] = 0 # set g(x0) = 0
         self.OPEN1 = queue.QueuePrior() # store [point,priority]
         self.OPEN2 = queue.QueuePrior()
-        self.h1 = Heuristic(self.Space,self.goal) # tree NO.1
-        self.h2 = Heuristic(self.Space,self.start) # tree NO.2
+        self.h1 = Heuristic(self.g,self.goal) # tree NO.1
+        self.h2 = Heuristic(self.g,self.start) # tree NO.2
         self.Parent1, self.Parent2 = {}, {}
         self.CLOSED1, self.CLOSED2 = set(), set()
         self.V = []
@@ -48,21 +48,20 @@ class Weighted_A_star(object):
         return allchild
 
     def run(self):
-        x0, xt = hash3D(self.start), hash3D(self.goal)
-        self.OPEN1.put(x0, self.Space[x0] + self.h1[x0]) # item, priority = g + h
-        self.OPEN2.put(xt, self.Space[xt] + self.h2[xt]) # item, priority = g + h
+        x0, xt = self.start, self.goal
+        self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h
+        self.OPEN2.put(xt, self.g[xt] + self.h2[xt]) # item, priority = g + h
         self.ind = 0
         while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty
-            strxi1, strxi2 = self.OPEN1.get(), self.OPEN2.get() 
-            xi1, xi2 = dehash(strxi1), dehash(strxi2)
-            self.CLOSED1.add(strxi1) # add the point in CLOSED set
-            self.CLOSED2.add(strxi2)
+            xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() 
+            self.CLOSED1.add(xi1) # add the point in CLOSED set
+            self.CLOSED2.add(xi2)
             self.V.append(xi1)
             self.V.append(xi2)
             visualization(self)
             allchild1,  allchild2 = self.children(xi1), self.children(xi2)
-            self.evaluation(allchild1,strxi1,xi1,conf=1)
-            self.evaluation(allchild2,strxi2,xi2,conf=2)
+            self.evaluation(allchild1,xi1,conf=1)
+            self.evaluation(allchild2,xi2,conf=2)
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
             self.ind += 1
         self.common = self.CLOSED1.intersection(self.CLOSED2)
@@ -71,45 +70,44 @@ class Weighted_A_star(object):
         visualization(self)
         plt.show()
 
-    def evaluation(self, allchild, strxi, xi, conf):
+    def evaluation(self, allchild, xi, conf):
         for xj in allchild:
-            strxj = hash3D(xj)
             if conf == 1:
-                if strxj not in self.CLOSED1:
-                    gi, gj = self.Space[strxi], self.Space[strxj]
+                if xj not in self.CLOSED1:
+                    gi, gj = self.g[xi], self.g[xj]
                     a = gi + cost(xi,xj)
                     if a < gj:
-                        self.Space[strxj] = a
-                        self.Parent1[strxj] = xi
-                        if (a, strxj) in self.OPEN1.enumerate():
-                            self.OPEN1.put(strxj, a+1*self.h1[strxj])
+                        self.g[xj] = a
+                        self.Parent1[xj] = xi
+                        if (a, xj) in self.OPEN1.enumerate():
+                            self.OPEN1.put(xj, a+1*self.h1[xj])
                         else:
-                            self.OPEN1.put(strxj, a+1*self.h1[strxj])
+                            self.OPEN1.put(xj, a+1*self.h1[xj])
             if conf == 2:
-                if strxj not in self.CLOSED2:
-                    gi, gj = self.Space[strxi], self.Space[strxj]
+                if xj not in self.CLOSED2:
+                    gi, gj = self.g[xi], self.g[xj]
                     a = gi + cost(xi,xj)
                     if a < gj:
-                        self.Space[strxj] = a
-                        self.Parent2[strxj] = xi
-                        if (a, strxj) in self.OPEN2.enumerate():
-                            self.OPEN2.put(strxj, a+1*self.h2[strxj])
+                        self.g[xj] = a
+                        self.Parent2[xj] = xi
+                        if (a, xj) in self.OPEN2.enumerate():
+                            self.OPEN2.put(xj, a+1*self.h2[xj])
                         else:
-                            self.OPEN2.put(strxj, a+1*self.h2[strxj])
+                            self.OPEN2.put(xj, a+1*self.h2[xj])
             
     def path(self):
         # TODO: fix path
         path = []
-        strgoal = hash3D(self.goal)
-        strstart = hash3D(self.start)
-        strx = list(self.common)[0]
-        while strx != strstart:
-            path.append([dehash(strx),self.Parent1[strx]])
-            strx = hash3D(self.Parent1[strx])
-        strx = list(self.common)[0]
-        while strx != strgoal:
-            path.append([dehash(strx),self.Parent2[strx]])
-            strx = hash3D(self.Parent2[strx])
+        goal = self.goal
+        start = self.start
+        x = list(self.common)[0]
+        while x != start:
+            path.append([x,self.Parent1[x]])
+            x = self.Parent1[x]
+        x = list(self.common)[0]
+        while x != goal:
+            path.append([x,self.Parent2[x]])
+            x = self.Parent2[x]
         path = np.flip(path,axis=0)
         return path
 

Search-based Planning/Search_3D/utils3D.py

@@ -14,8 +14,7 @@ def getManDist(pos1, pos2):
 def getNearest(Space,pt):
     '''get the nearest point on the grid'''
     mindis,minpt = 1000,None
-    for strpts in Space.keys(): 
-        pts = dehash(strpts)
+    for pts in Space: 
         dis = getDist(pts,pt)
         if dis < mindis:
             mindis,minpt = dis,pts
@@ -25,7 +24,7 @@ def Heuristic(Space,t):
     '''Max norm distance'''
     h = {}
     for k in Space.keys():
-        h[k] = max(abs(t-dehash(k)))
+        h[k] = max(abs(np.array([t[0]-k[0],t[1]-k[1],t[2]-k[2]])))
     return h
 
 def hash3D(x):
@@ -66,13 +65,13 @@ def g_Space(initparams):
     g = {}
     Space = StateSpace(initparams.env)
     for v in Space:
-        g[hash3D(v)] = np.inf # this hashmap initialize all g values at inf
+        g[v] = np.inf # this hashmap initialize all g values at inf
     return g
 
 def isCollide(initparams, x, direc):
     '''see if line intersects obstacle'''
     resolution = initparams.env.resolution
-    child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+    child = tuple(map(np.add,x,np.multiply(direc,resolution)))
     ray , dist = getRay(x, child) ,  getDist(x, child)
     if not isinbound(initparams.env.boundary,child):
         return True, child