Merge branch 'master' of github.com:zhm-real/path-planning-algorithms

README.md

@@ -11,10 +11,14 @@ Directory Structure
             ├── ARAstar.py                              # Anytime Reparing A*
             ├── IDAstar.py                              # Iteratively Deepening A*
             ├── LRTAstar.py                             # Learning Real-time A*
-            └── RTAAstar.py                             # Real-time Adaptive A*
+            ├── RTAAstar.py                             # Real-time Adaptive A*
+            ├── LPAstar.py                              # Lifelong Planning A*
+            ├── D_star.py                               # D* (Dynamic A*)
+            └── D_star_Lite.py                          # D* Lite
         └── Search_3D
             ├── Astar3D.py                              # A*_3D
             ├── bidirectional_Astar3D.py                # Bidirectional A*_3D
+            ├── RTA_Astar3D.py                          # Real-time Adaptive A*_3D
             └── LRT_Astar3D.py                          # Learning Real-time A*_3D
         └── gif                                         # Animations
     └── Sampling-based Planning

Sampling-based Planning/.idea/Sampling-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>

Sampling-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>

Sampling-based Planning/rrt_2D/rrt_star.py

@@ -161,7 +161,7 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 20000)
+    rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000)
     path = rrt_star.planning()
 
     if path:

Search-based Planning/.idea/workspace.xml

@@ -20,13 +20,21 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/.idea/Sampling-based Planning.iml" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/Sampling-based Planning.iml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/misc.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/rrt_2D/rrt_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/rrt_2D/rrt_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/Search-based Planning.iml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/Search-based Planning.iml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/misc.xml" 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/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$/gif/LRTA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/LRTA_star.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/RTAA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/RTAA_star.gif" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.py" 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/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="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/bidirectional_a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.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$/Search_2D/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/env.py" 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" />
@@ -54,10 +62,15 @@
     <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$/../../PythonRobotics-master/PathPlanning" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../Sampling-based Planning" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
+  <component name="RecentsManager">
+    <key name="MoveFile.RECENT_KEYS">
+      <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
+    </key>
+  </component>
   <component name="RunDashboard">
     <option name="ruleStates">
       <list>
@@ -70,8 +83,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.RTAAstar">
-    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.LPAstar">
+    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -79,11 +92,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <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_3D/Astar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ARAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -113,7 +126,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -125,7 +138,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/astar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -134,7 +147,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRT_Astar3D" 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" />
@@ -142,11 +155,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <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_3D/LRT_Astar3D.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" />
@@ -155,7 +168,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -167,7 +180,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/dfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -199,19 +212,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
+      <item itemvalue="Python.astar" />
+      <item itemvalue="Python.dfs" />
+      <item itemvalue="Python.bidirectional_a_star" />
+      <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.LRTAstar" />
-      <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.LRT_Astar3D" />
-      <item itemvalue="Python.Astar3D" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.RTAAstar" />
-        <item itemvalue="Python.LRTAstar" />
-        <item itemvalue="Python.LRT_Astar3D" />
-        <item itemvalue="Python.Astar3D" />
         <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.ARAstar" />
+        <item itemvalue="Python.bidirectional_a_star" />
+        <item itemvalue="Python.astar" />
+        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>
@@ -226,6 +239,14 @@
       <option name="presentableId" value="Default" />
       <updated>1592347358698</updated>
     </task>
+    <task id="LOCAL-00001" summary="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit.">
+      <created>1593715021929</created>
+      <option name="number" value="00001" />
+      <option name="presentableId" value="LOCAL-00001" />
+      <option name="project" value="LOCAL" />
+      <updated>1593715021929</updated>
+    </task>
+    <option name="localTasksCounter" value="2" />
     <servers />
   </component>
   <component name="Vcs.Log.Tabs.Properties">
@@ -241,39 +262,43 @@
       </map>
     </option>
   </component>
+  <component name="VcsManagerConfiguration">
+    <MESSAGE value="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit." />
+    <option name="LAST_COMMIT_MESSAGE" value="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit." />
+  </component>
   <component name="WindowStateProjectService">
     <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">
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593631602696">
       <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="1593282670147" />
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593631602696" />
     <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="1593282755222">
+    <state width="1832" height="146" key="GridCell.Tab.0.bottom" timestamp="1593715356351">
       <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="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593282755222">
+    <state width="1832" height="146" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
+    <state width="1832" height="146" key="GridCell.Tab.0.center" timestamp="1593715356351">
       <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="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593282755222">
+    <state width="1832" height="146" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
+    <state width="1832" height="146" key="GridCell.Tab.0.left" timestamp="1593715356351">
       <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="1593282755222" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593282755222">
+    <state width="1832" height="146" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
+    <state width="1832" height="146" key="GridCell.Tab.0.right" timestamp="1593715356351">
       <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="1593282755222" />
+    <state width="1832" height="146" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715356351" />
     <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="1593282573348" />
-    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2" timestamp="1593242832142">
+    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2" timestamp="1593715195096">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242832142" />
+    <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593715195096" />
   </component>
 </project>

Search-based Planning/Search_2D/ARAstar.py

@@ -5,6 +5,7 @@ ARA_star 2D (Anytime Repairing A*)
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -15,8 +16,8 @@ from Search_2D import env
 
 
 class AraStar:
-    def __init__(self, x_start, x_goal, e, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal, e, heuristic_type):
+        self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()                                    # class Env
@@ -24,17 +25,17 @@ class AraStar:
         self.u_set = self.Env.motions                           # feasible input set
         self.obs = self.Env.obs                                 # position of obstacles
         self.e = e                                              # initial weight
-        self.g = {self.xI: 0, self.xG: float("inf")}            # cost to come
+        self.g = {self.s_start: 0, self.s_goal: float("inf")}            # cost to come
 
-        self.OPEN = queue.QueuePrior()                          # priority queue / OPEN
+        self.OPEN = queue.QueuePrior()                          # priority queue / U
         self.CLOSED = set()                                     # closed set
         self.INCONS = []                                        # incons set
