update

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Sampling-based Planning/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
 </project>

Sampling-based Planning/rrt_2D/rrt_star.py

@@ -161,7 +161,7 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 20000)
+    rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000)
     path = rrt_star.planning()
 
     if path:

Search-based Planning/.idea/Search-based Planning.iml

@@ -2,7 +2,7 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="jdk" jdkName="Python 3.7 (Search-based Planning)" jdkType="Python SDK" />
+    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Search-based Planning/.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
 </project>

Search-based Planning/.idea/workspace.xml

@@ -19,9 +19,21 @@
     <select />
   </component>
   <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit.">
+    <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/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" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -50,7 +62,7 @@
     <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$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../Sampling-based Planning" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
   </component>
@@ -59,8 +71,20 @@
       <recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
     </key>
   </component>
-  <component name="RunManager" selected="Python.astar">
-    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+  <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.LPAstar">
+    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -72,7 +96,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/D_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ARAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -81,7 +105,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -93,7 +117,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/D_star_Lite.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LPAstar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -102,7 +126,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -114,7 +138,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LPAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/astar.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -123,7 +147,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" 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" />
@@ -135,7 +159,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/RTAAstar.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" />
@@ -144,7 +168,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="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" />
@@ -156,7 +180,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/dfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -188,19 +212,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.RTAAstar" />
-      <item itemvalue="Python.D_star_Lite" />
-      <item itemvalue="Python.D_star" />
       <item itemvalue="Python.astar" />
+      <item itemvalue="Python.dfs" />
+      <item itemvalue="Python.bidirectional_a_star" />
+      <item itemvalue="Python.ARAstar" />
+      <item itemvalue="Python.LPAstar" />
     </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.astar" />
-        <item itemvalue="Python.D_star_Lite" />
-        <item itemvalue="Python.D_star" />
         <item itemvalue="Python.LPAstar" />
-        <item itemvalue="Python.RTAAstar" />
+        <item itemvalue="Python.ARAstar" />
+        <item itemvalue="Python.bidirectional_a_star" />
+        <item itemvalue="Python.astar" />
+        <item itemvalue="Python.dfs" />
       </list>
     </recent_temporary>
   </component>
@@ -230,12 +254,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="VcsManagerConfiguration">
     <MESSAGE value="Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms&#10;&#10;# Please enter a commit message to explain why this merge is necessary,&#10;# especially if it merges an updated upstream into a topic branch.&#10;#&#10;# Lines starting with '#' will be ignored, and an empty message aborts&#10;# the commit." />

Search-based Planning/Search_2D/ARAstar.py

@@ -5,6 +5,7 @@ ARA_star 2D (Anytime Repairing A*)
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -15,8 +16,8 @@ from Search_2D import env
 
 
 class AraStar:
-    def __init__(self, x_start, x_goal, e, heuristic_type):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal, e, heuristic_type):
+        self.s_start, self.s_goal = s_start, s_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()                                    # class Env
@@ -24,17 +25,17 @@ class AraStar:
         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.xI: 0, self.xG: float("inf")}            # cost to come
+        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.xI: self.xI}                        # relations
+        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.xI, self.fvalue(self.xI))
+        self.OPEN.put(self.s_start, self.fvalue(self.s_start))
         self.ImprovePath()
         self.path.append(self.extract_path())
 
@@ -42,7 +43,7 @@ class AraStar:
             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.xI, self.fvalue(self.xI))
+            self.OPEN.put(self.s_start, self.fvalue(self.s_start))
 
             for x in OPEN_mid:
                 self.OPEN.put(x, self.fvalue(x))                # update priority
@@ -61,94 +62,105 @@ class AraStar:
 
         visited_each = []
 
-        while (self.fvalue(self.xG) >
+        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)
 
-            for u_next in self.u_set:
-                s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
-                if s_next not in self.obs:
-                    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]:
-                        self.g[s_next] = new_cost
-                        self.PARENT[s_next] = s
-                        visited_each.append(s_next)
+            for s_n in self.get_neighbor(s):
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g or new_cost < self.g[s_n]:
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
+                    visited_each.append(s_n)
 
