update

Search-based Planning/.idea/workspace.xml

@@ -20,16 +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/Potential_Field.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/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/Search_2D/Field_D_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/Field_D_star.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/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/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.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/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/env.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -82,7 +76,7 @@
     </option>
   </component>
   <component name="RunManager" selected="Python.Field_D_star">
-    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -94,7 +88,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/ARAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -103,7 +97,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <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" />
@@ -115,7 +109,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/D_star_Lite.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -124,7 +118,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -136,7 +130,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/Field_D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -145,7 +139,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Field_D_star" 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" />
@@ -157,7 +151,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/Field_D_star.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" />
@@ -166,7 +160,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -178,7 +172,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/Potential_Field.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -210,19 +204,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.ARAstar" />
       <item itemvalue="Python.D_star" />
       <item itemvalue="Python.LPAstar" />
       <item itemvalue="Python.D_star_Lite" />
       <item itemvalue="Python.Field_D_star" />
+      <item itemvalue="Python.Potential_Field" />
     </list>
     <recent_temporary>
       <list>
         <item itemvalue="Python.Field_D_star" />
+        <item itemvalue="Python.Potential_Field" />
         <item itemvalue="Python.D_star_Lite" />
         <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.D_star" />
-        <item itemvalue="Python.ARAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/Field_D_star.py

@@ -39,7 +39,7 @@ class FieldDStar:
             for j in range(self.Env.y_range):
                 self.rhs[(i, j)] = float("inf")
                 self.g[(i, j)] = float("inf")
-                self.bptr[(i, j)] = (0, 0)
+                self.bptr[(i, j)] = (i, j)
 
         self.rhs[self.s_goal] = 0.0
         self.OPEN[self.s_goal] = self.CalculateKey(self.s_goal)
@@ -74,6 +74,7 @@ class FieldDStar:
             print("Change position: x =", x, ",", "y =", y)
             self.visited = set()
             self.count += 1
+
             if (x, y) not in self.obs:
                 self.obs.add((x, y))
                 plt.plot(x, y, 'sk')
@@ -155,6 +156,9 @@ class FieldDStar:
         c = self.cost(s, s2)
         b = self.cost(s, s1)
 
+        if c != float("inf"):
+            c = c / math.sqrt(2)
+
         if min(c, b) == float("inf"):
             vs = float("inf")
         elif self.g[s1] <= self.g[s2]:
@@ -164,7 +168,6 @@ class FieldDStar:
             if f <= b:
                 if c <= f:
                     vs = math.sqrt(2) * c + self.g[s2]
-                    print("test loop!")
                 else:
                     y = min(f / (math.sqrt(c ** 2 - f ** 2)), 1)
                     vs = c * math.sqrt(1 + y ** 2) + f * (1 - y) + self.g[s2]

Search-based Planning/Search_2D/Potential_Field.py

@@ -0,0 +1,172 @@
+"""
+Potential_Field
+@author: huiming zhou
+"""
+
+import os
+import sys
+import numpy as np
+import matplotlib.pyplot as plt
+from collections import deque
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import plotting
+from Search_2D import env
+
+
+class PF:
+    def __init__(self, s_start, s_goal):
+        self.s_start = s_start
+        self.s_goal = s_goal
+        self.kp = 5.0
+        self.eta = 400
+        self.r = 30.0
+        self.OL = 10
+        self.rr = 2
+
+        self.Env = env.Env()
+        self.Plot = plotting.Plotting(s_start, s_goal)
+
+        self.u_set = self.Env.motions  # feasible input set
+        self.obs = self.Env.obs  # position of obstacles
+        self.x = self.Env.x_range
+        self.y = self.Env.y_range
+
+    def run(self):
+        pmap, minx, miny = self.calc_potential_field()
+
+        d = np.hypot(self.s_start[0] - self.s_goal[0],
+                     self.s_start[1] - self.s_goal[1])
+
+        ix = self.s_start[0] - minx
+        iy = self.s_start[1] - miny
+        gix = self.s_goal[0] - minx
+        giy = self.s_goal[1] - miny
+
+        self.draw_heatmap(pmap)
+        plt.gcf().canvas.mpl_connect('key_release_event',
+                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        plt.plot(ix, iy, "sk")
+        plt.plot(gix, giy, "sm")
+
+        rx, ry = [self.s_start[0]], [self.s_goal[0]]
+        ids_rec = deque()
+
+        while d >= 1:
+            minp = float("inf")
+            minix, miniy = -1, -1
+
+            for u in self.u_set:
+                inx = int(ix + u[0])
+                iny = int(iy + u[1])
+
+                if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
+                    p = float("inf")
+                    print("test")
+                else:
+                    p = pmap[inx][iny]
+
+                if minp > p:
+                    minp = p
+                    minix = inx
+                    miniy = iny
+
+            ix = minix
+            iy = miniy
+            xp = ix + minx
+            yp = iy + miny
+            d = np.hypot(self.s_goal[0] - xp, self.s_goal[1] - yp)
+            rx.append(xp)
+            ry.append(yp)
+
+            if self.is_oscillation(ids_rec, ix, iy):
+                print("Oscillation detected at ({},{})!".format(ix, iy))
+                break
+
+            plt.plot(ix, iy, ".r")
+            plt.pause(0.01)
+
+        return rx, ry
+
+    def calc_potential_field(self):
+        minx = 0
+        miny = 0
+        maxx = self.x - 1
+        maxy = self.y - 1
+        xw = maxx - minx
+        yw = maxy - miny
+
+        pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+
+        for ix in range(xw):
+            x = ix + minx
+
+            for iy in range(yw):
+                y = iy + miny
+                ug = self.calc_attractive_potential(x, y)
+                uo = self.calc_repulsive_potential(x, y)
+                uf = ug + uo
+                pmap[ix][iy] = uf
+
+        return pmap, minx, miny
+
+    def calc_attractive_potential(self, x, y):
+        return 0.5 * self.kp * \
+               np.hypot(x - self.s_goal[0], y - self.s_goal[1])
+
+    def calc_repulsive_potential(self, x, y):
+        dmin = float("inf")
+
+        for s in self.obs:
+            d = np.hypot(x - s[0], y - s[1])
+            if dmin >= d:
+                dmin = d
+
+        # calc repulsive potential
+        dq = dmin
+
+        if dq <= self.rr:
+            if dq <= 0.8:
+                dq = 0.8
+
+            return 0.5 * self.eta * (1.0 / dq - 1.0 / self.rr) ** 2
+        else:
+            return 0.0
+
+    def is_oscillation(self, ids_rec, ix, iy):
+        ids_rec.append((ix, iy))
+
+        if len(ids_rec) > self.OL:
+            ids_rec.popleft()
+
+        ids_set = set()
+        for s in ids_rec:
+            if s in ids_set:
+                return True
+            else:
+                ids_set.add(s)
+        return False
+
+    @staticmethod
+    def draw_heatmap(data):
+        data = np.array(data).T
+        plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
+
+
+def main():
+    s_start = (5, 5)
+    s_goal = (45, 25)
+
+    pf = PF(s_start, s_goal)
+
+    plt.grid(True)
+    plt.axis("equal")
+    _, _ = pf.run()
+    plt.show()
+
+
+if __name__ == '__main__':
+    print(__file__ + " start!!")
+    main()

Search-based Planning/Search_2D/env.py

@@ -39,6 +39,8 @@ class Env:
 
         for i in range(15, 30):
             obs.add((30, i))
+        # for i in range(15, 25):
+        #     obs.add((30, i))
         for i in range(16):
             obs.add((40, i))