diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 48f135c..a7fa2c4 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -20,20 +20,15 @@
-
-
-
-
-
+
+
-
-
-
-
-
-
+
+
+
+
@@ -64,6 +59,7 @@
+
@@ -83,7 +79,7 @@
-
+
@@ -126,7 +122,7 @@
-
+
@@ -138,7 +134,28 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -168,27 +185,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -212,19 +208,19 @@
-
diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py
index 654afef..68e11bf 100644
--- a/Search-based Planning/Search_2D/ARAstar.py
+++ b/Search-based Planning/Search_2D/ARAstar.py
@@ -10,7 +10,6 @@ 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
@@ -20,37 +19,33 @@ class AraStar:
self.s_start, self.s_goal = s_start, 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.e = e # initial weight
- self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
+ 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.s_start: 0, self.s_goal: float("inf")} # cost to come
- self.OPEN = queue.QueuePrior() # priority queue / U
- self.CLOSED = set() # closed set
- self.INCONS = [] # incons set
- self.PARENT = {self.s_start: self.s_start} # relations
- self.path = [] # planning path
- self.visited = [] # order of visited nodes
+ self.OPEN = {self.s_start: self.fvalue(self.s_start)} # priority queue / OPEN set
+ self.CLOSED = set() # CLOSED set
+ self.INCONS = {} # INCONS set
+ self.PARENT = {self.s_start: self.s_start} # relations
+ self.path = [] # planning path
+ self.visited = [] # order of visited nodes
def searching(self):
- self.OPEN.put(self.s_start, self.fvalue(self.s_start))
self.ImprovePath()
self.path.append(self.extract_path())
- 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.s_start, self.fvalue(self.s_start))
+ while self.update_e() > 1: # continue condition
+ self.e -= 0.5 # increase weight
+ self.OPEN.update(self.INCONS)
+ for s in self.OPEN:
+ self.OPEN[s] = self.fvalue(s)
- for x in OPEN_mid:
- self.OPEN.put(x, self.fvalue(x)) # update priority
-
- self.INCONS = []
+ self.INCONS = {}
self.CLOSED = set()
- self.ImprovePath() # improve path
+ self.ImprovePath() # improve path
self.path.append(self.extract_path())
return self.path, self.visited
@@ -62,12 +57,11 @@ class AraStar:
visited_each = []
- while (self.fvalue(self.s_goal) >
- min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
- s = self.OPEN.get()
-
- if s not in self.CLOSED:
- self.CLOSED.add(s)
+ while True:
+ s, f_small = self.get_smallest_f()
+ if self.fvalue(self.s_goal) <= f_small:
+ break
+ self.CLOSED.add(s)
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
@@ -77,12 +71,26 @@ class AraStar:
visited_each.append(s_n)
if s_n not in self.CLOSED:
- self.OPEN.put(s_n, self.fvalue(s_n))
+ self.OPEN[s_n] = self.fvalue(s_n)
else:
- self.INCONS.append(s_n)
+ self.INCONS[s_n] = 0
self.visited.append(visited_each)
+ def get_smallest_f(self):
+ """
+ :return: node with smallest f_value in OPEN set.
+ """
+ s_list = {}
+
+ for s in self.OPEN:
+ s_list[s] = self.fvalue(s)
+ s_small = min(s_list, key=s_list.get)
+
+ self.OPEN.pop(s_small)
+
+ return s_small, s_list[s_small]
+
def get_neighbor(self, s):
"""
find neighbors of state s that not in obstacles.
@@ -100,21 +108,17 @@ class AraStar:
return s_list
def update_e(self):
- c_OPEN, c_INCONS = float("inf"), float("inf")
+ v = float("inf")
if self.OPEN:
- c_OPEN = min(self.g[x] +
- self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
+ v = min(self.g[s] + self.h(s) for s in self.OPEN)
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"):
- return 1
+ v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS))
- return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS))
+ return min(self.e, self.g[self.s_goal] / v)
def fvalue(self, x):
- return self.g[x] + self.e * self.Heuristic(x)
+ return self.g[x] + self.e * self.h(x)
def extract_path(self):
"""
@@ -134,15 +138,15 @@ class AraStar:
return list(path)
- def Heuristic(self, s):
+ def h(self, s):
"""
Calculate heuristic.
:param s: current node (state)
: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])
@@ -163,16 +167,14 @@ class AraStar:
def main():
- x_start = (5, 5) # Starting node
- x_goal = (45, 25) # Goal node
+ s_start = (5, 5)
+ s_goal = (45, 25)
- arastar = AraStar(x_start, x_goal, 2.5, "manhattan")
- plot = plotting.Plotting(x_start, x_goal)
+ arastar = AraStar(s_start, s_goal, 2.5, "euclidean")
+ plot = plotting.Plotting(s_start, s_goal)
- fig_name = "Anytime Repairing A* (ARA*)"
path, visited = arastar.searching()
-
- plot.animation_ara_star(path, visited, fig_name)
+ plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)")
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py
index df67935..abce0fb 100644
--- a/Search-based Planning/Search_2D/D_star_Lite.py
+++ b/Search-based Planning/Search_2D/D_star_Lite.py
@@ -11,25 +11,24 @@ import matplotlib.pyplot as plt
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 DStar:
- def __init__(self, x_start, x_goal, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ 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.Plot = plotting.Plotting(x_start, x_goal)
+ self.Plot = plotting.Plotting(s_start, s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.x = self.Env.x_range
self.y = self.Env.y_range
- self.U = queue.QueuePrior() # priority queue / U set
+ self.U = {}
self.g, self.rhs = {}, {}
self.km = 0
@@ -38,8 +37,8 @@ class DStar:
self.rhs[(i, j)] = float("inf")
self.g[(i, j)] = float("inf")
- self.rhs[self.xG] = 0
- self.U.put(self.xG, self.Key(self.xG))
+ self.rhs[self.s_goal] = 0
+ self.U[self.s_goal] = self.CalculateKey(self.s_goal)
self.fig = plt.figure()
def run(self):
@@ -57,15 +56,15 @@ class DStar:
x, y = int(x), int(y)
print("Change position: x =", x, ",", "y =", y)
- s_curr = self.xI
- s_last = self.xI
+ s_curr = self.s_start
+ s_last = self.s_start
i = 0
path = []
- while s_curr != self.xG:
+ while s_curr != self.s_goal:
s_list = {}
for s in self.get_neighbor(s_curr):
- s_list[s] = self.g[s] + self.get_cost(s_curr, s)
+ s_list[s] = self.g[s] + self.cost(s_curr, s)
s_curr = min(s_list, key=s_list.get)
path.append(s_curr)
@@ -88,19 +87,18 @@ class DStar:
self.plot_path(path)
self.fig.canvas.draw_idle()
- @staticmethod
- def plot_path(path):
- px = [x[0] for x in path]
- py = [x[1] for x in path]
- plt.plot(px, py, marker='o')
-
def ComputePath(self):
- while self.U.top_key() < self.Key(self.xI) or \
- self.rhs[self.xI] != self.g[self.xI]:
- k_old = self.U.top_key()
- s = self.U.get()
- if k_old < self.Key(s):
- self.U.put(s, self.Key(s))
+ while True:
+ s, v = self.TopKey()
+ if v >= self.CalculateKey(self.s_start) and \
+ self.rhs[self.s_start] == self.g[self.s_start]:
+ break
+
+ k_old = v
+ self.U.pop(s)
+
+ if k_old < self.CalculateKey(s):
+ self.U[s] = self.CalculateKey(s)
elif self.g[s] > self.rhs[s]:
self.g[s] = self.rhs[s]
for x in self.get_neighbor(s):
@@ -112,29 +110,40 @@ class DStar:
self.UpdateVertex(x)
def UpdateVertex(self, s):
- if s != self.xG:
+ if s != self.s_goal:
self.rhs[s] = float("inf")
for x in self.get_neighbor(s):
- self.rhs[s] = min(self.rhs[s], self.g[x] + self.get_cost(s, x))
- self.U.remove(s)
- if self.g[s] != self.rhs[s]:
- self.U.put(s, self.Key(s))
+ self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x))
+ if s in self.U:
+ self.U.pop(s)
- def Key(self, s):
- return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
+ if self.g[s] != self.rhs[s]:
+ self.U[s] = self.CalculateKey(s)
+
+ def CalculateKey(self, s):
+ return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s) + self.km,
min(self.g[s], self.rhs[s])]
+ def TopKey(self):
+ """
+ :return: return the min key and its value.
+ """
+
+ s = min(self.U, key=self.U.get)
+ return s, self.U[s]
+
def h(self, s_start, s_goal):
- heuristic_type = self.heuristic_type # heuristic type
+ heuristic_type = self.heuristic_type # heuristic type
if heuristic_type == "manhattan":
return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
else:
return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
- def get_cost(self, s_start, s_end):
+ def cost(self, s_start, s_end):
if s_start in self.obs or s_end in self.obs:
return float("inf")
+
return 1
def get_neighbor(self, s):
@@ -148,7 +157,7 @@ class DStar:
def extract_path(self):
path = []
- s = self.xI
+ s = self.s_start
count = 0
while True:
count += 1
@@ -156,31 +165,22 @@ class DStar:
for x in self.get_neighbor(s):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
- if s == self.xG or count > 100:
+ if s == self.s_goal or count > 100:
return list(reversed(path))
path.append(s)
- def print_g(self):
- print("he")
- for k in range(self.Env.y_range):
- j = self.Env.y_range - k - 1
- string = ""
- for i in range(self.Env.x_range):
- if self.g[(i, j)] == float("inf"):
- string += ("00" + ', ')
- else:
- if self.g[(i, j)] // 10 == 0:
- string += ("0" + str(self.g[(i, j)]) + ', ')
- else:
- string += (str(self.g[(i, j)]) + ', ')
- print(string)
+ @staticmethod
+ def plot_path(path):
+ px = [x[0] for x in path]
+ py = [x[1] for x in path]
+ plt.plot(px, py, marker='o')
def main():
- x_start = (5, 5)
- x_goal = (45, 25)
+ s_start = (5, 5)
+ s_goal = (45, 25)
- dstar = DStar(x_start, x_goal, "euclidean")
+ dstar = DStar(s_start, s_goal, "euclidean")
dstar.run()
diff --git a/Search-based Planning/Search_2D/IDAstar.py b/Search-based Planning/Search_2D/IDAstar.py
deleted file mode 100644
index 6fd06e2..0000000
--- a/Search-based Planning/Search_2D/IDAstar.py
+++ /dev/null
@@ -1,96 +0,0 @@
-"""
-IDA_Star 2D (Iteratively Deepening A*)
-@author: huiming zhou
-"""
-
-import os
-import sys
-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
-
-
-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
-
- self.visited = []
-
- 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, self.visited
- if t == float("inf"):
- return [], self.visited
- bound = t
-
- def searching(self, path, g, bound):
- s = path[-1]
- self.visited.append(s)
- 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)
- 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()
-
- if path:
- plot.plot_grid("Iteratively Deepening A*")
- plot.plot_path(visited, 'gray', True)
- plot.plot_path(path)
- plt.show()
- else:
- print("Path not found!")
-
-
-if __name__ == '__main__':
- main()
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py
index 8bd2af3..8553347 100644
--- a/Search-based Planning/Search_2D/LPAstar.py
+++ b/Search-based Planning/Search_2D/LPAstar.py
@@ -11,25 +11,24 @@ import matplotlib.pyplot as plt
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 LpaStar:
- def __init__(self, x_start, x_goal, heuristic_type):
- self.s_start, self.s_goal = x_start, x_goal
+ 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()
- self.Plot = plotting.Plotting(x_start, x_goal)
+ self.Plot = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions
self.obs = self.Env.obs
self.x = self.Env.x_range
self.y = self.Env.y_range
- self.U = queue.QueuePrior()
+ self.U = {}
self.g, self.rhs = {}, {}
for i in range(self.Env.x_range):
@@ -38,7 +37,8 @@ class LpaStar:
self.g[(i, j)] = float("inf")
self.rhs[self.s_start] = 0
- self.U.put(self.s_start, self.Key(self.s_start))
+ self.U[self.s_start] = self.CalculateKey(self.s_start)
+
self.fig = plt.figure()
def run(self):
@@ -73,13 +73,16 @@ class LpaStar:
self.fig.canvas.draw_idle()
def ComputePath(self):
- while self.U.top_key() < self.Key(self.s_goal) or \
- self.rhs[self.s_goal] != self.g[self.s_goal]:
- s = self.U.get()
+ while True:
+ s, v = self.TopKey()
+ if v >= self.CalculateKey(self.s_goal) and \
+ self.rhs[self.s_goal] == self.g[self.s_goal]:
+ break
+ self.U.pop(s)
- if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles
+ if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles
self.g[s] = self.rhs[s]
- else: # under-consistent: added obstacles
+ else: # under-consistent: added obstacles
self.g[s] = float("inf")
self.UpdateVertex(s)
for s_n in self.get_neighbor(s):
@@ -87,11 +90,25 @@ class LpaStar:
def UpdateVertex(self, s):
if s != self.s_start:
- self.rhs[s] = min([self.g[s_n] + self.cost(s_n, s)
- for s_n in self.get_neighbor(s)])
- self.U.remove(s)
+ self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s)
+ for s_n in self.get_neighbor(s))
+ if s in self.U:
+ self.U.pop(s)
+
if self.g[s] != self.rhs[s]:
- self.U.put(s, self.Key(s))
+ self.U[s] = self.CalculateKey(s)
+
+ def TopKey(self):
+ """
+ :return: return the min key and its value.
+ """
+
+ s = min(self.U, key=self.U.get)
+ return s, self.U[s]
+
+ def CalculateKey(self, s):
+ return [min(self.g[s], self.rhs[s]) + self.h(s),
+ min(self.g[s], self.rhs[s])]
def get_neighbor(self, s):
"""
@@ -109,11 +126,13 @@ class LpaStar:
return s_list
- def Key(self, s):
- return [min(self.g[s], self.rhs[s]) + self.h(s),
- min(self.g[s], self.rhs[s])]
-
def h(self, s):
+ """
+ Calculate heuristic.
+ :param s: current node (state)
+ :return: heuristic function value
+ """
+
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
@@ -123,11 +142,25 @@ class LpaStar:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
def cost(self, s_start, s_end):
+ """
+ calculate edge cost: (s_start, s_end)
+ :param s_start: start node
+ :param s_end: end node
+ :return: cost
+ """
+
+ # if one of the vertex in obstacles: return infinity.
if s_start in self.obs or s_end in self.obs:
return float("inf")
+
return 1
def extract_path(self):
+ """
+ Extract the path based on the PARENT set.
+ :return: The planning path
+ """
+
path = []
s = self.s_goal
@@ -139,6 +172,7 @@ class LpaStar:
if s == self.s_start:
break
path.append(s)
+
return list(reversed(path))
@staticmethod
@@ -147,21 +181,6 @@ class LpaStar:
py = [x[1] for x in path]
plt.plot(px, py, marker='o')
- def print_g(self):
- print("he")
- for k in range(self.Env.y_range):
- j = self.Env.y_range - k - 1
- string = ""
- for i in range(self.Env.x_range):
- if self.g[(i, j)] == float("inf"):
- string += ("00" + ', ')
- else:
- if self.g[(i, j)] // 10 == 0:
- string += ("0" + str(self.g[(i, j)]) + ', ')
- else:
- string += (str(self.g[(i, j)]) + ', ')
- print(string)
-
def main():
x_start = (5, 5)
diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py
index 9e3d6d6..34bcd1e 100644
--- a/Search-based Planning/Search_2D/LRTAstar.py
+++ b/Search-based Planning/Search_2D/LRTAstar.py
@@ -6,6 +6,7 @@ LRTA_star 2D (Learning Real-time A*)
import os
import sys
import copy
+import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
@@ -16,8 +17,8 @@ from Search_2D import env
class LrtAstarN:
- def __init__(self, x_start, x_goal, N, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ 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()
@@ -35,7 +36,7 @@ class LrtAstarN:
self.h_table[(i, j)] = self.h((i, j)) # initialize h_value
def searching(self):
- s_start = self.xI # initialize start node
+ s_start = self.s_start # initialize start node
while True:
OPEN, CLOSED = self.Astar(s_start, self.N) # U, CLOSED sets in each iteration
@@ -58,13 +59,11 @@ class LrtAstarN:
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_table[s_next]
+ 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
@@ -82,13 +81,11 @@ class LrtAstarN:
h_value_rec = copy.deepcopy(h_value)
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_table[s_next])
- else:
- h_list.append(self.get_cost(s, s_next) + h_value[s_next])
+ for s_n in self.get_neighbor(s):
+ if s_n not in CLOSED:
+ 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
if h_value == h_value_rec: # h_value table converged
@@ -97,40 +94,53 @@ class LrtAstarN:
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # U set
OPEN.put(x_start, self.h(x_start))
- CLOSED = set() # CLOSED set
- g_table = {x_start: 0, self.xG: float("inf")} # cost to come
+ CLOSED = [] # CLOSED set
+ g_table = {x_start: 0, self.s_goal: 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)
+ CLOSED.append(s)
- if s == self.xG: # reach the goal node
- self.visited.append(visited)
+ if s == self.s_goal: # reach the goal node
+ self.visited.append(CLOSED)
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))])
- if s_next not in self.obs and s_next not in CLOSED:
- 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
- g_table[s_next] = new_cost
- PARENT[s_next] = s
- OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
+ 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
+ if count == N: # expand needed CLOSED nodes
break
- self.visited.append(visited) # visited nodes in each iteration
+ self.visited.append(CLOSED) # visited nodes in each iteration
return OPEN, CLOSED
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ 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:
+ s_list.add(s_next)
+
+ return s_list
+
def extract_path(self, x_start, parent):
"""
Extract the path based on the relationship of nodes.
@@ -138,8 +148,8 @@ class LrtAstarN:
:return: The planning path
"""
- path_back = [self.xG]
- x_current = self.xG
+ path_back = [self.s_goal]
+ x_current = self.s_goal
while True:
x_current = parent[x_current]
@@ -151,23 +161,25 @@ class LrtAstarN:
return list(reversed(path_back))
def h(self, s):
- heuristic_type = self.heuristic_type
- goal = self.xG
+ """
+ Calculate heuristic.
+ :param s: current node (state)
+ :return: heuristic function value
+ """
+
+ 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])
- elif heuristic_type == "euclidean":
- return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
else:
- print("Please choose right heuristic type!")
+ return math.hypot(goal[0] - s[0], goal[1] - s[1])
- @staticmethod
- def get_cost(x, u):
+ def cost(self, s_start, s_end):
"""
Calculate cost for this motion
-
- :param x: current node
- :param u: input
+ :param s_start: starting node
+ :param s_end: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
@@ -176,15 +188,15 @@ class LrtAstarN:
def main():
- x_start = (10, 5)
- x_goal = (45, 25)
+ s_start = (10, 5)
+ s_goal = (45, 25)
- lrta = LrtAstarN(x_start, x_goal, 200, "euclidean")
- plot = plotting.Plotting(x_start, x_goal)
- fig_name = "Learning Real-time A* (LRTA*)"
+ lrta = LrtAstarN(s_start, s_goal, 250, "euclidean")
+ plot = plotting.Plotting(s_start, s_goal)
lrta.searching()
- plot.animation_lrta(lrta.path, lrta.visited, fig_name)
+ plot.animation_lrta(lrta.path, lrta.visited,
+ "Learning Real-time A* (LRTA*)")
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py
index 233bfbd..97b2267 100644
--- a/Search-based Planning/Search_2D/RTAAstar.py
+++ b/Search-based Planning/Search_2D/RTAAstar.py
@@ -6,6 +6,7 @@ RTAAstar 2D (Real-time Adaptive A*)
import os
import sys
import copy
+import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
@@ -16,8 +17,8 @@ from Search_2D import env
class RtaAstar:
- def __init__(self, x_start, x_goal, N, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ 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()
@@ -35,7 +36,7 @@ class RtaAstar:
self.h_table[(i, j)] = self.h((i, j)) # initialize h_value
def searching(self):
- s_start = self.xI # initialize start node
+ s_start = self.s_start # initialize start node
while True:
OPEN, CLOSED, g_table, PARENT = \
@@ -59,29 +60,12 @@ class RtaAstar:
for (_, x) in OPEN.enumerate():
v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x]
s_open = min(v_open, key=v_open.get)
- f_min = min(v_open.values())
+ f_min = v_open[s_open]
for x in CLOSED:
h_value[x] = f_min - g_table[x]
return s_open, h_value
- def extract_path_in_CLOSE(self, s_end, s_start, h_value):
- path = [s_start]
- s = s_start
-
- 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 and s_next in h_value:
- h_list[s_next] = h_value[s_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 U set
- return s_start, list(reversed(path))
-
def iteration(self, CLOSED):
h_value = {}
@@ -92,13 +76,11 @@ class RtaAstar:
h_value_rec = copy.deepcopy(h_value)
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_table[s_next])
- else:
- h_list.append(self.get_cost(s, s_next) + h_value[s_next])
+ for s_n in self.get_neighbor(s):
+ if s_n not in CLOSED:
+ 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
if h_value == h_value_rec: # h_value table converged
@@ -107,77 +89,106 @@ class RtaAstar:
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # U set
OPEN.put(x_start, self.h_table[x_start])
- CLOSED = set() # CLOSED set
- g_table = {x_start: 0, self.xG: float("inf")} # cost to come
+ CLOSED = [] # CLOSED set
+ g_table = {x_start: 0, self.s_goal: 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)
+ CLOSED.append(s)
- if s == self.xG: # reach the goal node
- self.visited.append(visited)
+ if s == self.s_goal: # reach the goal node
+ self.visited.append(CLOSED)
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))])
- if s_next not in self.obs and s_next not in CLOSED:
- 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
- g_table[s_next] = new_cost
- PARENT[s_next] = s
- OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
+ 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(visited) # visited nodes in each iteration
+ self.visited.append(CLOSED) # visited nodes in each iteration
return OPEN, CLOSED, g_table, PARENT
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ 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:
+ s_list.add(s_next)
+
+ return s_list
+
+ def extract_path_in_CLOSE(self, s_end, s_start, h_value):
+ path = [s_start]
+ s = s_start
+
+ 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]
+ 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 U set
+ return s_start, list(reversed(path))
+
def extract_path(self, x_start, parent):
"""
Extract the path based on the relationship of nodes.
-
:return: The planning path
"""
- path_back = [self.xG]
- x_current = self.xG
+ path = [self.s_goal]
+ s = self.s_goal
while True:
- x_current = parent[x_current]
- path_back.append(x_current)
-
- if x_current == x_start:
+ s = parent[s]
+ path.append(s)
+ if s == x_start:
break
- return list(reversed(path_back))
+ return list(reversed(path))
def h(self, s):
- heuristic_type = self.heuristic_type
- goal = self.xG
+ """
+ Calculate heuristic.
+ :param s: current node (state)
+ :return: heuristic function value
+ """
+
+ 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])
- elif heuristic_type == "euclidean":
- return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
else:
- print("Please choose right heuristic type!")
+ return math.hypot(goal[0] - s[0], goal[1] - s[1])
- @staticmethod
- def get_cost(x, u):
+ def cost(self, s_start, s_end):
"""
Calculate cost for this motion
-
- :param x: current node
- :param u: input
+ :param s_start: starting node
+ :param s_end: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
@@ -186,15 +197,15 @@ class RtaAstar:
def main():
- x_start = (10, 5)
- x_goal = (45, 25)
+ s_start = (10, 5)
+ s_goal = (45, 25)
- rtaa = RtaAstar(x_start, x_goal, 200, "euclidean")
- plot = plotting.Plotting(x_start, x_goal)
- fig_name = "Real-time Adaptive A* (RTAA*)"
+ rtaa = RtaAstar(s_start, s_goal, 220, "euclidean")
+ plot = plotting.Plotting(s_start, s_goal)
rtaa.searching()
- plot.animation_lrta(rtaa.path, rtaa.visited, fig_name)
+ plot.animation_lrta(rtaa.path, rtaa.visited,
+ "Real-time Adaptive A* (RTAA*)")
if __name__ == '__main__':
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 ad642f8..a459242 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/queue.py b/Search-based Planning/Search_2D/queue.py
index 2a9fb3b..8f481ae 100644
--- a/Search-based Planning/Search_2D/queue.py
+++ b/Search-based Planning/Search_2D/queue.py
@@ -55,24 +55,8 @@ class QueuePrior:
def put(self, item, priority):
heapq.heappush(self.queue, (priority, item)) # reorder x using priority
- def update(self, item, priority):
- count = 0
- for (p, x) in self.queue:
- if x == item:
- self.queue[count] = (priority, item)
- break
- count += 1
-
def get(self):
return heapq.heappop(self.queue)[1] # pop out the smallest item
def enumerate(self):
return self.queue
-
- def remove(self, item):
- for x in self.queue:
- if item == x[1]:
- self.queue.remove(x)
-
- def top_key(self):
- return self.queue[0][0]
diff --git a/Search-based Planning/Search_2D/test.py b/Search-based Planning/Search_2D/test.py
deleted file mode 100644
index f5d910d..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
-from Search_2D import plotting
-from Search_2D import env
-
-
-U = queue.QueuePrior()
-U.put((1, 2), [2, 3])
-U.put((2, 3), [1, 5])
-print(U.get())
diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index 0664edb..e00de51 100644
--- a/Search-based Planning/Search_3D/Astar3D.py
+++ b/Search-based Planning/Search_3D/Astar3D.py
@@ -116,7 +116,7 @@ class Weighted_A_star(object):
self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h
self.CLOSED = set()
- # self.h = Heuristic(self.Space, self.goal)
+ # self.h = h(self.Space, self.goal)
if __name__ == '__main__':