-        self.PARENT = {self.xI: self.xI}                        # relations
+        self.PARENT = {self.s_start: self.s_start}                        # relations
         self.path = []                                          # planning path
         self.visited = []                                       # order of visited nodes
 
     def searching(self):
-        self.OPEN.put(self.xI, self.fvalue(self.xI))
+        self.OPEN.put(self.s_start, self.fvalue(self.s_start))
         self.ImprovePath()
         self.path.append(self.extract_path())
 
@@ -42,7 +43,7 @@ class AraStar:
             self.e -= 0.5                                       # increase weight
             OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS        # combine two sets
             self.OPEN = queue.QueuePrior()
-            self.OPEN.put(self.xI, self.fvalue(self.xI))
+            self.OPEN.put(self.s_start, self.fvalue(self.s_start))
 
             for x in OPEN_mid:
                 self.OPEN.put(x, self.fvalue(x))                # update priority
@@ -61,94 +62,105 @@ class AraStar:
 
         visited_each = []
 
-        while (self.fvalue(self.xG) >
+        while (self.fvalue(self.s_goal) >
                min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
             s = self.OPEN.get()
 
             if s not in self.CLOSED:
                 self.CLOSED.add(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)
+            for s_n in self.get_neighbor(s):
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g or new_cost < self.g[s_n]:
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
+                    visited_each.append(s_n)
 
-                        if s_next not in self.CLOSED:
-                            self.OPEN.put(s_next, self.fvalue(s_next))
-                        else:
-                            self.INCONS.append(s_next)
+                    if s_n not in self.CLOSED:
+                        self.OPEN.put(s_n, self.fvalue(s_n))
+                    else:
+                        self.INCONS.append(s_n)
 
         self.visited.append(visited_each)
 
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_list = set()
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.add(s_next)
+
+        return s_list
+
     def update_e(self):
         c_OPEN, c_INCONS = float("inf"), float("inf")
 
         if self.OPEN:
-            c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
-
+            c_OPEN = min(self.g[x] +
+                         self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
         if self.INCONS:
-            c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)
-
+            c_INCONS = min(self.g[x] +
+                           self.Heuristic(x) for x in self.INCONS)
         if min(c_OPEN, c_INCONS) == float("inf"):
             return 1
 
-        return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+        return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS))
 
     def fvalue(self, x):
         return self.g[x] + self.e * self.Heuristic(x)
 
     def extract_path(self):
         """
-        Extract the path based on the relationship of nodes.
-
+        Extract the path based on the PARENT set.
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
-            x_current = self.PARENT[x_current]
-            path_back.append(x_current)
+            s = self.PARENT[s]
+            path.append(s)
 
-            if x_current == self.xI:
+            if s == self.s_start:
                 break
 
-        return list(path_back)
+        return list(path)
+
+    def Heuristic(self, s):
+        """
+        Calculate heuristic.
+        :param s: current node (state)
+        :return: heuristic function value
+        """
+
+        heuristic_type = self.heuristic_type                # heuristic type
+        goal = self.s_goal                                  # goal node
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        else:
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
     @staticmethod
-    def get_cost(x, u):
+    def cost(s_start, s_goal):
         """
         Calculate cost for this motion
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :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)
-        :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

Search-based Planning/Search_2D/D_star.py

