update D* Lite

Search-based Planning/.idea/workspace.xml

@@ -20,21 +20,9 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
-      <change afterPath="$PROJECT_DIR$/Search_2D/LPAstar_backup.py" 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/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/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" />
-      <change beforePath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.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_backup.py" beforeDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -68,6 +56,11 @@
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
+  <component name="RecentsManager">
+    <key name="MoveFile.RECENT_KEYS">
+      <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
+    </key>
+  </component>
   <component name="RunDashboard">
     <option name="ruleStates">
       <list>
@@ -80,8 +73,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LPAstar">
-    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.D_star_Lite">
+    <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" />
@@ -89,11 +82,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -213,15 +206,15 @@
       <item itemvalue="Python.LRTAstar" />
       <item itemvalue="Python.RTAAstar" />
       <item itemvalue="Python.LRT_Astar3D" />
-      <item itemvalue="Python.Astar3D" />
+      <item itemvalue="Python.D_star_Lite" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.D_star_Lite" />
         <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.RTAAstar" />
         <item itemvalue="Python.LRTAstar" />
         <item itemvalue="Python.LRT_Astar3D" />
-        <item itemvalue="Python.Astar3D" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/D_star_Lite.py

@@ -16,15 +16,18 @@ from Search_2D import plotting
 from Search_2D import env
 
 
-class DStarLite:
+class LpaStar:
     def __init__(self, x_start, x_goal, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()  # class Env
+        self.Plot = plotting.Plotting(x_start, x_goal)
 
         self.u_set = self.Env.motions  # feasible input set
         self.obs = self.Env.obs  # position of obstacles
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
 
         self.U = queue.QueuePrior()  # priority queue / U set
         self.g, self.rhs = {}, {}
@@ -36,32 +39,177 @@ class DStarLite:
                 self.g[(i, j)] = float("inf")
 
         self.rhs[self.xG] = 0
-        self.U.put(self.xG, self.CalculateKey(self.xG))
+        self.U.put(self.xG, self.Key(self.xG))
+        self.fig = plt.figure()
 
-    def CalculateKey(self, s):
-        return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
+    def searching(self):
+        self.Plot.plot_grid("Lifelong Planning A*")
+
+        self.ComputePath()
+        self.plot_path(self.extract_path_test())
+
+        # self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Change position: x =", x, ",", "y =", y)
+            if (x, y) not in self.obs:
+                self.obs.add((x, y))
+                plt.plot(x, y, 'sk')
+                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()
+
+    @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):
+        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):
+                    self.UpdateVertex(x)
+            else:
+                self.g[s] = float("inf")
+                self.UpdateVertex(s)
+                for x in self.getPred(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):
+                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,
                 min(self.g[s], self.rhs[s])]
 
-    def h(self, s_start, s):
-        heuristic_type = self.heuristic_type  # heuristic type
+    def h(self, s):
+        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])
         else:
             return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
 
-    def UpdateVertex(self, s):
-        if s != self.xG:
+    @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!
+        """
 
-    def getNeighbor(self, s):
-        v_list = set()
+        return 1
+
+    def get_neighbor(self, s):
+        nei_list = set()
         for u in self.u_set:
             s_next = tuple([s[i] + u[i] for i in range(2)])
             if s_next not in self.obs:
-                v_list.add(s_next)
-
-        return v_list
-
-    def getCost(self, s_start, s_end):
-
+                nei_list.add(s_next)
+
+        return nei_list
+
+    def extract_path(self):
+        path = []
+        s = self.xG
+
+        while True:
+            g_list = {}
+            for x in self.get_neighbor(s):
+                g_list[x] = self.g[x]
+            s = min(g_list, key=g_list.get)
+            if s == self.xI:
+                return list(reversed(path))
+            path.append(s)
+
+    def 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()
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/LPAstar_backup.py

@@ -1,209 +0,0 @@
-"""
-LPA_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 queue
-from Search_2D import plotting
-from Search_2D import env
-
-
-class LpaStar:
-    def __init__(self, x_start, x_goal, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
-        self.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.OPEN = queue.QueuePrior()  # priority queue / U set
-        self.g, self.v = {}, {}
-
-        for i in range(self.Env.x_range):
-            for j in range(self.Env.y_range):
-                self.v[(i, j)] = float("inf")
-                self.g[(i, j)] = float("inf")
-
-        self.v[self.xI] = 0
-        self.OPEN.put(self.xI, self.Key(self.xI))
-        self.CLOSED = set()
-
-    def searching(self):
-        self.ComputePath()
-        path = [self.extract_path()]
-        # self.print_g()
-
-        obs_change = set()
-        for i in range(25, 30):
-            self.obs.add((i, 15))
-            obs_change.add((i, 15))
-
-        self.obs.add((30, 14))
-        obs_change.add((30, 14))
-
-        for s in obs_change:
-            self.v[s] = float("inf")
-            self.g[s] = float("inf")
-            for x in self.get_neighbor(s):
-                self.UpdateMembership(x)
-
-        # for x in obs_change:
-        #     self.obs.remove(x)
-        # for x in obs_change:
-        #     self.UpdateVertex(x)
-
-        self.ComputePath()
-        path.append(self.extract_path_test())
-        self.print_g()
-
-        return path, obs_change
-
-    def ComputePath(self):
-        while self.Key(self.xG) > self.OPEN.top_key() \
-                or self.v[self.xG] < self.g[self.xG]:
-            s = self.OPEN.get()
-            if self.v[s] > self.g[s]:
-                self.v[s] = self.g[s]
-                self.CLOSED.add(s)
-
-
-
-        while self.OPEN.top_key() < self.Key(self.xG) \
-                or self.v[self.xG] != self.g[self.xG]:
-            s = self.OPEN.get()
-            if self.g[s] > self.v[s]:
-                self.g[s] = self.v[s]
-            else:
-                self.g[s] = float("inf")
-                self.UpdateMembership(s)
-            for x in self.get_neighbor(s):
-                self.UpdateMembership(x)
-        # return self.extract_path()
-
-    def UpdateMembership(self, s):
-        if self.v[s] != self.g[s]:
-            if s not in self.CLOSED:
-                self.OPEN.put(s, self.Key(s))
-        else:
-            if s in self.OPEN:
-                self.OPEN.remove(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 extract_path(self):
-        path = []
-        s = self.xG
-
-        while True:
-            g_list = {}
-            for x in self.get_neighbor(s):
-                g_list[x] = self.g[x]
-            s = min(g_list, key=g_list.get)
-            if s == self.xI:
-                return list(reversed(path))
-            path.append(s)
-
-    def extract_path_test(self):
-        path = []
-        s = self.xG
-
-        for k in range(70):
-            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 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.v[s]) + self.h(s),
-                min(self.g[s], self.v[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])
-        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 get_cost(self, 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!
-        """
-
-        # if s_start not in self.obs:
-        #     if s_end not in self.obs:
-        #         return 1
-        #     else:
-        #         return float("inf")
-        # return float("inf")
-        return 1
-
-def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
-
-    lpastar = LpaStar(x_start, x_goal, "manhattan")
-    plot = plotting.Plotting(x_start, x_goal)
-
-    path, obs = lpastar.searching()
-
-    plot.plot_grid("Lifelong Planning A*")
-    p = path[0]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.5)
-
-    p = path[1]
-    px = [x[0] for x in p]
-    py = [x[1] for x in p]
-    plt.plot(px, py, marker='o')
-    plt.pause(0.01)
-    plt.show()
-
-
-if __name__ == '__main__':
-    main()