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}