@@ -0,0 +1,186 @@
+"""
+D_star 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import plotting
+from Search_2D import env
+
+
+class Dstar:
+    def __init__(self, x_start, x_goal):
+        self.xI, self.xG = x_start, x_goal
+
+        self.Env = env.Env()
+        self.Plot = plotting.Plotting(self.xI, self.xG)
+
+        self.u_set = self.Env.motions
+        self.obs = self.Env.obs
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
+
+        self.fig = plt.figure()
+        self.OPEN = set()
+        self.t = {}
+        self.PARENT = {}
+        self.h = {self.xG: 0}
+        self.k = {}
+        self.path = []
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.t[(i, j)] = 'NEW'
+                self.k[(i, j)] = 0
+                self.PARENT[(i, j)] = None
+
+    def run(self, s_start, s_end):
+        self.insert(s_end, 0)
+        while True:
+            self.process_state()
+            if self.t[s_start] == 'CLOSED':
+                break
+        self.path = self.extract_path(s_start, s_end)
+        self.Plot.plot_grid("Dynamic A* (D*)")
+        self.plot_path(self.path)
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Add obstacle at: x =", x, ",", "y =", y)
+            self.obs.add((x, y))
+            plt.plot(x, y, 'sk')
+            if (x, y) in self.path:
+                s = self.xI
+                while s != self.xG:
+                    if self.PARENT[s] in self.obs:
+                        self.modify(s)
+                        continue
+                    s = self.PARENT[s]
+                self.path = self.extract_path(self.xI, self.xG)
+                self.plot_path(self.path)
+            self.fig.canvas.draw_idle()
+
+    def extract_path(self, s_start, s_end):
+        path = []
+        s = s_start
+        while True:
+            s = self.PARENT[s]
+            if s == s_end:
+                return path
+            path.append(s)
+
+    def process_state(self):
+        s = self.min_state()
+        if s is None:
+            return -1
+        k_old = self.get_k_min()
+        self.delete(s)
+
+        if k_old < self.h[s]:
+            for s_n in self.get_neighbor(s):
+                if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s):
+                    self.PARENT[s] = s_n
+                    self.h[s] = self.h[s_n] + self.cost(s_n, s)
+        if k_old == self.h[s]:
+            for s_n in self.get_neighbor(s):
+                if self.t[s_n] == 'NEW' or \
+                        (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \
+                        (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)):
+                    self.PARENT[s_n] = s
+                    self.insert(s_n, self.h[s] + self.cost(s, s_n))
+        else:
+            for s_n in self.get_neighbor(s):
+                if self.t[s_n] == 'NEW' or \
+                        (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)):
+                    self.PARENT[s_n] = s
+                    self.insert(s_n, self.h[s] + self.cost(s, s_n))
+                else:
+                    if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n):
+                        self.insert(s, self.h[s])
+                    else:
+                        if self.PARENT[s_n] != s and \
+                                self.h[s] > self.h[s_n] + self.cost(s_n, s) and \
+                                self.t[s_n] == 'CLOSED' and \
+                                self.h[s_n] > k_old:
+                            self.insert(s_n, self.h[s_n])
+        return self.get_k_min()
+
+    def min_state(self):
+        if not self.OPEN:
+            return None
+        return min(self.OPEN, key=lambda x: self.k[x])
+
+    def get_k_min(self):
+        if not self.OPEN:
+            return -1
+        return min([self.k[x] for x in self.OPEN])
+
+    def insert(self, s, h_new):
+        if self.t[s] == 'NEW':
+            self.k[s] = h_new
+        elif self.t[s] == 'OPEN':
+            self.k[s] = min(self.k[s], h_new)
+        elif self.t[s] == 'CLOSED':
+            self.k[s] = min(self.h[s], h_new)
+        self.h[s] = h_new
+        self.t[s] = 'OPEN'
+        self.OPEN.add(s)
+
+    def delete(self, s):
+        if self.t[s] == 'OPEN':
+            self.t[s] = 'CLOSED'
+        self.OPEN.remove(s)
+
+    def modify(self, s):
+        self.modify_cost(s)
+        while True:
+            k_min = self.process_state()
+            if k_min >= self.h[s]:
+                break
+
+    def modify_cost(self, s):
+        if self.t[s] == 'CLOSED':
+            self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[s]))
+
+    def get_neighbor(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                nei_list.add(s_next)
+
+        return nei_list
+
+    def cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
+
+    @staticmethod
+    def plot_path(path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, marker='o')
+
+
+def main():
+    s_start = (5, 5)
+    s_goal = (45, 25)
+    dstar = Dstar(s_start, s_goal)
+    dstar.run(s_start, s_goal)
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/D_star_Lite.py

@@ -0,0 +1,188 @@
+"""
+D_star_Lite 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import matplotlib.pyplot as plt
+
+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 DStar:
+    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.Plot = plotting.Plotting(x_start, x_goal)
+
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
+
+        self.U = queue.QueuePrior()  # priority queue / U set
+        self.g, self.rhs = {}, {}
+        self.km = 0
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.rhs[(i, j)] = float("inf")
+                self.g[(i, j)] = float("inf")
+
+        self.rhs[self.xG] = 0
+        self.U.put(self.xG, self.Key(self.xG))
+        self.fig = plt.figure()
+
+    def run(self):
+        self.Plot.plot_grid("Dynamic A* (D*)")
+        self.ComputePath()
+        self.plot_path(self.extract_path())
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Change position: x =", x, ",", "y =", y)
+
+            s_curr = self.xI
+            s_last = self.xI
+            i = 0
+            path = []
+
+            while s_curr != self.xG:
+                s_list = {}
+                for s in self.get_neighbor(s_curr):
+                    s_list[s] = self.g[s] + self.get_cost(s_curr, s)
+                s_curr = min(s_list, key=s_list.get)
+                path.append(s_curr)
+
+                if i < 1:
+                    self.km += self.h(s_last, s_curr)
+                    s_last = s_curr
+                    if (x, y) not in self.obs:
+                        self.obs.add((x, y))
+                        plt.plot(x, y, 'sk')
+                        self.g[(x, y)] = float("inf")
+                        self.rhs[(x, y)] = float("inf")
+                    else:
+                        self.obs.remove((x, y))
+                        plt.plot(x, y, marker='s', color='white')
+                        self.UpdateVertex((x, y))
+                    for s in self.get_neighbor((x, y)):
+                        self.UpdateVertex(s)
+                    i += 1
+                self.ComputePath()
+                self.plot_path(path)
+                self.fig.canvas.draw_idle()
+
+    @staticmethod
+    def plot_path(path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, marker='o')
+
+    def ComputePath(self):
+        while self.U.top_key() < self.Key(self.xI) or \
+                self.rhs[self.xI] != self.g[self.xI]:
+            k_old = self.U.top_key()
+            s = self.U.get()
+            if k_old < self.Key(s):
+                self.U.put(s, self.Key(s))
+            elif self.g[s] > self.rhs[s]:
+                self.g[s] = self.rhs[s]
+                for x in self.get_neighbor(s):
+                    self.UpdateVertex(x)
+            else:
+                self.g[s] = float("inf")
+                self.UpdateVertex(s)
+                for x in self.get_neighbor(s):
+                    self.UpdateVertex(x)
+
+    def UpdateVertex(self, s):
+        if s != self.xG:
+            self.rhs[s] = float("inf")
+            for x in self.get_neighbor(s):
+                self.rhs[s] = min(self.rhs[s], self.g[x] + self.get_cost(s, x))
+        self.U.remove(s)
+        if self.g[s] != self.rhs[s]:
+            self.U.put(s, self.Key(s))
+
+    def Key(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
+                min(self.g[s], self.rhs[s])]
+
+    def h(self, s_start, s_goal):
+        heuristic_type = self.heuristic_type            # heuristic type
+
+        if heuristic_type == "manhattan":
+            return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
+        else:
+            return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def get_cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
+
+    def get_neighbor(self, s):
+        nei_list = set()
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                nei_list.add(s_next)
+
+        return nei_list
+
+    def extract_path(self):
+        path = []
+        s = self.xI
+        count = 0
+        while True:
+            count += 1
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            if s == self.xG or count > 100:
+                return list(reversed(path))
+            path.append(s)
+
+    def print_g(self):
+        print("he")
+        for k in range(self.Env.y_range):
+            j = self.Env.y_range - k - 1
+            string = ""
+            for i in range(self.Env.x_range):
+                if self.g[(i, j)] == float("inf"):
+                    string += ("00" + ', ')
+                else:
+                    if self.g[(i, j)] // 10 == 0:
+                        string += ("0" + str(self.g[(i, j)]) + ', ')
+                    else:
+                        string += (str(self.g[(i, j)]) + ', ')
+            print(string)
+
+
+def main():
+    x_start = (5, 5)
+    x_goal = (45, 25)
+
+    dstar = DStar(x_start, x_goal, "euclidean")
+    dstar.run()
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/LPAstar.py

