Merge branch 'master' of https://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

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" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" 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" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
 </project>

Search-based Planning/.idea/workspace.xml

@@ -19,8 +19,10 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
-    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
+    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="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.">
+      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="false" />
+    </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -48,7 +50,7 @@
     <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$" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
@@ -57,20 +59,8 @@
       <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
     </key>
   </component>
-  <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.D_star_Lite">
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.astar">
+    <configuration name="D_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" />
@@ -82,7 +72,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/D_star_Lite.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -91,7 +81,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -103,7 +93,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/LPAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -112,7 +102,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -124,7 +114,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/LPAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -133,7 +123,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -141,11 +131,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/RTAAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -154,7 +144,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" 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" />
@@ -166,7 +156,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/astar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -199,18 +189,18 @@
     <list>
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.LRT_Astar3D" />
       <item itemvalue="Python.D_star_Lite" />
+      <item itemvalue="Python.D_star" />
+      <item itemvalue="Python.astar" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.astar" />
         <item itemvalue="Python.D_star_Lite" />
+        <item itemvalue="Python.D_star" />
         <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.RTAAstar" />
-        <item itemvalue="Python.LRTAstar" />
-        <item itemvalue="Python.LRT_Astar3D" />
       </list>
     </recent_temporary>
   </component>
@@ -225,6 +215,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">
@@ -232,47 +230,50 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State>
-              <option name="COLUMN_ORDER" />
-            </State>
+            <State />
           </value>
         </entry>
       </map>
     </option>
+    <option name="oldMeFiltersMigrated" value="true" />
+  </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/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

@@ -16,7 +16,7 @@ from Search_2D import plotting
 from Search_2D import env
 
 
-class LpaStar:
+class DStar:
     def __init__(self, x_start, x_goal, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
@@ -42,14 +42,11 @@ class LpaStar:
         self.U.put(self.xG, self.Key(self.xG))
         self.fig = plt.figure()
 
-    def searching(self):
-        self.Plot.plot_grid("Lifelong Planning A*")
-
+    def run(self):
+        self.Plot.plot_grid("Dynamic A* (D*)")
         self.ComputePath()
-        self.plot_path(self.extract_path_test())
-
-        # self.fig.canvas.mpl_connect('button_press_event', self.on_press)
-
+        self.plot_path(self.extract_path())
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
         plt.show()
 
     def on_press(self, event):
@@ -59,20 +56,37 @@ class LpaStar:
         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')
-                self.rhs[(x, y)] = float("inf")
-                self.g[(x, y)] = float("inf")
-                for node in self.getSucc((x, y)):
-                    self.UpdateVertex(node)
-            else:
-                self.obs.remove((x, y))
-                plt.plot(x, y, marker='s', color='white')
-                self.UpdateVertex((x, y))
-            self.ComputePath()
-            self.plot_path(self.extract_path_test())
-            self.fig.canvas.draw_idle()
+
+            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):
@@ -81,88 +95,46 @@ class LpaStar:
         plt.plot(px, py, marker='o')
 
     def ComputePath(self):
-        count = 0
         while self.U.top_key() < self.Key(self.xI) or \
                 self.rhs[self.xI] != self.g[self.xI]:
-            count += 1
-            print(count)
             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.getPred(s):
+                for x in self.get_neighbor(s):
                     self.UpdateVertex(x)
             else:
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-                for x in self.getPred(s):
+                for x in self.get_neighbor(s):
                     self.UpdateVertex(x)
 
