diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index f4555aa..aed48a7 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,10 +20,10 @@ - - - + + + - + + + + - + - - - diff --git a/Search-based Planning/Search_2D/IDA_star.py b/Search-based Planning/Search_2D/IDA_star.py index 66a3d31..d6d945e 100644 --- a/Search-based Planning/Search_2D/IDA_star.py +++ b/Search-based Planning/Search_2D/IDA_star.py @@ -5,11 +5,11 @@ IDA_Star 2D import os import sys +import matplotlib.pyplot as plt 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 @@ -24,6 +24,8 @@ class IdaStar: self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles + self.visited = [] + def ida_star(self): bound = self.h(self.xI) path = [self.xI] @@ -31,13 +33,14 @@ class IdaStar: while True: t = self.searching(path, 0, bound) if t == self.xG: - return path + return path, self.visited if t == float("inf"): - return None + return [], self.visited bound = t def searching(self, path, g, bound): s = path[-1] + self.visited.append(s) f = g + self.h(s) if f > bound: @@ -73,17 +76,22 @@ class IdaStar: def main(): x_start = (5, 5) # Starting node - x_goal = (15, 25) # Goal node + x_goal = (15, 20) # Goal node ida_star = IdaStar(x_start, x_goal, "manhattan") plot = plotting.Plotting(x_start, x_goal) - path = ida_star.ida_star() + path, visited = ida_star.ida_star() + print(len(visited)) if path: - plot.animation(path, [], "IDA_Star") + plot.plot_grid("IDA_star") + plot.plot_path(visited, 'gray', True) + plot.plot_path(path) + plt.show() else: print("Path not found!") + plot.plot_grid("IDA") if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/LRTA_star.py b/Search-based Planning/Search_2D/LRTA_star.py index 7d3ba64..644afd1 100644 --- a/Search-based Planning/Search_2D/LRTA_star.py +++ b/Search-based Planning/Search_2D/LRTA_star.py @@ -1,10 +1,11 @@ """ -LRTA_star 2D +LRTA_star_N 2D @author: huiming zhou """ import os import sys +import copy import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + @@ -15,7 +16,7 @@ from Search_2D import plotting from Search_2D import env -class LrtAstar: +class LrtAstarN: def __init__(self, x_start, x_goal, heuristic_type): self.xI, self.xG = x_start, x_goal self.heuristic_type = heuristic_type @@ -25,50 +26,119 @@ class LrtAstar: 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")} - self.OPEN = queue.QueuePrior() # priority queue / OPEN - self.OPEN.put(self.xI, self.h(self.xI)) - self.CLOSED = set() - self.Parent = {self.xI: self.xI} + self.N = 150 + self.visited = [] def searching(self): - h = {self.xI: self.h(self.xI)} - s = self.xI - parent = {self.xI: self.xI} - visited = [] - count = 0 - while s != self.xG: - count += 1 - print(count) - visited.append(s) - h_list = {} - 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: - if s_next not in h: - h[s_next] = self.h(s_next) - h_list[s_next] = self.get_cost(s, s_next) + h[s_next] - h_new = min(h_list.values()) - if h_new > h[s]: - h[s] = h_new - s_child = min(h_list, key=h_list.get) - parent[s_child] = s - s = s_child - # path_get = self.extract_path(parent) - return [], visited + s_start = self.xI - def extract_path(self, parent): - path = [self.xG] - s = self.xG + path = [] + count = 0 while True: - s = parent[s] - path.append(s) + # if count == 2: + # return path + # count += 1 - if s == self.xI: + h_table = {} + OPEN, CLOSED = self.Astar(s_start, self.N) + + if OPEN == "end": + path.append(CLOSED) + return path + + for x in CLOSED: + h_table[x] = 2000 + + while True: + h_table_rec = copy.deepcopy(h_table) + for s in CLOSED: + h_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: + if s_next not in CLOSED: + h_list.append(self.get_cost(s, s_next) + self.h(s_next)) + else: + h_list.append(self.get_cost(s, s_next) + h_table[s_next]) + h_table[s] = min(h_list) + if h_table == h_table_rec: + break + + path_k = [s_start] + x = s_start + while True: + h_xlist = {} + for u in self.u_set: + x_next = tuple([x[i] + u[i] for i in range(2)]) + if x_next not in self.obs: + if x_next in CLOSED: + h_xlist[x_next] = h_table[x_next] + else: + h_xlist[x_next] = self.h(x_next) + s_key = min(h_xlist, key=h_xlist.get) + path_k.append(s_key) + x = s_key + if s_key not in CLOSED: + break + s_start = path_k[-1] + + path.append(path_k) + + def Astar(self, x_start, N): + OPEN = queue.QueuePrior() + OPEN.put(x_start, self.h(x_start)) + CLOSED = set() + g_table = {x_start: 0, self.xG: float("inf")} + parent = {x_start: x_start} + count = 0 + visited = [] + + while not OPEN.empty(): + count += 1 + s = OPEN.get() + CLOSED.add(s) + visited.append(s) + if s == self.xG: + path = self.extract_path(x_start, parent) + self.visited.append(visited) + return "end", path + + 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 CLOSED: + new_cost = g_table[s] + self.get_cost(s, u) + if s_next not in g_table: + g_table[s_next] = float("inf") + if new_cost < g_table[s_next]: # conditions for updating cost + g_table[s_next] = new_cost + parent[s_next] = s + OPEN.put(s_next, g_table[s_next] + self.h(s_next)) + + if count == N: + break + self.visited.append(visited) + + return OPEN, CLOSED + + def extract_path(self, x_start, parent): + """ + Extract the path based on the relationship of nodes. + + :return: The planning path + """ + + path_back = [self.xG] + x_current = self.xG + + while True: + x_current = parent[x_current] + path_back.append(x_current) + + if x_current == x_start: break - return path + return list(reversed(path_back)) def h(self, s): heuristic_type = self.heuristic_type @@ -99,17 +169,25 @@ def main(): x_start = (10, 5) # Starting node x_goal = (45, 25) # Goal node - lrtastar = LrtAstar(x_start, x_goal, "manhattan") - plot = plotting.Plotting(x_start, x_goal) # class Plotting + lrtastarn = LrtAstarN(x_start, x_goal, "euclidean") + plot = plotting.Plotting(x_start, x_goal) - path, visited = lrtastar.searching() - pathx = [x[0] for x in path] - pathy = [x[1] for x in path] - vx = [x[0] for x in visited] - vy = [x[1] for x in visited] - plot.plot_grid("test") - plt.plot(pathx, pathy, 'r') - plt.plot(vx, vy, 'gray') + path = lrtastarn.searching() + plot.plot_grid("LRTA_star_N") + + for k in range(len(path)): + plot.plot_visited(lrtastarn.visited[k]) + plt.pause(0.5) + plot.plot_path(path[k]) + plt.pause(0.5) + plt.pause(0.5) + + path_u = [] + for i in range(len(path)): + for j in range(len(path[i])): + path_u.append(path[i][j]) + plot.plot_path(path_u) + plt.pause(0.2) plt.show() 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 8bc3bac..61fa3ec 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/plotting.py b/Search-based Planning/Search_2D/plotting.py index a8ec8b2..73289e0 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search-based Planning/Search_2D/plotting.py @@ -117,7 +117,6 @@ class Plotting: 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/gif/LRTA_star.gif b/Search-based Planning/gif/LRTA_star.gif new file mode 100644 index 0000000..d699d44 Binary files /dev/null and b/Search-based Planning/gif/LRTA_star.gif differ