diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index d3c89d3..a9458b3 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,8 +20,13 @@ + + + + + - + + + + - - - - + + + + + + + + - - + + - - + - + + @@ -194,12 +233,13 @@ - + + - 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 04c0af7..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..f56a2d2 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: @@ -123,7 +124,6 @@ def main(): fig_name = "A* Algorithm" path, visited = astar.searching() - plot.animation(path, visited, fig_name) # animation generate 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/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())