update queue

Search-based Planning/.idea/workspace.xml

@@ -20,20 +20,15 @@
   </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/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.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/IDAstar.py" beforeDir="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" />
+      <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/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Astar3D.py" afterDir="false" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -64,6 +59,7 @@
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
     <property name="last_opened_file_path" value="$PROJECT_DIR$/../Sampling-based Planning" />
     <property name="restartRequiresConfirmation" value="false" />
+    <property name="run.code.analysis.last.selected.profile" value="aDefault" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
   <component name="RecentsManager">
@@ -83,7 +79,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LPAstar">
+  <component name="RunManager" selected="Python.bidirectional_a_star">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -126,7 +122,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+    <configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -138,7 +134,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/astar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -147,7 +143,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectional_a_star" 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" />
@@ -159,7 +155,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/bidirectional_a_star.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" />
@@ -168,7 +164,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="bidirectional_a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -180,7 +176,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dfs.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -212,19 +208,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.RTAAstar" />
+      <item itemvalue="Python.LRTAstar" />
+      <item itemvalue="Python.bidirectional_a_star" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.LPAstar" />
-        <item itemvalue="Python.ARAstar" />
         <item itemvalue="Python.bidirectional_a_star" />
-        <item itemvalue="Python.astar" />
-        <item itemvalue="Python.dfs" />
+        <item itemvalue="Python.ARAstar" />
+        <item itemvalue="Python.LPAstar" />
+        <item itemvalue="Python.RTAAstar" />
+        <item itemvalue="Python.LRTAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/ARAstar.py

@@ -10,7 +10,6 @@ import math
 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
 
@@ -20,37 +19,33 @@ class AraStar:
         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.e = e                                              # initial weight
-        self.g = {self.s_start: 0, self.s_goal: float("inf")}            # cost to come
+        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.s_start: 0, self.s_goal: float("inf")}               # cost to come
 
-        self.OPEN = queue.QueuePrior()                          # priority queue / U
-        self.CLOSED = set()                                     # closed set
-        self.INCONS = []                                        # incons set
-        self.PARENT = {self.s_start: self.s_start}                        # relations
-        self.path = []                                          # planning path
-        self.visited = []                                       # order of visited nodes
+        self.OPEN = {self.s_start: self.fvalue(self.s_start)}               # priority queue / OPEN set
+        self.CLOSED = set()                                                 # CLOSED set
+        self.INCONS = {}                                                    # INCONS set
+        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.s_start, self.fvalue(self.s_start))
         self.ImprovePath()
         self.path.append(self.extract_path())
 
-        while self.update_e() > 1:                              # continue condition
-            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.s_start, self.fvalue(self.s_start))
+        while self.update_e() > 1:                                          # continue condition
+            self.e -= 0.5                                                   # increase weight
+            self.OPEN.update(self.INCONS)
+            for s in self.OPEN:
+                self.OPEN[s] = self.fvalue(s)
 
-            for x in OPEN_mid:
-                self.OPEN.put(x, self.fvalue(x))                # update priority
-
-            self.INCONS = []
+            self.INCONS = {}
             self.CLOSED = set()
-            self.ImprovePath()                                  # improve path
+            self.ImprovePath()                                              # improve path
             self.path.append(self.extract_path())
 
         return self.path, self.visited
@@ -62,12 +57,11 @@ class AraStar:
 
         visited_each = []
 
-        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)
+        while True:
+            s, f_small = self.get_smallest_f()
+            if self.fvalue(self.s_goal) <= f_small:
+                break
+            self.CLOSED.add(s)
 
             for s_n in self.get_neighbor(s):
                 new_cost = self.g[s] + self.cost(s, s_n)
@@ -77,12 +71,26 @@ class AraStar:
                     visited_each.append(s_n)
 
                     if s_n not in self.CLOSED:
-                        self.OPEN.put(s_n, self.fvalue(s_n))
+                        self.OPEN[s_n] = self.fvalue(s_n)
                     else:
-                        self.INCONS.append(s_n)
+                        self.INCONS[s_n] = 0
 
         self.visited.append(visited_each)
 
