From 808784abaee8f5c401c60fdf6c2a7f6e5f45a4ab Mon Sep 17 00:00:00 2001 From: zhm-real Date: Sun, 2 Aug 2020 00:49:09 -0700 Subject: [PATCH] update --- CurvesGenerator/cubic_spline.py | 8 +- .../rrt_2D/batch_informed_trees.py | 342 +++++++++++------- .../rrt_2D/informed_rrt_star.py | 12 +- .../Search_3D/Anytime_Dstar3D.py | 2 +- .../Search_3D/DstarLite3D.py | 4 +- .../Search_3D/LRT_Astar3D.py | 2 +- 6 files changed, 218 insertions(+), 152 deletions(-) diff --git a/CurvesGenerator/cubic_spline.py b/CurvesGenerator/cubic_spline.py index 7385e02..d33b461 100644 --- a/CurvesGenerator/cubic_spline.py +++ b/CurvesGenerator/cubic_spline.py @@ -28,10 +28,10 @@ class Spline: self.nx = len(x) # dimension of x h = np.diff(x) - # calc coefficient c + # calc coefficient cBest self.a = [iy for iy in y] - # calc coefficient c + # calc coefficient cBest A = self.__calc_A(h) B = self.__calc_B(h) self.c = np.linalg.solve(A, B) @@ -104,7 +104,7 @@ class Spline: def __calc_A(self, h): u""" - calc matrix A for spline coefficient c + calc matrix A for spline coefficient cBest """ A = np.zeros((self.nx, self.nx)) A[0, 0] = 1.0 @@ -122,7 +122,7 @@ class Spline: def __calc_B(self, h): u""" - calc matrix B for spline coefficient c + calc matrix B for spline coefficient cBest """ B = np.zeros(self.nx) for i in range(self.nx - 2): diff --git a/Sampling_based_Planning/rrt_2D/batch_informed_trees.py b/Sampling_based_Planning/rrt_2D/batch_informed_trees.py index 8627cc9..c0f2688 100644 --- a/Sampling_based_Planning/rrt_2D/batch_informed_trees.py +++ b/Sampling_based_Planning/rrt_2D/batch_informed_trees.py @@ -6,11 +6,12 @@ Batch Informed Trees (BIT*) import os import sys import math -import random import copy +import random import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches +from scipy.spatial.transform import Rotation as Rot sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") @@ -63,15 +64,14 @@ class BITStar: self.Tree = Tree(self.x_start, self.x_goal) self.X_sample = set() self.g_T = dict() - self.f_T = dict() def init(self): + print("init") self.Tree.V.add(self.x_start) self.X_sample.add(self.x_goal) - self.g_T[self.x_goal] = np.inf - self.f_T[self.x_goal] = 0.0 + self.g_T[self.x_start] = 0.0 - self.f_T[self.x_start] = self.f_estimated(self.x_start) + self.g_T[self.x_goal] = np.inf cMin, theta = self.calc_dist_and_angle(self.x_start, self.x_goal) C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin) @@ -81,12 +81,12 @@ class BITStar: return theta, cMin, xCenter, C def planning(self): - eTheta, cMin, xCenter, C = self.init() + m = 200 + theta, cMin, xCenter, C = self.init() for k in range(self.iter_max): if not self.Tree.QE and not self.Tree.QV: self.Prune(self.g_T[self.x_goal]) - m = 200 self.X_sample.update(self.Sample(m, self.g_T[self.x_goal], cMin, xCenter, C)) self.Tree.V_old = copy.deepcopy(self.Tree.V) self.Tree.QV = copy.deepcopy(self.Tree.V) @@ -99,13 +99,14 @@ class BITStar: self.Tree.QE.remove((vm, xm)) if self.g_T[vm] + self.calc_dist(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]: - if self.g_estimated(vm) + self.cost(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]: - if self.g_T[vm] + self.cost(vm, xm) < self.g_T[xm]: + actual_cost = self.cost(vm, xm) + if self.g_estimated(vm) + actual_cost + self.h_estimated(xm) < self.g_T[self.x_goal]: + if self.g_T[vm] + actual_cost < self.g_T[xm]: if xm in self.Tree.V: # remove edges - for vl, vr in self.Tree.E: - if vl == xm or vr == xm: - self.Tree.E.remove((vl, vr)) + for v, x in self.Tree.E: + if x == xm: + self.Tree.E.remove((v, x)) else: self.X_sample.remove(xm) self.Tree.V.add(xm) @@ -113,133 +114,20 @@ class BITStar: self.Tree.E.add((vm, xm)) - def ExpandVertex(self, v): - self.Tree.QV.remove(v) - X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r} - - for x in X_near: - if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]: - self.Tree.QE.add((v, x)) - - if v not in self.Tree.V_old: - V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r} - - for w in V_near: - if (v, w) not in self.Tree.E and (w, v) not in self.Tree.E and \ - self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and \ - self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]: - self.Tree.QE.add((v, w)) - - def BestVertexQueueValue(self): - if not self.Tree.QV: - return np.inf - - return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV) - - def BestEdgeQueueValue(self): - if not self.Tree.QE: - return np.inf - - return min(self.g_T[el] + self.calc_dist(el, er) + self.h_estimated(er) - for el, er in self.Tree.QE) - - def BestInVertexQueue(self): - v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV} - - return min(v_value, key=v_value.get) - - def BestInEdgeQueue(self): - e_value = {(el, er): self.g_T[el] + self.calc_dist(el, er) + self.h_estimated(er) - for el, er in self.Tree.QE} - - return min(e_value, key=e_value.get) - - def radius(self, q): - lambda_X = 0 - sigma = math.pi ** 2 - - for x in self.Tree.V: - if self.f_estimated(x) <= self.g_T[self.x_goal]: - lambda_X += 1 - - return 2 * self.eta * 1.5 ** 0.5 * (lambda_X / sigma * math.log(q) / q) ** 0.5 - - def Sample(self, m, cMax, cMin, xCenter, C): - if cMax < np.inf: - Sample = self.SampleEllipsoid(m, cMax, cMin, xCenter, C) - else: - Sample = self.SampleFreeSpace(m) - - return Sample - - def SampleEllipsoid(self, m, cMax, cMin, xCenter, C): - r = [cMax / 2.0, - math.sqrt(cMax ** 2 - cMin ** 2) / 2.0, - math.sqrt(cMax ** 2 - cMin ** 2) / 2.0] - L = np.diag(r) - - ind = 0 - delta = self.delta - Sample = set() - - while ind < m: - xBall = self.SampleUnitNBall() - x_rand = C @ L @ xBall + xCenter - node = Node(x_rand[0], x_rand[1]) - not_in_obs = ~self.utils.is_inside_obs(node) - in_x_range = self.x_range[0] + delta <= node.x <= self.x_range[1] - delta - in_y_range = self.y_range[0] + delta <= node.y <= self.y_range[1] - delta - - if not_in_obs and in_x_range and in_y_range: - Sample.add(node) - ind += 1 - - return Sample - - def SampleFreeSpace(self, m): - delta = self.utils.delta - Sample = set() - - ind = 0 - while ind < m: - node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), - random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) - if self.utils.is_inside_obs(node): - continue + for v, x in self.Tree.QE: + if x == xm and self.g_T[v] + self.calc_dist(v, xm) >= self.g_T[xm]: + self.Tree.QE.remove((v, xm)) else: - Sample.add(node) - ind += 1 + self.Tree.QE = set() + self.Tree.QV = set() - return Sample - - @staticmethod - def SampleUnitNBall(): - theta, r = random.uniform(0.0, 2 * math.pi), random.random() - x = r * math.cos(theta) - y = r * math.sin(theta) - - return np.array([[x], [y], [0.0]]) - - def Prune(self, c): - for x in self.X_sample: - if self.f_estimated(x) >= c: - self.X_sample.remove(x) - - for v in self.Tree.V: - if self.f_estimated(v) > c: - self.Tree.V.remove(v) - - for v, w in self.Tree.E: - if self.f_estimated(v) > c or self.f_estimated(w) > c: - self.Tree.E.remove((v, w)) - - for v in self.Tree.V: - if v.g_T == np.inf: - self.X_sample.add(v) - - for v in self.Tree.V: - if v.g_T == np.inf: - self.Tree.V.remove(v) + def Prune(self, cBest): + self.X_sample = {x for x in self.X_sample if self.f_estimated(x) < cBest} + self.Tree.V = {v for v in self.Tree.V if self.f_estimated(v) <= cBest} + self.Tree.E = {(v, w) for v, w in self.Tree.E + if self.f_estimated(v) <= cBest and self.f_estimated(w) <= cBest} + self.X_sample.update({v for v in self.Tree.V if self.g_T[v] == np.inf}) + self.Tree.V = {v for v in self.Tree.V if self.g_T[v] < np.inf} def cost(self, start, end): if self.utils.is_collision(start, end): @@ -256,9 +144,107 @@ class BITStar: def h_estimated(self, node): return self.calc_dist(node, self.x_goal) + def Sample(self, m, cMax, cMin, xCenter, C): + if cMax < np.inf: + return self.SampleEllipsoid(m, cMax, cMin, xCenter, C) + else: + return self.SampleFreeSpace(m) + + def SampleEllipsoid(self, m, cMax, cMin, xCenter, C): + r = [cMax / 2.0, + math.sqrt(cMax ** 2 - cMin ** 2) / 2.0, + math.sqrt(cMax ** 2 - cMin ** 2) / 2.0] + L = np.diag(r) + + ind = 0 + delta = self.delta + Sample = set() + + while ind < m: + xBall = self.SampleUnitNBall() + x_rand = np.dot(np.dot(C, L), xBall) + xCenter + node = Node(x_rand[(0, 0)], x_rand[(1, 0)]) + in_obs = self.utils.is_inside_obs(node) + in_x_range = self.x_range[0] + delta <= node.x <= self.x_range[1] - delta + in_y_range = self.y_range[0] + delta <= node.y <= self.y_range[1] - delta + + if not in_obs and in_x_range and in_y_range: + Sample.add(node) + ind += 1 + + return Sample + + def SampleFreeSpace(self, m): + delta = self.utils.delta + Sample = set() + + ind = 0 + while ind < m: + node = Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), + random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)) + if self.utils.is_inside_obs(node): + continue + else: + Sample.add(node) + ind += 1 + + return Sample + + def radius(self, q): + cBest = self.g_T[self.x_goal] + lambda_X = len([1 for v in self.Tree.V if self.f_estimated(v) <= cBest]) + radius = 2 * self.eta * (1.5 * lambda_X / math.pi * math.log(q) / q) ** 0.5 + + return radius + + def ExpandVertex(self, v): + self.Tree.QV.remove(v) + X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r} + edges_add = {(v, x) for x in X_near + if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]} + self.Tree.QE.update(edges_add) + + if v not in self.Tree.V_old: + V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r} + edges_add = {(v, w) for w in V_near if (v, w) not in self.Tree.E and + self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and + self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]} + self.Tree.QE.update(edges_add) + + def BestVertexQueueValue(self): + if not self.Tree.QV: + return np.inf + + return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV) + + def BestEdgeQueueValue(self): + if not self.Tree.QE: + return np.inf + + return min(self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x) + for v, x in self.Tree.QE) + + def BestInVertexQueue(self): + v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV} + + return min(v_value, key=v_value.get) + + def BestInEdgeQueue(self): + e_value = {(v, x): self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x) + for v, x in self.Tree.QE} + + return min(e_value, key=e_value.get) + + @staticmethod + def SampleUnitNBall(): + while True: + x, y = random.uniform(-1, 1), random.uniform(-1, 1) + if x ** 2 + y ** 2 < 1: + return np.array([[x], [y], [0.0]]) + @staticmethod def RotationToWorldFrame(x_start, x_goal, L): - a1 = np.array([[(x_start.x - x_start.x) / L], + a1 = np.array([[(x_goal.x - x_start.x) / L], [(x_goal.y - x_start.y) / L], [0.0]]) e1 = np.array([[1.0], [0.0], [0.0]]) M = a1 @ e1.T @@ -276,3 +262,83 @@ class BITStar: dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) + + def animation(self, name): + theta, cMin, xCenter, C = self.init() + cBest = 30 + self.plot_grid(name) + sample = self.Sample(300, cBest, cMin, xCenter, C) + for node in sample: + plt.plot(node.x, node.y, marker='.', color='lightgrey') + self.draw_ellipse(xCenter, cBest, cMin, theta) + plt.pause(0.001) + plt.show() + + def plot_grid(self, name): + for (ox, oy, w, h) in self.obs_boundary: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + for (ox, oy, w, h) in self.obs_rectangle: + self.ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + for (ox, oy, r) in self.obs_circle: + self.ax.add_patch( + patches.Circle( + (ox, oy), r, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3) + plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3) + + plt.title(name) + plt.axis("equal") + + @staticmethod + def draw_ellipse(x_center, c_best, dist, theta): + a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0 + b = c_best / 2.0 + angle = math.pi / 2.0 - theta + cx = x_center[0] + cy = x_center[1] + t = np.arange(0, 2 * math.pi + 0.1, 0.1) + x = [a * math.cos(it) for it in t] + y = [b * math.sin(it) for it in t] + rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2] + fx = rot @ np.array([x, y]) + px = np.array(fx[0, :] + cx).flatten() + py = np.array(fx[1, :] + cy).flatten() + plt.plot(cx, cy, marker='.', color='darkorange') + plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2) + + +def main(): + x_start = (18, 8) # Starting node + x_goal = (37, 18) # Goal node + eta = 2 + iter_max = 200 + print("start!!!") + bit = BITStar(x_start, x_goal, eta, iter_max) + # bit.planning() + bit.animation("Batch Informed Trees (BIT*)") + + +if __name__ == '__main__': + main() diff --git a/Sampling_based_Planning/rrt_2D/informed_rrt_star.py b/Sampling_based_Planning/rrt_2D/informed_rrt_star.py index f96f798..c49d6fa 100644 --- a/Sampling_based_Planning/rrt_2D/informed_rrt_star.py +++ b/Sampling_based_Planning/rrt_2D/informed_rrt_star.py @@ -164,7 +164,7 @@ class IRrtStar: @staticmethod def RotationToWorldFrame(x_start, x_goal, L): - a1 = np.array([[(x_start.x - x_start.x) / L], + a1 = np.array([[(x_goal.x - x_start.x) / L], [(x_goal.y - x_start.y) / L], [0.0]]) e1 = np.array([[1.0], [0.0], [0.0]]) M = a1 @ e1.T @@ -175,11 +175,11 @@ class IRrtStar: @staticmethod def SampleUnitNBall(): - theta, r = random.uniform(0.0, 2 * math.pi), random.random() - x = r * math.cos(theta) - y = r * math.sin(theta) + while True: + x, y = random.uniform(-1, 1), random.uniform(-1, 1) - return np.array([[x], [y], [0.0]]) + if x ** 2 + y ** 2 < 1: + return np.array([[x], [y], [0.0]]) @staticmethod def Nearest(nodelist, n): @@ -284,7 +284,7 @@ def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node - rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 10000) + rrt_star = IRrtStar(x_start, x_goal, 10, 0.10, 20, 1000) rrt_star.planning() diff --git a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py index 30e6200..bc7fa62 100644 --- a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py @@ -209,7 +209,7 @@ class Anytime_Dstar(object): '''After ComputeShortestPath() returns, one can then follow a shortest path from x_init to x_goal by always moving from the current vertex s, starting - at x_init. , to any successor s' that minimizes c(s,s') + g(s') + at x_init. , to any successor s' that minimizes cBest(s,s') + g(s') until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt diff --git a/Search_based_Planning/Search_3D/DstarLite3D.py b/Search_based_Planning/Search_3D/DstarLite3D.py index 8706fa3..316de8f 100644 --- a/Search_based_Planning/Search_3D/DstarLite3D.py +++ b/Search_based_Planning/Search_3D/DstarLite3D.py @@ -165,7 +165,7 @@ class D_star_Lite(object): self.env.start = self.x0 # ---------------------------------- if any Cost changed, update km, reset slast, # for all directed edgees (u,v) with chaged edge costs, - # update the edge Cost c(u,v) and update vertex u. then replan + # update the edge Cost cBest(u,v) and update vertex u. then replan if ischanged: self.km += heuristic_fun(self, self.x0, s_last) s_last = self.x0 @@ -188,7 +188,7 @@ class D_star_Lite(object): '''After ComputeShortestPath() returns, one can then follow a shortest path from x_init to x_goal by always moving from the current vertex s, starting - at x_init. , to any successor s' that minimizes c(s,s') + g(s') + at x_init. , to any successor s' that minimizes cBest(s,s') + g(s') until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt diff --git a/Search_based_Planning/Search_3D/LRT_Astar3D.py b/Search_based_Planning/Search_3D/LRT_Astar3D.py index cb04dd6..89acf9a 100644 --- a/Search_based_Planning/Search_3D/LRT_Astar3D.py +++ b/Search_based_Planning/Search_3D/LRT_Astar3D.py @@ -36,7 +36,7 @@ class LRT_A_star2: # update h values if they are smaller Children = children(self.Astar,xi) minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) - # h(s) = h(s') if h(s) > c(s,s') + h(s') + # h(s) = h(s') if h(s) > cBest(s,s') + h(s') if self.Astar.h[xi] >= minfval: self.Astar.h[xi] = minfval hvals.append(self.Astar.h[xi])