From 0ab4027e7bac8ed791976d3bc7685bf4e146160d Mon Sep 17 00:00:00 2001
From: zhm-real <zhou.hm0420@gmail.com>
Date: Thu, 25 Jun 2020 17:59:40 -0700
Subject: [PATCH] add ARA*

---
 Search-based Planning/.idea/workspace.xml     |  61 +++----
 .../__pycache__/plotting.cpython-37.pyc       | Bin 2651 -> 2651 bytes
 .../__pycache__/queue.cpython-37.pyc          | Bin 2529 -> 2652 bytes
 Search-based Planning/a_star.py               |  80 +++++-----
 Search-based Planning/ara_star.py             | 149 +++++++++++++++++-
 Search-based Planning/queue.py                |   3 +
 6 files changed, 218 insertions(+), 75 deletions(-)

diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 6847f38..3567508 100644
--- a/Search-based Planning/.idea/workspace.xml	
+++ b/Search-based Planning/.idea/workspace.xml	
@@ -24,6 +24,7 @@
       <change beforePath="$PROJECT_DIR$/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/a_star.py" 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" />
+      <change beforePath="$PROJECT_DIR$/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/queue.py" afterDir="false" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -78,6 +79,27 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
+    <configuration name="ara_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/ara_star.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
     <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -141,34 +163,13 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="searching" type="PythonConfigurationType" factoryName="Python" temporary="true">
-      <module name="Search-based Planning" />
-      <option name="INTERPRETER_OPTIONS" value="" />
-      <option name="PARENT_ENVS" value="true" />
-      <envs>
-        <env name="PYTHONUNBUFFERED" value="1" />
-      </envs>
-      <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
-      <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$/searching.py" />
-      <option name="PARAMETERS" value="" />
-      <option name="SHOW_COMMAND_LINE" value="false" />
-      <option name="EMULATE_TERMINAL" value="false" />
-      <option name="MODULE_MODE" value="false" />
-      <option name="REDIRECT_INPUT" value="false" />
-      <option name="INPUT_FILE" value="" />
-      <method v="2" />
-    </configuration>
     <recent_temporary>
       <list>
         <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.ara_star" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.dfs" />
         <item itemvalue="Python.dijkstra" />
-        <item itemvalue="Python.searching" />
       </list>
     </recent_temporary>
   </component>
@@ -203,22 +204,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="296" key="GridCell.Tab.0.bottom" timestamp="1593123745200">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593132890178">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
-    <state width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1593123745200">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593132890178" />
+    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593132890178">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
-    <state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1593123745199">
+    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593132890178" />
+    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593132890178">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745199" />
-    <state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1593123745200">
+    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593132890178" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593132890178">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593123745200" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593132890178" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc
index a7403eaac81f82c3cb71841137e4f8463037586f..b091284ac433d337150543a83bdfdcdd52e6ab3c 100644
GIT binary patch
delta 19
ZcmcaDa$AJUiI<m)0SJ6NHgW}W0RS#41Ni^|