+    def get_smallest_f(self):
+        """
+        :return: node with smallest f_value in OPEN set.
+        """
+        s_list = {}
+
+        for s in self.OPEN:
+            s_list[s] = self.fvalue(s)
+        s_small = min(s_list, key=s_list.get)
+
+        self.OPEN.pop(s_small)
+
+        return s_small, s_list[s_small]
+
     def get_neighbor(self, s):
         """
         find neighbors of state s that not in obstacles.
@@ -100,21 +108,17 @@ class AraStar:
         return s_list
 
     def update_e(self):
-        c_OPEN, c_INCONS = float("inf"), float("inf")
+        v = float("inf")
 
         if self.OPEN:
-            c_OPEN = min(self.g[x] +
-                         self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
+            v = min(self.g[s] + self.h(s) for s in self.OPEN)
         if 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
+            v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS))
 
-        return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS))
+        return min(self.e, self.g[self.s_goal] / v)
 
     def fvalue(self, x):
-        return self.g[x] + self.e * self.Heuristic(x)
+        return self.g[x] + self.e * self.h(x)
 
     def extract_path(self):
         """
@@ -134,15 +138,15 @@ class AraStar:
 
         return list(path)
 
-    def Heuristic(self, s):
+    def h(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
+        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])
@@ -163,16 +167,14 @@ class AraStar:
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (45, 25)  # Goal node
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    arastar = AraStar(x_start, x_goal, 2.5, "manhattan")
-    plot = plotting.Plotting(x_start, x_goal)
+    arastar = AraStar(s_start, s_goal, 2.5, "euclidean")
+    plot = plotting.Plotting(s_start, s_goal)
 
-    fig_name = "Anytime Repairing A* (ARA*)"
     path, visited = arastar.searching()
-
-    plot.animation_ara_star(path, visited, fig_name)
+    plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/D_star_Lite.py

@@ -11,25 +11,24 @@ 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
+    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.Plot = plotting.Plotting(x_start, x_goal)
+        self.Plot = plotting.Plotting(s_start, s_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.U = {}
         self.g, self.rhs = {}, {}
         self.km = 0
 
@@ -38,8 +37,8 @@ class DStar:
                 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.rhs[self.s_goal] = 0
+        self.U[self.s_goal] = self.CalculateKey(self.s_goal)
         self.fig = plt.figure()
 
     def run(self):
@@ -57,15 +56,15 @@ class DStar:
             x, y = int(x), int(y)
             print("Change position: x =", x, ",", "y =", y)
 
-            s_curr = self.xI
-            s_last = self.xI
+            s_curr = self.s_start
+            s_last = self.s_start
             i = 0
             path = []
 
-            while s_curr != self.xG:
+            while s_curr != self.s_goal:
                 s_list = {}
                 for s in self.get_neighbor(s_curr):
-                    s_list[s] = self.g[s] + self.get_cost(s_curr, s)
+                    s_list[s] = self.g[s] + self.cost(s_curr, s)
                 s_curr = min(s_list, key=s_list.get)
                 path.append(s_curr)
 
@@ -88,19 +87,18 @@ class DStar:
                 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))
+        while True:
+            s, v = self.TopKey()
+            if v >= self.CalculateKey(self.s_start) and \
+                    self.rhs[self.s_start] == self.g[self.s_start]:
+                break
+
+            k_old = v
+            self.U.pop(s)
+
+            if k_old < self.CalculateKey(s):
+                self.U[s] = self.CalculateKey(s)
             elif self.g[s] > self.rhs[s]:
                 self.g[s] = self.rhs[s]
                 for x in self.get_neighbor(s):
@@ -112,29 +110,40 @@ class DStar:
                     self.UpdateVertex(x)
 
     def UpdateVertex(self, s):
-        if s != self.xG:
+        if s != self.s_goal:
             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)
+                self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x))
+        if s in self.U:
+            self.U.pop(s)
+
         if self.g[s] != self.rhs[s]:
-            self.U.put(s, self.Key(s))
+            self.U[s] = self.CalculateKey(s)
 
-    def Key(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
+    def CalculateKey(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s) + self.km,
                 min(self.g[s], self.rhs[s])]
 
+    def TopKey(self):
+        """
+        :return: return the min key and its value.
+        """
+
+        s = min(self.U, key=self.U.get)
+        return s, self.U[s]
+
     def h(self, s_start, s_goal):
-        heuristic_type = self.heuristic_type            # heuristic type
+        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):
+    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 get_neighbor(self, s):
@@ -148,7 +157,7 @@ class DStar:
 
     def extract_path(self):
         path = []
-        s = self.xI
+        s = self.s_start
         count = 0
         while True:
             count += 1
@@ -156,31 +165,22 @@ class DStar:
             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:
