diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml
index c444878..5965bde 100644
--- a/Search-based Planning/.idea/Search-based Planning.iml
+++ b/Search-based Planning/.idea/Search-based Planning.iml
@@ -2,7 +2,7 @@
-
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml
index a2e120d..0e7ac62 100644
--- a/Search-based Planning/.idea/misc.xml
+++ b/Search-based Planning/.idea/misc.xml
@@ -1,4 +1,4 @@
-
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index e82275b..66da253 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -20,11 +20,15 @@
+
+
+
-
+
+
+
-
@@ -52,7 +56,7 @@
-
+
@@ -61,19 +65,28 @@
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -116,48 +129,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -179,6 +150,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -201,20 +193,20 @@
-
@@ -236,40 +228,39 @@
+
-
+
-
+
-
+
-
-
+
+
-
-
+
+
-
-
+
+
-
+
diff --git a/Search-based Planning/Search_2D/D_star.py b/Search-based Planning/Search_2D/D_star.py
new file mode 100644
index 0000000..c8ddbcd
--- /dev/null
+++ b/Search-based Planning/Search_2D/D_star.py
@@ -0,0 +1,186 @@
+"""
+D_star 2D
+@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 Dstar:
+ def __init__(self, x_start, x_goal):
+ self.xI, self.xG = x_start, x_goal
+
+ self.Env = env.Env()
+ self.Plot = plotting.Plotting(self.xI, self.xG)
+
+ self.u_set = self.Env.motions
+ self.obs = self.Env.obs
+ self.x = self.Env.x_range
+ self.y = self.Env.y_range
+
+ self.fig = plt.figure()
+ self.OPEN = set()
+ self.t = {}
+ self.PARENT = {}
+ self.h = {self.xG: 0}
+ self.k = {}
+ self.path = []
+
+ for i in range(self.Env.x_range):
+ for j in range(self.Env.y_range):
+ self.t[(i, j)] = 'NEW'
+ self.k[(i, j)] = 0
+ self.PARENT[(i, j)] = None
+
+ def run(self, s_start, s_end):
+ self.insert(s_end, 0)
+ while True:
+ self.process_state()
+ if self.t[s_start] == 'CLOSED':
+ break
+ self.path = self.extract_path(s_start, s_end)
+ self.Plot.plot_grid("Dynamic A* (D*)")
+ self.plot_path(self.path)
+ self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+ plt.show()
+
+ def on_press(self, event):
+ x, y = event.xdata, event.ydata
+ if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
+ print("Please choose right area!")
+ else:
+ x, y = int(x), int(y)
+ print("Add obstacle at: x =", x, ",", "y =", y)
+ self.obs.add((x, y))
+ plt.plot(x, y, 'sk')
+ if (x, y) in self.path:
+ s = self.xI
+ while s != self.xG:
+ if self.PARENT[s] in self.obs:
+ self.modify(s)
+ continue
+ s = self.PARENT[s]
+ self.path = self.extract_path(self.xI, self.xG)
+ self.plot_path(self.path)
+ self.fig.canvas.draw_idle()
+
+ def extract_path(self, s_start, s_end):
+ path = []
+ s = s_start
+ while True:
+ s = self.PARENT[s]
+ if s == s_end:
+ return path
+ path.append(s)
+
+ def process_state(self):
+ s = self.min_state()
+ if s is None:
+ return -1
+ k_old = self.get_k_min()
+ self.delete(s)
+
+ if k_old < self.h[s]:
+ for s_n in self.get_neighbor(s):
+ if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s):
+ self.PARENT[s] = s_n
+ self.h[s] = self.h[s_n] + self.cost(s_n, s)
+ if k_old == self.h[s]:
+ for s_n in self.get_neighbor(s):
+ if self.t[s_n] == 'NEW' or \
+ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \
+ (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)):
+ self.PARENT[s_n] = s
+ self.insert(s_n, self.h[s] + self.cost(s, s_n))
+ else:
+ for s_n in self.get_neighbor(s):
+ if self.t[s_n] == 'NEW' or \
+ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)):
+ self.PARENT[s_n] = s
+ self.insert(s_n, self.h[s] + self.cost(s, s_n))
+ else:
+ if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n):
+ self.insert(s, self.h[s])
+ else:
+ if self.PARENT[s_n] != s and \
+ self.h[s] > self.h[s_n] + self.cost(s_n, s) and \
+ self.t[s_n] == 'CLOSED' and \
+ self.h[s_n] > k_old:
+ self.insert(s_n, self.h[s_n])
+ return self.get_k_min()
+
+ def min_state(self):
+ if not self.OPEN:
+ return None
+ return min(self.OPEN, key=lambda x: self.k[x])
+
+ def get_k_min(self):
+ if not self.OPEN:
+ return -1
+ return min([self.k[x] for x in self.OPEN])
+
+ def insert(self, s, h_new):
+ if self.t[s] == 'NEW':
+ self.k[s] = h_new
+ elif self.t[s] == 'OPEN':
+ self.k[s] = min(self.k[s], h_new)
+ elif self.t[s] == 'CLOSED':
+ self.k[s] = min(self.h[s], h_new)
+ self.h[s] = h_new
+ self.t[s] = 'OPEN'
+ self.OPEN.add(s)
+
+ def delete(self, s):
+ if self.t[s] == 'OPEN':
+ self.t[s] = 'CLOSED'
+ self.OPEN.remove(s)
+
+ def modify(self, s):
+ self.modify_cost(s)
+ while True:
+ k_min = self.process_state()
+ if k_min >= self.h[s]:
+ break
+
+ def modify_cost(self, s):
+ if self.t[s] == 'CLOSED':
+ self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[s]))
+
+ def get_neighbor(self, s):
+ nei_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:
+ nei_list.add(s_next)
+
+ return nei_list
+
+ def cost(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return float("inf")
+ return 1
+
+ @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():
+ s_start = (5, 5)
+ s_goal = (45, 25)
+ dstar = Dstar(s_start, s_goal)
+ dstar.run(s_start, s_goal)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search-based Planning/Search_2D/D_star_Lite.py
index 15a9493..df67935 100644
--- a/Search-based Planning/Search_2D/D_star_Lite.py
+++ b/Search-based Planning/Search_2D/D_star_Lite.py
@@ -16,7 +16,7 @@ from Search_2D import plotting
from Search_2D import env
-class LpaStar:
+class DStar:
def __init__(self, x_start, x_goal, heuristic_type):
self.xI, self.xG = x_start, x_goal
self.heuristic_type = heuristic_type
@@ -42,14 +42,11 @@ class LpaStar:
self.U.put(self.xG, self.Key(self.xG))
self.fig = plt.figure()
- def searching(self):
- self.Plot.plot_grid("Lifelong Planning A*")
-
+ def run(self):
+ self.Plot.plot_grid("Dynamic A* (D*)")
self.ComputePath()
- self.plot_path(self.extract_path_test())
-
- # self.fig.canvas.mpl_connect('button_press_event', self.on_press)
-
+ self.plot_path(self.extract_path())
+ self.fig.canvas.mpl_connect('button_press_event', self.on_press)
plt.show()
def on_press(self, event):
@@ -59,20 +56,37 @@ class LpaStar:
else:
x, y = int(x), int(y)
print("Change position: x =", x, ",", "y =", y)
- if (x, y) not in self.obs:
- self.obs.add((x, y))
- plt.plot(x, y, 'sk')
- self.rhs[(x, y)] = float("inf")
- self.g[(x, y)] = float("inf")
- for node in self.getSucc((x, y)):
- self.UpdateVertex(node)
- else:
- self.obs.remove((x, y))
- plt.plot(x, y, marker='s', color='white')
- self.UpdateVertex((x, y))
- self.ComputePath()
- self.plot_path(self.extract_path_test())
- self.fig.canvas.draw_idle()
+
+ s_curr = self.xI
+ s_last = self.xI
+ i = 0
+ path = []
+
+ while s_curr != self.xG:
+ s_list = {}
+ for s in self.get_neighbor(s_curr):
+ s_list[s] = self.g[s] + self.get_cost(s_curr, s)
+ s_curr = min(s_list, key=s_list.get)
+ path.append(s_curr)
+
+ if i < 1:
+ self.km += self.h(s_last, s_curr)
+ s_last = s_curr
+ if (x, y) not in self.obs:
+ self.obs.add((x, y))
+ plt.plot(x, y, 'sk')
+ self.g[(x, y)] = float("inf")
+ self.rhs[(x, y)] = float("inf")
+ else:
+ self.obs.remove((x, y))
+ plt.plot(x, y, marker='s', color='white')
+ self.UpdateVertex((x, y))
+ for s in self.get_neighbor((x, y)):
+ self.UpdateVertex(s)
+ i += 1
+ self.ComputePath()
+ self.plot_path(path)
+ self.fig.canvas.draw_idle()
@staticmethod
def plot_path(path):
@@ -81,88 +95,46 @@ class LpaStar:
plt.plot(px, py, marker='o')
def ComputePath(self):
- count = 0
while self.U.top_key() < self.Key(self.xI) or \
self.rhs[self.xI] != self.g[self.xI]:
- count += 1
- print(count)
k_old = self.U.top_key()
s = self.U.get()
if k_old < self.Key(s):
self.U.put(s, self.Key(s))
elif self.g[s] > self.rhs[s]:
self.g[s] = self.rhs[s]
- for x in self.getPred(s):
+ for x in self.get_neighbor(s):
self.UpdateVertex(x)
else:
self.g[s] = float("inf")
self.UpdateVertex(s)
- for x in self.getPred(s):
+ for x in self.get_neighbor(s):
self.UpdateVertex(x)
- def getSucc(self, s):
- nei_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 and self.g[s_next] >= self.g[s]:
- nei_list.add(s_next)
- return nei_list
-
- def getPred(self, s):
- nei_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 and self.g[s_next] <= self.g[s]:
- nei_list.add(s_next)
- return nei_list
-
def UpdateVertex(self, s):
if s != self.xG:
self.rhs[s] = float("inf")
- for x in self.getSucc(s):
+ 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))
- def extract_path_test(self):
- path = []
- s = self.xG
-
- for k in range(100):
- g_list = {}
- for x in self.get_neighbor(s):
- g_list[x] = self.g[x]
- s = min(g_list, key=g_list.get)
- if s == self.xI:
- return list(reversed(path))
- path.append(s)
- return list(reversed(path))
-
def Key(self, s):
- return [min(self.g[s], self.rhs[s]) + self.h(s) + self.km,
+ return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
min(self.g[s], self.rhs[s])]
- def h(self, s):
+ def h(self, s_start, s_goal):
heuristic_type = self.heuristic_type # heuristic type
- s_start = self.xI # goal node
if heuristic_type == "manhattan":
- return abs(s[0] - s_start[0]) + abs(s[1] - s_start[1])
+ return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])
else:
- return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
-
- @staticmethod
- def get_cost(s_start, s_end):
- """
- Calculate cost for this motion
-
- :param s_start:
- :param s_end:
- :return: cost for this motion
- :note: cost function could be more complicate!
- """
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+ def get_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):
@@ -176,14 +148,15 @@ class LpaStar:
def extract_path(self):
path = []
- s = self.xG
-
+ s = self.xI
+ count = 0
while True:
+ count += 1
g_list = {}
for x in self.get_neighbor(s):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
- if s == self.xI:
+ if s == self.xG or count > 100:
return list(reversed(path))
path.append(s)
@@ -207,8 +180,8 @@ def main():
x_start = (5, 5)
x_goal = (45, 25)
- lpastar = LpaStar(x_start, x_goal, "euclidean")
- lpastar.searching()
+ dstar = DStar(x_start, x_goal, "euclidean")
+ dstar.run()
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py
index 4ef9d51..fd6310b 100644
--- a/Search-based Planning/Search_2D/LPAstar.py
+++ b/Search-based Planning/Search_2D/LPAstar.py
@@ -21,15 +21,15 @@ class LpaStar:
self.xI, self.xG = x_start, x_goal
self.heuristic_type = heuristic_type
- self.Env = env.Env() # class Env
+ self.Env = env.Env()
self.Plot = plotting.Plotting(x_start, x_goal)
- self.u_set = self.Env.motions # feasible input set
- self.obs = self.Env.obs # position of obstacles
+ 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() # priority queue / U set
+ self.U = queue.QueuePrior()
self.g, self.rhs = {}, {}
for i in range(self.Env.x_range):
@@ -39,9 +39,9 @@ class LpaStar:
self.rhs[self.xI] = 0
self.U.put(self.xI, self.Key(self.xI))
-
- def searching(self):
self.fig = plt.figure()
+
+ def run(self):
self.Plot.plot_grid("Lifelong Planning A*")
self.ComputePath()
@@ -62,9 +62,10 @@ class LpaStar:
if (x, y) not in self.obs:
self.obs.add((x, y))
plt.plot(x, y, 'sk')
+ plt.pause(0.001)
self.rhs[(x, y)] = float("inf")
self.g[(x, y)] = float("inf")
- for node in self.getSucc((x, y)):
+ for node in self.get_neighbor((x, y)):
self.UpdateVertex(node)
else:
self.obs.remove((x, y))
@@ -86,54 +87,48 @@ class LpaStar:
s = self.U.get()
if self.g[s] > self.rhs[s]:
self.g[s] = self.rhs[s]
- for x in self.getSucc(s):
- self.UpdateVertex(x)
else:
self.g[s] = float("inf")
self.UpdateVertex(s)
- for x in self.getSucc(s):
- self.UpdateVertex(x)
-
- def getSucc(self, s):
- nei_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 and self.g[s_next] > self.g[s]:
- nei_list.add(s_next)
- return nei_list
-
- def getPred(self, s):
- nei_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 and self.g[s_next] < self.g[s]:
- nei_list.add(s_next)
- return nei_list
+ for x in self.get_neighbor(s):
+ self.UpdateVertex(x)
def UpdateVertex(self, s):
if s != self.xI:
u_min = float("inf")
- for x in self.getPred(s):
- u_min = min(u_min, self.g[x] + self.get_cost(x, s))
+ for x in self.get_neighbor(s):
+ u_min = min(u_min, self.g[x] + self.cost(x, s))
self.rhs[s] = u_min
self.U.remove(s)
if self.g[s] != self.rhs[s]:
self.U.put(s, self.Key(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)
+ def get_neighbor(self, s):
+ nei_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:
+ nei_list.add(s_next)
+
+ return nei_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):
+ heuristic_type = self.heuristic_type # heuristic type
+ goal = self.xG # goal node
+
+ if heuristic_type == "manhattan":
+ return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+ else:
+ return math.hypot(goal[0] - s[0], goal[1] - s[1])
+
+ 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 extract_path(self):
path = []
@@ -162,48 +157,28 @@ class LpaStar:
path.append(s)
return list(reversed(path))
- def get_neighbor(self, s):
- nei_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:
- nei_list.add(s_next)
-
- return nei_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):
- heuristic_type = self.heuristic_type # heuristic type
- goal = self.xG # goal node
-
- if heuristic_type == "manhattan":
- return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
- else:
- return math.hypot(goal[0] - s[0], goal[1] - s[1])
-
- @staticmethod
- def get_cost(s_start, s_end):
- """
- Calculate cost for this motion
-
- :param s_start:
- :param s_end:
- :return: cost for this motion
- :note: cost function could be more complicate!
- """
-
- return 1
+ 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)
x_goal = (45, 25)
- lpastar = LpaStar(x_start, x_goal, "euclidean")
- lpastar.searching()
+ lpastar = LpaStar(x_start, x_goal, "manhattan")
+ lpastar.run()
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
index 8283245..ff2b686 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index 91533df..a39f99c 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ
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 324b066..9185c91 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/plotting.py b/Search-based Planning/Search_2D/plotting.py
index 1121d06..fc6ea9b 100644
--- a/Search-based Planning/Search_2D/plotting.py
+++ b/Search-based Planning/Search_2D/plotting.py
@@ -96,10 +96,10 @@ class Plotting:
def plot_path(self, path, cl='r', flag=False):
if self.xI in path:
- path.remove(self.xI)
+ path.delete(self.xI)
if self.xG in path:
- path.remove(self.xG)
+ path.delete(self.xG)
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
@@ -113,10 +113,10 @@ class Plotting:
def plot_visited_bi(self, v_fore, v_back):
if self.xI in v_fore:
- v_fore.remove(self.xI)
+ v_fore.delete(self.xI)
if self.xG in v_back:
- v_back.remove(self.xG)
+ v_back.delete(self.xG)
len_fore, len_back = len(v_fore), len(v_back)
diff --git a/Search-based Planning/Search_2D/queue.py b/Search-based Planning/Search_2D/queue.py
index 2721920..2a9fb3b 100644
--- a/Search-based Planning/Search_2D/queue.py
+++ b/Search-based Planning/Search_2D/queue.py
@@ -53,16 +53,15 @@ class QueuePrior:
return len(self.queue) == 0
def put(self, item, priority):
- flag = 0
+ 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)
- flag = 1
break
count += 1
- if flag == 0:
- heapq.heappush(self.queue, (priority, item)) # reorder x using priority
def get(self):
return heapq.heappop(self.queue)[1] # pop out the smallest item