update 2D

Search-based Planning/.idea/workspace.xml

@@ -20,11 +20,11 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/Search_2D/ida_star.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/Search_2D/test.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.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/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/a_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.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$/gif/Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Astar.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -70,7 +70,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.ida_star">
+  <component name="RunManager" selected="Python.bidirectional_a_star">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -113,7 +113,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="bidirectional_a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -125,7 +125,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/dfs.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -201,17 +201,17 @@
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.ara_star" />
       <item itemvalue="Python.a_star" />
-      <item itemvalue="Python.dfs" />
       <item itemvalue="Python.test" />
       <item itemvalue="Python.ida_star" />
+      <item itemvalue="Python.bidirectional_a_star" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.ida_star" />
+        <item itemvalue="Python.bidirectional_a_star" />
         <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.ida_star" />
         <item itemvalue="Python.test" />
         <item itemvalue="Python.ara_star" />
-        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/a_star.py

@@ -117,9 +117,9 @@ class Astar:
 
 def main():
     x_start = (5, 5)  # Starting node
-    x_goal = (49, 5)  # Goal node
+    x_goal = (49, 25)  # Goal node
 
-    astar = Astar(x_start, x_goal, 1, "manhattan")
+    astar = Astar(x_start, x_goal, 1, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)  # class Plotting
 
     fig_name = "A* Algorithm"

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -0,0 +1,149 @@
+"""
+Bidirectional_a_star 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import queue
+from Search_2D import plotting
+from Search_2D import env
+
+
+class BidirectionalAstar:
+    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.g_fore = {self.xI: 0, self.xG: float("inf")}
+        self.g_back = {self.xG: 0, self.xI: float("inf")}
+
+        self.OPEN_fore = queue.QueuePrior()
+        self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG))
+        self.OPEN_back = queue.QueuePrior()
+        self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
+
+        self.CLOSED_fore = []
+        self.CLOSED_back = []
+
+        self.Parent_fore = {self.xI: self.xI}
+        self.Parent_back = {self.xG: self.xG}
+
+    def searching(self):
+        visited_fore, visited_back = [], []
+        s_meet = self.xI
+
+        while not self.OPEN_fore.empty() and not self.OPEN_back.empty():
+
+            # solve foreward-search
+            s_fore = self.OPEN_fore.get()
+            if s_fore in self.Parent_back:
+                s_meet = s_fore
+                break
+            visited_fore.append(s_fore)
+            for u in self.u_set:
+                s_next = tuple([s_fore[i] + u[i] for i in range(len(s_fore))])
+                if s_next not in self.obs:
+                    new_cost = self.g_fore[s_fore] + self.get_cost(s_fore, u)
+                    if s_next not in self.g_fore:
+                        self.g_fore[s_next] = float("inf")
+                    if new_cost < self.g_fore[s_next]:
+                        self.g_fore[s_next] = new_cost
+                        self.Parent_fore[s_next] = s_fore
+                        self.OPEN_fore.put(s_next, new_cost + self.h(s_next, self.xG))
+
+            # solve backward-search
+            s_back = self.OPEN_back.get()
+            if s_back in self.Parent_fore:
+                s_meet = s_back
+                break
+            visited_back.append(s_back)
+            for u in self.u_set:
+                s_next = tuple([s_back[i] + u[i] for i in range(len(s_back))])
+                if s_next not in self.obs:
+                    new_cost = self.g_back[s_back] + self.get_cost(s_back, u)
+                    if s_next not in self.g_back:
+                        self.g_back[s_next] = float("inf")
+                    if new_cost < self.g_back[s_next]:
+                        self.g_back[s_next] = new_cost
+                        self.Parent_back[s_next] = s_back
+                        self.OPEN_back.put(s_next, new_cost + self.h(s_next, self.xI))
+
+        return self.extract_path(s_meet), visited_fore, visited_back
+
+    def extract_path(self, s):
+        path_back_fore = [s]
+        s_current = s
+
+        while True:
+            s_current = self.Parent_fore[s_current]
+            path_back_fore.append(s_current)
+
+            if s_current == self.xI:
+                break
+
+        path_back_back = []
+        s_current = s
+
+        while True:
+            s_current = self.Parent_back[s_current]
+            path_back_back.append(s_current)
+
+            if s_current == self.xG:
+                break
+
+        return list(reversed(path_back_fore)) + list(path_back_back)
+
+    def h(self, state, goal):
+        """
+        Calculate heuristic.
+        :param state: current node (state)
+        :param goal: goal node (state)
+        :return: heuristic
+        """
+
+        heuristic_type = self.heuristic_type
+
+        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!")
+
+    @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 main():
+    x_start = (5, 5)  # Starting node
+    x_goal = (49, 25)  # Goal node
+
+    bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
+    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+
+    fig_name = "Bidirectional-A* Algorithm"
+    path, v_fore, v_back = bastar.searching()
+    plot.animation_bi_astar(path, v_fore, v_back, fig_name)  # animation generate
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/plotting.py

@@ -89,6 +89,35 @@ class Plotting:
 
         plt.show()
 
+    def animation_bi_astar(self, path, v_fore, v_back, name):
+        self.plot_grid(name)
+        self.plot_visited_bi(v_fore, v_back)
+        self.plot_path(path)
+        plt.show()
+
+    def plot_visited_bi(self, v_fore, v_back):
+        if self.xI in v_fore:
+            v_fore.remove(self.xI)
+
+        if self.xG in v_back:
+            v_back.remove(self.xG)
+
+        len_fore, len_back = len(v_fore), len(v_back)
+
+        for k in range(max(len_fore, len_back)):
+            if k < len_fore:
+                plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o')
+            if k < len_back:
+                plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o')
+
+            plt.gcf().canvas.mpl_connect('key_release_event',
+                                         lambda event: [exit(0) if event.key == 'escape' else None])
+
+            if k % 10 == 0:
+                plt.pause(0.001)
+        plt.pause(0.01)
+
+
     @staticmethod
     def color_list():
         cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']