From 16e4f3ed28dc1a3cb2cc810e1142e0917d763df0 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Mon, 29 Jun 2020 11:48:17 -0700
Subject: [PATCH] LRTA*

---
 Search-based Planning/Search_3D/Astar3D.py    |  20 ++-
 .../Search_3D/LRT_Astar3D.py                  | 166 ++++++++++--------
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin 2798 -> 3114 bytes
 3 files changed, 109 insertions(+), 77 deletions(-)

diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index 734f944..305051b 100644
--- a/Search-based Planning/Search_3D/Astar3D.py	
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -35,6 +35,7 @@ class Weighted_A_star(object):
         self.V = []
         self.done = False
         self.Path = []
+        self.ind = 0
 
     def children(self,x):
         allchild = []
@@ -44,10 +45,9 @@ class Weighted_A_star(object):
                 allchild.append(child)
         return allchild
 
-    def run(self):
+    def run(self, N=None):
         x0, xt = hash3D(self.start), hash3D(self.goal)
         self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # item, priority = g + h
-        self.ind = 0
         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)
@@ -69,12 +69,18 @@ class Weighted_A_star(object):
                         else:
                             # add xj in to OPEN set
                             self.OPEN.put(strxj, a+1*self.h[strxj])
+            # For specified expanded nodes, used primarily in LRTA*
+            if N is not None:
+                if len(self.V) % N == 0:
+                    break
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
             self.ind += 1
-        self.done = True
-        self.Path = self.path()
-        visualization(self)
-        plt.show()
+        # if the path finding is finished
+        if xt in self.CLOSED:
+            self.done = True
+            self.Path = self.path()
+            visualization(self)
+            plt.show()
 
     def path(self):
         path = []
@@ -87,5 +93,5 @@ class Weighted_A_star(object):
         return path
 
 if __name__ == '__main__':
-    Astar = Weighted_A_star(0.5)
+    Astar = Weighted_A_star(1)
     Astar.run()
\ No newline at end of file
diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py
index 07e6c2a..925a608 100644
--- a/Search-based Planning/Search_3D/LRT_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LRT_Astar3D.py	
@@ -1,4 +1,4 @@
-# this is the three dimensional LRTA* algo
+# this is the three dimensional near-sighted 1 neighborhood LRTA* algo
 # !/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
@@ -12,84 +12,110 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
+from Search_3D.Astar3D import Weighted_A_star
 from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
 from Search_3D.plot_util3D import visualization
 import queue
 
 
