From 7114b96bc632b5a01b11c57a8d945d2dbf896a1c Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Thu, 9 Jul 2020 17:30:32 -0700 Subject: [PATCH] 'update' --- Search-based Planning/Search_3D/Dstar3D.py | 48 +++++++++--------- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3155 -> 3182 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 3620 -> 3647 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4770 -> 4797 bytes .../__pycache__/queue.cpython-37.pyc | Bin 3010 -> 2999 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 6157 -> 6075 bytes Search-based Planning/Search_3D/utils3D.py | 30 +++++------ 7 files changed, 40 insertions(+), 38 deletions(-) diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 3c4f870..9390fe4 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -135,11 +135,13 @@ class D_star(object): 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() while True: kmin = self.process_state() + # visualization(self) if kmin >= self.h[x]: break @@ -162,7 +164,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 @@ -173,29 +175,29 @@ 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) - # 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 cost(self, s, sparent) == np.inf: - # # print(s, " ", sparent) - # self.modify(s) - # continue - # self.ind += 1 - # s = sparent - # self.Path = self.path() - # visualization(self) + for i in range(2): + self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation') + visualization(self) + 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 cost(self, s, sparent) == np.inf: + # print(s, " ", sparent) + self.modify(s) + continue + self.ind += 1 + s = sparent + self.Path = self.path() + visualization(self) plt.show() if __name__ == '__main__': - D = D_star(0.5) + D = D_star(0.75) D.run() 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 58807f373000453986b5e5491e0202bdad2ba640..a14034838a09e1b4bd06f6b9625683610ddea84d 100644 GIT binary patch delta 82 zcmcaC@lJxviI?RvG#L delta 85 zcmaDSaan@PiI4DskXVwTP>_?Dmj_Zc`7?{wW@lDyb^x@C9Tflo 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 0c432ca41e957068fae3b70fd42a18483ccd90e9..6d8859ca0f9830ed4eaa158003b14b4bc0579eff 100644 GIT binary patch delta 82 zcmZ1?vtNeGiI&Nwlp+0jB!aV&MwI>h)K^Z$tX>V iDM&2I&@ISG%*)HnOV^!j&ZNbzucrq>o1K^*aRUGbdl`cO delta 85 zcmdllvqXl=iI4DskXVwTP>_?Dmj_ZcS%*n$a}CpBZUCX}9Q*(P diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 6ae7677054c3468a6275554178421b501dd01f78..d54e50d25b9dccff52231b86c1d90274cb54e3e9 100644 GIT binary patch delta 93 zcmZ3ax>uFUiI&Nwlp+0jB!aV&MwI>h)K^Z$tX>V tDM&2I&@ISG%*)HnOV^$3!lcEnucrq>oBf%@`B|fwiYlTuXA7D$0stQM8?pcZ delta 96 zcmdn1x=5AFiI)jD9y~x%u82@%E&K`aY-%CF3B&5 wanCHtC{5Ch^6-r*NG!=vD9B07%LA#JY{{gxxs&M!AL}iqqKaFa+XT%S0lbnRmjD0& diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 7c5dea494f4ae39cdd7feaff7ba4b43695b52162..6871e7f84465862e00e9ed3414b5452d84a6cae1 100644 GIT binary patch delta 99 zcmX>kzFnNhiIej`1kX%+1V8SBT2UFO6|YEzT~< zFNkr^EXgQM(v9-)jVVYh$xtZBNzBUwsoK1Tk&T`4*5=I|rA*v%Kog6UK!oz-h0GF@ Hy}5G$z9Ayl 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 d902d0c425f8613f84fe14f6adfe6f0baa5be9de..69a2b147b6570e34ad999dd66d5eec4dc9af2cf2 100644 GIT binary patch delta 1676 zcmbVM&2Jk;6rZ=dUT>V(PU4Tm#%aj`0FGxwKQkn$RQYi?9jPaAa7di2Ag7gJIl#B)BzZWMV1{r`2BGBR%tqa z$u0{!KQT2nF_xb%+nY|cmS3wl>(`65vKKVoNe7qZV zmR`()XhR~ah+f=CC2dsfid8I_=2OVAbp!@!&_MJbapR@}-SXE^A9ym_JU22%D*dFQ zBq4`$9A^lonSq$YU55-z`xF|;9ql#mDEa`Zi$VEUI1?EmaumUXB{3wQjAUS5z7Tl^ zP9NQfbV7oh6e>XvfktprejFVQdVy85T5#k~(X{?7v8Uvp(N$QHy!8}Zkat?Y(Jv7D ztXyuJ8L^PG{R*oZdKSXehy~=8$Tp1tt{|>CT=q@*Ra^gSlaA&DT(+TX=v2FTw$i(+ zIYAd(#Z?=8S4*+FCdN6q>Z-EHw#F~B?S5ZopnfOnH~IQnT@}SgT19!jeRMD6XI}K- ziQ+5)h2aH6!CEVOs#&%??cBN9S#jEDQ$cKzLpmD}7fGGm^#Y~!ipd4l!LOj8AREw3 z@}`PCS2fKFFK^?g9YURZQcXe&MAQ~Uv4nz>LSh`0jV4@_9s3T#8rxMcEVco= z>h?ATh~Zfno=QpD&|Po>cZ06xs)tHlt;1W)F>bL{;J}rA!~bW~Jfk#MVU4+LC9(?+ zbAb#TC|lZMBOlo5)Tmj-Q*xu2t39MaEF$Z1y8Z~AF)OxHs$Q$jyhAqW0G+*&X=J%< z<<6MZQo*!mka(#0KfQU*uRLD7OXLFGzW=x*UP11=xQ_kEMjDMWCEtv7PtKsgGp=1X zol4Ez@zjE~BVHz+UPLE1?M7$$O{EuVwX(&jbL5w?*vu5F2`aE3`onYmhA|$7N@e$R zN-v3lEt1Bm(30VJZNTr?Jelwtm=12_SAE=_qE~(t?@7H!mLdoY`!0mG8Yr&HpW}9h z8c$FW#Y+TK%Bw%s@l$nsA{((Fla@q8tA^Kw2PvAkhl*=@<#IM zt|FN{zBE~+J!QMH94K*7w#1_RHks-86uEW_0#oQR&;yYHu`d5k_CQMZbj(cnK~n$t zSJ1zc!hc`r74a{RxI)xb0y+@?{eH*uxgjJCN}p;33=L< delta 1736 zcmYjR-ES0C6uIa1wt=m$t#!!?Z=%xgwbEoXUba#7a zN`Op5OdBOJJ~a0Yd?+!#`Cvj~VoJiRF}z7k9Q6_N~)(N(ra) z#5(c^#-6iiyob9>_NpIp&k3E)e?C9ypL%2A!@w26!tAQI>@}|q)f=tVg&8-v5w_Y3 z?|5Njb@Ao;?Ae939X5vAzTIr1mHduYrOpaelNI(oHRUt*4b93=lxMU{UR3|4eR57K zxfBD#K(4|(1v5AbBqR~!)=$4v;=w`l(ZZHg?6{=Ek^wBT+r;Hw!SIvX8 zE;r0mJ;%@l!x2`ZWAJhLt68DPAN^(CX5(X^7ibcz9I(ZzZ7(jy%Cg5}`fkh`zL-FZ z6KFv%h*$8gGKTLp-KnYR>6(0;Jx(Vd{h95fy_jlxzZObZby1arx$#2+!^3sEVXWAk zTlrXVn>S;9$#%k)5O1SpP2S8c(U0ZtxkL1vRJwlD&!PIX{Hbg5mFkv+L>%xm;OOLoK2ai#z0lH$uwlb0S5PmQ##Y-fD zT3B?>+LOYl$&td~^bB~6jb-@Rcy0f7iZTJH+wHd7D?$WF?`vf5jrCo#VRs?!R>`{=0$2%+8JM^4Z!HX>jJ(mG=>|=&kul7S2@=5h7gJ