@@ -5,6 +5,7 @@ LPA_star 2D
 
 import os
 import sys
+import math
 import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
@@ -17,15 +18,18 @@ from Search_2D import env
 
 class LpaStar:
     def __init__(self, x_start, x_goal, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+        self.s_start, self.s_goal = x_start, x_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        self.Env = env.Env()
+        self.Plot = plotting.Plotting(x_start, x_goal)
 
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        self.u_set = self.Env.motions
+        self.obs = self.Env.obs
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
 
-        self.U = queue.QueuePrior()  # priority queue / OPEN set
+        self.U = queue.QueuePrior()
         self.g, self.rhs = {}, {}
 
         for i in range(self.Env.x_range):
@@ -33,152 +37,138 @@ class LpaStar:
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
 
-        self.rhs[self.xI] = 0
-        self.U.put(self.xI, self.CalculateKey(self.xI))
+        self.rhs[self.s_start] = 0
+        self.U.put(self.s_start, self.Key(self.s_start))
+        self.fig = plt.figure()
 
-    def searching(self):
-        self.computePath()
-        path = [self.extract_path()]
+    def run(self):
+        self.Plot.plot_grid("Lifelong Planning A*")
 
-        obs_change = set()
-        for j in range(14, 15):
-            self.obs.add((30, j))
-            obs_change.add((30, j))
-        for s in obs_change:
-            self.rhs[s] = float("inf")
-            self.g[s] = float("inf")
-            for x in self.get_neighbor(s):
-                self.UpdateVertex(x)
-        # for x in obs_change:
-        #     self.obs.remove(x)
-        # for x in obs_change:
-        #     self.UpdateVertex(x)
-        print(self.g[(29, 15)])
-        print(self.g[(29, 14)])
-        print(self.g[(29, 13)])
-        print(self.g[(30, 13)])
-        print(self.g[(31, 13)])
-        print(self.g[(32, 13)])
-        print(self.g[(33, 13)])
-        print(self.g[(34, 13)])
-
-        self.computePath()
-        path.append(self.extract_path_test())
-
-        return path, obs_change
-
-    def computePath(self):
-        while self.U.top_key() < self.CalculateKey(self.xG) \
-                or self.rhs[self.xG] != self.g[self.xG]:
+        self.ComputePath()
+        self.plot_path(self.extract_path())
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Change position: x =", x, ",", "y =", y)
+            if (x, y) not in self.obs:
+                self.obs.add((x, y))
+                plt.plot(x, y, 'sk')
+            else:
+                self.obs.remove((x, y))
+                plt.plot(x, y, marker='s', color='white')
+                self.UpdateVertex((x, y))
+
+            for s_n in self.get_neighbor((x, y)):
+                self.UpdateVertex(s_n)
+
+            self.ComputePath()
+            self.plot_path(self.extract_path())
+            self.fig.canvas.draw_idle()
+
+    def ComputePath(self):
+        while self.U.top_key() < self.Key(self.s_goal) or \
+                self.rhs[self.s_goal] != self.g[self.s_goal]:
             s = self.U.get()
-            if self.g[s] > self.rhs[s]:
+
+            if self.g[s] > self.rhs[s]:                 # over-consistent: deleted obstacles
                 self.g[s] = self.rhs[s]
-            else:
+            else:                                       # under-consistent: added obstacles
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-            for x in self.get_neighbor(s):
-                self.UpdateVertex(x)
-        # return self.extract_path()
+            for s_n in self.get_neighbor(s):
+                self.UpdateVertex(s_n)
 
-    def extract_path(self):
-        path = []
-        s = self.xG
+    def UpdateVertex(self, s):
+        if s != self.s_start:
+            self.rhs[s] = min([self.g[s_n] + self.cost(s_n, s)
+                               for s_n in self.get_neighbor(s)])
+        self.U.remove(s)
+        if self.g[s] != self.rhs[s]:
+            self.U.put(s, self.Key(s))
 
-        while True:
-            g_list = {}
-            for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
-            s = min(g_list, key=g_list.get)
-            if s == self.xI:
-                return list(reversed(path))
-            path.append(s)
-
-    def extract_path_test(self):
-        path = []
-        s = self.xG
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
 
-        for k in range(30):
-            g_list = {}
-            for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
-            s = min(g_list, key=g_list.get)
-            path.append(s)
-        return list(reversed(path))
+        s_list = set()
 
-    def get_neighbor(self, s):
-        nei_list = set()
         for u in self.u_set:
             s_next = tuple([s[i] + u[i] for i in range(2)])
             if s_next not in self.obs:
-                nei_list.add(s_next)
+                s_list.add(s_next)
 
-        return nei_list
+        return s_list
 
-    def CalculateKey(self, s):
+    def Key(self, s):
         return [min(self.g[s], self.rhs[s]) + self.h(s),
                 min(self.g[s], self.rhs[s])]
 
-    def UpdateVertex(self, u):
-        if u != self.xI:
-            u_min = float("inf")
-            for x in self.get_neighbor(u):
-                u_min = min(u_min, self.g[x] + self.get_cost(u, x))
-            self.rhs[u] = u_min
-        self.U.check_remove(u)
-        if self.g[u] != self.rhs[u]:
-            self.U.put(u, self.CalculateKey(u))
-
     def h(self, s):
         heuristic_type = self.heuristic_type  # heuristic type
-        goal = self.xG  # goal node
+        goal = self.s_goal  # goal node
 
         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!")
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
-    def get_cost(self, s_start, s_end):
-        """
-        Calculate cost for this motion
+    def cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
 
-        :param s_start:
-        :param s_end:
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
+    def extract_path(self):
+        path = []
+        s = self.s_goal
 
-        if s_start not in self.obs:
-            if s_end not in self.obs:
-                return 1
-            else:
-                return float("inf")
-        return float("inf")
+        for k in range(100):
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            if s == self.s_start:
+                break
+            path.append(s)
+        return list(reversed(path))
+
+    @staticmethod
+    def plot_path(path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, marker='o')
+
+    def print_g(self):
+        print("he")
+        for k in range(self.Env.y_range):
+            j = self.Env.y_range - k - 1
+            string = ""
+            for i in range(self.Env.x_range):
+                if self.g[(i, j)] == float("inf"):
+                    string += ("00" + ', ')
+                else:
+                    if self.g[(i, j)] // 10 == 0:
+                        string += ("0" + str(self.g[(i, j)]) + ', ')
+                    else:
+                        string += (str(self.g[(i, j)]) + ', ')
+            print(string)
 
 
 def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)
-
-    path, obs = lpastar.searching()
-
-    plot.plot_grid("Lifelong Planning A*")
-    p = path[0]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.5)
-
-    p = path[1]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.01)
-    plt.show()
+    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    lpastar.run()
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LRTAstar.py

@@ -6,7 +6,6 @@ LRTA_star 2D (Learning Real-time A*)
 import os
 import sys
 import copy
-import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -39,7 +38,7 @@ class LrtAstarN:
         s_start = self.xI                               # initialize start node
 
         while True:
-            OPEN, CLOSED = self.Astar(s_start, self.N)  # OPEN, CLOSED sets in each iteration
+            OPEN, CLOSED = self.Astar(s_start, self.N)  # U, CLOSED sets in each iteration
 
             if OPEN == "FOUND":                         # reach the goal node
                 self.path.append(CLOSED)
@@ -50,7 +49,7 @@ class LrtAstarN:
             for x in h_value:
                 self.h_table[x] = h_value[x]
 
-            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in OPEN set
+            s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value)      # s_start -> expected node in U set
             self.path.append(path_k)
 
     def extract_path_in_CLOSE(self, s_start, h_value):
@@ -70,7 +69,7 @@ class LrtAstarN:
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
 
-            if s_key not in h_value:                            # reach the expected node in OPEN set
+            if s_key not in h_value:                            # reach the expected node in U set
                 return s_key, path
 
     def iteration(self, CLOSED):
@@ -96,7 +95,7 @@ class LrtAstarN:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN set
+        OPEN = queue.QueuePrior()                               # U set
         OPEN.put(x_start, self.h(x_start))
         CLOSED = set()                                          # CLOSED set
         g_table = {x_start: 0, self.xG: float("inf")}           # cost to come
@@ -180,7 +179,7 @@ def main():
     x_start = (10, 5)
     x_goal = (45, 25)
 
-    lrta = LrtAstarN(x_start, x_goal, 150, "euclidean")
+    lrta = LrtAstarN(x_start, x_goal, 200, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
     fig_name = "Learning Real-time A* (LRTA*)"
 

Search-based Planning/Search_2D/RTAAstar.py

@@ -6,7 +6,6 @@ RTAAstar 2D (Real-time Adaptive A*)
 import os
 import sys
 import copy
-import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -80,7 +79,7 @@ class RtaAstar:
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
 
-            if s_key == s_end:                            # reach the expected node in OPEN set
+            if s_key == s_end:                            # reach the expected node in U set
                 return s_start, list(reversed(path))
 
     def iteration(self, CLOSED):
@@ -106,7 +105,7 @@ class RtaAstar:
                 return h_value
 
     def Astar(self, x_start, N):
-        OPEN = queue.QueuePrior()                               # OPEN set
+        OPEN = queue.QueuePrior()                               # U set
         OPEN.put(x_start, self.h_table[x_start])
         CLOSED = set()                                          # CLOSED set
         g_table = {x_start: 0, self.xG: float("inf")}           # cost to come
@@ -190,7 +189,7 @@ def main():
     x_start = (10, 5)
     x_goal = (45, 25)
 
-    rtaa = RtaAstar(x_start, x_goal, 150, "euclidean")
+    rtaa = RtaAstar(x_start, x_goal, 200, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
     fig_name = "Real-time Adaptive A* (RTAA*)"
 

Search-based Planning/Search_2D/astar.py

@@ -5,6 +5,7 @@ A_star 2D
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -15,165 +16,179 @@ from Search_2D import env
 
 
 class Astar:
-    def __init__(self, x_start, x_goal, e, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, start, goal, heuristic_type):
+        self.s_start, self.s_goal = start, goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()  # class Env
+        self.Env = env.Env()                                        # class Env
 
-        self.e = e  # weighted A*: e >= 1
-        self.u_set = self.Env.motions  # feasible input set
-        self.obs = self.Env.obs  # position of obstacles
+        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")}  # cost to come
-        self.OPEN = queue.QueuePrior()  # priority queue / OPEN set
-        self.OPEN.put(self.xI, self.fvalue(self.xI))
-        self.CLOSED = set()  # closed set & visited
-        self.VISITED = []
-        self.PARENT = {self.xI: self.xI}  # relations
+        self.g = {self.s_start: 0, self.s_goal: float("inf")}       # cost to come
+        self.OPEN = queue.QueuePrior()                              # priority queue / OPEN set
+        self.OPEN.put(self.s_start, self.fvalue(self.s_start))
+        self.CLOSED = []                                            # CLOSED set / VISITED order
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        Searching using A_star.
-
-        :return: path, order of visited nodes in the planning
+        A_star Searching.
+        :return: path, order of visited nodes
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            self.CLOSED.add(s)
-            self.VISITED.append(s)
+            self.CLOSED.append(s)
 
-            if s == self.xG:  # stop condition
+            if s == self.s_goal:                                    # stop condition
                 break
 
-            for u in self.u_set:  # explore neighborhoods of current node
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.obs and s_next not in self.CLOSED:
-                    new_cost = self.g[s] + self.get_cost(s, u)
-                    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.OPEN.put(s_next, self.fvalue(s_next))
+            for s_n in self.get_neighbor(s):
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g:
+                    self.g[s_n] = float("inf")
+                if new_cost < self.g[s_n]:  # conditions for updating cost
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
+                    self.OPEN.put(s_n, self.fvalue(s_n))
 
-        return self.extract_path(self.PARENT), self.VISITED
+        return self.extract_path(self.PARENT), self.CLOSED
+
+    def repeated_astar(self, e):
+        """
+        repeated a*.
+        :param e: weight of a*
+        :return: path and visited order
+        """
 
-    def repeated_Searching(self, xI, xG, e):
         path, visited = [], []
 
         while e >= 1:
-            p_k, v_k = self.repeated_Astar(xI, xG, e)
+            p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e)
             path.append(p_k)
             visited.append(v_k)
             e -= 0.5
 
         return path, visited
 
-    def repeated_Astar(self, xI, xG, e):
-        g = {xI: 0, xG: float("inf")}
+    def repeated_searching(self, s_start, s_goal, e):
+        """
+        run a* with weight e.
+        :param s_start: starting state
+        :param s_goal: goal state
+        :param e: weight of a*
+        :return: path and visited order.
+        """
+
+        g = {s_start: 0, s_goal: float("inf")}
         OPEN = queue.QueuePrior()
-        OPEN.put(xI, g[xI] + e * self.Heuristic(xI))
-        CLOSED = set()
-        PARENT = {xI: xI}
-        VISITED = []
+        OPEN.put(s_start, g[s_start] + e * self.Heuristic(s_start))
+        CLOSED = []
+        PARENT = {s_start: s_start}
 
         while OPEN:
             s = OPEN.get()
-            CLOSED.add(s)
-            VISITED.append(s)
+            CLOSED.append(s)
 
-            if s == xG:
+            if s == s_goal:
                 break
 
-            for u in self.u_set:  # explore neighborhoods of current node
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.obs and s_next not in CLOSED:
-                    new_cost = g[s] + self.get_cost(s, u)
-                    if s_next not in g:
-                        g[s_next] = float("inf")
-                    if new_cost < g[s_next]:  # conditions for updating cost
-                        g[s_next] = new_cost
-                        PARENT[s_next] = s
-                        OPEN.put(s_next, g[s_next] + e * self.Heuristic(s_next))
+            for s_n in self.get_neighbor(s):
+                if s_n not in CLOSED:
+                    new_cost = g[s] + self.cost(s, s_n)
+                    if s_n not in g:
+                        g[s_n] = float("inf")
+                    if new_cost < g[s_n]:                       # conditions for updating cost
+                        g[s_n] = new_cost
+                        PARENT[s_n] = s
+                        OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n))
 
-        return self.extract_path(PARENT), VISITED
+        return self.extract_path(PARENT), CLOSED
 
-    def fvalue(self, x, e=1):
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_list = set()
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.add(s_next)
+
+        return s_list
+
+    def fvalue(self, x):
         """
         f = g + h. (g: cost to come, h: heuristic function)
         :param x: current state
         :return: f
         """
 
-        return self.g[x] + e * self.Heuristic(x)
+        return self.g[x] + self.Heuristic(x)
 
     def extract_path(self, PARENT):
         """
-        Extract the path based on the relationship of nodes.
-
+        Extract the path based on the PARENT set.
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
-            x_current = PARENT[x_current]
-            path_back.append(x_current)
+            s = PARENT[s]
+            path.append(s)
 
-            if x_current == self.xI:
+            if s == self.s_start:
                 break
 
-        return list(path_back)
-
-    @staticmethod
-    def get_cost(x, u):
-        """
-        Calculate cost for this motion
-
-        :param x: current node
-        :param u: current input
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
+        return list(path)
 
-        return 1
-
-    def Heuristic(self, state):
+    def Heuristic(self, s):
         """
         Calculate heuristic.
-
-        :param state: current node (state)
+        :param s: current node (state)
         :return: heuristic function value
         """
 
-        heuristic_type = self.heuristic_type  # heuristic type
-        goal = self.xG  # goal node
+        heuristic_type = self.heuristic_type                    # heuristic type
+        goal = self.s_goal                                      # goal node
 
         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)
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
         else:
-            print("Please choose right heuristic type!")
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
+
+    @staticmethod
+    def cost(s_start, s_goal):
+        """
+        Calculate cost for this motion
+        :param s_start: starting node
+        :param s_goal: end node
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        return 1
 
 
 def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    astar = Astar(x_start, x_goal, 1, "euclidean")  # weight e = 1
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+    astar = Astar(s_start, s_goal, "euclidean")
+    plot = plotting.Plotting(s_start, s_goal)
 
-    fig_name = "A*"
     path, visited = astar.searching()
-    plot.animation(path, visited, fig_name)  # animation generate
+    plot.animation(path, visited, "A*")                         # animation
 
-    # fig_name = "Repeated A*"
-    # path, visited = astar.repeated_Searching(x_start, x_goal, 2.5)
-    # plot.animation_ara_star(path, visited, fig_name)
+    # path, visited = astar.repeated_astar(2.5)               # initial weight e = 2.5
+    # plot.animation_ara_star(path, visited, "Repeated A*")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/bfs.py

@@ -15,67 +15,83 @@ from Search_2D import env
 
 
 class BFS:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         self.u_set = self.Env.motions                       # feasible input set
         self.obs = self.Env.obs                             # position of obstacles
 
-        self.OPEN = queue.QueueFIFO()                       # OPEN set: visited nodes
-        self.OPEN.put(self.xI)
+        self.OPEN = queue.QueueFIFO()                       # U set: visited nodes
+        self.OPEN.put(self.s_start)
         self.CLOSED = []                                    # CLOSED set: explored nodes
-        self.PARENT = {self.xI: self.xI}                    # relations
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        :return: path, order of visited nodes in the planning
+        Breadth-first Searching.
+        :return: path, visited order
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:
+
+            if s == self.s_goal:
                 break
             self.CLOSED.append(s)
 
-            for u_next in self.u_set:                                       # explore neighborhoods
-                s_next = tuple([s[i] + u_next[i] for i in range(2)])
-                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
-                    self.OPEN.put(s_next)
-                    self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                if s_n not in self.PARENT:    # node not explored
+                    self.OPEN.put(s_n)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.CLOSED
 
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_list = set()
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.add(s_next)
+
+        return s_list
+
     def extract_path(self):
         """
-        Extract the path based on the relationship of nodes.
+        Extract the path based on the PARENT set.
         :return: The planning path
         """
 
-        path = [self.xG]
-        s = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
             s = self.PARENT[s]
             path.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         return list(path)
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (45, 25)  # Goal node
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    bfs = BFS(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Breadth-first Searching (BFS)"
+    bfs = BFS(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = bfs.searching()
-    plot.animation(path, visited, fig_name)  # animation
+    plot.animation(path, visited, "Breadth-first Searching (BFS)")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -5,6 +5,7 @@ Bidirectional_a_star 2D
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -15,50 +16,51 @@ from Search_2D import env
 
 
 class BidirectionalAstar:
-    def __init__(self, x_start, x_goal, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal, heuristic_type):
+        self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
-        self.Env = env.Env()                                    # class Env
+        self.Env = env.Env()                                                # class Env
 
-        self.u_set = self.Env.motions                           # feasible input set
-        self.obs = self.Env.obs                                 # position of obstacles
+        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")}       # cost to come: from x_start
-        self.g_back = {self.xG: 0, self.xI: float("inf")}       # cost to come: form x_goal
+        self.g_fore = {self.s_start: 0, self.s_goal: float("inf")}          # cost to come: from s_start
+        self.g_back = {self.s_goal: 0, self.s_start: float("inf")}          # cost to come: form s_goal
 
-        self.OPEN_fore = queue.QueuePrior()                     # OPEN set for foreward searching
-        self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG))
-        self.OPEN_back = queue.QueuePrior()                     # OPEN set for backward searching
-        self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
+        self.OPEN_fore = queue.QueuePrior()                                 # U set for foreward searching
+        self.OPEN_fore.put(self.s_start,
+                           self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
+        self.OPEN_back = queue.QueuePrior()                                 # U set for backward searching
+        self.OPEN_back.put(self.s_goal,
+                           self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
 
-        self.CLOSED_fore = []                                   # CLOSED set for foreward
-        self.CLOSED_back = []                                   # CLOSED set for backward
+        self.CLOSED_fore = []                                               # CLOSED set for foreward
+        self.CLOSED_back = []                                               # CLOSED set for backward
 
-        self.PARENT_fore = {self.xI: self.xI}
-        self.PARENT_back = {self.xG: self.xG}
+        self.PARENT_fore = {self.s_start: self.s_start}
+        self.PARENT_back = {self.s_goal: self.s_goal}
 
     def searching(self):
-        s_meet = self.xI
+        s_meet = self.s_start
 
-        while not self.OPEN_fore.empty() and not self.OPEN_back.empty():
+        while self.OPEN_fore and self.OPEN_back:
             # solve foreward-search
             s_fore = self.OPEN_fore.get()
+
             if s_fore in self.PARENT_back:
                 s_meet = s_fore
                 break
             self.CLOSED_fore.append(s_fore)
 
-            for u in self.u_set:
-                s_next = tuple([s_fore[i] + u[i] for i in range(2)])
-                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))
+            for s_n in self.get_neighbor(s_fore):
+                new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n)
+                if s_n not in self.g_fore:
+                    self.g_fore[s_n] = float("inf")
+                if new_cost < self.g_fore[s_n]:
+                    self.g_fore[s_n] = new_cost
+                    self.PARENT_fore[s_n] = s_fore
+                    self.OPEN_fore.put(s_n, new_cost + self.h(s_n, self.s_goal))
 
             # solve backward-search
             s_back = self.OPEN_back.get()