-class LRT_A_star(object):
-    def __init__(self,resolution=0.5):
-        self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
-                      [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
-                      [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 = StateSpace(self)
-        self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
-        self.AABB = getAABB(self.env.blocks)
-        self.Space[hash3D(getNearest(self.Space,self.start))] = 0 
-        self.OPEN = queue.QueuePrior() 
-        self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
-        self.Child = {}
-        self.CLOSED = set()
-        self.V = []
-        self.done = False
-        self.Path = []
+# class LRT_A_star1(object):
+#     def __init__(self,resolution=0.5):
+#         self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
+#                       [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
+#                       [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 = StateSpace(self)
+#         self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
+#         self.AABB = getAABB(self.env.blocks)
+#         self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # this is g
+#         self.OPEN = queue.QueuePrior() 
+#         self.h = Heuristic(self.Space,self.goal) # 1. initialize heuristic h = h0
+#         self.Child = {}
+#         self.CLOSED = set()
+#         self.V = []
+#         self.done = False
+#         self.Path = []
 
-    def children(self,x):
-        allchild = []
-        for j in self.Alldirec:
-            collide,child = isCollide(self,x,j)
-            if not collide:
-                allchild.append(child)
-        return allchild
+#     def children(self,x):
+#         allchild = []
+#         for j in self.Alldirec:
+#             collide,child = isCollide(self,x,j)
+#             if not collide:
+#                 allchild.append(child)
+#         return allchild
 
-    def step(self, xi, strxi):
-        childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
-        fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
-        xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
-        strxj = hash3D(xj)
-        # add the child of xi
-        self.Child[strxi] = xj
-        if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update  h(s) = h'(s)
-            self.h[strxi] = fmin 
-        # TODO: action to move to xj
-        self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
+#     def step(self, xi, strxi):
+#         childs = self.children(xi) # 4. generate depth 1 neighborhood S(s,1) = {s' in S | norm(s,s') = 1}
+#         fvals = [cost(xi,i) + self.h[hash3D(i)] for i in childs]
+#         xj , fmin = childs[np.argmin(fvals)], min(fvals) # 5. compute h'(s) = min(dist(s,s') + h(s'))
+#         strxj = hash3D(xj)
+#         # add the child of xi
+#         self.Child[strxi] = xj
+#         if fmin >= self.h[strxi]: # 6. if h'(s) > h(s) then update  h(s) = h'(s)
+#             self.h[strxi] = fmin 
+#         # TODO: action to move to xj
+#         self.OPEN.put(strxj, self.h[strxj]) # 7. update current state s = argmin (dist(s,s') + h(s'))
+
+#     def run(self):
+#         x0 = hash3D(self.start)
+#         xt = hash3D(self.goal)
+#         self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
+#         self.ind = 0
+#         while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
+#             strxi = self.OPEN.get()           
+#             xi = dehash(strxi) 
+#             self.CLOSED.add(strxi) 
+#             self.V.append(xi)
+#             visualization(self)
+#             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
+#             self.ind += 1
+#         self.done = True
+#         self.Path = self.path()
+#         visualization(self)
+#         plt.show()
+
+#     def path(self):
+#         # this is a suboptimal path. 
+#         path = []
+#         strgoal = hash3D(self.goal)
+#         strx = hash3D(self.start)
+#         ind = 0
+#         while strx != strgoal:
+#             path.append([dehash(strx),self.Child[strx]])
+#             strx = hash3D(self.Child[strx])
+#             ind += 1
+#             if ind == 1000:
+#                 return np.flip(path,axis=0)
+#         path = np.flip(path,axis=0)
+#         return path
+
+class LRT_A_star2():
+    def __init__(self,resolution=0.5, N=7):
+        self.lookahead = N
+        self.Astar = Weighted_A_star()
+        self.Astar.env.resolution = resolution
+    
+    def expand(self):
+        self.Astar.run(self.lookahead) 
+
+    def updateHeuristic(self):
+        for strxi in self.Astar.CLOSED:
+            self.Astar.h[strxi] = np.inf
+            xi = dehash(strxi)
+            self.Astar.h[strxi] = min([cost(xi,xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
+    
+    #def move(self):
+    #    print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()]))
+        
 
     def run(self):
-        x0 = hash3D(self.start)
-        xt = hash3D(self.goal)
-        self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # 2. reset the current state
-        self.ind = 0
-        while xt not in self.CLOSED and self.OPEN: # 3. while s not in Sg do
-            strxi = self.OPEN.get()           
-            xi = dehash(strxi) 
-            self.CLOSED.add(strxi) 
-            self.V.append(xi)
-            visualization(self)
-            self.step(xi , strxi)
-            if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
-            self.ind += 1
-        self.done = True
-        self.Path = self.path()
-        visualization(self)
-        plt.show()
+        xt = hash3D(self.Astar.goal)
+        while xt not in self.Astar.CLOSED:
+            self.expand()
+            #self.updateHeuristic()
 
-    def path(self):
-        # this is a suboptimal path. 
-        path = []
-        strgoal = hash3D(self.goal)
-        strx = hash3D(self.start)
-        ind = 0
-        while strx != strgoal:
-            path.append([dehash(strx),self.Child[strx]])
-            strx = hash3D(self.Child[strx])
-            ind += 1
-            if ind == 1000:
-                return np.flip(path,axis=0)
-        path = np.flip(path,axis=0)
-        return path
 
 if __name__ == '__main__':
-    Astar = LRT_A_star(0.5)
-    Astar.run()
\ No newline at end of file
+    T = LRT_A_star2(resolution = 1, N = 2)
+    T.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 cf152275033871ae2590872fde16b5789509c063..b60701231db1978095e88ce1702ad857d412d101 100644
GIT binary patch
literal 3114
zcmaJ@OK&5`5$>KBheJxD-jDUl^d@$~z$-1XNa7e9XSFNm4Oy`gFEB_v7;$FFp=L;`
zr|EEo;U(NdE)M(?C?J;rKIf2A5ad_%DL@Xv{smu>uX;qu5aNu~)z#J2-SsX$Sy=EG
zzTS7f`1tROjQxui^REKrF-HCmfQb1l;*pVZ-_Wy}S-#Cnn&Uf~R`DxwrRUD7u><I7
zSc~hI{E|oJCcEUC-%1y<MSoG_cDj@`{6@CyFJ~+MO1A2+W^4Xhw(hTI8~#SN>2GFR
z{#Lf_Z)<)h-O29wcdp)j&42C3EUHBAOBT8D-KpvC#;-@!Gei6>-i^E|_nUEZ_QoXx
zzj4A^wTor%aX6CQfq1w1ViY&e6Ymw2V@DTv_xJYb|0E7YyL<mM%;TteoQC~=((mlI
zxH9AZCoNA^I<Y)Bc=ALehe<A#1ImZt3+0{2P{t?2upO)FPvVilX3|z3wvTXW4AyFr
zKOLlL62;2thI#klp>m>_FlDs|xx_OIpCtJxOp}XHCWC&PgqwdRkjEJL1Av(M96m9k
zT4Y4#OYWPI71=MDZ$))&5$@YjHS$1lA_uxJsHJCd(&@@L3J!u?hT`A22H!t_*fvUx
z(Y%ZC6eIsVXH2pgC(lMk#{>kXXJDB1j-Jak?a%2n(}|KF*wr~x)5|sO(;86Ebb{81
z70pi=oul{5b2snI&z0}Z&*S9P{c}0y_m%Q!`8qq7ykLPL&4NePr-Yl*D%g}TTRH{n
z*%)T8GBv)ma;JvDz)`_)WmR(^!mxq|=!~pibK~TcKVouWws=WrmSp4VR@{R1azfM<
zxjLnJtwV4<!mGiC#y9E4U@Kzqn{)IEj$neotN-9bka&ouK{}!c`B7_I8U3NMLm?1^
z?m?PHiHO^Rd?_3NW$Ty}uExpnQdylrm<kFg<(#I2_F1khifTdCK-uS`coZw|moy!V
zWFVCF!SVY?f+~XIUF94jp8Hand5rjf`il=v-al0QBV|Q{eyptHP<EA>^rM!ctUOMi
zixnLFGZWvy^u=_)JILbwFpbCihjD%;2gChNBD<s0{UHwCA6_Nh{V?qe1U9n#E3&f8
z)!^ab|IOVSt^-p3;o;s8b_#-|pU5Cshe-K70LCBjclZO0I`{Zne35TqZg7XY81#3s
zbt{4P=9gS25ZT+t&<XTiARRU_#&5&<?2!HT-o%s$n1UDP)clPxu_kt5<@Lgt8HG*7
z@x1<w6;!DBQOgu0ny3OOCmarAn9~t&(UOmq8>VTyo1~HAJymI66%|{Pw3$orO=5RT
zYTTz)UI$>@f|d=y#s8tzoc-upEjmNtqt*JWt_x6!Pq{?o_7Lj^)SQ^W&6!o`Y$%Lv
z+H+_|<u^F{jE_Goh^KN^`HYJ{5chnZxtDBWA*-w@hf%;Y^9qYh0=zb>m*$ZRl%+(Q
zEfQtoj6W|N81_$-3M8rEtXsI{3*wKMVO}kTUAWsQZj<W7E2?s-u)%9=vPrF|72X*G
zZ%uQ%j~Mn?uTc<IE8OR#i#c1qWM>X!-OwuwZ`2|<658S3wJZJ3=|$52D|X>pRP3`V
z?)gZrfVYO~!8g#E&J3a*ldh|gc}*G1waAj|M4j@@E6pw9;opNc!Rq-txU;k<N%xns
zg1|bPy6SAr?IpKoJGuv!^+Yt_%CeqBz%Eu384KNRn*C9B8jI!+n!jq@(X~Kq67Lp)
zy8y}@j-=S2CG`pMhNhvziJHd3C=!%R;#&k32vD;U6hPv209B3g20a%mGmZP=O(NPu
zk?3+Fh4>l~EUE<J>x7!a6!j|a4u03FUxojXGR6-uq_T4<#tG0wyiLr;xL4A8zR^iA
zdWwg)^T8COK1r3o2f+9S-_@z^A|>ibb`O69H;^3VYR%X+n})~NxWOB2g|8TOlmp_r
zSiM~maCe(ZHGLMwbv5`H3si$LWso`(W1c({bNp>2jd^B@pA~#Y%?K$4x7(CF9fQ(9
z^XfI7Jl}c7q|>V)pwJvYH6{ooR1Nb;dle1B3dc#_a!VI#?`c13uaKv-FT^S^Wj#-m
zVaw8{72nqsGOBU`b+fo-KF#xM@Z}UnR7uES9!$7G)nx6qLCBLWE|$PcWvxP`w;yJ4
z5GXGQvOzRT39kpiIezz)JE8^dGI9<HBeA|D2DK4E#Z2HQEAkJrP<4W{R$bS7Q&Z`g
zEKK@Apu@d&PgQ~-8np40jR+jRD79bCLODzaGEGj=76?;LIg593r7{LNy4;Jr6oKrg
zD(J5yrmBReIcY~vJx{b2OV`b24?h<l9_lWvmT#!&*=Y_swO&E-At*p}pVr+~cWGVp
zOW$c{QTC|yn@dQ{fNsJ?I>+u1gEKho#cipz7e6J6ts9=uCe${&0c1VpJ}S-s9<{A}
Y2f#v1*A17uMiaU4fbzj?{2XikU)}HP{{R30

literal 2798
zcmZ`*TW=#t6|U-=+a2F>wKIfKgg_!l6R&n8c7>v0GLu<&K$$E-Gs0SEb=)1ho%SWw
zWpJYMllOJe2qB)6x8+ap2lxl|gv86vGZGKHu-~bUqfxZ9UFT9&=k%#_tA0_f1`N+X
z{`sHZzpOI$FJhKY3B+@Z{LcU}>#&%|R>nJ)!FI<s*y%WlGjMOb#0D%G=qKe{e#>LK
z$!<9(IVTH-E1eZXyIEye?No=gPHkB4)Q62uW4PK`HEUkBHeBzlbM^}smty}*7W>J@
z!tQJ)TXFfq5`RiI7p&8aty8uaT(1Vtqp9qV#m{!HrpfMQ8oY<8?D+co?frfFpC*y$
z_Me<Zc@poQWYK7pj(Y7quI*&>hdo!9dWk$deDT5%M`<p#hnZhTS2{SAkxWh}Q8&@$
zUnWz5#k32_;PYuRO-@8Q7F~MT^07cX$H;#OkgyJi$Bd`GWbl+7JHSrt#vZU6mteoI
z>u-~^*Oy5g9)`JusT{KO5PppD5+nZ<oMbng+#Opz8xWMipn%Pa!39t2%Xz{iQS1kG
zCuf`af~R%j0SzVz;t@cz-vrw5#Xj0edg(n&x#E3G`C^{*EbUn8E9^1$^&Z`F#X?Kk
z3kGaUM<Rt5H0Q~Zne$~?v4MyAU}0fCxaU_4zalHNj#2Q{lr+^0tj7#ac8=eZGX;V7
z|HHG_(AH?8-AD+8*+0y(I2B1(Q2c}kp#8J)bQDM8O1mch!lzm1^~tMa9Uwi9kyHs%
z#ifu*?exY`Ca736?`!V_izDRG%U``deRYI{aq=WP{~HtA*zw;l+Wqk`X-8QyYab=~
zg&a@Xy;Sz6XYENO`%fl!Ir=2ZdSiiwVg7%T_>kTnK0Rt1;_1=;1g8nZbd<_4T*tfe
z769W1{8RoBhR-V)4fFH(b?ZUu?Jq^|bCB#UW0)NL#^j*n3qI!(5ksuph5dy!x8}C=
zl%<@71<sv2hVu+pntLgm`?JTgBz<KqAbrGsSDTk*c@QXHl@HkEXK&FSK$i}fTq*R-
z16iS5MDaUc@%ajLRA|RGQvtyyQ(n(fD<p%pYRu;~S&fl7oW0|-mU30)qV$f7zpIr3
z-qdc{ysql8x6NqG%b+=ML)GHa7A!z(`9@Y%jdZB8s;CC+se|IM$(9YuO`O=r8CU0P
zv#-sG))qYWTFjj4FY|S^rq-czL#-OTsW$KI*n#y8tZV^q0you`T!rq9h1F#9mTIZy
z1uPdkps?eBsfMZ=ys1_$JgodxwPFvahx7)Wf5V&~J4cZI?v;1^a_23>`Mgiqymt9D
z=qhSsO*w$;@DU3)6e=g&R3*SIoRfvk*hkHjubY4DCnDLE{bcv>C%b9BE2gG(nlif2
z5UMFEn^TOOibTPa+f;SdvDs;I9woqMhAO}#Y}Eci77fqh=-Ef${*A$|fAlaT@y`8&
zY}}2q{27R1KlSd5<m$RQ5#w%>=ji7!QP3^+J_b16YY7`zduI^6$W2L$I?V*A+Hp3O
z>3F2Q^QbGwLafn(E22>^(RP-MM1>~obabwR`>y}7_9mBc+C|AvM2g}rO~#W{CPVFa
z`)L-V;6;t(obEW6Vhg0$Al|3efRc;_6{IekhC#gMZ}zqY6{vRT{t;~=tv4SVIq?LX
z5fS?|&wHAuVv{CU2{Z}NTeJf!gtpR|c2lF*kFpH<lbI&USclWZGtCFu9hfavFV$9W
zpn3E#C^kI)1E|g405HpU8oY*|!`A?P9`Ks&@D_Ln+(yw+EOZsi=a2Xnc-lj-c83H0
z$bO8z<K6$R*S{mm_X$OTb1+QH`3(p(cGg470p@dS_G3!YfpuYvp90&;LNQs2-yo$2
zE^+j=aoKpwq&Fxjb1i;lp<#J9zQXqi^l&7nT_h*9=4h7Yd*u(DPW~=j>qV1Eg2+*1
zimY(XvveZxl>`-cv7??%*%Ci=W##omrb7h(1Ur5Uk-P!G;2RJA2;e>P(e=(lZ+s|r
zyysCzA4S6?40RBO!*M*#h^~a;<uuBQ74bbN7C$HOiU9TP4*->$pgvBip3?<~2-W>h
z%pB<-M(HRF_gcCXhVi%y_3mgooLm)$w#K=(^Q&A?OvE98F5$wYSL;%AmKzU1%Xw-n
zt1fRNd|y31GMB2ZFXvEtS&mr>v!FN&mjKyR+@J0xeOndxtv5a!BwcCD6rW*>c8#77
hj%H4J*G>xGJR@B>)gDq5Y3W<LaQ6<c*p1J9{w;STq}Kod