Repository: zhm-real/PathPlanning Branch: master Commit: 89c200e0ab85 Files: 75 Total size: 389.8 KB Directory structure: gitextract_yrm3dru6/ ├── .idea/ │ ├── .gitignore │ ├── PathPlanning.iml │ ├── inspectionProfiles/ │ │ └── profiles_settings.xml │ ├── misc.xml │ ├── modules.xml │ └── vcs.xml ├── CurvesGenerator/ │ ├── bezier_path.py │ ├── bspline_curve.py │ ├── cubic_spline.py │ ├── draw.py │ ├── dubins_path.py │ ├── quartic_polynomial.py │ ├── quintic_polynomial.py │ └── reeds_shepp.py ├── LICENSE ├── README.md ├── Sampling_based_Planning/ │ ├── rrt_2D/ │ │ ├── adaptively_informed_trees.py │ │ ├── advanced_batch_informed_trees.py │ │ ├── batch_informed_trees.py │ │ ├── dubins_rrt_star.py │ │ ├── dynamic_rrt.py │ │ ├── env.py │ │ ├── extended_rrt.py │ │ ├── fast_marching_trees.py │ │ ├── informed_rrt_star.py │ │ ├── plotting.py │ │ ├── queue.py │ │ ├── rrt.py │ │ ├── rrt_connect.py │ │ ├── rrt_sharp.py │ │ ├── rrt_star.py │ │ ├── rrt_star_smart.py │ │ └── utils.py │ └── rrt_3D/ │ ├── ABIT_star3D.py │ ├── BIT_star3D.py │ ├── FMT_star3D.py │ ├── dynamic_rrt3D.py │ ├── env3D.py │ ├── extend_rrt3D.py │ ├── informed_rrt_star3D.py │ ├── plot_util3D.py │ ├── queue.py │ ├── rrt3D.py │ ├── rrt_connect3D.py │ ├── rrt_star3D.py │ ├── rrt_star_smart3D.py │ └── utils3D.py └── Search_based_Planning/ ├── Search_2D/ │ ├── ARAstar.py │ ├── Anytime_D_star.py │ ├── Astar.py │ ├── Best_First.py │ ├── Bidirectional_a_star.py │ ├── D_star.py │ ├── D_star_Lite.py │ ├── Dijkstra.py │ ├── LPAstar.py │ ├── LRTAstar.py │ ├── RTAAStar.py │ ├── bfs.py │ ├── dfs.py │ ├── env.py │ ├── plotting.py │ └── queue.py └── Search_3D/ ├── Anytime_Dstar3D.py ├── Astar3D.py ├── Dstar3D.py ├── DstarLite3D.py ├── LP_Astar3D.py ├── LRT_Astar3D.py ├── RTA_Astar3D.py ├── bidirectional_Astar3D.py ├── env3D.py ├── plot_util3D.py ├── queue.py └── utils3D.py ================================================ FILE CONTENTS ================================================ ================================================ FILE: .idea/.gitignore ================================================ # Default ignored files /workspace.xml ================================================ FILE: .idea/PathPlanning.iml ================================================ ================================================ FILE: .idea/inspectionProfiles/profiles_settings.xml ================================================ ================================================ FILE: .idea/misc.xml ================================================ ================================================ FILE: .idea/modules.xml ================================================ ================================================ FILE: .idea/vcs.xml ================================================ ================================================ FILE: CurvesGenerator/bezier_path.py ================================================ """ bezier path author: Atsushi Sakai(@Atsushi_twi) modified: huiming zhou """ import numpy as np import matplotlib.pyplot as plt from scipy.special import comb import draw def calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset): dist = np.hypot(sx - gx, sy - gy) / offset control_points = np.array( [[sx, sy], [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)], [gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)], [gx, gy]]) path = calc_bezier_path(control_points, n_points=100) return path, control_points def calc_bezier_path(control_points, n_points=100): traj = [] for t in np.linspace(0, 1, n_points): traj.append(bezier(t, control_points)) return np.array(traj) def Comb(n, i, t): return comb(n, i) * t ** i * (1 - t) ** (n - i) def bezier(t, control_points): n = len(control_points) - 1 return np.sum([Comb(n, i, t) * control_points[i] for i in range(n + 1)], axis=0) def bezier_derivatives_control_points(control_points, n_derivatives): w = {0: control_points} for i in range(n_derivatives): n = len(w[i]) w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)]) return w def curvature(dx, dy, ddx, ddy): return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2) def simulation(): sx = [-3, 0, 4, 6] sy = [2, 0, 1.5, 6] ratio = np.linspace(0, 1, 100) pathx, pathy = [], [] for t in ratio: x, y = [], [] for i in range(len(sx) - 1): x.append(sx[i + 1] * t + sx[i] * (1 - t)) y.append(sy[i + 1] * t + sy[i] * (1 - t)) xx, yy = [], [] for i in range(len(x) - 1): xx.append(x[i + 1] * t + x[i] * (1 - t)) yy.append(y[i + 1] * t + y[i] * (1 - t)) px = xx[1] * t + xx[0] * (1 - t) py = yy[1] * t + yy[0] * (1 - t) pathx.append(px) pathy.append(py) plt.cla() plt.plot(sx, sy, linestyle='-', marker='o', color='dimgray', label="Control Points") plt.plot(x, y, color='dodgerblue') plt.plot(xx, yy, color='cyan') plt.plot(pathx, pathy, color='darkorange', linewidth=2, label="Bezier Path") plt.plot(px, py, marker='o') plt.axis("equal") plt.legend() plt.title("Cubic Bezier Curve demo") plt.grid(True) plt.pause(0.001) plt.show() def main(): sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0) gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0) offset = 3.0 path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset) t = 0.8 # Number in [0, 1] x_target, y_target = bezier(t, control_points) derivatives_cp = bezier_derivatives_control_points(control_points, 2) point = bezier(t, control_points) dt = bezier(t, derivatives_cp[1]) ddt = bezier(t, derivatives_cp[2]) # Radius of curv radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1]) # Normalize derivative dt /= np.linalg.norm(dt, 2) tangent = np.array([point, point + dt]) normal = np.array([point, point + [- dt[1], dt[0]]]) curvature_center = point + np.array([- dt[1], dt[0]]) * radius circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1) assert path.T[0][0] == sx, "path is invalid" assert path.T[1][0] == sy, "path is invalid" assert path.T[0][-1] == gx, "path is invalid" assert path.T[1][-1] == gy, "path is invalid" fig, ax = plt.subplots() ax.plot(path.T[0], path.T[1], label="Bezier Path") ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points") ax.plot(x_target, y_target) ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") ax.plot(normal[:, 0], normal[:, 1], label="Normal") ax.add_artist(circle) draw.Arrow(sx, sy, syaw, 1, "darkorange") draw.Arrow(gx, gy, gyaw, 1, "darkorange") plt.grid(True) plt.title("Bezier Path: from Atsushi's work") ax.axis("equal") plt.show() if __name__ == '__main__': main() # simulation() ================================================ FILE: CurvesGenerator/bspline_curve.py ================================================ """ Path Planner with B-Spline author: Atsushi Sakai (@Atsushi_twi) """ import numpy as np import matplotlib.pyplot as plt import scipy.interpolate as scipy_interpolate import cubic_spline as cs def approximate_b_spline_path(x, y, n_path_points, degree=3): t = range(len(x)) x_tup = scipy_interpolate.splrep(t, x, k=degree) y_tup = scipy_interpolate.splrep(t, y, k=degree) x_list = list(x_tup) x_list[1] = x + [0.0, 0.0, 0.0, 0.0] y_list = list(y_tup) y_list[1] = y + [0.0, 0.0, 0.0, 0.0] ipl_t = np.linspace(0.0, len(x) - 1, n_path_points) rx = scipy_interpolate.splev(ipl_t, x_list) ry = scipy_interpolate.splev(ipl_t, y_list) return rx, ry def interpolate_b_spline_path(x, y, n_path_points, degree=3): ipl_t = np.linspace(0.0, len(x) - 1, len(x)) spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree) spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree) travel = np.linspace(0.0, len(x) - 1, n_path_points) return spl_i_x(travel), spl_i_y(travel) def main(): print(__file__ + " start!!") # way points # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0] # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0] way_point_x = [-2, 2.0, 3.5, 5.5, 6.0, 8.0] way_point_y = [0, 2.7, -0.5, 0.5, 3.0, 4.0] sp = cs.Spline2D(way_point_x, way_point_y) s = np.arange(0, sp.s[-1], 0.1) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = sp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(sp.calc_yaw(i_s)) rk.append(sp.calc_curvature(i_s)) n_course_point = 100 # sampling number rax, ray = approximate_b_spline_path(way_point_x, way_point_y, n_course_point) rix, riy = interpolate_b_spline_path(way_point_x, way_point_y, n_course_point) # show results plt.plot(way_point_x, way_point_y, '-og', label="Control Points") plt.plot(rax, ray, '-r', label="Approximated B-Spline path") plt.plot(rix, riy, '-b', label="Interpolated B-Spline path") plt.plot(rx, ry, color='dimgray', label="Cubic Spline") plt.grid(True) plt.title("Curves Comparison") plt.legend() plt.axis("equal") plt.show() if __name__ == '__main__': main() ================================================ FILE: CurvesGenerator/cubic_spline.py ================================================ #! /usr/bin/python # -*- coding: utf-8 -*- u""" Cubic Spline library on python author Atsushi Sakai usage: see test codes as below license: MIT """ import math import numpy as np import bisect class Spline: u""" Cubic Spline class """ def __init__(self, x, y): self.b, self.c, self.d, self.w = [], [], [], [] self.x = x self.y = y self.nx = len(x) # dimension of s h = np.diff(x) # calc coefficient cBest self.a = [iy for iy in y] # calc coefficient cBest A = self.__calc_A(h) B = self.__calc_B(h) self.c = np.linalg.solve(A, B) # print(self.c1) # calc spline coefficient b and d for i in range(self.nx - 1): self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 self.b.append(tb) def calc(self, t): u""" Calc position if t is outside of the input s, return None """ if t < self.x[0]: return None elif t > self.x[-1]: return None i = self.__search_index(t) dx = t - self.x[i] result = self.a[i] + self.b[i] * dx + \ self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 return result def calcd(self, t): u""" Calc first derivative if t is outside of the input s, return None """ if t < self.x[0]: return None elif t > self.x[-1]: return None i = self.__search_index(t) dx = t - self.x[i] result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 return result def calcdd(self, t): u""" Calc second derivative """ if t < self.x[0]: return None elif t > self.x[-1]: return None i = self.__search_index(t) dx = t - self.x[i] result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx return result def __search_index(self, x): u""" search data segment index """ return bisect.bisect(self.x, x) - 1 def __calc_A(self, h): u""" calc matrix A for spline coefficient cBest """ A = np.zeros((self.nx, self.nx)) A[0, 0] = 1.0 for i in range(self.nx - 1): if i != (self.nx - 2): A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) A[i + 1, i] = h[i] A[i, i + 1] = h[i] A[0, 1] = 0.0 A[self.nx - 1, self.nx - 2] = 0.0 A[self.nx - 1, self.nx - 1] = 1.0 # print(A) return A def __calc_B(self, h): u""" calc matrix B for spline coefficient cBest """ B = np.zeros(self.nx) for i in range(self.nx - 2): B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] # print(B) return B class Spline2D: u""" 2D Cubic Spline class """ def __init__(self, x, y): self.s = self.__calc_s(x, y) self.sx = Spline(self.s, x) self.sy = Spline(self.s, y) def __calc_s(self, x, y): dx = np.diff(x) dy = np.diff(y) self.ds = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] s = [0] s.extend(np.cumsum(self.ds)) return s def calc_position(self, s): u""" calc position """ x = self.sx.calc(s) y = self.sy.calc(s) return x, y def calc_curvature(self, s): u""" calc curvature """ dx = self.sx.calcd(s) ddx = self.sx.calcdd(s) dy = self.sy.calcd(s) ddy = self.sy.calcdd(s) k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) return k def calc_yaw(self, s): u""" calc yaw """ dx = self.sx.calcd(s) dy = self.sy.calcd(s) yaw = math.atan2(dy, dx) return yaw def calc_spline_course(x, y, ds=0.1): sp = Spline2D(x, y) s = np.arange(0, sp.s[-1], ds) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = sp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(sp.calc_yaw(i_s)) rk.append(sp.calc_curvature(i_s)) return rx, ry, ryaw, rk, s def test_spline2d(): print("Spline 2D test") import matplotlib.pyplot as plt x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] sp = Spline2D(x, y) s = np.arange(0, sp.s[-1], 0.1) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = sp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(sp.calc_yaw(i_s)) rk.append(sp.calc_curvature(i_s)) flg, ax = plt.subplots(1) plt.plot(x, y, "xb", label="input") plt.plot(rx, ry, "-r", label="spline") plt.grid(True) plt.axis("equal") plt.xlabel("s[m]") plt.ylabel("y[m]") plt.legend() flg, ax = plt.subplots(1) plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("yaw angle[deg]") flg, ax = plt.subplots(1) plt.plot(s, rk, "-r", label="curvature") plt.grid(True) plt.legend() plt.xlabel("line length[m]") plt.ylabel("curvature [1/m]") plt.show() def test_spline(): print("Spline test") import matplotlib.pyplot as plt x = [-0.5, 0.0, 0.5, 1.0, 1.5] y = [3.2, 2.7, 6, 5, 6.5] spline = Spline(x, y) rx = np.arange(-2.0, 4, 0.01) ry = [spline.calc(i) for i in rx] plt.plot(x, y, "xb") plt.plot(rx, ry, "-r") plt.grid(True) plt.axis("equal") plt.show() if __name__ == '__main__': test_spline() # test_spline2d() ================================================ FILE: CurvesGenerator/draw.py ================================================ import matplotlib.pyplot as plt import numpy as np PI = np.pi class Arrow: def __init__(self, x, y, theta, L, c): angle = np.deg2rad(30) d = 0.5 * L w = 2 x_start = x y_start = y x_end = x + L * np.cos(theta) y_end = y + L * np.sin(theta) theta_hat_L = theta + PI - angle theta_hat_R = theta + PI + angle x_hat_start = x_end x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) y_hat_start = y_end y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) plt.plot([x_hat_start, x_hat_end_L], [y_hat_start, y_hat_end_L], color=c, linewidth=w) plt.plot([x_hat_start, x_hat_end_R], [y_hat_start, y_hat_end_R], color=c, linewidth=w) class Car: def __init__(self, x, y, yaw, w, L): theta_B = PI + yaw xB = x + L / 4 * np.cos(theta_B) yB = y + L / 4 * np.sin(theta_B) theta_BL = theta_B + PI / 2 theta_BR = theta_B - PI / 2 x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex y_BL = yB + w / 2 * np.sin(theta_BL) x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex y_BR = yB + w / 2 * np.sin(theta_BR) x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex y_FL = y_BL + L * np.sin(yaw) x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex y_FR = y_BR + L * np.sin(yaw) plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL], [y_BL, y_BR, y_FR, y_FL, y_BL], linewidth=1, color='black') Arrow(x, y, yaw, L / 2, 'black') # plt.axis("equal") # plt.show() if __name__ == '__main__': # Arrow(-1, 2, 60) Car(0, 0, 1, 2, 60) ================================================ FILE: CurvesGenerator/dubins_path.py ================================================ """ Dubins Path """ import math import numpy as np import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as Rot import CurvesGenerator.draw as draw # class for PATH element class PATH: def __init__(self, L, mode, x, y, yaw): self.L = L # total path length [float] self.mode = mode # type of each part of the path [string] self.x = x # final s positions [m] self.y = y # final y positions [m] self.yaw = yaw # final yaw angles [rad] # utils def pi_2_pi(theta): while theta > math.pi: theta -= 2.0 * math.pi while theta < -math.pi: theta += 2.0 * math.pi return theta def mod2pi(theta): return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0) def LSL(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b) if p_lsl < 0: return None, None, None, ["L", "S", "L"] else: p_lsl = math.sqrt(p_lsl) denominate = dist + sin_a - sin_b t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate)) q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate)) return t_lsl, p_lsl, q_lsl, ["L", "S", "L"] def RSR(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a) if p_rsr < 0: return None, None, None, ["R", "S", "R"] else: p_rsr = math.sqrt(p_rsr) denominate = dist - sin_a + sin_b t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate)) q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate)) return t_rsr, p_rsr, q_rsr, ["R", "S", "R"] def LSR(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b) if p_lsr < 0: return None, None, None, ["L", "S", "R"] else: p_lsr = math.sqrt(p_lsr) rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr) t_lsr = mod2pi(-alpha + rec) q_lsr = mod2pi(-mod2pi(beta) + rec) return t_lsr, p_lsr, q_lsr, ["L", "S", "R"] def RSL(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b) if p_rsl < 0: return None, None, None, ["R", "S", "L"] else: p_rsl = math.sqrt(p_rsl) rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl) t_rsl = mod2pi(alpha - rec) q_rsl = mod2pi(beta - rec) return t_rsl, p_rsl, q_rsl, ["R", "S", "L"] def RLR(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0 if abs(rec) > 1.0: return None, None, None, ["R", "L", "R"] p_rlr = mod2pi(2 * math.pi - math.acos(rec)) t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0)) q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr)) return t_rlr, p_rlr, q_rlr, ["R", "L", "R"] def LRL(alpha, beta, dist): sin_a = math.sin(alpha) sin_b = math.sin(beta) cos_a = math.cos(alpha) cos_b = math.cos(beta) cos_a_b = math.cos(alpha - beta) rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0 if abs(rec) > 1.0: return None, None, None, ["L", "R", "L"] p_lrl = mod2pi(2 * math.pi - math.acos(rec)) t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0) q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl)) return t_lrl, p_lrl, q_lrl, ["L", "R", "L"] def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions): if m == "S": px[ind] = ox + l / maxc * math.cos(oyaw) py[ind] = oy + l / maxc * math.sin(oyaw) pyaw[ind] = oyaw else: ldx = math.sin(l) / maxc if m == "L": ldy = (1.0 - math.cos(l)) / maxc elif m == "R": ldy = (1.0 - math.cos(l)) / (-maxc) gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy px[ind] = ox + gdx py[ind] = oy + gdy if m == "L": pyaw[ind] = oyaw + l elif m == "R": pyaw[ind] = oyaw - l if l > 0.0: directions[ind] = 1 else: directions[ind] = -1 return px, py, pyaw, directions def generate_local_course(L, lengths, mode, maxc, step_size): point_num = int(L / step_size) + len(lengths) + 3 px = [0.0 for _ in range(point_num)] py = [0.0 for _ in range(point_num)] pyaw = [0.0 for _ in range(point_num)] directions = [0 for _ in range(point_num)] ind = 1 if lengths[0] > 0.0: directions[0] = 1 else: directions[0] = -1 if lengths[0] > 0.0: d = step_size else: d = -step_size ll = 0.0 for m, l, i in zip(mode, lengths, range(len(mode))): if l > 0.0: d = step_size else: d = -step_size ox, oy, oyaw = px[ind], py[ind], pyaw[ind] ind -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: pd = -d - ll else: pd = d - ll while abs(pd) <= abs(l): ind += 1 px, py, pyaw, directions = \ interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) pd += d ll = l - pd - d # calc remain length ind += 1 px, py, pyaw, directions = \ interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) if len(px) <= 1: return [], [], [], [] # remove unused data while len(px) >= 1 and px[-1] == 0.0: px.pop() py.pop() pyaw.pop() directions.pop() return px, py, pyaw, directions def planning_from_origin(gx, gy, gyaw, curv, step_size): D = math.hypot(gx, gy) d = D * curv theta = mod2pi(math.atan2(gy, gx)) alpha = mod2pi(-theta) beta = mod2pi(gyaw - theta) planners = [LSL, RSR, LSR, RSL, RLR, LRL] best_cost = float("inf") bt, bp, bq, best_mode = None, None, None, None for planner in planners: t, p, q, mode = planner(alpha, beta, d) if t is None: continue cost = (abs(t) + abs(p) + abs(q)) if best_cost > cost: bt, bp, bq, best_mode = t, p, q, mode best_cost = cost lengths = [bt, bp, bq] x_list, y_list, yaw_list, directions = generate_local_course( sum(lengths), lengths, best_mode, curv, step_size) return x_list, y_list, yaw_list, best_mode, best_cost def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1): gx = gx - sx gy = gy - sy l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2] le_xy = np.stack([gx, gy]).T @ l_rot le_yaw = gyaw - syaw lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin( le_xy[0], le_xy[1], le_yaw, curv, step_size) rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2] converted_xy = np.stack([lp_x, lp_y]).T @ rot x_list = converted_xy[:, 0] + sx y_list = converted_xy[:, 1] + sy yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw] return PATH(lengths, mode, x_list, y_list, yaw_list) def main(): # choose states pairs: (s, y, yaw) # simulation-1 states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120), (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)] # simulation-2 # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), # (35, 10, 180), (32, -10, 180), (5, -12, 90)] max_c = 0.25 # max curvature path_x, path_y, yaw = [], [], [] for i in range(len(states) - 1): s_x = states[i][0] s_y = states[i][1] s_yaw = np.deg2rad(states[i][2]) g_x = states[i + 1][0] g_y = states[i + 1][1] g_yaw = np.deg2rad(states[i + 1][2]) path_i = calc_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c) for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw): path_x.append(x) path_y.append(y) yaw.append(iyaw) # animation plt.ion() plt.figure(1) for i in range(len(path_x)): plt.clf() plt.plot(path_x, path_y, linewidth=1, color='gray') for x, y, theta in states: draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet') draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3) plt.axis("equal") plt.title("Simulation of Dubins Path") plt.axis([-10, 42, -20, 20]) plt.draw() plt.pause(0.001) plt.pause(1) if __name__ == '__main__': main() ================================================ FILE: CurvesGenerator/quartic_polynomial.py ================================================ """ Quartic Polynomial """ import numpy as np class QuarticPolynomial: def __init__(self, x0, v0, a0, v1, a1, T): A = np.array([[3 * T ** 2, 4 * T ** 3], [6 * T, 12 * T ** 2]]) b = np.array([v1 - v0 - a0 * T, a1 - a0]) X = np.linalg.solve(A, b) self.a0 = x0 self.a1 = v0 self.a2 = a0 / 2.0 self.a3 = X[0] self.a4 = X[1] def calc_xt(self, t): xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ self.a3 * t ** 3 + self.a4 * t ** 4 return xt def calc_dxt(self, t): xt = self.a1 + 2 * self.a2 * t + \ 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 return xt def calc_ddxt(self, t): xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 return xt def calc_dddxt(self, t): xt = 6 * self.a3 + 24 * self.a4 * t return xt ================================================ FILE: CurvesGenerator/quintic_polynomial.py ================================================ """ Quintic Polynomial """ import math import numpy as np import matplotlib.pyplot as plt import draw class QuinticPolynomial: def __init__(self, x0, v0, a0, x1, v1, a1, T): A = np.array([[T ** 3, T ** 4, T ** 5], [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], [6 * T, 12 * T ** 2, 20 * T ** 3]]) b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2, v1 - v0 - a0 * T, a1 - a0]) X = np.linalg.solve(A, b) self.a0 = x0 self.a1 = v0 self.a2 = a0 / 2.0 self.a3 = X[0] self.a4 = X[1] self.a5 = X[2] def calc_xt(self, t): xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 return xt def calc_dxt(self, t): dxt = self.a1 + 2 * self.a2 * t + \ 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 return dxt def calc_ddxt(self, t): ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 return ddxt def calc_dddxt(self, t): dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 return dddxt class Trajectory: def __init__(self): self.t = [] self.x = [] self.y = [] self.yaw = [] self.v = [] self.a = [] self.jerk = [] def simulation(): sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1 gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1 MAX_ACCEL = 1.0 # max accel [m/s2] MAX_JERK = 0.5 # max jerk [m/s3] dt = 0.1 # T tick [s] MIN_T = 5 MAX_T = 100 T_STEP = 5 sv_x = sv * math.cos(syaw) sv_y = sv * math.sin(syaw) gv_x = gv * math.cos(gyaw) gv_y = gv * math.sin(gyaw) sa_x = sa * math.cos(syaw) sa_y = sa * math.sin(syaw) ga_x = ga * math.cos(gyaw) ga_y = ga * math.sin(gyaw) path = Trajectory() for T in np.arange(MIN_T, MAX_T, T_STEP): path = Trajectory() xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T) yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T) for t in np.arange(0.0, T + dt, dt): path.t.append(t) path.x.append(xqp.calc_xt(t)) path.y.append(yqp.calc_xt(t)) vx = xqp.calc_dxt(t) vy = yqp.calc_dxt(t) path.v.append(np.hypot(vx, vy)) path.yaw.append(math.atan2(vy, vx)) ax = xqp.calc_ddxt(t) ay = yqp.calc_ddxt(t) a = np.hypot(ax, ay) if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0: a *= -1 path.a.append(a) jx = xqp.calc_dddxt(t) jy = yqp.calc_dddxt(t) j = np.hypot(jx, jy) if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0: j *= -1 path.jerk.append(j) if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK: break print("t_len: ", path.t, "s") print("max_v: ", max(path.v), "m/s") print("max_a: ", max(np.abs(path.a)), "m/s2") print("max_jerk: ", max(np.abs(path.jerk)), "m/s3") for i in range(len(path.t)): plt.cla() plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") plt.plot(path.x, path.y, linewidth=2, color='gray') draw.Car(sx, sy, syaw, 1.5, 3) draw.Car(gx, gy, gyaw, 1.5, 3) draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3) plt.title("Quintic Polynomial Curves") plt.grid(True) plt.pause(0.001) plt.show() if __name__ == '__main__': simulation() ================================================ FILE: CurvesGenerator/reeds_shepp.py ================================================ import math import numpy as np import matplotlib.pyplot as plt import draw # parameters initiation STEP_SIZE = 0.2 MAX_LENGTH = 1000.0 PI = math.pi # class for PATH element class PATH: def __init__(self, lengths, ctypes, L, x, y, yaw, directions): self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float] self.ctypes = ctypes # type of each part of the path [string] self.L = L # total path length [float] self.x = x # final s positions [m] self.y = y # final y positions [m] self.yaw = yaw # final yaw angles [rad] self.directions = directions # forward: 1, backward:-1 def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size) minL = paths[0].L mini = 0 for i in range(len(paths)): if paths[i].L <= minL: minL, mini = paths[i].L, i return paths[mini] def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): q0 = [sx, sy, syaw] q1 = [gx, gy, gyaw] paths = generate_path(q0, q1, maxc) for path in paths: x, y, yaw, directions = \ generate_local_course(path.L, path.lengths, path.ctypes, maxc, step_size * maxc) # convert global coordinate path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)] path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)] path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw] path.directions = directions path.lengths = [l / maxc for l in path.lengths] path.L = path.L / maxc return paths def set_path(paths, lengths, ctypes): path = PATH([], [], 0.0, [], [], [], []) path.ctypes = ctypes path.lengths = lengths # check same path exist for path_e in paths: if path_e.ctypes == path.ctypes: if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01: return paths # not insert path path.L = sum([abs(i) for i in lengths]) if path.L >= MAX_LENGTH: return paths assert path.L >= 0.01 paths.append(path) return paths def LSL(x, y, phi): u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) if t >= 0.0: v = M(phi - t) if v >= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def LSR(x, y, phi): u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi)) u1 = u1 ** 2 if u1 >= 4.0: u = math.sqrt(u1 - 4.0) theta = math.atan2(2.0, u) t = M(t1 + theta) v = M(t - phi) if t >= 0.0 and v >= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def LRL(x, y, phi): u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) if u1 <= 4.0: u = -2.0 * math.asin(0.25 * u1) t = M(t1 + 0.5 * u + PI) v = M(phi - t + u) if t >= 0.0 and u <= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def SCS(x, y, phi, paths): flag, t, u, v = SLS(x, y, phi) if flag: paths = set_path(paths, [t, u, v], ["S", "L", "S"]) flag, t, u, v = SLS(x, -y, -phi) if flag: paths = set_path(paths, [t, u, v], ["S", "R", "S"]) return paths def SLS(x, y, phi): phi = M(phi) if y > 0.0 and 0.0 < phi < PI * 0.99: xd = -y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) return True, t, u, v elif y < 0.0 and 0.0 < phi < PI * 0.99: xd = -y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) return True, t, u, v return False, 0.0, 0.0, 0.0 def CSC(x, y, phi, paths): flag, t, u, v = LSL(x, y, phi) if flag: paths = set_path(paths, [t, u, v], ["L", "S", "L"]) flag, t, u, v = LSL(-x, y, -phi) if flag: paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"]) flag, t, u, v = LSL(x, -y, -phi) if flag: paths = set_path(paths, [t, u, v], ["R", "S", "R"]) flag, t, u, v = LSL(-x, -y, phi) if flag: paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"]) flag, t, u, v = LSR(x, y, phi) if flag: paths = set_path(paths, [t, u, v], ["L", "S", "R"]) flag, t, u, v = LSR(-x, y, -phi) if flag: paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"]) flag, t, u, v = LSR(x, -y, -phi) if flag: paths = set_path(paths, [t, u, v], ["R", "S", "L"]) flag, t, u, v = LSR(-x, -y, phi) if flag: paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"]) return paths def CCC(x, y, phi, paths): flag, t, u, v = LRL(x, y, phi) if flag: paths = set_path(paths, [t, u, v], ["L", "R", "L"]) flag, t, u, v = LRL(-x, y, -phi) if flag: paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"]) flag, t, u, v = LRL(x, -y, -phi) if flag: paths = set_path(paths, [t, u, v], ["R", "L", "R"]) flag, t, u, v = LRL(-x, -y, phi) if flag: paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"]) # backwards xb = x * math.cos(phi) + y * math.sin(phi) yb = x * math.sin(phi) - y * math.cos(phi) flag, t, u, v = LRL(xb, yb, phi) if flag: paths = set_path(paths, [v, u, t], ["L", "R", "L"]) flag, t, u, v = LRL(-xb, yb, -phi) if flag: paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"]) flag, t, u, v = LRL(xb, -yb, -phi) if flag: paths = set_path(paths, [v, u, t], ["R", "L", "R"]) flag, t, u, v = LRL(-xb, -yb, phi) if flag: paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"]) return paths def calc_tauOmega(u, v, xi, eta, phi): delta = M(u - v) A = math.sin(u) - math.sin(delta) B = math.cos(u) - math.cos(delta) - 1.0 t1 = math.atan2(eta * A - xi * B, xi * A + eta * B) t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0 if t2 < 0: tau = M(t1 + PI) else: tau = M(t1) omega = M(tau - u + v - phi) return tau, omega def LRLRn(x, y, phi): xi = x + math.sin(phi) eta = y - 1.0 - math.cos(phi) rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta)) if rho <= 1.0: u = math.acos(rho) t, v = calc_tauOmega(u, -u, xi, eta, phi) if t >= 0.0 and v <= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def LRLRp(x, y, phi): xi = x + math.sin(phi) eta = y - 1.0 - math.cos(phi) rho = (20.0 - xi * xi - eta * eta) / 16.0 if 0.0 <= rho <= 1.0: u = -math.acos(rho) if u >= -0.5 * PI: t, v = calc_tauOmega(u, u, xi, eta, phi) if t >= 0.0 and v >= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def CCCC(x, y, phi, paths): flag, t, u, v = LRLRn(x, y, phi) if flag: paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"]) flag, t, u, v = LRLRn(-x, y, -phi) if flag: paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"]) flag, t, u, v = LRLRn(x, -y, -phi) if flag: paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"]) flag, t, u, v = LRLRn(-x, -y, phi) if flag: paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"]) flag, t, u, v = LRLRp(x, y, phi) if flag: paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"]) flag, t, u, v = LRLRp(-x, y, -phi) if flag: paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"]) flag, t, u, v = LRLRp(x, -y, -phi) if flag: paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"]) flag, t, u, v = LRLRp(-x, -y, phi) if flag: paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"]) return paths def LRSR(x, y, phi): xi = x + math.sin(phi) eta = y - 1.0 - math.cos(phi) rho, theta = R(-eta, xi) if rho >= 2.0: t = theta u = 2.0 - rho v = M(t + 0.5 * PI - phi) if t >= 0.0 and u <= 0.0 and v <= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def LRSL(x, y, phi): xi = x - math.sin(phi) eta = y - 1.0 + math.cos(phi) rho, theta = R(xi, eta) if rho >= 2.0: r = math.sqrt(rho * rho - 4.0) u = 2.0 - r t = M(theta + math.atan2(r, -2.0)) v = M(phi - 0.5 * PI - t) if t >= 0.0 and u <= 0.0 and v <= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def CCSC(x, y, phi, paths): flag, t, u, v = LRSL(x, y, phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"]) flag, t, u, v = LRSL(-x, y, -phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"]) flag, t, u, v = LRSL(x, -y, -phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"]) flag, t, u, v = LRSL(-x, -y, phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"]) flag, t, u, v = LRSR(x, y, phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"]) flag, t, u, v = LRSR(-x, y, -phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"]) flag, t, u, v = LRSR(x, -y, -phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"]) flag, t, u, v = LRSR(-x, -y, phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"]) # backwards xb = x * math.cos(phi) + y * math.sin(phi) yb = x * math.sin(phi) - y * math.cos(phi) flag, t, u, v = LRSL(xb, yb, phi) if flag: paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"]) flag, t, u, v = LRSL(-xb, yb, -phi) if flag: paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"]) flag, t, u, v = LRSL(xb, -yb, -phi) if flag: paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"]) flag, t, u, v = LRSL(-xb, -yb, phi) if flag: paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"]) flag, t, u, v = LRSR(xb, yb, phi) if flag: paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"]) flag, t, u, v = LRSR(-xb, yb, -phi) if flag: paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"]) flag, t, u, v = LRSR(xb, -yb, -phi) if flag: paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"]) flag, t, u, v = LRSR(-xb, -yb, phi) if flag: paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"]) return paths def LRSLR(x, y, phi): # formula 8.11 *** TYPO IN PAPER *** xi = x + math.sin(phi) eta = y - 1.0 - math.cos(phi) rho, theta = R(xi, eta) if rho >= 2.0: u = 4.0 - math.sqrt(rho * rho - 4.0) if u <= 0.0: t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta)) v = M(t - phi) if t >= 0.0 and v >= 0.0: return True, t, u, v return False, 0.0, 0.0, 0.0 def CCSCC(x, y, phi, paths): flag, t, u, v = LRSLR(x, y, phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"]) flag, t, u, v = LRSLR(-x, y, -phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"]) flag, t, u, v = LRSLR(x, -y, -phi) if flag: paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"]) flag, t, u, v = LRSLR(-x, -y, phi) if flag: paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"]) return paths def generate_local_course(L, lengths, mode, maxc, step_size): point_num = int(L / step_size) + len(lengths) + 3 px = [0.0 for _ in range(point_num)] py = [0.0 for _ in range(point_num)] pyaw = [0.0 for _ in range(point_num)] directions = [0 for _ in range(point_num)] ind = 1 if lengths[0] > 0.0: directions[0] = 1 else: directions[0] = -1 if lengths[0] > 0.0: d = step_size else: d = -step_size pd = d ll = 0.0 for m, l, i in zip(mode, lengths, range(len(mode))): if l > 0.0: d = step_size else: d = -step_size ox, oy, oyaw = px[ind], py[ind], pyaw[ind] ind -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: pd = -d - ll else: pd = d - ll while abs(pd) <= abs(l): ind += 1 px, py, pyaw, directions = \ interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) pd += d ll = l - pd - d # calc remain length ind += 1 px, py, pyaw, directions = \ interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) # remove unused data while px[-1] == 0.0: px.pop() py.pop() pyaw.pop() directions.pop() return px, py, pyaw, directions def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions): if m == "S": px[ind] = ox + l / maxc * math.cos(oyaw) py[ind] = oy + l / maxc * math.sin(oyaw) pyaw[ind] = oyaw else: ldx = math.sin(l) / maxc if m == "L": ldy = (1.0 - math.cos(l)) / maxc elif m == "R": ldy = (1.0 - math.cos(l)) / (-maxc) gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy px[ind] = ox + gdx py[ind] = oy + gdy if m == "L": pyaw[ind] = oyaw + l elif m == "R": pyaw[ind] = oyaw - l if l > 0.0: directions[ind] = 1 else: directions[ind] = -1 return px, py, pyaw, directions def generate_path(q0, q1, maxc): dx = q1[0] - q0[0] dy = q1[1] - q0[1] dth = q1[2] - q0[2] c = math.cos(q0[2]) s = math.sin(q0[2]) x = (c * dx + s * dy) * maxc y = (-s * dx + c * dy) * maxc paths = [] paths = SCS(x, y, dth, paths) paths = CSC(x, y, dth, paths) paths = CCC(x, y, dth, paths) paths = CCCC(x, y, dth, paths) paths = CCSC(x, y, dth, paths) paths = CCSCC(x, y, dth, paths) return paths # utils def pi_2_pi(theta): while theta > PI: theta -= 2.0 * PI while theta < -PI: theta += 2.0 * PI return theta def R(x, y): """ Return the polar coordinates (r, theta) of the point (s, y) """ r = math.hypot(x, y) theta = math.atan2(y, x) return r, theta def M(theta): """ Regulate theta to -pi <= theta < pi """ phi = theta % (2.0 * PI) if phi < -PI: phi += 2.0 * PI if phi > PI: phi -= 2.0 * PI return phi def get_label(path): label = "" for m, l in zip(path.ctypes, path.lengths): label = label + m if l > 0.0: label = label + "+" else: label = label + "-" return label def calc_curvature(x, y, yaw, directions): c, ds = [], [] for i in range(1, len(x) - 1): dxn = x[i] - x[i - 1] dxp = x[i + 1] - x[i] dyn = y[i] - y[i - 1] dyp = y[i + 1] - y[i] dn = math.hypot(dxn, dyn) dp = math.hypot(dxp, dyp) dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp) ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn) dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp) ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn) curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) d = (dn + dp) / 2.0 if np.isnan(curvature): curvature = 0.0 if directions[i] <= 0.0: curvature = -curvature if len(c) == 0: ds.append(d) c.append(curvature) ds.append(d) c.append(curvature) ds.append(ds[-1]) c.append(c[-1]) return c, ds def check_path(sx, sy, syaw, gx, gy, gyaw, maxc): paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc) assert len(paths) >= 1 for path in paths: assert abs(path.x[0] - sx) <= 0.01 assert abs(path.y[0] - sy) <= 0.01 assert abs(path.yaw[0] - syaw) <= 0.01 assert abs(path.x[-1] - gx) <= 0.01 assert abs(path.y[-1] - gy) <= 0.01 assert abs(path.yaw[-1] - gyaw) <= 0.01 # course distance check d = [math.hypot(dx, dy) for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]), np.diff(path.y[0:len(path.y) - 1]))] for i in range(len(d)): assert abs(d[i] - STEP_SIZE) <= 0.001 def main(): # choose states pairs: (s, y, yaw) # simulation-1 # states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120), # (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)] # simulation-2 states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), (35, 10, 180), (32, -10, 180), (5, -12, 90)] max_c = 0.1 # max curvature path_x, path_y, yaw = [], [], [] for i in range(len(states) - 1): s_x = states[i][0] s_y = states[i][1] s_yaw = np.deg2rad(states[i][2]) g_x = states[i + 1][0] g_y = states[i + 1][1] g_yaw = np.deg2rad(states[i + 1][2]) path_i = calc_optimal_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c) path_x += path_i.x path_y += path_i.y yaw += path_i.yaw # animation plt.ion() plt.figure(1) for i in range(len(path_x)): plt.clf() plt.plot(path_x, path_y, linewidth=1, color='gray') for x, y, theta in states: draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet') draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3) plt.axis("equal") plt.title("Simulation of Reeds-Shepp Curves") plt.axis([-10, 42, -20, 20]) plt.draw() plt.pause(0.001) plt.pause(1) if __name__ == '__main__': main() ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2020 Huiming Zhou Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ Overview ------ This repository implements some common path planning algorithms used in robotics, including Search-based algorithms and Sampling-based algorithms. We designed animation for each algorithm to display the running process. The related papers are listed in [Papers](https://github.com/zhm-real/PathPlanning#papers). Directory Structure ------ . └── Search-based Planning ├── Breadth-First Searching (BFS) ├── Depth-First Searching (DFS) ├── Best-First Searching ├── Dijkstra's ├── A* ├── Bidirectional A* ├── Anytime Repairing A* ├── Learning Real-time A* (LRTA*) ├── Real-time Adaptive A* (RTAA*) ├── Lifelong Planning A* (LPA*) ├── Dynamic A* (D*) ├── D* Lite └── Anytime D* └── Sampling-based Planning ├── RRT ├── RRT-Connect ├── Extended-RRT ├── Dynamic-RRT ├── RRT* ├── Informed RRT* ├── RRT* Smart ├── Anytime RRT* ├── Closed-Loop RRT* ├── Spline-RRT* ├── Fast Marching Trees (FMT*) └── Batch Informed Trees (BIT*) └── Papers ## Animations - Search-Based ### Best-First & Dijkstra
dfs dijkstra
### A* and A* Variants
astar biastar
repeatedastar arastar
lrtastar rtaastar
lpastar dstarlite
lpastar dstarlite
## Animation - Sampling-Based ### RRT & Variants
value iteration value iteration
value iteration value iteration
value iteration value iteration
value iteration value iteration
value iteration value iteration
## Papers ### Search-base Planning * [A*: ](https://ieeexplore.ieee.org/document/4082128) A Formal Basis for the heuristic Determination of Minimum Cost Paths * [Learning Real-Time A*: ](https://arxiv.org/pdf/1110.4076.pdf) Learning in Real-Time Search: A Unifying Framework * [Real-Time Adaptive A*: ](http://idm-lab.org/bib/abstracts/papers/aamas06.pdf) Real-Time Adaptive A* * [Lifelong Planning A*: ](https://www.cs.cmu.edu/~maxim/files/aij04.pdf) Lifelong Planning A* * [Anytime Repairing A*: ](https://papers.nips.cc/paper/2382-ara-anytime-a-with-provable-bounds-on-sub-optimality.pdf) ARA*: Anytime A* with Provable Bounds on Sub-Optimality * [D*: ](http://web.mit.edu/16.412j/www/html/papers/original_dstar_icra94.pdf) Optimal and Efficient Path Planning for Partially-Known Environments * [D* Lite: ](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf) D* Lite * [Field D*: ](http://robots.stanford.edu/isrr-papers/draft/stentz.pdf) Field D*: An Interpolation-based Path Planner and Replanner * [Anytime D*: ](http://www.cs.cmu.edu/~ggordon/likhachev-etal.anytime-dstar.pdf) Anytime Dynamic A*: An Anytime, Replanning Algorithm * [Focussed D*: ](http://robotics.caltech.edu/~jwb/courses/ME132/handouts/Dstar_ijcai95.pdf) The Focussed D* Algorithm for Real-Time Replanning * [Potential Field, ](https://journals.sagepub.com/doi/abs/10.1177/027836498600500106) [[PPT]: ](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots * [Hybrid A*: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving ### Sampling-based Planning * [RRT: ](http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf) Rapidly-Exploring Random Trees: A New Tool for Path Planning * [RRT-Connect: ](http://www-cgi.cs.cmu.edu/afs/cs/academic/class/15494-s12/readings/kuffner_icra2000.pdf) RRT-Connect: An Efficient Approach to Single-Query Path Planning * [Extended-RRT: ](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.1.7617&rep=rep1&type=pdf) Real-Time Randomized Path Planning for Robot Navigation * [Dynamic-RRT: ](https://www.ri.cmu.edu/pub_files/pub4/ferguson_david_2006_2/ferguson_david_2006_2.pdf) Replanning with RRTs * [RRT*: ](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761) Sampling-based algorithms for optimal motion planning * [Anytime-RRT*: ](https://dspace.mit.edu/handle/1721.1/63170) Anytime Motion Planning using the RRT* * [Closed-loop RRT* (CL-RRT*): ](http://acl.mit.edu/papers/KuwataTCST09.pdf) Real-time Motion Planning with Applications to Autonomous Urban Driving * [Spline-RRT*: ](https://ieeexplore.ieee.org/abstract/document/6987895?casa_token=B9GUwVDbbncAAAAA:DWscGFLIa97ptgH7NpUQUL0A2ModiiBDBGklk1z7aDjI11Kyfzo8rpuFstdYcjOofJfCjR-mNw) Optimal path planning based on spline-RRT* for fixed-wing UAVs operating in three-dimensional environments * [LQR-RRT*: ](https://lis.csail.mit.edu/pubs/perez-icra12.pdf) Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics * [RRT#: ](http://dcsl.gatech.edu/papers/icra13.pdf) Use of Relaxation Methods in Sampling-Based Algorithms for Optimal Motion Planning * [RRT*-Smart: ](http://save.seecs.nust.edu.pk/pubs/ICMA2012.pdf) Rapid convergence implementation of RRT* towards optimal solution * [Informed RRT*: ](https://arxiv.org/abs/1404.2334) Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic * [Fast Marching Trees (FMT*): ](https://arxiv.org/abs/1306.3532) a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions * [Motion Planning using Lower Bounds (MPLB): ](https://ieeexplore.ieee.org/document/7139773) Asymptotically-optimal Motion Planning using lower bounds on cost * [Batch Informed Trees (BIT*): ](https://arxiv.org/abs/1405.5848) Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs * [Advanced Batch Informed Trees (ABIT*): ](https://arxiv.org/abs/2002.06589) Sampling-Based Planning with Advanced Graph-Search Techniques ((ICRA) 2020) * [Adaptively Informed Trees (AIT*): ](https://arxiv.org/abs/2002.06599) Fast Asymptotically Optimal Path Planning through Adaptive Heuristics ((ICRA) 2020) ================================================ FILE: Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py ================================================ ================================================ FILE: Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py ================================================ ================================================ FILE: Sampling_based_Planning/rrt_2D/batch_informed_trees.py ================================================ """ Batch Informed Trees (BIT*) @author: huiming zhou """ import os import sys import math 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/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None class Tree: def __init__(self, x_start, x_goal): self.x_start = x_start self.goal = x_goal self.r = 4.0 self.V = set() self.E = set() self.QE = set() self.QV = set() self.V_old = set() class BITStar: def __init__(self, x_start, x_goal, eta, iter_max): self.x_start = Node(x_start[0], x_start[1]) self.x_goal = Node(x_goal[0], x_goal[1]) self.eta = eta self.iter_max = iter_max self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.Tree = Tree(self.x_start, self.x_goal) self.X_sample = set() self.g_T = dict() def init(self): self.Tree.V.add(self.x_start) self.X_sample.add(self.x_goal) self.g_T[self.x_start] = 0.0 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) xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0], [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]]) return theta, cMin, xCenter, C def planning(self): theta, cMin, xCenter, C = self.init() for k in range(500): if not self.Tree.QE and not self.Tree.QV: if k == 0: m = 350 else: m = 200 if self.x_goal.parent is not None: path_x, path_y = self.ExtractPath() plt.plot(path_x, path_y, linewidth=2, color='r') plt.pause(0.5) self.Prune(self.g_T[self.x_goal]) self.X_sample.update(self.Sample(m, self.g_T[self.x_goal], cMin, xCenter, C)) self.Tree.V_old = {v for v in self.Tree.V} self.Tree.QV = {v for v in self.Tree.V} # self.Tree.r = self.radius(len(self.Tree.V) + len(self.X_sample)) while self.BestVertexQueueValue() <= self.BestEdgeQueueValue(): self.ExpandVertex(self.BestInVertexQueue()) vm, xm = self.BestInEdgeQueue() 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]: 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 edge_delete = set() for v, x in self.Tree.E: if x == xm: edge_delete.add((v, x)) for edge in edge_delete: self.Tree.E.remove(edge) else: self.X_sample.remove(xm) self.Tree.V.add(xm) self.Tree.QV.add(xm) self.g_T[xm] = self.g_T[vm] + actual_cost self.Tree.E.add((vm, xm)) xm.parent = vm set_delete = set() for v, x in self.Tree.QE: if x == xm and self.g_T[v] + self.calc_dist(v, xm) >= self.g_T[xm]: set_delete.add((v, x)) for edge in set_delete: self.Tree.QE.remove(edge) else: self.Tree.QE = set() self.Tree.QV = set() if k % 5 == 0: self.animation(xCenter, self.g_T[self.x_goal], cMin, theta) path_x, path_y = self.ExtractPath() plt.plot(path_x, path_y, linewidth=2, color='r') plt.pause(0.01) plt.show() def ExtractPath(self): node = self.x_goal path_x, path_y = [node.x], [node.y] while node.parent: node = node.parent path_x.append(node.x) path_y.append(node.y) return path_x, path_y 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): return np.inf return self.calc_dist(start, end) def f_estimated(self, node): return self.g_estimated(node) + self.h_estimated(node) def g_estimated(self, node): return self.calc_dist(self.x_start, node) 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} 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.g_T[x] = np.inf 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 \ 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)) if w not in self.g_T: self.g_T[w] = np.inf 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): if not self.Tree.QV: print("QV is Empty!") return None 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): if not self.Tree.QE: print("QE is Empty!") return None 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_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 U, _, V_T = np.linalg.svd(M, True, True) C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T return C @staticmethod def calc_dist(start, end): return math.hypot(start.x - end.x, start.y - end.y) @staticmethod def calc_dist_and_angle(node_start, node_end): 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, xCenter, cMax, cMin, theta): plt.cla() self.plot_grid("Batch Informed Trees (BIT*)") plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for v in self.X_sample: plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2') if cMax < np.inf: self.draw_ellipse(xCenter, cMax, cMin, theta) for v, w in self.Tree.E: plt.plot([v.x, w.x], [v.y, w.y], '-g') plt.pause(0.001) 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.2) 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.animation("Batch Informed Trees (BIT*)") bit.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/dubins_rrt_star.py ================================================ """ DUBINS_RRT_STAR 2D @author: huiming zhou """ import os import sys import math 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/") from Sampling_based_Planning.rrt_2D import env, plotting, utils import CurvesGenerator.dubins_path as dubins import CurvesGenerator.draw as draw class Node: def __init__(self, x, y, yaw): self.x = x self.y = y self.yaw = yaw self.parent = None self.cost = 0.0 self.path_x = [] self.path_y = [] self.paty_yaw = [] class DubinsRRTStar: def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len, goal_sample_rate, search_radius, iter_max): self.s_start = Node(sx, sy, syaw) self.s_goal = Node(gx, gy, gyaw) self.vr = vehicle_radius self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius self.iter_max = iter_max self.curv = 1 self.env = env.Env() self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.obs_circle() self.obs_boundary = self.env.obs_boundary self.utils.update_obs(self.obs_circle, self.obs_boundary, []) self.V = [self.s_start] self.path = None def planning(self): for i in range(self.iter_max): print("Iter:", i, ", number of nodes:", len(self.V)) rnd = self.Sample() node_nearest = self.Nearest(self.V, rnd) new_node = self.Steer(node_nearest, rnd) if new_node and not self.is_collision(new_node): near_indexes = self.Near(self.V, new_node) new_node = self.choose_parent(new_node, near_indexes) if new_node: self.V.append(new_node) self.rewire(new_node, near_indexes) if i % 5 == 0: self.draw_graph() last_index = self.search_best_goal_node() path = self.generate_final_course(last_index) print("get!") px = [s[0] for s in path] py = [s[1] for s in path] plt.plot(px, py, '-r') plt.pause(0.01) plt.show() def draw_graph(self, rnd=None): plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for node in self.V: if node.parent: plt.plot(node.path_x, node.path_y, "-g") self.plot_grid("dubins rrt*") plt.plot(self.s_start.x, self.s_start.y, "xr") plt.plot(self.s_goal.x, self.s_goal.y, "xr") plt.grid(True) self.plot_start_goal_arrow() plt.pause(0.01) def plot_start_goal_arrow(self): draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2, "darkorange") draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2, "darkorange") def generate_final_course(self, goal_index): print("final") path = [[self.s_goal.x, self.s_goal.y]] node = self.V[goal_index] while node.parent: for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)): path.append([ix, iy]) node = node.parent path.append([self.s_start.x, self.s_start.y]) return path def calc_dist_to_goal(self, x, y): dx = x - self.s_goal.x dy = y - self.s_goal.y return math.hypot(dx, dy) def search_best_goal_node(self): dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.V] goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.step_len] safe_goal_inds = [] for goal_ind in goal_inds: t_node = self.Steer(self.V[goal_ind], self.s_goal) if t_node and not self.is_collision(t_node): safe_goal_inds.append(goal_ind) if not safe_goal_inds: return None min_cost = min([self.V[i].cost for i in safe_goal_inds]) for i in safe_goal_inds: if self.V[i].cost == min_cost: return i return None def rewire(self, new_node, near_inds): for i in near_inds: near_node = self.V[i] edge_node = self.Steer(new_node, near_node) if not edge_node: continue edge_node.cost = self.calc_new_cost(new_node, near_node) no_collision = ~self.is_collision(edge_node) improved_cost = near_node.cost > edge_node.cost if no_collision and improved_cost: self.V[i] = edge_node self.propagate_cost_to_leaves(new_node) def choose_parent(self, new_node, near_inds): if not near_inds: return None costs = [] for i in near_inds: near_node = self.V[i] t_node = self.Steer(near_node, new_node) if t_node and not self.is_collision(t_node): costs.append(self.calc_new_cost(near_node, new_node)) else: costs.append(float("inf")) # the cost of collision node min_cost = min(costs) if min_cost == float("inf"): print("There is no good path.(min_cost is inf)") return None min_ind = near_inds[costs.index(min_cost)] new_node = self.Steer(self.V[min_ind], new_node) return new_node def calc_new_cost(self, from_node, to_node): d, _ = self.get_distance_and_angle(from_node, to_node) return from_node.cost + d def propagate_cost_to_leaves(self, parent_node): for node in self.V: if node.parent == parent_node: node.cost = self.calc_new_cost(parent_node, node) self.propagate_cost_to_leaves(node) @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) def Near(self, nodelist, node): n = len(nodelist) + 1 r = min(self.search_radius * math.sqrt((math.log(n)) / n), self.step_len) dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist] node_near_ind = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2] return node_near_ind def Steer(self, node_start, node_end): sx, sy, syaw = node_start.x, node_start.y, node_start.yaw gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw maxc = self.curv path = dubins.calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, maxc) if len(path.x) <= 1: return None node_new = Node(path.x[-1], path.y[-1], path.yaw[-1]) node_new.path_x = path.x node_new.path_y = path.y node_new.path_yaw = path.yaw node_new.cost = node_start.cost + path.L node_new.parent = node_start return node_new def Sample(self): delta = self.utils.delta if random.random() > self.goal_sample_rate: return 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), random.uniform(-math.pi, math.pi)) else: return self.s_goal @staticmethod def Nearest(nodelist, n): return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 for nd in nodelist]))] def is_collision(self, node): for ox, oy, r in self.obs_circle: dx = [ox - x for x in node.path_x] dy = [oy - y for y in node.path_y] dist = np.hypot(dx, dy) if min(dist) < r + self.delta: return True return False def animation(self): self.plot_grid("dubins rrt*") self.plot_arrow() plt.show() def plot_arrow(self): draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2.5, "darkorange") draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2.5, "darkorange") 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, r) in self.obs_circle: self.ax.add_patch( patches.Circle( (ox, oy), r, edgecolor='black', facecolor='gray', fill=True ) ) plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3) plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) plt.title(name) plt.axis("equal") @staticmethod def obs_circle(): obs_cir = [ [10, 10, 3], [15, 22, 3], [22, 8, 2.5], [26, 16, 2], [37, 10, 3], [37, 23, 3], [45, 15, 2] ] return obs_cir def main(): sx, sy, syaw = 5, 5, np.deg2rad(90) gx, gy, gyaw = 45, 25, np.deg2rad(0) goal_sample_rate = 0.1 search_radius = 50.0 step_len = 30.0 iter_max = 250 vehicle_radius = 2.0 drrtstar = DubinsRRTStar(sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len, goal_sample_rate, search_radius, iter_max) drrtstar.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/dynamic_rrt.py ================================================ """ DYNAMIC_RRT_2D @author: huiming zhou """ import os import sys import math import copy import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None self.flag = "VALID" class Edge: def __init__(self, n_p, n_c): self.parent = n_p self.child = n_c self.flag = "VALID" class DynamicRrt: def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max): self.s_start = Node(s_start) self.s_goal = Node(s_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.waypoint_sample_rate = waypoint_sample_rate self.iter_max = iter_max self.vertex = [self.s_start] self.vertex_old = [] self.vertex_new = [] self.edges = [] self.env = env.Env() self.plotting = plotting.Plotting(s_start, s_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.obs_add = [0, 0, 0] self.path = [] self.waypoint = [] def planning(self): for i in range(self.iter_max): node_rand = self.generate_random_node(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) self.edges.append(Edge(node_near, node_new)) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) self.plot_grid("Dynamic_RRT") self.plot_visited() self.plot_path(path) self.path = path self.waypoint = self.extract_waypoint(node_new) self.fig.canvas.mpl_connect('button_press_event', self.on_press) plt.show() return return None def on_press(self, event): x, y = event.xdata, event.ydata if x < 0 or x > 50 or y < 0 or y > 30: print("Please choose right area!") else: x, y = int(x), int(y) print("Add circle obstacle at: s =", x, ",", "y =", y) self.obs_add = [x, y, 2] self.obs_circle.append([x, y, 2]) self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle) self.InvalidateNodes() if self.is_path_invalid(): print("Path is Replanning ...") path, waypoint = self.replanning() print("len_vertex: ", len(self.vertex)) print("len_vertex_old: ", len(self.vertex_old)) print("len_vertex_new: ", len(self.vertex_new)) plt.cla() self.plot_grid("Dynamic_RRT") self.plot_vertex_old() self.plot_path(self.path, color='blue') self.plot_vertex_new() self.vertex_new = [] self.plot_path(path) self.path = path self.waypoint = waypoint else: print("Trimming Invalid Nodes ...") self.TrimRRT() plt.cla() self.plot_grid("Dynamic_RRT") self.plot_visited(animation=False) self.plot_path(self.path) self.fig.canvas.draw_idle() def InvalidateNodes(self): for edge in self.edges: if self.is_collision_obs_add(edge.parent, edge.child): edge.child.flag = "INVALID" def is_path_invalid(self): for node in self.waypoint: if node.flag == "INVALID": return True def is_collision_obs_add(self, start, end): delta = self.utils.delta obs_add = self.obs_add if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta: return True if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta: return True o, d = self.utils.get_ray(start, end) if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]): return True return False def replanning(self): self.TrimRRT() for i in range(self.iter_max): node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) self.vertex_new.append(node_new) self.edges.append(Edge(node_near, node_new)) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) waypoint = self.extract_waypoint(node_new) print("path: ", len(path)) print("waypoint: ", len(waypoint)) return path, waypoint return None def TrimRRT(self): for i in range(1, len(self.vertex)): node = self.vertex[i] node_p = node.parent if node_p.flag == "INVALID": node.flag = "INVALID" self.vertex = [node for node in self.vertex if node.flag == "VALID"] self.vertex_old = copy.deepcopy(self.vertex) self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]] def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.s_goal def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate): delta = self.utils.delta p = np.random.random() if p < goal_sample_rate: return self.s_goal elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate: return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)] else: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) for nd in node_list]))] def new_state(self, node_start, node_end): dist, theta = self.get_distance_and_angle(node_start, node_end) dist = min(self.step_len, dist) node_new = Node((node_start.x + dist * math.cos(theta), node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new def extract_path(self, node_end): path = [(self.s_goal.x, self.s_goal.y)] node_now = node_end while node_now.parent is not None: node_now = node_now.parent path.append((node_now.x, node_now.y)) return path def extract_waypoint(self, node_end): waypoint = [self.s_goal] node_now = node_end while node_now.parent is not None: node_now = node_now.parent waypoint.append(node_now) return waypoint @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) 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.s_start.x, self.s_start.y, "bs", linewidth=3) plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) plt.title(name) plt.axis("equal") def plot_visited(self, animation=True): if animation: count = 0 for node in self.vertex: count += 1 if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count % 10 == 0: plt.pause(0.001) else: for node in self.vertex: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") def plot_vertex_old(self): for node in self.vertex_old: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") def plot_vertex_new(self): count = 0 for node in self.vertex_new: count += 1 if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange') plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count % 10 == 0: plt.pause(0.001) @staticmethod def plot_path(path, color='red'): plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color) plt.pause(0.01) def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node drrt = DynamicRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000) drrt.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/env.py ================================================ """ Environment for rrt_2D @author: huiming zhou """ class Env: def __init__(self): self.x_range = (0, 50) self.y_range = (0, 30) self.obs_boundary = self.obs_boundary() self.obs_circle = self.obs_circle() self.obs_rectangle = self.obs_rectangle() @staticmethod def obs_boundary(): obs_boundary = [ [0, 0, 1, 30], [0, 30, 50, 1], [1, 0, 50, 1], [50, 1, 1, 30] ] return obs_boundary @staticmethod def obs_rectangle(): obs_rectangle = [ [14, 12, 8, 2], [18, 22, 8, 3], [26, 7, 2, 12], [32, 14, 10, 2] ] return obs_rectangle @staticmethod def obs_circle(): obs_cir = [ [7, 12, 3], [46, 20, 2], [15, 5, 2], [37, 7, 3], [37, 23, 3] ] return obs_cir ================================================ FILE: Sampling_based_Planning/rrt_2D/extended_rrt.py ================================================ """ EXTENDED_RRT_2D @author: huiming zhou """ import os import sys import math import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class ExtendedRrt: def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max): self.s_start = Node(s_start) self.s_goal = Node(s_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.waypoint_sample_rate = waypoint_sample_rate self.iter_max = iter_max self.vertex = [self.s_start] self.env = env.Env() self.plotting = plotting.Plotting(s_start, s_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.path = [] self.waypoint = [] def planning(self): for i in range(self.iter_max): node_rand = self.generate_random_node(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) self.plot_grid("Extended_RRT") self.plot_visited() self.plot_path(path) self.path = path self.waypoint = self.extract_waypoint(node_new) self.fig.canvas.mpl_connect('button_press_event', self.on_press) plt.show() return return None def on_press(self, event): x, y = event.xdata, event.ydata if x < 0 or x > 50 or y < 0 or y > 30: print("Please choose right area!") else: x, y = int(x), int(y) print("Add circle obstacle at: s =", x, ",", "y =", y) self.obs_circle.append([x, y, 2]) self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle) path, waypoint = self.replanning() plt.cla() self.plot_grid("Extended_RRT") self.plot_path(self.path, color='blue') self.plot_visited() self.plot_path(path) self.path = path self.waypoint = waypoint self.fig.canvas.draw_idle() def replanning(self): self.vertex = [self.s_start] for i in range(self.iter_max): node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len: self.new_state(node_new, self.s_goal) path = self.extract_path(node_new) waypoint = self.extract_waypoint(node_new) return path, waypoint return None def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.s_goal def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate): delta = self.utils.delta p = np.random.random() if p < goal_sample_rate: return self.s_goal elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate: return self.waypoint[np.random.randint(0, len(self.path) - 1)] else: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) for nd in node_list]))] def new_state(self, node_start, node_end): dist, theta = self.get_distance_and_angle(node_start, node_end) dist = min(self.step_len, dist) node_new = Node((node_start.x + dist * math.cos(theta), node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new def extract_path(self, node_end): path = [(self.s_goal.x, self.s_goal.y)] node_now = node_end while node_now.parent is not None: node_now = node_now.parent path.append((node_now.x, node_now.y)) return path def extract_waypoint(self, node_end): waypoint = [self.s_goal] node_now = node_end while node_now.parent is not None: node_now = node_now.parent waypoint.append(node_now) return waypoint @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) 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.s_start.x, self.s_start.y, "bs", linewidth=3) plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3) plt.title(name) plt.axis("equal") def plot_visited(self): animation = True if animation: count = 0 for node in self.vertex: count += 1 if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count % 10 == 0: plt.pause(0.001) else: for node in self.vertex: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") @staticmethod def plot_path(path, color='red'): plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color) plt.pause(0.01) def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node errt = ExtendedRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000) errt.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/fast_marching_trees.py ================================================ """ Fast Marching Trees (FMT*) @author: huiming zhou """ import os import sys import math import random import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None self.cost = np.inf class FMT: def __init__(self, x_start, x_goal, search_radius): self.x_init = Node(x_start) self.x_goal = Node(x_goal) self.search_radius = search_radius self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.V = set() self.V_unvisited = set() self.V_open = set() self.V_closed = set() self.sample_numbers = 1000 def Init(self): samples = self.SampleFree() self.x_init.cost = 0.0 self.V.add(self.x_init) self.V.update(samples) self.V_unvisited.update(samples) self.V_unvisited.add(self.x_goal) self.V_open.add(self.x_init) def Planning(self): self.Init() z = self.x_init n = self.sample_numbers rn = self.search_radius * math.sqrt((math.log(n) / n)) Visited = [] while z is not self.x_goal: V_open_new = set() X_near = self.Near(self.V_unvisited, z, rn) Visited.append(z) for x in X_near: Y_near = self.Near(self.V_open, x, rn) cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near} y_min = min(cost_list, key=cost_list.get) if not self.utils.is_collision(y_min, x): x.parent = y_min V_open_new.add(x) self.V_unvisited.remove(x) x.cost = y_min.cost + self.Cost(y_min, x) self.V_open.update(V_open_new) self.V_open.remove(z) self.V_closed.add(z) if not self.V_open: print("open set empty!") break cost_open = {y: y.cost for y in self.V_open} z = min(cost_open, key=cost_open.get) # node_end = self.ChooseGoalPoint() path_x, path_y = self.ExtractPath() self.animation(path_x, path_y, Visited[1: len(Visited)]) def ChooseGoalPoint(self): Near = self.Near(self.V, self.x_goal, 2.0) cost = {y: y.cost + self.Cost(y, self.x_goal) for y in Near} return min(cost, key=cost.get) def ExtractPath(self): path_x, path_y = [], [] node = self.x_goal while node.parent: path_x.append(node.x) path_y.append(node.y) node = node.parent path_x.append(self.x_init.x) path_y.append(self.x_init.y) return path_x, path_y def Cost(self, x_start, x_end): if self.utils.is_collision(x_start, x_end): return np.inf else: return self.calc_dist(x_start, x_end) @staticmethod def calc_dist(x_start, x_end): return math.hypot(x_start.x - x_end.x, x_start.y - x_end.y) @staticmethod def Near(nodelist, z, rn): return {nd for nd in nodelist if 0 < (nd.x - z.x) ** 2 + (nd.y - z.y) ** 2 <= rn ** 2} def SampleFree(self): n = self.sample_numbers delta = self.utils.delta Sample = set() ind = 0 while ind < n: 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 animation(self, path_x, path_y, visited): self.plot_grid("Fast Marching Trees (FMT*)") for node in self.V: plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3) count = 0 for node in visited: count += 1 plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g') plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count % 10 == 0: plt.pause(0.001) plt.plot(path_x, path_y, linewidth=2, color='red') plt.pause(0.01) 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_init.x, self.x_init.y, "bs", linewidth=3) plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3) plt.title(name) plt.axis("equal") def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node fmt = FMT(x_start, x_goal, 40) fmt.Planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/informed_rrt_star.py ================================================ """ INFORMED_RRT_STAR 2D @author: huiming zhou """ import os import sys import math import random import numpy as np import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as Rot import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class IRrtStar: def __init__(self, x_start, x_goal, step_len, goal_sample_rate, search_radius, iter_max): self.x_start = Node(x_start) self.x_goal = Node(x_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius self.iter_max = iter_max self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.V = [self.x_start] self.X_soln = set() self.path = None def init(self): cMin, theta = self.get_distance_and_angle(self.x_start, self.x_goal) C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin) xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0], [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]]) x_best = self.x_start return theta, cMin, xCenter, C, x_best def planning(self): theta, dist, x_center, C, x_best = self.init() c_best = np.inf for k in range(self.iter_max): if self.X_soln: cost = {node: self.Cost(node) for node in self.X_soln} x_best = min(cost, key=cost.get) c_best = cost[x_best] x_rand = self.Sample(c_best, dist, x_center, C) x_nearest = self.Nearest(self.V, x_rand) x_new = self.Steer(x_nearest, x_rand) if x_new and not self.utils.is_collision(x_nearest, x_new): X_near = self.Near(self.V, x_new) c_min = self.Cost(x_nearest) + self.Line(x_nearest, x_new) self.V.append(x_new) # choose parent for x_near in X_near: c_new = self.Cost(x_near) + self.Line(x_near, x_new) if c_new < c_min: x_new.parent = x_near c_min = c_new # rewire for x_near in X_near: c_near = self.Cost(x_near) c_new = self.Cost(x_new) + self.Line(x_new, x_near) if c_new < c_near: x_near.parent = x_new if self.InGoalRegion(x_new): if not self.utils.is_collision(x_new, self.x_goal): self.X_soln.add(x_new) # new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal) # if new_cost < c_best: # c_best = new_cost # x_best = x_new if k % 20 == 0: self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta) self.path = self.ExtractPath(x_best) self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta) plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r') plt.pause(0.01) plt.show() def Steer(self, x_start, x_goal): dist, theta = self.get_distance_and_angle(x_start, x_goal) dist = min(self.step_len, dist) node_new = Node((x_start.x + dist * math.cos(theta), x_start.y + dist * math.sin(theta))) node_new.parent = x_start return node_new def Near(self, nodelist, node): n = len(nodelist) + 1 r = 50 * math.sqrt((math.log(n) / n)) dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist] X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and not self.utils.is_collision(nodelist[ind], node)] return X_near def Sample(self, c_max, c_min, x_center, C): if c_max < np.inf: r = [c_max / 2.0, math.sqrt(c_max ** 2 - c_min ** 2) / 2.0, math.sqrt(c_max ** 2 - c_min ** 2) / 2.0] L = np.diag(r) while True: x_ball = self.SampleUnitBall() x_rand = np.dot(np.dot(C, L), x_ball) + x_center if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] - self.delta and \ self.y_range[0] + self.delta <= x_rand[1] <= self.y_range[1] - self.delta: break x_rand = Node((x_rand[(0, 0)], x_rand[(1, 0)])) else: x_rand = self.SampleFreeSpace() return x_rand @staticmethod def SampleUnitBall(): 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]]) def SampleFreeSpace(self): delta = self.delta if np.random.random() > self.goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.x_goal def ExtractPath(self, node): path = [[self.x_goal.x, self.x_goal.y]] while node.parent: path.append([node.x, node.y]) node = node.parent path.append([self.x_start.x, self.x_start.y]) return path def InGoalRegion(self, node): if self.Line(node, self.x_goal) < self.step_len: return True return False @staticmethod def RotationToWorldFrame(x_start, x_goal, 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 U, _, V_T = np.linalg.svd(M, True, True) C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T return C @staticmethod def Nearest(nodelist, n): return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 for nd in nodelist]))] @staticmethod def Line(x_start, x_goal): return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) def Cost(self, node): if node == self.x_start: return 0.0 if node.parent is None: return np.inf cost = 0.0 while node.parent: cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) node = node.parent return cost @staticmethod def get_distance_and_angle(node_start, node_end): 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, x_center=None, c_best=None, dist=None, theta=None): plt.cla() self.plot_grid("Informed rrt*, N = " + str(self.iter_max)) plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for node in self.V: if node.parent: plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g") if c_best != np.inf: self.draw_ellipse(x_center, c_best, dist, theta) plt.pause(0.01) 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, ".b") plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2) def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node rrt_star = IRrtStar(x_start, x_goal, 1, 0.10, 12, 1000) rrt_star.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/plotting.py ================================================ """ Plotting tools for Sampling-based algorithms @author: huiming zhou """ import matplotlib.pyplot as plt import matplotlib.patches as patches import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env class Plotting: def __init__(self, x_start, x_goal): self.xI, self.xG = x_start, x_goal self.env = env.Env() self.obs_bound = self.env.obs_boundary self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle def animation(self, nodelist, path, name, animation=False): self.plot_grid(name) self.plot_visited(nodelist, animation) self.plot_path(path) def animation_connect(self, V1, V2, path, name): self.plot_grid(name) self.plot_visited_connect(V1, V2) self.plot_path(path) def plot_grid(self, name): fig, ax = plt.subplots() for (ox, oy, w, h) in self.obs_bound: ax.add_patch( patches.Rectangle( (ox, oy), w, h, edgecolor='black', facecolor='black', fill=True ) ) for (ox, oy, w, h) in self.obs_rectangle: ax.add_patch( patches.Rectangle( (ox, oy), w, h, edgecolor='black', facecolor='gray', fill=True ) ) for (ox, oy, r) in self.obs_circle: ax.add_patch( patches.Circle( (ox, oy), r, edgecolor='black', facecolor='gray', fill=True ) ) plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3) plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) plt.title(name) plt.axis("equal") @staticmethod def plot_visited(nodelist, animation): if animation: count = 0 for node in nodelist: count += 1 if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count % 10 == 0: plt.pause(0.001) else: for node in nodelist: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") @staticmethod def plot_visited_connect(V1, V2): len1, len2 = len(V1), len(V2) for k in range(max(len1, len2)): if k < len1: if V1[k].parent: plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g") if k < len2: if V2[k].parent: plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g") plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if k % 2 == 0: plt.pause(0.001) plt.pause(0.01) @staticmethod def plot_path(path): if len(path) != 0: plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) plt.pause(0.01) plt.show() ================================================ FILE: Sampling_based_Planning/rrt_2D/queue.py ================================================ import collections import heapq class QueueFIFO: """ Class: QueueFIFO Description: QueueFIFO is designed for First-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.popleft() # leave from front class QueueLIFO: """ Class: QueueLIFO Description: QueueLIFO is designed for Last-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.pop() # leave from back class QueuePrior: """ Class: QueuePrior Description: QueuePrior reorders elements using value [priority] """ def __init__(self): self.queue = [] def empty(self): return len(self.queue) == 0 def put(self, item, priority): heapq.heappush(self.queue, (priority, item)) # reorder s using priority def get(self): return heapq.heappop(self.queue)[1] # pop out the smallest item def enumerate(self): return self.queue ================================================ FILE: Sampling_based_Planning/rrt_2D/rrt.py ================================================ """ RRT_2D @author: huiming zhou """ import os import sys import math import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class Rrt: def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max): self.s_start = Node(s_start) self.s_goal = Node(s_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.iter_max = iter_max self.vertex = [self.s_start] self.env = env.Env() self.plotting = plotting.Plotting(s_start, s_goal) self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary def planning(self): for i in range(self.iter_max): node_rand = self.generate_random_node(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.vertex.append(node_new) dist, _ = self.get_distance_and_angle(node_new, self.s_goal) if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal): self.new_state(node_new, self.s_goal) return self.extract_path(node_new) return None def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.s_goal @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) for nd in node_list]))] def new_state(self, node_start, node_end): dist, theta = self.get_distance_and_angle(node_start, node_end) dist = min(self.step_len, dist) node_new = Node((node_start.x + dist * math.cos(theta), node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new def extract_path(self, node_end): path = [(self.s_goal.x, self.s_goal.y)] node_now = node_end while node_now.parent is not None: node_now = node_now.parent path.append((node_now.x, node_now.y)) return path @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000) path = rrt.planning() if path: rrt.plotting.animation(rrt.vertex, path, "RRT", True) else: print("No Path Found!") if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/rrt_connect.py ================================================ """ RRT_CONNECT_2D @author: huiming zhou """ import os import sys import math import copy import numpy as np import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class RrtConnect: def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max): self.s_start = Node(s_start) self.s_goal = Node(s_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.iter_max = iter_max self.V1 = [self.s_start] self.V2 = [self.s_goal] self.env = env.Env() self.plotting = plotting.Plotting(s_start, s_goal) self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary def planning(self): for i in range(self.iter_max): node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate) node_near = self.nearest_neighbor(self.V1, node_rand) node_new = self.new_state(node_near, node_rand) if node_new and not self.utils.is_collision(node_near, node_new): self.V1.append(node_new) node_near_prim = self.nearest_neighbor(self.V2, node_new) node_new_prim = self.new_state(node_near_prim, node_new) if node_new_prim and not self.utils.is_collision(node_new_prim, node_near_prim): self.V2.append(node_new_prim) while True: node_new_prim2 = self.new_state(node_new_prim, node_new) if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim): self.V2.append(node_new_prim2) node_new_prim = self.change_node(node_new_prim, node_new_prim2) else: break if self.is_node_same(node_new_prim, node_new): break if self.is_node_same(node_new_prim, node_new): return self.extract_path(node_new, node_new_prim) if len(self.V2) < len(self.V1): list_mid = self.V2 self.V2 = self.V1 self.V1 = list_mid return None @staticmethod def change_node(node_new_prim, node_new_prim2): node_new = Node((node_new_prim2.x, node_new_prim2.y)) node_new.parent = node_new_prim return node_new @staticmethod def is_node_same(node_new_prim, node_new): if node_new_prim.x == node_new.x and \ node_new_prim.y == node_new.y: return True return False def generate_random_node(self, sample_goal, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return sample_goal @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) for nd in node_list]))] def new_state(self, node_start, node_end): dist, theta = self.get_distance_and_angle(node_start, node_end) dist = min(self.step_len, dist) node_new = Node((node_start.x + dist * math.cos(theta), node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new @staticmethod def extract_path(node_new, node_new_prim): path1 = [(node_new.x, node_new.y)] node_now = node_new while node_now.parent is not None: node_now = node_now.parent path1.append((node_now.x, node_now.y)) path2 = [(node_new_prim.x, node_new_prim.y)] node_now = node_new_prim while node_now.parent is not None: node_now = node_now.parent path2.append((node_now.x, node_now.y)) return list(list(reversed(path1)) + path2) @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.05, 5000) path = rrt_conn.planning() rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT") if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/rrt_sharp.py ================================================ ================================================ FILE: Sampling_based_Planning/rrt_2D/rrt_star.py ================================================ """ RRT_star 2D @author: huiming zhou """ import os import sys import math import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class RrtStar: def __init__(self, x_start, x_goal, step_len, goal_sample_rate, search_radius, iter_max): self.s_start = Node(x_start) self.s_goal = Node(x_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius self.iter_max = iter_max self.vertex = [self.s_start] self.path = [] self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) self.utils = utils.Utils() self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary def planning(self): for k in range(self.iter_max): node_rand = self.generate_random_node(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) node_new = self.new_state(node_near, node_rand) if k % 500 == 0: print(k) if node_new and not self.utils.is_collision(node_near, node_new): neighbor_index = self.find_near_neighbor(node_new) self.vertex.append(node_new) if neighbor_index: self.choose_parent(node_new, neighbor_index) self.rewire(node_new, neighbor_index) index = self.search_goal_parent() self.path = self.extract_path(self.vertex[index]) self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max)) def new_state(self, node_start, node_goal): dist, theta = self.get_distance_and_angle(node_start, node_goal) dist = min(self.step_len, dist) node_new = Node((node_start.x + dist * math.cos(theta), node_start.y + dist * math.sin(theta))) node_new.parent = node_start return node_new def choose_parent(self, node_new, neighbor_index): cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index] cost_min_index = neighbor_index[int(np.argmin(cost))] node_new.parent = self.vertex[cost_min_index] def rewire(self, node_new, neighbor_index): for i in neighbor_index: node_neighbor = self.vertex[i] if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor): node_neighbor.parent = node_new def search_goal_parent(self): dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex] node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len] if len(node_index) > 0: cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index if not self.utils.is_collision(self.vertex[i], self.s_goal)] return node_index[int(np.argmin(cost_list))] return len(self.vertex) - 1 def get_new_cost(self, node_start, node_end): dist, _ = self.get_distance_and_angle(node_start, node_end) return self.cost(node_start) + dist def generate_random_node(self, goal_sample_rate): delta = self.utils.delta if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.s_goal def find_near_neighbor(self, node_new): n = len(self.vertex) + 1 r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and not self.utils.is_collision(node_new, self.vertex[ind])] return dist_table_index @staticmethod def nearest_neighbor(node_list, n): return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) for nd in node_list]))] @staticmethod def cost(node_p): node = node_p cost = 0.0 while node.parent: cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) node = node.parent return cost def update_cost(self, parent_node): OPEN = queue.QueueFIFO() OPEN.put(parent_node) while not OPEN.empty(): node = OPEN.get() if len(node.child) == 0: continue for node_c in node.child: node_c.Cost = self.get_new_cost(node, node_c) OPEN.put(node_c) def extract_path(self, node_end): path = [[self.s_goal.x, self.s_goal.y]] node = node_end while node.parent is not None: path.append([node.x, node.y]) node = node.parent path.append([node.x, node.y]) return path @staticmethod def get_distance_and_angle(node_start, node_end): dx = node_end.x - node_start.x dy = node_end.y - node_start.y return math.hypot(dx, dy), math.atan2(dy, dx) def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000) rrt_star.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/rrt_star_smart.py ================================================ """ RRT_STAR_SMART 2D @author: huiming zhou """ import os import sys import math 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/") from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class RrtStarSmart: def __init__(self, x_start, x_goal, step_len, goal_sample_rate, search_radius, iter_max): self.x_start = Node(x_start) self.x_goal = Node(x_goal) self.step_len = step_len self.goal_sample_rate = goal_sample_rate self.search_radius = search_radius self.iter_max = iter_max self.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) self.utils = utils.Utils() self.fig, self.ax = plt.subplots() self.delta = self.utils.delta self.x_range = self.env.x_range self.y_range = self.env.y_range self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary self.V = [self.x_start] self.beacons = [] self.beacons_radius = 2 self.direct_cost_old = np.inf self.obs_vertex = self.utils.get_obs_vertex() self.path = None def planning(self): n = 0 b = 2 InitPathFlag = False self.ReformObsVertex() for k in range(self.iter_max): if k % 200 == 0: print(k) if (k - n) % b == 0 and len(self.beacons) > 0: x_rand = self.Sample(self.beacons) else: x_rand = self.Sample() x_nearest = self.Nearest(self.V, x_rand) x_new = self.Steer(x_nearest, x_rand) if x_new and not self.utils.is_collision(x_nearest, x_new): X_near = self.Near(self.V, x_new) self.V.append(x_new) if X_near: # choose parent cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near] x_new.parent = X_near[int(np.argmin(cost_list))] # rewire c_min = self.Cost(x_new) for x_near in X_near: c_near = self.Cost(x_near) c_new = c_min + self.Line(x_new, x_near) if c_new < c_near: x_near.parent = x_new if not InitPathFlag and self.InitialPathFound(x_new): InitPathFlag = True n = k if InitPathFlag: self.PathOptimization(x_new) if k % 5 == 0: self.animation() self.path = self.ExtractPath() self.animation() plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r') plt.pause(0.01) plt.show() def PathOptimization(self, node): direct_cost_new = 0.0 node_end = self.x_goal while node.parent: node_parent = node.parent if not self.utils.is_collision(node_parent, node_end): node_end.parent = node_parent else: direct_cost_new += self.Line(node, node_end) node_end = node node = node_parent if direct_cost_new < self.direct_cost_old: self.direct_cost_old = direct_cost_new self.UpdateBeacons() def UpdateBeacons(self): node = self.x_goal beacons = [] while node.parent: near_vertex = [v for v in self.obs_vertex if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9] if len(near_vertex) > 0: for v in near_vertex: beacons.append(v) node = node.parent self.beacons = beacons def ReformObsVertex(self): obs_vertex = [] for obs in self.obs_vertex: for vertex in obs: obs_vertex.append(vertex) self.obs_vertex = obs_vertex def Steer(self, x_start, x_goal): dist, theta = self.get_distance_and_angle(x_start, x_goal) dist = min(self.step_len, dist) node_new = Node((x_start.x + dist * math.cos(theta), x_start.y + dist * math.sin(theta))) node_new.parent = x_start return node_new def Near(self, nodelist, node): n = len(self.V) + 1 r = 50 * math.sqrt((math.log(n) / n)) dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist] X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and not self.utils.is_collision(node, nodelist[ind])] return X_near def Sample(self, goal=None): if goal is None: delta = self.utils.delta goal_sample_rate = self.goal_sample_rate if np.random.random() > goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.x_goal else: R = self.beacons_radius r = random.uniform(0, R) theta = random.uniform(0, 2 * math.pi) ind = random.randint(0, len(goal) - 1) return Node((goal[ind][0] + r * math.cos(theta), goal[ind][1] + r * math.sin(theta))) def SampleFreeSpace(self): delta = self.delta if np.random.random() > self.goal_sample_rate: return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) return self.x_goal def ExtractPath(self): path = [] node = self.x_goal while node.parent: path.append([node.x, node.y]) node = node.parent path.append([self.x_start.x, self.x_start.y]) return path def InitialPathFound(self, node): if self.Line(node, self.x_goal) < self.step_len: return True return False @staticmethod def Nearest(nodelist, n): return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 for nd in nodelist]))] @staticmethod def Line(x_start, x_goal): return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) @staticmethod def Cost(node): cost = 0.0 if node.parent is None: return cost while node.parent: cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) node = node.parent return cost @staticmethod def get_distance_and_angle(node_start, node_end): 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): plt.cla() self.plot_grid("rrt*-Smart, N = " + str(self.iter_max)) plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) for node in self.V: if node.parent: plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g") if self.beacons: theta = np.arange(0, 2 * math.pi, 0.1) r = self.beacons_radius for v in self.beacons: x = v[0] + r * np.cos(theta) y = v[1] + r * np.sin(theta) plt.plot(x, y, linestyle='--', linewidth=2, color='darkorange') plt.pause(0.01) 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") def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node rrt = RrtStarSmart(x_start, x_goal, 1.5, 0.10, 0, 1000) rrt.planning() if __name__ == '__main__': main() ================================================ FILE: Sampling_based_Planning/rrt_2D/utils.py ================================================ """ utils for collision check @author: huiming zhou """ import math import numpy as np import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from Sampling_based_Planning.rrt_2D import env from Sampling_based_Planning.rrt_2D.rrt import Node class Utils: def __init__(self): self.env = env.Env() self.delta = 0.5 self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle self.obs_boundary = self.env.obs_boundary def update_obs(self, obs_cir, obs_bound, obs_rec): self.obs_circle = obs_cir self.obs_boundary = obs_bound self.obs_rectangle = obs_rec def get_obs_vertex(self): delta = self.delta obs_list = [] for (ox, oy, w, h) in self.obs_rectangle: vertex_list = [[ox - delta, oy - delta], [ox + w + delta, oy - delta], [ox + w + delta, oy + h + delta], [ox - delta, oy + h + delta]] obs_list.append(vertex_list) return obs_list def is_intersect_rec(self, start, end, o, d, a, b): v1 = [o[0] - a[0], o[1] - a[1]] v2 = [b[0] - a[0], b[1] - a[1]] v3 = [-d[1], d[0]] div = np.dot(v2, v3) if div == 0: return False t1 = np.linalg.norm(np.cross(v2, v1)) / div t2 = np.dot(v1, v3) / div if t1 >= 0 and 0 <= t2 <= 1: shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1])) dist_obs = self.get_dist(start, shot) dist_seg = self.get_dist(start, end) if dist_obs <= dist_seg: return True return False def is_intersect_circle(self, o, d, a, r): d2 = np.dot(d, d) delta = self.delta if d2 == 0: return False t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2 if 0 <= t <= 1: shot = Node((o[0] + t * d[0], o[1] + t * d[1])) if self.get_dist(shot, Node(a)) <= r + delta: return True return False def is_collision(self, start, end): if self.is_inside_obs(start) or self.is_inside_obs(end): return True o, d = self.get_ray(start, end) obs_vertex = self.get_obs_vertex() for (v1, v2, v3, v4) in obs_vertex: if self.is_intersect_rec(start, end, o, d, v1, v2): return True if self.is_intersect_rec(start, end, o, d, v2, v3): return True if self.is_intersect_rec(start, end, o, d, v3, v4): return True if self.is_intersect_rec(start, end, o, d, v4, v1): return True for (x, y, r) in self.obs_circle: if self.is_intersect_circle(o, d, [x, y], r): return True return False def is_inside_obs(self, node): delta = self.delta for (x, y, r) in self.obs_circle: if math.hypot(node.x - x, node.y - y) <= r + delta: return True for (x, y, w, h) in self.obs_rectangle: if 0 <= node.x - (x - delta) <= w + 2 * delta \ and 0 <= node.y - (y - delta) <= h + 2 * delta: return True for (x, y, w, h) in self.obs_boundary: if 0 <= node.x - (x - delta) <= w + 2 * delta \ and 0 <= node.y - (y - delta) <= h + 2 * delta: return True return False @staticmethod def get_ray(start, end): orig = [start.x, start.y] direc = [end.x - start.x, end.y - start.y] return orig, direc @staticmethod def get_dist(start, end): return math.hypot(end.x - start.x, end.y - start.y) ================================================ FILE: Sampling_based_Planning/rrt_3D/ABIT_star3D.py ================================================ # This is Advanced Batched Informed Tree star 3D algorithm # implementation """ This is ABIT* code for 3D @author: yue qi source: M.P.Strub, J.D.Gammel. "Advanced BIT* (ABIT*): Sampling-Based Planning with Advanced Graph-Search Techniques" """ import numpy as np import matplotlib.pyplot as plt import time import copy import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent from rrt_3D.queue import MinheapPQ class ABIT_star: def __init__(self): self.env = env() self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) self.maxiter = 1000 self.done = False self.n = 1000# used in radius calc r(q) self.lam = 10 # used in radius calc r(q) def run(self): V, E = {self.xstart}, set() T = (V,E) Xunconnected = {self.xgoal} q = len(V) + len(Xunconnected) eps_infl, eps_trunc = np.inf, np.inf Vclosed, Vinconsistent = set(), set() Q = self.expand(self.xstart, T, Xunconnected, np.inf) ind = 0 while True: if self.is_search_marked_finished(): if self.update_approximation(eps_infl, eps_trunc): T, Xunconnected = self.prune(T, Xunconnected, self.xgoal) Xunconnected.update(self.sample(m, self.xgoal)) q = len(V) + len(Xunconnected) Q = self.expand({self.xstart}, T, Xunconnected, self.r(q)) else: Q.update(self.expand(Vinconsistent, T, Xunconnected, self.r(q))) eps_infl = self.update_inflation_factor() eps_trunc = self.update_truncation_factor() Vclosed = set() Vinconsistent = set() self.mark_search_unfinished() else: state_tuple = list(Q) (xp, xc) = state_tuple[np.argmin( [self.g_T[xi] + self.c_hat(xi,xj) + eps_infl * self.h_hat(xj) for (xi,xj) in Q] )] Q = Q.difference({(xp, xc)}) if (xp, xc) in E: if xc in Vclosed: Vinconsistent.add(xc) else: Q.update(self.expand({xc}, T, Xunconnected, self.r(q))) Vclosed.add(xc) elif eps_trunc * (self.g_T(xp) + self.c_hat(xi, xj) + self.h_hat(xc)) <= self.g_T(self.xgoal): if self.g_T(xp) + self.c_hat(xp, xc) < self.g_T(xc): if self.g_T(xp) + self.c(xp, xc) + self.h_hat(xc) < self.g_T(self.xgoal): if self.g_T(xp) + self.c(xp, xc) < self.g_T(xc): if xc in V: E = E.difference({(xprev, xc)}) else: Xunconnected.difference_update({xc}) V.add(xc) E.add((xp, xc)) if xc in Vclosed: Vinconsistent.add(xc) else: Q.update(self.expand({xc}, T, Xunconnected, self.r(q))) Vclosed.add(xc) else: self.mark_search_finished() ind += 1 # until stop if ind > self.maxiter: break def sample(self, m, xgoal): pass def expand(self, set_xi, T, Xunconnected, r): V, E = T Eout = set() for xp in set_xi: Eout.update({(x1, x2) for (x1, x2) in E if x1 == xp}) for xc in {x for x in Xunconnected.union(V) if getDist(xp, x) <= r}: if self.g_hat(xp) + self.c_hat(xp, xc) + self.h_hat(xc) <= self.g_T(self.xgoal): if self.g_hat(xp) + self.c_hat(xp, xc) <= self.g_hat(xc): Eout.add((xp,xc)) return Eout def prune(self, T, Xunconnected, xgoal): V, E = T Xunconnected.difference_update({x for x in Xunconnected if self.f_hat(x) >= self.g_T(xgoal)}) V.difference_update({x for x in V if self.f_hat(x) > self.g_T(xgoal)}) E.difference_update({(xp, xc) for (xp, xc) in E if self.f_hat(xp) > self.g_T(xgoal) or self.f_hat(xc) > self.g_T(xgoal)}) Xunconnected.update({xc for (xp, xc) in E if (xp not in V) and (xc in V)}) V.difference_update({xc for (xp, xc) in E if (xp not in V) and (xc in V)}) T = (V,E) return T, Xunconnected def g_hat(self, x): pass def h_hat(self, x): pass def c_hat(self, x1, x2): pass def f_hat(self, x): pass def g_T(self, x): pass def r(self, q): return self.eta * (2 * (1 + 1/self.n) * (self.Lambda(self.Xf_hat) / self.Zeta) * (np.log(q) / q)) ** (1 / self.n) def Lambda(self, inputset): pass def Zeta(self): pass def is_search_marked_finished(self): return self.done def mark_search_unfinished(self): self.done = False return self.done def mark_search_finished(self): self.done = True return self.done ================================================ FILE: Sampling_based_Planning/rrt_3D/BIT_star3D.py ================================================ # This is Batched Informed Tree star 3D algorithm # implementation """ This is ABIT* code for 3D @author: yue qi Algorithm 1 source: Gammell, Jonathan D., Siddhartha S. Srinivasa, and Timothy D. Barfoot. "Batch informed trees (BIT*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs." 2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015. and source: Gammell, Jonathan D., Timothy D. Barfoot, and Siddhartha S. Srinivasa. "Batch Informed Trees (BIT*): Informed asymptotically optimal anytime search." The International Journal of Robotics Research 39.5 (2020): 543-567. """ import numpy as np import matplotlib.pyplot as plt from numpy.matlib import repmat import time import copy import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, isinbound from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent from rrt_3D.queue import MinheapPQ #---------methods to draw ellipse during sampling def CreateUnitSphere(r = 1): phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle radius = r # Transformation formulae for a spherical coordinate system. x = radius*np.sin(theta)*np.cos(phi) y = radius*np.sin(theta)*np.sin(phi) z = radius*np.cos(theta) return (x, y, z) def draw_ellipsoid(ax, C, L, xcenter): (xs, ys, zs) = CreateUnitSphere() pts = np.array([xs, ys, zs]) pts_in_world_frame = C@L@pts + xcenter ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color="g") class BIT_star: # ---------initialize and run def __init__(self, show_ellipse=False): self.env = env() self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.maxiter = 1000 # used for determining how many batches needed # radius calc parameter: # larger value makes better 1-time-performance, but longer time trade off self.eta = 7 # bigger or equal to 1 # sampling self.m = 400 # number of samples for one time sample self.d = 3 # dimension we work with # instance of the cost to come gT self.g = {self.xstart:0, self.xgoal:np.inf} # draw ellipse self.show_ellipse = show_ellipse # denote if the path is found self.done = False self.Path = [] # for drawing the ellipse self.C = np.zeros([3,3]) self.L = np.zeros([3,3]) self.xcenter = np.zeros(3) self.show_ellipse = show_ellipse def run(self): self.V = {self.xstart} # node expanded self.E = set() # edge set self.Parent = {} # Parent relation # self.T = (self.V, self.E) # tree self.Xsamples = {self.xgoal} # sampled set self.QE = set() # edges in queue self.QV = set() # nodes in queue self.r = np.inf # radius for evaluation self.ind = 0 num_resample = 0 while True: # for the first round print('round '+str(self.ind)) self.visualization() # print(len(self.V)) if len(self.QE) == 0 and len(self.QV) == 0: self.Prune(self.g_T(self.xgoal)) self.Xsamples = self.Sample(self.m, self.g_T(self.xgoal)) # sample function self.Xsamples.add(self.xgoal) # adding goal into the sample self.Vold = {v for v in self.V} self.QV = {v for v in self.V} # setting the radius if self.done: self.r = 2 # sometimes the original radius criteria makes the radius too small to improve existing tree num_resample += 1 else: self.r = self.radius(len(self.V) + len(self.Xsamples)) # radius determined with the sample size and dimension of conf space while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'): self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV')) (vm, xm) = self.BestInQueue(self.QE, mode = 'QE') self.QE.remove((vm, xm)) if self.g_T(vm) + self.c_hat(vm, xm) + self.h_hat(xm) < self.g_T(self.xgoal): cost = self.c(vm, xm) if self.g_hat(vm) + cost + self.h_hat(xm) < self.g_T(self.xgoal): if self.g_T(vm) + cost < self.g_T(xm): if xm in self.V: self.E.difference_update({(v, x) for (v, x) in self.E if x == xm}) else: self.Xsamples.remove(xm) self.V.add(xm) self.QV.add(xm) self.g[xm] = self.g[vm] + cost self.E.add((vm, xm)) self.Parent[xm] = vm # add parent or update parent self.QE.difference_update({(v, x) for (v, x) in self.QE if x == xm and (self.g_T(v) + self.c_hat(v, xm)) >= self.g_T(xm)}) # reinitializing sampling else: self.QE = set() self.QV = set() self.ind += 1 # if the goal is reached if self.xgoal in self.Parent: print('locating path...') self.done = True self.Path = self.path() # if the iteration is bigger if self.ind > self.maxiter: break print('complete') print('number of times resampling ' + str(num_resample)) # ---------IRRT utils def Sample(self, m, cmax, bias = 0.05, xrand = set()): # sample within a eclipse print('new sample') if cmax < np.inf: cmin = getDist(self.xgoal, self.xstart) xcenter = np.array([(self.xgoal[0] + self.xstart[0]) / 2, (self.xgoal[1] + self.xstart[1]) / 2, (self.xgoal[2] + self.xstart[2]) / 2]) C = self.RotationToWorldFrame(self.xstart, self.xgoal) r = np.zeros(3) r[0] = cmax /2 for i in range(1,3): r[i] = np.sqrt(cmax**2 - cmin**2) / 2 L = np.diag(r) # R3*3 xball = self.SampleUnitBall(m) # np.array x = (C@L@xball).T + repmat(xcenter, len(xball.T), 1) # x2 = set(map(tuple, x)) self.C = C # save to global var self.xcenter = xcenter self.L = L x2 = set(map(tuple, x[np.array([not isinside(self, state) and isinbound(self.env.boundary, state) for state in x])])) # intersection with the state space xrand.update(x2) # if there are samples inside obstacle: recursion if len(x2) < m: return self.Sample(m - len(x2), cmax, bias=bias, xrand=xrand) else: for i in range(m): xrand.add(tuple(sampleFree(self, bias = bias))) return xrand def SampleUnitBall(self, n): # uniform sampling in spherical coordinate system in 3D # sample radius r = np.random.uniform(0.0, 1.0, size = n) theta = np.random.uniform(0, np.pi, size = n) phi = np.random.uniform(0, 2 * np.pi, size = n) x = r * np.sin(theta) * np.cos(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(theta) return np.array([x,y,z]) def RotationToWorldFrame(self, xstart, xgoal): # S0(n): such that the xstart and xgoal are the center points d = getDist(xstart, xgoal) xstart, xgoal = np.array(xstart), np.array(xgoal) a1 = (xgoal - xstart) / d M = np.outer(a1,[1,0,0]) U, S, V = np.linalg.svd(M) C = U@np.diag([1, 1, np.linalg.det(U)*np.linalg.det(V)])@V.T return C #----------BIT_star particular def ExpandVertex(self, v): self.QV.remove(v) Xnear = {x for x in self.Xsamples if getDist(x, v) <= self.r} self.QE.update({(v, x) for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)}) if v not in self.Vold: Vnear = {w for w in self.V if getDist(w, v) <= self.r} self.QE.update({(v,w) for w in Vnear if \ ((v,w) not in self.E) and \ (self.g_hat(v) + self.c_hat(v, w) + self.h_hat(w) < self.g_T(self.xgoal)) and \ (self.g_T(v) + self.c_hat(v, w) < self.g_T(w))}) def Prune(self, c): self.Xsamples = {x for x in self.Xsamples if self.f_hat(x) >= c} self.V.difference_update({v for v in self.V if self.f_hat(v) >= c}) self.E.difference_update({(v, w) for (v, w) in self.E if (self.f_hat(v) > c) or (self.f_hat(w) > c)}) self.Xsamples.update({v for v in self.V if self.g_T(v) == np.inf}) self.V.difference_update({v for v in self.V if self.g_T(v) == np.inf}) def radius(self, q): return 2 * self.eta * (1 + 1/self.d) ** (1/self.d) * \ (self.Lambda(self.Xf_hat(self.V)) / self.Zeta() ) ** (1/self.d) * \ (np.log(q) / q) ** (1/self.d) def Lambda(self, inputset): # lebesgue measure of a set, defined as # mu: L(Rn) --> [0, inf], e.g. volume return len(inputset) def Xf_hat(self, X): # the X is a set, defined as {x in X | fhat(x) <= cbest} # where cbest is current best cost. cbest = self.g_T(self.xgoal) return {x for x in X if self.f_hat(x) <= cbest} def Zeta(self): # Lebesgue measure of a n dimensional unit ball # since it's the 3D, use volume return 4/3 * np.pi def BestInQueue(self, inputset, mode): # returns the best vertex in the vertex queue given this ordering # mode = 'QE' or 'QV' if mode == 'QV': V = {state: self.g_T(state) + self.h_hat(state) for state in self.QV} if mode == 'QE': V = {state: self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE} if len(V) == 0: print(mode + 'empty') return None return min(V, key = V.get) def BestQueueValue(self, inputset, mode): # returns the best value in the vertex queue given this ordering # mode = 'QE' or 'QV' if mode == 'QV': V = {self.g_T(state) + self.h_hat(state) for state in self.QV} if mode == 'QE': V = {self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE} if len(V) == 0: return np.inf return min(V) def g_hat(self, v): return getDist(self.xstart, v) def h_hat(self, v): return getDist(self.xgoal, v) def f_hat(self, v): # f = g + h: estimate cost return self.g_hat(v) + self.h_hat(v) def c(self, v, w): # admissible estimate of the cost of an edge between state v, w collide, dist = isCollide(self, v, w) if collide: return np.inf else: return dist def c_hat(self, v, w): # c_hat < c < np.inf # heuristic estimate of the edge cost, since c is expensive return getDist(v, w) def g_T(self, v): # represent cost-to-come from the start in the tree, # if the state is not in tree, or unreachable, return inf if v not in self.g: self.g[v] = np.inf return self.g[v] def path(self): path = [] s = self.xgoal i = 0 while s != self.xstart: path.append((s, self.Parent[s])) s = self.Parent[s] if i > self.m: break i += 1 return path def visualization(self): if self.ind % 20 == 0: V = np.array(list(self.V)) Xsample = np.array(list(self.Xsamples)) edges = list(map(list, self.E)) Path = np.array(self.Path) start = self.env.start goal = self.env.goal # edges = E.get_edge() #----------- list structure # edges = [] # for i in self.Parent: # edges.append([i,self.Parent[i]]) #----------- end # generate axis objects ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) ax.view_init(elev=90., azim=60.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, self.env.balls) draw_block_list(ax, self.env.blocks) if self.env.OBB is not None: draw_obb(ax, self.env.OBB) draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, edges, visibility=0.75, color='g') draw_line(ax, Path, color='r') if self.show_ellipse: draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis if len(V) > 0: ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) if len(Xsample) > 0: # plot the sampled points ax.scatter3D(Xsample[:, 0], Xsample[:, 1], Xsample[:, 2], s=1, color='b',) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio ax.dist = 11 set_axes_equal(ax) make_transparent(ax) #plt.xlabel('s') #plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) if __name__ == '__main__': Newprocess = BIT_star(show_ellipse=False) Newprocess.run() ================================================ FILE: Sampling_based_Planning/rrt_3D/FMT_star3D.py ================================================ """ This is fast marching tree* code for 3D @author: yue qi source: Janson, Lucas, et al. "Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions." The International journal of robotics research 34.7 (2015): 883-921. """ import numpy as np import matplotlib.pyplot as plt import time import copy import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent from rrt_3D.queue import MinheapPQ class FMT_star: def __init__(self, radius = 1, n = 1000): self.env = env() # init start and goal # note that the xgoal could be a region since this algorithm is a multiquery method self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # used for sample free self.n = n # number of samples self.radius = radius # radius of the ball # self.radius = 40 * np.sqrt((np.log(self.n) / self.n)) # sets self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets() # make space for save self.neighbors = {} # additional self.done = True self.Path = [] self.Parent = {} def generateSampleSet(self, n): V = set() for i in range(n): V.add(tuple(sampleFree(self, bias = 0.0))) return V def initNodeSets(self): # open set Vopen = {self.xinit} # open set # closed set closed = set() # V, Vunvisited set V = self.generateSampleSet(self.n - 2) # set of all nodes Vunvisited = copy.deepcopy(V) # unvisited set Vunvisited.add(self.xgoal) V.add(self.xinit) V.add(self.xgoal) # initialize all cost to come at inf c = {node : np.inf for node in V} c[self.xinit] = 0 # use a min heap to speed up Vopen_queue = MinheapPQ() Vopen_queue.put(self.xinit, c[self.xinit]) # priority organized as the cost to come return Vopen, Vopen_queue, closed, V, Vunvisited, c def Near(self, nodeset, node, rn): if node in self.neighbors: return self.neighbors[node] validnodes = {i for i in nodeset if getDist(i, node) < rn} return validnodes def Save(self, V_associated, node): self.neighbors[node] = V_associated def path(self, z, initT): path = [] s = self.xgoal i = 0 while s != self.xinit: path.append((s, self.Parent[s])) s = self.Parent[s] if i > self.n: break i += 1 return path def Cost(self, x, y): # collide, dist = isCollide(self, x, y) # if collide: # return np.inf # return dist return getDist(x, y) def FMTrun(self): z = self.xinit rn = self.radius Nz = self.Near(self.Vunvisited, z, rn) E = set() self.Save(Nz, z) ind = 0 while z != self.xgoal: Vopen_new = set() #Nz = self.Near(self.Vunvisited, z, rn) #self.Save(Nz, z) #Xnear = Nz.intersection(self.Vunvisited) Xnear = self.Near(self.Vunvisited, z ,rn) self.Save(Xnear, z) for x in Xnear: #Nx = self.Near(self.V.difference({x}), x, rn) #self.Save(Nx, x) #Ynear = list(Nx.intersection(self.Vopen)) Ynear = list(self.Near(self.Vopen, x, rn)) # self.Save(set(Ynear), x) ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation collide, _ = isCollide(self, ymin, x) if not collide: E.add((ymin, x)) # straight line joining ymin and x is collision free Vopen_new.add(x) self.Parent[x] = z self.Vunvisited = self.Vunvisited.difference({x}) self.c[x] = self.c[ymin] + self.Cost(ymin, x) # estimated cost-to-arrive from xinit in tree T = (VopenUVclosed, E) # update open set self.Vopen = self.Vopen.union(Vopen_new).difference({z}) self.Vclosed.add(z) if len(self.Vopen) == 0: print('Failure') return ind += 1 print(str(ind) + ' node expanded') self.visualization(ind, E) # update current node Vopenlist = list(self.Vopen) z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])] # creating the tree T = (self.Vopen.union(self.Vclosed), E) self.done = True self.Path = self.path(z, T) self.visualization(ind, E) plt.show() # return self.path(z, T) def visualization(self, ind, E): if ind % 100 == 0 or self.done: #----------- list structure # V = np.array(list(initparams.V)) # E = initparams.E #----------- end # edges = initparams.E Path = np.array(self.Path) start = self.env.start goal = self.env.goal # edges = E.get_edge() #----------- list structure edges = np.array(list(E)) #----------- end # generate axis objects ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) ax.view_init(elev=65., azim=60.) ax.dist = 15 # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, self.env.balls) draw_block_list(ax, self.env.blocks) if self.env.OBB is not None: draw_obb(ax, self.env.OBB) draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, edges, visibility=0.75, color='g') draw_line(ax, Path, color='r') # if len(V) > 0: # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio set_axes_equal(ax) make_transparent(ax) #plt.xlabel('x') #plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) if __name__ == '__main__': A = FMT_star(radius = 1, n = 3000) A.FMTrun() ================================================ FILE: Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py ================================================ """ This is dynamic rrt code for 3D @author: yue qi """ import numpy as np import time import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent class dynamic_rrt_3D: def __init__(self): self.env = env() self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.qrobot = self.x0 self.current = tuple(self.env.start) self.stepsize = 0.25 self.maxiter = 10000 self.GoalProb = 0.05 # probability biased to the goal self.WayPointProb = 0.02 # probability falls back on to the way points self.done = False self.invalid = False self.V = [] # vertices self.Parent = {} # parent child relation self.Edge = set() # edge relation (node, parent node) tuple self.Path = [] self.flag = {} # flag dictionary self.ind = 0 self.i = 0 # --------Dynamic RRT algorithm def RegrowRRT(self): self.TrimRRT() self.GrowRRT() def TrimRRT(self): S = [] i = 1 print('trimming...') while i < len(self.V): qi = self.V[i] qp = self.Parent[qi] if self.flag[qp] == 'Invalid': self.flag[qi] = 'Invalid' if self.flag[qi] != 'Invalid': S.append(qi) i += 1 self.CreateTreeFromNodes(S) def InvalidateNodes(self, obstacle): Edges = self.FindAffectedEdges(obstacle) for edge in Edges: qe = self.ChildEndpointNode(edge) self.flag[qe] = 'Invalid' # --------Extend RRT algorithm----- def initRRT(self): self.V.append(self.x0) self.flag[self.x0] = 'Valid' def GrowRRT(self): print('growing...') qnew = self.x0 distance_threshold = self.stepsize self.ind = 0 while self.ind <= self.maxiter: qtarget = self.ChooseTarget() qnearest = self.Nearest(qtarget) qnew, collide = self.Extend(qnearest, qtarget) if not collide: self.AddNode(qnearest, qnew) if getDist(qnew, self.xt) < distance_threshold: self.AddNode(qnearest, self.xt) self.flag[self.xt] = 'Valid' break self.i += 1 self.ind += 1 # self.visualization() def ChooseTarget(self): # return the goal, or randomly choose a state in the waypoints based on probs p = np.random.uniform() if len(self.V) == 1: i = 0 else: i = np.random.randint(0, high=len(self.V) - 1) if 0 < p < self.GoalProb: return self.xt elif self.GoalProb < p < self.GoalProb + self.WayPointProb: return self.V[i] elif self.GoalProb + self.WayPointProb < p < 1: return tuple(self.RandomState()) def RandomState(self): # generate a random, obstacle free state xrand = sampleFree(self, bias=0) return xrand def AddNode(self, nearest, extended): self.V.append(extended) self.Parent[extended] = nearest self.Edge.add((extended, nearest)) self.flag[extended] = 'Valid' def Nearest(self, target): # TODO use kdTree to speed up search return nearest(self, target, isset=True) def Extend(self, nearest, target): extended, dist = steer(self, nearest, target, DIST=True) collide, _ = isCollide(self, nearest, target, dist) return extended, collide # --------Main function def Main(self): # qstart = qgoal self.x0 = tuple(self.env.goal) # qgoal = qrobot self.xt = tuple(self.env.start) self.initRRT() self.GrowRRT() self.Path, D = self.path() self.done = True self.visualization() t = 0 while True: # move the block while the robot is moving new, _ = self.env.move_block(a=[0.2, 0, -0.2], mode='translation') self.InvalidateNodes(new) self.TrimRRT() # if solution path contains invalid node self.visualization() self.invalid = self.PathisInvalid(self.Path) if self.invalid: self.done = False self.RegrowRRT() self.Path = [] self.Path, D = self.path() self.done = True self.visualization() if t == 8: break t += 1 self.visualization() plt.show() # --------Additional utility functions def FindAffectedEdges(self, obstacle): # scan the graph for the changed edges in the tree. # return the end point and the affected print('finding affected edges...') Affectededges = [] for e in self.Edge: child, parent = e collide, _ = isCollide(self, child, parent) if collide: Affectededges.append(e) return Affectededges def ChildEndpointNode(self, edge): return edge[0] def CreateTreeFromNodes(self, Nodes): print('creating tree...') # self.Parent = {node: self.Parent[node] for node in Nodes} self.V = [node for node in Nodes] self.Edge = {(node, self.Parent[node]) for node in Nodes} # if self.invalid: # del self.Parent[self.xt] def PathisInvalid(self, path): for edge in path: if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid': return True def path(self, dist=0): Path=[] x = self.xt i = 0 while x != self.x0: x2 = self.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 if i > 10000: print('Path is not found') return i+= 1 return Path, dist # --------Visualization specialized for dynamic RRT def visualization(self): if self.ind % 100 == 0 or self.done: V = np.array(self.V) Path = np.array(self.Path) start = self.env.start goal = self.env.goal # edges = [] # for i in self.Parent: # edges.append([i, self.Parent[i]]) edges = np.array([list(i) for i in self.Edge]) ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) ax.view_init(elev=90., azim=0.) ax.clear() # drawing objects draw_Spheres(ax, self.env.balls) draw_block_list(ax, self.env.blocks) if self.env.OBB is not None: draw_obb(ax, self.env.OBB) draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, edges, visibility=0.75, color='g') draw_line(ax, Path, color='r') # if len(V) > 0: # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio set_axes_equal(ax) make_transparent(ax) # plt.xlabel('s') # plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) if __name__ == '__main__': rrt = dynamic_rrt_3D() rrt.Main() ================================================ FILE: Sampling_based_Planning/rrt_3D/env3D.py ================================================ # this is the three dimensional configuration space for rrt # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np # from utils3D import OBB2AABB def R_matrix(z_angle,y_angle,x_angle): # s angle: row; y angle: pitch; z angle: yaw # generate rotation matrix in SO3 # RzRyRx = R, ZYX intrinsic rotation # also (r1,r2,r3) in R3*3 in {W} frame # used in obb.O # [[R p] # [0T 1]] gives transformation from body to world return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) def getblocks(): # AABBs block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00], [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00], [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00], [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00], [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]] Obstacles = [] for i in block: i = np.array(i) Obstacles.append([j for j in i]) return np.array(Obstacles) def getballs(): spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]] Obstacles = [] for i in spheres: Obstacles.append([j for j in i]) return np.array(Obstacles) def getAABB(blocks): # used for Pyrr package for detecting collision AABB = [] for i in blocks: AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger return AABB def getAABB2(blocks): # used in lineAABB AABB = [] for i in blocks: AABB.append(aabb(i)) return AABB def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): return block class aabb(object): # make AABB out of blocks, # P: center point # E: extents # O: Rotation matrix in SO(3), in {w} def __init__(self,AABB): self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents self.O = [[1,0,0],[0,1,0],[0,0,1]] class obb(object): # P: center point # E: extents # O: Rotation matrix in SO(3), in {w} def __init__(self, P, E, O): self.P = P self.E = E self.O = O self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1): # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): self.resolution = resolution self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) self.blocks = getblocks() self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) self.balls = getballs() self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)), obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))]) self.start = np.array([2.0, 2.0, 2.0]) self.goal = np.array([6.0, 16.0, 0.0]) self.t = 0 # time def New_block(self): newblock = add_block() self.blocks = np.vstack([self.blocks,newblock]) self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) def move_start(self, x): self.start = x def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'): # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, # R is an orthorgonal transform in R3*3, is the rotation matrix # (s',t') = (s + tv, t) is uniform transformation # (s',t') = (s + a, t + s) is a translation if mode == 'translation': ori = np.array(self.blocks[block_to_move]) self.blocks[block_to_move] = \ np.array([ori[0] + a[0],\ ori[1] + a[1],\ ori[2] + a[2],\ ori[3] + a[0],\ ori[4] + a[1],\ ori[5] + a[2]]) self.AABB[block_to_move].P = \ [self.AABB[block_to_move].P[0] + a[0], \ self.AABB[block_to_move].P[1] + a[1], \ self.AABB[block_to_move].P[2] + a[2]] self.t += s # return a range of block that the block might moved a = self.blocks[block_to_move] return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) # return a,ori # (s',t') = (Rx, t) def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]): # theta stands for rotational angles around three principle axis in world frame # translation stands for translation in the world frame ori = [self.OBB[obb_to_move]] # move obb position self.OBB[obb_to_move].P = \ [self.OBB[obb_to_move].P[0] + translation[0], self.OBB[obb_to_move].P[1] + translation[1], self.OBB[obb_to_move].P[2] + translation[2]] # Calculate orientation self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) # generating transformation matrix self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\ -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]]) return self.OBB[obb_to_move], ori[0] if __name__ == '__main__': newenv = env() ================================================ FILE: Sampling_based_Planning/rrt_3D/extend_rrt3D.py ================================================ # Real-Time Randomized Path Planning for Robot Navigation∗ """ This is rrt extend code for 3D @author: yue qi """ import numpy as np from numpy.matlib import repmat from collections import defaultdict import time import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path # here attempt to use a KD tree for the data structure implementation import scipy.spatial.kdtree as KDtree class extend_rrt(object): def __init__(self): self.env = env() self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.current = tuple(self.env.start) self.stepsize = 0.5 self.maxiter = 10000 self.GoalProb = 0.05 # probability biased to the goal self.WayPointProb = 0.05 # probability falls back on to the way points self.done = False self.V = [] # vertices self.Parent = {} self.Path = [] self.ind = 0 self.i = 0 #--------- basic rrt algorithm---------- def RRTplan(self, env, initial, goal): threshold = self.stepsize nearest = initial # state structure self.V.append(initial) rrt_tree = initial # TODO KDtree structure while self.ind <= self.maxiter: target = self.ChooseTarget(goal) nearest = self.Nearest(rrt_tree, target) extended, collide = self.Extend(env, nearest, target) if not collide: self.AddNode(rrt_tree, nearest, extended) if getDist(nearest, goal) <= threshold: self.AddNode(rrt_tree, nearest, self.xt) break self.i += 1 self.ind += 1 visualization(self) # return rrt_tree self.done = True self.Path, _ = path(self) visualization(self) plt.show() def Nearest(self, tree, target): # TODO use kdTree to speed up search return nearest(self, target, isset=True) def Extend(self, env, nearest, target): extended, dist = steer(self, nearest, target, DIST = True) collide, _ = isCollide(self, nearest, target, dist) return extended, collide def AddNode(self, tree, nearest, extended): # TODO use a kdtree self.V.append(extended) self.Parent[extended] = nearest def RandomState(self): # generate a random, obstacle free state xrand = sampleFree(self, bias=0) return xrand #--------- insight to the rrt_extend def ChooseTarget(self, state): # return the goal, or randomly choose a state in the waypoints based on probs p = np.random.uniform() if len(self.V) == 1: i = 0 else: i = np.random.randint(0, high = len(self.V) - 1) if 0 < p < self.GoalProb: return self.xt elif self.GoalProb < p < self.GoalProb + self.WayPointProb: return self.V[i] elif self.GoalProb + self.WayPointProb < p < 1: return tuple(self.RandomState()) if __name__ == '__main__': t = extend_rrt() _ = t.RRTplan(t.env, t.x0, t.xt) ================================================ FILE: Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py ================================================ # informed RRT star in 3D """ This is IRRT* code for 3D @author: yue qi source: J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in IROS, 2997–3004, 2014. """ import numpy as np import matplotlib.pyplot as plt import time import copy import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, near, nearest, path from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent from rrt_3D.queue import MinheapPQ def CreateUnitSphere(r = 1): phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle radius = r # Transformation formulae for a spherical coordinate system. x = radius*np.sin(theta)*np.cos(phi) y = radius*np.sin(theta)*np.sin(phi) z = radius*np.cos(theta) return (x, y, z) def draw_ellipsoid(ax, C, L, xcenter): (xs, ys, zs) = CreateUnitSphere() pts = np.array([xs, ys, zs]) pts_in_world_frame = C@L@pts + xcenter ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color="g") class IRRT: def __init__(self, show_ellipse = False): self.env = env() self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.Parent = {} self.Path = [] self.N = 10000 # used for determining how many batches needed self.ind = 0 self.i = 0 # rrt* near and other utils self.stepsize = 1 self.gamma = 500 self.eta = self.stepsize self.rgoal = self.stepsize self.done = False # for drawing the ellipse self.C = np.zeros([3,3]) self.L = np.zeros([3,3]) self.xcenter = np.zeros(3) self.show_ellipse = show_ellipse def Informed_rrt(self): self.V = [self.xstart] self.E = set() self.Xsoln = set() self.T = (self.V, self.E) c = 1 while self.ind <= self.N: print(self.ind) self.visualization() # print(self.i) if len(self.Xsoln) == 0: cbest = np.inf else: cbest = min({self.cost(xsln) for xsln in self.Xsoln}) xrand = self.Sample(self.xstart, self.xgoal, cbest) xnearest = nearest(self, xrand) xnew, dist = steer(self, xnearest, xrand) # print(xnew) collide, _ = isCollide(self, xnearest, xnew, dist=dist) if not collide: self.V.append(xnew) Xnear = near(self, xnew) xmin = xnearest cmin = self.cost(xmin) + c * self.line(xnearest, xnew) for xnear in Xnear: xnear = tuple(xnear) cnew = self.cost(xnear) + c * self.line(xnear, xnew) if cnew < cmin: collide, _ = isCollide(self, xnear, xnew) if not collide: xmin = xnear cmin = cnew self.E.add((xmin, xnew)) self.Parent[xnew] = xmin for xnear in Xnear: xnear = tuple(xnear) cnear = self.cost(xnear) cnew = self.cost(xnew) + c * self.line(xnew, xnear) # rewire if cnew < cnear: collide, _ = isCollide(self, xnew, xnear) if not collide: xparent = self.Parent[xnear] self.E.difference_update((xparent, xnear)) self.E.add((xnew, xnear)) self.Parent[xnear] = xnew self.i += 1 if self.InGoalRegion(xnew): print('reached') self.done = True self.Parent[self.xgoal] = xnew self.Path, _ = path(self) self.Xsoln.add(xnew) # update path if self.done: self.Path, _ = path(self, Path = []) self.ind += 1 # return tree return self.T def Sample(self, xstart, xgoal, cmax, bias = 0.05): # sample within a eclipse if cmax < np.inf: cmin = getDist(xgoal, xstart) xcenter = np.array([(xgoal[0] + xstart[0]) / 2, (xgoal[1] + xstart[1]) / 2, (xgoal[2] + xstart[2]) / 2]) C = self.RotationToWorldFrame(xstart, xgoal) r = np.zeros(3) r[0] = cmax /2 for i in range(1,3): r[i] = np.sqrt(cmax**2 - cmin**2) / 2 L = np.diag(r) # R3*3 xball = self.SampleUnitBall() # np.array x = C@L@xball + xcenter self.C = C # save to global var self.xcenter = xcenter self.L = L if not isinside(self, x): # intersection with the state space xrand = x else: return self.Sample(xstart, xgoal, cmax) else: xrand = sampleFree(self, bias = bias) return xrand def SampleUnitBall(self): # uniform sampling in spherical coordinate system in 3D # sample radius r = np.random.uniform(0.0, 1.0) theta = np.random.uniform(0, np.pi) phi = np.random.uniform(0, 2 * np.pi) x = r * np.sin(theta) * np.cos(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(theta) return np.array([x,y,z]) def RotationToWorldFrame(self, xstart, xgoal): # S0(n): such that the xstart and xgoal are the center points d = getDist(xstart, xgoal) xstart, xgoal = np.array(xstart), np.array(xgoal) a1 = (xgoal - xstart) / d M = np.outer(a1,[1,0,0]) U, S, V = np.linalg.svd(M) C = U@np.diag([1, 1, np.linalg.det(U)*np.linalg.det(V)])@V.T return C def InGoalRegion(self, x): # Xgoal = {x in Xfree | \\x-xgoal\\2 <= rgoal} return getDist(x, self.xgoal) <= self.rgoal def cost(self, x): # actual cost '''here use the additive recursive cost function''' if x == self.xstart: return 0.0 if x not in self.Parent: return np.inf return self.cost(self.Parent[x]) + getDist(x, self.Parent[x]) def line(self, x, y): return getDist(x, y) def visualization(self): if self.ind % 500 == 0: V = np.array(self.V) edges = list(map(list, self.E)) Path = np.array(self.Path) start = self.env.start goal = self.env.goal # edges = E.get_edge() #----------- list structure # edges = [] # for i in self.Parent: # edges.append([i,self.Parent[i]]) #----------- end # generate axis objects ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) ax.view_init(elev=90., azim=0.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, self.env.balls) draw_block_list(ax, self.env.blocks) if self.env.OBB is not None: draw_obb(ax, self.env.OBB) draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, edges, visibility=0.75, color='g') draw_line(ax, Path, color='r') if self.show_ellipse: draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis if len(V) > 0: ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio ax.dist = 5 set_axes_equal(ax) make_transparent(ax) #plt.xlabel('s') #plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) if __name__ == '__main__': A = IRRT(show_ellipse=False) A.Informed_rrt() ================================================ FILE: Sampling_based_Planning/rrt_3D/plot_util3D.py ================================================ # plotting import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d.art3d import Poly3DCollection import mpl_toolkits.mplot3d as plt3d from mpl_toolkits.mplot3d import proj3d import numpy as np def CreateSphere(center, r): u = np.linspace(0, 2 * np.pi, 30) v = np.linspace(0, np.pi, 30) x = np.outer(np.cos(u), np.sin(v)) y = np.outer(np.sin(u), np.sin(v)) z = np.outer(np.ones(np.size(u)), np.cos(v)) x, y, z = r * x + center[0], r * y + center[1], r * z + center[2] return (x, y, z) def draw_Spheres(ax, balls): for i in balls: (xs, ys, zs) = CreateSphere(i[0:3], i[-1]) ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b") def draw_block_list(ax, blocks, color=None, alpha=0.15): ''' drawing the blocks on the graph ''' v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], dtype='float') f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) n = blocks.shape[0] d = blocks[:, 3:6] - blocks[:, :3] vl = np.zeros((8 * n, 3)) fl = np.zeros((6 * n, 4), dtype='int64') for k in range(n): vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] fl[k * 6:(k + 1) * 6, :] = f + k * 8 if type(ax) is Poly3DCollection: ax.set_verts(vl[fl]) else: h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k')) return h def obb_verts(obb): # 0.017004013061523438 for 1000 iters ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \ [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]]) # P + (ori * E) ori_body = np.multiply(ori_body, obb.E) # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) verts = (obb.O @ ori_body.T).T + obb.P return verts def draw_obb(ax, OBB, color=None, alpha=0.15): f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) n = OBB.shape[0] vl = np.zeros((8 * n, 3)) fl = np.zeros((6 * n, 4), dtype='int64') for k in range(n): vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) fl[k * 6:(k + 1) * 6, :] = f + k * 8 if type(ax) is Poly3DCollection: ax.set_verts(vl[fl]) else: h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k')) return h def draw_line(ax, SET, visibility=1, color=None): if SET != []: for i in SET: xs = i[0][0], i[1][0] ys = i[0][1], i[1][1] zs = i[0][2], i[1][2] line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) ax.add_line(line) def visualization(initparams): if initparams.ind % 100 == 0 or initparams.done: #----------- list structure # V = np.array(list(initparams.V)) # E = initparams.E #----------- end # edges = initparams.E Path = np.array(initparams.Path) start = initparams.env.start goal = initparams.env.goal # edges = E.get_edge() #----------- list structure edges = [] for i in initparams.Parent: edges.append([i,initparams.Parent[i]]) #----------- end # generate axis objects ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) ax.view_init(elev=65., azim=60.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, initparams.env.balls) draw_block_list(ax, initparams.env.blocks) if initparams.env.OBB is not None: draw_obb(ax, initparams.env.OBB) draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0) draw_line(ax, edges, visibility=0.75, color='g') draw_line(ax, Path, color='r') # if len(V) > 0: # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio ax.dist = 15 set_axes_equal(ax) make_transparent(ax) #plt.xlabel('s') #plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) def set_axes_equal(ax): '''Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to Input ax: a matplotlib axis, e.g., as output from plt.gca(). ''' x_limits = ax.get_xlim3d() y_limits = ax.get_ylim3d() z_limits = ax.get_zlim3d() x_range = abs(x_limits[1] - x_limits[0]) x_middle = np.mean(x_limits) y_range = abs(y_limits[1] - y_limits[0]) y_middle = np.mean(y_limits) z_range = abs(z_limits[1] - z_limits[0]) z_middle = np.mean(z_limits) # The plot bounding box is a sphere in the sense of the infinity # norm, hence I call half the max range the plot radius. plot_radius = 0.5*max([x_range, y_range, z_range]) ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) def make_transparent(ax): # make the panes transparent ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) # make the grid lines transparent ax.xaxis._axinfo["grid"]['color'] = (1,1,1,0) ax.yaxis._axinfo["grid"]['color'] = (1,1,1,0) ax.zaxis._axinfo["grid"]['color'] = (1,1,1,0) if __name__ == '__main__': pass ================================================ FILE: Sampling_based_Planning/rrt_3D/queue.py ================================================ # min heap used in the FMT* import collections import heapq import itertools class MinheapPQ: """ A priority queue based on min heap, which takes O(logn) on element removal https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes """ def __init__(self): self.pq = [] # lis of the entries arranged in a heap self.nodes = set() self.entry_finder = {} # mapping of the item entries self.counter = itertools.count() # unique sequence count self.REMOVED = '' def put(self, item, priority): '''add a new task or update the priority of an existing item''' if item in self.entry_finder: self.check_remove(item) count = next(self.counter) entry = [priority, count, item] self.entry_finder[item] = entry heapq.heappush(self.pq, entry) self.nodes.add(item) def put_set(self, dictin): '''add a new dict into the priority queue''' for item, priority in enumerate(dictin): self.put(item, priority) def check_remove(self, item): if item not in self.entry_finder: return entry = self.entry_finder.pop(item) entry[-1] = self.REMOVED self.nodes.remove(item) def check_remove_set(self, set_input): if len(set_input) == 0: return for item in set_input: if item not in self.entry_finder: continue entry = self.entry_finder.pop(item) entry[-1] = self.REMOVED self.nodes.remove(item) def priority_filtering(self, threshold, mode): # mode: bigger: check and remove those key vals bigger than threshold if mode == 'lowpass': for entry in self.enumerate(): item = entry[2] if entry[0] >= threshold: # priority _ = self.entry_finder.pop(item) entry[-1] = self.REMOVED self.nodes.remove(item) # mode: smaller: check and remove those key vals smaller than threshold elif mode == 'highpass': for entry in self.enumerate(): item = entry[2] if entry[0] <= threshold: # priority _ = self.entry_finder.pop(item) entry[-1] = self.REMOVED self.nodes.remove(item) def get(self): """Remove and return the lowest priority task. Raise KeyError if empty.""" while self.pq: priority, count, item = heapq.heappop(self.pq) if item is not self.REMOVED: del self.entry_finder[item] self.nodes.remove(item) return item raise KeyError('pop from an empty priority queue') def top_key(self): return self.pq[0][0] def enumerate(self): return self.pq def allnodes(self): return self.nodes ================================================ FILE: Sampling_based_Planning/rrt_3D/rrt3D.py ================================================ """ This is rrt star code for 3D @author: yue qi """ import numpy as np from numpy.matlib import repmat from collections import defaultdict import time import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path class rrt(): def __init__(self): self.env = env() self.Parent = {} self.V = [] # self.E = edgeset() self.i = 0 self.maxiter = 10000 self.stepsize = 0.5 self.Path = [] self.done = False self.x0 = tuple(self.env.start) self.xt = tuple(self.env.goal) self.ind = 0 # self.fig = plt.figure(figsize=(10, 8)) def wireup(self, x, y): # self.E.add_edge([s, y]) # add edge self.Parent[x] = y def run(self): self.V.append(self.x0) while self.ind < self.maxiter: xrand = sampleFree(self) xnearest = nearest(self, xrand) xnew, dist = steer(self, xnearest, xrand) collide, _ = isCollide(self, xnearest, xnew, dist=dist) if not collide: self.V.append(xnew) # add point self.wireup(xnew, xnearest) if getDist(xnew, self.xt) <= self.stepsize: self.wireup(self.xt, xnew) self.Path, D = path(self) print('Total distance = ' + str(D)) break visualization(self) self.i += 1 self.ind += 1 # if the goal is really reached self.done = True visualization(self) plt.show() if __name__ == '__main__': p = rrt() starttime = time.time() p.run() print('time used = ' + str(time.time() - starttime)) ================================================ FILE: Sampling_based_Planning/rrt_3D/rrt_connect3D.py ================================================ # rrt connect algorithm """ This is rrt connect implementation for 3D @author: yue qi """ import numpy as np from numpy.matlib import repmat from collections import defaultdict import time import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent class Tree(): def __init__(self, node): self.V = [] self.Parent = {} self.V.append(node) # self.Parent[node] = None def add_vertex(self, node): if node not in self.V: self.V.append(node) def add_edge(self, parent, child): # here edge is defined a tuple of (parent, child) (qnear, qnew) self.Parent[child] = parent class rrt_connect(): def __init__(self): self.env = env() self.Parent = {} self.V = [] self.E = set() self.i = 0 self.maxiter = 10000 self.stepsize = 0.5 self.Path = [] self.done = False self.qinit = tuple(self.env.start) self.qgoal = tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.qnew = None self.done = False self.ind = 0 self.fig = plt.figure(figsize=(10, 8)) #----------Normal RRT algorithm def BUILD_RRT(self, qinit): tree = Tree(qinit) for k in range(self.maxiter): qrand = self.RANDOM_CONFIG() self.EXTEND(tree, qrand) return tree def EXTEND(self, tree, q): qnear = tuple(self.NEAREST_NEIGHBOR(q, tree)) qnew, dist = steer(self, qnear, q) self.qnew = qnew # store qnew outside if self.NEW_CONFIG(q, qnear, qnew, dist=None): tree.add_vertex(qnew) tree.add_edge(qnear, qnew) if qnew == q: return 'Reached' else: return 'Advanced' return 'Trapped' def NEAREST_NEIGHBOR(self, q, tree): # find the nearest neighbor in the tree V = np.array(tree.V) if len(V) == 1: return V[0] xr = repmat(q, len(V), 1) dists = np.linalg.norm(xr - V, axis=1) return tuple(tree.V[np.argmin(dists)]) def RANDOM_CONFIG(self): return tuple(sampleFree(self)) def NEW_CONFIG(self, q, qnear, qnew, dist = None): # to check if the new configuration is valid or not by # making a move is used for steer # check in bound collide, _ = isCollide(self, qnear, qnew, dist = dist) return not collide #----------RRT connect algorithm def CONNECT(self, Tree, q): print('in connect') while True: S = self.EXTEND(Tree, q) if S != 'Advanced': break return S def RRT_CONNECT_PLANNER(self, qinit, qgoal): Tree_A = Tree(qinit) Tree_B = Tree(qgoal) for k in range(self.maxiter): print(k) qrand = self.RANDOM_CONFIG() if self.EXTEND(Tree_A, qrand) != 'Trapped': qnew = self.qnew # get qnew from outside if self.CONNECT(Tree_B, qnew) == 'Reached': print('reached') self.done = True self.Path = self.PATH(Tree_A, Tree_B) self.visualization(Tree_A, Tree_B, k) plt.show() return # return Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B) self.visualization(Tree_A, Tree_B, k) return 'Failure' # def PATH(self, tree_a, tree_b): def SWAP(self, tree_a, tree_b): tree_a, tree_b = tree_b, tree_a return tree_a, tree_b def PATH(self, tree_a, tree_b): qnew = self.qnew patha = [] pathb = [] while True: patha.append((tree_a.Parent[qnew], qnew)) qnew = tree_a.Parent[qnew] if qnew == self.qinit or qnew == self.qgoal: break qnew = self.qnew while True: pathb.append((tree_b.Parent[qnew], qnew)) qnew = tree_b.Parent[qnew] if qnew == self.qinit or qnew == self.qgoal: break return patha + pathb #----------RRT connect algorithm def visualization(self, tree_a, tree_b, index): if (index % 20 == 0 and index != 0) or self.done: # a_V = np.array(tree_a.V) # b_V = np.array(tree_b.V) Path = self.Path start = self.env.start goal = self.env.goal a_edges, b_edges = [], [] for i in tree_a.Parent: a_edges.append([i,tree_a.Parent[i]]) for i in tree_b.Parent: b_edges.append([i,tree_b.Parent[i]]) ax = plt.subplot(111, projection='3d') ax.view_init(elev=90., azim=0.) ax.clear() draw_Spheres(ax, self.env.balls) draw_block_list(ax, self.env.blocks) if self.env.OBB is not None: draw_obb(ax, self.env.OBB) draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, a_edges, visibility=0.75, color='g') draw_line(ax, b_edges, visibility=0.75, color='y') draw_line(ax, Path, color='r') ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') set_axes_equal(ax) make_transparent(ax) ax.set_axis_off() plt.pause(0.0001) if __name__ == '__main__': p = rrt_connect() starttime = time.time() p.RRT_CONNECT_PLANNER(p.qinit, p.qgoal) print('time used = ' + str(time.time() - starttime)) ================================================ FILE: Sampling_based_Planning/rrt_3D/rrt_star3D.py ================================================ """ This is rrt star code for 3D @author: yue qi """ import numpy as np from numpy.matlib import repmat from collections import defaultdict import time import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path class rrtstar(): def __init__(self): self.env = env() self.Parent = {} self.V = [] # self.E = edgeset() self.COST = {} self.i = 0 self.maxiter = 4000 # at least 2000 in this env self.stepsize = 2 self.gamma = 7 self.eta = self.stepsize self.Path = [] self.done = False self.x0 = tuple(self.env.start) self.xt = tuple(self.env.goal) self.V.append(self.x0) self.ind = 0 def wireup(self,x,y): # self.E.add_edge([s,y]) # add edge self.Parent[x] = y def removewire(self,xnear): xparent = self.Parent[xnear] a = [xnear,xparent] # self.E.remove_edge(a) # remove and replace old the connection def reached(self): self.done = True goal = self.xt xn = near(self,self.env.goal) c = [cost(self,tuple(x)) for x in xn] xncmin = xn[np.argmin(c)] self.wireup(goal , tuple(xncmin)) self.V.append(goal) self.Path,self.D = path(self) def run(self): xnew = self.x0 print('start rrt*... ') self.fig = plt.figure(figsize = (10,8)) while self.ind < self.maxiter: xrand = sampleFree(self) xnearest = nearest(self,xrand) xnew, dist = steer(self,xnearest,xrand) collide, _ = isCollide(self,xnearest,xnew,dist=dist) if not collide: Xnear = near(self,xnew) self.V.append(xnew) # add point visualization(self) plt.title('rrt*') # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) # connecting along minimal cost path Collide = [] for xnear in Xnear: xnear = tuple(xnear) c1 = cost(self, xnear) + getDist(xnew, xnear) collide, _ = isCollide(self, xnew, xnear) Collide.append(collide) if not collide and c1 < cmin: xmin, cmin = xnear, c1 self.wireup(xnew, xmin) # rewire for i in range(len(Xnear)): collide = Collide[i] xnear = tuple(Xnear[i]) c2 = cost(self, xnew) + getDist(xnew, xnear) if not collide and c2 < cost(self, xnear): # self.removewire(xnear) self.wireup(xnear, xnew) self.i += 1 self.ind += 1 # max sample reached self.reached() print('time used = ' + str(time.time()-starttime)) print('Total distance = '+str(self.D)) visualization(self) plt.show() if __name__ == '__main__': p = rrtstar() starttime = time.time() p.run() ================================================ FILE: Sampling_based_Planning/rrt_3D/rrt_star_smart3D.py ================================================ ================================================ FILE: Sampling_based_Planning/rrt_3D/utils3D.py ================================================ import numpy as np from numpy.matlib import repmat import pyrr as pyrr from collections import deque import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.plot_util3D import visualization def getRay(x, y): direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] return np.array([x, direc]) def getAABB(blocks): AABB = [] for i in blocks: AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger return AABB def getDist(pos1, pos2): return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) ''' The following utils can be used for rrt or rrt*, required param initparams should have env, environement generated from env3D V, node set E, edge set i, nodes added maxiter, maximum iteration allowed stepsize, leaf growth restriction ''' def sampleFree(initparams, bias = 0.1): '''biased sampling''' x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) i = np.random.random() if isinside(initparams, x): return sampleFree(initparams) else: if i < bias: return np.array(initparams.xt) + 1 else: return x return x # ---------------------- Collision checking algorithms def isinside(initparams, x): '''see if inside obstacle''' for i in initparams.env.blocks: if isinbound(i, x): return True for i in initparams.env.OBB: if isinbound(i, x, mode = 'obb'): return True for i in initparams.env.balls: if isinball(i, x): return True return False def isinbound(i, x, mode = False, factor = 0, isarray = False): if mode == 'obb': return isinobb(i, x, isarray) if isarray: compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) return compx & compy & compz else: return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] def isinobb(i, x, isarray = False): # transform the point from {W} to {body} if isarray: pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3] block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] return isinbound(block, pts, isarray = isarray) else: pt = i.T@np.append(x,1) block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] return isinbound(block, pt) def isinball(i, x, factor = 0): if getDist(i[0:3], x) <= i[3] + factor: return True return False def lineSphere(p0, p1, ball): # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf c, r = ball[0:3], ball[-1] line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]] d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]] t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * ( line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2]) if t <= 0: if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True elif t >= 1: d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True elif 0 < t < 1: x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True return False def lineAABB(p0, p1, dist, aabb): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 # aabb should have the attributes of P, E as center point and extents mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction hl = dist / 2 # radius T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] # do any of the principal axis form a separting axis? if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False # I.cross(s axis) ? r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) if abs(T[1] * I[2] - T[2] * I[1]) > r: return False # I.cross(y axis) ? r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) if abs(T[2] * I[0] - T[0] * I[2]) > r: return False # I.cross(z axis) ? r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) if abs(T[0] * I[1] - T[1] * I[0]) > r: return False return True def lineOBB(p0, p1, dist, obb): # transform points to obb frame res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T # record old position and set the position to origin oldP, obb.P= obb.P, [0,0,0] # calculate segment-AABB testing ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) # reset the position obb.P = oldP return ans def isCollide(initparams, x, child, dist=None): '''see if line intersects obstacle''' '''specified for expansion in A* 3D lookup table''' if dist==None: dist = getDist(x, child) # check in bound if not isinbound(initparams.env.boundary, child): return True, dist # check collision in AABB for i in range(len(initparams.env.AABB)): if lineAABB(x, child, dist, initparams.env.AABB[i]): return True, dist # check collision in ball for i in initparams.env.balls: if lineSphere(x, child, i): return True, dist # check collision with obb for i in initparams.env.OBB: if lineOBB(x, child, dist, i): return True, dist return False, dist # ---------------------- leaf node extending algorithms def nearest(initparams, x, isset=False): V = np.array(initparams.V) if initparams.i == 0: return initparams.V[0] xr = repmat(x, len(V), 1) dists = np.linalg.norm(xr - V, axis=1) return tuple(initparams.V[np.argmin(dists)]) def near(initparams, x): # s = np.array(s) V = np.array(initparams.V) if initparams.i == 0: return [initparams.V[0]] cardV = len(initparams.V) eta = initparams.eta gamma = initparams.gamma # min{γRRT∗ (log(card (V ))/ card (V ))1/d, η} r = min(gamma * ((np.log(cardV) / cardV) ** (1/3)), eta) if initparams.done: r = 1 xr = repmat(x, len(V), 1) inside = np.linalg.norm(xr - V, axis=1) < r nearpoints = V[inside] return np.array(nearpoints) def steer(initparams, x, y, DIST=False): # steer from s to y if np.equal(x, y).all(): return x, 0.0 dist, step = getDist(y, x), initparams.stepsize step = min(dist, step) increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step) xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) # direc = (y - s) / np.linalg.norm(y - s) # xnew = s + initparams.stepsize * direc if DIST: return xnew, dist return xnew, dist def cost(initparams, x): '''here use the additive recursive cost function''' if x == initparams.x0: return 0 return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x]) def cost_from_set(initparams, x): '''here use a incremental cost set function''' if x == initparams.x0: return 0 return initparams.COST[initparams.Parent[x]] + getDist(x, initparams.Parent[x]) def path(initparams, Path=[], dist=0): x = initparams.xt while x != initparams.x0: x2 = initparams.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 return Path, dist class edgeset(object): def __init__(self): self.E = {} def add_edge(self, edge): x, y = edge[0], edge[1] if x in self.E: self.E[x].add(y) else: self.E[x] = set() self.E[x].add(y) def remove_edge(self, edge): x, y = edge[0], edge[1] self.E[x].remove(y) def get_edge(self, nodes = None): edges = [] if nodes is None: for v in self.E: for n in self.E[v]: # if (n,v) not in edges: edges.append((v, n)) else: for v in nodes: for n in self.E[tuple(v)]: edges.append((v, n)) return edges def isEndNode(self, node): return node not in self.E #------------------------ use a linked list to express the tree class Node: def __init__(self, data): self.pos = data self.Parent = None self.child = set() def tree_add_edge(node_in_tree, x): # add an edge at the specified parent node_to_add = Node(x) # node_in_tree = tree_bfs(head, xparent) node_in_tree.child.add(node_to_add) node_to_add.Parent = node_in_tree return node_to_add def tree_bfs(head, x): # searches s in order of bfs node = head Q = [] Q.append(node) while Q: curr = Q.pop() if curr.pos == x: return curr for child_node in curr.child: Q.append(child_node) def tree_nearest(head, x): # find the node nearest to s D = np.inf min_node = None Q = [] Q.append(head) while Q: curr = Q.pop() dist = getDist(curr.pos, x) # record the current best if dist < D: D, min_node = dist, curr # bfs for child_node in curr.child: Q.append(child_node) return min_node def tree_steer(initparams, node, x): # steer from node to s dist, step = getDist(node.pos, x), initparams.stepsize increment = ((node.pos[0] - x[0]) / dist * step, (node.pos[1] - x[1]) / dist * step, (node.pos[2] - x[2]) / dist * step) xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) return xnew def tree_print(head): Q = [] Q.append(head) verts = [] edge = [] while Q: curr = Q.pop() # print(curr.pos) verts.append(curr.pos) if curr.Parent == None: pass else: edge.append([curr.pos, curr.Parent.pos]) for child in curr.child: Q.append(child) return verts, edge def tree_path(initparams, end_node): path = [] curr = end_node while curr.pos != initparams.x0: path.append([curr.pos, curr.Parent.pos]) curr = curr.Parent return path #---------------KD tree, used for nearest neighbor search class kdTree: def __init__(self): pass def R1_dist(self, q, p): return abs(q-p) def S1_dist(self, q, p): return min(abs(q-p), 1- abs(q-p)) def P3_dist(self, q, p): # cubes with antipodal points q1, q2, q3 = q p1, p2, p3 = p d1 = np.sqrt((q1-p1)**2 + (q2-p2)**2 + (q3-p3)**2) d2 = np.sqrt((1-abs(q1-p1))**2 + (1-abs(q2-p2))**2 + (1-abs(q3-p3))**2) d3 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3+1-p3)**2) d4 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3-1-p3)**2) d5 = np.sqrt((-q1-p1)**2 + (q2+1-p2)**2 + (-q3-p3)**2) d6 = np.sqrt((-q1-p1)**2 + (q2-1-p2)**2 + (-q3-p3)**2) d7 = np.sqrt((q1+1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2) d8 = np.sqrt((q1-1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2) return min(d1,d2,d3,d4,d5,d6,d7,d8) if __name__ == '__main__': from rrt_3D.env3D import env import time import matplotlib.pyplot as plt class rrt_demo: def __init__(self): self.env = env() self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.stepsize = 0.5 self.maxiter = 10000 self.ind, self.i = 0, 0 self.done = False self.Path = [] self.V = [] self.head = Node(self.x0) def run(self): while self.ind < self.maxiter: xrand = sampleFree(self) # O(1) nearest_node = tree_nearest(self.head, xrand) # O(N) xnew = tree_steer(self, nearest_node, xrand) # O(1) collide, _ = isCollide(self, nearest_node.pos, xnew) # O(num obs) if not collide: new_node = tree_add_edge(nearest_node, xnew) # O(1) # if the path is found if getDist(xnew, self.xt) <= self.stepsize: end_node = tree_add_edge(new_node, self.xt) self.Path = tree_path(self, end_node) break self.i += 1 self.ind += 1 self.done = True self.V, self.E = tree_print(self.head) print(self.E) visualization(self) plt.show() A = rrt_demo() st = time.time() A.run() print(time.time() - st) ================================================ FILE: Search_based_Planning/Search_2D/ARAstar.py ================================================ """ ARA_star 2D (Anytime Repairing A*) @author: huiming zhou @description: local inconsistency: g-value decreased. g(s) decreased introduces a local inconsistency between s and its successors. """ import os import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env class AraStar: def __init__(self, s_start, s_goal, e, 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.e = e # weight self.g = dict() # Cost to come self.OPEN = dict() # priority queue / OPEN set self.CLOSED = set() # CLOSED set self.INCONS = {} # INCONSISTENT set self.PARENT = dict() # relations self.path = [] # planning path self.visited = [] # order of visited nodes def init(self): """ initialize each set. """ self.g[self.s_start] = 0.0 self.g[self.s_goal] = math.inf self.OPEN[self.s_start] = self.f_value(self.s_start) self.PARENT[self.s_start] = self.s_start def searching(self): self.init() self.ImprovePath() self.path.append(self.extract_path()) while self.update_e() > 1: # continue condition self.e -= 0.4 # increase weight self.OPEN.update(self.INCONS) self.OPEN = {s: self.f_value(s) for s in self.OPEN} # update f_value of OPEN set self.INCONS = dict() self.CLOSED = set() self.ImprovePath() # improve path self.path.append(self.extract_path()) return self.path, self.visited def ImprovePath(self): """ :return: a e'-suboptimal path """ visited_each = [] while True: s, f_small = self.calc_smallest_f() if self.f_value(self.s_goal) <= f_small: break self.OPEN.pop(s) self.CLOSED.add(s) for s_n in self.get_neighbor(s): if s_n in self.obs: continue new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g or new_cost < self.g[s_n]: self.g[s_n] = new_cost self.PARENT[s_n] = s visited_each.append(s_n) if s_n not in self.CLOSED: self.OPEN[s_n] = self.f_value(s_n) else: self.INCONS[s_n] = 0.0 self.visited.append(visited_each) def calc_smallest_f(self): """ :return: node with smallest f_value in OPEN set. """ s_small = min(self.OPEN, key=self.OPEN.get) return s_small, self.OPEN[s_small] def get_neighbor(self, s): """ find neighbors of state s that not in obstacles. :param s: state :return: neighbors """ return {(s[0] + u[0], s[1] + u[1]) for u in self.u_set} def update_e(self): v = float("inf") if self.OPEN: v = min(self.g[s] + self.h(s) for s in self.OPEN) if self.INCONS: v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS)) return min(self.e, self.g[self.s_goal] / v) def f_value(self, x): """ f = g + e * h f = cost-to-come + weight * cost-to-go :param x: current state :return: f_value """ return self.g[x] + self.e * self.h(x) def extract_path(self): """ Extract the path based on the PARENT set. :return: The planning path """ path = [self.s_goal] s = self.s_goal while True: s = self.PARENT[s] path.append(s) if s == self.s_start: break return list(path) 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 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_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 math.inf return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) def is_collision(self, s_start, s_end): """ check if the line segment (s_start, s_end) is collision. :param s_start: start node :param s_end: end node :return: True: is collision / False: not collision """ 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 main(): s_start = (5, 5) s_goal = (45, 25) arastar = AraStar(s_start, s_goal, 2.5, "euclidean") plot = plotting.Plotting(s_start, s_goal) path, visited = arastar.searching() plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)") if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/Anytime_D_star.py ================================================ """ Anytime_D_star 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 ADStar: def __init__(self, s_start, s_goal, eps, 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.OPEN = {}, {}, {} for i in range(1, self.Env.x_range - 1): for j in range(1, self.Env.y_range - 1): self.rhs[(i, j)] = float("inf") self.g[(i, j)] = float("inf") self.rhs[self.s_goal] = 0.0 self.eps = eps self.OPEN[self.s_goal] = self.Key(self.s_goal) self.CLOSED, self.INCONS = set(), dict() self.visited = set() self.count = 0 self.count_env_change = 0 self.obs_add = set() self.obs_remove = set() self.title = "Anytime D*: Small changes" # Significant changes self.fig = plt.figure() def run(self): self.Plot.plot_grid(self.title) self.ComputeOrImprovePath() self.plot_visited() self.plot_path(self.extract_path()) self.visited = set() while True: if self.eps <= 1.0: break self.eps -= 0.5 self.OPEN.update(self.INCONS) for s in self.OPEN: self.OPEN[s] = self.Key(s) self.CLOSED = set() self.ComputeOrImprovePath() self.plot_visited() self.plot_path(self.extract_path()) self.visited = set() plt.pause(0.5) 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: self.count_env_change += 1 x, y = int(x), int(y) print("Change position: s =", x, ",", "y =", y) # for small changes if self.title == "Anytime D*: Small changes": if (x, y) not in self.obs: self.obs.add((x, y)) self.g[(x, y)] = float("inf") self.rhs[(x, y)] = float("inf") else: self.obs.remove((x, y)) self.UpdateState((x, y)) self.Plot.update_obs(self.obs) for sn in self.get_neighbor((x, y)): self.UpdateState(sn) plt.cla() self.Plot.plot_grid(self.title) while True: if len(self.INCONS) == 0: break self.OPEN.update(self.INCONS) for s in self.OPEN: self.OPEN[s] = self.Key(s) self.CLOSED = set() self.ComputeOrImprovePath() self.plot_visited() self.plot_path(self.extract_path()) # plt.plot(self.title) self.visited = set() if self.eps <= 1.0: break else: if (x, y) not in self.obs: self.obs.add((x, y)) self.obs_add.add((x, y)) plt.plot(x, y, 'sk') if (x, y) in self.obs_remove: self.obs_remove.remove((x, y)) else: self.obs.remove((x, y)) self.obs_remove.add((x, y)) plt.plot(x, y, marker='s', color='white') if (x, y) in self.obs_add: self.obs_add.remove((x, y)) self.Plot.update_obs(self.obs) if self.count_env_change >= 15: self.count_env_change = 0 self.eps += 2.0 for s in self.obs_add: self.g[(x, y)] = float("inf") self.rhs[(x, y)] = float("inf") for sn in self.get_neighbor(s): self.UpdateState(sn) for s in self.obs_remove: for sn in self.get_neighbor(s): self.UpdateState(sn) self.UpdateState(s) plt.cla() self.Plot.plot_grid(self.title) while True: if self.eps <= 1.0: break self.eps -= 0.5 self.OPEN.update(self.INCONS) for s in self.OPEN: self.OPEN[s] = self.Key(s) self.CLOSED = set() self.ComputeOrImprovePath() self.plot_visited() self.plot_path(self.extract_path()) plt.title(self.title) self.visited = set() plt.pause(0.5) self.fig.canvas.draw_idle() def ComputeOrImprovePath(self): while True: s, v = self.TopKey() if v >= self.Key(self.s_start) and \ self.rhs[self.s_start] == self.g[self.s_start]: break self.OPEN.pop(s) self.visited.add(s) if self.g[s] > self.rhs[s]: self.g[s] = self.rhs[s] self.CLOSED.add(s) for sn in self.get_neighbor(s): self.UpdateState(sn) else: self.g[s] = float("inf") for sn in self.get_neighbor(s): self.UpdateState(sn) self.UpdateState(s) def UpdateState(self, s): 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.cost(s, x)) if s in self.OPEN: self.OPEN.pop(s) if self.g[s] != self.rhs[s]: if s not in self.CLOSED: self.OPEN[s] = self.Key(s) else: self.INCONS[s] = 0 def Key(self, s): if self.g[s] > self.rhs[s]: return [self.rhs[s] + self.eps * self.h(self.s_start, s), self.rhs[s]] else: return [self.g[s] + self.h(self.s_start, s), self.g[s]] def TopKey(self): """ :return: return the min key and its value. """ s = min(self.OPEN, key=self.OPEN.get) return s, self.OPEN[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): 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 extract_path(self): """ Extract the path based on the PARENT set. :return: The planning path """ path = [self.s_start] s = self.s_start for k in range(100): g_list = {} for x in self.get_neighbor(s): if not self.is_collision(s, x): g_list[x] = self.g[x] s = min(g_list, key=g_list.get) path.append(s) if s == self.s_goal: break return list(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): self.count += 1 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 self.visited: plt.plot(x[0], x[1], marker='s', color=color[self.count]) def main(): s_start = (5, 5) s_goal = (45, 25) dstar = ADStar(s_start, s_goal, 2.5, "euclidean") dstar.run() if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/Astar.py ================================================ """ A_star 2D @author: huiming zhou """ import os import sys import math import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env class AStar: """AStar set the cost + heuristics as the priority """ def __init__(self, s_start, s_goal, heuristic_type): self.s_start = s_start self.s_goal = 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.OPEN = [] # priority queue / OPEN set self.CLOSED = [] # CLOSED set / VISITED order self.PARENT = dict() # recorded parent self.g = dict() # cost to come def searching(self): """ A_star Searching. :return: path, visited order """ self.PARENT[self.s_start] = self.s_start self.g[self.s_start] = 0 self.g[self.s_goal] = math.inf heapq.heappush(self.OPEN, (self.f_value(self.s_start), self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) if s == self.s_goal: # stop condition break for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = math.inf if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s heapq.heappush(self.OPEN, (self.f_value(s_n), s_n)) return self.extract_path(self.PARENT), self.CLOSED def searching_repeated_astar(self, e): """ repeated A*. :param e: weight of A* :return: path and visited order """ path, visited = [], [] while e >= 1: p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e) path.append(p_k) visited.append(v_k) e -= 0.5 return path, visited 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")} PARENT = {s_start: s_start} OPEN = [] CLOSED = [] heapq.heappush(OPEN, (g[s_start] + e * self.heuristic(s_start), s_start)) while OPEN: _, s = heapq.heappop(OPEN) CLOSED.append(s) if s == s_goal: break for s_n in self.get_neighbor(s): new_cost = g[s] + self.cost(s, s_n) if s_n not in g: g[s_n] = math.inf if new_cost < g[s_n]: # conditions for updating Cost g[s_n] = new_cost PARENT[s_n] = s heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n)) return self.extract_path(PARENT), CLOSED def get_neighbor(self, s): """ find neighbors of state s that not in obstacles. :param s: state :return: neighbors """ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] 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 math.inf return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) def is_collision(self, s_start, s_end): """ check if the line segment (s_start, s_end) is collision. :param s_start: start node :param s_end: end node :return: True: is collision / False: not collision """ 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 f_value(self, s): """ f = g + h. (g: Cost to come, h: heuristic value) :param s: current state :return: f """ return self.g[s] + self.heuristic(s) def extract_path(self, PARENT): """ Extract the path based on the PARENT set. :return: The planning path """ path = [self.s_goal] s = self.s_goal while True: s = PARENT[s] path.append(s) if s == self.s_start: break return list(path) def heuristic(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 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 main(): s_start = (5, 5) s_goal = (45, 25) astar = AStar(s_start, s_goal, "euclidean") plot = plotting.Plotting(s_start, s_goal) path, visited = astar.searching() plot.animation(path, visited, "A*") # animation # path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5 # plot.animation_ara_star(path, visited, "Repeated A*") if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/Best_First.py ================================================ """ Best-First Searching @author: huiming zhou """ import os import sys import math import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env from Search_2D.Astar import AStar class BestFirst(AStar): """BestFirst set the heuristics as the priority """ def searching(self): """ Breadth-first Searching. :return: path, visited order """ self.PARENT[self.s_start] = self.s_start self.g[self.s_start] = 0 self.g[self.s_goal] = math.inf heapq.heappush(self.OPEN, (self.heuristic(self.s_start), self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) if s == self.s_goal: break for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = math.inf if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s # best first set the heuristics as the priority heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n)) return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) BF = BestFirst(s_start, s_goal, 'euclidean') plot = plotting.Plotting(s_start, s_goal) path, visited = BF.searching() plot.animation(path, visited, "Best-first Searching") # animation if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/Bidirectional_a_star.py ================================================ """ Bidirectional_a_star 2D @author: huiming zhou """ import os import sys import math import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env class BidirectionalAStar: def __init__(self, s_start, s_goal, heuristic_type): self.s_start = s_start self.s_goal = 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.OPEN_fore = [] # OPEN set for forward searching self.OPEN_back = [] # OPEN set for backward searching self.CLOSED_fore = [] # CLOSED set for forward self.CLOSED_back = [] # CLOSED set for backward self.PARENT_fore = dict() # recorded parent for forward self.PARENT_back = dict() # recorded parent for backward self.g_fore = dict() # cost to come for forward self.g_back = dict() # cost to come for backward def init(self): """ initialize parameters """ self.g_fore[self.s_start] = 0.0 self.g_fore[self.s_goal] = math.inf self.g_back[self.s_goal] = 0.0 self.g_back[self.s_start] = math.inf self.PARENT_fore[self.s_start] = self.s_start self.PARENT_back[self.s_goal] = self.s_goal heapq.heappush(self.OPEN_fore, (self.f_value_fore(self.s_start), self.s_start)) heapq.heappush(self.OPEN_back, (self.f_value_back(self.s_goal), self.s_goal)) def searching(self): """ Bidirectional A* :return: connected path, visited order of forward, visited order of backward """ self.init() s_meet = self.s_start while self.OPEN_fore and self.OPEN_back: # solve foreward-search _, s_fore = heapq.heappop(self.OPEN_fore) if s_fore in self.PARENT_back: s_meet = s_fore break self.CLOSED_fore.append(s_fore) for s_n in self.get_neighbor(s_fore): new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n) if s_n not in self.g_fore: self.g_fore[s_n] = math.inf if new_cost < self.g_fore[s_n]: self.g_fore[s_n] = new_cost self.PARENT_fore[s_n] = s_fore heapq.heappush(self.OPEN_fore, (self.f_value_fore(s_n), s_n)) # solve backward-search _, s_back = heapq.heappop(self.OPEN_back) if s_back in self.PARENT_fore: s_meet = s_back break self.CLOSED_back.append(s_back) for s_n in self.get_neighbor(s_back): new_cost = self.g_back[s_back] + self.cost(s_back, s_n) if s_n not in self.g_back: self.g_back[s_n] = math.inf if new_cost < self.g_back[s_n]: self.g_back[s_n] = new_cost self.PARENT_back[s_n] = s_back heapq.heappush(self.OPEN_back, (self.f_value_back(s_n), s_n)) return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back def get_neighbor(self, s): """ find neighbors of state s that not in obstacles. :param s: state :return: neighbors """ return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] def extract_path(self, s_meet): """ extract path from start and goal :param s_meet: meet point of bi-direction a* :return: path """ # extract path for foreward part path_fore = [s_meet] s = s_meet while True: s = self.PARENT_fore[s] path_fore.append(s) if s == self.s_start: break # extract path for backward part path_back = [] s = s_meet while True: s = self.PARENT_back[s] path_back.append(s) if s == self.s_goal: break return list(reversed(path_fore)) + list(path_back) def f_value_fore(self, s): """ forward searching: f = g + h. (g: Cost to come, h: heuristic value) :param s: current state :return: f """ return self.g_fore[s] + self.h(s, self.s_goal) def f_value_back(self, s): """ backward searching: f = g + h. (g: Cost to come, h: heuristic value) :param s: current state :return: f """ return self.g_back[s] + self.h(s, self.s_start) def h(self, s, goal): """ Calculate heuristic value. :param s: current node (state) :param goal: goal node (state) :return: heuristic value """ heuristic_type = self.heuristic_type 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_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 math.inf return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) def is_collision(self, s_start, s_end): """ check if the line segment (s_start, s_end) is collision. :param s_start: start node :param s_end: end node :return: True: is collision / False: not collision """ 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 main(): x_start = (5, 5) x_goal = (45, 25) bastar = BidirectionalAStar(x_start, x_goal, "euclidean") plot = plotting.Plotting(x_start, x_goal) path, visited_fore, visited_back = bastar.searching() plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/D_star.py ================================================ """ D_star 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, env class DStar: def __init__(self, s_start, s_goal): self.s_start, self.s_goal = s_start, s_goal self.Env = env.Env() 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.fig = plt.figure() self.OPEN = set() self.t = dict() self.PARENT = dict() self.h = dict() self.k = dict() self.path = [] self.visited = set() self.count = 0 def init(self): 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.0 self.h[(i, j)] = float("inf") self.PARENT[(i, j)] = None self.h[self.s_goal] = 0.0 def run(self, s_start, s_end): self.init() 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) if (x, y) not in self.obs: print("Add obstacle at: s =", x, ",", "y =", y) self.obs.add((x, y)) self.Plot.update_obs(self.obs) s = self.s_start self.visited = set() self.count += 1 while s != self.s_goal: if self.is_collision(s, self.PARENT[s]): self.modify(s) continue s = self.PARENT[s] self.path = self.extract_path(self.s_start, self.s_goal) plt.cla() self.Plot.plot_grid("Dynamic A* (D*)") self.plot_visited(self.visited) self.plot_path(self.path) self.fig.canvas.draw_idle() def extract_path(self, s_start, s_end): path = [s_start] s = s_start while True: s = self.PARENT[s] path.append(s) if s == s_end: return path def process_state(self): s = self.min_state() # get node in OPEN set with min k value self.visited.add(s) if s is None: return -1 # OPEN set is empty k_old = self.get_k_min() # record the min k value of this iteration (min path cost) self.delete(s) # move state s from OPEN set to CLOSED set # k_min < h[s] --> s: RAISE state (increased cost) 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): # update h_value and choose parent self.PARENT[s] = s_n self.h[s] = self.h[s_n] + self.cost(s_n, s) # s: k_min >= h[s] -- > s: LOWER state (cost reductions) 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)): # Condition: # 1) t[s_n] == 'NEW': not visited # 2) s_n's parent: cost reduction # 3) s_n find a better parent 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)): # Condition: # 1) t[s_n] == 'NEW': not visited # 2) s_n's parent: cost reduction 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): # Condition: LOWER happened in OPEN set (s), s should be explored again 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: # Condition: LOWER happened in CLOSED set (s_n), s_n should be explored again self.insert(s_n, self.h[s_n]) return self.get_k_min() def min_state(self): """ choose the node with the minimum k value in OPEN set. :return: state """ if not self.OPEN: return None return min(self.OPEN, key=lambda x: self.k[x]) def get_k_min(self): """ calc the min k value for nodes in OPEN set. :return: k value """ if not self.OPEN: return -1 return min([self.k[x] for x in self.OPEN]) def insert(self, s, h_new): """ insert node into OPEN set. :param s: node :param h_new: new or better cost to come value """ 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): """ delete: move state s from OPEN set to CLOSED set. :param s: state should be deleted """ if self.t[s] == 'OPEN': self.t[s] = 'CLOSED' self.OPEN.remove(s) def modify(self, s): """ start processing from state s. :param s: is a node whose status is RAISE or LOWER. """ self.modify_cost(s) while True: k_min = self.process_state() if k_min >= self.h[s]: break def modify_cost(self, s): # if node in CLOSED set, put it into OPEN set. # Since cost may be changed between s - s.parent, calc cost(s, s.p) again 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_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 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) dstar = DStar(s_start, s_goal) dstar.run(s_start, s_goal) if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/D_star_Lite.py ================================================ """ D_star_Lite 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, env class DStar: 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.km = 0 for i in range(1, self.Env.x_range - 1): for j in range(1, self.Env.y_range - 1): self.rhs[(i, j)] = float("inf") self.g[(i, j)] = float("inf") 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("D* Lite") self.ComputePath() 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: s =", x, ",", "y =", y) s_curr = self.s_start s_last = self.s_start i = 0 path = [self.s_start] while s_curr != self.s_goal: s_list = {} for s in self.get_neighbor(s_curr): s_list[s] = self.g[s] + self.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.count += 1 self.visited = set() self.ComputePath() self.plot_visited(self.visited) self.plot_path(path) self.fig.canvas.draw_idle() def ComputePath(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: self.rhs[s] = float("inf") for x in self.get_neighbor(s): self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x)) if s in self.U: self.U.pop(s) 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 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): 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 extract_path(self): """ Extract the path based on the PARENT set. :return: The planning path """ path = [self.s_start] s = self.s_start for k in range(100): g_list = {} for x in self.get_neighbor(s): if not self.is_collision(s, x): g_list[x] = self.g[x] s = min(g_list, key=g_list.get) path.append(s) if s == self.s_goal: break return list(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) dstar = DStar(s_start, s_goal, "euclidean") dstar.run() if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/Dijkstra.py ================================================ """ Dijkstra 2D @author: huiming zhou """ import os import sys import math import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env from Search_2D.Astar import AStar class Dijkstra(AStar): """Dijkstra set the cost as the priority """ def searching(self): """ Breadth-first Searching. :return: path, visited order """ self.PARENT[self.s_start] = self.s_start self.g[self.s_start] = 0 self.g[self.s_goal] = math.inf heapq.heappush(self.OPEN, (0, self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) if s == self.s_goal: break for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = math.inf if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s # best first set the heuristics as the priority heapq.heappush(self.OPEN, (new_cost, s_n)) return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) dijkstra = Dijkstra(s_start, s_goal, 'None') plot = plotting.Plotting(s_start, s_goal) path, visited = dijkstra.searching() plot.animation(path, visited, "Dijkstra's") # animation generate if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/LPAstar.py ================================================ """ LPA_star 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, env class LPAStar: 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(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.g, self.rhs, self.U = {}, {}, {} 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.rhs[self.s_start] = 0 self.U[self.s_start] = self.CalculateKey(self.s_start) self.visited = set() self.count = 0 self.fig = plt.figure() def run(self): self.Plot.plot_grid("Lifelong Planning A*") 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: s =", x, ",", "y =", y) self.visited = set() self.count += 1 if (x, y) not in self.obs: self.obs.add((x, y)) else: self.obs.remove((x, y)) self.UpdateVertex((x, y)) self.Plot.update_obs(self.obs) for s_n in self.get_neighbor((x, y)): self.UpdateVertex(s_n) self.ComputeShortestPath() plt.cla() self.Plot.plot_grid("Lifelong Planning A*") 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_goal) and \ self.rhs[self.s_goal] == self.g[self.s_goal]: break self.U.pop(s) self.visited.add(s) if self.g[s] > self.rhs[s]: # Condition: over-consistent (eg: deleted obstacles) # So, rhs[s] decreased -- > rhs[s] < g[s] self.g[s] = self.rhs[s] else: # Condition: # under-consistent (eg: added obstacles) # So, rhs[s] increased --> rhs[s] > g[s] self.g[s] = float("inf") self.UpdateVertex(s) for s_n in self.get_neighbor(s): self.UpdateVertex(s_n) def UpdateVertex(self, s): """ update the status and the current cost to come of state s. :param s: state s """ if s != self.s_start: # Condition: cost of parent of s changed # Since we do not record the children of a state, we need to enumerate its neighbors 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]: # Condition: current cost to come is different to that of last time # state s should be added into OPEN set (set U) 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): """ 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 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 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_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 extract_path(self): """ Extract the path based on the PARENT set. :return: The planning path """ path = [self.s_goal] s = self.s_goal for k in range(100): g_list = {} for x in self.get_neighbor(s): if not self.is_collision(s, x): g_list[x] = self.g[x] s = min(g_list, key=g_list.get) path.append(s) if s == self.s_start: break 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(): x_start = (5, 5) x_goal = (45, 25) lpastar = LPAStar(x_start, x_goal, "Euclidean") lpastar.run() if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/LRTAstar.py ================================================ """ LRTA_star 2D (Learning Real-time A*) @author: huiming zhou """ import os import sys import copy import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import queue, plotting, env class LrtAStarN: 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() self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles self.N = N # number of expand nodes each iteration self.visited = [] # order of visited nodes in planning self.path = [] # path of each iteration self.h_table = {} # h_value table def init(self): """ initialize the h_value of all nodes in the environment. it is a global table. """ for i in range(self.Env.x_range): for j in range(self.Env.y_range): self.h_table[(i, j)] = self.h((i, j)) def searching(self): self.init() s_start = self.s_start # initialize start node while True: OPEN, CLOSED = self.AStar(s_start, self.N) # OPEN, CLOSED sets in each iteration if OPEN == "FOUND": # reach the goal node self.path.append(CLOSED) break h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes for x in h_value: self.h_table[x] = h_value[x] s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set self.path.append(path_k) def extract_path_in_CLOSE(self, 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] 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 if s_key not in h_value: # reach the expected node in OPEN set return s_key, path def iteration(self, CLOSED): h_value = {} for s in CLOSED: h_value[s] = float("inf") # initialize h_value of CLOSED nodes while True: h_value_rec = copy.deepcopy(h_value) for s in CLOSED: h_list = [] 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 return h_value def AStar(self, x_start, N): OPEN = queue.QueuePrior() # OPEN set OPEN.put(x_start, self.h(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 def get_neighbor(self, s): """ find neighbors of state s that not in obstacles. :param s: state :return: neighbors """ 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, x_start, parent): """ Extract the path based on the relationship of nodes. :return: The planning path """ path_back = [self.s_goal] x_current = self.s_goal while True: x_current = parent[x_current] path_back.append(x_current) if x_current == x_start: break return list(reversed(path_back)) 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 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_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 main(): s_start = (10, 5) s_goal = (45, 25) lrta = LrtAStarN(s_start, s_goal, 250, "euclidean") plot = plotting.Plotting(s_start, s_goal) lrta.searching() plot.animation_lrta(lrta.path, lrta.visited, "Learning Real-time A* (LRTA*)") if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/RTAAStar.py ================================================ """ RTAAstar 2D (Real-time Adaptive A*) @author: huiming zhou """ import os import sys import copy import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import queue, plotting, env class RTAAStar: 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() self.u_set = self.Env.motions # feasible input set self.obs = self.Env.obs # position of obstacles self.N = N # number of expand nodes each iteration self.visited = [] # order of visited nodes in planning self.path = [] # path of each iteration self.h_table = {} # h_value table def init(self): """ initialize the h_value of all nodes in the environment. it is a global table. """ for i in range(self.Env.x_range): for j in range(self.Env.y_range): self.h_table[(i, j)] = self.h((i, j)) def searching(self): self.init() s_start = self.s_start # initialize start node while True: OPEN, CLOSED, g_table, PARENT = \ self.Astar(s_start, self.N) if OPEN == "FOUND": # reach the goal node self.path.append(CLOSED) break s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT) for x in h_value: self.h_table[x] = h_value[x] s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value) self.path.append(path_k) def cal_h_value(self, OPEN, CLOSED, g_table, PARENT): v_open = {} h_value = {} 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 = v_open[s_open] for x in CLOSED: h_value[x] = f_min - g_table[x] return s_open, h_value def iteration(self, CLOSED): h_value = {} for s in CLOSED: h_value[s] = float("inf") # initialize h_value of CLOSED nodes while True: h_value_rec = copy.deepcopy(h_value) for s in CLOSED: h_list = [] 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 return h_value 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 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 OPEN 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 = [self.s_goal] s = self.s_goal while True: s = parent[s] path.append(s) if s == x_start: break return list(reversed(path)) 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 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_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 main(): s_start = (10, 5) s_goal = (45, 25) rtaa = RTAAStar(s_start, s_goal, 240, "euclidean") plot = plotting.Plotting(s_start, s_goal) rtaa.searching() plot.animation_lrta(rtaa.path, rtaa.visited, "Real-time Adaptive A* (RTAA*)") if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/bfs.py ================================================ """ Breadth-first Searching_2D (BFS) @author: huiming zhou """ import os import sys from collections import deque sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env from Search_2D.Astar import AStar import math import heapq class BFS(AStar): """BFS add the new visited node in the end of the openset """ def searching(self): """ Breadth-first Searching. :return: path, visited order """ self.PARENT[self.s_start] = self.s_start self.g[self.s_start] = 0 self.g[self.s_goal] = math.inf heapq.heappush(self.OPEN, (0, self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) if s == self.s_goal: break for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = math.inf if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s # bfs, add new node to the end of the openset prior = self.OPEN[-1][0]+1 if len(self.OPEN)>0 else 0 heapq.heappush(self.OPEN, (prior, s_n)) return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) bfs = BFS(s_start, s_goal, 'None') plot = plotting.Plotting(s_start, s_goal) path, visited = bfs.searching() plot.animation(path, visited, "Breadth-first Searching (BFS)") if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/dfs.py ================================================ import os import sys import math import heapq sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_2D import plotting, env from Search_2D.Astar import AStar class DFS(AStar): """DFS add the new visited node in the front of the openset """ def searching(self): """ Breadth-first Searching. :return: path, visited order """ self.PARENT[self.s_start] = self.s_start self.g[self.s_start] = 0 self.g[self.s_goal] = math.inf heapq.heappush(self.OPEN, (0, self.s_start)) while self.OPEN: _, s = heapq.heappop(self.OPEN) self.CLOSED.append(s) if s == self.s_goal: break for s_n in self.get_neighbor(s): new_cost = self.g[s] + self.cost(s, s_n) if s_n not in self.g: self.g[s_n] = math.inf if new_cost < self.g[s_n]: # conditions for updating Cost self.g[s_n] = new_cost self.PARENT[s_n] = s # dfs, add new node to the front of the openset prior = self.OPEN[0][0]-1 if len(self.OPEN)>0 else 0 heapq.heappush(self.OPEN, (prior, s_n)) return self.extract_path(self.PARENT), self.CLOSED def main(): s_start = (5, 5) s_goal = (45, 25) dfs = DFS(s_start, s_goal, 'None') plot = plotting.Plotting(s_start, s_goal) path, visited = dfs.searching() visited = list(dict.fromkeys(visited)) plot.animation(path, visited, "Depth-first Searching (DFS)") # animation if __name__ == '__main__': main() ================================================ FILE: Search_based_Planning/Search_2D/env.py ================================================ """ Env 2D @author: huiming zhou """ class Env: def __init__(self): self.x_range = 51 # size of background self.y_range = 31 self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1), (1, 0), (1, -1), (0, -1), (-1, -1)] self.obs = self.obs_map() def update_obs(self, obs): self.obs = obs def obs_map(self): """ Initialize obstacles' positions :return: map of obstacles """ x = self.x_range y = self.y_range obs = set() for i in range(x): obs.add((i, 0)) for i in range(x): obs.add((i, y - 1)) for i in range(y): obs.add((0, i)) for i in range(y): obs.add((x - 1, i)) for i in range(10, 21): obs.add((i, 15)) for i in range(15): obs.add((20, i)) for i in range(15, 30): obs.add((30, i)) for i in range(16): obs.add((40, i)) return obs ================================================ FILE: Search_based_Planning/Search_2D/plotting.py ================================================ """ Plot tools 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 env class Plotting: def __init__(self, xI, xG): self.xI, self.xG = xI, xG self.env = env.Env() self.obs = self.env.obs_map() def update_obs(self, obs): self.obs = obs def animation(self, path, visited, name): self.plot_grid(name) self.plot_visited(visited) self.plot_path(path) plt.show() def animation_lrta(self, path, visited, name): self.plot_grid(name) cl = self.color_list_2() path_combine = [] for k in range(len(path)): self.plot_visited(visited[k], cl[k]) plt.pause(0.2) self.plot_path(path[k]) path_combine += path[k] plt.pause(0.2) if self.xI in path_combine: path_combine.remove(self.xI) self.plot_path(path_combine) plt.show() def animation_ara_star(self, path, visited, name): self.plot_grid(name) cl_v, cl_p = self.color_list() for k in range(len(path)): self.plot_visited(visited[k], cl_v[k]) self.plot_path(path[k], cl_p[k], True) plt.pause(0.5) plt.show() def animation_bi_astar(self, path, v_fore, v_back, name): self.plot_grid(name) self.plot_visited_bi(v_fore, v_back) self.plot_path(path) plt.show() def plot_grid(self, name): obs_x = [x[0] for x in self.obs] obs_y = [x[1] for x in self.obs] plt.plot(self.xI[0], self.xI[1], "bs") plt.plot(self.xG[0], self.xG[1], "gs") plt.plot(obs_x, obs_y, "sk") plt.title(name) plt.axis("equal") def plot_visited(self, visited, cl='gray'): if self.xI in visited: visited.remove(self.xI) if self.xG in visited: visited.remove(self.xG) count = 0 for x in visited: count += 1 plt.plot(x[0], x[1], color=cl, marker='o') plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if count < len(visited) / 3: length = 20 elif count < len(visited) * 2 / 3: length = 30 else: length = 40 # # length = 15 if count % length == 0: plt.pause(0.001) plt.pause(0.01) def plot_path(self, path, cl='r', flag=False): path_x = [path[i][0] for i in range(len(path))] path_y = [path[i][1] for i in range(len(path))] if not flag: plt.plot(path_x, path_y, linewidth='3', color='r') else: plt.plot(path_x, path_y, linewidth='3', color=cl) plt.plot(self.xI[0], self.xI[1], "bs") plt.plot(self.xG[0], self.xG[1], "gs") plt.pause(0.01) def plot_visited_bi(self, v_fore, v_back): if self.xI in v_fore: v_fore.remove(self.xI) if self.xG in v_back: v_back.remove(self.xG) len_fore, len_back = len(v_fore), len(v_back) for k in range(max(len_fore, len_back)): if k < len_fore: plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o') if k < len_back: plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o') plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if k % 10 == 0: plt.pause(0.001) plt.pause(0.01) @staticmethod def color_list(): cl_v = ['silver', 'wheat', 'lightskyblue', 'royalblue', 'slategray'] cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm'] return cl_v, cl_p @staticmethod def color_list_2(): cl = ['silver', 'steelblue', 'dimgray', 'cornflowerblue', 'dodgerblue', 'royalblue', 'plum', 'mediumslateblue', 'mediumpurple', 'blueviolet', ] return cl ================================================ FILE: Search_based_Planning/Search_2D/queue.py ================================================ import collections import heapq class QueueFIFO: """ Class: QueueFIFO Description: QueueFIFO is designed for First-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.popleft() # leave from front class QueueLIFO: """ Class: QueueLIFO Description: QueueLIFO is designed for Last-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.pop() # leave from back class QueuePrior: """ Class: QueuePrior Description: QueuePrior reorders elements using value [priority] """ def __init__(self): self.queue = [] def empty(self): return len(self.queue) == 0 def put(self, item, priority): heapq.heappush(self.queue, (priority, item)) # reorder s using priority def get(self): return heapq.heappop(self.queue)[1] # pop out the smallest item def enumerate(self): return self.queue ================================================ FILE: Search_based_Planning/Search_3D/Anytime_Dstar3D.py ================================================ # check paper of # [Likhachev2005] import numpy as np import matplotlib.pyplot as plt import os import sys from collections import defaultdict sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, isinobb, \ cost, children, StateSpace from Search_3D.plot_util3D import visualization from Search_3D import queue import time class Anytime_Dstar(object): def __init__(self, resolution=1): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1): np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.env = env(resolution=resolution) self.settings = 'CollisionChecking' # for collision checking self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.OPEN = queue.MinheapPQ() self.g = {} # all g initialized at inf self.h = {} self.rhs = {self.xt: 0} # rhs(x0) = 0 self.OPEN.put(self.xt, self.key(self.xt)) self.INCONS = set() self.CLOSED = set() # init children set: self.CHILDREN = {} # init Cost set self.COST = defaultdict(lambda: defaultdict(dict)) # for visualization self.V = set() # vertice in closed self.ind = 0 self.Path = [] self.done = False # epsilon in the key caculation self.epsilon = 1 self.increment = 0.1 self.decrement = 0.2 def getcost(self, xi, xj): # use a LUT for getting the costd if xi not in self.COST: for (xj, xjcost) in children(self, xi, settings=1): self.COST[xi][xj] = cost(self, xi, xj, xjcost) # this might happen when there is a node changed. if xj not in self.COST[xi]: self.COST[xi][xj] = cost(self, xi, xj) return self.COST[xi][xj] def getchildren(self, xi): if xi not in self.CHILDREN: allchild = children(self, xi) self.CHILDREN[xi] = set(allchild) return self.CHILDREN[xi] def geth(self, xi): # when the heurisitic is first calculated if xi not in self.h: self.h[xi] = heuristic_fun(self, xi, self.x0) return self.h[xi] def getg(self, xi): if xi not in self.g: self.g[xi] = np.inf return self.g[xi] def getrhs(self, xi): if xi not in self.rhs: self.rhs[xi] = np.inf return self.rhs[xi] def updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if self.isinobs(old, xi, mode) or self.isinobs(new, xi, mode): # if self.isinobs(new, xi, mode): self.V.remove(xi) # self.V.difference_update({i for i in children(self, xi)}) newchildren = set(children(self, xi)) # B self.CHILDREN[xi] = newchildren for xj in newchildren: self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) return CHANGED def isinobs(self, obs, x, mode): if mode == 'obb': return isinobb(obs, x) elif mode == 'aabb': return isinbound(obs, x, mode) # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False): # # TODO scan graph for changed Cost, if Cost is changed update it # # make the graph Cost via vectorization # CHANGED = set() # Allnodes = np.array(list(self.CLOSED)) # isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \ # isinbound(new, Allnodes, mode = mode, isarray = True) # Changednodes = Allnodes[isChanged] # for xi in Changednodes: # xi = tuple(xi) # CHANGED.add(xi) # self.CHILDREN[xi] = set(children(self, xi)) # for xj in self.CHILDREN: # self.COST[xi][xj] = Cost(self, xi, xj) # --------------main functions for Anytime D star def key(self, s, epsilon=1): if self.getg(s) > self.getrhs(s): return [self.rhs[s] + epsilon * heuristic_fun(self, s, self.x0), self.rhs[s]] else: return [self.getg(s) + heuristic_fun(self, s, self.x0), self.getg(s)] def UpdateState(self, s): if s not in self.CLOSED: # TODO if s is not visited before self.g[s] = np.inf if s != self.xt: self.rhs[s] = min([self.getcost(s, s_p) + self.getg(s_p) for s_p in self.getchildren(s)]) self.OPEN.check_remove(s) if self.getg(s) != self.getrhs(s): if s not in self.CLOSED: self.OPEN.put(s, self.key(s)) else: self.INCONS.add(s) def ComputeorImprovePath(self): while self.OPEN.top_key() < self.key(self.x0, self.epsilon) or self.rhs[self.x0] != self.g[self.x0]: s = self.OPEN.get() if getDist(s, tuple(self.env.start)) < self.env.resolution: break if self.g[s] > self.rhs[s]: self.g[s] = self.rhs[s] self.CLOSED.add(s) self.V.add(s) for s_p in self.getchildren(s): self.UpdateState(s_p) else: self.g[s] = np.inf self.UpdateState(s) for s_p in self.getchildren(s): self.UpdateState(s_p) self.ind += 1 def Main(self): ischanged = False islargelychanged = False t = 0 self.ComputeorImprovePath() # TODO publish current epsilon sub-optimal solution self.done = True self.ind = 0 self.Path = self.path() visualization(self) while True: visualization(self) if t == 20: break # change environment # new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation') # new2, old2 = self.env.move_block(a=[0, 0, -0.2], mode='translation') new2, old2 = self.env.move_OBB(theta=[10*t, 0, 0], translation=[0, 0.1*t, 0]) mmode = 'obb' # obb or aabb ischanged = True # islargelychanged = True self.Path = [] # update Cost with changed environment if ischanged: # CHANGED = self.updatecost(True, new2, old2, mode='obb') CHANGED = self.updatecost(True, new2, old2, mode=mmode) for u in CHANGED: self.UpdateState(u) self.ComputeorImprovePath() ischanged = False if islargelychanged: self.epsilon += self.increment # or replan from scratch elif self.epsilon > 1: self.epsilon -= self.decrement # move states from the INCONS to OPEN # update priorities in OPEN Allnodes = self.INCONS.union(self.OPEN.allnodes()) for node in Allnodes: self.OPEN.put(node, self.key(node, self.epsilon)) self.INCONS = set() self.CLOSED = set() self.ComputeorImprovePath() # publish current epsilon sub optimal solution self.Path = self.path() # if epsilon == 1: # wait for change to occur t += 1 def path(self, s_start=None): '''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 cBest(s,s') + g(s') until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt s = self.x0 ind = 0 while getDist(s, s_goal) > self.env.resolution: if s == self.x0: children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution * np.sqrt(3)] else: children = list(self.CHILDREN[s]) snext = children[np.argmin([self.getcost(s, s_p) + self.getg(s_p) for s_p in children])] path.append([s, snext]) s = snext if ind > 100: break ind += 1 return path if __name__ == '__main__': AD = Anytime_Dstar(resolution=1) AD.Main() ================================================ FILE: Search_based_Planning/Search_3D/Astar3D.py ================================================ # this is the three dimensional A* algo # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ cost, children, StateSpace, heuristic_fun from Search_3D.plot_util3D import visualization import queue import time class Weighted_A_star(object): def __init__(self, resolution=0.5): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.settings = 'NonCollisionChecking' # 'NonCollisionChecking' or 'CollisionChecking' self.env = env(resolution=resolution) self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) self.g = {self.start:0,self.goal:np.inf} self.Parent = {} self.CLOSED = set() self.V = [] self.done = False self.Path = [] self.ind = 0 self.x0, self.xt = self.start, self.goal self.OPEN = queue.MinheapPQ() # store [point,priority] self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h self.lastpoint = self.x0 def run(self, N=None): xt = self.xt xi = self.x0 while self.OPEN: # while xt not reached and open is not empty xi = self.OPEN.get() if xi not in self.CLOSED: self.V.append(np.array(xi)) self.CLOSED.add(xi) # add the point in CLOSED set if getDist(xi,xt) < self.env.resolution: break # visualization(self) for xj in children(self,xi): # if xj not in self.CLOSED: if xj not in self.g: self.g[xj] = np.inf else: pass a = self.g[xi] + cost(self, xi, xj) if a < self.g[xj]: self.g[xj] = a self.Parent[xj] = xi # assign or update the priority in the open self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) # For specified expanded nodes, used primarily in LRTA* if N: if len(self.CLOSED) % N == 0: break if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V))) self.ind += 1 self.lastpoint = xi # if the path finding is finished if self.lastpoint in self.CLOSED: self.done = True self.Path = self.path() if N is None: visualization(self) plt.show() return True return False def path(self): path = [] x = self.lastpoint start = self.x0 while x != start: path.append([x, self.Parent[x]]) x = self.Parent[x] # path = np.flip(path, axis=0) return path # utility used in LRTA* def reset(self, xj): self.g = g_Space(self) # key is the point, store g value self.start = xj self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0 self.x0 = xj self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h self.CLOSED = set() # self.h = h(self.Space, self.goal) if __name__ == '__main__': Astar = Weighted_A_star(0.5) sta = time.time() Astar.run() print(time.time() - sta) ================================================ FILE: Search_based_Planning/Search_3D/Dstar3D.py ================================================ import numpy as np import matplotlib.pyplot as plt import os import sys from collections import defaultdict sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \ initcost from Search_3D.plot_util3D import visualization class D_star(object): def __init__(self, resolution=1): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.settings = 'CollisionChecking' self.env = env(resolution=resolution) self.X = StateSpace(self.env) self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal) # self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.b = defaultdict(lambda: defaultdict(dict)) # back pointers every state has one except xt. self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value self.h = {} # estimate from a point to the end point self.tag = {} # set all states to new self.V = set() # vertice in closed # for visualization self.ind = 0 self.Path = [] self.done = False self.Obstaclemap = {} def checkState(self, y): if y not in self.h: self.h[y] = 0 if y not in self.tag: self.tag[y] = 'New' def get_kmin(self): # get the minimum of the k val in OPEN # -1 if it does not exist if self.OPEN: return min(self.OPEN.values()) return -1 def min_state(self): # returns the state in OPEN with min k(.) # if empty, returns None and -1 # it also removes this min value form the OPEN set. if self.OPEN: minvalue = min(self.OPEN.values()) for k in self.OPEN.keys(): if self.OPEN[k] == minvalue: return k, self.OPEN.pop(k) return None, -1 def insert(self, x, h_new): # inserting a key and value into OPEN list (s, kx) # depending on following situations if self.tag[x] == 'New': kx = h_new if self.tag[x] == 'Open': kx = min(self.OPEN[x], h_new) if self.tag[x] == 'Closed': kx = min(self.h[x], h_new) self.OPEN[x] = kx self.h[x], self.tag[x] = h_new, 'Open' def process_state(self): # main function of the D star algorithm, perform the process state # around the old path when needed. x, kold = self.min_state() self.tag[x] = 'Closed' self.V.add(x) if x is None: return -1 # check if 1st timer s self.checkState(x) if kold < self.h[x]: # raised states for y in children(self, x): # check y self.checkState(y) a = self.h[y] + cost(self, y, x) if self.h[y] <= kold and self.h[x] > a: self.b[x], self.h[x] = y, a if kold == self.h[x]: # lower for y in children(self, x): # check y self.checkState(y) bb = self.h[x] + cost(self, x, y) if self.tag[y] == 'New' or \ (self.b[y] == x and self.h[y] != bb) or \ (self.b[y] != x and self.h[y] > bb): self.b[y] = x self.insert(y, bb) else: for y in children(self, x): # check y self.checkState(y) bb = self.h[x] + cost(self, x, y) if self.tag[y] == 'New' or \ (self.b[y] == x and self.h[y] != bb): self.b[y] = x self.insert(y, bb) else: if self.b[y] != x and self.h[y] > bb: self.insert(x, self.h[x]) else: if self.b[y] != x and self.h[y] > bb and \ self.tag[y] == 'Closed' and self.h[y] == kold: self.insert(y, self.h[y]) return self.get_kmin() def modify_cost(self, x): xparent = self.b[x] if self.tag[x] == 'Closed': self.insert(x, self.h[xparent] + cost(self, x, xparent)) def modify(self, x): self.modify_cost(x) while True: kmin = self.process_state() # visualization(self) if kmin >= self.h[x]: break def path(self, goal=None): path = [] if not goal: x = self.x0 else: x = goal start = self.xt while x != start: path.append([np.array(x), np.array(self.b[x])]) x = self.b[x] return path def run(self): # put G (ending state) into the OPEN list self.OPEN[self.xt] = 0 self.tag[self.x0] = 'New' # first run while True: # TODO: self.x0 = self.process_state() # visualization(self) if self.tag[self.x0] == "Closed": break self.ind += 1 self.Path = self.path() self.done = True visualization(self) plt.pause(0.2) # plt.show() # when the environemnt changes over time for i in range(5): self.env.move_block(a=[0, -0.50, 0], s=0.5, block_to_move=1, mode='translation') self.env.move_block(a=[-0.25, 0, 0], s=0.5, block_to_move=0, mode='translation') # travel from end to start s = tuple(self.env.start) # self.V = set() while s != self.xt: if s == tuple(self.env.start): sparent = self.b[self.x0] else: sparent = self.b[s] # if there is a change of Cost, or a collision. if cost(self, s, sparent) == np.inf: self.modify(s) continue self.ind += 1 s = sparent self.Path = self.path() visualization(self) plt.show() if __name__ == '__main__': D = D_star(1) D.run() ================================================ FILE: Search_based_Planning/Search_3D/DstarLite3D.py ================================================ import numpy as np import matplotlib.pyplot as plt import os import sys from collections import defaultdict sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ cost, children, StateSpace from Search_3D.plot_util3D import visualization from Search_3D import queue import time class D_star_Lite(object): # Original version of the D*lite def __init__(self, resolution = 1): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.env = env(resolution=resolution) #self.X = StateSpace(self.env) #self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal) self.settings = 'CollisionChecking' # for collision checking self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # self.OPEN = queue.QueuePrior() self.OPEN = queue.MinheapPQ() self.km = 0 self.g = {} # all g initialized at inf self.rhs = {self.xt:0} # rhs(x0) = 0 self.h = {} self.OPEN.put(self.xt, self.CalculateKey(self.xt)) self.CLOSED = set() # init children set: self.CHILDREN = {} # init Cost set self.COST = defaultdict(lambda: defaultdict(dict)) # for visualization self.V = set() # vertice in closed self.ind = 0 self.Path = [] self.done = False def updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if isinbound(old, xi, mode) or isinbound(new, xi, mode): newchildren = set(children(self, xi)) # B self.CHILDREN[xi] = newchildren for xj in newchildren: self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) return CHANGED def getcost(self, xi, xj): # use a LUT for getting the costd if xi not in self.COST: for (xj,xjcost) in children(self, xi, settings=1): self.COST[xi][xj] = cost(self, xi, xj, xjcost) # this might happen when there is a node changed. if xj not in self.COST[xi]: self.COST[xi][xj] = cost(self, xi, xj) return self.COST[xi][xj] def getchildren(self, xi): if xi not in self.CHILDREN: allchild = children(self, xi) self.CHILDREN[xi] = set(allchild) return self.CHILDREN[xi] def geth(self, xi): # when the heurisitic is first calculated if xi not in self.h: self.h[xi] = heuristic_fun(self, xi, self.x0) return self.h[xi] def getg(self, xi): if xi not in self.g: self.g[xi] = np.inf return self.g[xi] def getrhs(self, xi): if xi not in self.rhs: self.rhs[xi] = np.inf return self.rhs[xi] #-------------main functions for D*Lite------------- def CalculateKey(self, s, epsilion = 1): return [min(self.getg(s), self.getrhs(s)) + epsilion * self.geth(s) + self.km, min(self.getg(s), self.getrhs(s))] def UpdateVertex(self, u): # if still in the hunt if not getDist(self.xt, u) <= self.env.resolution: # originally: u != x_goal if u in self.CHILDREN and len(self.CHILDREN[u]) == 0: self.rhs[u] = np.inf else: self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)]) # if u is in OPEN, remove it self.OPEN.check_remove(u) # if rhs(u) not equal to g(u) if self.getg(u) != self.getrhs(u): self.OPEN.put(u, self.CalculateKey(u)) def ComputeShortestPath(self): while self.OPEN.top_key() < self.CalculateKey(self.x0) or self.getrhs(self.x0) != self.getg(self.x0) : kold = self.OPEN.top_key() u = self.OPEN.get() self.V.add(u) self.CLOSED.add(u) if not self.done: # first time running, we need to stop on this condition if getDist(self.x0,u) < 1*self.env.resolution: self.x0 = u break if kold < self.CalculateKey(u): self.OPEN.put(u, self.CalculateKey(u)) if self.getg(u) > self.getrhs(u): self.g[u] = self.rhs[u] else: self.g[u] = np.inf self.UpdateVertex(u) for s in self.getchildren(u): self.UpdateVertex(s) # visualization(self) self.ind += 1 def main(self): s_last = self.x0 print('first run ...') self.ComputeShortestPath() self.Path = self.path() self.done = True visualization(self) plt.pause(0.5) # plt.show() print('running with map update ...') t = 0 # count time ischanged = False self.V = set() while getDist(self.x0, self.xt) > 2*self.env.resolution: #---------------------------------- at specific times, the environment is changed and Cost is updated if t % 2 == 0: new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation') new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation') new2,old2 = self.env.move_OBB(theta = [0,0.1*t,0]) #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation') ischanged = True self.Path = [] #----------------------------------- traverse the route as originally planned if t == 0: children_new = [i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution*np.sqrt(3)] else: children_new = list(children(self,self.x0)) self.x0 = children_new[np.argmin([self.getcost(self.x0,s_p) + self.getg(s_p) for s_p in children_new])] # TODO add the moving robot position codes 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 cBest(u,v) and update vertex u. then replan if ischanged: self.km += heuristic_fun(self, self.x0, s_last) s_last = self.x0 CHANGED = self.updatecost(True, new0, old0) CHANGED1 = self.updatecost(True, new1, old1) CHANGED2 = self.updatecost(True, new2, old2, mode='obb') CHANGED = CHANGED.union(CHANGED1, CHANGED2) # self.V = set() for u in CHANGED: self.UpdateVertex(u) self.ComputeShortestPath() ischanged = False self.Path = self.path(self.x0) visualization(self) t += 1 plt.show() def path(self, s_start=None): '''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 cBest(s,s') + g(s') until x_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt if not s_start: s = self.x0 else: s= s_start ind = 0 while s != s_goal: if s == self.x0: children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)] else: children = list(self.CHILDREN[s]) snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])] path.append([s, snext]) s = snext if ind > 100: break ind += 1 return path if __name__ == '__main__': D_lite = D_star_Lite(1) a = time.time() D_lite.main() print('used time (s) is ' + str(time.time() - a)) ================================================ FILE: Search_based_Planning/Search_3D/LP_Astar3D.py ================================================ import numpy as np import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \ cost, obstacleFree, isCollide from Search_3D.plot_util3D import visualization import queue import pyrr import time class Lifelong_Astar(object): def __init__(self,resolution = 1): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.env = env(resolution=resolution) self.g = g_Space(self) self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) self.x0, self.xt = self.start, self.goal self.rhs = g_Space(self) # rhs(.) = g(.) = inf self.rhs[self.start] = 0 # rhs(x0) = 0 self.h = Heuristic(self.g, self.goal) self.OPEN = queue.MinheapPQ() # store [point,priority] self.OPEN.put(self.x0, [self.h[self.x0],0]) self.CLOSED = set() # used for A* self.done = False self.Path = [] self.V = [] self.ind = 0 # initialize children list self.CHILDREN = {} self.getCHILDRENset() # initialize Cost list self.COST = {} _ = self.costset() def costset(self): NodeToChange = set() for xi in self.CHILDREN.keys(): children = self.CHILDREN[xi] toUpdate = [self.cost(xj,xi) for xj in children] if xi in self.COST: # if the old Cost not equal to new Cost diff = np.not_equal(self.COST[xi],toUpdate) cd = np.array(children)[diff] for i in cd: NodeToChange.add(tuple(i)) self.COST[xi] = toUpdate else: self.COST[xi] = toUpdate return NodeToChange def getCOSTset(self,xi,xj): ind, children = 0, self.CHILDREN[xi] for i in children: if i == xj: return self.COST[xi][ind] ind += 1 def children(self, x): allchild = [] resolution = self.env.resolution for direc in self.Alldirec: child = tuple(map(np.add,x,np.multiply(direc,resolution))) if isinbound(self.env.boundary,child): allchild.append(child) return allchild def getCHILDRENset(self): for xi in self.g.keys(): self.CHILDREN[xi] = self.children(xi) def isCollide(self, x, child): ray , dist = getRay(x, child) , getDist(x, child) if not isinbound(self.env.boundary,child): return True, dist for i in self.env.AABB_pyrr: shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) if shot is not None: dist_wall = getDist(x, shot) if dist_wall <= dist: # collide return True, dist for i in self.env.balls: if isinball(i, child): return True, dist shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) if shot != []: dists_ball = [getDist(x, j) for j in shot] if all(dists_ball <= dist): # collide return True, dist return False, dist def cost(self, x, y): collide, dist = isCollide(self, x, y) if collide: return np.inf else: return dist def key(self,xi,epsilion = 1): return [min(self.g[xi],self.rhs[xi]) + epsilion*self.h[xi],min(self.g[xi],self.rhs[xi])] def path(self): path = [] x = self.xt start = self.x0 ind = 0 while x != start: j = x nei = self.CHILDREN[x] gset = [self.g[xi] for xi in nei] # collision check and make g Cost inf for i in range(len(nei)): if isCollide(self, nei[i],j)[0]: gset[i] = np.inf parent = nei[np.argmin(gset)] path.append([x, parent]) x = parent if ind > 100: break ind += 1 return path #------------------Lifelong Plannning A* def UpdateMembership(self, xi, xparent=None): if xi != self.x0: self.rhs[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]]) self.OPEN.check_remove(xi) if self.g[xi] != self.rhs[xi]: self.OPEN.put(xi,self.key(xi)) def ComputePath(self): print('computing path ...') while self.key(self.xt) > self.OPEN.top_key() or self.rhs[self.xt] != self.g[self.xt]: xi = self.OPEN.get() # if g > rhs, overconsistent if self.g[xi] > self.rhs[xi]: self.g[xi] = self.rhs[xi] # add xi to expanded node set if xi not in self.CLOSED: self.V.append(xi) self.CLOSED.add(xi) else: # underconsistent and consistent self.g[xi] = np.inf self.UpdateMembership(xi) for xj in self.CHILDREN[xi]: self.UpdateMembership(xj) # visualization(self) self.ind += 1 self.Path = self.path() self.done = True visualization(self) plt.pause(1) def change_env(self): _, _ = self.env.move_block(block_to_move=1,a = [0,2,0]) self.done = False self.Path = [] self.CLOSED = set() N = self.costset() for xi in N: self.UpdateMembership(xi) if __name__ == '__main__': sta = time.time() Astar = Lifelong_Astar(1) Astar.ComputePath() for i in range(5): Astar.change_env() Astar.ComputePath() plt.pause(1) print(time.time() - sta) ================================================ FILE: Search_based_Planning/Search_3D/LRT_Astar3D.py ================================================ # this is the three dimensional N>1 LRTA* algo # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue class LRT_A_star2: def __init__(self, resolution=0.5, N=7): self.N = N self.Astar = Astar3D.Weighted_A_star(resolution=resolution) self.path = [] def updateHeuristic(self): # Initialize hvalues at infinity for xi in self.Astar.CLOSED: self.Astar.h[xi] = np.inf Diff = True while Diff: # repeat DP until converge hvals, lasthvals = [], [] for xi in self.Astar.CLOSED: lasthvals.append(self.Astar.h[xi]) # 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) > cBest(s,s') + h(s') if self.Astar.h[xi] >= minfval: self.Astar.h[xi] = minfval hvals.append(self.Astar.h[xi]) if lasthvals == hvals: Diff = False def move(self): st = self.Astar.x0 ind = 0 # find the lowest path down hill while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop Children = children(self.Astar,st) minh, minchild = np.inf, None for child in Children: # check collision here, not a supper efficient collide, _ = isCollide(self.Astar,st, child) if collide: continue h = self.Astar.h[child] if h <= minh: minh, minchild = h, child self.path.append([st, minchild]) st = minchild for (_, p) in self.Astar.OPEN.enumerate(): if p == st: break ind += 1 if ind > 1000: break self.Astar.reset(st) def run(self): while True: if self.Astar.run(N=self.N): self.Astar.Path = self.Astar.Path + self.path self.Astar.done = True visualization(self.Astar) plt.show() break self.updateHeuristic() self.move() if __name__ == '__main__': T = LRT_A_star2(resolution=0.5, N=100) T.run() ================================================ FILE: Search_based_Planning/Search_3D/RTA_Astar3D.py ================================================ # this is the three dimensional Real-time Adaptive LRTA* algo # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np import matplotlib.pyplot as plt import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue class RTA_A_star: def __init__(self, resolution=0.5, N=7): self.N = N # node to expand self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star self.path = [] # empty path self.st = [] self.localhvals = [] def updateHeuristic(self): # Initialize hvalues at infinity self.localhvals = [] nodeset, vals = [], [] for (_,_,xi) in self.Astar.OPEN.enumerate(): nodeset.append(xi) vals.append(self.Astar.g[xi] + self.Astar.h[xi]) j, fj = nodeset[np.argmin(vals)], min(vals) self.st = j # single pass update of hvals for xi in self.Astar.CLOSED: self.Astar.h[xi] = fj - self.Astar.g[xi] self.localhvals.append(self.Astar.h[xi]) def move(self): st, localhvals = self.st, self.localhvals maxhval = max(localhvals) sthval = self.Astar.h[st] # find the lowest path up hill while sthval < maxhval: parentsvals , parents = [] , [] # find the max child for xi in children(self.Astar,st): if xi in self.Astar.CLOSED: parents.append(xi) parentsvals.append(self.Astar.h[xi]) lastst = st st = parents[np.argmax(parentsvals)] self.path.append([st,lastst]) # add to path sthval = self.Astar.h[st] self.Astar.reset(self.st) def run(self): while True: if self.Astar.run(N=self.N): self.Astar.Path = self.Astar.Path + self.path self.Astar.done = True visualization(self.Astar) plt.show() break self.updateHeuristic() self.move() if __name__ == '__main__': T = RTA_A_star(resolution=1, N=100) T.run() ================================================ FILE: Search_based_Planning/Search_3D/bidirectional_Astar3D.py ================================================ # this is the three dimensional bidirectional A* algo # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np import matplotlib.pyplot as plt from collections import defaultdict import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun from Search_3D.plot_util3D import visualization import queue class Weighted_A_star(object): def __init__(self,resolution=0.5): self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.env = env(resolution = resolution) self.settings = 'NonCollisionChecking' self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) self.g = {self.start:0,self.goal:0} self.OPEN1 = queue.MinheapPQ() # store [point,priority] self.OPEN2 = queue.MinheapPQ() self.Parent1, self.Parent2 = {}, {} self.CLOSED1, self.CLOSED2 = set(), set() self.V = [] self.done = False self.Path = [] def run(self): x0, xt = self.start, self.goal self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h self.ind = 0 while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() self.CLOSED1.add(xi1) # add the point in CLOSED set self.CLOSED2.add(xi2) self.V.append(xi1) self.V.append(xi2) # visualization(self) allchild1, allchild2 = children(self,xi1), children(self,xi2) self.evaluation(allchild1,xi1,conf=1) self.evaluation(allchild2,xi2,conf=2) if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) self.ind += 1 self.common = self.CLOSED1.intersection(self.CLOSED2) self.done = True self.Path = self.path() visualization(self) plt.show() def evaluation(self, allchild, xi, conf): for xj in allchild: if conf == 1: if xj not in self.CLOSED1: if xj not in self.g: self.g[xj] = np.inf else: pass gi = self.g[xi] a = gi + cost(self,xi,xj) if a < self.g[xj]: self.g[xj] = a self.Parent1[xj] = xi self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal)) if conf == 2: if xj not in self.CLOSED2: if xj not in self.g: self.g[xj] = np.inf else: pass gi = self.g[xi] a = gi + cost(self,xi,xj) if a < self.g[xj]: self.g[xj] = a self.Parent2[xj] = xi self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start)) def path(self): # TODO: fix path path = [] goal = self.goal start = self.start x = list(self.common)[0] while x != start: path.append([x,self.Parent1[x]]) x = self.Parent1[x] x = list(self.common)[0] while x != goal: path.append([x,self.Parent2[x]]) x = self.Parent2[x] path = np.flip(path,axis=0) return path if __name__ == '__main__': Astar = Weighted_A_star(0.5) Astar.run() ================================================ FILE: Search_based_Planning/Search_3D/env3D.py ================================================ # this is the three dimensional space # !/usr/bin/env python3 # -*- coding: utf-8 -*- """ @author: yue qi """ import numpy as np # from utils3D import OBB2AABB def R_matrix(z_angle,y_angle,x_angle): # s angle: row; y angle: pitch; z angle: yaw # generate rotation matrix in SO3 # RzRyRx = R, ZYX intrinsic rotation # also (r1,r2,r3) in R3*3 in {W} frame # used in obb.O # [[R p] # [0T 1]] gives transformation from body to world return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) def getblocks(): # AABBs block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00], [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00], [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00], [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00], [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]] Obstacles = [] for i in block: i = np.array(i) Obstacles.append([j for j in i]) return np.array(Obstacles) def getballs(): spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]] Obstacles = [] for i in spheres: Obstacles.append([j for j in i]) return np.array(Obstacles) def getAABB(blocks): # used for Pyrr package for detecting collision AABB = [] for i in blocks: AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger return AABB def getAABB2(blocks): # used in lineAABB AABB = [] for i in blocks: AABB.append(aabb(i)) return AABB def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): return block class aabb(object): # make AABB out of blocks, # P: center point # E: extents # O: Rotation matrix in SO(3), in {w} def __init__(self,AABB): self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents self.O = [[1,0,0],[0,1,0],[0,0,1]] class obb(object): # P: center point # E: extents # O: Rotation matrix in SO(3), in {w} def __init__(self, P, E, O): self.P = P self.E = E self.O = O self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1): # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): self.resolution = resolution self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) self.blocks = getblocks() self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) self.balls = getballs() self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)), obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))]) self.start = np.array([2.0, 2.0, 2.0]) self.goal = np.array([6.0, 16.0, 0.0]) self.t = 0 # time def New_block(self): newblock = add_block() self.blocks = np.vstack([self.blocks,newblock]) self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) def move_start(self, x): self.start = x def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'): # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, # R is an orthorgonal transform in R3*3, is the rotation matrix # (s',t') = (s + tv, t) is uniform transformation # (s',t') = (s + a, t + s) is a translation if mode == 'translation': ori = np.array(self.blocks[block_to_move]) self.blocks[block_to_move] = \ np.array([ori[0] + a[0],\ ori[1] + a[1],\ ori[2] + a[2],\ ori[3] + a[0],\ ori[4] + a[1],\ ori[5] + a[2]]) self.AABB[block_to_move].P = \ [self.AABB[block_to_move].P[0] + a[0], \ self.AABB[block_to_move].P[1] + a[1], \ self.AABB[block_to_move].P[2] + a[2]] self.t += s # return a range of block that the block might moved a = self.blocks[block_to_move] return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) # return a,ori # (s',t') = (Rx, t) def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]): # theta stands for rotational angles around three principle axis in world frame # translation stands for translation in the world frame ori = [self.OBB[obb_to_move]] self.OBB[obb_to_move].P = \ [self.OBB[obb_to_move].P[0] + translation[0], self.OBB[obb_to_move].P[1] + translation[1], self.OBB[obb_to_move].P[2] + translation[2]] # Calculate orientation self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) # generating transformation matrix self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\ -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]]) return self.OBB[obb_to_move], ori[0] if __name__ == '__main__': newenv = env() ================================================ FILE: Search_based_Planning/Search_3D/plot_util3D.py ================================================ # plotting import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d.art3d import Poly3DCollection import mpl_toolkits.mplot3d as plt3d from mpl_toolkits.mplot3d import proj3d import numpy as np def CreateSphere(center,r): u = np.linspace(0,2* np.pi,30) v = np.linspace(0,np.pi,30) x = np.outer(np.cos(u),np.sin(v)) y = np.outer(np.sin(u),np.sin(v)) z = np.outer(np.ones(np.size(u)),np.cos(v)) x, y, z = r*x + center[0], r*y + center[1], r*z + center[2] return (x,y,z) def draw_Spheres(ax,balls): for i in balls: (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") def draw_block_list(ax, blocks ,color=None,alpha=0.15): ''' drawing the blocks on the graph ''' v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], dtype='float') f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) n = blocks.shape[0] d = blocks[:, 3:6] - blocks[:, :3] vl = np.zeros((8 * n, 3)) fl = np.zeros((6 * n, 4), dtype='int64') for k in range(n): vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] fl[k * 6:(k + 1) * 6, :] = f + k * 8 if type(ax) is Poly3DCollection: ax.set_verts(vl[fl]) else: pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') pc.set_facecolor(color) h = ax.add_collection3d(pc) return h def obb_verts(obb): # 0.017004013061523438 for 1000 iters ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) # P + (ori * E) ori_body = np.multiply(ori_body,obb.E) # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) verts = (obb.O@ori_body.T).T + obb.P return verts def draw_obb(ax, OBB, color=None,alpha=0.15): f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) n = OBB.shape[0] vl = np.zeros((8 * n, 3)) fl = np.zeros((6 * n, 4), dtype='int64') for k in range(n): vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) fl[k * 6:(k + 1) * 6, :] = f + k * 8 if type(ax) is Poly3DCollection: ax.set_verts(vl[fl]) else: pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') pc.set_facecolor(color) h = ax.add_collection3d(pc) return h def draw_line(ax,SET,visibility=1,color=None): if SET != []: for i in SET: xs = i[0][0], i[1][0] ys = i[0][1], i[1][1] zs = i[0][2], i[1][2] line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) ax.add_line(line) def visualization(initparams): if initparams.ind % 20 == 0 or initparams.done: V = np.array(list(initparams.V)) # E = initparams.E Path = np.array(initparams.Path) start = initparams.env.start goal = initparams.env.goal # edges = E.get_edge() # generate axis objects ax = plt.subplot(111, projection='3d') #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) #ax.view_init(elev=0., azim=90.) ax.view_init(elev=90., azim=0.) #ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects draw_Spheres(ax, initparams.env.balls) draw_block_list(ax, initparams.env.blocks) if initparams.env.OBB is not None: draw_obb(ax,initparams.env.OBB) draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) # draw_line(ax,edges,visibility=0.25) draw_line(ax,Path,color='r') if len(V) > 0: ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') # adjust the aspect ratio set_axes_equal(ax) make_transparent(ax) # plt.xlabel('s') # plt.ylabel('y') plt.pause(0.0001) def set_axes_equal(ax): '''Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to Input ax: a matplotlib axis, e.g., as output from plt.gca(). ''' x_limits = ax.get_xlim3d() y_limits = ax.get_ylim3d() z_limits = ax.get_zlim3d() x_range = abs(x_limits[1] - x_limits[0]) x_middle = np.mean(x_limits) y_range = abs(y_limits[1] - y_limits[0]) y_middle = np.mean(y_limits) z_range = abs(z_limits[1] - z_limits[0]) z_middle = np.mean(z_limits) # The plot bounding box is a sphere in the sense of the infinity # norm, hence I call half the max range the plot radius. plot_radius = 0.5*max([x_range, y_range, z_range]) ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) def make_transparent(ax): # make the panes transparent ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) # make the grid lines transparent ax.xaxis._axinfo["grid"]['color'] = (1,1,1,0) ax.yaxis._axinfo["grid"]['color'] = (1,1,1,0) ax.zaxis._axinfo["grid"]['color'] = (1,1,1,0) if __name__ == '__main__': pass ================================================ FILE: Search_based_Planning/Search_3D/queue.py ================================================ import collections import heapq import itertools class QueueFIFO: """ Class: QueueFIFO Description: QueueFIFO is designed for First-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.popleft() # leave from front class QueueLIFO: """ Class: QueueLIFO Description: QueueLIFO is designed for Last-in-First-out rule. """ def __init__(self): self.queue = collections.deque() def empty(self): return len(self.queue) == 0 def put(self, node): self.queue.append(node) # enter from back def get(self): return self.queue.pop() # leave from back class QueuePrior: """ Class: QueuePrior Description: QueuePrior reorders elements using value [priority] """ def __init__(self): self.queue = [] def empty(self): return len(self.queue) == 0 def put(self, item, priority): heapq.heappush(self.queue, (priority, item)) # reorder s using priority def get(self): return heapq.heappop(self.queue)[1] # pop out the smallest item def enumerate(self): return self.queue def check_remove(self, item): for (p, x) in self.queue: if item == x: self.queue.remove((p, x)) def top_key(self): return self.queue[0][0] class MinheapPQ: """ A priority queue based on min heap, which takes O(logn) on element removal https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes """ def __init__(self): self.pq = [] # lis of the entries arranged in a heap self.nodes = set() self.entry_finder = {} # mapping of the item entries self.counter = itertools.count() # unique sequence count self.REMOVED = '' def put(self, item, priority): '''add a new task or update the priority of an existing item''' if item in self.entry_finder: self.check_remove(item) count = next(self.counter) entry = [priority, count, item] self.entry_finder[item] = entry heapq.heappush(self.pq, entry) self.nodes.add(item) def check_remove(self, item): if item not in self.entry_finder: return entry = self.entry_finder.pop(item) entry[-1] = self.REMOVED self.nodes.remove(item) def get(self): """Remove and return the lowest priority task. Raise KeyError if empty.""" while self.pq: priority, count, item = heapq.heappop(self.pq) if item is not self.REMOVED: del self.entry_finder[item] self.nodes.remove(item) return item raise KeyError('pop from an empty priority queue') def top_key(self): return self.pq[0][0] def enumerate(self): return self.pq def allnodes(self): return self.nodes # class QueuePrior: # """ # Class: QueuePrior # Description: QueuePrior reorders elements using value [priority] # """ # def __init__(self): # self.queue = [] # def empty(self): # return len(self.queue) == 0 # def put(self, item, priority): # count = 0 # for (p, s) in self.queue: # if s == item: # self.queue[count] = (priority, item) # break # count += 1 # if count == len(self.queue): # heapq.heappush(self.queue, (priority, item)) # reorder s using priority # def get(self): # return heapq.heappop(self.queue)[1] # pop out the smallest item # def enumerate(self): # return self.queue # def check_remove(self, item): # for (p, s) in self.queue: # if item == s: # self.queue.remove((p, s)) # def top_key(self): # return self.queue[0][0] ================================================ FILE: Search_based_Planning/Search_3D/utils3D.py ================================================ import numpy as np import pyrr from collections import defaultdict import copy def getRay(x, y): direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] return np.array([x, direc]) def getDist(pos1, pos2): return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) def getManDist(pos1, pos2): return sum([abs(pos1[0] - pos2[0]), abs(pos1[1] - pos2[1]), abs(pos1[2] - pos2[2])]) def getNearest(Space, pt): '''get the nearest point on the grid''' mindis, minpt = 1000, None for pts in Space: dis = getDist(pts, pt) if dis < mindis: mindis, minpt = dis, pts return minpt def Heuristic(Space, t): '''Max norm distance''' h = {} for k in Space.keys(): h[k] = max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])]) return h def heuristic_fun(initparams, k, t=None): if t is None: t = initparams.goal return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])]) def isinbound(i, x, mode = False, factor = 0, isarray = False): if mode == 'obb': return isinobb(i, x, isarray) if isarray: compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) return compx & compy & compz else: return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor def isinball(i, x, factor = 0): if getDist(i[0:3], x) <= i[3] + factor: return True return False def isinobb(i, x, isarray = False): # transform the point from {W} to {body} if isarray: pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3] block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] return isinbound(block, pts, isarray = isarray) else: pt = i.T@np.append(x,1) block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] return isinbound(block, pt) def OBB2AABB(obb): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 aabb = copy.deepcopy(obb) P = obb.P a = obb.E A = obb.O # a1(A1 dot s) + a2(A2 dot s) + a3(A3 dot s) Ex = a[0]*abs(A[0][0]) + a[1]*abs(A[1][0]) + a[2]*abs(A[2][0]) Ey = a[0]*abs(A[0][1]) + a[1]*abs(A[1][1]) + a[2]*abs(A[2][1]) Ez = a[0]*abs(A[0][2]) + a[1]*abs(A[1][2]) + a[2]*abs(A[2][2]) E = np.array([Ex, Ey, Ez]) aabb.P = P aabb.E = E aabb.O = np.array([[1,0,0],[0,1,0],[0,0,1]]) return aabb def lineSphere(p0, p1, ball): # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf c, r = ball[0:3], ball[-1] line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]] d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]] t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * ( line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2]) if t <= 0: if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True elif t >= 1: d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True elif 0 < t < 1: x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True return False def lineAABB(p0, p1, dist, aabb): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 # aabb should have the attributes of P, E as center point and extents mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction hl = dist / 2 # radius T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] # do any of the principal axis form a separting axis? if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False # I.cross(s axis) ? r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) if abs(T[1] * I[2] - T[2] * I[1]) > r: return False # I.cross(y axis) ? r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) if abs(T[2] * I[0] - T[0] * I[2]) > r: return False # I.cross(z axis) ? r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) if abs(T[0] * I[1] - T[1] * I[0]) > r: return False return True def lineOBB(p0, p1, dist, obb): # transform points to obb frame res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T # record old position and set the position to origin oldP, obb.P= obb.P, [0,0,0] # calculate segment-AABB testing ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) # reset the position obb.P = oldP return ans def OBBOBB(obb1, obb2): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 # each obb class should contain attributes: # E: extents along three principle axis in R3 # P: position of the center axis in R3 # O: orthornormal basis in R3*3 a , b = np.array(obb1.E), np.array(obb2.E) Pa, Pb = np.array(obb1.P), np.array(obb2.P) A , B = np.array(obb1.O), np.array(obb2.O) # check if two oriented bounding boxes overlap # translation, in parent frame v = Pb - Pa # translation, in A's frame # vdotA[0],vdotA[1],vdotA[2] T = [v@B[0], v@B[1], v@B[2]] R = np.zeros([3,3]) for i in range(0,3): for k in range(0,3): R[i][k] = A[i]@B[k] # use separating axis thm for all 15 separating axes # if the separating axis cannot be found, then overlap # A's basis vector for i in range(0,3): ra = a[i] rb = b[0]*abs(R[i][0]) + b[1]*abs(R[i][1]) + b[2]*abs(R[i][2]) t = abs(T[i]) if t > ra + rb: return False for k in range(0,3): ra = a[0]*abs(R[0][k]) + a[1]*abs(R[1][k]) + a[2]*abs(R[2][k]) rb = b[k] t = abs(T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k]) if t > ra + rb: return False #9 cross products #L = A0 s B0 ra = a[1]*abs(R[2][0]) + a[2]*abs(R[1][0]) rb = b[1]*abs(R[0][2]) + b[2]*abs(R[0][1]) t = abs(T[2]*R[1][0] - T[1]*R[2][0]) if t > ra + rb: return False #L = A0 s B1 ra = a[1]*abs(R[2][1]) + a[2]*abs(R[1][1]) rb = b[0]*abs(R[0][2]) + b[2]*abs(R[0][0]) t = abs(T[2]*R[1][1] - T[1]*R[2][1]) if t > ra + rb: return False #L = A0 s B2 ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2]) rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0]) t = abs(T[2]*R[1][2] - T[1]*R[2][2]) if t > ra + rb: return False #L = A1 s B0 ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0]) rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1]) t = abs( T[0]*R[2][0] - T[2]*R[0][0] ) if t > ra + rb: return False # L = A1 s B1 ra = a[0]*abs(R[2][1]) + a[2]*abs(R[0][1]) rb = b[0]*abs(R[1][2]) + b[2]*abs(R[1][0]) t = abs( T[0]*R[2][1] - T[2]*R[0][1] ) if t > ra + rb: return False #L = A1 s B2 ra = a[0]*abs(R[2][2]) + a[2]*abs(R[0][2]) rb = b[0]*abs(R[1][1]) + b[1]*abs(R[1][0]) t = abs( T[0]*R[2][2] - T[2]*R[0][2] ) if t > ra + rb: return False #L = A2 s B0 ra = a[0]*abs(R[1][0]) + a[1]*abs(R[0][0]) rb = b[1]*abs(R[2][2]) + b[2]*abs(R[2][1]) t = abs( T[1]*R[0][0] - T[0]*R[1][0] ) if t > ra + rb: return False # L = A2 s B1 ra = a[0]*abs(R[1][1]) + a[1]*abs(R[0][1]) rb = b[0] *abs(R[2][2]) + b[2]*abs(R[2][0]) t = abs( T[1]*R[0][1] - T[0]*R[1][1] ) if t > ra + rb: return False #L = A2 s B2 ra = a[0]*abs(R[1][2]) + a[1]*abs(R[0][2]) rb = b[0]*abs(R[2][1]) + b[1]*abs(R[2][0]) t = abs( T[1]*R[0][2] - T[0]*R[1][2] ) if t > ra + rb: return False # no separating axis found, # the two boxes overlap return True def StateSpace(env, factor=0): boundary = env.boundary resolution = env.resolution xmin, xmax = boundary[0] + factor * resolution, boundary[3] - factor * resolution ymin, ymax = boundary[1] + factor * resolution, boundary[4] - factor * resolution zmin, zmax = boundary[2] + factor * resolution, boundary[5] - factor * resolution xarr = np.arange(xmin, xmax, resolution).astype(float) yarr = np.arange(ymin, ymax, resolution).astype(float) zarr = np.arange(zmin, zmax, resolution).astype(float) Space = set() for x in xarr: for y in yarr: for z in zarr: Space.add((x, y, z)) return Space def g_Space(initparams): '''This function is used to get nodes and discretize the space. State space is by s*y*z,3 where each 3 is a point in 3D.''' g = {} Space = StateSpace(initparams.env) for v in Space: g[v] = np.inf # this hashmap initialize all g values at inf return g def isCollide(initparams, x, child, dist=None): '''see if line intersects obstacle''' '''specified for expansion in A* 3D lookup table''' if dist==None: dist = getDist(x, child) # check in bound if not isinbound(initparams.env.boundary, child): return True, dist # check collision in AABB for i in range(len(initparams.env.AABB)): if lineAABB(x, child, dist, initparams.env.AABB[i]): return True, dist # check collision in ball for i in initparams.env.balls: if lineSphere(x, child, i): return True, dist # check collision with obb for i in initparams.env.OBB: if lineOBB(x, child, dist, i): return True, dist return False, dist def children(initparams, x, settings = 0): # get the neighbor of a specific state allchild = [] allcost = [] resolution = initparams.env.resolution for direc in initparams.Alldirec: child = tuple(map(np.add, x, np.multiply(direc, resolution))) if any([isinobb(i, child) for i in initparams.env.OBB]): continue if any([isinball(i ,child) for i in initparams.env.balls]): continue if any([isinbound(i ,child) for i in initparams.env.blocks]): continue if isinbound(initparams.env.boundary, child): allchild.append(child) allcost.append((child,initparams.Alldirec[direc]*resolution)) if settings == 0: return allchild if settings == 1: return allcost def obstacleFree(initparams, x): for i in initparams.env.blocks: if isinbound(i, x): return False for i in initparams.env.balls: if isinball(i, x): return False return True def cost(initparams, i, j, dist=None, settings='Euclidean'): if initparams.settings == 'NonCollisionChecking': if dist==None: dist = getDist(i,j) collide = False else: collide, dist = isCollide(initparams, i, j, dist) # collide, dist= False, getDist(i, j) if settings == 'Euclidean': if collide: return np.inf else: return dist if settings == 'Manhattan': if collide: return np.inf else: return getManDist(i, j) def initcost(initparams): # initialize Cost dictionary, could be modifed lateron c = defaultdict(lambda: defaultdict(dict)) # two key dicionary for xi in initparams.X: cdren = children(initparams, xi) for child in cdren: c[xi][child] = cost(initparams, xi, child) return c if __name__ == "__main__": import time from env3D import R_matrix, obb obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45)) # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]]) p0, p1 = [2.9,2.5,1],[1.9,2.5,1] pts = np.array([[1,2,3],[4,5,6],[7,8,9],[2,2,2],[1,1,1],[3,3,3]]) start = time.time() isinbound(obb1, pts, mode='obb', factor = 0, isarray = True) print(time.time() - start)