@@ -67,20 +69,40 @@ class BidirectionalAstar:
                 break
             self.CLOSED_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))
+            for s_n in self.get_neighbor(s_back):
+                new_cost = self.g_back[s_back] + self.cost(s_back, s_n)
+                if s_n not in self.g_back:
+                    self.g_back[s_n] = float("inf")
+                if new_cost < self.g_back[s_n]:
+                    self.g_back[s_n] = new_cost
+                    self.PARENT_back[s_n] = s_back
+                    self.OPEN_back.put(s_n, new_cost + self.h(s_n, self.s_start))
 
         return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back
 
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_list = set()
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.add(s_next)
+
+        return s_list
+
     def extract_path(self, s_meet):
+        """
+        extract path from start and goal
+        :param s_meet: meet point of bi-direction a*
+        :return: path
+        """
+
         # extract path for foreward part
         path_fore = [s_meet]
         s = s_meet
@@ -88,7 +110,7 @@ class BidirectionalAstar:
         while True:
             s = self.PARENT_fore[s]
             path_fore.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         # extract path for backward part
@@ -98,7 +120,7 @@ class BidirectionalAstar:
         while True:
             s = self.PARENT_back[s]
             path_back.append(s)
-            if s == self.xG:
+            if s == self.s_goal:
                 break
 
         return list(reversed(path_fore)) + list(path_back)