+            if s == self.s_goal 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)
+    @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():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    dstar = DStar(x_start, x_goal, "euclidean")
+    dstar = DStar(s_start, s_goal, "euclidean")
     dstar.run()
 
 

Search-based Planning/Search_2D/IDAstar.py

@@ -1,96 +0,0 @@
-"""
-IDA_Star 2D (Iteratively Deepening A*)
-@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 IdaStar:
-    def __init__(self, x_start, x_goal, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
-        self.heuristic_type = heuristic_type
-
-        self.Env = env.Env()                    # class Env
-
-        self.u_set = self.Env.motions           # feasible input set
-        self.obs = self.Env.obs                 # position of obstacles
-
-        self.visited = []
-
-    def ida_star(self):
-        bound = self.h(self.xI)
-        path = [self.xI]
-
-        while True:
-            t = self.searching(path, 0, bound)
-            if t == self.xG:
-                return path, self.visited
-            if t == float("inf"):
-                return [], self.visited
-            bound = t
-
-    def searching(self, path, g, bound):
-        s = path[-1]
-        self.visited.append(s)
-        f = g + self.h(s)
-
-        if f > bound:
-            return f
-        if s == self.xG:
-            return s
-
-        res_min = float("inf")
-        for u in self.u_set:
-            s_next = tuple([s[i] + u[i] for i in range(len(s))])
-            if s_next not in self.obs and s_next not in path:
-                path.append(s_next)
-                t = self.searching(path, g + 1, bound)
-                if t == self.xG:
-                    return self.xG
-                if t < res_min:
-                    res_min = t
-                path.pop()
-
-        return res_min
-
-    def h(self, s):
-        heuristic_type = self.heuristic_type
-        goal = self.xG
-
-        if heuristic_type == "manhattan":
-            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
-        elif heuristic_type == "euclidean":
-            return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
-        else:
-            print("Please choose right heuristic type!")
-
-
-def main():
-    x_start = (5, 5)
-    x_goal = (15, 20)
-
-    ida_star = IdaStar(x_start, x_goal, "manhattan")
-    plot = plotting.Plotting(x_start, x_goal)
-
-    path, visited = ida_star.ida_star()
-
-    if path:
-        plot.plot_grid("Iteratively Deepening A*")
-        plot.plot_path(visited, 'gray', True)
-        plot.plot_path(path)
-        plt.show()
-    else:
-        print("Path not found!")
-
-
-if __name__ == '__main__':
-    main()

Search-based Planning/Search_2D/LPAstar.py

@@ -11,25 +11,24 @@ 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 LpaStar:
-    def __init__(self, x_start, x_goal, heuristic_type):
-        self.s_start, self.s_goal = 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()
-        self.Plot = plotting.Plotting(x_start, x_goal)
+        self.Plot = plotting.Plotting(self.s_start, self.s_goal)
 
         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()
+        self.U = {}
         self.g, self.rhs = {}, {}
 
         for i in range(self.Env.x_range):
@@ -38,7 +37,8 @@ class LpaStar:
                 self.g[(i, j)] = float("inf")
 
         self.rhs[self.s_start] = 0
-        self.U.put(self.s_start, self.Key(self.s_start))
+        self.U[self.s_start] = self.CalculateKey(self.s_start)
+
         self.fig = plt.figure()
 
     def run(self):
@@ -73,13 +73,16 @@ class LpaStar:
             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()
+        while True:
+            s, v = self.TopKey()
+            if v >= self.CalculateKey(self.s_goal) and \
+                    self.rhs[self.s_goal] == self.g[self.s_goal]:
+                break
+            self.U.pop(s)
 
-            if self.g[s] > self.rhs[s]:                 # over-consistent: deleted obstacles
+            if self.g[s] > self.rhs[s]:                         # over-consistent: deleted obstacles
                 self.g[s] = self.rhs[s]
-            else:                                       # under-consistent: added obstacles
+            else:                                               # under-consistent: added obstacles
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
             for s_n in self.get_neighbor(s):
@@ -87,11 +90,25 @@ class LpaStar:
 
     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)
+            self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s)
+                              for s_n in self.get_neighbor(s))
+        if s in self.U:
+            self.U.pop(s)
+
         if self.g[s] != self.rhs[s]:
-            self.U.put(s, self.Key(s))
+            self.U[s] = self.CalculateKey(s)
+
+    def TopKey(self):
+        """
+        :return: return the min key and its value.
+        """
+
+        s = min(self.U, key=self.U.get)
+        return s, self.U[s]
+
+    def CalculateKey(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(s),
+                min(self.g[s], self.rhs[s])]
 
     def get_neighbor(self, s):
         """