-                        if s_next not in self.CLOSED:
-                            self.OPEN.put(s_next, self.fvalue(s_next))
-                        else:
-                            self.INCONS.append(s_next)
+                    if s_n not in self.CLOSED:
+                        self.OPEN.put(s_n, self.fvalue(s_n))
+                    else:
+                        self.INCONS.append(s_n)
 
         self.visited.append(visited_each)
 
+    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 update_e(self):
         c_OPEN, c_INCONS = float("inf"), float("inf")
 
         if self.OPEN:
-            c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
-
+            c_OPEN = min(self.g[x] +
+                         self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
         if self.INCONS:
-            c_INCONS = min(self.g[x] + self.Heuristic(x) for x in 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
 
-        return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+        return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS))
 
     def fvalue(self, x):
         return self.g[x] + self.e * self.Heuristic(x)
 
     def extract_path(self):
         """
-        Extract the path based on the relationship of nodes.
-
+        Extract the path based on the PARENT set.
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
-            x_current = self.PARENT[x_current]
-            path_back.append(x_current)
+            s = self.PARENT[s]
+            path.append(s)
 
-            if x_current == self.xI:
+            if s == self.s_start:
                 break
 
-        return list(path_back)
+        return list(path)
+
+    def Heuristic(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
+
+        if heuristic_type == "manhattan":
+            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+        else:
+            return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
     @staticmethod
-    def get_cost(x, u):
+    def cost(s_start, s_goal):
         """
         Calculate cost for this motion
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
 
         return 1
 
-    def Heuristic(self, state):
-        """
-        Calculate heuristic.
-        :param state: current node (state)
-        :return: heuristic
-        """
-
-        heuristic_type = self.heuristic_type
-        goal = self.xG
-
-        if heuristic_type == "manhattan":
-            return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
-        elif heuristic_type == "euclidean":
-            return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
-        else:
-            print("Please choose right heuristic type!")
-
 
 def main():
     x_start = (5, 5)  # Starting node

Search-based Planning/Search_2D/LPAstar.py

@@ -18,7 +18,7 @@ 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.s_start, self.s_goal = x_start, x_goal
         self.heuristic_type = heuristic_type
 
         self.Env = env.Env()
@@ -37,18 +37,16 @@ class LpaStar:
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
 
-        self.rhs[self.xI] = 0
-        self.U.put(self.xI, self.Key(self.xI))
+        self.rhs[self.s_start] = 0
+        self.U.put(self.s_start, self.Key(self.s_start))
         self.fig = plt.figure()
 
     def run(self):
         self.Plot.plot_grid("Lifelong Planning A*")
 
         self.ComputePath()
-        self.plot_path(self.extract_path_test())
-
+        self.plot_path(self.extract_path())
         self.fig.canvas.mpl_connect('button_press_event', self.on_press)
-        print("hahha")
 
         plt.show()
 
@@ -62,55 +60,54 @@ class LpaStar:
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
-                plt.pause(0.001)
-                self.rhs[(x, y)] = float("inf")
-                self.g[(x, y)] = float("inf")
-                for node in self.get_neighbor((x, y)):
-                    self.UpdateVertex(node)
             else:
                 self.obs.remove((x, y))
                 plt.plot(x, y, marker='s', color='white')
                 self.UpdateVertex((x, y))
+
+            for s_n in self.get_neighbor((x, y)):
+                self.UpdateVertex(s_n)
+
             self.ComputePath()
-            self.plot_path(self.extract_path_test())
+            self.plot_path(self.extract_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.xG) or \
-                self.rhs[self.xG] != self.g[self.xG]:
+        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()
-            if self.g[s] > self.rhs[s]:
+
+            if self.g[s] > self.rhs[s]:                 # over-consistent: deleted obstacles
                 self.g[s] = self.rhs[s]
-            else:
+            else:                                       # under-consistent: added obstacles
                 self.g[s] = float("inf")
                 self.UpdateVertex(s)
-            for x in self.get_neighbor(s):
-                self.UpdateVertex(x)
+            for s_n in self.get_neighbor(s):
+                self.UpdateVertex(s_n)
 
     def UpdateVertex(self, s):
-        if s != self.xI:
-            u_min = float("inf")
-            for x in self.get_neighbor(s):
-                u_min = min(u_min, self.g[x] + self.cost(x, s))
-            self.rhs[s] = u_min
+        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)
         if self.g[s] != self.rhs[s]:
             self.U.put(s, self.Key(s))
 
     def get_neighbor(self, s):
