diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index aed48a7..b1a111a 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,9 +21,23 @@ + + + + + + + + + + + + + + - - + + - + - - - + + + + + - + - - - - + - + + diff --git a/Search-based Planning/Search_2D/ARA_star.py b/Search-based Planning/Search_2D/ARA_star.py index 8591941..d45d016 100644 --- a/Search-based Planning/Search_2D/ARA_star.py +++ b/Search-based Planning/Search_2D/ARA_star.py @@ -1,5 +1,5 @@ """ -ARA_star 2D +ARA_star 2D (Anytime Repairing A*) @author: huiming zhou """ @@ -15,66 +15,66 @@ from Search_2D import env class AraStar: - def __init__(self, x_start, x_goal, heuristic_type): + def __init__(self, x_start, x_goal, e, heuristic_type): 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.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + self.e = e # initial weight + self.g = {self.xI: 0, self.xG: float("inf")} # cost to come - self.e = 2.5 - self.g = {self.xI: 0, self.xG: float("inf")} - self.fig_name = "ARA_Star Algorithm" - - self.OPEN = queue.QueuePrior() # priority queue / OPEN - self.CLOSED = [] - self.INCONS = [] - self.parent = {self.xI: self.xI} - - self.path = [] - self.visited = [] + self.OPEN = queue.QueuePrior() # priority queue / OPEN + self.CLOSED = set() # closed set + self.INCONS = [] # incons set + self.PARENT = {self.xI: self.xI} # relations + self.path = [] # planning path + self.visited = [] # order of visited nodes def searching(self): self.OPEN.put(self.xI, self.fvalue(self.xI)) self.ImprovePath() self.path.append(self.extract_path()) - while self.update_e() > 1: - self.e -= 0.5 - print(self.e) - OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS + while self.update_e() > 1: # continue condition + self.e -= 0.5 # increase weight + OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS # combine two sets self.OPEN = queue.QueuePrior() self.OPEN.put(self.xI, self.fvalue(self.xI)) for x in OPEN_mid: - self.OPEN.put(x, self.fvalue(x)) + self.OPEN.put(x, self.fvalue(x)) # update priority self.INCONS = [] - self.CLOSED = [] - self.ImprovePath() + self.CLOSED = set() + self.ImprovePath() # improve path self.path.append(self.extract_path()) return self.path, self.visited def ImprovePath(self): + """ + :return: a e'-suboptimal path + """ + visited_each = [] + while (self.fvalue(self.xG) > min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])): s = self.OPEN.get() if s not in self.CLOSED: - self.CLOSED.append(s) + self.CLOSED.add(s) for u_next in self.u_set: s_next = tuple([s[i] + u_next[i] for i in range(len(s))]) - if s_next not in self.obs: 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]: self.g[s_next] = new_cost - self.parent[s_next] = s + self.PARENT[s_next] = s visited_each.append(s_next) if s_next not in self.CLOSED: @@ -87,10 +87,10 @@ class AraStar: def update_e(self): c_OPEN, c_INCONS = float("inf"), float("inf") - if not self.OPEN.empty(): + if self.OPEN: c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate()) - if len(self.INCONS) != 0: + if self.INCONS: c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS) if min(c_OPEN, c_INCONS) == float("inf"): @@ -99,14 +99,12 @@ class AraStar: return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS)) def fvalue(self, x): - h = self.e * self.Heuristic(x) - return self.g[x] + h + return self.g[x] + self.e * self.Heuristic(x) def extract_path(self): """ Extract the path based on the relationship of nodes. - :param policy: Action needed for transfer between two nodes :return: The planning path """ @@ -114,7 +112,7 @@ class AraStar: 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: @@ -126,7 +124,6 @@ class AraStar: def get_cost(x, u): """ Calculate cost for this motion - :param x: current node :param u: input :return: cost for this motion @@ -139,8 +136,6 @@ class AraStar: """ Calculate heuristic. :param state: current node (state) - :param goal: goal node (state) - :param heuristic_type: choosing different heuristic functions :return: heuristic """ @@ -159,10 +154,10 @@ def main(): x_start = (5, 5) # Starting node x_goal = (49, 5) # Goal node - arastar = AraStar(x_start, x_goal, "manhattan") + arastar = AraStar(x_start, x_goal, 2.5, "manhattan") plot = plotting.Plotting(x_start, x_goal) - fig_name = "ARA* algorithm" + fig_name = "Anytime Repairing A* (ARA*)" path, visited = arastar.searching() plot.animation_ara_star(path, visited, fig_name) diff --git a/Search-based Planning/Search_2D/IDA_star.py b/Search-based Planning/Search_2D/IDA_star.py index d6d945e..6fd06e2 100644 --- a/Search-based Planning/Search_2D/IDA_star.py +++ b/Search-based Planning/Search_2D/IDA_star.py @@ -1,5 +1,5 @@ """ -IDA_Star 2D +IDA_Star 2D (Iteratively Deepening A*) @author: huiming zhou """ @@ -19,10 +19,10 @@ class IdaStar: 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.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles self.visited = [] @@ -75,23 +75,21 @@ class IdaStar: def main(): - x_start = (5, 5) # Starting node - x_goal = (15, 20) # Goal node + x_start = (5, 5) + x_goal = (15, 20) ida_star = IdaStar(x_start, x_goal, "manhattan") plot = plotting.Plotting(x_start, x_goal) path, visited = ida_star.ida_star() - print(len(visited)) if path: - plot.plot_grid("IDA_star") + plot.plot_grid("Iteratively Deepening A*") 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 644afd1..858300c 100644 --- a/Search-based Planning/Search_2D/LRTA_star.py +++ b/Search-based Planning/Search_2D/LRTA_star.py @@ -1,5 +1,5 @@ """ -LRTA_star_N 2D +LRTA_star 2D (Learning Real-time A*) @author: huiming zhou """ @@ -17,92 +17,93 @@ from Search_2D import env class LrtAstarN: - def __init__(self, x_start, x_goal, heuristic_type): + def __init__(self, x_start, x_goal, N, heuristic_type): self.xI, self.xG = x_start, x_goal self.heuristic_type = heuristic_type - self.Env = env.Env() # class Env + self.Env = env.Env() - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - self.N = 150 - self.visited = [] + self.N = N # number of expand nodes each iteration + self.visited = [] # order of visited nodes in planning + self.path = [] # path of each iteration def searching(self): - s_start = self.xI - - path = [] - count = 0 + s_start = self.xI # initialize start node while True: - # if count == 2: - # return path - # count += 1 + OPEN, CLOSED = self.Astar(s_start, self.N) # OPEN, CLOSED sets in each iteration - h_table = {} - OPEN, CLOSED = self.Astar(s_start, self.N) + if OPEN == "FOUND": # reach the goal node + self.path.append(CLOSED) + break - if OPEN == "end": - path.append(CLOSED) - return path + h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes + s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in OPEN set + self.path.append(path_k) - for x in CLOSED: - h_table[x] = 2000 + def extract_path_in_CLOSE(self, s_start, h_value): + path = [s_start] + s = s_start - 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 + while True: + 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 in h_value: + h_list[s_next] = h_value[s_next] + else: + h_list[s_next] = self.h(s_next) + s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value + path.append(s_key) # generate path + s = s_key # use end of this iteration as the start of next - path_k = [s_start] - x = s_start - while True: - h_xlist = {} + if s_key not in h_value: # reach the expected node in OPEN set + return s_key, path + + def iteration(self, CLOSED): + h_value = {} + + for s in CLOSED: + h_value[s] = float("inf") # initialize h_value of CLOSED nodes + + while True: + h_value_rec = copy.deepcopy(h_value) + for s in CLOSED: + h_list = [] 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] + 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_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] + h_list.append(self.get_cost(s, s_next) + h_value[s_next]) + h_value[s] = min(h_list) # update h_value of current node - path.append(path_k) + if h_value == h_value_rec: # h_value table converged + return h_value def Astar(self, x_start, N): - OPEN = queue.QueuePrior() + OPEN = queue.QueuePrior() # OPEN set 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 = [] + CLOSED = set() # CLOSED set + g_table = {x_start: 0, self.xG: float("inf")} # cost to come + PARENT = {x_start: x_start} # relations + visited = [] # order of visited nodes + count = 0 # counter 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) + + if s == self.xG: # reach the goal node self.visited.append(visited) - return "end", path + return "FOUND", self.extract_path(x_start, PARENT) for u in self.u_set: s_next = tuple([s[i] + u[i] for i in range(len(s))]) @@ -110,14 +111,15 @@ class LrtAstarN: 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 + if new_cost < g_table[s_next]: # conditions for updating cost g_table[s_next] = new_cost - parent[s_next] = s + PARENT[s_next] = s OPEN.put(s_next, g_table[s_next] + self.h(s_next)) - if count == N: + if count == N: # expand needed CLOSED nodes break - self.visited.append(visited) + + self.visited.append(visited) # visited nodes in each iteration return OPEN, CLOSED @@ -166,29 +168,15 @@ class LrtAstarN: def main(): - x_start = (10, 5) # Starting node - x_goal = (45, 25) # Goal node + x_start = (10, 5) + x_goal = (45, 25) - lrtastarn = LrtAstarN(x_start, x_goal, "euclidean") + lrta = LrtAstarN(x_start, x_goal, 150, "euclidean") plot = plotting.Plotting(x_start, x_goal) + fig_name = "Learning Real-time A* (LRTA*)" - 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() + lrta.searching() + plot.animation_lrta(lrta.path, lrta.visited, fig_name) if __name__ == '__main__': 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 61fa3ec..67b7b79 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/a_star.py b/Search-based Planning/Search_2D/a_star.py index fa5e2bd..f75d4df 100644 --- a/Search-based Planning/Search_2D/a_star.py +++ b/Search-based Planning/Search_2D/a_star.py @@ -19,48 +19,53 @@ 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 - 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")} - self.OPEN = queue.QueuePrior() # priority queue / OPEN + 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 = [] - self.Parent = {self.xI: self.xI} + self.CLOSED = [] # closed set & visited + self.PARENT = {self.xI: self.xI} # relations def searching(self): """ Searching using A_star. - :return: planning path, action in each node, visited nodes in the planning process + :return: path, order of visited nodes in the planning """ while not self.OPEN.empty(): s = self.OPEN.get() self.CLOSED.append(s) - if s == self.xG: # stop condition + if s == self.xG: # stop condition break - 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))]) + 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_next) + new_cost = self.g[s] + self.get_cost(s, u) 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(), self.CLOSED def fvalue(self, x): - h = self.e * self.Heuristic(x) - return self.g[x] + h + """ + 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) def extract_path(self): """ @@ -73,7 +78,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: @@ -87,7 +92,7 @@ class Astar: Calculate cost for this motion :param x: current node - :param u: input + :param u: current input :return: cost for this motion :note: cost function could be more complicate! """ @@ -99,13 +104,11 @@ class Astar: Calculate heuristic. :param state: current node (state) - :param goal: goal node (state) - :param heuristic_type: choosing different heuristic functions - :return: heuristic + :return: heuristic function value """ - heuristic_type = self.heuristic_type - goal = self.xG + 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]) @@ -116,15 +119,15 @@ class Astar: def main(): - x_start = (5, 5) # Starting node - x_goal = (49, 25) # Goal node + x_start = (5, 5) + x_goal = (45, 25) - astar = Astar(x_start, x_goal, 1, "euclidean") - plot = plotting.Plotting(x_start, x_goal) # class Plotting + astar = Astar(x_start, x_goal, 1, "euclidean") # weight e = 1 + plot = plotting.Plotting(x_start, x_goal) # class Plotting - fig_name = "A* Algorithm" + fig_name = "A*" path, visited = astar.searching() - plot.animation(path, visited, fig_name) # animation generate + plot.animation(path, visited, fig_name) # animation generate if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py index b66a505..70b8675 100644 --- a/Search-based Planning/Search_2D/bfs.py +++ b/Search-based Planning/Search_2D/bfs.py @@ -1,5 +1,5 @@ """ -BFS 2D +BFS 2D (Breadth-first Searching) @author: huiming zhou """ @@ -21,69 +21,62 @@ class BFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + self.OPEN = queue.QueueFIFO() # OPEN set: visited nodes + self.OPEN.put(self.xI) + self.CLOSED = [] # CLOSED set: explored nodes + self.PARENT = {self.xI: self.xI} # relations - self.fig_name = "Breadth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - - def searching(self, xI, xG): + def searching(self): """ - Searching using BFS. - - :return: planning path, action in each node, visited nodes in the planning process + :return: path, order of visited nodes in the planning """ - q_bfs = queue.QueueFIFO() # first-in-first-out queue - q_bfs.put(xI) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes - visited = [] - - while not q_bfs.empty(): - x_current = q_bfs.get() - if x_current == xG: + while not self.OPEN.empty(): + s = self.OPEN.get() + if s == self.xG: break - visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node - x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles - q_bfs.put(x_next) - parent[x_next], action[x_next] = x_current, u_next + self.CLOSED.append(s) - [path, policy] = self.extract_path(xI, xG, parent, action) # extract path + for u_next in self.u_set: # explore neighborhoods + s_next = tuple([s[i] + u_next[i] for i in range(2)]) + if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles + self.OPEN.put(s_next) + self.PARENT[s_next] = s - return path, policy, visited + return self.extract_path(), self.CLOSED - @staticmethod - def extract_path(xI, xG, parent, policy): + def extract_path(self): """ Extract the path based on the relationship of nodes. - - :param xI: Starting node - :param xG: Goal node - :param parent: Relationship between nodes - :param policy: Action needed for transfer between two nodes :return: The planning path """ - path_back = [xG] - acts_back = [policy[xG]] - x_current = xG - while True: - x_current = parent[x_current] - path_back.append(x_current) - acts_back.append(policy[x_current]) + path = [self.xG] + s = self.xG - if x_current == xI: + while True: + s = self.PARENT[s] + path.append(s) + if s == self.xI: break - return list(path_back), list(acts_back) + return list(path) + + +def main(): + x_start = (5, 5) # Starting node + x_goal = (49, 25) # Goal node + + bfs = BFS(x_start, x_goal) + plot = plotting.Plotting(x_start, x_goal) + fig_name = "Breadth-first Searching (BFS)" + + path, visited = bfs.searching() + plot.animation(path, visited, fig_name) # animation if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 25) # Goal node - bfs = BFS(x_Start, x_Goal) + main() diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py index 41f7f89..65d256f 100644 --- a/Search-based Planning/Search_2D/bidirectional_a_star.py +++ b/Search-based Planning/Search_2D/bidirectional_a_star.py @@ -19,54 +19,54 @@ class BidirectionalAstar: 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.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + 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.g_fore = {self.xI: 0, self.xG: float("inf")} # cost to come: from x_start + self.g_back = {self.xG: 0, self.xI: float("inf")} # cost to come: form x_goal - self.OPEN_fore = queue.QueuePrior() + self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching 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 = queue.QueuePrior() # OPEN set for backward searching self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI)) - self.CLOSED_fore = [] - self.CLOSED_back = [] + self.CLOSED_fore = [] # CLOSED set for foreward + self.CLOSED_back = [] # CLOSED set for backward - self.Parent_fore = {self.xI: self.xI} - self.Parent_back = {self.xG: self.xG} + 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: + if s_fore in self.PARENT_back: s_meet = s_fore break - visited_fore.append(s_fore) + self.CLOSED_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))]) + s_next = tuple([s_fore[i] + u[i] for i in range(2)]) 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.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: + if s_back in self.PARENT_fore: s_meet = s_back break - visited_back.append(s_back) + self.CLOSED_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: @@ -75,48 +75,48 @@ class BidirectionalAstar: 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.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 + return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back - def extract_path(self, s): - path_back_fore = [s] - s_current = s + def extract_path(self, s_meet): + # extract path for foreward part + path_fore = [s_meet] + s = s_meet while True: - s_current = self.Parent_fore[s_current] - path_back_fore.append(s_current) - - if s_current == self.xI: + s = self.PARENT_fore[s] + path_fore.append(s) + if s == self.xI: break - path_back_back = [] - s_current = s + # extract path for backward part + path_back = [] + s = s_meet while True: - s_current = self.Parent_back[s_current] - path_back_back.append(s_current) - - if s_current == self.xG: + s = self.PARENT_back[s] + path_back.append(s) + if s == self.xG: break - return list(reversed(path_back_fore)) + list(path_back_back) + return list(reversed(path_fore)) + list(path_back) - def h(self, state, goal): + def h(self, s, goal): """ - Calculate heuristic. - :param state: current node (state) + Calculate heuristic value. + :param s: current node (state) :param goal: goal node (state) - :return: heuristic + :return: heuristic value """ heuristic_type = self.heuristic_type if heuristic_type == "manhattan": - return abs(goal[0] - state[0]) + abs(goal[1] - state[1]) + return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) elif heuristic_type == "euclidean": - return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2) + return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2) else: print("Please choose right heuristic type!") @@ -134,15 +134,15 @@ class BidirectionalAstar: def main(): - x_start = (5, 5) # Starting node - x_goal = (49, 25) # Goal node + x_start = (5, 5) + x_goal = (45, 25) 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 + plot = plotting.Plotting(x_start, x_goal) + fig_name = "Bidirectional-A*" + + path, visited_fore, visited_back = bastar.searching() + plot.animation_bi_astar(path, visited_fore, visited_back, fig_name) # animation if __name__ == '__main__': diff --git a/Search-based Planning/Search_2D/dfs.py b/Search-based Planning/Search_2D/dfs.py index 5ae3683..1b0350f 100644 --- a/Search-based Planning/Search_2D/dfs.py +++ b/Search-based Planning/Search_2D/dfs.py @@ -21,69 +21,64 @@ class DFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + self.OPEN = queue.QueueLIFO() # OPEN set: visited nodes + self.OPEN.put(self.xI) + self.CLOSED = [] # CLOSED set: explored nodes + self.PARENT = {self.xI: self.xI} # relations - self.fig_name = "Depth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - - def searching(self, xI, xG): + def searching(self): """ Searching using DFS. :return: planning path, action in each node, visited nodes in the planning process """ - q_dfs = queue.QueueLIFO() # last-in-first-out queue - q_dfs.put(xI) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes - visited = [] - - while not q_dfs.empty(): - x_current = q_dfs.get() - if x_current == xG: + while not self.OPEN.empty(): + s = self.OPEN.get() + if s == self.xG: break - visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node - x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles - q_dfs.put(x_next) - parent[x_next], action[x_next] = x_current, u_next + self.CLOSED.append(s) - [path, policy] = self.extract_path(xI, xG, parent, action) + for u in self.u_set: # explore neighborhoods + s_next = tuple([s[i] + u[i] for i in range(2)]) + if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles + self.OPEN.put(s_next) + self.PARENT[s_next] = s - return path, policy, visited + return self.extract_path(), self.CLOSED - @staticmethod - def extract_path(xI, xG, parent, policy): + def extract_path(self): """ Extract the path based on the relationship of nodes. - - :param xI: Starting node - :param xG: Goal node - :param parent: Relationship between nodes - :param policy: Action needed for transfer between two nodes :return: The planning path """ - path_back = [xG] - acts_back = [policy[xG]] - x_current = xG - while True: - x_current = parent[x_current] - path_back.append(x_current) - acts_back.append(policy[x_current]) + path = [self.xG] + s = self.xG - if x_current == xI: + while True: + s = self.PARENT[s] + path.append(s) + if s == self.xI: break - return list(path_back), list(acts_back) + return list(path) + + +def main(): + x_start = (5, 5) + x_goal = (45, 25) + + dfs = DFS(x_start, x_goal) + plot = plotting.Plotting(x_start, x_goal) + fig_name = "Depth-first Searching (DFS)" + + path, visited = dfs.searching() + plot.animation(path, visited, fig_name) # animation if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 25) # Goal node - dfs = DFS(x_Start, x_Goal) + main() diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py index c9a1b0b..20f3532 100644 --- a/Search-based Planning/Search_2D/dijkstra.py +++ b/Search-based Planning/Search_2D/dijkstra.py @@ -13,6 +13,7 @@ from Search_2D import queue from Search_2D import plotting from Search_2D import env + class Dijkstra: def __init__(self, x_start, x_goal): self.xI, self.xG = x_start, x_goal @@ -20,46 +21,58 @@ class Dijkstra: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - self.u_set = self.Env.motions # feasible input set - self.obs = self.Env.obs # position of obstacles + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles - [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + 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, 0) + self.CLOSED = [] # closed set & visited + self.PARENT = {self.xI: self.xI} # relations - self.fig_name = "Dijkstra's Algorithm" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - - def searching(self, xI, xG): + def searching(self): """ Searching using Dijkstra. - - :return: planning path, action in each node, visited nodes in the planning process + :return: path, order of visited nodes in the planning """ - q_dijk = queue.QueuePrior() # priority queue - q_dijk.put(xI, 0) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes - visited = [] # record visited nodes - cost = {xI: 0} - - while not q_dijk.empty(): - x_current = q_dijk.get() - if x_current == xG: # stop condition + while not self.OPEN.empty(): + s = self.OPEN.get() + if s == self.xG: # stop condition break - visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node - x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in self.obs: # node not visited and not in obstacles - new_cost = cost[x_current] + self.get_cost(x_current, u_next) - if x_next not in cost or new_cost < cost[x_next]: - cost[x_next] = new_cost - priority = new_cost - q_dijk.put(x_next, priority) # put node into queue using cost to come as priority - parent[x_next], action[x_next] = x_current, u_next + self.CLOSED.append(s) - [path, policy] = self.extract_path(xI, xG, parent, action) + for u in self.u_set: # explore neighborhoods + s_next = tuple([s[i] + u[i] for i in range(2)]) + if s_next not in self.obs: # node not visited and not in obstacles + new_cost = self.g[s] + self.get_cost(s, u) + if s_next not in self.g: + self.g[s_next] = float("inf") + if new_cost < self.g[s_next]: + self.g[s_next] = new_cost + self.OPEN.put(s_next, new_cost) + self.PARENT[s_next] = s - return path, policy, visited + return self.extract_path(), self.CLOSED + + def extract_path(self): + """ + 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 = self.PARENT[x_current] + path_back.append(x_current) + + if x_current == self.xI: + break + + return list(path_back) @staticmethod def get_cost(x, u): @@ -74,31 +87,18 @@ class Dijkstra: return 1 - @staticmethod - def extract_path(xI, xG, parent, policy): - """ - Extract the path based on the relationship of nodes. - :param xI: Starting node - :param xG: Goal node - :param parent: Relationship between nodes - :param policy: Action needed for transfer between two nodes - :return: The planning path - """ +def main(): + x_start = (5, 5) + x_goal = (45, 25) - path_back = [xG] - acts_back = [policy[xG]] - x_current = xG - while True: - x_current = parent[x_current] - path_back.append(x_current) - acts_back.append(policy[x_current]) - if x_current == xI: break + dijkstra = Dijkstra(x_start, x_goal) + plot = plotting.Plotting(x_start, x_goal) # class Plotting - return list(path_back), list(acts_back) + fig_name = "Dijkstra's" + path, visited = dijkstra.searching() + plot.animation(path, visited, fig_name) # animation generate if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 25) # Goal node - dijkstra = Dijkstra(x_Start, x_Goal) + main() diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py index 73289e0..cce02a8 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search-based Planning/Search_2D/plotting.py @@ -25,6 +25,38 @@ class Plotting: self.plot_path(path) plt.show() + def animation_lrta(self, path, visited, name): + self.plot_grid(name) + cl = self.color_list_2() + path_combine = [] + + for k in range(len(path)): + self.plot_visited(visited[k], cl[k]) + plt.pause(0.2) + self.plot_path(path[k]) + path_combine += path[k] + plt.pause(0.2) + + self.plot_path(path_combine) + plt.show() + + def animation_ara_star(self, path, visited, name): + self.plot_grid(name) + cl_v, cl_p = self.color_list() + + for k in range(len(path)): + self.plot_visited(visited[k], cl_v[k]) + self.plot_path(path[k], cl_p[k], True) + plt.pause(0.5) + + 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_grid(self, name): obs_x = [self.obs[i][0] for i in range(len(self.obs))] obs_y = [self.obs[i][1] for i in range(len(self.obs))] @@ -78,23 +110,6 @@ class Plotting: plt.pause(0.01) - def animation_ara_star(self, path, visited, name): - self.plot_grid(name) - cl_v, cl_p = self.color_list() - - for k in range(len(path)): - self.plot_visited(visited[k], cl_v[k]) - self.plot_path(path[k], cl_p[k], True) - plt.pause(0.5) - - 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) @@ -119,6 +134,29 @@ class Plotting: @staticmethod def color_list(): - cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray'] - cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm'] + cl_v = ['silver', + 'wheat', + 'lightskyblue', + 'plum', + 'slategray'] + cl_p = ['gray', + 'orange', + 'deepskyblue', + 'red', + 'm'] return cl_v, cl_p + + @staticmethod + def color_list_2(): + cl = ['silver', + 'steelblue', + 'dimgray', + 'cornflowerblue', + 'dodgerblue', + 'royalblue', + 'plum', + 'mediumslateblue', + 'mediumpurple', + 'blueviolet', + ] + return cl diff --git a/Search-based Planning/Search_2D/test.py b/Search-based Planning/Search_2D/test.py deleted file mode 100644 index 786fce4..0000000 --- a/Search-based Planning/Search_2D/test.py +++ /dev/null @@ -1,20 +0,0 @@ -""" -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/gif/ARA_star.gif b/Search-based Planning/gif/ARA_star.gif index 67a3898..5551531 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/Astar.gif b/Search-based Planning/gif/Astar.gif index 0cc0254..ee699c6 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 bf24974..ddccff8 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 index 4e4301a..93defaf 100644 Binary files a/Search-based Planning/gif/Bi-Astar.gif 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 aa07524..3fe684d 100644 Binary files a/Search-based Planning/gif/DFS.gif and b/Search-based Planning/gif/DFS.gif differ diff --git a/Search-based Planning/gif/Dijkstra.gif b/Search-based Planning/gif/Dijkstra.gif index 7706004..d0901eb 100644 Binary files a/Search-based Planning/gif/Dijkstra.gif and b/Search-based Planning/gif/Dijkstra.gif differ diff --git a/Search-based Planning/gif/LRTA_star.gif b/Search-based Planning/gif/LRTA_star.gif index d699d44..6f2ddb6 100644 Binary files a/Search-based Planning/gif/LRTA_star.gif and b/Search-based Planning/gif/LRTA_star.gif differ