diff --git a/README.md b/README.md index 5ff51c6..022bfbf 100644 --- a/README.md +++ b/README.md @@ -27,15 +27,25 @@ Directory Structure └── rrtstar3D.py ## Animations -### Dijkstra's & A* +### DFS & BFS (Dijkstra) * Blue: starting state * Green: goal state
- - + + + +
dijkstraAstardfsbfs
+
+ +### A* and A* Variants +
+ + + +
astarbiastar
diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml index 5965bde..c444878 100644 --- a/Search-based Planning/.idea/Search-based Planning.iml +++ b/Search-based Planning/.idea/Search-based Planning.iml @@ -2,7 +2,7 @@ - + \ No newline at end of file diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml index 0e7ac62..a2e120d 100644 --- a/Search-based Planning/.idea/misc.xml +++ b/Search-based Planning/.idea/misc.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 93aa0ea..4df6d5d 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,20 +20,13 @@ - - - - - - - - - - - - + + + + + - - + + + + + - - - + + + - + + + + + + + + + + + + - - - + + + @@ -199,39 +233,44 @@ - + + - - - + + - + + + + + - + - - + + - - + + - - + + - - + + - + diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc index 1265f1c..a03b5a8 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ 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 8ef4f0e..8bc3bac 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/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc index e83b463..d42d249 100644 Binary files a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search-based Planning/Search_2D/a_star.py b/Search-based Planning/Search_2D/a_star.py index a401961..fa5e2bd 100644 --- a/Search-based Planning/Search_2D/a_star.py +++ b/Search-based Planning/Search_2D/a_star.py @@ -26,11 +26,10 @@ class Astar: self.obs = self.Env.obs # position of obstacles self.g = {self.xI: 0, self.xG: float("inf")} - self.fig_name = "A* Algorithm" - self.OPEN = queue.QueuePrior() # priority queue / OPEN self.OPEN.put(self.xI, self.fvalue(self.xI)) - self.parent = {self.xI: self.xI} + self.CLOSED = [] + self.Parent = {self.xI: self.xI} def searching(self): """ @@ -39,23 +38,25 @@ class Astar: :return: planning path, action in each node, visited nodes in the planning process """ - visited = [] - while not self.OPEN.empty(): s = self.OPEN.get() + self.CLOSED.append(s) + if s == self.xG: # stop condition break - visited.append(s) + for u_next in self.u_set: # explore neighborhoods of current node s_next = tuple([s[i] + u_next[i] for i in range(len(s))]) - if s_next not in self.obs: + if s_next not in self.obs and s_next not in self.CLOSED: 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]: # conditions for updating cost + if s_next not in self.g: + self.g[s_next] = float("inf") + if new_cost < self.g[s_next]: # conditions for updating cost self.g[s_next] = new_cost - self.parent[s_next] = s + self.Parent[s_next] = s self.OPEN.put(s_next, self.fvalue(s_next)) - return self.extract_path(), visited + return self.extract_path(), self.CLOSED def fvalue(self, x): h = self.e * self.Heuristic(x) @@ -72,7 +73,7 @@ class Astar: x_current = self.xG while True: - x_current = self.parent[x_current] + x_current = self.Parent[x_current] path_back.append(x_current) if x_current == self.xI: @@ -116,14 +117,13 @@ class Astar: def main(): x_start = (5, 5) # Starting node - x_goal = (49, 5) # Goal node + x_goal = (49, 25) # Goal node - astar = Astar(x_start, x_goal, 1, "manhattan") + astar = Astar(x_start, x_goal, 1, "euclidean") plot = plotting.Plotting(x_start, x_goal) # class Plotting fig_name = "A* Algorithm" path, visited = astar.searching() - plot.animation(path, visited, fig_name) # animation generate diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py index a33fec5..b66a505 100644 --- a/Search-based Planning/Search_2D/bfs.py +++ b/Search-based Planning/Search_2D/bfs.py @@ -85,5 +85,5 @@ class BFS: if __name__ == '__main__': x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Goal = (49, 25) # Goal node bfs = BFS(x_Start, x_Goal) diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py new file mode 100644 index 0000000..41f7f89 --- /dev/null +++ b/Search-based Planning/Search_2D/bidirectional_a_star.py @@ -0,0 +1,149 @@ +""" +Bidirectional_a_star 2D +@author: huiming zhou +""" + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Search-based Planning/") + +from Search_2D import queue +from Search_2D import plotting +from Search_2D import env + + +class BidirectionalAstar: + def __init__(self, x_start, x_goal, heuristic_type): + self.xI, self.xG = x_start, x_goal + self.heuristic_type = heuristic_type + + self.Env = env.Env() # class Env + + 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")} + self.g_back = {self.xG: 0, self.xI: float("inf")} + + self.OPEN_fore = queue.QueuePrior() + self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG)) + self.OPEN_back = queue.QueuePrior() + self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI)) + + self.CLOSED_fore = [] + self.CLOSED_back = [] + + self.Parent_fore = {self.xI: self.xI} + self.Parent_back = {self.xG: self.xG} + + def searching(self): + visited_fore, visited_back = [], [] + s_meet = self.xI + + while not self.OPEN_fore.empty() and not self.OPEN_back.empty(): + + # solve foreward-search + s_fore = self.OPEN_fore.get() + if s_fore in self.Parent_back: + s_meet = s_fore + break + visited_fore.append(s_fore) + for u in self.u_set: + s_next = tuple([s_fore[i] + u[i] for i in range(len(s_fore))]) + 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)) + + # solve backward-search + s_back = self.OPEN_back.get() + if s_back in self.Parent_fore: + s_meet = s_back + break + visited_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)) + + return self.extract_path(s_meet), visited_fore, visited_back + + def extract_path(self, s): + path_back_fore = [s] + s_current = s + + while True: + s_current = self.Parent_fore[s_current] + path_back_fore.append(s_current) + + if s_current == self.xI: + break + + path_back_back = [] + s_current = s + + while True: + s_current = self.Parent_back[s_current] + path_back_back.append(s_current) + + if s_current == self.xG: + break + + return list(reversed(path_back_fore)) + list(path_back_back) + + def h(self, state, goal): + """ + Calculate heuristic. + :param state: current node (state) + :param goal: goal node (state) + :return: heuristic + """ + + heuristic_type = self.heuristic_type + + 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!") + + @staticmethod + def get_cost(x, u): + """ + Calculate cost for this motion + :param x: current node + :param u: input + :return: cost for this motion + :note: cost function could be more complicate! + """ + + return 1 + + +def main(): + x_start = (5, 5) # Starting node + x_goal = (49, 25) # Goal node + + bastar = BidirectionalAstar(x_start, x_goal, "euclidean") + plot = plotting.Plotting(x_start, x_goal) # class Plotting + + fig_name = "Bidirectional-A* Algorithm" + path, v_fore, v_back = bastar.searching() + plot.animation_bi_astar(path, v_fore, v_back, fig_name) # animation generate + + +if __name__ == '__main__': + main() diff --git a/Search-based Planning/Search_2D/dfs.py b/Search-based Planning/Search_2D/dfs.py index 6f9314a..5ae3683 100644 --- a/Search-based Planning/Search_2D/dfs.py +++ b/Search-based Planning/Search_2D/dfs.py @@ -85,5 +85,5 @@ class DFS: if __name__ == '__main__': x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Goal = (49, 25) # Goal node dfs = DFS(x_Start, x_Goal) diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py index c2b478f..c9a1b0b 100644 --- a/Search-based Planning/Search_2D/dijkstra.py +++ b/Search-based Planning/Search_2D/dijkstra.py @@ -100,5 +100,5 @@ class Dijkstra: if __name__ == '__main__': x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Goal = (49, 25) # Goal node dijkstra = Dijkstra(x_Start, x_Goal) diff --git a/Search-based Planning/Search_2D/ida_star.py b/Search-based Planning/Search_2D/ida_star.py new file mode 100644 index 0000000..66a3d31 --- /dev/null +++ b/Search-based Planning/Search_2D/ida_star.py @@ -0,0 +1,90 @@ +""" +IDA_Star 2D +@author: huiming zhou +""" + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Search-based Planning/") + +from Search_2D import queue +from Search_2D import plotting +from Search_2D import env + + +class IdaStar: + def __init__(self, x_start, x_goal, heuristic_type): + self.xI, self.xG = x_start, x_goal + self.heuristic_type = heuristic_type + + self.Env = env.Env() # class Env + + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + + def ida_star(self): + bound = self.h(self.xI) + path = [self.xI] + + while True: + t = self.searching(path, 0, bound) + if t == self.xG: + return path + if t == float("inf"): + return None + bound = t + + def searching(self, path, g, bound): + s = path[-1] + f = g + self.h(s) + + if f > bound: + return f + if s == self.xG: + return s + + res_min = float("inf") + for u in self.u_set: + s_next = tuple([s[i] + u[i] for i in range(len(s))]) + if s_next not in self.obs and s_next not in path: + path.append(s_next) + t = self.searching(path, g + 1, bound) + if t == self.xG: + return self.xG + if t < res_min: + res_min = t + path.pop() + + return res_min + + def h(self, s): + heuristic_type = self.heuristic_type + goal = self.xG + + 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!") + + +def main(): + x_start = (5, 5) # Starting node + x_goal = (15, 25) # Goal node + + ida_star = IdaStar(x_start, x_goal, "manhattan") + plot = plotting.Plotting(x_start, x_goal) + + path = ida_star.ida_star() + + if path: + plot.animation(path, [], "IDA_Star") + else: + print("Path not found!") + + +if __name__ == '__main__': + main() diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py index f021782..a8ec8b2 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search-based Planning/Search_2D/plotting.py @@ -89,6 +89,35 @@ class Plotting: plt.show() + def animation_bi_astar(self, path, v_fore, v_back, name): + self.plot_grid(name) + self.plot_visited_bi(v_fore, v_back) + self.plot_path(path) + plt.show() + + def plot_visited_bi(self, v_fore, v_back): + if self.xI in v_fore: + v_fore.remove(self.xI) + + if self.xG in v_back: + v_back.remove(self.xG) + + len_fore, len_back = len(v_fore), len(v_back) + + for k in range(max(len_fore, len_back)): + if k < len_fore: + plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o') + if k < len_back: + plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o') + + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + + if k % 10 == 0: + plt.pause(0.001) + plt.pause(0.01) + + @staticmethod def color_list(): cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray'] diff --git a/Search-based Planning/Search_2D/queue.py b/Search-based Planning/Search_2D/queue.py index 8f481ae..0bdacb2 100644 --- a/Search-based Planning/Search_2D/queue.py +++ b/Search-based Planning/Search_2D/queue.py @@ -53,7 +53,14 @@ class QueuePrior: return len(self.queue) == 0 def put(self, item, priority): - heapq.heappush(self.queue, (priority, item)) # reorder x using priority + count = 0 + for (p, x) in self.queue: + if x == item: + self.queue[count] = (priority, item) + break + count += 1 + if count == len(self.queue): + heapq.heappush(self.queue, (priority, item)) # reorder x using priority def get(self): return heapq.heappop(self.queue)[1] # pop out the smallest item diff --git a/Search-based Planning/Search_2D/test.py b/Search-based Planning/Search_2D/test.py new file mode 100644 index 0000000..786fce4 --- /dev/null +++ b/Search-based Planning/Search_2D/test.py @@ -0,0 +1,20 @@ +""" +A_star 2D +@author: huiming zhou +""" + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Search-based Planning/") + +from Search_2D import queue + +q = queue.QueuePrior() +q.put((1, 2), 3) +print(q.enumerate()) +q.put((1, 2), 2) +print(q.enumerate()) +q.put((1, 2), 4) +print(q.enumerate()) diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index ac341d9..d514936 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -51,9 +51,9 @@ class Weighted_A_star(object): while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty strxi = self.OPEN.get() xi = dehash(strxi) + self.CLOSED[strxi] = [] # add the point in CLOSED set self.V.append(xi) visualization(self) - self.CLOSED[strxi] = [] # add the point in CLOSED set allchild = self.children(xi) for xj in allchild: strxj = hash3D(xj) diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 73e363c..d76f6ad 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index cf4be50..d1f5eb2 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search-based Planning/gif/ARA_star.gif b/Search-based Planning/gif/ARA_star.gif new file mode 100644 index 0000000..67a3898 Binary files /dev/null and b/Search-based Planning/gif/ARA_star.gif differ diff --git a/Search-based Planning/gif/Astar.gif b/Search-based Planning/gif/Astar.gif index 786bd9d..0cc0254 100644 Binary files a/Search-based Planning/gif/Astar.gif and b/Search-based Planning/gif/Astar.gif differ diff --git a/Search-based Planning/gif/BFS.gif b/Search-based Planning/gif/BFS.gif index fa0840a..bf24974 100644 Binary files a/Search-based Planning/gif/BFS.gif and b/Search-based Planning/gif/BFS.gif differ diff --git a/Search-based Planning/gif/Bi-Astar.gif b/Search-based Planning/gif/Bi-Astar.gif new file mode 100644 index 0000000..4e4301a Binary files /dev/null and b/Search-based Planning/gif/Bi-Astar.gif differ diff --git a/Search-based Planning/gif/DFS.gif b/Search-based Planning/gif/DFS.gif index d533a12..aa07524 100644 Binary files a/Search-based Planning/gif/DFS.gif and b/Search-based Planning/gif/DFS.gif differ