-        nei_list = set()
+        """
+        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:
-                nei_list.add(s_next)
+                s_list.add(s_next)
 
-        return nei_list
+        return s_list
 
     def Key(self, s):
         return [min(self.g[s], self.rhs[s]) + self.h(s),
@@ -118,7 +115,7 @@ class LpaStar:
 
     def h(self, s):
         heuristic_type = self.heuristic_type  # heuristic type
-        goal = self.xG  # goal node
+        goal = self.s_goal  # goal node
 
         if heuristic_type == "manhattan":
             return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -132,31 +129,24 @@ class LpaStar:
 
     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
+        s = self.s_goal
 
         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))
+            if s == self.s_start:
+                break
             path.append(s)
         return list(reversed(path))
 
+    @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 print_g(self):
         print("he")
         for k in range(self.Env.y_range):

Search-based Planning/Search_2D/astar.py

@@ -37,7 +37,7 @@ class Astar:
         :return: path, order of visited nodes
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
             self.CLOSED.append(s)
 
@@ -45,14 +45,13 @@ class Astar:
                 break
 
             for s_n in self.get_neighbor(s):
-                if s_n not in self.CLOSED:
-                    new_cost = self.g[s] + self.cost(s, s_n)
-                    if s_n not in self.g:
-                        self.g[s_n] = float("inf")
-                    if new_cost < self.g[s_n]:                      # conditions for updating cost
-                        self.g[s_n] = new_cost
-                        self.PARENT[s_n] = s
-                        self.OPEN.put(s_n, self.fvalue(s_n))
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g:
+                    self.g[s_n] = float("inf")
+                if new_cost < self.g[s_n]:  # conditions for updating cost
+                    self.g[s_n] = new_cost
+                    self.PARENT[s_n] = s
+                    self.OPEN.put(s_n, self.fvalue(s_n))
 
         return self.extract_path(self.PARENT), self.CLOSED
 
@@ -150,18 +149,6 @@ class Astar:
 
         return list(path)
 
-    @staticmethod
-    def cost(s_start, s_goal):
-        """
-        Calculate cost for this motion
-        :param s_start: starting node
-        :param s_goal: end node
-        :return:  cost for this motion
-        :note: cost function could be more complicate!
-        """
-
-        return 1
-
     def Heuristic(self, s):
         """
         Calculate heuristic.
@@ -177,6 +164,18 @@ class Astar:
         else:
             return math.hypot(goal[0] - s[0], goal[1] - s[1])
 
+    @staticmethod
+    def cost(s_start, s_goal):
+        """
+        Calculate cost for this motion
+        :param s_start: starting node
+        :param s_goal: end node
+        :return:  cost for this motion
+        :note: cost function could be more complicate!
+        """
+
+        return 1
+
 
 def main():
     s_start = (5, 5)

Search-based Planning/Search_2D/bfs.py

@@ -15,67 +15,83 @@ from Search_2D import env
 
 
 class BFS:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         self.u_set = self.Env.motions                       # feasible input set
         self.obs = self.Env.obs                             # position of obstacles
 
         self.OPEN = queue.QueueFIFO()                       # U set: visited nodes
-        self.OPEN.put(self.xI)
+        self.OPEN.put(self.s_start)
         self.CLOSED = []                                    # CLOSED set: explored nodes
-        self.PARENT = {self.xI: self.xI}                    # relations
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        :return: path, order of visited nodes in the planning
+        Breadth-first Searching.
+        :return: path, visited order
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:
+
+            if s == self.s_goal:
                 break
             self.CLOSED.append(s)
 
-            for u_next in self.u_set:                                       # explore neighborhoods
-                s_next = tuple([s[i] + u_next[i] for i in range(2)])
-                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
-                    self.OPEN.put(s_next)
-                    self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                if s_n not in self.PARENT:    # node not explored
+                    self.OPEN.put(s_n)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.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):
         """
-        Extract the path based on the relationship of nodes.
+        Extract the path based on the PARENT set.
         :return: The planning path
         """
 
-        path = [self.xG]
-        s = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
             s = self.PARENT[s]
             path.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         return list(path)
 
 
 def main():
