diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index efc3266..e31aa0f 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,9 +21,7 @@ - - - + - + + + + - - - - + + - diff --git a/Search-based Planning/Search_2D/FieldD_star.py b/Search-based Planning/Search_2D/FieldD_star.py deleted file mode 100644 index e69de29..0000000 diff --git a/Search-based Planning/Search_2D/Field_D_star.py b/Search-based Planning/Search_2D/Field_D_star.py new file mode 100644 index 0000000..e5193fa --- /dev/null +++ b/Search-based Planning/Search_2D/Field_D_star.py @@ -0,0 +1,267 @@ +""" +Field D* 2D +@author: huiming zhou +""" + +import os +import sys +import math +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 FieldDStar: + 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(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.g, self.rhs, self.U = {}, {}, {} + self.parent = {} + + for i in range(self.Env.x_range): + for j in range(self.Env.y_range): + self.rhs[(i, j)] = float("inf") + self.g[(i, j)] = float("inf") + self.parent[(i, j)] = (0, 0) + + self.rhs[self.s_goal] = 0.0 + self.U[self.s_goal] = self.CalculateKey(self.s_goal) + self.visited = set() + self.count = 0 + self.fig = plt.figure() + + def run(self): + self.Plot.plot_grid("Field D*") + self.ComputeShortestPath() + self.plot_path(self.extract_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("Change position: x =", x, ",", "y =", y) + self.visited = set() + self.count += 1 + if (x, y) not in self.obs: + self.obs.add((x, y)) + plt.plot(x, y, 'sk') + else: + self.obs.remove((x, y)) + plt.plot(x, y, marker='s', color='white') + self.UpdateVertex((x, y)) + + for s_n in self.get_neighbor((x, y)): + self.UpdateVertex(s_n) + + self.ComputeShortestPath() + self.plot_visited(self.visited) + self.plot_path(self.extract_path()) + self.fig.canvas.draw_idle() + + def ComputeShortestPath(self): + 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) + self.visited.add(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): + self.UpdateVertex(x) + else: + self.g[s] = float("inf") + self.UpdateVertex(s) + for x in self.get_neighbor(s): + self.UpdateVertex(x) + + def UpdateVertex(self, s): + if s != self.s_goal: + value = [] + s_plist = [] + sn_list = self.get_neighbor_pure(s) + sn_list.append(sn_list[0]) + for k in range(8): + v, sp = self.ComputeCost(s, sn_list[k], sn_list[k + 1]) + value.append(v) + s_plist.append(sp) + self.rhs[s] = min(value) + self.parent[s] = s_plist[value.index(min(value))] + + if s in self.U: + self.U.pop(s) + + if self.g[s] != self.rhs[s]: + self.U[s] = self.CalculateKey(s) + + def get_neighbor_pure(self, s): + s_list = [] + + for u in self.u_set: + s_next = tuple([s[i] + u[i] for i in range(2)]) + s_list.append(s_next) + + return s_list + + def CalculateKey(self, s): + return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s), + min(self.g[s], self.rhs[s])] + + def ComputeCost(self, s, sa, sb): + if sa[0] != s[0] and sa[1] != s[1]: + s1, s2 = sb, sa + else: + s1, s2 = sa, sb + + c = self.cost(s, s2) + b = self.cost(s, s1) + y = 0 + + if min(c, b) == float("inf"): + vs = float("inf") + elif self.g[s1] <= self.g[s2]: + vs = min(c, b) + self.g[s1] + else: + f = self.g[s1] - self.g[s2] + if f <= b: + if c <= f: + vs = math.sqrt(2) * c + self.g[s2] + else: + y = min(f / (math.sqrt(c ** 2 - f ** 2)), 1) + vs = c * math.sqrt(1 + y ** 2) + f * (1 - y) + self.g[s2] + else: + if c <= b: + vs = math.sqrt(2) * c + self.g[s2] + else: + x = 1 - min(b / (math.sqrt(c ** 2 - b ** 2)), 1) + vs = c * math.sqrt(1 + (1 - x) ** 2) + b * x + self.g[s2] + + ss = (y * s1[0] + (1 - y) * s2[0], y * s1[1] + (1 - y) * s2[1]) + + return vs, ss + + 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 + + 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 cost(self, s_start, s_goal): + """ + Calculate cost for this motion + :param s_start: starting node + :param s_goal: end node + :return: cost for this motion + :note: cost function could be more complicate! + """ + + if self.is_collision(s_start, s_goal): + return float("inf") + + return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) + + def is_collision(self, s_start, s_end): + if s_start in self.obs or s_end in self.obs: + return True + + if s_start[0] != s_end[0] and s_start[1] != s_end[1]: + if s_end[0] - s_start[0] == s_start[1] - s_end[1]: + s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) + s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) + else: + s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) + s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) + + if s1 in self.obs or s2 in self.obs: + return True + + return False + + def get_neighbor(self, s): + s_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: + s_list.append(s_next) + + return s_list + + def extract_path(self): + path = [self.s_start] + s = self.s_start + count = 0 + while True: + count += 1 + g_list = {} + for x in self.get_neighbor(s): + if not self.is_collision(s, x): + g_list[x] = self.g[x] + ss = self.parent[s] + s = min(g_list, key=g_list.get) + path.append(s) + + if s == self.s_goal or count > 100: + return list(reversed(path)) + + def plot_path(self, path): + px = [x[0] for x in path] + py = [x[1] for x in path] + plt.plot(px, py, linewidth=2) + plt.plot(self.s_start[0], self.s_start[1], "bs") + plt.plot(self.s_goal[0], self.s_goal[1], "gs") + + def plot_visited(self, visited): + color = ['gainsboro', 'lightgray', 'silver', 'darkgray', + 'bisque', 'navajowhite', 'moccasin', 'wheat', + 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue'] + + if self.count >= len(color) - 1: + self.count = 0 + + for x in visited: + plt.plot(x[0], x[1], marker='s', color=color[self.count]) + + +def main(): + s_start = (5, 5) + s_goal = (45, 25) + + fielddstar = FieldDStar(s_start, s_goal, "euclidean") + fielddstar.run() + + +if __name__ == '__main__': + main()