diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 9fe608b..12b5e2e 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/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) diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index ebb0cfd..2991238 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/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''' diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index 25ae599..c64e59c 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/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() diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 6dd9632..528bbbb 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/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: diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py index ca6d299..88c18e0 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/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() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index e71e672..1e8e448 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 9b09abd..890cc05 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/bidirectional_Astar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py index 26a5522..0796cc5 100644 --- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py +++ b/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 diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 715ba0f..d1f3efc 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/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