From 31694871e4fa8936fe7ff59490a8536953b46013 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Fri, 10 Jul 2020 17:56:46 -0700 Subject: [PATCH] 'D*' --- Search-based Planning/Search_3D/Dstar3D.py | 31 +++++++----------- Search-based Planning/Search_3D/LP_Astar3D.py | 6 ++-- .../__pycache__/env3D.cpython-37.pyc | Bin 3647 -> 3919 bytes Search-based Planning/Search_3D/env3D.py | 14 ++++---- 4 files changed, 23 insertions(+), 28 deletions(-) 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 6d8859ca0f9830ed4eaa158003b14b4bc0579eff..cbb9b745009ea1b4207f3e86ce03a1cdb8ffd6b7 100644 GIT binary patch delta 1138 zcmY*XOKTKC5bm01&pu|8O-$A##?_5kC+a~kCTQ>x1x3Y+2W3#kO?q7N+Ks(KqOvi< z#+wMz;MsWaq6bA!9`q*&9_QvUC&7atc(AHNLh#t|q0ilQYc8NZ`*Vi$^Oq><{3x<3UXF4p*#rZfl?{@&0u#IahD7}06 z$XEwz3_lYki*gR7bqQ6St--xBZFZjBnp`v5@PZ}q!ay*#s>9Q-RIl8G?6CyBEXJ)ZEyUZHBe(?{SgR9iGX*^f$ngfaEqegzfv+<5{cs^YF5 zDA8$QCuLUmgb`U1&1j-NyAxAwHg9*J%_1+sk)X{Ir^Kc(>*gk%MP)$@sijhebqS1=qPTn)G!!DO5m)^b)_No{svo3%2*^pfsF2IvKb98~LG zwQCxK?HOT}BG&leo#2OdGJbmd;NtsIrfRHD(tX+y&9Ww4`V{IJ5W!(Kk+nu^v6jp= z8!4B~M0M2=yx(E;Mc0&emG Qce`xva<^pqr#RC52VZU2hX4Qo delta 849 zcmY+Czi-n(6vzGi^X$})o0c@CEhLnZ7y+Wt5(y!s7KsW8v9MVntIe6xrcJAJNCl!Q zC>0Y6oS0bP4`4uu5izsCz;-)Bkyw#nf%lvREWOX2zt4B?-uL|F$j8ZG(f2(KpXMVc z`4&D4DtkSA<65k7;Q9u{Iybnvuf@ia#w~8|YurxFz7bp8;S6<~yWGRk;RQa#eID>4 z4_ECwz=wHhA7X~O%2wvW!s4R*ZQh28EEdN~BESPyF)|&&Km_u!bv-0CGL*ftvDIp4VoWOQ zJWR>J-U){>B|^jjwJJ*Th5Z$(a?N=PHTl!onHiy+o#zlOhMW7T*KsCc)PR;x1x}Fh;1#U3Ut~@}=8o%#c9c z3}Df$f@or7bZ`x9r~s2c+|$sI6JGrc*U&Gx5!s@KY2hH0-OIYmt+x2TQlyKreB#xE z1^gzXE@`)t{NP<=4SdP-f4mx8eOD{g_33%k1Q1FDDldwJ+(xoW$+kZ}n^&Q_f;>jxa~K zOt-%$j^X?QMwZ_WcpkHuk*kU&$>#D}CrP3}IZ48|T1ld;&b`g;`?*2F$92lyT3Jt9 z-TXHi+(#AVfsM3zevH`eq&sP+r>t$U*6E5mCGdu1xp=ff4Vu6VRt_!ZFsEV!ml%5b E7r8T`nE(I) 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