add LRTA_star

Search-based Planning/.idea/workspace.xml

@@ -20,10 +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/LRTA_star.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/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARA_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/ida_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/IDA_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/IDA_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/IDA_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/LRTA_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTA_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -69,8 +69,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.LRTA_star">
-    <configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.LRTA_star_N">
+    <configuration name="IDA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -82,7 +82,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/LRTA_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/IDA_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -91,7 +91,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -103,7 +103,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/bfs.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTA_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -112,7 +112,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectionalAstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LRTA_star_N" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -120,11 +120,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/bidirectionalAstar3D.py" />
+      <option name="SCRIPT_NAME" value="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D\LRTA_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -133,7 +133,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bidirectional_a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="bidirectionalAstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -141,11 +141,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
       <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_3D/bidirectionalAstar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -154,7 +154,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" />
@@ -166,7 +166,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" />
@@ -199,18 +199,18 @@
     <list>
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.bidirectional_a_star" />
-      <item itemvalue="Python.bfs" />
-      <item itemvalue="Python.dfs" />
       <item itemvalue="Python.bidirectionalAstar3D" />
       <item itemvalue="Python.LRTA_star" />
+      <item itemvalue="Python.IDA_star" />
+      <item itemvalue="Python.LRTA_star_N" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.LRTA_star_N" />
         <item itemvalue="Python.LRTA_star" />
+        <item itemvalue="Python.IDA_star" />
         <item itemvalue="Python.bidirectional_a_star" />
         <item itemvalue="Python.bidirectionalAstar3D" />
-        <item itemvalue="Python.bfs" />
-        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/IDA_star.py

@@ -5,11 +5,11 @@ IDA_Star 2D
 
 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
 
@@ -24,6 +24,8 @@ class IdaStar:
         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]
@@ -31,13 +33,14 @@ class IdaStar:
         while True:
             t = self.searching(path, 0, bound)
             if t == self.xG:
-                return path
+                return path, self.visited
             if t == float("inf"):
-                return None
+                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:
@@ -73,17 +76,22 @@ class IdaStar:
 
 def main():
     x_start = (5, 5)  # Starting node
-    x_goal = (15, 25)  # Goal node
+    x_goal = (15, 20)  # Goal node
 
     ida_star = IdaStar(x_start, x_goal, "manhattan")
     plot = plotting.Plotting(x_start, x_goal)
 
-    path = ida_star.ida_star()
+    path, visited = ida_star.ida_star()
+    print(len(visited))
 
     if path:
-        plot.animation(path, [], "IDA_Star")
+        plot.plot_grid("IDA_star")
+        plot.plot_path(visited, 'gray', True)
+        plot.plot_path(path)
+        plt.show()
     else:
         print("Path not found!")
+    plot.plot_grid("IDA")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/LRTA_star.py

@@ -1,10 +1,11 @@
 """
-LRTA_star 2D
+LRTA_star_N 2D
 @author: huiming zhou
 """
 
 import os
 import sys
+import copy
 import matplotlib.pyplot as plt
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
@@ -15,7 +16,7 @@ from Search_2D import plotting
 from Search_2D import env
 
 
-class LrtAstar:
+class LrtAstarN:
     def __init__(self, x_start, x_goal, heuristic_type):
         self.xI, self.xG = x_start, x_goal
         self.heuristic_type = heuristic_type
@@ -25,50 +26,119 @@ class LrtAstar:
         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")}
-        self.OPEN = queue.QueuePrior()  # priority queue / OPEN
-        self.OPEN.put(self.xI, self.h(self.xI))
-        self.CLOSED = set()
-        self.Parent = {self.xI: self.xI}
+        self.N = 150
+        self.visited = []
 
     def searching(self):
