A*3d

README.md

@@ -27,15 +27,25 @@ Directory Structure
             └── rrtstar3D.py
 
 ## Animations
-### Dijkstra's & A*
+### DFS & BFS (Dijkstra)
 * Blue: starting state
 * Green: goal state
 
 <div align=right>
 <table>
   <tr>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Dijkstra.gif" alt="dijkstra" width="400"/></a></td>
-    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Astar.gif" alt="Astar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/DFS.gif" alt="dfs" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/BFS.gif" alt="bfs" width="400"/></a></td>
+  </tr>
+</table>
+</div>
+
+### A* and A* Variants
+<div align=right>
+<table>
+  <tr>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
+    <td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search-based%20Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
   </tr>
 </table>
 </div>

Search-based Planning/.idea/Search-based Planning.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Search-based Planning/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
 </project>

Search-based Planning/.idea/workspace.xml

@@ -20,20 +20,13 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../nano.save" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/../nano.save.1" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/a_star.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/ara_star.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/bfs.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/dfs.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/dijkstra.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/env.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/plotting.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/queue.py" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bfs.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dfs.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/DFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/DFS.gif" afterDir="false" />
     </list>
+    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -61,12 +54,24 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
     <property name="restartRequiresConfirmation" value="false" />
-    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
+    <property name="settings.editor.selected.configurable" value="reference.settingsdialog.IDE.editor.colors.Console Colors" />
   </component>
-  <component name="RunManager" selected="Python.ara_star">
-    <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunDashboard">
+    <option name="ruleStates">
+      <list>
+        <RuleState>
+          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
+        </RuleState>
+        <RuleState>
+          <option name="name" value="StatusDashboardGroupingRule" />
+        </RuleState>
+      </list>
+    </option>
+  </component>
+  <component name="RunManager" selected="Python.bfs">
+    <configuration name="a_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" />
@@ -74,11 +79,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <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$/a_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/a_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -87,7 +92,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="ara_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+    <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -95,11 +100,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <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="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -108,7 +113,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bfs" 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" />
@@ -116,11 +121,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <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$/bfs.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" />
@@ -137,11 +142,32 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <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="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="dijkstra" type="PythonConfigurationType" factoryName="Python" 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$/Search_2D" />
       <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$/dfs.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" />
@@ -150,7 +176,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="ida_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -158,11 +184,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <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$/dijkstra.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ida_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -171,13 +197,21 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
+    <list>
+      <item itemvalue="Python.dijkstra" />
+      <item itemvalue="Python.a_star" />
+      <item itemvalue="Python.ida_star" />
+      <item itemvalue="Python.bidirectional_a_star" />
+      <item itemvalue="Python.bfs" />
+      <item itemvalue="Python.dfs" />
+    </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.ara_star" />
-        <item itemvalue="Python.a_star" />
         <item itemvalue="Python.bfs" />
         <item itemvalue="Python.dfs" />
-        <item itemvalue="Python.dijkstra" />
+        <item itemvalue="Python.bidirectional_a_star" />
+        <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.ida_star" />
       </list>
     </recent_temporary>
   </component>
@@ -199,39 +233,44 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State />
+            <State>
+              <option name="COLUMN_ORDER" />
+            </State>
           </value>
         </entry>
       </map>
     </option>
-    <option name="oldMeFiltersMigrated" value="true" />
   </component>
   <component name="WindowStateProjectService">
-    <state x="819" y="314" width="424" height="482" key="FileChooserDialogImpl" timestamp="1592933974409">
-      <screen x="65" y="24" width="1855" height="1056" />
+    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog" timestamp="1593282711181">
+      <screen x="1920" y="0" width="1920" height="1080" />
+    </state>
+    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282711181" />
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593282670147">
+      <screen x="1920" y="0" width="1920" height="1080" />
     </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="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282670147" />
     <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="1593243345942">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593282755222">
       <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="1593243345942" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593282755222">
       <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="1593243345941" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593282755222">
       <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="1593243345941" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593282755222">
       <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="1593243345941" />
-    <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
+    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state x="2406" y="174" key="SettingsEditor" timestamp="1593282573348">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801555194" />
+    <state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282573348" />
     <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2" timestamp="1593242832142">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>

Search-based Planning/Search_2D/a_star.py

@@ -26,11 +26,10 @@ class Astar:
         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}