@@ -109,11 +126,13 @@ class LpaStar:
 
         return s_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):
+        """
+        Calculate heuristic.
+        :param s: current node (state)
+        :return: heuristic function value
+        """
+
         heuristic_type = self.heuristic_type  # heuristic type
         goal = self.s_goal  # goal node
 
@@ -123,11 +142,25 @@ class LpaStar:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
     def cost(self, s_start, s_end):
+        """
+        calculate edge cost: (s_start, s_end)
+        :param s_start: start node
+        :param s_end: end node
+        :return: cost
+        """
+
+        # if one of the vertex in obstacles: return infinity.
         if s_start in self.obs or s_end in self.obs:
             return float("inf")
+
         return 1
 
     def extract_path(self):
+        """
+        Extract the path based on the PARENT set.
+        :return: The planning path
+        """
+
         path = []
         s = self.s_goal
 
@@ -139,6 +172,7 @@ class LpaStar:
             if s == self.s_start:
                 break
             path.append(s)
+
         return list(reversed(path))
 
     @staticmethod
@@ -147,21 +181,6 @@ class LpaStar:
         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)

Search-based Planning/Search_2D/LRTAstar.py

@@ -6,6 +6,7 @@ LRTA_star 2D (Learning Real-time A*)
 import os
 import sys
 import copy
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -16,8 +17,8 @@ from Search_2D import env
 
 
 class LrtAstarN:
-    def __init__(self, x_start, x_goal, N, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal, N, heuristic_type):
+        self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()
@@ -35,7 +36,7 @@ class LrtAstarN:
                 self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
 
     def searching(self):
-        s_start = self.xI                               # initialize start node
+        s_start = self.s_start                               # initialize start node
 
         while True:
             OPEN, CLOSED = self.Astar(s_start, self.N)  # U, CLOSED sets in each iteration
@@ -58,13 +59,11 @@ class LrtAstarN:
 
         while True:
             h_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:
-                    if s_next in h_value:
-                        h_list[s_next] = h_value[s_next]
-                    else:
-                        h_list[s_next] = self.h_table[s_next]
+            for s_n in self.get_neighbor(s):
+                if s_n in h_value:
+                    h_list[s_n] = h_value[s_n]
+                else:
+                    h_list[s_n] = self.h_table[s_n]
             s_key = min(h_list, key=h_list.get)                 # move to the smallest node with min h_value
             path.append(s_key)                                  # generate path
             s = s_key                                           # use end of this iteration as the start of next
@@ -82,13 +81,11 @@ class LrtAstarN:
             h_value_rec = copy.deepcopy(h_value)
             for s in CLOSED:
                 h_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:
-                        if s_next not in CLOSED:
-                            h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
-                        else:
-                            h_list.append(self.get_cost(s, s_next) + h_value[s_next])
+                for s_n in self.get_neighbor(s):
+                    if s_n not in CLOSED:
+                        h_list.append(self.cost(s, s_n) + self.h_table[s_n])
+                    else:
+                        h_list.append(self.cost(s, s_n) + h_value[s_n])
                 h_value[s] = min(h_list)                        # update h_value of current node
 
             if h_value == h_value_rec:                          # h_value table converged
@@ -97,40 +94,53 @@ class LrtAstarN:
     def Astar(self, x_start, N):
         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
+        CLOSED = []                                             # CLOSED set
+        g_table = {x_start: 0, self.s_goal: float("inf")}           # cost to come
         PARENT = {x_start: x_start}                             # relations
-        visited = []                                            # order of visited nodes
         count = 0                                               # counter
 
         while not OPEN.empty():
             count += 1
             s = OPEN.get()
-            CLOSED.add(s)
-            visited.append(s)
+            CLOSED.append(s)
 
-            if s == self.xG:                                                # reach the goal node
-                self.visited.append(visited)
+            if s == self.s_goal:                                                # reach the goal node
+                self.visited.append(CLOSED)
                 return "FOUND", self.extract_path(x_start, PARENT)
 