-        h = {self.xI: self.h(self.xI)}
-        s = self.xI
-        parent = {self.xI: self.xI}
-        visited = []
+        s_start = self.xI
+
+        path = []
+        count = 0
+
+        while True:
+            # if count == 2:
+            #     return path
+            # count += 1
+
+            h_table = {}
+            OPEN, CLOSED = self.Astar(s_start, self.N)
+
+            if OPEN == "end":
+                path.append(CLOSED)
+                return path
+
+            for x in CLOSED:
+                h_table[x] = 2000
+
+            while True:
+                h_table_rec = copy.deepcopy(h_table)
+                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(s_next))
+                            else:
+                                h_list.append(self.get_cost(s, s_next) + h_table[s_next])
+                    h_table[s] = min(h_list)
+                if h_table == h_table_rec:
+                    break
+
+            path_k = [s_start]
+            x = s_start
+            while True:
+                h_xlist = {}
+                for u in self.u_set:
+                    x_next = tuple([x[i] + u[i] for i in range(2)])
+                    if x_next not in self.obs:
+                        if x_next in CLOSED:
+                            h_xlist[x_next] = h_table[x_next]
+                        else:
+                            h_xlist[x_next] = self.h(x_next)
+                s_key = min(h_xlist, key=h_xlist.get)
+                path_k.append(s_key)
+                x = s_key
+                if s_key not in CLOSED:
+                    break
+            s_start = path_k[-1]
+
+            path.append(path_k)
+
+    def Astar(self, x_start, N):
+        OPEN = queue.QueuePrior()
+        OPEN.put(x_start, self.h(x_start))
+        CLOSED = set()
+        g_table = {x_start: 0, self.xG: float("inf")}
+        parent = {x_start: x_start}
         count = 0
-        while s != self.xG:
+        visited = []
+
+        while not OPEN.empty():
             count += 1
-            print(count)
+            s = OPEN.get()
+            CLOSED.add(s)
             visited.append(s)
-            h_list = {}
+            if s == self.xG:
+                path = self.extract_path(x_start, parent)
+                self.visited.append(visited)
+                return "end", path
+
             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:
-                    if s_next not in h:
-                        h[s_next] = self.h(s_next)
-                    h_list[s_next] = self.get_cost(s, s_next) + h[s_next]
-            h_new = min(h_list.values())
-            if h_new > h[s]:
-                h[s] = h_new
-            s_child = min(h_list, key=h_list.get)
-            parent[s_child] = s
-            s = s_child
-        # path_get = self.extract_path(parent)
-        return [], visited
-
-    def extract_path(self, parent):
-        path = [self.xG]
-        s = self.xG
+                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(s_next))
+
+            if count == N:
+                break
+        self.visited.append(visited)
+
+        return OPEN, CLOSED
+
+    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
 
         while True:
-            s = parent[s]
-            path.append(s)
+            x_current = parent[x_current]
+            path_back.append(x_current)
 
-            if s == self.xI:
+            if x_current == x_start:
                 break
 
-        return path
+        return list(reversed(path_back))
 
     def h(self, s):
         heuristic_type = self.heuristic_type
@@ -99,17 +169,25 @@ def main():
     x_start = (10, 5)  # Starting node
     x_goal = (45, 25)  # Goal node
 
-    lrtastar = LrtAstar(x_start, x_goal, "manhattan")
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
-
-    path, visited = lrtastar.searching()
-    pathx = [x[0] for x in path]
-    pathy = [x[1] for x in path]
-    vx = [x[0] for x in visited]
-    vy = [x[1] for x in visited]
-    plot.plot_grid("test")
-    plt.plot(pathx, pathy, 'r')
-    plt.plot(vx, vy, 'gray')
+    lrtastarn = LrtAstarN(x_start, x_goal, "euclidean")
+    plot = plotting.Plotting(x_start, x_goal)
+
+    path = lrtastarn.searching()
+    plot.plot_grid("LRTA_star_N")
+
+    for k in range(len(path)):
+        plot.plot_visited(lrtastarn.visited[k])
+        plt.pause(0.5)
+        plot.plot_path(path[k])
+        plt.pause(0.5)
+    plt.pause(0.5)
+
+    path_u = []
+    for i in range(len(path)):
+        for j in range(len(path[i])):
+            path_u.append(path[i][j])
+    plot.plot_path(path_u)
+    plt.pause(0.2)
     plt.show()
 
 

Search-based Planning/Search_2D/plotting.py

@@ -117,7 +117,6 @@ class Plotting:
                 plt.pause(0.001)
         plt.pause(0.01)
 
-
     @staticmethod
     def color_list():
         cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']