update 3D

Search-based Planning/.idea/workspace.xml

@@ -21,12 +21,8 @@
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" 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_2D/LRTAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTAstar.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/RTAAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/RTAAstar.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/env.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/test.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/test.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" />
@@ -72,8 +68,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.RTAAstar">
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.LRT_Astar3D">
+    <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" />
@@ -81,11 +77,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/LPAstar.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" />
@@ -94,7 +90,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -106,7 +102,7 @@
       <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_2D/LPAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -115,7 +111,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -127,7 +123,7 @@
       <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/RTAAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -136,7 +132,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bfs" 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" />
@@ -144,11 +140,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/bfs.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" />
@@ -157,7 +153,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
+    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -169,7 +165,7 @@
       <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/dijkstra.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/RTAAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -178,7 +174,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="test" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -190,7 +186,7 @@
       <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/test.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -201,19 +197,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.test" />
       <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.bfs" />
       <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
+      <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.RTAAstar" />
         <item itemvalue="Python.LRTAstar" />
         <item itemvalue="Python.LPAstar" />
-        <item itemvalue="Python.bfs" />
-        <item itemvalue="Python.test" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_3D/Astar3D.py

@@ -54,6 +54,7 @@ class Weighted_A_star(object):
 
     def run(self, N=None):
         xt = self.xt
+        strxi = self.x0
         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)
@@ -85,25 +86,29 @@ class Weighted_A_star(object):
 
         self.lastpoint = strxi
         # if the path finding is finished
-        if xt in self.CLOSED and N is None:
+        if xt in self.CLOSED:
             self.done = True
             self.Path = self.path()
-            visualization(self)
-            plt.show()
+            if N is None:
+                visualization(self)
+                plt.show()
+            return True
+
+        return False
 
     def path(self):
         path = []
         strx = self.lastpoint
-        #strstart = hash3D(getNearest(self.Space, self.env.start))
+        # strstart = hash3D(getNearest(self.Space, self.env.start))
         strstart = self.x0
         while strx != strstart:
             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
 
     # utility used in LRTA*
-    def reset(self,xj):
+    def reset(self, xj):
         self.Space = StateSpace(self)  # key is the point, store g value
         self.start = xj
         self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
@@ -113,6 +118,7 @@ class Weighted_A_star(object):
 
         # self.h = Heuristic(self.Space, self.goal)
 
+
 if __name__ == '__main__':
     Astar = Weighted_A_star(1)
     Astar.run()

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -101,43 +101,43 @@ class LRT_A_star2:
         allchild = []
         resolution = self.Astar.env.resolution
         for direc in self.Astar.Alldirec:
-            child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+            child = np.array(list(map(np.add, x, np.multiply(direc, resolution))))
             allchild.append(hash3D(child))
         return allchild
-        
+
     def updateHeuristic(self):
         # Initialize at infinity
         for strxi in self.Astar.CLOSED:
             self.Astar.h[strxi] = np.inf
         # initialize difference
         Diff = True
-        while Diff: # repeat until converge
+        while Diff:  # repeat until converge
             hvals, lasthvals = [], []
             for strxi in self.Astar.CLOSED:
                 xi = dehash(strxi)
-                lasthvals.append(self.Astar.h[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]) 
+                hvals.append(self.Astar.h[strxi])
             if lasthvals == hvals: Diff = False
 
     def move(self):
         strst = self.Astar.x0
         st = self.Astar.start
         ind = 0
-        while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop
+        while strst in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in OPEN, stop
             # strChildren = self.children(st)
             strChildren = [hash3D(i) for i in self.Astar.children(st)]
-            minh , minchild = np.inf , None
+            minh, minchild = np.inf, None
             for child in strChildren:
                 h = self.Astar.h[child]
                 if h <= minh:
-                    minh , minchild = h , dehash(child)
-            self.path.append([st,minchild])
-            strst, st = hash3D(minchild), minchild 
-            for (_,strp) in self.Astar.OPEN.enumerate():
+                    minh, minchild = h, dehash(child)
+            self.path.append([st, minchild])
+            strst, st = hash3D(minchild), minchild
+            for (_, strp) in self.Astar.OPEN.enumerate():
                 if strp == strst:
                     break
             ind += 1
@@ -146,21 +146,17 @@ class LRT_A_star2:
         self.Astar.reset(st)
 
     def run(self):
-        while self.Astar.lastpoint != hash3D(self.Astar.goal):
-            self.Astar.run(N=self.N)
-            #print(path)
-            visualization(self.Astar)
+        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()
-        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=0.5, N=1)
+    T = LRT_A_star2(resolution=1, N=30)
     T.run()