-            for u in self.u_set:
-                s_next = tuple([s[i] + u[i] for i in range(len(s))])
-                if s_next not in self.obs and s_next not in CLOSED:
-                    new_cost = g_table[s] + self.get_cost(s, u)
-                    if s_next not in g_table:
-                        g_table[s_next] = float("inf")
-                    if new_cost < g_table[s_next]:                           # conditions for updating cost
-                        g_table[s_next] = new_cost
-                        PARENT[s_next] = s
-                        OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
-
-            if count == N:                                                   # expand needed CLOSED nodes
+            for s_n in self.get_neighbor(s):
+                if s_n not in CLOSED:
+                    new_cost = g_table[s] + self.cost(s, s_n)
+                    if s_n not in g_table:
+                        g_table[s_n] = float("inf")
+                    if new_cost < g_table[s_n]:                                 # conditions for updating cost
+                        g_table[s_n] = new_cost
+                        PARENT[s_n] = s
+                        OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
+
+            if count == N:                                                      # expand needed CLOSED nodes
                 break
 
-        self.visited.append(visited)                                         # visited nodes in each iteration
+        self.visited.append(CLOSED)                                             # visited nodes in each iteration
 
         return OPEN, 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, x_start, parent):
         """
         Extract the path based on the relationship of nodes.
@@ -138,8 +148,8 @@ class LrtAstarN:
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path_back = [self.s_goal]
+        x_current = self.s_goal
 
         while True:
             x_current = parent[x_current]
@@ -151,23 +161,25 @@ class LrtAstarN:
         return list(reversed(path_back))
 
     def h(self, s):
-        heuristic_type = self.heuristic_type
-        goal = self.xG
+        """
+        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])
-        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(self, s_start, s_end):
         """
         Calculate cost for this motion
-
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_end: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -176,15 +188,15 @@ class LrtAstarN:
 
 
 def main():
-    x_start = (10, 5)
-    x_goal = (45, 25)
+    s_start = (10, 5)
+    s_goal = (45, 25)
 
-    lrta = LrtAstarN(x_start, x_goal, 200, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Learning Real-time A* (LRTA*)"
+    lrta = LrtAstarN(s_start, s_goal, 250, "euclidean")
+    plot = plotting.Plotting(s_start, s_goal)
 
     lrta.searching()
-    plot.animation_lrta(lrta.path, lrta.visited, fig_name)
+    plot.animation_lrta(lrta.path, lrta.visited,
+                        "Learning Real-time A* (LRTA*)")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/RTAAstar.py

@@ -6,6 +6,7 @@ RTAAstar 2D (Real-time Adaptive A*)
 import os
 import sys
 import copy
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -16,8 +17,8 @@ from Search_2D import env
 
 
 class RtaAstar:
-    def __init__(self, x_start, x_goal, N, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal, N, heuristic_type):
+        self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()
@@ -35,7 +36,7 @@ class RtaAstar:
                 self.h_table[(i, j)] = self.h((i, j))   # initialize h_value
 
     def searching(self):
-        s_start = self.xI                               # initialize start node
+        s_start = self.s_start                               # initialize start node
 
         while True:
             OPEN, CLOSED, g_table, PARENT = \
@@ -59,29 +60,12 @@ class RtaAstar:
         for (_, x) in OPEN.enumerate():
             v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x]
         s_open = min(v_open, key=v_open.get)
-        f_min = min(v_open.values())
+        f_min = v_open[s_open]
         for x in CLOSED:
             h_value[x] = f_min - g_table[x]
 
         return s_open, h_value
 
-    def extract_path_in_CLOSE(self, s_end, s_start, h_value):
-        path = [s_start]
-        s = s_start
-
-        while True:
-            h_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 and s_next in h_value:
-                    h_list[s_next] = h_value[s_next]
-            s_key = max(h_list, key=h_list.get)                 # move to the smallest node with min h_value
-            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 U set
-                return s_start, list(reversed(path))
-
     def iteration(self, CLOSED):
         h_value = {}
 
@@ -92,13 +76,11 @@ class RtaAstar:
             h_value_rec = copy.deepcopy(h_value)
             for s in CLOSED:
                 h_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:
-                        if s_next not in CLOSED:
-                            h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
-                        else:
-                            h_list.append(self.get_cost(s, s_next) + h_value[s_next])
+                for s_n in self.get_neighbor(s):
+                    if s_n not in CLOSED:
+                        h_list.append(self.cost(s, s_n) + self.h_table[s_n])
+                    else:
+                        h_list.append(self.cost(s, s_n) + h_value[s_n])
                 h_value[s] = min(h_list)                        # update h_value of current node
 
             if h_value == h_value_rec:                          # h_value table converged
@@ -107,77 +89,106 @@ class RtaAstar:
     def Astar(self, x_start, N):
         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
+        CLOSED = []                                             # CLOSED set
+        g_table = {x_start: 0, self.s_goal: float("inf")}       # cost to come
         PARENT = {x_start: x_start}                             # relations
-        visited = []                                            # order of visited nodes
         count = 0                                               # counter
 
         while not OPEN.empty():
             count += 1
             s = OPEN.get()
-            CLOSED.add(s)
-            visited.append(s)
+            CLOSED.append(s)
 
-            if s == self.xG:                                                # reach the goal node
-                self.visited.append(visited)
+            if s == self.s_goal:                                                # reach the goal node
+                self.visited.append(CLOSED)
                 return "FOUND", self.extract_path(x_start, PARENT), [], []
 
-            for u in self.u_set:
-                s_next = tuple([s[i] + u[i] for i in range(len(s))])
-                if s_next not in self.obs and s_next not in CLOSED:
-                    new_cost = g_table[s] + self.get_cost(s, u)
-                    if s_next not in g_table:
-                        g_table[s_next] = float("inf")
-                    if new_cost < g_table[s_next]:                           # conditions for updating cost
-                        g_table[s_next] = new_cost
-                        PARENT[s_next] = s
-                        OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
+            for s_n in self.get_neighbor(s):
+                if s_n not in CLOSED:
+                    new_cost = g_table[s] + self.cost(s, s_n)
+                    if s_n not in g_table:
+                        g_table[s_n] = float("inf")
+                    if new_cost < g_table[s_n]:                           # conditions for updating cost
+                        g_table[s_n] = new_cost
+                        PARENT[s_n] = s
+                        OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
 
             if count == N:                                                   # expand needed CLOSED nodes
                 break
 
-        self.visited.append(visited)                                         # visited nodes in each iteration
+        self.visited.append(CLOSED)                                         # visited nodes in each iteration
 
         return OPEN, CLOSED, g_table, PARENT
 
+    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_in_CLOSE(self, s_end, s_start, h_value):
+        path = [s_start]
+        s = s_start
+
+        while True:
+            h_list = {}
+            for s_n in self.get_neighbor(s):
+                if s_n in h_value:
+                    h_list[s_n] = h_value[s_n]
+            s_key = max(h_list, key=h_list.get)                 # move to the smallest node with min h_value
+            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 U set
+                return s_start, list(reversed(path))
+
     def extract_path(self, x_start, parent):
         """
         Extract the path based on the relationship of nodes.
