diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 9390fe4..e618237 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -66,7 +66,7 @@ class D_star(object): # get the minimum of the k val in OPEN # -1 if it does not exist if self.OPEN: - return min([x for x in self.OPEN.values()]) + return min(self.OPEN.values()) return -1 def min_state(self): @@ -74,12 +74,10 @@ class D_star(object): # if empty, returns None and -1 # it also removes this min value form the OPEN set. if self.OPEN: - mink = -1 - minv = np.inf - for v, k in enumerate(self.OPEN): - if v < minv: - mink, minv = k, v - return mink, self.OPEN.pop(mink) + minvalue = min(self.OPEN.values()) + for k in self.OPEN.keys(): + if self.OPEN[k] == minvalue: + return k, self.OPEN.pop(k) return None, -1 def insert(self, x, h_new): @@ -130,12 +128,9 @@ class D_star(object): return self.get_kmin() def modify_cost(self, x): - # TODO: implement own function - # self.c[x][y] = cval xparent = self.b[x] if self.tag[x] == 'Closed': self.insert(x, self.h[xparent] + cost(self, x, xparent)) - # self.insert(x, self.h[xparent]) def modify(self, x): self.modify_cost(x) self.V = set() @@ -164,7 +159,7 @@ class D_star(object): while True: # TODO: self.x0 = self.process_state() - visualization(self) + # visualization(self) if self.tag[self.x0] == "Closed": break self.ind += 1 @@ -175,20 +170,18 @@ class D_star(object): # plt.show() # when the environemnt changes over time - for i in range(2): - self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation') - visualization(self) + for i in range(5): + self.env.move_block(a=[0.25, 0, 0], s=0.5, block_to_move=1, mode='translation') + self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation') + # travel from end to start s = tuple(self.env.start) - while s != self.xt: if s == tuple(self.env.start): sparent = self.b[self.x0] else: sparent = self.b[s] - # self.update_obs() - + # if there is a change of cost, or a collision. if cost(self, s, sparent) == np.inf: - # print(s, " ", sparent) self.modify(s) continue self.ind += 1 @@ -199,5 +192,5 @@ class D_star(object): if __name__ == '__main__': - D = D_star(0.75) + D = D_star(1) D.run() diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index b1d2be5..de4632e 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/Search-based Planning/Search_3D/LP_Astar3D.py @@ -7,7 +7,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, isinbound, isinball, hash3D, dehash, \ +from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \ cost, obstacleFree from Search_3D.plot_util3D import visualization import queue @@ -89,7 +89,7 @@ class Lifelong_Astar(object): ray , dist = getRay(x, child) , getDist(x, child) if not isinbound(self.env.boundary,child): return True, dist - for i in self.env.AABB: + for i in self.env.AABB_pyrr: shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) if shot is not None: dist_wall = getDist(x, shot) @@ -177,7 +177,7 @@ class Lifelong_Astar(object): if __name__ == '__main__': sta = time.time() - Astar = Lifelong_Astar(1) + Astar = Lifelong_Astar(0.5) Astar.ComputePath() Astar.change_env() Astar.ComputePath() diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 6d8859c..cbb9b74 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index 7e76a61..fb503c5 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -22,12 +22,12 @@ def getblocks(): Obstacles.append([j for j in i]) return np.array(Obstacles) -# def getAABB(blocks): -# # used for Pyrr package for detecting collision -# AABB = [] -# for i in blocks: -# AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger -# return AABB +def getAABB(blocks): + # used for Pyrr package for detecting collision + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB class aabb(object): def __init__(self,AABB): @@ -57,6 +57,7 @@ class env(): self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) self.blocks = getblocks() self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) self.balls = getballs() self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) @@ -66,6 +67,7 @@ class env(): newblock = add_block() self.blocks = np.vstack([self.blocks,newblock]) self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) def move_start(self, x): self.start = x