@@ -115,17 +137,15 @@ class BidirectionalAstar:
 
         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!")
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
     @staticmethod
-    def get_cost(x, u):
+    def cost(s_start, s_goal):
         """
         Calculate cost for this motion
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -139,10 +159,9 @@ def main():
 
     bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Bidirectional-A*"
     
     path, visited_fore, visited_back = bastar.searching()
-    plot.animation_bi_astar(path, visited_fore, visited_back, fig_name)  # animation
+    plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*")  # animation
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/dfs.py

@@ -1,5 +1,5 @@
 """
-DFS 2D
+Depth-first Searching_2D (DFS)
 @author: huiming zhou
 """
 
@@ -15,69 +15,83 @@ from Search_2D import env
 
 
 class DFS:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         self.u_set = self.Env.motions                           # feasible input set
         self.obs = self.Env.obs                                 # position of obstacles
 
-        self.OPEN = queue.QueueLIFO()                           # OPEN set: visited nodes
-        self.OPEN.put(self.xI)
-        self.CLOSED = []                                        # CLOSED set: explored nodes
-        self.PARENT = {self.xI: self.xI}                        # relations
+        self.OPEN = queue.QueueLIFO()                           # OPEN set
+        self.OPEN.put(self.s_start)
+        self.CLOSED = []                                        # CLOSED set / visited order
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        Searching using DFS.
-
-        :return: planning path, action in each node, visited nodes in the planning process
+        Depth-first Searching
+        :return: planning path, visited order
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:
+
+            if s == self.s_goal:
                 break
             self.CLOSED.append(s)
 
-            for u in self.u_set:                                            # explore neighborhoods
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
-                    self.OPEN.put(s_next)
-                    self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                if s_n not in self.PARENT:                      # node not explored
+                    self.OPEN.put(s_n)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.CLOSED
 
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_list = []
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.append(s_next)
+
+        return s_list
+
     def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
         :return: The planning path
         """
 
