def __init__(self, s_start, s_goal, heuristic_type): self.s_start, self.s_goal = s_start, s_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.s_start: 0, self.s_goal: float("inf") } # cost to come: from s_start self.g_back = { self.s_goal: 0, self.s_start: float("inf") } # cost to come: form s_goal self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching self.OPEN_fore.put( self.s_start, self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal)) self.OPEN_back = queue.QueuePrior() # OPEN set for backward searching self.OPEN_back.put( self.s_goal, self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start)) self.CLOSED_fore = [] # CLOSED set for foreward self.CLOSED_back = [] # CLOSED set for backward self.PARENT_fore = {self.s_start: self.s_start} self.PARENT_back = {self.s_goal: self.s_goal}
def Astar(self, x_start, N): 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 while not OPEN.empty(): count += 1 s = OPEN.get() CLOSED.append(s) if s == self.s_goal: # reach the goal node self.visited.append(CLOSED) return "FOUND", self.extract_path(x_start, PARENT), [], [] for s_n in self.get_neighbor(s): if s_n not in CLOSED: 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 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 break self.visited.append(CLOSED) # visited nodes in each iteration return OPEN, CLOSED, g_table, PARENT
def repeated_searching(self, s_start, s_goal, e): """ run a* with weight e. :param s_start: starting state :param s_goal: goal state :param e: weight of a* :return: path and visited order. """ g = {s_start: 0, s_goal: float("inf")} OPEN = queue.QueuePrior() OPEN.put(s_start, g[s_start] + e * self.Heuristic(s_start)) CLOSED = [] PARENT = {s_start: s_start} while OPEN: s = OPEN.get() CLOSED.append(s) if s == s_goal: break for s_n in self.get_neighbor(s): if s_n not in CLOSED: new_cost = g[s] + self.cost(s, s_n) if s_n not in g: g[s_n] = float("inf") if new_cost < g[s_n]: # conditions for updating cost g[s_n] = new_cost PARENT[s_n] = s OPEN.put(s_n, g[s_n] + e * self.Heuristic(s_n)) return self.extract_path(PARENT), CLOSED
def __init__(self, s_start, s_goal): self.s_start, self.s_goal = s_start, s_goal 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.OPEN = queue.QueuePrior() # OPEN set self.OPEN.put(self.s_start, self.Heuristic(self.s_start)) self.CLOSED = [] # CLOSED set / visited order self.PARENT = {self.s_start: self.s_start}
def __init__(self, start, goal, heuristic_type): self.s_start, self.s_goal = start, 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 = {self.s_start: 0, self.s_goal: float("inf")} # cost to come self.OPEN = queue.QueuePrior() # priority queue / OPEN set self.OPEN.put(self.s_start, self.fvalue(self.s_start)) self.CLOSED = [] # CLOSED set / VISITED order self.PARENT = {self.s_start: self.s_start}
def __init__(self, s_start, s_goal): self.s_start, self.s_goal = s_start, s_goal 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.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come self.OPEN = queue.QueuePrior() # priority queue / OPEN set self.OPEN.put(self.s_start, 0) self.CLOSED = [] # closed set & visited self.PARENT = {self.s_start: self.s_start}