Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms

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()

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):
+        

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()