From b0dbc5966e978352974c27d4dce298ef9b2e54fd Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Tue, 30 Jun 2020 14:07:45 -0700
Subject: [PATCH] LRTA*

---
 Search-based Planning/Search_3D/Astar3D.py    |   4 ++-
 Search-based Planning/Search_3D/DP.py         |   5 ++++
 .../Search_3D/LRT_Astar3D.py                  |  26 +++++++++++++-----
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin 3471 -> 3471 bytes
 4 files changed, 27 insertions(+), 8 deletions(-)
 create mode 100644 Search-based Planning/Search_3D/DP.py

diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index a1341ae..702c8f7 100644
--- a/Search-based Planning/Search_3D/Astar3D.py	
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -111,6 +111,8 @@ class Weighted_A_star(object):
         self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0])  # item, priority = g + h
         self.CLOSED = set()
 
+        # self.h = Heuristic(self.Space, self.goal)
+
 if __name__ == '__main__':
-    Astar = Weighted_A_star(0.5)
+    Astar = Weighted_A_star(1)
     Astar.run()
diff --git a/Search-based Planning/Search_3D/DP.py b/Search-based Planning/Search_3D/DP.py
new file mode 100644
index 0000000..827a570
--- /dev/null
+++ b/Search-based Planning/Search_3D/DP.py	
@@ -0,0 +1,5 @@
+# Dynamic programming algorithm
+
+class DP(object):
+    def __init__(self, X, U, p0, pf, T, l, q, gamma):
+        
\ 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 30e6429..d414d57 100644
--- a/Search-based Planning/Search_3D/LRT_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LRT_Astar3D.py	
@@ -106,15 +106,24 @@ class LRT_A_star2:
         return allchild
         
     def updateHeuristic(self):
+        # Initialize at infinity
         for strxi in self.Astar.CLOSED:
             self.Astar.h[strxi] = np.inf
-            xi = dehash(strxi)
-            minfval = min([cost(xi, xj, settings=1) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
-            if self.Astar.h[strxi] >= minfval:
-                self.Astar.h[strxi] = minfval
+        # initialize difference
+        Diff = True
+        while Diff: # repeat until converge
+            hvals, lasthvals = [], []
+            for strxi in self.Astar.CLOSED:
+                xi = dehash(strxi)
+                lasthvals.append(self.Astar.h[strxi]) 
+                # update h values if they are smaller
+                minfval = min([cost(xi, xj, settings=1) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
+                if self.Astar.h[strxi] >= minfval:
+                    self.Astar.h[strxi] = minfval
+                hvals.append(self.Astar.h[strxi]) 
+            if lasthvals == hvals: Diff = False
 
     def move(self):
-        #self.Astar.Parent[xj] = dehash(self.Astar.lastpoint)
         strst = self.Astar.x0
         st = self.Astar.start
         ind = 0
@@ -143,12 +152,15 @@ class LRT_A_star2:
             visualization(self.Astar)
             self.updateHeuristic()
             self.move()
-        self.Astar.Path = self.path
+        print(hash3D(self.Astar.goal) in self.Astar.CLOSED)
+        self.updateHeuristic()
+        self.move()
+        self.Astar.Path = self.path # previous path (determined from DP) + last path (determined from A*)
         self.Astar.done = True
         visualization(self.Astar)
         plt.show()
 
 
 if __name__ == '__main__':
-    T = LRT_A_star2(resolution=1, N=100)
+    T = LRT_A_star2(resolution=0.5, N=1)
     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 339ed0a0505ccc63e50df3bf32ec6770c09bcf1c..faf44864dcbbcca64df898d793d48cf3c5807c87 100644
GIT binary patch
delta 643
zcmY*WzfTlF6u#MC?%v&+6G0OWe}E|Lfh5FOoDIQ55`*zia9y|s)-1P^w`1Tc8#@b%
zi81yxl;(Q=2s)>tA%up+&cs6Jn>BK7@$GxxoA>5>-yF0L+Trsstk?AS_3+o5%WvUN
z7Mvrk)lOH2b?b~-7fk-Jzx!jFI;v4LJaL|~i3y!iph0;F=V`wec{DGlyxB&Cih<5>
z-TTd0ORoE!4-Y`!0X#&Py`2hbAiWD%G+ff_QHXRI09AAg&;+zKs37gX-%TkIDLn!?
z3DVBfK20NXIc0sJ>{1ZTp?(jn!xQm%hwdY`c`lSw?i71bgXY1o*92|K)lK`Evi37t
zlGDND=pu40xL7s=XY^5{wh|9fmC&1*;cVp@j~p|Yy7Fy(yftD6&UWR;`teJ%)$M=1
zJ|Z(2-dt@)Q%GT2WK}a)K#Yu(pBmfI0!U-oDxxmn#s3GZ_mJ&o=M}YdDVkB&;<#6A
z#<6m>rF=kUfg8Jt;CUhPY-5QJ5Gk+v(=*+stfJKJ4$A71Mfth;<;g4xUa|2m-4t{k
a_pX6)S4)u)8A(@){|br21vmDY&;9_g+Kxd0

delta 679
zcmZWmO=}cE5KYflX0tm95fji=6Jty=yC{JWh!>5b2R#@Oye#N2$&Q;@COb_{i@PVU
z-u5tvzrkbJyZ!;sf_?JhAwNJ6FIulhOu&YDQ}wFr<5iEF<7RL#2xg}Ed_Mm5;?r^P
zQ@AIB-N~%=U05_%{;<FL^Gtl0qK(NHXIm_sXOC(O+KZ)32Hnu3^>W!;t#2S=Ae`)Z
zzlE49_x#qI70{OetH`o@iK05(%Ye4w;%=7$xSIe>L<@ih;39*rC4*N-3B@X*>mV<H
zw6kPD%dlKY=|E|_P(*8pUjggLSUo+YCD^vily-{4{B>BTbui*FVK(h@(lFK5Fr^#t
ztzo}oxPHg#rTq1^h$~^0y7E!&<eBMmmG2t~l_E@9O@9uBoR?WDL&tQZ9cDTXT4|rL
zP%){qMUlEwzMc8_&?sO0SIUQF8exB@>Zu7Aiw&)+#cdF0UZEV<dtnQtS?ew=j6wGS
zrx*XKT57bO^f<!j+nsPlPe)NVKZqjj^6KS?jHdacSjj9`S-O8yj$mo8`qM2=)K*^b
sD~yV&QM-KCc)xWG0WaV0CkKjV*`Wtu+`Cu;?SlAkm?&tw&wb&GKN@12T>t<8