q

Search-based Planning/.idea/workspace.xml

@@ -21,9 +21,7 @@
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
       <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.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/Search_2D/LPAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/FieldD_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/Field_D_star.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -75,7 +73,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.D_star_Lite">
+  <component name="RunManager" selected="Python.Field_D_star">
     <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -139,6 +137,27 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
+    <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" />
+      <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/Field_D_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="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -160,27 +179,6 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="RTAAstar" 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/RTAAstar.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="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -204,19 +202,19 @@
     </configuration>
     <list>
       <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.RTAAstar" />
       <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" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.Field_D_star" />
         <item itemvalue="Python.D_star_Lite" />
         <item itemvalue="Python.D_star" />
         <item itemvalue="Python.LPAstar" />
         <item itemvalue="Python.ARAstar" />
-        <item itemvalue="Python.RTAAstar" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/Field_D_star.py

@@ -0,0 +1,267 @@
+"""
+Field D* 2D
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Search-based Planning/")
+
+from Search_2D import plotting
+from Search_2D import env
+
+
+class FieldDStar:
+    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.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
+
+        self.g, self.rhs, self.U = {}, {}, {}
+        self.parent = {}
+
+        for i in range(self.Env.x_range):
+            for j in range(self.Env.y_range):
+                self.rhs[(i, j)] = float("inf")
+                self.g[(i, j)] = float("inf")
+                self.parent[(i, j)] = (0, 0)
+
+        self.rhs[self.s_goal] = 0.0
+        self.U[self.s_goal] = self.CalculateKey(self.s_goal)
+        self.visited = set()
+        self.count = 0
+        self.fig = plt.figure()
+
+    def run(self):
+        self.Plot.plot_grid("Field D*")
+        self.ComputeShortestPath()
+        self.plot_path(self.extract_path())
+        self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        plt.show()
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Change position: x =", x, ",", "y =", y)
+            self.visited = set()
+            self.count += 1
+            if (x, y) not in self.obs:
+                self.obs.add((x, y))
+                plt.plot(x, y, 'sk')
+            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.ComputeShortestPath()
+            self.plot_visited(self.visited)
+            self.plot_path(self.extract_path())
+            self.fig.canvas.draw_idle()
+
+    def ComputeShortestPath(self):
+        while True:
+            s, v = self.TopKey()
+            if v >= self.CalculateKey(self.s_start) and \
+                    self.rhs[self.s_start] == self.g[self.s_start]:
+                break
+
+            k_old = v
+            self.U.pop(s)
+            self.visited.add(s)
+
+            if k_old < self.CalculateKey(s):
+                self.U[s] = self.CalculateKey(s)
+            elif self.g[s] > self.rhs[s]:
+                self.g[s] = self.rhs[s]
+                for x in self.get_neighbor(s):
+                    self.UpdateVertex(x)
+            else:
+                self.g[s] = float("inf")
+                self.UpdateVertex(s)
+                for x in self.get_neighbor(s):
+                    self.UpdateVertex(x)
+
+    def UpdateVertex(self, s):
+        if s != self.s_goal:
+            value = []
+            s_plist = []
+            sn_list = self.get_neighbor_pure(s)
+            sn_list.append(sn_list[0])
+            for k in range(8):
+                v, sp = self.ComputeCost(s, sn_list[k], sn_list[k + 1])
+                value.append(v)
+                s_plist.append(sp)
+            self.rhs[s] = min(value)
+            self.parent[s] = s_plist[value.index(min(value))]
+
+        if s in self.U:
+            self.U.pop(s)
+
+        if self.g[s] != self.rhs[s]:
+            self.U[s] = self.CalculateKey(s)
+
+    def get_neighbor_pure(self, s):
+        s_list = []
+
+        for u in self.u_set:
+            s_next = tuple([s[i] + u[i] for i in range(2)])
+            s_list.append(s_next)
+
+        return s_list
+
+    def CalculateKey(self, s):
+        return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s),
+                min(self.g[s], self.rhs[s])]
+
+    def ComputeCost(self, s, sa, sb):
+        if sa[0] != s[0] and sa[1] != s[1]:
+            s1, s2 = sb, sa
+        else:
+            s1, s2 = sa, sb
+
+        c = self.cost(s, s2)
+        b = self.cost(s, s1)
+        y = 0
+
+        if min(c, b) == float("inf"):
+            vs = float("inf")
+        elif self.g[s1] <= self.g[s2]:
+            vs = min(c, b) + self.g[s1]
+        else:
+            f = self.g[s1] - self.g[s2]
+            if f <= b:
+                if c <= f:
+                    vs = math.sqrt(2) * c + self.g[s2]
+                else:
+                    y = min(f / (math.sqrt(c ** 2 - f ** 2)), 1)
+                    vs = c * math.sqrt(1 + y ** 2) + f * (1 - y) + self.g[s2]
+            else:
+                if c <= b:
+                    vs = math.sqrt(2) * c + self.g[s2]
+                else:
+                    x = 1 - min(b / (math.sqrt(c ** 2 - b ** 2)), 1)
+                    vs = c * math.sqrt(1 + (1 - x) ** 2) + b * x + self.g[s2]
+
+        ss = (y * s1[0] + (1 - y) * s2[0], y * s1[1] + (1 - y) * s2[1])
+
+        return vs, ss
+
+    def TopKey(self):
+        """
+        :return: return the min key and its value.
+        """
+
+        s = min(self.U, key=self.U.get)
+        return s, self.U[s]
+
+    def h(self, s_start, s_goal):
+        heuristic_type = self.heuristic_type  # heuristic type
+
+        if heuristic_type == "manhattan":
+            return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
+        else:
+            return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def cost(self, 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!
+        """
+
+        if self.is_collision(s_start, s_goal):
+            return float("inf")
+
+        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+    def is_collision(self, s_start, s_end):
+        if s_start in self.obs or s_end in self.obs:
+            return True
+
+        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+            else:
+                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+            if s1 in self.obs or s2 in self.obs:
+                return True
+
+        return False
+
+    def get_neighbor(self, s):
+        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):
+        path = [self.s_start]
+        s = self.s_start
+        count = 0
+        while True:
+            count += 1
+            g_list = {}
+            for x in self.get_neighbor(s):
+                if not self.is_collision(s, x):
+                    g_list[x] = self.g[x]
+            ss = self.parent[s]
+            s = min(g_list, key=g_list.get)
+            path.append(s)
+
+            if s == self.s_goal or count > 100:
+                return list(reversed(path))
+
+    def plot_path(self, path):
+        px = [x[0] for x in path]
+        py = [x[1] for x in path]
+        plt.plot(px, py, linewidth=2)
+        plt.plot(self.s_start[0], self.s_start[1], "bs")
+        plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+    def plot_visited(self, visited):
+        color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+                 'bisque', 'navajowhite', 'moccasin', 'wheat',
+                 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+        if self.count >= len(color) - 1:
+            self.count = 0
+
+        for x in visited:
+            plt.plot(x[0], x[1], marker='s', color=color[self.count])
+
+
+def main():
+    s_start = (5, 5)
+    s_goal = (45, 25)
+
+    fielddstar = FieldDStar(s_start, s_goal, "euclidean")
+    fielddstar.run()
+
+
+if __name__ == '__main__':
+    main()