add repeated A*

2026-08-30 00:50:46 +08:00 · 2020-06-29 10:26:52 -07:00
parent 6a575c3226
commit cc8670ffcf
7 changed files with 81 additions and 31 deletions
@@ -19,7 +19,13 @@
    <select />
  </component>
  <component name="ChangeListManager">
-    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <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/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.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/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/gif/ARA_star.gif" beforeDir="false" afterPath="$PROJECT_DIR$/gif/ARA_star.gif" afterDir="false" />
+    </list>
    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
    <option name="SHOW_DIALOG" value="false" />
    <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -64,7 +70,7 @@
      </list>
    </option>
  </component>
-  <component name="RunManager" selected="Python.ARAstar">
+  <component name="RunManager" selected="Python.astar">
    <configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
      <module name="Search-based Planning" />
      <option name="INTERPRETER_OPTIONS" value="" />
@@ -149,7 +155,7 @@
      <option name="INPUT_FILE" value="" />
      <method v="2" />
    </configuration>
-    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true">
      <module name="Search-based Planning" />
      <option name="INTERPRETER_OPTIONS" value="" />
      <option name="PARENT_ENVS" value="true" />
@@ -161,7 +167,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/astar.py" />
      <option name="PARAMETERS" value="" />
      <option name="SHOW_COMMAND_LINE" value="false" />
      <option name="EMULATE_TERMINAL" value="false" />
@@ -193,19 +199,19 @@
    </configuration>
    <list>
      <item itemvalue="Python.dijkstra" />
-      <item itemvalue="Python.dfs" />
      <item itemvalue="Python.LRTA_star" />
      <item itemvalue="Python.LRTAstar" />
      <item itemvalue="Python.RTAAstar" />
      <item itemvalue="Python.ARAstar" />
+      <item itemvalue="Python.astar" />
    </list>
    <recent_temporary>
      <list>
+        <item itemvalue="Python.astar" />
        <item itemvalue="Python.ARAstar" />
        <item itemvalue="Python.RTAAstar" />
        <item itemvalue="Python.LRTAstar" />
        <item itemvalue="Python.LRTA_star" />
-        <item itemvalue="Python.dfs" />
      </list>
    </recent_temporary>
  </component>
@@ -152,9 +152,9 @@ class AraStar:

 def main():
    x_start = (5, 5)  # Starting node
-    x_goal = (45, 5)  # Goal node
+    x_goal = (45, 25)  # Goal node

-    arastar = AraStar(x_start, x_goal, 2.5, "euclidean")
+    arastar = AraStar(x_start, x_goal, 2.5, "manhattan")
    plot = plotting.Plotting(x_start, x_goal)

    fig_name = "Anytime Repairing A* (ARA*)"
@@ -19,17 +19,17 @@ class Astar:
        self.xI, self.xG = x_start, x_goal
        self.heuristic_type = heuristic_type

-        self.Env = env.Env()                                # class Env
+        self.Env = env.Env()  # class Env

-        self.e = e                                          # weighted A*: e >= 1
-        self.u_set = self.Env.motions                       # feasible input set
-        self.obs = self.Env.obs                             # position of obstacles
+        self.e = e  # weighted A*: e >= 1
+        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.OPEN = queue.QueuePrior()                      # priority queue / OPEN set
+        self.g = {self.xI: 0, self.xG: float("inf")}  # cost to come
+        self.OPEN = queue.QueuePrior()  # priority queue / OPEN set
        self.OPEN.put(self.xI, self.fvalue(self.xI))