-    def getSucc(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 and self.g[s_next] >= self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
-    def getPred(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 and self.g[s_next] <= self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
     def UpdateVertex(self, s):
         if s != self.xG:
             self.rhs[s] = float("inf")
-            for x in self.getSucc(s):
+            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 extract_path_test(self):
-        path = []
-        s = self.xG
-
-        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.xI:
-                return list(reversed(path))
-            path.append(s)
-        return list(reversed(path))
-
     def Key(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(s) + self.km,
+        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):
+    def h(self, s_start, s_goal):
         heuristic_type = self.heuristic_type            # heuristic type
-        s_start = self.xI                               # goal node
 
         if heuristic_type == "manhattan":
-            return abs(s[0] - s_start[0]) + abs(s[1] - s_start[1])
+            return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
         else:
-            return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
-
-    @staticmethod
-    def get_cost(s_start, s_end):
-        """
-        Calculate cost for this motion
-
-        :param s_start:
-        :param s_end:
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
+            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):
@@ -176,14 +148,15 @@ class LpaStar:
 
     def extract_path(self):
         path = []
-        s = self.xG
-
+        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.xI:
+            if s == self.xG or count > 100:
                 return list(reversed(path))
             path.append(s)
 
@@ -207,8 +180,8 @@ def main():
     x_start = (5, 5)
     x_goal = (45, 25)
 
-    lpastar = LpaStar(x_start, x_goal, "euclidean")
-    lpastar.searching()
+    dstar = DStar(x_start, x_goal, "euclidean")
+    dstar.run()
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LPAstar.py

@@ -21,15 +21,15 @@ class LpaStar:
         self.xI, self.xG = 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 / U set
+        self.U = queue.QueuePrior()
         self.g, self.rhs = {}, {}
 
         for i in range(self.Env.x_range):
@@ -39,9 +39,9 @@ class LpaStar:
 
         self.rhs[self.xI] = 0
         self.U.put(self.xI, self.Key(self.xI))
-
-    def searching(self):
         self.fig = plt.figure()
+
+    def run(self):
         self.Plot.plot_grid("Lifelong Planning A*")
 
         self.ComputePath()
@@ -62,9 +62,10 @@ class LpaStar:
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
+                plt.pause(0.001)
                 self.rhs[(x, y)] = float("inf")
                 self.g[(x, y)] = float("inf")
-                for node in self.getSucc((x, y)):
+                for node in self.get_neighbor((x, y)):
                     self.UpdateVertex(node)
             else:
                 self.obs.remove((x, y))
@@ -86,54 +87,48 @@ class LpaStar:
             s = self.U.get()
             if self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
-                for x in self.getSucc(s):
-                    self.UpdateVertex(x)
             else:
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-                for x in self.getSucc(s):
-                    self.UpdateVertex(x)
-
-    def getSucc(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 and self.g[s_next] > self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
-
-    def getPred(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 and self.g[s_next] < self.g[s]:
-                nei_list.add(s_next)
-        return nei_list
+            for x in self.get_neighbor(s):
+                self.UpdateVertex(x)
 
     def UpdateVertex(self, s):
         if s != self.xI:
             u_min = float("inf")
-            for x in self.getPred(s):
-                u_min = min(u_min, self.g[x] + self.get_cost(x, s))
+            for x in self.get_neighbor(s):
+                u_min = min(u_min, self.g[x] + self.cost(x, s))
             self.rhs[s] = u_min
         self.U.remove(s)
         if self.g[s] != self.rhs[s]:
             self.U.put(s, self.Key(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 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 Key(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(s),
+                min(self.g[s], self.rhs[s])]
+
+    def h(self, s):
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.xG  # 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])
+
+    def cost(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return float("inf")
+        return 1
 
     def extract_path(self):
         path = []
@@ -162,48 +157,28 @@ class LpaStar:
             path.append(s)
         return list(reversed(path))
 
-    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 Key(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(s),
-                min(self.g[s], self.rhs[s])]
-
-    def h(self, s):
-        heuristic_type = self.heuristic_type  # heuristic type
-        goal = self.xG  # 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(s_start, s_end):
-        """
-        Calculate cost for this motion
-
-        :param s_start:
-        :param s_end:
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
-
-        return 1
+    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")
-    lpastar.searching()
+    lpastar = LpaStar(x_start, x_goal, "manhattan")
+    lpastar.run()
 
 
 if __name__ == '__main__':

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,180 @@ 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 / U 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():
             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):
+                if s_n not in self.CLOSED:
+                    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.CLOSED
 
-        return self.extract_path(self.PARENT), self.VISITED
+    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), CLOSED
 
-        return self.extract_path(PARENT), VISITED
+    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)
 
-    def fvalue(self, x, e=1):
+        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)
+        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: current 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):
+    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])
 
 
 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/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,16 +53,15 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
-        flag = 0
+        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)
-                flag = 1
                 break
             count += 1
-        if flag == 0:
-            heapq.heappush(self.queue, (priority, item))  # reorder x using priority
 
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item