-
         :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)
-
-            if x_current == x_start:
+            s = parent[s]
+            path.append(s)
+            if s == x_start:
                 break
 
-        return list(reversed(path_back))
+        return list(reversed(path))
 
     def h(self, s):
-        heuristic_type = self.heuristic_type
-        goal = self.xG
+        """
+        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])
-        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(self, s_start, s_end):
         """
         Calculate cost for this motion
-
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_end: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -186,15 +197,15 @@ class RtaAstar:
 
 
 def main():
-    x_start = (10, 5)
-    x_goal = (45, 25)
+    s_start = (10, 5)
+    s_goal = (45, 25)
 
-    rtaa = RtaAstar(x_start, x_goal, 200, "euclidean")
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Real-time Adaptive A* (RTAA*)"
+    rtaa = RtaAstar(s_start, s_goal, 220, "euclidean")
+    plot = plotting.Plotting(s_start, s_goal)
 
     rtaa.searching()
-    plot.animation_lrta(rtaa.path, rtaa.visited, fig_name)
+    plot.animation_lrta(rtaa.path, rtaa.visited,
+                        "Real-time Adaptive A* (RTAA*)")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/queue.py

@@ -55,24 +55,8 @@ class QueuePrior:
     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
-
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item
 
     def enumerate(self):
         return self.queue
-
-    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_2D/test.py

@@ -1,20 +0,0 @@
-"""
-A_star 2D
-@author: huiming zhou
-"""
-
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
-                "/../../Search-based Planning/")
-
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
-
-
-U = queue.QueuePrior()
-U.put((1, 2), [2, 3])
-U.put((2, 3), [1, 5])
-print(U.get())

Search-based Planning/Search_3D/Astar3D.py

@@ -116,7 +116,7 @@ class Weighted_A_star(object):
         self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0])  # item, priority = g + h
         self.CLOSED = set()
 
-        # self.h = Heuristic(self.Space, self.goal)
+        # self.h = h(self.Space, self.goal)
 
 
 if __name__ == '__main__':