diff --git a/README.md b/README.md index 33ba91f..04e6387 100644 --- a/README.md +++ b/README.md @@ -50,8 +50,8 @@ Directory Structure
- - + +
astarbiastarastarbiastar
diff --git a/Search_based_Planning/Search_2D/Astar.py b/Search_based_Planning/Search_2D/Astar.py index d3dacd6..7f040ec 100644 --- a/Search_based_Planning/Search_2D/Astar.py +++ b/Search_based_Planning/Search_2D/Astar.py @@ -20,15 +20,15 @@ class AStar: self.s_goal = s_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.OPEN = [] # priority queue / OPEN set - self.CLOSED = [] # CLOSED set / VISITED order - self.PARENT = dict() # recorded parent - self.g = dict() # cost to come + self.OPEN = [] # priority queue / OPEN set + self.CLOSED = [] # CLOSED set / VISITED order + self.PARENT = dict() # recorded parent + self.g = dict() # cost to come def searching(self): """ @@ -46,7 +46,7 @@ class AStar: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) - if s == self.s_goal: # stop condition + if s == self.s_goal: # stop condition break for s_n in self.get_neighbor(s): @@ -55,7 +55,7 @@ class AStar: if s_n not in self.g: self.g[s_n] = math.inf - if new_cost < self.g[s_n]: # conditions for updating Cost + if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s heapq.heappush(self.OPEN, (self.f_value(s_n), s_n)) @@ -108,7 +108,7 @@ class AStar: if s_n not in g: g[s_n] = math.inf - if new_cost < g[s_n]: # conditions for updating Cost + if new_cost < g[s_n]: # conditions for updating Cost g[s_n] = new_cost PARENT[s_n] = s heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n)) @@ -196,8 +196,8 @@ class AStar: :return: heuristic function value """ - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node + heuristic_type = self.heuristic_type # heuristic type + goal = self.s_goal # goal node if heuristic_type == "manhattan": return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) @@ -213,7 +213,7 @@ def main(): plot = plotting.Plotting(s_start, s_goal) path, visited = astar.searching() - plot.animation(path, visited, "A*") # animation + plot.animation(path, visited, "A*") # animation # path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5 # plot.animation_ara_star(path, visited, "Repeated A*") diff --git a/Search_based_Planning/Search_2D/Best_First.py b/Search_based_Planning/Search_2D/Best_First.py index c820a0a..1ca2bb6 100644 --- a/Search_based_Planning/Search_2D/Best_First.py +++ b/Search_based_Planning/Search_2D/Best_First.py @@ -22,12 +22,12 @@ class BestFirst: self.Env = env.Env() self.plotting = plotting.Plotting(self.s_start, self.s_goal) - 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.OPEN = [] # OPEN set: visited nodes - self.CLOSED = [] # CLOSED set / visited order - self.PARENT = dict() # recorded parent + self.OPEN = [] # OPEN set: visited nodes + self.CLOSED = [] # CLOSED set / visited order + self.PARENT = dict() # recorded parent def searching(self): """ @@ -50,7 +50,7 @@ class BestFirst: if self.is_collision(s, s_n): continue - if s_n not in self.PARENT: # node not explored + if s_n not in self.PARENT: # node not explored heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n)) self.PARENT[s_n] = s diff --git a/Search_based_Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py index 604c410..3b6bf12 100644 --- a/Search_based_Planning/Search_2D/Bidirectional_a_star.py +++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py @@ -20,19 +20,19 @@ class BidirectionalAStar: self.s_goal = s_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.OPEN_fore = [] # OPEN set for forward searching - self.OPEN_back = [] # OPEN set for backward searching - self.CLOSED_fore = [] # CLOSED set for forward - self.CLOSED_back = [] # CLOSED set for backward - self.PARENT_fore = dict() # recorded parent for forward - self.PARENT_back = dict() # recorded parent for backward - self.g_fore = dict() # cost to come for forward - self.g_back = dict() # cost to come for backward + self.OPEN_fore = [] # OPEN set for forward searching + self.OPEN_back = [] # OPEN set for backward searching + self.CLOSED_fore = [] # CLOSED set for forward + self.CLOSED_back = [] # CLOSED set for backward + self.PARENT_fore = dict() # recorded parent for forward + self.PARENT_back = dict() # recorded parent for backward + self.g_fore = dict() # cost to come for forward + self.g_back = dict() # cost to come for backward def init(self): """ @@ -220,7 +220,7 @@ def main(): bastar = BidirectionalAStar(x_start, x_goal, "euclidean") plot = plotting.Plotting(x_start, x_goal) - + path, visited_fore, visited_back = bastar.searching() plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation diff --git a/Search_based_Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py index 6d6f716..7f3f090 100644 --- a/Search_based_Planning/Search_2D/D_star.py +++ b/Search_based_Planning/Search_2D/D_star.py @@ -11,8 +11,7 @@ import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") -from Search_2D import plotting -from Search_2D import env +from Search_based_Planning.Search_2D import plotting, env class Dstar: diff --git a/Search_based_Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py index efa99aa..1f9414a 100644 --- a/Search_based_Planning/Search_2D/Dijkstra.py +++ b/Search_based_Planning/Search_2D/Dijkstra.py @@ -22,13 +22,13 @@ class Dijkstra: self.Env = env.Env() self.plotting = plotting.Plotting(self.s_start, self.s_goal) - 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.OPEN = [] # priority queue / OPEN set - self.CLOSED = [] # closed set & visited - self.PARENT = dict() # record parent - self.g = dict() # Cost to come + self.OPEN = [] # priority queue / OPEN set + self.CLOSED = [] # closed set & visited + self.PARENT = dict() # record parent + self.g = dict() # Cost to come def searching(self): """ @@ -132,7 +132,7 @@ def main(): plot = plotting.Plotting(s_start, s_goal) path, visited = dijkstra.searching() - plot.animation(path, visited, "Dijkstra's") # animation generate + plot.animation(path, visited, "Dijkstra's") # animation generate if __name__ == '__main__': diff --git a/Search_based_Planning/Search_2D/LRTAstar.py b/Search_based_Planning/Search_2D/LRTAstar.py index 3d7bc23..71418d3 100644 --- a/Search_based_Planning/Search_2D/LRTAstar.py +++ b/Search_based_Planning/Search_2D/LRTAstar.py @@ -11,46 +11,51 @@ import math 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 +from Search_based_Planning.Search_2D import queue, plotting, env -class LrtAstarN: +class LrtAStarN: def __init__(self, s_start, s_goal, N, heuristic_type): self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type 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 = N # number of expand nodes each iteration - self.visited = [] # order of visited nodes in planning - self.path = [] # path of each iteration - self.h_table = {} + self.N = N # number of expand nodes each iteration + self.visited = [] # order of visited nodes in planning + self.path = [] # path of each iteration + self.h_table = {} # h_value table + + def init(self): + """ + initialize the h_value of all nodes in the environment. + it is a global table. + """ for i in range(self.Env.x_range): for j in range(self.Env.y_range): - self.h_table[(i, j)] = self.h((i, j)) # initialize h_value + self.h_table[(i, j)] = self.h((i, j)) def searching(self): - s_start = self.s_start # initialize start node + self.init() + s_start = self.s_start # initialize start node while True: - OPEN, CLOSED = self.Astar(s_start, self.N) # OPEN, CLOSED sets in each iteration + OPEN, CLOSED = self.AStar(s_start, self.N) # OPEN, CLOSED sets in each iteration - if OPEN == "FOUND": # reach the goal node + if OPEN == "FOUND": # reach the goal node self.path.append(CLOSED) break - h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes + h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes for x in h_value: self.h_table[x] = h_value[x] - s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set + s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set self.path.append(path_k) def extract_path_in_CLOSE(self, s_start, h_value): @@ -59,23 +64,25 @@ class LrtAstarN: while True: h_list = {} + for s_n in self.get_neighbor(s): if s_n in h_value: h_list[s_n] = h_value[s_n] else: h_list[s_n] = self.h_table[s_n] - 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 - if s_key not in h_value: # reach the expected node in OPEN set + 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 + + 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 + h_value[s] = float("inf") # initialize h_value of CLOSED nodes while True: h_value_rec = copy.deepcopy(h_value) @@ -86,25 +93,25 @@ class LrtAstarN: h_list.append(self.cost(s, s_n) + self.h_table[s_n]) else: h_list.append(self.cost(s, s_n) + h_value[s_n]) - h_value[s] = min(h_list) # update h_value of current node + h_value[s] = min(h_list) # update h_value of current node - if h_value == h_value_rec: # h_value table converged + if h_value == h_value_rec: # h_value table converged return h_value - def Astar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set + def AStar(self, x_start, N): + OPEN = queue.QueuePrior() # OPEN set OPEN.put(x_start, self.h(x_start)) - CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come - PARENT = {x_start: x_start} # relations - count = 0 # counter + CLOSED = [] # CLOSED set + g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come + PARENT = {x_start: x_start} # relations + count = 0 # counter while not OPEN.empty(): count += 1 s = OPEN.get() CLOSED.append(s) - if s == self.s_goal: # reach the goal node + if s == self.s_goal: # reach the goal node self.visited.append(CLOSED) return "FOUND", self.extract_path(x_start, PARENT) @@ -113,15 +120,15 @@ class LrtAstarN: new_cost = g_table[s] + self.cost(s, s_n) if s_n not in g_table: g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating Cost + if new_cost < g_table[s_n]: # conditions for updating Cost g_table[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) - if count == N: # expand needed CLOSED nodes + if count == N: # expand needed CLOSED nodes break - self.visited.append(CLOSED) # visited nodes in each iteration + self.visited.append(CLOSED) # visited nodes in each iteration return OPEN, CLOSED @@ -211,7 +218,7 @@ def main(): s_start = (10, 5) s_goal = (45, 25) - lrta = LrtAstarN(s_start, s_goal, 250, "euclidean") + lrta = LrtAStarN(s_start, s_goal, 250, "euclidean") plot = plotting.Plotting(s_start, s_goal) lrta.searching() diff --git a/Search_based_Planning/Search_2D/RTAAstar.py b/Search_based_Planning/Search_2D/RTAAStar.py similarity index 68% rename from Search_based_Planning/Search_2D/RTAAstar.py rename to Search_based_Planning/Search_2D/RTAAStar.py index 8c59d91..42eb90d 100644 --- a/Search_based_Planning/Search_2D/RTAAstar.py +++ b/Search_based_Planning/Search_2D/RTAAStar.py @@ -11,38 +11,43 @@ import math 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 +from Search_based_Planning.Search_2D import queue, plotting, env -class RtaAstar: +class RTAAStar: def __init__(self, s_start, s_goal, N, heuristic_type): self.s_start, self.s_goal = s_start, s_goal self.heuristic_type = heuristic_type 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 = N # number of expand nodes each iteration - self.visited = [] # order of visited nodes in planning - self.path = [] # path of each iteration - self.h_table = {} + self.N = N # number of expand nodes each iteration + self.visited = [] # order of visited nodes in planning + self.path = [] # path of each iteration + self.h_table = {} # h_value table + + def init(self): + """ + initialize the h_value of all nodes in the environment. + it is a global table. + """ for i in range(self.Env.x_range): for j in range(self.Env.y_range): - self.h_table[(i, j)] = self.h((i, j)) # initialize h_value + self.h_table[(i, j)] = self.h((i, j)) def searching(self): - s_start = self.s_start # initialize start node + self.init() + s_start = self.s_start # initialize start node while True: OPEN, CLOSED, g_table, PARENT = \ self.Astar(s_start, self.N) - if OPEN == "FOUND": # reach the goal node + if OPEN == "FOUND": # reach the goal node self.path.append(CLOSED) break @@ -70,7 +75,7 @@ class RtaAstar: h_value = {} for s in CLOSED: - h_value[s] = float("inf") # initialize h_value of CLOSED nodes + h_value[s] = float("inf") # initialize h_value of CLOSED nodes while True: h_value_rec = copy.deepcopy(h_value) @@ -81,25 +86,25 @@ class RtaAstar: h_list.append(self.cost(s, s_n) + self.h_table[s_n]) else: h_list.append(self.cost(s, s_n) + h_value[s_n]) - h_value[s] = min(h_list) # update h_value of current node + h_value[s] = min(h_list) # update h_value of current node - if h_value == h_value_rec: # h_value table converged + if h_value == h_value_rec: # h_value table converged return h_value def Astar(self, x_start, N): - OPEN = queue.QueuePrior() # OPEN set + OPEN = queue.QueuePrior() # OPEN set OPEN.put(x_start, self.h_table[x_start]) - CLOSED = [] # CLOSED set - g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come - PARENT = {x_start: x_start} # relations - count = 0 # counter + CLOSED = [] # CLOSED set + g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come + PARENT = {x_start: x_start} # relations + count = 0 # counter while not OPEN.empty(): count += 1 s = OPEN.get() CLOSED.append(s) - if s == self.s_goal: # reach the goal node + if s == self.s_goal: # reach the goal node self.visited.append(CLOSED) return "FOUND", self.extract_path(x_start, PARENT), [], [] @@ -108,15 +113,15 @@ class RtaAstar: new_cost = g_table[s] + self.cost(s, s_n) if s_n not in g_table: g_table[s_n] = float("inf") - if new_cost < g_table[s_n]: # conditions for updating Cost + if new_cost < g_table[s_n]: # conditions for updating Cost g_table[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) - if count == N: # expand needed CLOSED nodes + if count == N: # expand needed CLOSED nodes break - self.visited.append(CLOSED) # visited nodes in each iteration + self.visited.append(CLOSED) # visited nodes in each iteration return OPEN, CLOSED, g_table, PARENT @@ -145,11 +150,11 @@ class RtaAstar: for s_n in self.get_neighbor(s): if s_n in h_value: h_list[s_n] = h_value[s_n] - s_key = max(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 + s_key = max(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 - if s_key == s_end: # reach the expected node in OPEN set + if s_key == s_end: # reach the expected node in OPEN set return s_start, list(reversed(path)) def extract_path(self, x_start, parent): @@ -176,8 +181,8 @@ class RtaAstar: :return: heuristic function value """ - heuristic_type = self.heuristic_type # heuristic type - goal = self.s_goal # goal node + heuristic_type = self.heuristic_type # heuristic type + goal = self.s_goal # goal node if heuristic_type == "manhattan": return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) @@ -220,7 +225,7 @@ def main(): s_start = (10, 5) s_goal = (45, 25) - rtaa = RtaAstar(s_start, s_goal, 240, "euclidean") + rtaa = RTAAStar(s_start, s_goal, 240, "euclidean") plot = plotting.Plotting(s_start, s_goal) rtaa.searching() 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 c5190fa..6c5f684 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/bfs.py b/Search_based_Planning/Search_2D/bfs.py index 315e6d8..7acd282 100644 --- a/Search_based_Planning/Search_2D/bfs.py +++ b/Search_based_Planning/Search_2D/bfs.py @@ -21,12 +21,12 @@ class BFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.s_start, self.s_goal) - 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.OPEN = deque() # OPEN set: visited nodes - self.PARENT = dict() # recorded parent - self.CLOSED = [] # CLOSED set: explored nodes + self.OPEN = deque() # OPEN set: visited nodes + self.PARENT = dict() # recorded parent + self.CLOSED = [] # CLOSED set: explored nodes def searching(self): """ @@ -47,7 +47,7 @@ class BFS: for s_n in self.get_neighbor(s): if self.is_collision(s, s_n): continue - if s_n not in self.PARENT: # node not explored + if s_n not in self.PARENT: # node not explored self.OPEN.append(s_n) self.PARENT[s_n] = s diff --git a/Search_based_Planning/Search_2D/dfs.py b/Search_based_Planning/Search_2D/dfs.py index f835b9e..c6e43dd 100644 --- a/Search_based_Planning/Search_2D/dfs.py +++ b/Search_based_Planning/Search_2D/dfs.py @@ -21,12 +21,12 @@ class DFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.s_start, self.s_goal) - 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.OPEN = deque() # OPEN set: visited nodes - self.PARENT = dict() # recorded parent - self.CLOSED = [] # CLOSED set / visited order + self.OPEN = deque() # OPEN set: visited nodes + self.PARENT = dict() # recorded parent + self.CLOSED = [] # CLOSED set / visited order def searching(self): """ @@ -47,7 +47,7 @@ class DFS: for s_n in self.get_neighbor(s): if self.is_collision(s, s_n): continue - if s_n not in self.PARENT: # node not explored + if s_n not in self.PARENT: # node not explored self.OPEN.append(s_n) self.PARENT[s_n] = s