-        path = [self.xG]
-        s = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
             s = self.PARENT[s]
             path.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         return list(path)
 
 
 def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    dfs = DFS(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Depth-first Searching (DFS)"
+    dfs = DFS(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = dfs.searching()
-    plot.animation(path, visited, fig_name)  # animation
+    plot.animation(path, visited, "Depth-first Searching (DFS)")  # animation
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/dijkstra.py

@@ -15,72 +15,85 @@ from Search_2D import env
 
 
 class Dijkstra:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         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")}                # cost to come
-        self.OPEN = queue.QueuePrior()                              # priority queue / OPEN set
-        self.OPEN.put(self.xI, 0)
+        self.g = {self.s_start: 0, self.s_goal: float("inf")}       # cost to come
+        self.OPEN = queue.QueuePrior()                              # priority queue / U set
+        self.OPEN.put(self.s_start, 0)
         self.CLOSED = []                                            # closed set & visited
-        self.PARENT = {self.xI: self.xI}                            # relations
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        Searching using Dijkstra.
+        Dijkstra Searching.
         :return: path, order of visited nodes in the planning
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:                                        # stop condition
+
+            if s == self.s_goal:                                    # stop condition
                 break
             self.CLOSED.append(s)
 
-            for u in self.u_set:                                    # explore neighborhoods
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.obs:                          # node not visited and not in obstacles
-                    new_cost = self.g[s] + self.get_cost(s, u)
-                    if s_next not in self.g:
-                        self.g[s_next] = float("inf")
-                    if new_cost < self.g[s_next]:
-                        self.g[s_next] = new_cost
-                        self.OPEN.put(s_next, new_cost)
-                        self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g:
+                    self.g[s_n] = float("inf")
+                if new_cost < self.g[s_n]:
+                    self.g[s_n] = new_cost
+                    self.OPEN.put(s_n, new_cost)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.CLOSED
 
-    def extract_path(self):
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
         """
-        Extract the path based on the relationship of nodes.
 
+        s_list = set()
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            if s_next not in self.obs:
+                s_list.add(s_next)
+
+        return s_list
+
+    def extract_path(self):
+        """
+        Extract the path based on PARENT set.
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
-            x_current = self.PARENT[x_current]
-            path_back.append(x_current)
+            s = self.PARENT[s]
+            path.append(s)
 
-            if x_current == self.xI:
+            if s == self.s_start:
                 break
 
-        return list(path_back)
+        return list(path)
 
     @staticmethod
-    def get_cost(x, u):
+    def cost(s_start, s_goal):
         """
         Calculate cost for this motion
-
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -89,15 +102,14 @@ class Dijkstra:
 
 
 def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    dijkstra = Dijkstra(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+    dijkstra = Dijkstra(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
-    fig_name = "Dijkstra's"
     path, visited = dijkstra.searching()
-    plot.animation(path, visited, fig_name)  # animation generate
+    plot.animation(path, visited, "Dijkstra's")                         # animation generate
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/env.py

@@ -14,7 +14,6 @@ class Env:
     def obs_map(self):
         """
         Initialize obstacles' positions
-
         :return: map of obstacles
         """
 

Search-based Planning/Search_2D/plotting.py

@@ -88,7 +88,7 @@ class Plotting:
             elif count < len(visited) * 2 / 3:
                 length = 25
             else:
-                length = 35
+                length = 30
 
             if count % length == 0:
                 plt.pause(0.001)

Search-based Planning/Search_2D/queue.py

@@ -53,14 +53,15 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
+        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+    def update(self, item, 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
@@ -68,10 +69,10 @@ class QueuePrior:
     def enumerate(self):
         return self.queue
 
-    def check_remove(self, item):
-        for (p, x) in self.queue:
-            if item == x:
-                self.queue.remove((p, x))
+    def remove(self, item):
+        for x in self.queue:
+            if item == x[1]:
+                self.queue.remove(x)
 
     def top_key(self):
         return self.queue[0][0]

Search-based Planning/Search_3D/Astar3D.py

@@ -75,7 +75,7 @@ class Weighted_A_star(object):
                             # update priority of xj
                             self.OPEN.put(strxj, a + 1 * self.h[strxj])
                         else:
-                            # add xj in to OPEN set
+                            # add xj in to U set
                             self.OPEN.put(strxj, a + 1 * self.h[strxj])
             # For specified expanded nodes, used primarily in LRTA*
             if N:

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -48,7 +48,7 @@ class LRT_A_star2:
         st = self.Astar.start
         ind = 0
         # find the lowest path down hill
-        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 U, stop
             # strChildren = self.children(st)
             strChildren = [hash3D(i) for i in self.Astar.children(st)]
             minh, minchild = np.inf, None