update

Search-based Planning/.idea/workspace.xml

@@ -20,31 +20,10 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/Search_2D/FieldD_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml" beforeDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" 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/D_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.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/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$/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" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/ARA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/ARA_star.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Astar.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/BFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/BFS.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/Bi-Astar.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Bi-Astar.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/DFS.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/DFS.gif" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/gif/Dijkstra.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/Dijkstra.gif" 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" />
-      <change beforePath="$PROJECT_DIR$/gif/RepeatedA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/RepeatedA_star.gif" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -96,7 +75,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LPAstar">
+  <component name="RunManager" selected="Python.D_star_Lite">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -139,7 +118,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" />
@@ -151,7 +130,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" />
@@ -160,7 +139,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" />
@@ -172,7 +151,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" />
@@ -225,19 +204,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
       <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.D_star" />
       <item itemvalue="Python.LPAstar" />
+      <item itemvalue="Python.D_star_Lite" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.D_star_Lite" />
         <item itemvalue="Python.D_star" />
+        <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.ARAstar" />
         <item itemvalue="Python.RTAAstar" />
-        <item itemvalue="Python.LRTAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/D_star.py

@@ -34,7 +34,7 @@ class Dstar:
         self.h = {}
         self.k = {}
         self.path = []
-        self.visited = []
+        self.visited = set()
         self.count = 0
 
         for i in range(self.Env.x_range):
@@ -68,16 +68,18 @@ class Dstar:
             self.obs.add((x, y))
             plt.plot(x, y, 'sk')
             s = self.s_start
-            self.visited = []
+            self.visited = set()
+            self.count += 1
+
             while s != self.s_goal:
                 if self.is_collision(s, self.PARENT[s]):
                     self.modify(s)
                     continue
                 s = self.PARENT[s]
+
             self.path = self.extract_path(self.s_start, self.s_goal)
             self.plot_visited(self.visited)
             self.plot_path(self.path)
-            self.count += 1
             self.fig.canvas.draw_idle()
 
     def extract_path(self, s_start, s_end):
@@ -91,7 +93,7 @@ class Dstar:
 
     def process_state(self):
         s = self.min_state()
-        self.visited.append(s)
+        self.visited.add(s)
         if s is None:
             return -1
         k_old = self.get_k_min()
@@ -220,8 +222,7 @@ class Dstar:
             self.count = 0
 
         for x in visited:
-            if x not in self.obs:
-                plt.plot(x[0], x[1], marker='s', color=color[self.count])
+            plt.plot(x[0], x[1], marker='s', color=color[self.count])
 
 
 def main():

Search-based Planning/Search_2D/D_star_Lite.py

@@ -28,8 +28,7 @@ class DStar:
         self.x = self.Env.x_range
         self.y = self.Env.y_range
 
-        self.U = {}
-        self.g, self.rhs = {}, {}
+        self.g, self.rhs, self.U = {}, {}, {}
         self.km = 0
 
         for i in range(self.Env.x_range):
@@ -37,12 +36,14 @@ class DStar:
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
 
-        self.rhs[self.s_goal] = 0
+        self.rhs[self.s_goal] = 0.0
         self.U[self.s_goal] = self.CalculateKey(self.s_goal)
+        self.visited = set()
+        self.count = 0
         self.fig = plt.figure()
 
     def run(self):
-        self.Plot.plot_grid("Dynamic A* (D*)")
+        self.Plot.plot_grid("D* Lite")
         self.ComputePath()
         self.plot_path(self.extract_path())
         self.fig.canvas.mpl_connect('button_press_event', self.on_press)
@@ -63,6 +64,7 @@ class DStar:
 
             while s_curr != self.s_goal:
                 s_list = {}
+
                 for s in self.get_neighbor(s_curr):
                     s_list[s] = self.g[s] + self.cost(s_curr, s)
                 s_curr = min(s_list, key=s_list.get)
@@ -83,9 +85,13 @@ class DStar:
                     for s in self.get_neighbor((x, y)):
                         self.UpdateVertex(s)
                     i += 1
-                self.ComputePath()
-                self.plot_path(path)
-                self.fig.canvas.draw_idle()
+
+                    self.count += 1
+                    self.visited = set()
+                    self.ComputePath()
+            self.plot_visited(self.visited)
+            self.plot_path(path)
+            self.fig.canvas.draw_idle()
 
     def ComputePath(self):
         while True:
@@ -96,6 +102,7 @@ class DStar:
 
             k_old = v
             self.U.pop(s)
+            self.visited.add(s)
 
             if k_old < self.CalculateKey(s):
                 self.U[s] = self.CalculateKey(s)
@@ -140,11 +147,36 @@ class DStar:
         else:
             return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
 
-    def cost(self, s_start, s_end):
-        if s_start in self.obs or s_end in self.obs:
+    def cost(self, 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!
+        """
+
+        if self.is_collision(s_start, s_goal):
             return float("inf")
 
-        return 1
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
 
     def get_neighbor(self, s):
         nei_list = set()
@@ -163,17 +195,30 @@ class DStar:
             count += 1
             g_list = {}
             for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
+                if not self.is_collision(s, x):
+                    g_list[x] = self.g[x]
             s = min(g_list, key=g_list.get)
             if s == self.s_goal or count > 100:
                 return list(reversed(path))
             path.append(s)
 
-    @staticmethod
-    def plot_path(path):
+    def plot_path(self, path):
         px = [x[0] for x in path]
         py = [x[1] for x in path]
-        plt.plot(px, py, marker='o')
+        plt.plot(px, py, linewidth=2)
+        plt.plot(self.s_start[0], self.s_start[1], "bs")
+        plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+    def plot_visited(self, visited):
+        color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+                 'bisque', 'navajowhite', 'moccasin', 'wheat',
+                 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+        if self.count >= len(color) - 1:
+            self.count = 0
+
+        for x in visited:
+            plt.plot(x[0], x[1], marker='s', color=color[self.count])
 
 
 def main():

Search-based Planning/Search_2D/LPAstar.py

@@ -37,7 +37,7 @@ class LpaStar:
 
         self.rhs[self.s_start] = 0
         self.U[self.s_start] = self.CalculateKey(self.s_start)
-        self.visited = []
+        self.visited = set()
         self.count = 0
 
         self.fig = plt.figure()
@@ -58,7 +58,7 @@ class LpaStar:
         else:
             x, y = int(x), int(y)
             print("Change position: x =", x, ",", "y =", y)
-            self.visited = []
+            self.visited = set()
             self.count += 1
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
@@ -79,11 +79,12 @@ class LpaStar:
     def ComputeShortestPath(self):
         while True:
             s, v = self.TopKey()
-            self.visited.append(s)
+
             if v >= self.CalculateKey(self.s_goal) and \
                     self.rhs[self.s_goal] == self.g[self.s_goal]:
                 break
             self.U.pop(s)
+            self.visited.add(s)
 
             if self.g[s] > self.rhs[s]:                         # over-consistent: deleted obstacles
                 self.g[s] = self.rhs[s]
@@ -214,8 +215,7 @@ class LpaStar:
             self.count = 0
 
         for x in visited:
-            if x not in self.obs:
-                plt.plot(x[0], x[1], marker='s', color=color[self.count])
+            plt.plot(x[0], x[1], marker='s', color=color[self.count])
 
 
 def main():