+        self.CLOSED = []
+        self.Parent = {self.xI: self.xI}
 
     def searching(self):
         """
@@ -39,23 +38,25 @@ class Astar:
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        visited = []
-
         while not self.OPEN.empty():
             s = self.OPEN.get()
+            self.CLOSED.append(s)
+
             if s == self.xG:  # stop condition
                 break
-            visited.append(s)
+
             for u_next in self.u_set:  # explore neighborhoods of current node
                 s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
-                if s_next not in self.obs:
+                if s_next not in self.obs and s_next not in self.CLOSED:
                     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
+                    if s_next not in self.g:
+                        self.g[s_next] = float("inf")
+                    if new_cost < self.g[s_next]:  # conditions for updating cost
                         self.g[s_next] = new_cost
-                        self.parent[s_next] = s
+                        self.Parent[s_next] = s
                         self.OPEN.put(s_next, self.fvalue(s_next))
 
-        return self.extract_path(), visited
+        return self.extract_path(), self.CLOSED
 
     def fvalue(self, x):
         h = self.e * self.Heuristic(x)
@@ -72,7 +73,7 @@ class Astar:
         x_current = self.xG
 
         while True:
-            x_current = self.parent[x_current]
+            x_current = self.Parent[x_current]
             path_back.append(x_current)
 
             if x_current == self.xI:
@@ -116,14 +117,13 @@ 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"
     path, visited = astar.searching()
-
     plot.animation(path, visited, fig_name)  # animation generate
 
 

Search-based Planning/Search_2D/bfs.py

@@ -85,5 +85,5 @@ class BFS:
 
 if __name__ == '__main__':
     x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 5)  # Goal node
+    x_Goal = (49, 25)  # Goal node
     bfs = BFS(x_Start, x_Goal)

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/dfs.py

@@ -85,5 +85,5 @@ class DFS:
 
 if __name__ == '__main__':
     x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 5)  # Goal node
+    x_Goal = (49, 25)  # Goal node
     dfs = DFS(x_Start, x_Goal)

Search-based Planning/Search_2D/dijkstra.py

@@ -100,5 +100,5 @@ class Dijkstra:
 
 if __name__ == '__main__':
     x_Start = (5, 5)  # Starting node
-    x_Goal = (49, 5)  # Goal node
+    x_Goal = (49, 25)  # Goal node
     dijkstra = Dijkstra(x_Start, x_Goal)

Search-based Planning/Search_2D/ida_star.py

@@ -0,0 +1,90 @@
+"""
+IDA_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 IdaStar:
+    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
+
+    def ida_star(self):
+        bound = self.h(self.xI)
+        path = [self.xI]
+
+        while True:
+            t = self.searching(path, 0, bound)
+            if t == self.xG:
+                return path
+            if t == float("inf"):
+                return None
+            bound = t
+
+    def searching(self, path, g, bound):
+        s = path[-1]
+        f = g + self.h(s)
+
+        if f > bound:
+            return f
+        if s == self.xG:
+            return s
+
+        res_min = float("inf")
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(len(s))])
+            if s_next not in self.obs and s_next not in path:
+                path.append(s_next)
+                t = self.searching(path, g + 1, bound)
+                if t == self.xG:
+                    return self.xG
+                if t < res_min:
+                    res_min = t
+                path.pop()
+
+        return res_min
+
+    def h(self, s):
+        heuristic_type = self.heuristic_type
+        goal = self.xG
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        elif heuristic_type == "euclidean":
+            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
+        else:
+            print("Please choose right heuristic type!")
+
+
+def main():
+    x_start = (5, 5)  # Starting node
+    x_goal = (15, 25)  # Goal node
+
+    ida_star = IdaStar(x_start, x_goal, "manhattan")
+    plot = plotting.Plotting(x_start, x_goal)
+
+    path = ida_star.ida_star()
+
+    if path:
+        plot.animation(path, [], "IDA_Star")
+    else:
+        print("Path not found!")
+
+
+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']

Search-based Planning/Search_2D/queue.py

@@ -53,7 +53,14 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
-        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+        count = 0
+        for (p, x) in self.queue:
+            if x == item:
+                self.queue[count] = (priority, item)
+                break
+            count += 1
+        if count == len(self.queue):
+            heapq.heappush(self.queue, (priority, item))  # reorder x using priority
 
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item

Search-based Planning/Search_2D/test.py

@@ -0,0 +1,20 @@
+"""
+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
+
+q = queue.QueuePrior()
+q.put((1, 2), 3)
+print(q.enumerate())
+q.put((1, 2), 2)
+print(q.enumerate())
+q.put((1, 2), 4)
+print(q.enumerate())

Search-based Planning/Search_3D/Astar3D.py

@@ -51,9 +51,9 @@ class Weighted_A_star(object):
         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[strxi] = [] # add the point in CLOSED set
             self.V.append(xi)
             visualization(self)
-            self.CLOSED[strxi] = [] # add the point in CLOSED set
             allchild = self.children(xi)
             for xj in allchild:
                 strxj = hash3D(xj)