delta 19
ZcmcaDa$AJUiI<m)0SMTYHgW}W0RSt`1C9Uy

diff --git a/Search-based Planning/__pycache__/queue.cpython-37.pyc b/Search-based Planning/__pycache__/queue.cpython-37.pyc
index e1eea171de274cc2c058317faa45ecf23bf03d09..82b863fb23cba1e5f87d009af29470226dc0ab7a 100644
GIT binary patch
delta 164
zcmaDTd`E=OiI<m)0SE+izsBjXZseQC#%M8lEt?@v3U>=b6h{hAFoP!V=0|Lc85wU)
zZsXW0o6HDO4g???gq?x7m<>qOFl0?GWLMU|#hIE{nwwgbSdwZBl4AlA42)GGfu*UX
tsR2cq`9*qg1%8^GlkGS)8RaIg=M<k@#mU9R160hx!otMCJh_K66aXu2CA<It

delta 63
zcmca3@=%!1iI<m)0SKzhKg9iD*~mAKZDO1G=GSbC7#X7`_i=2UY{ljvC=OJh4aCJD
Nr5r3QOq1EULIFsk4|4zj

diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py
index e7fabf1..5d4cfb9 100644
--- a/Search-based Planning/a_star.py	
+++ b/Search-based Planning/a_star.py	
@@ -6,14 +6,21 @@ import env
 class Astar:
     def __init__(self, x_start, x_goal, e, heuristic_type):
         self.xI, self.xG = x_start, x_goal
-        self.e = e
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()  # class Env
 
+        self.e = e
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
 
+        self.g = {self.xI: 0, self.xG: float("inf")}
+        self.fig_name = "A* Algorithm"
+
+        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
+        self.OPEN.put(self.xI, self.fvalue(self.xI))
+        self.parent = {self.xI: self.xI}
+
     def searching(self):
         """
         Searching using A_star.
@@ -21,61 +28,46 @@ class Astar:
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        xI = self.xI
-        xG = self.xG
-        heuristic_type = self.heuristic_type
-        e = self.e
-
-        q_astar = queue.QueuePrior()  # priority queue / OPEN
-        q_astar.put(xI, e * self.Heuristic(xI, xG, heuristic_type))
-        parent = {xI: xI}  # record parents of nodes
-        action = {xI: (0, 0)}  # record actions of nodes
         visited = []
-        cost = {xI: 0}
 
-        while not q_astar.empty():
-            x_current = q_astar.get()
-            if x_current == xG:  # stop condition
+        while not self.OPEN.empty():
+            s = self.OPEN.get()
+            if s == self.xG:  # stop condition
                 break
-            visited.append(x_current)
+            visited.append(s)
             for u_next in self.u_set:  # explore neighborhoods of current node
-                x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
-                if x_next not in self.obs:
-                    new_cost = cost[x_current] + self.get_cost(x_current, u_next)
-                    if x_next not in cost or new_cost < cost[x_next]:  # conditions for updating cost
-                        cost[x_next] = new_cost
-                        priority = new_cost + e * self.Heuristic(x_next, xG, heuristic_type)
-                        q_astar.put(x_next, priority)  # put node into queue using priority "f+h"
-                        parent[x_next], action[x_next] = x_current, u_next
+                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]:  # conditions for updating cost
+                        self.g[s_next] = new_cost
+                        self.parent[s_next] = s
+                        self.OPEN.put(s_next, self.fvalue(s_next))
 
-        [path, policy] = self.extract_path(xI, xG, parent, action)
+        return self.extract_path(), visited
 
-        return path, policy, visited
+    def fvalue(self, x):
+        h = self.e * self.Heuristic(x)
+        return self.g[x] + h
 
-    @staticmethod
-    def extract_path(xI, xG, parent, policy):
+    def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
 
-        :param xI: Starting node
-        :param xG: Goal node
-        :param parent: Relationship between nodes
-        :param policy: Action needed for transfer between two nodes
         :return: The planning path
         """
 
-        path_back = [xG]
-        acts_back = [policy[xG]]
-        x_current = xG
-        while True:
-            x_current = parent[x_current]
-            path_back.append(x_current)
-            acts_back.append(policy[x_current])
+        path_back = [self.xG]
+        x_current = self.xG
 
-            if x_current == xI:
+        while True:
+            x_current = self.parent[x_current]
+            path_back.append(x_current)
+
+            if x_current == self.xI:
                 break
 
-        return list(path_back), list(acts_back)
+        return list(path_back)
 
     @staticmethod
     def get_cost(x, u):
@@ -90,8 +82,7 @@ class Astar:
 
         return 1
 
-    @staticmethod
-    def Heuristic(state, goal, heuristic_type):
+    def Heuristic(self, state):
         """
         Calculate heuristic.
 
@@ -101,6 +92,9 @@ class Astar:
         :return: heuristic
         """
 
+        heuristic_type = self.heuristic_type
+        goal = self.xG
+
         if heuristic_type == "manhattan":
             return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
         elif heuristic_type == "euclidean":
@@ -117,7 +111,7 @@ def main():
     plot = plotting.Plotting(x_start, x_goal)  # class Plotting
 
     fig_name = "A* Algorithm"
-    path, policy, visited = astar.searching()
+    path, visited = astar.searching()
 
     plot.animation(path, visited, fig_name)  # animation generate
 
diff --git a/Search-based Planning/ara_star.py b/Search-based Planning/ara_star.py
index 992b5fe..c5f9683 100644
--- a/Search-based Planning/ara_star.py	
+++ b/Search-based Planning/ara_star.py	
@@ -2,20 +2,165 @@ import queue
 import plotting
 import env
 
+import matplotlib.pyplot as plt
+
 
 class AraStar:
-    def __init__(self, x_start, x_goal):
+    def __init__(self, x_start, x_goal, heuristic_type):
         self.xI, self.xG = x_start, x_goal
+        self.heuristic_type = heuristic_type
 
         self.Env = env.Env()  # class Env
 
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
 
+        self.e = 3
+        self.g = {self.xI: 0, self.xG: float("inf")}
+        self.fig_name = "ARA_Star Algorithm"
+
+        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
+        self.CLOSED = []
+        self.INCONS = []
+        self.parent = {self.xI: self.xI}
+
+    def searching(self):
+        path = []
+
+        self.OPEN.put(self.xI, self.fvalue(self.xI))
+        self.ImprovePath()
+
+        path.append(self.extract_path())
+
+        while self.update_e() > 1:
+            self.e -= 0.5
+            OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS
+            self.OPEN = queue.QueuePrior()
+
+            for x in OPEN_mid:
+                self.OPEN.put(x, self.fvalue(x))
+            self.INCONS = []
+            self.CLOSED = []
+            self.ImprovePath()
+
+            path.append(self.extract_path())
+
+        return path
+
+    def ImprovePath(self):
+        while (not self.OPEN.empty() and self.fvalue(self.xG) >
+               min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
+            s = self.OPEN.get()
+            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
+                        if s_next not in self.CLOSED:
+                            self.OPEN.put(s_next, self.fvalue(s_next))
+                        else:
+                            self.INCONS.append(s_next)
+
+    def update_e(self):
+        c_OPEN, c_INCONS = float("inf"), float("inf")
+
+        if not self.OPEN.empty():
+            c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
+
+        if len(self.INCONS) != 0:
+            c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)
+
+        if min(c_OPEN, c_INCONS) == float("inf"):
+            return 1
+        else:
+            return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+
+    def fvalue(self, x):
+        h = self.e * self.Heuristic(x)
+        return self.g[x] + h
+
+    def extract_path(self):
+        """
+        Extract the path based on the relationship of nodes.
+
+        :param policy: Action needed for transfer between two nodes
+        :return: The planning path
+        """
+
+        path_back = [self.xG]
+        x_current = self.xG
+
+        while True:
+            x_current = self.parent[x_current]
+            path_back.append(x_current)
+
+            if x_current == self.xI:
+                break
+
+        return list(path_back)
+
+    @staticmethod
+    def get_cost(x, u):
+        """
+        Calculate cost for this motion
+
+        :param x: current node
+        :param u: input
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        return 1
+
+    def Heuristic(self, state):
+        """
+        Calculate heuristic.
+        :param state: current node (state)
+        :param goal: goal node (state)
+        :param heuristic_type: choosing different heuristic functions
+        :return: heuristic
+        """
+
+        heuristic_type = self.heuristic_type
+        goal = self.xG
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
+        elif heuristic_type == "euclidean":
+            return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
+        else:
+            print("Please choose right heuristic type!")
 
 
 def main():
     x_start = (5, 5)  # Starting node
     x_goal = (49, 5)  # Goal node
 
-    arastar = AraStar(x_start, x_goal)
+    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)
+
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/Search-based Planning/queue.py b/Search-based Planning/queue.py
index 6250cd3..8f481ae 100644
--- a/Search-based Planning/queue.py	
+++ b/Search-based Planning/queue.py	
@@ -57,3 +57,6 @@ class QueuePrior:
 
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item
+
+    def enumerate(self):
+        return self.queue