-    x_start = (5, 5)  # Starting node
-    x_goal = (45, 25)  # Goal node
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    bfs = BFS(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Breadth-first Searching (BFS)"
+    bfs = BFS(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = bfs.searching()
-    plot.animation(path, visited, fig_name)  # animation
+    plot.animation(path, visited, "Breadth-first Searching (BFS)")
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/bidirectional_a_star.py

@@ -5,6 +5,7 @@ Bidirectional_a_star 2D
 
 import os
 import sys
+import math
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Search-based Planning/")
@@ -15,50 +16,51 @@ from Search_2D import env
 
 
 class BidirectionalAstar:
-    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.Env = env.Env()                                                # class Env
 
-        self.u_set = self.Env.motions                           # feasible input set
-        self.obs = self.Env.obs                                 # position of obstacles
+        self.u_set = self.Env.motions                                       # feasible input set
+        self.obs = self.Env.obs                                             # position of obstacles
 
-        self.g_fore = {self.xI: 0, self.xG: float("inf")}       # cost to come: from x_start
-        self.g_back = {self.xG: 0, self.xI: float("inf")}       # cost to come: form x_goal
+        self.g_fore = {self.s_start: 0, self.s_goal: float("inf")}          # cost to come: from s_start
+        self.g_back = {self.s_goal: 0, self.s_start: float("inf")}          # cost to come: form s_goal
 
-        self.OPEN_fore = queue.QueuePrior()                     # U set for foreward searching
-        self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG))
-        self.OPEN_back = queue.QueuePrior()                     # U set for backward searching
-        self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
+        self.OPEN_fore = queue.QueuePrior()                                 # U set for foreward searching
+        self.OPEN_fore.put(self.s_start,
+                           self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
+        self.OPEN_back = queue.QueuePrior()                                 # U set for backward searching
+        self.OPEN_back.put(self.s_goal,
+                           self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
 
-        self.CLOSED_fore = []                                   # CLOSED set for foreward
-        self.CLOSED_back = []                                   # CLOSED set for backward
+        self.CLOSED_fore = []                                               # CLOSED set for foreward
+        self.CLOSED_back = []                                               # CLOSED set for backward
 
-        self.PARENT_fore = {self.xI: self.xI}
-        self.PARENT_back = {self.xG: self.xG}
+        self.PARENT_fore = {self.s_start: self.s_start}
+        self.PARENT_back = {self.s_goal: self.s_goal}
 
     def searching(self):
-        s_meet = self.xI
+        s_meet = self.s_start
 
-        while not self.OPEN_fore.empty() and not self.OPEN_back.empty():
+        while self.OPEN_fore and self.OPEN_back:
             # solve foreward-search
             s_fore = self.OPEN_fore.get()
+
             if s_fore in self.PARENT_back:
                 s_meet = s_fore
                 break
             self.CLOSED_fore.append(s_fore)
 
-            for u in self.u_set:
-                s_next = tuple([s_fore[i] + u[i] for i in range(2)])
-                if s_next not in self.obs:
-                    new_cost = self.g_fore[s_fore] + self.get_cost(s_fore, u)
-                    if s_next not in self.g_fore:
-                        self.g_fore[s_next] = float("inf")
-                    if new_cost < self.g_fore[s_next]:
-                        self.g_fore[s_next] = new_cost
-                        self.PARENT_fore[s_next] = s_fore
-                        self.OPEN_fore.put(s_next, new_cost + self.h(s_next, self.xG))
+            for s_n in self.get_neighbor(s_fore):
+                new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n)
+                if s_n not in self.g_fore:
+                    self.g_fore[s_n] = float("inf")
+                if new_cost < self.g_fore[s_n]:
+                    self.g_fore[s_n] = new_cost
+                    self.PARENT_fore[s_n] = s_fore
+                    self.OPEN_fore.put(s_n, new_cost + self.h(s_n, self.s_goal))
 
             # solve backward-search
             s_back = self.OPEN_back.get()
@@ -67,20 +69,40 @@ class BidirectionalAstar:
                 break
             self.CLOSED_back.append(s_back)
 
-            for u in self.u_set:
-                s_next = tuple([s_back[i] + u[i] for i in range(len(s_back))])
-                if s_next not in self.obs:
-                    new_cost = self.g_back[s_back] + self.get_cost(s_back, u)
-                    if s_next not in self.g_back:
-                        self.g_back[s_next] = float("inf")
-                    if new_cost < self.g_back[s_next]:
-                        self.g_back[s_next] = new_cost
-                        self.PARENT_back[s_next] = s_back
-                        self.OPEN_back.put(s_next, new_cost + self.h(s_next, self.xI))
+            for s_n in self.get_neighbor(s_back):
+                new_cost = self.g_back[s_back] + self.cost(s_back, s_n)
+                if s_n not in self.g_back:
+                    self.g_back[s_n] = float("inf")
+                if new_cost < self.g_back[s_n]:
+                    self.g_back[s_n] = new_cost
+                    self.PARENT_back[s_n] = s_back
+                    self.OPEN_back.put(s_n, new_cost + self.h(s_n, self.s_start))
 
         return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back
 
+    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, s_meet):
+        """
+        extract path from start and goal
+        :param s_meet: meet point of bi-direction a*
+        :return: path
+        """
+
         # extract path for foreward part
         path_fore = [s_meet]
         s = s_meet
