update 3D

Search-based Planning/.idea/workspace.xml

@@ -20,9 +20,10 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/LPAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Astar3D.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -68,7 +69,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.astar">
+  <component name="RunManager" selected="Python.LRT_Astar3D">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -90,7 +91,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -98,11 +99,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D\LRTAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -111,7 +112,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -119,11 +120,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -197,19 +198,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.LRTA_star" />
-      <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
       <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.astar" />
+      <item itemvalue="Python.LRT_Astar3D" />
+      <item itemvalue="Python.Astar3D" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.LRT_Astar3D" />
+        <item itemvalue="Python.Astar3D" />
         <item itemvalue="Python.astar" />
         <item itemvalue="Python.ARAstar" />
         <item itemvalue="Python.RTAAstar" />
-        <item itemvalue="Python.LRTAstar" />
-        <item itemvalue="Python.LRTA_star" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/LPAstar.py

@@ -15,3 +15,4 @@ from Search_2D import env
 
 class LpaStar:
     def __init__(self):
+        return

Search-based Planning/Search_3D/Astar3D.py

@@ -12,46 +12,51 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
+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 Weighted_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) # key is the point, store g value
-        self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
+    def __init__(self, resolution=1):
+        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)  # key is the point, store g value
+        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 # set g(x0) = 0
-        self.OPEN = queue.QueuePrior() # store [point,priority]
-        self.h = Heuristic(self.Space,self.goal)
+        self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
+
+        self.h = Heuristic(self.Space, self.goal)
         self.Parent = {}
         self.CLOSED = set()
         self.V = []
         self.done = False
         self.Path = []
         self.ind = 0
+        self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
+        self.OPEN = queue.QueuePrior()  # store [point,priority]
+        self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0])  # item, priority = g + h
 
-    def children(self,x):
+    def children(self, x):
         allchild = []
         for j in self.Alldirec:
-            collide,child = isCollide(self,x,j)
+            collide, child = isCollide(self, x, j)
             if not collide:
                 allchild.append(child)
         return allchild
 
     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
-        while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
-            strxi = self.OPEN.get()           
+        xt = self.xt
+        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)
-            self.CLOSED.add(strxi) # add the point in CLOSED set
+            self.CLOSED.add(strxi)  # add the point in CLOSED set
             self.V.append(xi)
             visualization(self)
             allchild = self.children(xi)
@@ -59,22 +64,23 @@ class Weighted_A_star(object):
                 strxj = hash3D(xj)
                 if strxj not in self.CLOSED:
                     gi, gj = self.Space[strxi], self.Space[strxj]
-                    a = gi + cost(xi,xj)
+                    a = gi + cost(xi, xj)
                     if a < gj:
                         self.Space[strxj] = a
                         self.Parent[strxj] = xi
                         if (a, strxj) in self.OPEN.enumerate():
                             # update priority of xj
-                            self.OPEN.put(strxj, a+1*self.h[strxj])
+                            self.OPEN.put(strxj, a + 1 * self.h[strxj])
                         else:
                             # add xj in to OPEN set
-                            self.OPEN.put(strxj, a+1*self.h[strxj])
+                            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:
+            if N:
+                if len(self.CLOSED) % N == 0:
                     break
-            if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
+            if self.ind % 100 == 0: print('iteration number = ' + str(self.ind))
             self.ind += 1
+
         # if the path finding is finished
         if xt in self.CLOSED:
             self.done = True
@@ -87,11 +93,12 @@ class Weighted_A_star(object):
         strx = hash3D(self.goal)
         strstart = hash3D(self.start)
         while strx != strstart:
-            path.append([dehash(strx),self.Parent[strx]])
+            path.append([dehash(strx), self.Parent[strx]])
             strx = hash3D(self.Parent[strx])
-        path = np.flip(path,axis=0)
+        path = np.flip(path, axis=0)
         return path
 
+
 if __name__ == '__main__':
     Astar = Weighted_A_star(1)
-    Astar.run()
+    Astar.run()

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -12,8 +12,9 @@ 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 import Astar3D
+from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+    cost
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -91,31 +92,22 @@ import queue
 #         return path
 
 class LRT_A_star2():
-    def __init__(self,resolution=0.5, N=7):
+    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) 
+        self.Astar = Astar3D.Weighted_A_star()
+
+        while True:
+            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)])
-    
+            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):
-        xt = hash3D(self.Astar.goal)
-        while xt not in self.Astar.CLOSED:
-            self.expand()
-            #self.updateHeuristic()
 
 
 if __name__ == '__main__':
-    T = LRT_A_star2(resolution = 1, N = 2)
-    T.run()
+    T = LRT_A_star2(resolution=1, N=50)