update ARA*

Search-based Planning/.idea/workspace.xml

@@ -19,7 +19,11 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <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$/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/ara_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
+    </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -51,7 +55,7 @@
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
-  <component name="RunManager" selected="Python.a_star">
+  <component name="RunManager" selected="Python.ara_star">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -159,8 +163,8 @@
     </configuration>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.ara_star" />
+        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.dfs" />
         <item itemvalue="Python.dijkstra" />
@@ -198,22 +202,22 @@
     </state>
     <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592802293738" />
     <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593133319558">
+    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593242428508">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>

Search-based Planning/ara_star.py

@@ -15,7 +15,7 @@ class AraStar:
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
 
-        self.e = 3
+        self.e = 2.5
         self.g = {self.xI: 0, self.xG: float("inf")}
         self.fig_name = "ARA_Star Algorithm"
 
@@ -24,47 +24,57 @@ class AraStar:
         self.INCONS = []
         self.parent = {self.xI: self.xI}
 
-    def searching(self):
-        path = []
+        self.path = []
+        self.visited = []
 
+    def searching(self):
         self.OPEN.put(self.xI, self.fvalue(self.xI))
         self.ImprovePath()
-
-        path.append(self.extract_path())
+        self.path.append(self.extract_path())
 
         while self.update_e() > 1:
             self.e -= 0.5
+            print(self.e)
             OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS
             self.OPEN = queue.QueuePrior()
+            self.OPEN.put(self.xI, self.fvalue(self.xI))
 
             for x in OPEN_mid:
                 self.OPEN.put(x, self.fvalue(x))
+
             self.INCONS = []
             self.CLOSED = []
             self.ImprovePath()
+            self.path.append(self.extract_path())
 
-            path.append(self.extract_path())
-
-        return path
+        return self.path, self.visited
 
     def ImprovePath(self):
-        while (not self.OPEN.empty() and self.fvalue(self.xG) >
+        visited_each = []
+        while (self.fvalue(self.xG) >
                min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
             s = self.OPEN.get()
-            self.CLOSED.append(s)
+
+            if s not in self.CLOSED:
+                self.CLOSED.append(s)
 
             for u_next in self.u_set:
                 s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
+
                 if s_next not in self.obs:
                     new_cost = self.g[s] + self.get_cost(s, u_next)
                     if s_next not in self.g or new_cost < self.g[s_next]:
                         self.g[s_next] = new_cost
                         self.parent[s_next] = s
+                        visited_each.append(s_next)
+
                         if s_next not in self.CLOSED:
                             self.OPEN.put(s_next, self.fvalue(s_next))
                         else:
                             self.INCONS.append(s_next)
 
+        self.visited.append(visited_each)
+
     def update_e(self):
         c_OPEN, c_INCONS = float("inf"), float("inf")
 
@@ -76,8 +86,8 @@ class AraStar:
 
         if min(c_OPEN, c_INCONS) == float("inf"):
             return 1
-        else:
-            return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+
+        return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
 
     def fvalue(self, x):
         h = self.e * self.Heuristic(x)
@@ -143,23 +153,10 @@ def main():
     arastar = AraStar(x_start, x_goal, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
-    path = arastar.searching()
-
-    plot.plot_grid("ARA*")
-
-    print(arastar.e)
-
-    for path_i in path:
-        path_i.remove(x_start)
-        path_i.remove(x_goal)
-
-        path_x = [path_i[i][0] for i in range(len(path_i))]
-        path_y = [path_i[i][1] for i in range(len(path_i))]
-
-        plt.plot(path_x, path_y, linewidth='3', marker='o')
-        plt.pause(1)
+    fig_name = "ARA* algorithm"
+    path, visited = arastar.searching()
 
-    plt.show()
+    plot.animation_ara_star(path, visited, fig_name)
 
 
 if __name__ == '__main__':

Search-based Planning/plotting.py

@@ -12,6 +12,7 @@ class Plotting:
         self.plot_grid(name)
         self.plot_visited(visited)
         self.plot_path(path)
+        plt.show()
 
     def plot_grid(self, name):
         obs_x = [self.obs[i][0] for i in range(len(self.obs))]
@@ -23,31 +24,62 @@ class Plotting:
         plt.title(name)
         plt.axis("equal")
 
-    def plot_visited(self, visited):
-        visited.remove(self.xI)
+    def plot_visited(self, visited, cl='gray'):
+        if self.xI in visited:
+            visited.remove(self.xI)
+
+        if self.xG in visited:
+            visited.remove(self.xG)
+
         count = 0
 
         for x in visited:
             count += 1
-            plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
+            plt.plot(x[0], x[1], linewidth='3', color=cl, marker='o')
             plt.gcf().canvas.mpl_connect('key_release_event',
                                          lambda event: [exit(0) if event.key == 'escape' else None])
 
             if count < len(visited) / 3:
                 length = 15
             elif count < len(visited) * 2 / 3:
-                length = 30
+                length = 25
             else:
-                length = 45
+                length = 35
+
+            if count % length == 0:
+                plt.pause(0.001)
+        plt.pause(0.01)
+
+    def plot_path(self, path, cl='r', flag=False):
+        if self.xI in path:
+            path.remove(self.xI)
 
-            if count % length == 0: plt.pause(0.001)
+        if self.xG in path:
+            path.remove(self.xG)
 
-    def plot_path(self, path):
-        path.remove(self.xI)
-        path.remove(self.xG)
         path_x = [path[i][0] for i in range(len(path))]
         path_y = [path[i][1] for i in range(len(path))]
 
-        plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+        if not flag:
+            plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+        else:
+            plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
+
         plt.pause(0.01)
+
+    def animation_ara_star(self, path, visited, name):
+        self.plot_grid(name)
+        cl_v, cl_p = self.color_list()
+
+        for k in range(len(path)):
+            self.plot_visited(visited[k], cl_v[k])
+            self.plot_path(path[k], cl_p[k], True)
+            plt.pause(0.5)
+
         plt.show()
+
+    @staticmethod
+    def color_list():
+        cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']
+        cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm']
+        return cl_v, cl_p