@@ -88,7 +110,7 @@ class BidirectionalAstar:
         while True:
             s = self.PARENT_fore[s]
             path_fore.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         # extract path for backward part
@@ -98,7 +120,7 @@ class BidirectionalAstar:
         while True:
             s = self.PARENT_back[s]
             path_back.append(s)
-            if s == self.xG:
+            if s == self.s_goal:
                 break
 
         return list(reversed(path_fore)) + list(path_back)
@@ -115,17 +137,15 @@ class BidirectionalAstar:
 
         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(s_start, s_goal):
         """
         Calculate cost for this motion
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -139,10 +159,9 @@ def main():
 
     bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
     plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Bidirectional-A*"
     
     path, visited_fore, visited_back = bastar.searching()
-    plot.animation_bi_astar(path, visited_fore, visited_back, fig_name)  # animation
+    plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*")  # animation
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/dfs.py

@@ -1,5 +1,5 @@
 """
-DFS 2D
+Depth-first Searching_2D (DFS)
 @author: huiming zhou
 """
 
@@ -15,69 +15,83 @@ from Search_2D import env
 
 
 class DFS:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         self.u_set = self.Env.motions                           # feasible input set
         self.obs = self.Env.obs                                 # position of obstacles
 
-        self.OPEN = queue.QueueLIFO()                           # U set: visited nodes
-        self.OPEN.put(self.xI)
-        self.CLOSED = []                                        # CLOSED set: explored nodes
-        self.PARENT = {self.xI: self.xI}                        # relations
+        self.OPEN = queue.QueueLIFO()                           # OPEN set
+        self.OPEN.put(self.s_start)
+        self.CLOSED = []                                        # CLOSED set / visited order
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        Searching using DFS.
-
-        :return: planning path, action in each node, visited nodes in the planning process
+        Depth-first Searching
+        :return: planning path, visited order
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:
+
+            if s == self.s_goal:
                 break
             self.CLOSED.append(s)
 
-            for u in self.u_set:                                            # explore neighborhoods
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.PARENT and s_next not in self.obs:    # node not visited and not in obstacles
-                    self.OPEN.put(s_next)
-                    self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                if s_n not in self.PARENT:                      # node not explored
+                    self.OPEN.put(s_n)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.CLOSED
 
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
+        """
+
+        s_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:
+                s_list.append(s_next)
+
+        return s_list
+
     def extract_path(self):
         """
         Extract the path based on the relationship of nodes.
         :return: The planning path
         """
 
-        path = [self.xG]
-        s = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
             s = self.PARENT[s]
             path.append(s)
-            if s == self.xI:
+            if s == self.s_start:
                 break
 
         return list(path)
 
 
 def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    dfs = DFS(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)