-        self.CLOSED = []                                    # closed set & visited
-        self.PARENT = {self.xI: self.xI}                    # relations
+        self.CLOSED = []  # closed set & visited
+        self.PARENT = {self.xI: self.xI}  # relations

    def searching(self):
        """
@@ -42,10 +42,10 @@ class Astar:
            s = self.OPEN.get()
            self.CLOSED.append(s)

-            if s == self.xG:                                                # stop condition
+            if s == self.xG:  # stop condition
                break

-            for u in self.u_set:                                       # explore neighborhoods of current node
+            for u in self.u_set:  # explore neighborhoods of current node
                s_next = tuple([s[i] + u[i] for i in range(2)])
                if s_next not in self.obs and s_next not in self.CLOSED:
                    new_cost = self.g[s] + self.get_cost(s, u)
@@ -56,18 +56,58 @@ class Astar:
                        self.PARENT[s_next] = s
                        self.OPEN.put(s_next, self.fvalue(s_next))

-        return self.extract_path(), self.CLOSED
+        return self.extract_path(self.PARENT), self.CLOSED

-    def fvalue(self, x):
+    def repeated_Searching(self, xI, xG, e):
+        path, visited = [], []
+
+        while e >= 1:
+            p_k, v_k = self.repeated_Astar(xI, xG, e)
+            path.append(p_k)
+            visited.append(v_k)
+            e -= 0.5
+
+        return path, visited
+
+    def repeated_Astar(self, xI, xG, e):
+        g = {xI: 0, xG: float("inf")}
+        OPEN = queue.QueuePrior()
+        OPEN.put(xI, g[xI] + e * self.Heuristic(xI))
+        CLOSED = set()
+        PARENT = {xI: xI}
+        VISITED = []
+
+        while OPEN:
+            s = OPEN.get()
+            CLOSED.add(s)
+            VISITED.append(s)
+
+            if s == xG:
+                break
+
+            for u in self.u_set:  # explore neighborhoods of current node
+                s_next = tuple([s[i] + u[i] for i in range(2)])
+                if s_next not in self.obs and s_next not in CLOSED:
+                    new_cost = g[s] + self.get_cost(s, u)
+                    if s_next not in g:
+                        g[s_next] = float("inf")
+                    if new_cost < g[s_next]:  # conditions for updating cost
+                        g[s_next] = new_cost
+                        PARENT[s_next] = s
+                        OPEN.put(s_next, g[s_next] + e * self.Heuristic(s_next))
+
+        return self.extract_path(PARENT), VISITED
+
+    def fvalue(self, x, e=1):
        """
        f = g + h. (g: cost to come, h: heuristic function)
        :param x: current state
        :return: f
        """

-        return self.g[x] + self.e * self.Heuristic(x)
+        return self.g[x] + e * self.Heuristic(x)

-    def extract_path(self):
+    def extract_path(self, PARENT):
        """
        Extract the path based on the relationship of nodes.

@@ -78,7 +118,7 @@ class Astar:
        x_current = self.xG

        while True:
-            x_current = self.PARENT[x_current]
+            x_current = PARENT[x_current]
            path_back.append(x_current)

            if x_current == self.xI:
@@ -107,8 +147,8 @@ class Astar:
        :return: heuristic function value
        """

-        heuristic_type = self.heuristic_type                    # heuristic type
-        goal = self.xG                                          # goal node
+        heuristic_type = self.heuristic_type  # heuristic type
+        goal = self.xG  # goal node

        if heuristic_type == "manhattan":
            return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
@@ -122,12 +162,16 @@ def main():
    x_start = (5, 5)
    x_goal = (45, 25)

-    astar = Astar(x_start, x_goal, 1, "euclidean")              # weight e = 1
-    plot = plotting.Plotting(x_start, x_goal)                   # class Plotting
+    astar = Astar(x_start, x_goal, 1, "manhattan")  # weight e = 1
+    plot = plotting.Plotting(x_start, x_goal)  # class Plotting
+    #
+    # fig_name = "A*"
+    # path, visited = astar.searching()
+    # plot.animation(path, visited, fig_name)  # animation generate

-    fig_name = "A*"
-    path, visited = astar.searching()
-    plot.animation(path, visited, fig_name)                     # animation generate
+    fig_name = "Repeated A*"
+    path, visited = astar.repeated_Searching(x_start, x_goal, 2.5)
+    plot.animation_ara_star(path, visited, fig_name)


 if __name__ == '__main__':
@@ -138,7 +138,7 @@ class Plotting:
        cl_v = ['silver',
                'wheat',
                'lightskyblue',
-                'plum',
+                'royalblue',
                'slategray']
        cl_p = ['gray',
                'orange',