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

Search-based Planning/Search_3D/DP.py

@@ -1,5 +0,0 @@
-# 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

@@ -1,4 +1,4 @@
-# this is the three dimensional near-sighted 1 neighborhood LRTA* algo
+# this is the three dimensional N>1 LRTA* algo
 # !/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
@@ -24,14 +24,6 @@ class LRT_A_star2:
         self.Astar = Astar3D.Weighted_A_star(resolution=resolution)
         self.path = []
 
-    def children(self, x):
-        allchild = []
-        resolution = self.Astar.env.resolution
-        for direc in self.Astar.Alldirec:
-            child = np.array(list(map(np.add, x, np.multiply(direc, resolution))))
-            allchild.append(hash3D(child))
-        return allchild
-
     def updateHeuristic(self):
         # Initialize hvalues at infinity
         for strxi in self.Astar.CLOSED:
@@ -74,7 +66,6 @@ class LRT_A_star2:
                 break
         self.Astar.reset(st)
 
-    
     def run(self):
         while True:
             if self.Astar.run(N=self.N):
@@ -88,5 +79,5 @@ class LRT_A_star2:
 
 
 if __name__ == '__main__':
-    T = LRT_A_star2(resolution=1, N=150)
+    T = LRT_A_star2(resolution=0.5, N=1)
     T.run()

Search-based Planning/Search_3D/RTA_Astar3D.py

@@ -0,0 +1,78 @@
+# this is the three dimensional Real-time Adaptive LRTA* algo
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+
+import os
+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 getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+    cost, obstacleFree
+from Search_3D.plot_util3D import visualization
+import queue
+
+class RTA_A_star:
+    def __init__(self, resolution=0.5, N=7):
+        self.N = N # node to expand 
+        self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star
+        self.path = [] # empty path
+        self.strst = []
+        self.localhvals = []
+
+    def updateHeuristic(self):
+        # Initialize hvalues at infinity
+        self.localhvals = []
+        nodeset, vals = [], []
+        for (_,strxi) in self.Astar.OPEN.enumerate():
+            nodeset.append(strxi)
+            vals.append(self.Astar.Space[strxi] + self.Astar.h[strxi])
+        strj, fj = nodeset[np.argmin(vals)], min(vals)
+        self.strst = strj
+        # single pass update of hvals
+        for strxi in self.Astar.CLOSED:
+            # xi = dehash(strxi)
+            self.Astar.h[strxi] = fj - self.Astar.Space[strxi]
+            self.localhvals.append(self.Astar.h[strxi])
+        
+    def move(self):
+        strst, localhvals = self.strst, self.localhvals
+        maxhval = max(localhvals)
+        st = dehash(strst)
+        sthval = self.Astar.h[strst]
+        # find the lowest path up hill
+        while sthval < maxhval:
+            parentsvals , parents = [] , []
+            # find the max child
+            for xi in self.Astar.children(st):
+                strxi = hash3D(xi)
+                if strxi in self.Astar.CLOSED:
+                    parents.append(xi)
+                    parentsvals.append(self.Astar.h[strxi])
+            lastst = st            
+            st, strst = parents[np.argmax(parentsvals)], hash3D(st)
+            self.path.append([st,lastst]) # add to path
+            sthval = self.Astar.h[strst]
+        self.Astar.reset(dehash(self.strst))
+
+    def run(self):
+        while True:
+            if self.Astar.run(N=self.N):
+                self.Astar.Path = self.Astar.Path + self.path
+                self.Astar.done = True
+                visualization(self.Astar)
+                plt.show()
+                break
+            self.updateHeuristic()
+            self.move()
+
+
+if __name__ == '__main__':
+    T = RTA_A_star(resolution=0.5, N=500)
+    T.run()