-    fig_name = "Depth-first Searching (DFS)"
+    dfs = DFS(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
     path, visited = dfs.searching()
-    plot.animation(path, visited, fig_name)  # animation
+    plot.animation(path, visited, "Depth-first Searching (DFS)")  # animation
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/dijkstra.py

@@ -15,72 +15,85 @@ from Search_2D import env
 
 
 class Dijkstra:
-    def __init__(self, x_start, x_goal):
-        self.xI, self.xG = x_start, x_goal
+    def __init__(self, s_start, s_goal):
+        self.s_start, self.s_goal = s_start, s_goal
 
         self.Env = env.Env()
-        self.plotting = plotting.Plotting(self.xI, self.xG)
+        self.plotting = plotting.Plotting(self.s_start, self.s_goal)
 
         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")}                # cost to come
+        self.g = {self.s_start: 0, self.s_goal: float("inf")}       # cost to come
         self.OPEN = queue.QueuePrior()                              # priority queue / U set
-        self.OPEN.put(self.xI, 0)
+        self.OPEN.put(self.s_start, 0)
         self.CLOSED = []                                            # closed set & visited
-        self.PARENT = {self.xI: self.xI}                            # relations
+        self.PARENT = {self.s_start: self.s_start}
 
     def searching(self):
         """
-        Searching using Dijkstra.
+        Dijkstra Searching.
         :return: path, order of visited nodes in the planning
         """
 
-        while not self.OPEN.empty():
+        while self.OPEN:
             s = self.OPEN.get()
-            if s == self.xG:                                        # stop condition
+
+            if s == self.s_goal:                                    # stop condition
                 break
             self.CLOSED.append(s)
 
-            for u in self.u_set:                                    # explore neighborhoods
-                s_next = tuple([s[i] + u[i] for i in range(2)])
-                if s_next not in self.obs:                          # node not visited and not in obstacles
-                    new_cost = self.g[s] + self.get_cost(s, u)
-                    if s_next not in self.g:
-                        self.g[s_next] = float("inf")
-                    if new_cost < self.g[s_next]:
-                        self.g[s_next] = new_cost
-                        self.OPEN.put(s_next, new_cost)
-                        self.PARENT[s_next] = s
+            for s_n in self.get_neighbor(s):
+                new_cost = self.g[s] + self.cost(s, s_n)
+                if s_n not in self.g:
+                    self.g[s_n] = float("inf")
+                if new_cost < self.g[s_n]:
+                    self.g[s_n] = new_cost
+                    self.OPEN.put(s_n, new_cost)
+                    self.PARENT[s_n] = s
 
         return self.extract_path(), self.CLOSED
 
-    def extract_path(self):
+    def get_neighbor(self, s):
+        """
+        find neighbors of state s that not in obstacles.
+        :param s: state
+        :return: neighbors
         """
-        Extract the path based on the relationship of nodes.
 
+        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):
+        """
+        Extract the path based on PARENT set.
         :return: The planning path
         """
 
-        path_back = [self.xG]
-        x_current = self.xG
+        path = [self.s_goal]
+        s = self.s_goal
 
         while True:
-            x_current = self.PARENT[x_current]
-            path_back.append(x_current)
+            s = self.PARENT[s]
+            path.append(s)
 
-            if x_current == self.xI:
+            if s == self.s_start:
                 break
 
-        return list(path_back)
+        return list(path)
 
     @staticmethod
-    def get_cost(x, u):
+    def cost(s_start, s_goal):
         """
         Calculate cost for this motion
-
-        :param x: current node
-        :param u: input
+        :param s_start: starting node
+        :param s_goal: end node
         :return:  cost for this motion
         :note: cost function could be more complicate!
         """
@@ -89,15 +102,14 @@ class Dijkstra:
 
 
 def main():
-    x_start = (5, 5)
-    x_goal = (45, 25)
+    s_start = (5, 5)
+    s_goal = (45, 25)
 
-    dijkstra = Dijkstra(x_start, x_goal)
-    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+    dijkstra = Dijkstra(s_start, s_goal)
+    plot = plotting.Plotting(s_start, s_goal)
 
-    fig_name = "Dijkstra's"
     path, visited = dijkstra.searching()
-    plot.animation(path, visited, fig_name)  # animation generate
+    plot.animation(path, visited, "Dijkstra's")                         # animation generate
 
 
 if __name__ == '__main__':

Search-based Planning/Search_2D/env.py

@@ -14,7 +14,6 @@ class Env:
     def obs_map(self):
         """
         Initialize obstacles' positions
-
         :return: map of obstacles
         """