diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 5deda0f..de16140 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -19,7 +19,13 @@
-
+
+
+
+
+
+
+
@@ -64,7 +70,7 @@
-
+
@@ -149,7 +155,7 @@
-
+
@@ -161,7 +167,7 @@
-
+
@@ -193,19 +199,19 @@
+
diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py
index e7ac85c..b9b305b 100644
--- a/Search-based Planning/Search_2D/ARAstar.py
+++ b/Search-based Planning/Search_2D/ARAstar.py
@@ -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*)"
diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index 26d5762..178a3ab 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/astar.py b/Search-based Planning/Search_2D/astar.py
index f75d4df..fb7f12f 100644
--- a/Search-based Planning/Search_2D/astar.py
+++ b/Search-based Planning/Search_2D/astar.py
@@ -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__':
diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py
index 6b5e52b..492cf8f 100644
--- a/Search-based Planning/Search_2D/plotting.py
+++ b/Search-based Planning/Search_2D/plotting.py
@@ -138,7 +138,7 @@ class Plotting:
cl_v = ['silver',
'wheat',
'lightskyblue',
- 'plum',
+ 'royalblue',
'slategray']
cl_p = ['gray',
'orange',
diff --git a/Search-based Planning/gif/ARA_star.gif b/Search-based Planning/gif/ARA_star.gif
index 7b40a17..dbe5aca 100644
Binary files a/Search-based Planning/gif/ARA_star.gif and b/Search-based Planning/gif/ARA_star.gif differ
diff --git a/Search-based Planning/gif/RepeatedA_star.gif b/Search-based Planning/gif/RepeatedA_star.gif
new file mode 100644
index 0000000..7fdd49c
Binary files /dev/null and b/Search-based Planning/gif/RepeatedA_star.gif differ