update 2D

Search-based Planning/.idea/workspace.xml

@@ -20,8 +20,13 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/ida_star.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/Search_2D/test.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/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/a_star.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.py" afterDir="false" />
     </list>
+    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -49,11 +54,23 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$/Search_2D/bfs.py" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
     <property name="restartRequiresConfirmation" value="false" />
-    <property name="settings.editor.selected.configurable" value="preferences.editor" />
+    <property name="settings.editor.selected.configurable" value="reference.settingsdialog.IDE.editor.colors.Console Colors" />
   </component>
-  <component name="RunManager" selected="Python.dijkstra">
+  <component name="RunDashboard">
+    <option name="ruleStates">
+      <list>
+        <RuleState>
+          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
+        </RuleState>
+        <RuleState>
+          <option name="name" value="StatusDashboardGroupingRule" />
+        </RuleState>
+      </list>
+    </option>
+  </component>
+  <component name="RunManager" selected="Python.ida_star">
     <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -96,7 +113,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -108,7 +125,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/dfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -117,7 +134,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -129,7 +146,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/dijkstra.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -138,7 +155,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+    <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" />
@@ -150,7 +167,28 @@
       <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/dijkstra.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" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="test" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <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_2D/test.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -160,19 +198,20 @@
       <method v="2" />
     </configuration>
     <list>
+      <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.ara_star" />
-      <item itemvalue="Python.bfs" />
       <item itemvalue="Python.a_star" />
       <item itemvalue="Python.dfs" />
-      <item itemvalue="Python.dijkstra" />
+      <item itemvalue="Python.test" />
+      <item itemvalue="Python.ida_star" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.dijkstra" />
-        <item itemvalue="Python.dfs" />
+        <item itemvalue="Python.ida_star" />
         <item itemvalue="Python.a_star" />
-        <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.test" />
         <item itemvalue="Python.ara_star" />
+        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>
@@ -194,12 +233,13 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State />
+            <State>
+              <option name="COLUMN_ORDER" />
+            </State>
           </value>
         </entry>
       </map>
     </option>
-    <option name="oldMeFiltersMigrated" value="true" />
   </component>
   <component name="WindowStateProjectService">
     <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog" timestamp="1593282711181">

Search-based Planning/Search_2D/a_star.py

@@ -26,11 +26,10 @@ class Astar:
         self.obs = self.Env.obs  # position of obstacles
 
         self.g = {self.xI: 0, self.xG: float("inf")}
-        self.fig_name = "A* Algorithm"
-
         self.OPEN = queue.QueuePrior()  # priority queue / OPEN
         self.OPEN.put(self.xI, self.fvalue(self.xI))
-        self.parent = {self.xI: self.xI}
+        self.CLOSED = []
+        self.Parent = {self.xI: self.xI}
 
     def searching(self):
         """
@@ -39,23 +38,25 @@ class Astar:
         :return: planning path, action in each node, visited nodes in the planning process
         """
 
-        visited = []
-
         while not self.OPEN.empty():
             s = self.OPEN.get()
+            self.CLOSED.append(s)
+
             if s == self.xG:  # stop condition
                 break
-            visited.append(s)
+
             for u_next in self.u_set:  # explore neighborhoods of current node
                 s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
-                if s_next not in self.obs:
+                if s_next not in self.obs and s_next not in self.CLOSED:
                     new_cost = self.g[s] + self.get_cost(s, u_next)
-                    if s_next not in self.g or new_cost < self.g[s_next]:  # conditions for updating cost
+                    if s_next not in self.g:
+                        self.g[s_next] = float("inf")
+                    if new_cost < self.g[s_next]:  # conditions for updating cost
                         self.g[s_next] = new_cost
-                        self.parent[s_next] = s
+                        self.Parent[s_next] = s
                         self.OPEN.put(s_next, self.fvalue(s_next))
 
-        return self.extract_path(), visited
+        return self.extract_path(), self.CLOSED
 
     def fvalue(self, x):
         h = self.e * self.Heuristic(x)
@@ -72,7 +73,7 @@ class Astar:
         x_current = self.xG
 
         while True:
-            x_current = self.parent[x_current]
+            x_current = self.Parent[x_current]
             path_back.append(x_current)
 
             if x_current == self.xI:
@@ -123,7 +124,6 @@ def main():
 
     fig_name = "A* Algorithm"
     path, visited = astar.searching()
-
     plot.animation(path, visited, fig_name)  # animation generate
 
 

Search-based Planning/Search_2D/ida_star.py

@@ -0,0 +1,90 @@
+"""
+IDA_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
+
+
+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
+
+    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
+            if t == float("inf"):
+                return None
+            bound = t
+
+    def searching(self, path, g, bound):
+        s = path[-1]
+        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)  # Starting node
+    x_goal = (15, 25)  # Goal node
+
+    ida_star = IdaStar(x_start, x_goal, "manhattan")
+    plot = plotting.Plotting(x_start, x_goal)
+
+    path = ida_star.ida_star()
+
+    if path:
+        plot.animation(path, [], "IDA_Star")
+    else:
+        print("Path not found!")
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/queue.py

@@ -53,7 +53,14 @@ class QueuePrior:
         return len(self.queue) == 0
 
     def put(self, item, priority):
-        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+        count = 0
+        for (p, x) in self.queue:
+            if x == item:
+                self.queue[count] = (priority, item)
+                break
+            count += 1
+        if count == len(self.queue):
+            heapq.heappush(self.queue, (priority, item))  # reorder x using priority
 
     def get(self):
         return heapq.heappop(self.queue)[1]  # pop out the smallest item

Search-based Planning/Search_2D/test.py

@@ -0,0 +1,20 @@
+"""
+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
+
+q = queue.QueuePrior()
+q.put((1, 2), 3)
+print(q.enumerate())
+q.put((1, 2), 2)
+print(q.enumerate())
+q.put((1, 2), 4)
+print(q.enumerate())