├── .idea ├── .gitignore ├── PathPlanning.iml ├── inspectionProfiles │ └── profiles_settings.xml ├── misc.xml ├── modules.xml └── vcs.xml ├── CurvesGenerator ├── __pycache__ │ ├── draw.cpython-37.pyc │ └── dubins_path.cpython-37.pyc ├── 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 ├── gif │ ├── BIT.gif │ ├── BIT2.gif │ ├── Dynamic_RRT_2D.gif │ ├── Extended_RRT_2D.gif │ ├── FMT.gif │ ├── Goal_biasd_RRT_2D.gif │ ├── INFORMED_RRT_STAR_2D.gif │ ├── INFORMED_RRT_STAR_2D2.gif │ ├── INFORMED_RRT_STAR_2D3.gif │ ├── RRT_2D.gif │ ├── RRT_CONNECT_2D.gif │ ├── RRT_STAR2_2D.gif │ ├── RRT_STAR_2D.gif │ └── RRT_STAR_SMART_2D.gif ├── rrt_2D │ ├── __pycache__ │ │ ├── env.cpython-37.pyc │ │ ├── plotting.cpython-37.pyc │ │ ├── queue.cpython-37.pyc │ │ ├── rrt.cpython-37.pyc │ │ └── utils.cpython-37.pyc │ ├── 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 ├── __pycache__ │ ├── Astar.cpython-38.pyc │ ├── env.cpython-37.pyc │ ├── env.cpython-38.pyc │ ├── plotting.cpython-37.pyc │ ├── plotting.cpython-38.pyc │ ├── queue.cpython-37.pyc │ └── queue.cpython-38.pyc ├── 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 ├── __pycache__ │ ├── Astar3D.cpython-37.pyc │ ├── env3D.cpython-37.pyc │ ├── plot_util3D.cpython-37.pyc │ ├── queue.cpython-37.pyc │ └── utils3D.cpython-37.pyc ├── bidirectional_Astar3D.py ├── env3D.py ├── plot_util3D.py ├── queue.py └── utils3D.py └── gif ├── ADstar_sig.gif ├── ADstar_small.gif ├── ARA_star.gif ├── Astar.gif ├── BF.gif ├── BFS.gif ├── Bi-Astar.gif ├── DFS.gif ├── D_star.gif ├── D_star_Lite.gif ├── Dijkstra.gif ├── LPA_star.gif ├── LPAstar.gif ├── LRTA_star.gif ├── RTAA_star.gif └── RepeatedA_star.gif /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Default ignored files 3 | /workspace.xml -------------------------------------------------------------------------------- /.idea/PathPlanning.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/draw.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/CurvesGenerator/__pycache__/draw.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/CurvesGenerator/__pycache__/dubins_path.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/bezier_path.py: -------------------------------------------------------------------------------- 1 | """ 2 | bezier path 3 | 4 | author: Atsushi Sakai(@Atsushi_twi) 5 | modified: huiming zhou 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | from scipy.special import comb 11 | import draw 12 | 13 | 14 | def calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset): 15 | 16 | dist = np.hypot(sx - gx, sy - gy) / offset 17 | control_points = np.array( 18 | [[sx, sy], 19 | [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)], 20 | [gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)], 21 | [gx, gy]]) 22 | 23 | path = calc_bezier_path(control_points, n_points=100) 24 | 25 | return path, control_points 26 | 27 | 28 | def calc_bezier_path(control_points, n_points=100): 29 | traj = [] 30 | 31 | for t in np.linspace(0, 1, n_points): 32 | traj.append(bezier(t, control_points)) 33 | 34 | return np.array(traj) 35 | 36 | 37 | def Comb(n, i, t): 38 | return comb(n, i) * t ** i * (1 - t) ** (n - i) 39 | 40 | 41 | def bezier(t, control_points): 42 | n = len(control_points) - 1 43 | return np.sum([Comb(n, i, t) * control_points[i] for i in range(n + 1)], axis=0) 44 | 45 | 46 | def bezier_derivatives_control_points(control_points, n_derivatives): 47 | w = {0: control_points} 48 | 49 | for i in range(n_derivatives): 50 | n = len(w[i]) 51 | w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) 52 | for j in range(n - 1)]) 53 | 54 | return w 55 | 56 | 57 | def curvature(dx, dy, ddx, ddy): 58 | return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2) 59 | 60 | 61 | def simulation(): 62 | sx = [-3, 0, 4, 6] 63 | sy = [2, 0, 1.5, 6] 64 | 65 | ratio = np.linspace(0, 1, 100) 66 | pathx, pathy = [], [] 67 | 68 | for t in ratio: 69 | x, y = [], [] 70 | for i in range(len(sx) - 1): 71 | x.append(sx[i + 1] * t + sx[i] * (1 - t)) 72 | y.append(sy[i + 1] * t + sy[i] * (1 - t)) 73 | 74 | xx, yy = [], [] 75 | for i in range(len(x) - 1): 76 | xx.append(x[i + 1] * t + x[i] * (1 - t)) 77 | yy.append(y[i + 1] * t + y[i] * (1 - t)) 78 | 79 | px = xx[1] * t + xx[0] * (1 - t) 80 | py = yy[1] * t + yy[0] * (1 - t) 81 | pathx.append(px) 82 | pathy.append(py) 83 | 84 | plt.cla() 85 | plt.plot(sx, sy, linestyle='-', marker='o', color='dimgray', label="Control Points") 86 | plt.plot(x, y, color='dodgerblue') 87 | plt.plot(xx, yy, color='cyan') 88 | plt.plot(pathx, pathy, color='darkorange', linewidth=2, label="Bezier Path") 89 | plt.plot(px, py, marker='o') 90 | plt.axis("equal") 91 | plt.legend() 92 | plt.title("Cubic Bezier Curve demo") 93 | plt.grid(True) 94 | plt.pause(0.001) 95 | 96 | plt.show() 97 | 98 | 99 | def main(): 100 | sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0) 101 | gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0) 102 | offset = 3.0 103 | 104 | path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset) 105 | 106 | t = 0.8 # Number in [0, 1] 107 | x_target, y_target = bezier(t, control_points) 108 | derivatives_cp = bezier_derivatives_control_points(control_points, 2) 109 | point = bezier(t, control_points) 110 | dt = bezier(t, derivatives_cp[1]) 111 | ddt = bezier(t, derivatives_cp[2]) 112 | # Radius of curv 113 | radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1]) 114 | # Normalize derivative 115 | dt /= np.linalg.norm(dt, 2) 116 | tangent = np.array([point, point + dt]) 117 | normal = np.array([point, point + [- dt[1], dt[0]]]) 118 | curvature_center = point + np.array([- dt[1], dt[0]]) * radius 119 | circle = plt.Circle(tuple(curvature_center), radius, 120 | color=(0, 0.8, 0.8), fill=False, linewidth=1) 121 | 122 | assert path.T[0][0] == sx, "path is invalid" 123 | assert path.T[1][0] == sy, "path is invalid" 124 | assert path.T[0][-1] == gx, "path is invalid" 125 | assert path.T[1][-1] == gy, "path is invalid" 126 | 127 | fig, ax = plt.subplots() 128 | ax.plot(path.T[0], path.T[1], label="Bezier Path") 129 | ax.plot(control_points.T[0], control_points.T[1], 130 | '--o', label="Control Points") 131 | ax.plot(x_target, y_target) 132 | ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") 133 | ax.plot(normal[:, 0], normal[:, 1], label="Normal") 134 | ax.add_artist(circle) 135 | draw.Arrow(sx, sy, syaw, 1, "darkorange") 136 | draw.Arrow(gx, gy, gyaw, 1, "darkorange") 137 | plt.grid(True) 138 | plt.title("Bezier Path: from Atsushi's work") 139 | ax.axis("equal") 140 | plt.show() 141 | 142 | 143 | if __name__ == '__main__': 144 | main() 145 | # simulation() 146 | -------------------------------------------------------------------------------- /CurvesGenerator/bspline_curve.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path Planner with B-Spline 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import scipy.interpolate as scipy_interpolate 12 | import cubic_spline as cs 13 | 14 | 15 | def approximate_b_spline_path(x, y, n_path_points, degree=3): 16 | t = range(len(x)) 17 | x_tup = scipy_interpolate.splrep(t, x, k=degree) 18 | y_tup = scipy_interpolate.splrep(t, y, k=degree) 19 | 20 | x_list = list(x_tup) 21 | x_list[1] = x + [0.0, 0.0, 0.0, 0.0] 22 | 23 | y_list = list(y_tup) 24 | y_list[1] = y + [0.0, 0.0, 0.0, 0.0] 25 | 26 | ipl_t = np.linspace(0.0, len(x) - 1, n_path_points) 27 | rx = scipy_interpolate.splev(ipl_t, x_list) 28 | ry = scipy_interpolate.splev(ipl_t, y_list) 29 | 30 | return rx, ry 31 | 32 | 33 | def interpolate_b_spline_path(x, y, n_path_points, degree=3): 34 | ipl_t = np.linspace(0.0, len(x) - 1, len(x)) 35 | spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree) 36 | spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree) 37 | 38 | travel = np.linspace(0.0, len(x) - 1, n_path_points) 39 | return spl_i_x(travel), spl_i_y(travel) 40 | 41 | 42 | def main(): 43 | print(__file__ + " start!!") 44 | # way points 45 | # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0] 46 | # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0] 47 | way_point_x = [-2, 2.0, 3.5, 5.5, 6.0, 8.0] 48 | way_point_y = [0, 2.7, -0.5, 0.5, 3.0, 4.0] 49 | 50 | sp = cs.Spline2D(way_point_x, way_point_y) 51 | s = np.arange(0, sp.s[-1], 0.1) 52 | 53 | rx, ry, ryaw, rk = [], [], [], [] 54 | for i_s in s: 55 | ix, iy = sp.calc_position(i_s) 56 | rx.append(ix) 57 | ry.append(iy) 58 | ryaw.append(sp.calc_yaw(i_s)) 59 | rk.append(sp.calc_curvature(i_s)) 60 | 61 | n_course_point = 100 # sampling number 62 | rax, ray = approximate_b_spline_path(way_point_x, way_point_y, 63 | n_course_point) 64 | rix, riy = interpolate_b_spline_path(way_point_x, way_point_y, 65 | n_course_point) 66 | 67 | # show results 68 | plt.plot(way_point_x, way_point_y, '-og', label="Control Points") 69 | plt.plot(rax, ray, '-r', label="Approximated B-Spline path") 70 | plt.plot(rix, riy, '-b', label="Interpolated B-Spline path") 71 | plt.plot(rx, ry, color='dimgray', label="Cubic Spline") 72 | plt.grid(True) 73 | plt.title("Curves Comparison") 74 | plt.legend() 75 | plt.axis("equal") 76 | plt.show() 77 | 78 | 79 | if __name__ == '__main__': 80 | main() 81 | -------------------------------------------------------------------------------- /CurvesGenerator/cubic_spline.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | u""" 4 | Cubic Spline library on python 5 | 6 | author Atsushi Sakai 7 | 8 | usage: see test codes as below 9 | 10 | license: MIT 11 | """ 12 | import math 13 | import numpy as np 14 | import bisect 15 | 16 | 17 | class Spline: 18 | u""" 19 | Cubic Spline class 20 | """ 21 | 22 | def __init__(self, x, y): 23 | self.b, self.c, self.d, self.w = [], [], [], [] 24 | 25 | self.x = x 26 | self.y = y 27 | 28 | self.nx = len(x) # dimension of s 29 | h = np.diff(x) 30 | 31 | # calc coefficient cBest 32 | self.a = [iy for iy in y] 33 | 34 | # calc coefficient cBest 35 | A = self.__calc_A(h) 36 | B = self.__calc_B(h) 37 | self.c = np.linalg.solve(A, B) 38 | # print(self.c1) 39 | 40 | # calc spline coefficient b and d 41 | for i in range(self.nx - 1): 42 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 43 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 44 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 45 | self.b.append(tb) 46 | 47 | def calc(self, t): 48 | u""" 49 | Calc position 50 | 51 | if t is outside of the input s, return None 52 | 53 | """ 54 | 55 | if t < self.x[0]: 56 | return None 57 | elif t > self.x[-1]: 58 | return None 59 | 60 | i = self.__search_index(t) 61 | dx = t - self.x[i] 62 | result = self.a[i] + self.b[i] * dx + \ 63 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 64 | 65 | return result 66 | 67 | def calcd(self, t): 68 | u""" 69 | Calc first derivative 70 | 71 | if t is outside of the input s, return None 72 | """ 73 | 74 | if t < self.x[0]: 75 | return None 76 | elif t > self.x[-1]: 77 | return None 78 | 79 | i = self.__search_index(t) 80 | dx = t - self.x[i] 81 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 82 | return result 83 | 84 | def calcdd(self, t): 85 | u""" 86 | Calc second derivative 87 | """ 88 | 89 | if t < self.x[0]: 90 | return None 91 | elif t > self.x[-1]: 92 | return None 93 | 94 | i = self.__search_index(t) 95 | dx = t - self.x[i] 96 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 97 | return result 98 | 99 | def __search_index(self, x): 100 | u""" 101 | search data segment index 102 | """ 103 | return bisect.bisect(self.x, x) - 1 104 | 105 | def __calc_A(self, h): 106 | u""" 107 | calc matrix A for spline coefficient cBest 108 | """ 109 | A = np.zeros((self.nx, self.nx)) 110 | A[0, 0] = 1.0 111 | for i in range(self.nx - 1): 112 | if i != (self.nx - 2): 113 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 114 | A[i + 1, i] = h[i] 115 | A[i, i + 1] = h[i] 116 | 117 | A[0, 1] = 0.0 118 | A[self.nx - 1, self.nx - 2] = 0.0 119 | A[self.nx - 1, self.nx - 1] = 1.0 120 | # print(A) 121 | return A 122 | 123 | def __calc_B(self, h): 124 | u""" 125 | calc matrix B for spline coefficient cBest 126 | """ 127 | B = np.zeros(self.nx) 128 | for i in range(self.nx - 2): 129 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 130 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 131 | # print(B) 132 | return B 133 | 134 | 135 | class Spline2D: 136 | u""" 137 | 2D Cubic Spline class 138 | 139 | """ 140 | 141 | def __init__(self, x, y): 142 | self.s = self.__calc_s(x, y) 143 | self.sx = Spline(self.s, x) 144 | self.sy = Spline(self.s, y) 145 | 146 | def __calc_s(self, x, y): 147 | dx = np.diff(x) 148 | dy = np.diff(y) 149 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 150 | for (idx, idy) in zip(dx, dy)] 151 | s = [0] 152 | s.extend(np.cumsum(self.ds)) 153 | return s 154 | 155 | def calc_position(self, s): 156 | u""" 157 | calc position 158 | """ 159 | x = self.sx.calc(s) 160 | y = self.sy.calc(s) 161 | 162 | return x, y 163 | 164 | def calc_curvature(self, s): 165 | u""" 166 | calc curvature 167 | """ 168 | dx = self.sx.calcd(s) 169 | ddx = self.sx.calcdd(s) 170 | dy = self.sy.calcd(s) 171 | ddy = self.sy.calcdd(s) 172 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 173 | return k 174 | 175 | def calc_yaw(self, s): 176 | u""" 177 | calc yaw 178 | """ 179 | dx = self.sx.calcd(s) 180 | dy = self.sy.calcd(s) 181 | yaw = math.atan2(dy, dx) 182 | return yaw 183 | 184 | 185 | def calc_spline_course(x, y, ds=0.1): 186 | sp = Spline2D(x, y) 187 | s = np.arange(0, sp.s[-1], ds) 188 | 189 | rx, ry, ryaw, rk = [], [], [], [] 190 | for i_s in s: 191 | ix, iy = sp.calc_position(i_s) 192 | rx.append(ix) 193 | ry.append(iy) 194 | ryaw.append(sp.calc_yaw(i_s)) 195 | rk.append(sp.calc_curvature(i_s)) 196 | 197 | return rx, ry, ryaw, rk, s 198 | 199 | 200 | def test_spline2d(): 201 | print("Spline 2D test") 202 | import matplotlib.pyplot as plt 203 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 204 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 205 | 206 | sp = Spline2D(x, y) 207 | s = np.arange(0, sp.s[-1], 0.1) 208 | 209 | rx, ry, ryaw, rk = [], [], [], [] 210 | for i_s in s: 211 | ix, iy = sp.calc_position(i_s) 212 | rx.append(ix) 213 | ry.append(iy) 214 | ryaw.append(sp.calc_yaw(i_s)) 215 | rk.append(sp.calc_curvature(i_s)) 216 | 217 | flg, ax = plt.subplots(1) 218 | plt.plot(x, y, "xb", label="input") 219 | plt.plot(rx, ry, "-r", label="spline") 220 | plt.grid(True) 221 | plt.axis("equal") 222 | plt.xlabel("s[m]") 223 | plt.ylabel("y[m]") 224 | plt.legend() 225 | 226 | flg, ax = plt.subplots(1) 227 | plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") 228 | plt.grid(True) 229 | plt.legend() 230 | plt.xlabel("line length[m]") 231 | plt.ylabel("yaw angle[deg]") 232 | 233 | flg, ax = plt.subplots(1) 234 | plt.plot(s, rk, "-r", label="curvature") 235 | plt.grid(True) 236 | plt.legend() 237 | plt.xlabel("line length[m]") 238 | plt.ylabel("curvature [1/m]") 239 | 240 | plt.show() 241 | 242 | 243 | def test_spline(): 244 | print("Spline test") 245 | import matplotlib.pyplot as plt 246 | x = [-0.5, 0.0, 0.5, 1.0, 1.5] 247 | y = [3.2, 2.7, 6, 5, 6.5] 248 | 249 | spline = Spline(x, y) 250 | rx = np.arange(-2.0, 4, 0.01) 251 | ry = [spline.calc(i) for i in rx] 252 | 253 | plt.plot(x, y, "xb") 254 | plt.plot(rx, ry, "-r") 255 | plt.grid(True) 256 | plt.axis("equal") 257 | plt.show() 258 | 259 | 260 | if __name__ == '__main__': 261 | test_spline() 262 | # test_spline2d() 263 | -------------------------------------------------------------------------------- /CurvesGenerator/draw.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | PI = np.pi 4 | 5 | 6 | class Arrow: 7 | def __init__(self, x, y, theta, L, c): 8 | angle = np.deg2rad(30) 9 | d = 0.5 * L 10 | w = 2 11 | 12 | x_start = x 13 | y_start = y 14 | x_end = x + L * np.cos(theta) 15 | y_end = y + L * np.sin(theta) 16 | 17 | theta_hat_L = theta + PI - angle 18 | theta_hat_R = theta + PI + angle 19 | 20 | x_hat_start = x_end 21 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 22 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 23 | 24 | y_hat_start = y_end 25 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 26 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 27 | 28 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 29 | plt.plot([x_hat_start, x_hat_end_L], 30 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 31 | plt.plot([x_hat_start, x_hat_end_R], 32 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 33 | 34 | 35 | class Car: 36 | def __init__(self, x, y, yaw, w, L): 37 | theta_B = PI + yaw 38 | 39 | xB = x + L / 4 * np.cos(theta_B) 40 | yB = y + L / 4 * np.sin(theta_B) 41 | 42 | theta_BL = theta_B + PI / 2 43 | theta_BR = theta_B - PI / 2 44 | 45 | x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex 46 | y_BL = yB + w / 2 * np.sin(theta_BL) 47 | x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex 48 | y_BR = yB + w / 2 * np.sin(theta_BR) 49 | 50 | x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex 51 | y_FL = y_BL + L * np.sin(yaw) 52 | x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex 53 | y_FR = y_BR + L * np.sin(yaw) 54 | 55 | plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL], 56 | [y_BL, y_BR, y_FR, y_FL, y_BL], 57 | linewidth=1, color='black') 58 | 59 | Arrow(x, y, yaw, L / 2, 'black') 60 | # plt.axis("equal") 61 | # plt.show() 62 | 63 | 64 | if __name__ == '__main__': 65 | # Arrow(-1, 2, 60) 66 | Car(0, 0, 1, 2, 60) 67 | -------------------------------------------------------------------------------- /CurvesGenerator/quartic_polynomial.py: -------------------------------------------------------------------------------- 1 | """ 2 | Quartic Polynomial 3 | """ 4 | 5 | import numpy as np 6 | 7 | 8 | class QuarticPolynomial: 9 | def __init__(self, x0, v0, a0, v1, a1, T): 10 | A = np.array([[3 * T ** 2, 4 * T ** 3], 11 | [6 * T, 12 * T ** 2]]) 12 | b = np.array([v1 - v0 - a0 * T, 13 | a1 - a0]) 14 | X = np.linalg.solve(A, b) 15 | 16 | self.a0 = x0 17 | self.a1 = v0 18 | self.a2 = a0 / 2.0 19 | self.a3 = X[0] 20 | self.a4 = X[1] 21 | 22 | def calc_xt(self, t): 23 | xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ 24 | self.a3 * t ** 3 + self.a4 * t ** 4 25 | 26 | return xt 27 | 28 | def calc_dxt(self, t): 29 | xt = self.a1 + 2 * self.a2 * t + \ 30 | 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 31 | 32 | return xt 33 | 34 | def calc_ddxt(self, t): 35 | xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 36 | 37 | return xt 38 | 39 | def calc_dddxt(self, t): 40 | xt = 6 * self.a3 + 24 * self.a4 * t 41 | 42 | return xt 43 | 44 | -------------------------------------------------------------------------------- /CurvesGenerator/quintic_polynomial.py: -------------------------------------------------------------------------------- 1 | """ 2 | Quintic Polynomial 3 | """ 4 | 5 | import math 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | 9 | import draw 10 | 11 | 12 | class QuinticPolynomial: 13 | def __init__(self, x0, v0, a0, x1, v1, a1, T): 14 | A = np.array([[T ** 3, T ** 4, T ** 5], 15 | [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], 16 | [6 * T, 12 * T ** 2, 20 * T ** 3]]) 17 | b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2, 18 | v1 - v0 - a0 * T, 19 | a1 - a0]) 20 | X = np.linalg.solve(A, b) 21 | 22 | self.a0 = x0 23 | self.a1 = v0 24 | self.a2 = a0 / 2.0 25 | self.a3 = X[0] 26 | self.a4 = X[1] 27 | self.a5 = X[2] 28 | 29 | def calc_xt(self, t): 30 | xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ 31 | self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 32 | 33 | return xt 34 | 35 | def calc_dxt(self, t): 36 | dxt = self.a1 + 2 * self.a2 * t + \ 37 | 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 38 | 39 | return dxt 40 | 41 | def calc_ddxt(self, t): 42 | ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 43 | 44 | return ddxt 45 | 46 | def calc_dddxt(self, t): 47 | dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 48 | 49 | return dddxt 50 | 51 | 52 | class Trajectory: 53 | def __init__(self): 54 | self.t = [] 55 | self.x = [] 56 | self.y = [] 57 | self.yaw = [] 58 | self.v = [] 59 | self.a = [] 60 | self.jerk = [] 61 | 62 | 63 | def simulation(): 64 | sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1 65 | gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1 66 | 67 | MAX_ACCEL = 1.0 # max accel [m/s2] 68 | MAX_JERK = 0.5 # max jerk [m/s3] 69 | dt = 0.1 # T tick [s] 70 | 71 | MIN_T = 5 72 | MAX_T = 100 73 | T_STEP = 5 74 | 75 | sv_x = sv * math.cos(syaw) 76 | sv_y = sv * math.sin(syaw) 77 | gv_x = gv * math.cos(gyaw) 78 | gv_y = gv * math.sin(gyaw) 79 | 80 | sa_x = sa * math.cos(syaw) 81 | sa_y = sa * math.sin(syaw) 82 | ga_x = ga * math.cos(gyaw) 83 | ga_y = ga * math.sin(gyaw) 84 | 85 | path = Trajectory() 86 | 87 | for T in np.arange(MIN_T, MAX_T, T_STEP): 88 | path = Trajectory() 89 | xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T) 90 | yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T) 91 | 92 | for t in np.arange(0.0, T + dt, dt): 93 | path.t.append(t) 94 | path.x.append(xqp.calc_xt(t)) 95 | path.y.append(yqp.calc_xt(t)) 96 | 97 | vx = xqp.calc_dxt(t) 98 | vy = yqp.calc_dxt(t) 99 | path.v.append(np.hypot(vx, vy)) 100 | path.yaw.append(math.atan2(vy, vx)) 101 | 102 | ax = xqp.calc_ddxt(t) 103 | ay = yqp.calc_ddxt(t) 104 | a = np.hypot(ax, ay) 105 | 106 | if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0: 107 | a *= -1 108 | path.a.append(a) 109 | 110 | jx = xqp.calc_dddxt(t) 111 | jy = yqp.calc_dddxt(t) 112 | j = np.hypot(jx, jy) 113 | 114 | if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0: 115 | j *= -1 116 | path.jerk.append(j) 117 | 118 | if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK: 119 | break 120 | 121 | print("t_len: ", path.t, "s") 122 | print("max_v: ", max(path.v), "m/s") 123 | print("max_a: ", max(np.abs(path.a)), "m/s2") 124 | print("max_jerk: ", max(np.abs(path.jerk)), "m/s3") 125 | 126 | for i in range(len(path.t)): 127 | plt.cla() 128 | plt.gcf().canvas.mpl_connect('key_release_event', 129 | lambda event: [exit(0) if event.key == 'escape' else None]) 130 | plt.axis("equal") 131 | plt.plot(path.x, path.y, linewidth=2, color='gray') 132 | draw.Car(sx, sy, syaw, 1.5, 3) 133 | draw.Car(gx, gy, gyaw, 1.5, 3) 134 | draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3) 135 | plt.title("Quintic Polynomial Curves") 136 | plt.grid(True) 137 | plt.pause(0.001) 138 | 139 | plt.show() 140 | 141 | 142 | if __name__ == '__main__': 143 | simulation() 144 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Huiming Zhou 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/BIT.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/BIT.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/BIT2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/BIT2.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/Dynamic_RRT_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/Dynamic_RRT_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/Extended_RRT_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/Extended_RRT_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/FMT.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/FMT.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D2.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D3.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/RRT_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/RRT_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/RRT_CONNECT_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/RRT_CONNECT_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/RRT_STAR2_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/RRT_STAR2_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/RRT_STAR_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/RRT_STAR_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Environment for rrt_2D 3 | @author: huiming zhou 4 | """ 5 | 6 | 7 | class Env: 8 | def __init__(self): 9 | self.x_range = (0, 50) 10 | self.y_range = (0, 30) 11 | self.obs_boundary = self.obs_boundary() 12 | self.obs_circle = self.obs_circle() 13 | self.obs_rectangle = self.obs_rectangle() 14 | 15 | @staticmethod 16 | def obs_boundary(): 17 | obs_boundary = [ 18 | [0, 0, 1, 30], 19 | [0, 30, 50, 1], 20 | [1, 0, 50, 1], 21 | [50, 1, 1, 30] 22 | ] 23 | return obs_boundary 24 | 25 | @staticmethod 26 | def obs_rectangle(): 27 | obs_rectangle = [ 28 | [14, 12, 8, 2], 29 | [18, 22, 8, 3], 30 | [26, 7, 2, 12], 31 | [32, 14, 10, 2] 32 | ] 33 | return obs_rectangle 34 | 35 | @staticmethod 36 | def obs_circle(): 37 | obs_cir = [ 38 | [7, 12, 3], 39 | [46, 20, 2], 40 | [15, 5, 2], 41 | [37, 7, 3], 42 | [37, 23, 3] 43 | ] 44 | 45 | return obs_cir 46 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/fast_marching_trees.py: -------------------------------------------------------------------------------- 1 | """ 2 | Fast Marching Trees (FMT*) 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import random 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import matplotlib.patches as patches 13 | 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 15 | "/../../Sampling_based_Planning/") 16 | 17 | from Sampling_based_Planning.rrt_2D import env, plotting, utils 18 | 19 | 20 | class Node: 21 | def __init__(self, n): 22 | self.x = n[0] 23 | self.y = n[1] 24 | self.parent = None 25 | self.cost = np.inf 26 | 27 | 28 | class FMT: 29 | def __init__(self, x_start, x_goal, search_radius): 30 | self.x_init = Node(x_start) 31 | self.x_goal = Node(x_goal) 32 | self.search_radius = search_radius 33 | 34 | self.env = env.Env() 35 | self.plotting = plotting.Plotting(x_start, x_goal) 36 | self.utils = utils.Utils() 37 | 38 | self.fig, self.ax = plt.subplots() 39 | self.delta = self.utils.delta 40 | self.x_range = self.env.x_range 41 | self.y_range = self.env.y_range 42 | self.obs_circle = self.env.obs_circle 43 | self.obs_rectangle = self.env.obs_rectangle 44 | self.obs_boundary = self.env.obs_boundary 45 | 46 | self.V = set() 47 | self.V_unvisited = set() 48 | self.V_open = set() 49 | self.V_closed = set() 50 | self.sample_numbers = 1000 51 | 52 | def Init(self): 53 | samples = self.SampleFree() 54 | 55 | self.x_init.cost = 0.0 56 | self.V.add(self.x_init) 57 | self.V.update(samples) 58 | self.V_unvisited.update(samples) 59 | self.V_unvisited.add(self.x_goal) 60 | self.V_open.add(self.x_init) 61 | 62 | def Planning(self): 63 | self.Init() 64 | z = self.x_init 65 | n = self.sample_numbers 66 | rn = self.search_radius * math.sqrt((math.log(n) / n)) 67 | Visited = [] 68 | 69 | while z is not self.x_goal: 70 | V_open_new = set() 71 | X_near = self.Near(self.V_unvisited, z, rn) 72 | Visited.append(z) 73 | 74 | for x in X_near: 75 | Y_near = self.Near(self.V_open, x, rn) 76 | cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near} 77 | y_min = min(cost_list, key=cost_list.get) 78 | 79 | if not self.utils.is_collision(y_min, x): 80 | x.parent = y_min 81 | V_open_new.add(x) 82 | self.V_unvisited.remove(x) 83 | x.cost = y_min.cost + self.Cost(y_min, x) 84 | 85 | self.V_open.update(V_open_new) 86 | self.V_open.remove(z) 87 | self.V_closed.add(z) 88 | 89 | if not self.V_open: 90 | print("open set empty!") 91 | break 92 | 93 | cost_open = {y: y.cost for y in self.V_open} 94 | z = min(cost_open, key=cost_open.get) 95 | 96 | # node_end = self.ChooseGoalPoint() 97 | path_x, path_y = self.ExtractPath() 98 | self.animation(path_x, path_y, Visited[1: len(Visited)]) 99 | 100 | def ChooseGoalPoint(self): 101 | Near = self.Near(self.V, self.x_goal, 2.0) 102 | cost = {y: y.cost + self.Cost(y, self.x_goal) for y in Near} 103 | 104 | return min(cost, key=cost.get) 105 | 106 | def ExtractPath(self): 107 | path_x, path_y = [], [] 108 | node = self.x_goal 109 | 110 | while node.parent: 111 | path_x.append(node.x) 112 | path_y.append(node.y) 113 | node = node.parent 114 | 115 | path_x.append(self.x_init.x) 116 | path_y.append(self.x_init.y) 117 | 118 | return path_x, path_y 119 | 120 | def Cost(self, x_start, x_end): 121 | if self.utils.is_collision(x_start, x_end): 122 | return np.inf 123 | else: 124 | return self.calc_dist(x_start, x_end) 125 | 126 | @staticmethod 127 | def calc_dist(x_start, x_end): 128 | return math.hypot(x_start.x - x_end.x, x_start.y - x_end.y) 129 | 130 | @staticmethod 131 | def Near(nodelist, z, rn): 132 | return {nd for nd in nodelist 133 | if 0 < (nd.x - z.x) ** 2 + (nd.y - z.y) ** 2 <= rn ** 2} 134 | 135 | def SampleFree(self): 136 | n = self.sample_numbers 137 | delta = self.utils.delta 138 | Sample = set() 139 | 140 | ind = 0 141 | while ind < n: 142 | node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 143 | random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 144 | if self.utils.is_inside_obs(node): 145 | continue 146 | else: 147 | Sample.add(node) 148 | ind += 1 149 | 150 | return Sample 151 | 152 | def animation(self, path_x, path_y, visited): 153 | self.plot_grid("Fast Marching Trees (FMT*)") 154 | 155 | for node in self.V: 156 | plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3) 157 | 158 | count = 0 159 | for node in visited: 160 | count += 1 161 | plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g') 162 | plt.gcf().canvas.mpl_connect( 163 | 'key_release_event', 164 | lambda event: [exit(0) if event.key == 'escape' else None]) 165 | if count % 10 == 0: 166 | plt.pause(0.001) 167 | 168 | plt.plot(path_x, path_y, linewidth=2, color='red') 169 | plt.pause(0.01) 170 | plt.show() 171 | 172 | def plot_grid(self, name): 173 | 174 | for (ox, oy, w, h) in self.obs_boundary: 175 | self.ax.add_patch( 176 | patches.Rectangle( 177 | (ox, oy), w, h, 178 | edgecolor='black', 179 | facecolor='black', 180 | fill=True 181 | ) 182 | ) 183 | 184 | for (ox, oy, w, h) in self.obs_rectangle: 185 | self.ax.add_patch( 186 | patches.Rectangle( 187 | (ox, oy), w, h, 188 | edgecolor='black', 189 | facecolor='gray', 190 | fill=True 191 | ) 192 | ) 193 | 194 | for (ox, oy, r) in self.obs_circle: 195 | self.ax.add_patch( 196 | patches.Circle( 197 | (ox, oy), r, 198 | edgecolor='black', 199 | facecolor='gray', 200 | fill=True 201 | ) 202 | ) 203 | 204 | plt.plot(self.x_init.x, self.x_init.y, "bs", linewidth=3) 205 | plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3) 206 | 207 | plt.title(name) 208 | plt.axis("equal") 209 | 210 | 211 | def main(): 212 | x_start = (18, 8) # Starting node 213 | x_goal = (37, 18) # Goal node 214 | 215 | fmt = FMT(x_start, x_goal, 40) 216 | fmt.Planning() 217 | 218 | 219 | if __name__ == '__main__': 220 | main() 221 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/plotting.py: -------------------------------------------------------------------------------- 1 | """ 2 | Plotting tools for Sampling-based algorithms 3 | @author: huiming zhou 4 | """ 5 | 6 | import matplotlib.pyplot as plt 7 | import matplotlib.patches as patches 8 | import os 9 | import sys 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Sampling_based_Planning/") 13 | 14 | from Sampling_based_Planning.rrt_2D import env 15 | 16 | 17 | class Plotting: 18 | def __init__(self, x_start, x_goal): 19 | self.xI, self.xG = x_start, x_goal 20 | self.env = env.Env() 21 | self.obs_bound = self.env.obs_boundary 22 | self.obs_circle = self.env.obs_circle 23 | self.obs_rectangle = self.env.obs_rectangle 24 | 25 | def animation(self, nodelist, path, name, animation=False): 26 | self.plot_grid(name) 27 | self.plot_visited(nodelist, animation) 28 | self.plot_path(path) 29 | 30 | def animation_connect(self, V1, V2, path, name): 31 | self.plot_grid(name) 32 | self.plot_visited_connect(V1, V2) 33 | self.plot_path(path) 34 | 35 | def plot_grid(self, name): 36 | fig, ax = plt.subplots() 37 | 38 | for (ox, oy, w, h) in self.obs_bound: 39 | ax.add_patch( 40 | patches.Rectangle( 41 | (ox, oy), w, h, 42 | edgecolor='black', 43 | facecolor='black', 44 | fill=True 45 | ) 46 | ) 47 | 48 | for (ox, oy, w, h) in self.obs_rectangle: 49 | ax.add_patch( 50 | patches.Rectangle( 51 | (ox, oy), w, h, 52 | edgecolor='black', 53 | facecolor='gray', 54 | fill=True 55 | ) 56 | ) 57 | 58 | for (ox, oy, r) in self.obs_circle: 59 | ax.add_patch( 60 | patches.Circle( 61 | (ox, oy), r, 62 | edgecolor='black', 63 | facecolor='gray', 64 | fill=True 65 | ) 66 | ) 67 | 68 | plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3) 69 | plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) 70 | 71 | plt.title(name) 72 | plt.axis("equal") 73 | 74 | @staticmethod 75 | def plot_visited(nodelist, animation): 76 | if animation: 77 | count = 0 78 | for node in nodelist: 79 | count += 1 80 | if node.parent: 81 | plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") 82 | plt.gcf().canvas.mpl_connect('key_release_event', 83 | lambda event: 84 | [exit(0) if event.key == 'escape' else None]) 85 | if count % 10 == 0: 86 | plt.pause(0.001) 87 | else: 88 | for node in nodelist: 89 | if node.parent: 90 | plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") 91 | 92 | @staticmethod 93 | def plot_visited_connect(V1, V2): 94 | len1, len2 = len(V1), len(V2) 95 | 96 | for k in range(max(len1, len2)): 97 | if k < len1: 98 | if V1[k].parent: 99 | plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], "-g") 100 | if k < len2: 101 | if V2[k].parent: 102 | plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], "-g") 103 | 104 | plt.gcf().canvas.mpl_connect('key_release_event', 105 | lambda event: [exit(0) if event.key == 'escape' else None]) 106 | 107 | if k % 2 == 0: 108 | plt.pause(0.001) 109 | 110 | plt.pause(0.01) 111 | 112 | @staticmethod 113 | def plot_path(path): 114 | if len(path) != 0: 115 | plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) 116 | plt.pause(0.01) 117 | plt.show() 118 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/queue.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import heapq 3 | 4 | 5 | class QueueFIFO: 6 | """ 7 | Class: QueueFIFO 8 | Description: QueueFIFO is designed for First-in-First-out rule. 9 | """ 10 | 11 | def __init__(self): 12 | self.queue = collections.deque() 13 | 14 | def empty(self): 15 | return len(self.queue) == 0 16 | 17 | def put(self, node): 18 | self.queue.append(node) # enter from back 19 | 20 | def get(self): 21 | return self.queue.popleft() # leave from front 22 | 23 | 24 | class QueueLIFO: 25 | """ 26 | Class: QueueLIFO 27 | Description: QueueLIFO is designed for Last-in-First-out rule. 28 | """ 29 | 30 | def __init__(self): 31 | self.queue = collections.deque() 32 | 33 | def empty(self): 34 | return len(self.queue) == 0 35 | 36 | def put(self, node): 37 | self.queue.append(node) # enter from back 38 | 39 | def get(self): 40 | return self.queue.pop() # leave from back 41 | 42 | 43 | class QueuePrior: 44 | """ 45 | Class: QueuePrior 46 | Description: QueuePrior reorders elements using value [priority] 47 | """ 48 | 49 | def __init__(self): 50 | self.queue = [] 51 | 52 | def empty(self): 53 | return len(self.queue) == 0 54 | 55 | def put(self, item, priority): 56 | heapq.heappush(self.queue, (priority, item)) # reorder s using priority 57 | 58 | def get(self): 59 | return heapq.heappop(self.queue)[1] # pop out the smallest item 60 | 61 | def enumerate(self): 62 | return self.queue 63 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/rrt.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT_2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Sampling_based_Planning/") 13 | 14 | from Sampling_based_Planning.rrt_2D import env, plotting, utils 15 | 16 | 17 | class Node: 18 | def __init__(self, n): 19 | self.x = n[0] 20 | self.y = n[1] 21 | self.parent = None 22 | 23 | 24 | class Rrt: 25 | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max): 26 | self.s_start = Node(s_start) 27 | self.s_goal = Node(s_goal) 28 | self.step_len = step_len 29 | self.goal_sample_rate = goal_sample_rate 30 | self.iter_max = iter_max 31 | self.vertex = [self.s_start] 32 | 33 | self.env = env.Env() 34 | self.plotting = plotting.Plotting(s_start, s_goal) 35 | self.utils = utils.Utils() 36 | 37 | self.x_range = self.env.x_range 38 | self.y_range = self.env.y_range 39 | self.obs_circle = self.env.obs_circle 40 | self.obs_rectangle = self.env.obs_rectangle 41 | self.obs_boundary = self.env.obs_boundary 42 | 43 | def planning(self): 44 | for i in range(self.iter_max): 45 | node_rand = self.generate_random_node(self.goal_sample_rate) 46 | node_near = self.nearest_neighbor(self.vertex, node_rand) 47 | node_new = self.new_state(node_near, node_rand) 48 | 49 | if node_new and not self.utils.is_collision(node_near, node_new): 50 | self.vertex.append(node_new) 51 | dist, _ = self.get_distance_and_angle(node_new, self.s_goal) 52 | 53 | if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal): 54 | self.new_state(node_new, self.s_goal) 55 | return self.extract_path(node_new) 56 | 57 | return None 58 | 59 | def generate_random_node(self, goal_sample_rate): 60 | delta = self.utils.delta 61 | 62 | if np.random.random() > goal_sample_rate: 63 | return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 64 | np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 65 | 66 | return self.s_goal 67 | 68 | @staticmethod 69 | def nearest_neighbor(node_list, n): 70 | return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) 71 | for nd in node_list]))] 72 | 73 | def new_state(self, node_start, node_end): 74 | dist, theta = self.get_distance_and_angle(node_start, node_end) 75 | 76 | dist = min(self.step_len, dist) 77 | node_new = Node((node_start.x + dist * math.cos(theta), 78 | node_start.y + dist * math.sin(theta))) 79 | node_new.parent = node_start 80 | 81 | return node_new 82 | 83 | def extract_path(self, node_end): 84 | path = [(self.s_goal.x, self.s_goal.y)] 85 | node_now = node_end 86 | 87 | while node_now.parent is not None: 88 | node_now = node_now.parent 89 | path.append((node_now.x, node_now.y)) 90 | 91 | return path 92 | 93 | @staticmethod 94 | def get_distance_and_angle(node_start, node_end): 95 | dx = node_end.x - node_start.x 96 | dy = node_end.y - node_start.y 97 | return math.hypot(dx, dy), math.atan2(dy, dx) 98 | 99 | 100 | def main(): 101 | x_start = (2, 2) # Starting node 102 | x_goal = (49, 24) # Goal node 103 | 104 | rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000) 105 | path = rrt.planning() 106 | 107 | if path: 108 | rrt.plotting.animation(rrt.vertex, path, "RRT", True) 109 | else: 110 | print("No Path Found!") 111 | 112 | 113 | if __name__ == '__main__': 114 | main() 115 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/rrt_connect.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT_CONNECT_2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import copy 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 14 | "/../../Sampling_based_Planning/") 15 | 16 | from Sampling_based_Planning.rrt_2D import env, plotting, utils 17 | 18 | 19 | class Node: 20 | def __init__(self, n): 21 | self.x = n[0] 22 | self.y = n[1] 23 | self.parent = None 24 | 25 | 26 | class RrtConnect: 27 | def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max): 28 | self.s_start = Node(s_start) 29 | self.s_goal = Node(s_goal) 30 | self.step_len = step_len 31 | self.goal_sample_rate = goal_sample_rate 32 | self.iter_max = iter_max 33 | self.V1 = [self.s_start] 34 | self.V2 = [self.s_goal] 35 | 36 | self.env = env.Env() 37 | self.plotting = plotting.Plotting(s_start, s_goal) 38 | self.utils = utils.Utils() 39 | 40 | self.x_range = self.env.x_range 41 | self.y_range = self.env.y_range 42 | self.obs_circle = self.env.obs_circle 43 | self.obs_rectangle = self.env.obs_rectangle 44 | self.obs_boundary = self.env.obs_boundary 45 | 46 | def planning(self): 47 | for i in range(self.iter_max): 48 | node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate) 49 | node_near = self.nearest_neighbor(self.V1, node_rand) 50 | node_new = self.new_state(node_near, node_rand) 51 | 52 | if node_new and not self.utils.is_collision(node_near, node_new): 53 | self.V1.append(node_new) 54 | node_near_prim = self.nearest_neighbor(self.V2, node_new) 55 | node_new_prim = self.new_state(node_near_prim, node_new) 56 | 57 | if node_new_prim and not self.utils.is_collision(node_new_prim, node_near_prim): 58 | self.V2.append(node_new_prim) 59 | 60 | while True: 61 | node_new_prim2 = self.new_state(node_new_prim, node_new) 62 | if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim): 63 | self.V2.append(node_new_prim2) 64 | node_new_prim = self.change_node(node_new_prim, node_new_prim2) 65 | else: 66 | break 67 | 68 | if self.is_node_same(node_new_prim, node_new): 69 | break 70 | 71 | if self.is_node_same(node_new_prim, node_new): 72 | return self.extract_path(node_new, node_new_prim) 73 | 74 | if len(self.V2) < len(self.V1): 75 | list_mid = self.V2 76 | self.V2 = self.V1 77 | self.V1 = list_mid 78 | 79 | return None 80 | 81 | @staticmethod 82 | def change_node(node_new_prim, node_new_prim2): 83 | node_new = Node((node_new_prim2.x, node_new_prim2.y)) 84 | node_new.parent = node_new_prim 85 | 86 | return node_new 87 | 88 | @staticmethod 89 | def is_node_same(node_new_prim, node_new): 90 | if node_new_prim.x == node_new.x and \ 91 | node_new_prim.y == node_new.y: 92 | return True 93 | 94 | return False 95 | 96 | def generate_random_node(self, sample_goal, goal_sample_rate): 97 | delta = self.utils.delta 98 | 99 | if np.random.random() > goal_sample_rate: 100 | return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 101 | np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 102 | 103 | return sample_goal 104 | 105 | @staticmethod 106 | def nearest_neighbor(node_list, n): 107 | return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) 108 | for nd in node_list]))] 109 | 110 | def new_state(self, node_start, node_end): 111 | dist, theta = self.get_distance_and_angle(node_start, node_end) 112 | 113 | dist = min(self.step_len, dist) 114 | node_new = Node((node_start.x + dist * math.cos(theta), 115 | node_start.y + dist * math.sin(theta))) 116 | node_new.parent = node_start 117 | 118 | return node_new 119 | 120 | @staticmethod 121 | def extract_path(node_new, node_new_prim): 122 | path1 = [(node_new.x, node_new.y)] 123 | node_now = node_new 124 | 125 | while node_now.parent is not None: 126 | node_now = node_now.parent 127 | path1.append((node_now.x, node_now.y)) 128 | 129 | path2 = [(node_new_prim.x, node_new_prim.y)] 130 | node_now = node_new_prim 131 | 132 | while node_now.parent is not None: 133 | node_now = node_now.parent 134 | path2.append((node_now.x, node_now.y)) 135 | 136 | return list(list(reversed(path1)) + path2) 137 | 138 | @staticmethod 139 | def get_distance_and_angle(node_start, node_end): 140 | dx = node_end.x - node_start.x 141 | dy = node_end.y - node_start.y 142 | return math.hypot(dx, dy), math.atan2(dy, dx) 143 | 144 | 145 | def main(): 146 | x_start = (2, 2) # Starting node 147 | x_goal = (49, 24) # Goal node 148 | 149 | rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.05, 5000) 150 | path = rrt_conn.planning() 151 | 152 | rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT") 153 | 154 | 155 | if __name__ == '__main__': 156 | main() 157 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/rrt_sharp.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_2D/rrt_sharp.py -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/rrt_star.py: -------------------------------------------------------------------------------- 1 | """ 2 | RRT_star 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Sampling_based_Planning/") 13 | 14 | from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue 15 | 16 | 17 | class Node: 18 | def __init__(self, n): 19 | self.x = n[0] 20 | self.y = n[1] 21 | self.parent = None 22 | 23 | 24 | class RrtStar: 25 | def __init__(self, x_start, x_goal, step_len, 26 | goal_sample_rate, search_radius, iter_max): 27 | self.s_start = Node(x_start) 28 | self.s_goal = Node(x_goal) 29 | self.step_len = step_len 30 | self.goal_sample_rate = goal_sample_rate 31 | self.search_radius = search_radius 32 | self.iter_max = iter_max 33 | self.vertex = [self.s_start] 34 | self.path = [] 35 | 36 | self.env = env.Env() 37 | self.plotting = plotting.Plotting(x_start, x_goal) 38 | self.utils = utils.Utils() 39 | 40 | self.x_range = self.env.x_range 41 | self.y_range = self.env.y_range 42 | self.obs_circle = self.env.obs_circle 43 | self.obs_rectangle = self.env.obs_rectangle 44 | self.obs_boundary = self.env.obs_boundary 45 | 46 | def planning(self): 47 | for k in range(self.iter_max): 48 | node_rand = self.generate_random_node(self.goal_sample_rate) 49 | node_near = self.nearest_neighbor(self.vertex, node_rand) 50 | node_new = self.new_state(node_near, node_rand) 51 | 52 | if k % 500 == 0: 53 | print(k) 54 | 55 | if node_new and not self.utils.is_collision(node_near, node_new): 56 | neighbor_index = self.find_near_neighbor(node_new) 57 | self.vertex.append(node_new) 58 | 59 | if neighbor_index: 60 | self.choose_parent(node_new, neighbor_index) 61 | self.rewire(node_new, neighbor_index) 62 | 63 | index = self.search_goal_parent() 64 | self.path = self.extract_path(self.vertex[index]) 65 | 66 | self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max)) 67 | 68 | def new_state(self, node_start, node_goal): 69 | dist, theta = self.get_distance_and_angle(node_start, node_goal) 70 | 71 | dist = min(self.step_len, dist) 72 | node_new = Node((node_start.x + dist * math.cos(theta), 73 | node_start.y + dist * math.sin(theta))) 74 | 75 | node_new.parent = node_start 76 | 77 | return node_new 78 | 79 | def choose_parent(self, node_new, neighbor_index): 80 | cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index] 81 | 82 | cost_min_index = neighbor_index[int(np.argmin(cost))] 83 | node_new.parent = self.vertex[cost_min_index] 84 | 85 | def rewire(self, node_new, neighbor_index): 86 | for i in neighbor_index: 87 | node_neighbor = self.vertex[i] 88 | 89 | if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor): 90 | node_neighbor.parent = node_new 91 | 92 | def search_goal_parent(self): 93 | dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex] 94 | node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len] 95 | 96 | if len(node_index) > 0: 97 | cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index 98 | if not self.utils.is_collision(self.vertex[i], self.s_goal)] 99 | return node_index[int(np.argmin(cost_list))] 100 | 101 | return len(self.vertex) - 1 102 | 103 | def get_new_cost(self, node_start, node_end): 104 | dist, _ = self.get_distance_and_angle(node_start, node_end) 105 | 106 | return self.cost(node_start) + dist 107 | 108 | def generate_random_node(self, goal_sample_rate): 109 | delta = self.utils.delta 110 | 111 | if np.random.random() > goal_sample_rate: 112 | return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), 113 | np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) 114 | 115 | return self.s_goal 116 | 117 | def find_near_neighbor(self, node_new): 118 | n = len(self.vertex) + 1 119 | r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) 120 | 121 | dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex] 122 | dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and 123 | not self.utils.is_collision(node_new, self.vertex[ind])] 124 | 125 | return dist_table_index 126 | 127 | @staticmethod 128 | def nearest_neighbor(node_list, n): 129 | return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) 130 | for nd in node_list]))] 131 | 132 | @staticmethod 133 | def cost(node_p): 134 | node = node_p 135 | cost = 0.0 136 | 137 | while node.parent: 138 | cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) 139 | node = node.parent 140 | 141 | return cost 142 | 143 | def update_cost(self, parent_node): 144 | OPEN = queue.QueueFIFO() 145 | OPEN.put(parent_node) 146 | 147 | while not OPEN.empty(): 148 | node = OPEN.get() 149 | 150 | if len(node.child) == 0: 151 | continue 152 | 153 | for node_c in node.child: 154 | node_c.Cost = self.get_new_cost(node, node_c) 155 | OPEN.put(node_c) 156 | 157 | def extract_path(self, node_end): 158 | path = [[self.s_goal.x, self.s_goal.y]] 159 | node = node_end 160 | 161 | while node.parent is not None: 162 | path.append([node.x, node.y]) 163 | node = node.parent 164 | path.append([node.x, node.y]) 165 | 166 | return path 167 | 168 | @staticmethod 169 | def get_distance_and_angle(node_start, node_end): 170 | dx = node_end.x - node_start.x 171 | dy = node_end.y - node_start.y 172 | return math.hypot(dx, dy), math.atan2(dy, dx) 173 | 174 | 175 | def main(): 176 | x_start = (18, 8) # Starting node 177 | x_goal = (37, 18) # Goal node 178 | 179 | rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000) 180 | rrt_star.planning() 181 | 182 | 183 | if __name__ == '__main__': 184 | main() 185 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_2D/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | utils for collision check 3 | @author: huiming zhou 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | import os 9 | import sys 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Sampling_based_Planning/") 13 | 14 | from Sampling_based_Planning.rrt_2D import env 15 | from Sampling_based_Planning.rrt_2D.rrt import Node 16 | 17 | 18 | class Utils: 19 | def __init__(self): 20 | self.env = env.Env() 21 | 22 | self.delta = 0.5 23 | self.obs_circle = self.env.obs_circle 24 | self.obs_rectangle = self.env.obs_rectangle 25 | self.obs_boundary = self.env.obs_boundary 26 | 27 | def update_obs(self, obs_cir, obs_bound, obs_rec): 28 | self.obs_circle = obs_cir 29 | self.obs_boundary = obs_bound 30 | self.obs_rectangle = obs_rec 31 | 32 | def get_obs_vertex(self): 33 | delta = self.delta 34 | obs_list = [] 35 | 36 | for (ox, oy, w, h) in self.obs_rectangle: 37 | vertex_list = [[ox - delta, oy - delta], 38 | [ox + w + delta, oy - delta], 39 | [ox + w + delta, oy + h + delta], 40 | [ox - delta, oy + h + delta]] 41 | obs_list.append(vertex_list) 42 | 43 | return obs_list 44 | 45 | def is_intersect_rec(self, start, end, o, d, a, b): 46 | v1 = [o[0] - a[0], o[1] - a[1]] 47 | v2 = [b[0] - a[0], b[1] - a[1]] 48 | v3 = [-d[1], d[0]] 49 | 50 | div = np.dot(v2, v3) 51 | 52 | if div == 0: 53 | return False 54 | 55 | t1 = np.linalg.norm(np.cross(v2, v1)) / div 56 | t2 = np.dot(v1, v3) / div 57 | 58 | if t1 >= 0 and 0 <= t2 <= 1: 59 | shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1])) 60 | dist_obs = self.get_dist(start, shot) 61 | dist_seg = self.get_dist(start, end) 62 | if dist_obs <= dist_seg: 63 | return True 64 | 65 | return False 66 | 67 | def is_intersect_circle(self, o, d, a, r): 68 | d2 = np.dot(d, d) 69 | delta = self.delta 70 | 71 | if d2 == 0: 72 | return False 73 | 74 | t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2 75 | 76 | if 0 <= t <= 1: 77 | shot = Node((o[0] + t * d[0], o[1] + t * d[1])) 78 | if self.get_dist(shot, Node(a)) <= r + delta: 79 | return True 80 | 81 | return False 82 | 83 | def is_collision(self, start, end): 84 | if self.is_inside_obs(start) or self.is_inside_obs(end): 85 | return True 86 | 87 | o, d = self.get_ray(start, end) 88 | obs_vertex = self.get_obs_vertex() 89 | 90 | for (v1, v2, v3, v4) in obs_vertex: 91 | if self.is_intersect_rec(start, end, o, d, v1, v2): 92 | return True 93 | if self.is_intersect_rec(start, end, o, d, v2, v3): 94 | return True 95 | if self.is_intersect_rec(start, end, o, d, v3, v4): 96 | return True 97 | if self.is_intersect_rec(start, end, o, d, v4, v1): 98 | return True 99 | 100 | for (x, y, r) in self.obs_circle: 101 | if self.is_intersect_circle(o, d, [x, y], r): 102 | return True 103 | 104 | return False 105 | 106 | def is_inside_obs(self, node): 107 | delta = self.delta 108 | 109 | for (x, y, r) in self.obs_circle: 110 | if math.hypot(node.x - x, node.y - y) <= r + delta: 111 | return True 112 | 113 | for (x, y, w, h) in self.obs_rectangle: 114 | if 0 <= node.x - (x - delta) <= w + 2 * delta \ 115 | and 0 <= node.y - (y - delta) <= h + 2 * delta: 116 | return True 117 | 118 | for (x, y, w, h) in self.obs_boundary: 119 | if 0 <= node.x - (x - delta) <= w + 2 * delta \ 120 | and 0 <= node.y - (y - delta) <= h + 2 * delta: 121 | return True 122 | 123 | return False 124 | 125 | @staticmethod 126 | def get_ray(start, end): 127 | orig = [start.x, start.y] 128 | direc = [end.x - start.x, end.y - start.y] 129 | return orig, direc 130 | 131 | @staticmethod 132 | def get_dist(start, end): 133 | return math.hypot(end.x - start.x, end.y - start.y) 134 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/ABIT_star3D.py: -------------------------------------------------------------------------------- 1 | # This is Advanced Batched Informed Tree star 3D algorithm 2 | # implementation 3 | """ 4 | This is ABIT* code for 3D 5 | @author: yue qi 6 | source: M.P.Strub, J.D.Gammel. "Advanced BIT* (ABIT*): 7 | Sampling-Based Planning with Advanced Graph-Search Techniques" 8 | """ 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import time 13 | import copy 14 | 15 | 16 | import os 17 | import sys 18 | 19 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 20 | from rrt_3D.env3D import env 21 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide 22 | from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent 23 | from rrt_3D.queue import MinheapPQ 24 | 25 | class ABIT_star: 26 | 27 | def __init__(self): 28 | self.env = env() 29 | self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal) 30 | self.maxiter = 1000 31 | self.done = False 32 | self.n = 1000# used in radius calc r(q) 33 | self.lam = 10 # used in radius calc r(q) 34 | 35 | def run(self): 36 | V, E = {self.xstart}, set() 37 | T = (V,E) 38 | Xunconnected = {self.xgoal} 39 | q = len(V) + len(Xunconnected) 40 | eps_infl, eps_trunc = np.inf, np.inf 41 | Vclosed, Vinconsistent = set(), set() 42 | Q = self.expand(self.xstart, T, Xunconnected, np.inf) 43 | 44 | ind = 0 45 | while True: 46 | if self.is_search_marked_finished(): 47 | if self.update_approximation(eps_infl, eps_trunc): 48 | T, Xunconnected = self.prune(T, Xunconnected, self.xgoal) 49 | Xunconnected.update(self.sample(m, self.xgoal)) 50 | q = len(V) + len(Xunconnected) 51 | Q = self.expand({self.xstart}, T, Xunconnected, self.r(q)) 52 | else: 53 | Q.update(self.expand(Vinconsistent, T, Xunconnected, self.r(q))) 54 | eps_infl = self.update_inflation_factor() 55 | eps_trunc = self.update_truncation_factor() 56 | Vclosed = set() 57 | Vinconsistent = set() 58 | self.mark_search_unfinished() 59 | else: 60 | state_tuple = list(Q) 61 | (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] )] 62 | Q = Q.difference({(xp, xc)}) 63 | if (xp, xc) in E: 64 | if xc in Vclosed: 65 | Vinconsistent.add(xc) 66 | else: 67 | Q.update(self.expand({xc}, T, Xunconnected, self.r(q))) 68 | Vclosed.add(xc) 69 | elif eps_trunc * (self.g_T(xp) + self.c_hat(xi, xj) + self.h_hat(xc)) <= self.g_T(self.xgoal): 70 | if self.g_T(xp) + self.c_hat(xp, xc) < self.g_T(xc): 71 | if self.g_T(xp) + self.c(xp, xc) + self.h_hat(xc) < self.g_T(self.xgoal): 72 | if self.g_T(xp) + self.c(xp, xc) < self.g_T(xc): 73 | if xc in V: 74 | E = E.difference({(xprev, xc)}) 75 | else: 76 | Xunconnected.difference_update({xc}) 77 | V.add(xc) 78 | E.add((xp, xc)) 79 | if xc in Vclosed: 80 | Vinconsistent.add(xc) 81 | else: 82 | Q.update(self.expand({xc}, T, Xunconnected, self.r(q))) 83 | Vclosed.add(xc) 84 | else: 85 | self.mark_search_finished() 86 | ind += 1 87 | # until stop 88 | if ind > self.maxiter: 89 | break 90 | 91 | def sample(self, m, xgoal): 92 | pass 93 | 94 | def expand(self, set_xi, T, Xunconnected, r): 95 | V, E = T 96 | Eout = set() 97 | for xp in set_xi: 98 | Eout.update({(x1, x2) for (x1, x2) in E if x1 == xp}) 99 | for xc in {x for x in Xunconnected.union(V) if getDist(xp, x) <= r}: 100 | if self.g_hat(xp) + self.c_hat(xp, xc) + self.h_hat(xc) <= self.g_T(self.xgoal): 101 | if self.g_hat(xp) + self.c_hat(xp, xc) <= self.g_hat(xc): 102 | Eout.add((xp,xc)) 103 | return Eout 104 | 105 | def prune(self, T, Xunconnected, xgoal): 106 | V, E = T 107 | Xunconnected.difference_update({x for x in Xunconnected if self.f_hat(x) >= self.g_T(xgoal)}) 108 | V.difference_update({x for x in V if self.f_hat(x) > self.g_T(xgoal)}) 109 | 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)}) 110 | Xunconnected.update({xc for (xp, xc) in E if (xp not in V) and (xc in V)}) 111 | V.difference_update({xc for (xp, xc) in E if (xp not in V) and (xc in V)}) 112 | T = (V,E) 113 | return T, Xunconnected 114 | 115 | def g_hat(self, x): 116 | pass 117 | 118 | def h_hat(self, x): 119 | pass 120 | 121 | def c_hat(self, x1, x2): 122 | pass 123 | 124 | def f_hat(self, x): 125 | pass 126 | 127 | def g_T(self, x): 128 | pass 129 | 130 | def r(self, q): 131 | return self.eta * (2 * (1 + 1/self.n) * (self.Lambda(self.Xf_hat) / self.Zeta) * (np.log(q) / q)) ** (1 / self.n) 132 | 133 | def Lambda(self, inputset): 134 | pass 135 | 136 | def Zeta(self): 137 | pass 138 | 139 | def is_search_marked_finished(self): 140 | return self.done 141 | 142 | def mark_search_unfinished(self): 143 | self.done = False 144 | return self.done 145 | 146 | def mark_search_finished(self): 147 | self.done = True 148 | return self.done 149 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/FMT_star3D.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is fast marching tree* code for 3D 3 | @author: yue qi 4 | source: Janson, Lucas, et al. "Fast marching tree: A fast marching sampling-based method 5 | for optimal motion planning in many dimensions." 6 | The International journal of robotics research 34.7 (2015): 883-921. 7 | """ 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | import time 11 | import copy 12 | 13 | 14 | import os 15 | import sys 16 | 17 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 18 | from rrt_3D.env3D import env 19 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide 20 | from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent 21 | from rrt_3D.queue import MinheapPQ 22 | 23 | class FMT_star: 24 | 25 | def __init__(self, radius = 1, n = 1000): 26 | self.env = env() 27 | # init start and goal 28 | # note that the xgoal could be a region since this algorithm is a multiquery method 29 | self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal) 30 | self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # used for sample free 31 | self.n = n # number of samples 32 | self.radius = radius # radius of the ball 33 | # self.radius = 40 * np.sqrt((np.log(self.n) / self.n)) 34 | # sets 35 | self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets() 36 | # make space for save 37 | self.neighbors = {} 38 | # additional 39 | self.done = True 40 | self.Path = [] 41 | self.Parent = {} 42 | 43 | def generateSampleSet(self, n): 44 | V = set() 45 | for i in range(n): 46 | V.add(tuple(sampleFree(self, bias = 0.0))) 47 | return V 48 | 49 | def initNodeSets(self): 50 | # open set 51 | Vopen = {self.xinit} # open set 52 | # closed set 53 | closed = set() 54 | # V, Vunvisited set 55 | V = self.generateSampleSet(self.n - 2) # set of all nodes 56 | Vunvisited = copy.deepcopy(V) # unvisited set 57 | Vunvisited.add(self.xgoal) 58 | V.add(self.xinit) 59 | V.add(self.xgoal) 60 | # initialize all cost to come at inf 61 | c = {node : np.inf for node in V} 62 | c[self.xinit] = 0 63 | # use a min heap to speed up 64 | Vopen_queue = MinheapPQ() 65 | Vopen_queue.put(self.xinit, c[self.xinit]) # priority organized as the cost to come 66 | return Vopen, Vopen_queue, closed, V, Vunvisited, c 67 | 68 | def Near(self, nodeset, node, rn): 69 | if node in self.neighbors: 70 | return self.neighbors[node] 71 | validnodes = {i for i in nodeset if getDist(i, node) < rn} 72 | return validnodes 73 | 74 | def Save(self, V_associated, node): 75 | self.neighbors[node] = V_associated 76 | 77 | def path(self, z, initT): 78 | path = [] 79 | s = self.xgoal 80 | i = 0 81 | while s != self.xinit: 82 | path.append((s, self.Parent[s])) 83 | s = self.Parent[s] 84 | if i > self.n: 85 | break 86 | i += 1 87 | return path 88 | 89 | def Cost(self, x, y): 90 | # collide, dist = isCollide(self, x, y) 91 | # if collide: 92 | # return np.inf 93 | # return dist 94 | return getDist(x, y) 95 | 96 | def FMTrun(self): 97 | z = self.xinit 98 | rn = self.radius 99 | Nz = self.Near(self.Vunvisited, z, rn) 100 | E = set() 101 | self.Save(Nz, z) 102 | ind = 0 103 | while z != self.xgoal: 104 | Vopen_new = set() 105 | #Nz = self.Near(self.Vunvisited, z, rn) 106 | #self.Save(Nz, z) 107 | #Xnear = Nz.intersection(self.Vunvisited) 108 | Xnear = self.Near(self.Vunvisited, z ,rn) 109 | self.Save(Xnear, z) 110 | for x in Xnear: 111 | #Nx = self.Near(self.V.difference({x}), x, rn) 112 | #self.Save(Nx, x) 113 | #Ynear = list(Nx.intersection(self.Vopen)) 114 | Ynear = list(self.Near(self.Vopen, x, rn)) 115 | # self.Save(set(Ynear), x) 116 | ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation 117 | collide, _ = isCollide(self, ymin, x) 118 | if not collide: 119 | E.add((ymin, x)) # straight line joining ymin and x is collision free 120 | Vopen_new.add(x) 121 | self.Parent[x] = z 122 | self.Vunvisited = self.Vunvisited.difference({x}) 123 | self.c[x] = self.c[ymin] + self.Cost(ymin, x) # estimated cost-to-arrive from xinit in tree T = (VopenUVclosed, E) 124 | # update open set 125 | self.Vopen = self.Vopen.union(Vopen_new).difference({z}) 126 | self.Vclosed.add(z) 127 | if len(self.Vopen) == 0: 128 | print('Failure') 129 | return 130 | ind += 1 131 | print(str(ind) + ' node expanded') 132 | self.visualization(ind, E) 133 | # update current node 134 | Vopenlist = list(self.Vopen) 135 | z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])] 136 | # creating the tree 137 | T = (self.Vopen.union(self.Vclosed), E) 138 | self.done = True 139 | self.Path = self.path(z, T) 140 | self.visualization(ind, E) 141 | plt.show() 142 | # return self.path(z, T) 143 | 144 | def visualization(self, ind, E): 145 | if ind % 100 == 0 or self.done: 146 | #----------- list structure 147 | # V = np.array(list(initparams.V)) 148 | # E = initparams.E 149 | #----------- end 150 | # edges = initparams.E 151 | Path = np.array(self.Path) 152 | start = self.env.start 153 | goal = self.env.goal 154 | # edges = E.get_edge() 155 | #----------- list structure 156 | edges = np.array(list(E)) 157 | #----------- end 158 | # generate axis objects 159 | ax = plt.subplot(111, projection='3d') 160 | 161 | # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) 162 | # ax.view_init(elev=0., azim=90.) 163 | ax.view_init(elev=65., azim=60.) 164 | ax.dist = 15 165 | # ax.view_init(elev=-8., azim=180) 166 | ax.clear() 167 | # drawing objects 168 | draw_Spheres(ax, self.env.balls) 169 | draw_block_list(ax, self.env.blocks) 170 | if self.env.OBB is not None: 171 | draw_obb(ax, self.env.OBB) 172 | draw_block_list(ax, np.array([self.env.boundary]), alpha=0) 173 | draw_line(ax, edges, visibility=0.75, color='g') 174 | draw_line(ax, Path, color='r') 175 | # if len(V) > 0: 176 | # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) 177 | ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') 178 | ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') 179 | # adjust the aspect ratio 180 | set_axes_equal(ax) 181 | make_transparent(ax) 182 | #plt.xlabel('x') 183 | #plt.ylabel('y') 184 | ax.set_axis_off() 185 | plt.pause(0.0001) 186 | 187 | if __name__ == '__main__': 188 | A = FMT_star(radius = 1, n = 3000) 189 | A.FMTrun() 190 | 191 | 192 | 193 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/env3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional configuration space for rrt 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | # from utils3D import OBB2AABB 9 | 10 | def R_matrix(z_angle,y_angle,x_angle): 11 | # s angle: row; y angle: pitch; z angle: yaw 12 | # generate rotation matrix in SO3 13 | # RzRyRx = R, ZYX intrinsic rotation 14 | # also (r1,r2,r3) in R3*3 in {W} frame 15 | # used in obb.O 16 | # [[R p] 17 | # [0T 1]] gives transformation from body to world 18 | 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]])@ \ 19 | 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)]])@ \ 20 | 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)]]) 21 | 22 | def getblocks(): 23 | # AABBs 24 | block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00], 25 | [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00], 26 | [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00], 27 | [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00], 28 | [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]] 29 | Obstacles = [] 30 | for i in block: 31 | i = np.array(i) 32 | Obstacles.append([j for j in i]) 33 | return np.array(Obstacles) 34 | 35 | def getballs(): 36 | spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]] 37 | Obstacles = [] 38 | for i in spheres: 39 | Obstacles.append([j for j in i]) 40 | return np.array(Obstacles) 41 | 42 | def getAABB(blocks): 43 | # used for Pyrr package for detecting collision 44 | AABB = [] 45 | for i in blocks: 46 | AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger 47 | return AABB 48 | 49 | def getAABB2(blocks): 50 | # used in lineAABB 51 | AABB = [] 52 | for i in blocks: 53 | AABB.append(aabb(i)) 54 | return AABB 55 | 56 | def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): 57 | return block 58 | 59 | class aabb(object): 60 | # make AABB out of blocks, 61 | # P: center point 62 | # E: extents 63 | # O: Rotation matrix in SO(3), in {w} 64 | def __init__(self,AABB): 65 | self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point 66 | self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents 67 | self.O = [[1,0,0],[0,1,0],[0,0,1]] 68 | 69 | class obb(object): 70 | # P: center point 71 | # E: extents 72 | # O: Rotation matrix in SO(3), in {w} 73 | def __init__(self, P, E, O): 74 | self.P = P 75 | self.E = E 76 | self.O = O 77 | self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) 78 | 79 | class env(): 80 | def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1): 81 | # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): 82 | self.resolution = resolution 83 | self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 84 | self.blocks = getblocks() 85 | self.AABB = getAABB2(self.blocks) 86 | self.AABB_pyrr = getAABB(self.blocks) 87 | self.balls = getballs() 88 | self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)), 89 | obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))]) 90 | self.start = np.array([2.0, 2.0, 2.0]) 91 | self.goal = np.array([6.0, 16.0, 0.0]) 92 | self.t = 0 # time 93 | 94 | def New_block(self): 95 | newblock = add_block() 96 | self.blocks = np.vstack([self.blocks,newblock]) 97 | self.AABB = getAABB2(self.blocks) 98 | self.AABB_pyrr = getAABB(self.blocks) 99 | 100 | def move_start(self, x): 101 | self.start = x 102 | 103 | def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'): 104 | # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 105 | # R is an orthorgonal transform in R3*3, is the rotation matrix 106 | # (s',t') = (s + tv, t) is uniform transformation 107 | # (s',t') = (s + a, t + s) is a translation 108 | if mode == 'translation': 109 | ori = np.array(self.blocks[block_to_move]) 110 | self.blocks[block_to_move] = \ 111 | np.array([ori[0] + a[0],\ 112 | ori[1] + a[1],\ 113 | ori[2] + a[2],\ 114 | ori[3] + a[0],\ 115 | ori[4] + a[1],\ 116 | ori[5] + a[2]]) 117 | 118 | self.AABB[block_to_move].P = \ 119 | [self.AABB[block_to_move].P[0] + a[0], \ 120 | self.AABB[block_to_move].P[1] + a[1], \ 121 | self.AABB[block_to_move].P[2] + a[2]] 122 | self.t += s 123 | # return a range of block that the block might moved 124 | a = self.blocks[block_to_move] 125 | return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ 126 | a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ 127 | np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ 128 | ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) 129 | # return a,ori 130 | # (s',t') = (Rx, t) 131 | def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]): 132 | # theta stands for rotational angles around three principle axis in world frame 133 | # translation stands for translation in the world frame 134 | ori = [self.OBB[obb_to_move]] 135 | # move obb position 136 | self.OBB[obb_to_move].P = \ 137 | [self.OBB[obb_to_move].P[0] + translation[0], 138 | self.OBB[obb_to_move].P[1] + translation[1], 139 | self.OBB[obb_to_move].P[2] + translation[2]] 140 | # Calculate orientation 141 | self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) 142 | # generating transformation matrix 143 | self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\ 144 | -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]]) 145 | return self.OBB[obb_to_move], ori[0] 146 | 147 | if __name__ == '__main__': 148 | newenv = env() 149 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/extend_rrt3D.py: -------------------------------------------------------------------------------- 1 | # Real-Time Randomized Path Planning for Robot Navigation∗ 2 | """ 3 | This is rrt extend code for 3D 4 | @author: yue qi 5 | """ 6 | import numpy as np 7 | from numpy.matlib import repmat 8 | from collections import defaultdict 9 | import time 10 | import matplotlib.pyplot as plt 11 | 12 | import os 13 | import sys 14 | 15 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 16 | from rrt_3D.env3D import env 17 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path 18 | 19 | # here attempt to use a KD tree for the data structure implementation 20 | import scipy.spatial.kdtree as KDtree 21 | 22 | 23 | class extend_rrt(object): 24 | def __init__(self): 25 | self.env = env() 26 | self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) 27 | self.current = tuple(self.env.start) 28 | self.stepsize = 0.5 29 | self.maxiter = 10000 30 | self.GoalProb = 0.05 # probability biased to the goal 31 | self.WayPointProb = 0.05 # probability falls back on to the way points 32 | 33 | self.done = False 34 | self.V = [] # vertices 35 | self.Parent = {} 36 | self.Path = [] 37 | self.ind = 0 38 | self.i = 0 39 | 40 | #--------- basic rrt algorithm---------- 41 | def RRTplan(self, env, initial, goal): 42 | threshold = self.stepsize 43 | nearest = initial # state structure 44 | self.V.append(initial) 45 | rrt_tree = initial # TODO KDtree structure 46 | while self.ind <= self.maxiter: 47 | target = self.ChooseTarget(goal) 48 | nearest = self.Nearest(rrt_tree, target) 49 | extended, collide = self.Extend(env, nearest, target) 50 | if not collide: 51 | self.AddNode(rrt_tree, nearest, extended) 52 | if getDist(nearest, goal) <= threshold: 53 | self.AddNode(rrt_tree, nearest, self.xt) 54 | break 55 | self.i += 1 56 | self.ind += 1 57 | visualization(self) 58 | 59 | # return rrt_tree 60 | self.done = True 61 | self.Path, _ = path(self) 62 | visualization(self) 63 | plt.show() 64 | 65 | 66 | def Nearest(self, tree, target): 67 | # TODO use kdTree to speed up search 68 | return nearest(self, target, isset=True) 69 | 70 | def Extend(self, env, nearest, target): 71 | extended, dist = steer(self, nearest, target, DIST = True) 72 | collide, _ = isCollide(self, nearest, target, dist) 73 | return extended, collide 74 | 75 | def AddNode(self, tree, nearest, extended): 76 | # TODO use a kdtree 77 | self.V.append(extended) 78 | self.Parent[extended] = nearest 79 | 80 | def RandomState(self): 81 | # generate a random, obstacle free state 82 | xrand = sampleFree(self, bias=0) 83 | return xrand 84 | 85 | #--------- insight to the rrt_extend 86 | def ChooseTarget(self, state): 87 | # return the goal, or randomly choose a state in the waypoints based on probs 88 | p = np.random.uniform() 89 | if len(self.V) == 1: 90 | i = 0 91 | else: 92 | i = np.random.randint(0, high = len(self.V) - 1) 93 | if 0 < p < self.GoalProb: 94 | return self.xt 95 | elif self.GoalProb < p < self.GoalProb + self.WayPointProb: 96 | return self.V[i] 97 | elif self.GoalProb + self.WayPointProb < p < 1: 98 | return tuple(self.RandomState()) 99 | 100 | if __name__ == '__main__': 101 | t = extend_rrt() 102 | _ = t.RRTplan(t.env, t.x0, t.xt) -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/plot_util3D.py: -------------------------------------------------------------------------------- 1 | # plotting 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d import Axes3D 4 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 5 | import mpl_toolkits.mplot3d as plt3d 6 | from mpl_toolkits.mplot3d import proj3d 7 | import numpy as np 8 | 9 | 10 | def CreateSphere(center, r): 11 | u = np.linspace(0, 2 * np.pi, 30) 12 | v = np.linspace(0, np.pi, 30) 13 | x = np.outer(np.cos(u), np.sin(v)) 14 | y = np.outer(np.sin(u), np.sin(v)) 15 | z = np.outer(np.ones(np.size(u)), np.cos(v)) 16 | x, y, z = r * x + center[0], r * y + center[1], r * z + center[2] 17 | return (x, y, z) 18 | 19 | 20 | def draw_Spheres(ax, balls): 21 | for i in balls: 22 | (xs, ys, zs) = CreateSphere(i[0:3], i[-1]) 23 | ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b") 24 | 25 | 26 | def draw_block_list(ax, blocks, color=None, alpha=0.15): 27 | ''' 28 | drawing the blocks on the graph 29 | ''' 30 | 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]], 31 | dtype='float') 32 | 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]]) 33 | n = blocks.shape[0] 34 | d = blocks[:, 3:6] - blocks[:, :3] 35 | vl = np.zeros((8 * n, 3)) 36 | fl = np.zeros((6 * n, 4), dtype='int64') 37 | for k in range(n): 38 | vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] 39 | fl[k * 6:(k + 1) * 6, :] = f + k * 8 40 | if type(ax) is Poly3DCollection: 41 | ax.set_verts(vl[fl]) 42 | else: 43 | h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k')) 44 | return h 45 | 46 | 47 | def obb_verts(obb): 48 | # 0.017004013061523438 for 1000 iters 49 | ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \ 50 | [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]]) 51 | # P + (ori * E) 52 | ori_body = np.multiply(ori_body, obb.E) 53 | # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) 54 | verts = (obb.O @ ori_body.T).T + obb.P 55 | return verts 56 | 57 | 58 | def draw_obb(ax, OBB, color=None, alpha=0.15): 59 | 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]]) 60 | n = OBB.shape[0] 61 | vl = np.zeros((8 * n, 3)) 62 | fl = np.zeros((6 * n, 4), dtype='int64') 63 | for k in range(n): 64 | vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) 65 | fl[k * 6:(k + 1) * 6, :] = f + k * 8 66 | if type(ax) is Poly3DCollection: 67 | ax.set_verts(vl[fl]) 68 | else: 69 | h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k')) 70 | return h 71 | 72 | 73 | def draw_line(ax, SET, visibility=1, color=None): 74 | if SET != []: 75 | for i in SET: 76 | xs = i[0][0], i[1][0] 77 | ys = i[0][1], i[1][1] 78 | zs = i[0][2], i[1][2] 79 | line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) 80 | ax.add_line(line) 81 | 82 | 83 | def visualization(initparams): 84 | if initparams.ind % 100 == 0 or initparams.done: 85 | #----------- list structure 86 | # V = np.array(list(initparams.V)) 87 | # E = initparams.E 88 | #----------- end 89 | # edges = initparams.E 90 | Path = np.array(initparams.Path) 91 | start = initparams.env.start 92 | goal = initparams.env.goal 93 | # edges = E.get_edge() 94 | #----------- list structure 95 | edges = [] 96 | for i in initparams.Parent: 97 | edges.append([i,initparams.Parent[i]]) 98 | #----------- end 99 | # generate axis objects 100 | ax = plt.subplot(111, projection='3d') 101 | 102 | # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) 103 | # ax.view_init(elev=0., azim=90.) 104 | ax.view_init(elev=65., azim=60.) 105 | # ax.view_init(elev=-8., azim=180) 106 | ax.clear() 107 | # drawing objects 108 | draw_Spheres(ax, initparams.env.balls) 109 | draw_block_list(ax, initparams.env.blocks) 110 | if initparams.env.OBB is not None: 111 | draw_obb(ax, initparams.env.OBB) 112 | draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0) 113 | draw_line(ax, edges, visibility=0.75, color='g') 114 | draw_line(ax, Path, color='r') 115 | # if len(V) > 0: 116 | # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) 117 | ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') 118 | ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') 119 | # adjust the aspect ratio 120 | ax.dist = 15 121 | set_axes_equal(ax) 122 | make_transparent(ax) 123 | #plt.xlabel('s') 124 | #plt.ylabel('y') 125 | ax.set_axis_off() 126 | plt.pause(0.0001) 127 | 128 | def set_axes_equal(ax): 129 | '''Make axes of 3D plot have equal scale so that spheres appear as spheres, 130 | cubes as cubes, etc.. This is one possible solution to Matplotlib's 131 | ax.set_aspect('equal') and ax.axis('equal') not working for 3D. 132 | https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to 133 | Input 134 | ax: a matplotlib axis, e.g., as output from plt.gca(). 135 | ''' 136 | 137 | x_limits = ax.get_xlim3d() 138 | y_limits = ax.get_ylim3d() 139 | z_limits = ax.get_zlim3d() 140 | 141 | x_range = abs(x_limits[1] - x_limits[0]) 142 | x_middle = np.mean(x_limits) 143 | y_range = abs(y_limits[1] - y_limits[0]) 144 | y_middle = np.mean(y_limits) 145 | z_range = abs(z_limits[1] - z_limits[0]) 146 | z_middle = np.mean(z_limits) 147 | 148 | # The plot bounding box is a sphere in the sense of the infinity 149 | # norm, hence I call half the max range the plot radius. 150 | plot_radius = 0.5*max([x_range, y_range, z_range]) 151 | 152 | ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) 153 | ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) 154 | ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) 155 | 156 | def make_transparent(ax): 157 | # make the panes transparent 158 | ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 159 | ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 160 | ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 161 | # make the grid lines transparent 162 | ax.xaxis._axinfo["grid"]['color'] = (1,1,1,0) 163 | ax.yaxis._axinfo["grid"]['color'] = (1,1,1,0) 164 | ax.zaxis._axinfo["grid"]['color'] = (1,1,1,0) 165 | 166 | if __name__ == '__main__': 167 | pass 168 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/queue.py: -------------------------------------------------------------------------------- 1 | # min heap used in the FMT* 2 | 3 | import collections 4 | import heapq 5 | import itertools 6 | 7 | class MinheapPQ: 8 | """ 9 | A priority queue based on min heap, which takes O(logn) on element removal 10 | https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes 11 | """ 12 | def __init__(self): 13 | self.pq = [] # lis of the entries arranged in a heap 14 | self.nodes = set() 15 | self.entry_finder = {} # mapping of the item entries 16 | self.counter = itertools.count() # unique sequence count 17 | self.REMOVED = '' 18 | 19 | def put(self, item, priority): 20 | '''add a new task or update the priority of an existing item''' 21 | if item in self.entry_finder: 22 | self.check_remove(item) 23 | count = next(self.counter) 24 | entry = [priority, count, item] 25 | self.entry_finder[item] = entry 26 | heapq.heappush(self.pq, entry) 27 | self.nodes.add(item) 28 | 29 | def put_set(self, dictin): 30 | '''add a new dict into the priority queue''' 31 | for item, priority in enumerate(dictin): 32 | self.put(item, priority) 33 | 34 | def check_remove(self, item): 35 | if item not in self.entry_finder: 36 | return 37 | entry = self.entry_finder.pop(item) 38 | entry[-1] = self.REMOVED 39 | self.nodes.remove(item) 40 | 41 | def check_remove_set(self, set_input): 42 | if len(set_input) == 0: 43 | return 44 | for item in set_input: 45 | if item not in self.entry_finder: 46 | continue 47 | entry = self.entry_finder.pop(item) 48 | entry[-1] = self.REMOVED 49 | self.nodes.remove(item) 50 | 51 | def priority_filtering(self, threshold, mode): 52 | # mode: bigger: check and remove those key vals bigger than threshold 53 | if mode == 'lowpass': 54 | for entry in self.enumerate(): 55 | item = entry[2] 56 | if entry[0] >= threshold: # priority 57 | _ = self.entry_finder.pop(item) 58 | entry[-1] = self.REMOVED 59 | self.nodes.remove(item) 60 | # mode: smaller: check and remove those key vals smaller than threshold 61 | elif mode == 'highpass': 62 | for entry in self.enumerate(): 63 | item = entry[2] 64 | if entry[0] <= threshold: # priority 65 | _ = self.entry_finder.pop(item) 66 | entry[-1] = self.REMOVED 67 | self.nodes.remove(item) 68 | 69 | def get(self): 70 | """Remove and return the lowest priority task. Raise KeyError if empty.""" 71 | while self.pq: 72 | priority, count, item = heapq.heappop(self.pq) 73 | if item is not self.REMOVED: 74 | del self.entry_finder[item] 75 | self.nodes.remove(item) 76 | return item 77 | raise KeyError('pop from an empty priority queue') 78 | 79 | def top_key(self): 80 | return self.pq[0][0] 81 | 82 | def enumerate(self): 83 | return self.pq 84 | 85 | def allnodes(self): 86 | return self.nodes 87 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/rrt3D.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is rrt star code for 3D 3 | @author: yue qi 4 | """ 5 | import numpy as np 6 | from numpy.matlib import repmat 7 | from collections import defaultdict 8 | import time 9 | import matplotlib.pyplot as plt 10 | 11 | import os 12 | import sys 13 | 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 15 | 16 | from rrt_3D.env3D import env 17 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path 18 | 19 | 20 | class rrt(): 21 | def __init__(self): 22 | self.env = env() 23 | self.Parent = {} 24 | self.V = [] 25 | # self.E = edgeset() 26 | self.i = 0 27 | self.maxiter = 10000 28 | self.stepsize = 0.5 29 | self.Path = [] 30 | self.done = False 31 | self.x0 = tuple(self.env.start) 32 | self.xt = tuple(self.env.goal) 33 | 34 | 35 | self.ind = 0 36 | # self.fig = plt.figure(figsize=(10, 8)) 37 | 38 | def wireup(self, x, y): 39 | # self.E.add_edge([s, y]) # add edge 40 | self.Parent[x] = y 41 | 42 | def run(self): 43 | self.V.append(self.x0) 44 | while self.ind < self.maxiter: 45 | xrand = sampleFree(self) 46 | xnearest = nearest(self, xrand) 47 | xnew, dist = steer(self, xnearest, xrand) 48 | collide, _ = isCollide(self, xnearest, xnew, dist=dist) 49 | if not collide: 50 | self.V.append(xnew) # add point 51 | self.wireup(xnew, xnearest) 52 | 53 | if getDist(xnew, self.xt) <= self.stepsize: 54 | self.wireup(self.xt, xnew) 55 | self.Path, D = path(self) 56 | print('Total distance = ' + str(D)) 57 | break 58 | visualization(self) 59 | self.i += 1 60 | self.ind += 1 61 | # if the goal is really reached 62 | 63 | self.done = True 64 | visualization(self) 65 | plt.show() 66 | 67 | 68 | if __name__ == '__main__': 69 | p = rrt() 70 | starttime = time.time() 71 | p.run() 72 | print('time used = ' + str(time.time() - starttime)) 73 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/rrt_connect3D.py: -------------------------------------------------------------------------------- 1 | # rrt connect algorithm 2 | """ 3 | This is rrt connect implementation for 3D 4 | @author: yue qi 5 | """ 6 | import numpy as np 7 | from numpy.matlib import repmat 8 | from collections import defaultdict 9 | import time 10 | import matplotlib.pyplot as plt 11 | 12 | import os 13 | import sys 14 | 15 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 16 | 17 | from rrt_3D.env3D import env 18 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset 19 | from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent 20 | 21 | 22 | class Tree(): 23 | def __init__(self, node): 24 | self.V = [] 25 | self.Parent = {} 26 | self.V.append(node) 27 | # self.Parent[node] = None 28 | 29 | def add_vertex(self, node): 30 | if node not in self.V: 31 | self.V.append(node) 32 | 33 | def add_edge(self, parent, child): 34 | # here edge is defined a tuple of (parent, child) (qnear, qnew) 35 | self.Parent[child] = parent 36 | 37 | 38 | class rrt_connect(): 39 | def __init__(self): 40 | self.env = env() 41 | self.Parent = {} 42 | self.V = [] 43 | self.E = set() 44 | self.i = 0 45 | self.maxiter = 10000 46 | self.stepsize = 0.5 47 | self.Path = [] 48 | self.done = False 49 | self.qinit = tuple(self.env.start) 50 | self.qgoal = tuple(self.env.goal) 51 | self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) 52 | self.qnew = None 53 | self.done = False 54 | 55 | self.ind = 0 56 | self.fig = plt.figure(figsize=(10, 8)) 57 | 58 | 59 | #----------Normal RRT algorithm 60 | def BUILD_RRT(self, qinit): 61 | tree = Tree(qinit) 62 | for k in range(self.maxiter): 63 | qrand = self.RANDOM_CONFIG() 64 | self.EXTEND(tree, qrand) 65 | return tree 66 | 67 | def EXTEND(self, tree, q): 68 | qnear = tuple(self.NEAREST_NEIGHBOR(q, tree)) 69 | qnew, dist = steer(self, qnear, q) 70 | self.qnew = qnew # store qnew outside 71 | if self.NEW_CONFIG(q, qnear, qnew, dist=None): 72 | tree.add_vertex(qnew) 73 | tree.add_edge(qnear, qnew) 74 | if qnew == q: 75 | return 'Reached' 76 | else: 77 | return 'Advanced' 78 | return 'Trapped' 79 | 80 | def NEAREST_NEIGHBOR(self, q, tree): 81 | # find the nearest neighbor in the tree 82 | V = np.array(tree.V) 83 | if len(V) == 1: 84 | return V[0] 85 | xr = repmat(q, len(V), 1) 86 | dists = np.linalg.norm(xr - V, axis=1) 87 | return tuple(tree.V[np.argmin(dists)]) 88 | 89 | def RANDOM_CONFIG(self): 90 | return tuple(sampleFree(self)) 91 | 92 | def NEW_CONFIG(self, q, qnear, qnew, dist = None): 93 | # to check if the new configuration is valid or not by 94 | # making a move is used for steer 95 | # check in bound 96 | collide, _ = isCollide(self, qnear, qnew, dist = dist) 97 | return not collide 98 | 99 | #----------RRT connect algorithm 100 | def CONNECT(self, Tree, q): 101 | print('in connect') 102 | while True: 103 | S = self.EXTEND(Tree, q) 104 | if S != 'Advanced': 105 | break 106 | return S 107 | 108 | def RRT_CONNECT_PLANNER(self, qinit, qgoal): 109 | Tree_A = Tree(qinit) 110 | Tree_B = Tree(qgoal) 111 | for k in range(self.maxiter): 112 | print(k) 113 | qrand = self.RANDOM_CONFIG() 114 | if self.EXTEND(Tree_A, qrand) != 'Trapped': 115 | qnew = self.qnew # get qnew from outside 116 | if self.CONNECT(Tree_B, qnew) == 'Reached': 117 | print('reached') 118 | self.done = True 119 | self.Path = self.PATH(Tree_A, Tree_B) 120 | self.visualization(Tree_A, Tree_B, k) 121 | plt.show() 122 | return 123 | # return 124 | Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B) 125 | self.visualization(Tree_A, Tree_B, k) 126 | return 'Failure' 127 | 128 | # def PATH(self, tree_a, tree_b): 129 | def SWAP(self, tree_a, tree_b): 130 | tree_a, tree_b = tree_b, tree_a 131 | return tree_a, tree_b 132 | 133 | def PATH(self, tree_a, tree_b): 134 | qnew = self.qnew 135 | patha = [] 136 | pathb = [] 137 | while True: 138 | patha.append((tree_a.Parent[qnew], qnew)) 139 | qnew = tree_a.Parent[qnew] 140 | if qnew == self.qinit or qnew == self.qgoal: 141 | break 142 | qnew = self.qnew 143 | while True: 144 | pathb.append((tree_b.Parent[qnew], qnew)) 145 | qnew = tree_b.Parent[qnew] 146 | if qnew == self.qinit or qnew == self.qgoal: 147 | break 148 | return patha + pathb 149 | 150 | #----------RRT connect algorithm 151 | def visualization(self, tree_a, tree_b, index): 152 | if (index % 20 == 0 and index != 0) or self.done: 153 | # a_V = np.array(tree_a.V) 154 | # b_V = np.array(tree_b.V) 155 | Path = self.Path 156 | start = self.env.start 157 | goal = self.env.goal 158 | a_edges, b_edges = [], [] 159 | for i in tree_a.Parent: 160 | a_edges.append([i,tree_a.Parent[i]]) 161 | for i in tree_b.Parent: 162 | b_edges.append([i,tree_b.Parent[i]]) 163 | ax = plt.subplot(111, projection='3d') 164 | ax.view_init(elev=90., azim=0.) 165 | ax.clear() 166 | draw_Spheres(ax, self.env.balls) 167 | draw_block_list(ax, self.env.blocks) 168 | if self.env.OBB is not None: 169 | draw_obb(ax, self.env.OBB) 170 | draw_block_list(ax, np.array([self.env.boundary]), alpha=0) 171 | draw_line(ax, a_edges, visibility=0.75, color='g') 172 | draw_line(ax, b_edges, visibility=0.75, color='y') 173 | draw_line(ax, Path, color='r') 174 | ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') 175 | ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') 176 | set_axes_equal(ax) 177 | make_transparent(ax) 178 | ax.set_axis_off() 179 | plt.pause(0.0001) 180 | 181 | 182 | 183 | if __name__ == '__main__': 184 | p = rrt_connect() 185 | starttime = time.time() 186 | p.RRT_CONNECT_PLANNER(p.qinit, p.qgoal) 187 | print('time used = ' + str(time.time() - starttime)) 188 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/rrt_star3D.py: -------------------------------------------------------------------------------- 1 | """ 2 | This is rrt star code for 3D 3 | @author: yue qi 4 | """ 5 | import numpy as np 6 | from numpy.matlib import repmat 7 | from collections import defaultdict 8 | import time 9 | import matplotlib.pyplot as plt 10 | 11 | import os 12 | import sys 13 | 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") 15 | from rrt_3D.env3D import env 16 | from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path 17 | 18 | 19 | class rrtstar(): 20 | def __init__(self): 21 | self.env = env() 22 | 23 | self.Parent = {} 24 | self.V = [] 25 | # self.E = edgeset() 26 | self.COST = {} 27 | 28 | self.i = 0 29 | self.maxiter = 4000 # at least 2000 in this env 30 | self.stepsize = 2 31 | self.gamma = 7 32 | self.eta = self.stepsize 33 | self.Path = [] 34 | self.done = False 35 | self.x0 = tuple(self.env.start) 36 | self.xt = tuple(self.env.goal) 37 | 38 | self.V.append(self.x0) 39 | self.ind = 0 40 | def wireup(self,x,y): 41 | # self.E.add_edge([s,y]) # add edge 42 | self.Parent[x] = y 43 | 44 | def removewire(self,xnear): 45 | xparent = self.Parent[xnear] 46 | a = [xnear,xparent] 47 | # self.E.remove_edge(a) # remove and replace old the connection 48 | 49 | def reached(self): 50 | self.done = True 51 | goal = self.xt 52 | xn = near(self,self.env.goal) 53 | c = [cost(self,tuple(x)) for x in xn] 54 | xncmin = xn[np.argmin(c)] 55 | self.wireup(goal , tuple(xncmin)) 56 | self.V.append(goal) 57 | self.Path,self.D = path(self) 58 | 59 | def run(self): 60 | xnew = self.x0 61 | print('start rrt*... ') 62 | self.fig = plt.figure(figsize = (10,8)) 63 | while self.ind < self.maxiter: 64 | xrand = sampleFree(self) 65 | xnearest = nearest(self,xrand) 66 | xnew, dist = steer(self,xnearest,xrand) 67 | collide, _ = isCollide(self,xnearest,xnew,dist=dist) 68 | if not collide: 69 | Xnear = near(self,xnew) 70 | self.V.append(xnew) # add point 71 | visualization(self) 72 | plt.title('rrt*') 73 | # minimal path and minimal cost 74 | xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) 75 | # connecting along minimal cost path 76 | Collide = [] 77 | for xnear in Xnear: 78 | xnear = tuple(xnear) 79 | c1 = cost(self, xnear) + getDist(xnew, xnear) 80 | collide, _ = isCollide(self, xnew, xnear) 81 | Collide.append(collide) 82 | if not collide and c1 < cmin: 83 | xmin, cmin = xnear, c1 84 | self.wireup(xnew, xmin) 85 | # rewire 86 | for i in range(len(Xnear)): 87 | collide = Collide[i] 88 | xnear = tuple(Xnear[i]) 89 | c2 = cost(self, xnew) + getDist(xnew, xnear) 90 | if not collide and c2 < cost(self, xnear): 91 | # self.removewire(xnear) 92 | self.wireup(xnear, xnew) 93 | self.i += 1 94 | self.ind += 1 95 | # max sample reached 96 | self.reached() 97 | print('time used = ' + str(time.time()-starttime)) 98 | print('Total distance = '+str(self.D)) 99 | visualization(self) 100 | plt.show() 101 | 102 | 103 | if __name__ == '__main__': 104 | p = rrtstar() 105 | starttime = time.time() 106 | p.run() 107 | 108 | -------------------------------------------------------------------------------- /Sampling_based_Planning/rrt_3D/rrt_star_smart3D.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Sampling_based_Planning/rrt_3D/rrt_star_smart3D.py -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/ARAstar.py: -------------------------------------------------------------------------------- 1 | """ 2 | ARA_star 2D (Anytime Repairing A*) 3 | @author: huiming zhou 4 | 5 | @description: local inconsistency: g-value decreased. 6 | g(s) decreased introduces a local inconsistency between s and its successors. 7 | 8 | """ 9 | 10 | import os 11 | import sys 12 | import math 13 | 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 15 | "/../../Search_based_Planning/") 16 | 17 | from Search_2D import plotting, env 18 | 19 | 20 | class AraStar: 21 | def __init__(self, s_start, s_goal, e, heuristic_type): 22 | self.s_start, self.s_goal = s_start, s_goal 23 | self.heuristic_type = heuristic_type 24 | 25 | self.Env = env.Env() # class Env 26 | 27 | self.u_set = self.Env.motions # feasible input set 28 | self.obs = self.Env.obs # position of obstacles 29 | self.e = e # weight 30 | 31 | self.g = dict() # Cost to come 32 | self.OPEN = dict() # priority queue / OPEN set 33 | self.CLOSED = set() # CLOSED set 34 | self.INCONS = {} # INCONSISTENT set 35 | self.PARENT = dict() # relations 36 | self.path = [] # planning path 37 | self.visited = [] # order of visited nodes 38 | 39 | def init(self): 40 | """ 41 | initialize each set. 42 | """ 43 | 44 | self.g[self.s_start] = 0.0 45 | self.g[self.s_goal] = math.inf 46 | self.OPEN[self.s_start] = self.f_value(self.s_start) 47 | self.PARENT[self.s_start] = self.s_start 48 | 49 | def searching(self): 50 | self.init() 51 | self.ImprovePath() 52 | self.path.append(self.extract_path()) 53 | 54 | while self.update_e() > 1: # continue condition 55 | self.e -= 0.4 # increase weight 56 | self.OPEN.update(self.INCONS) 57 | self.OPEN = {s: self.f_value(s) for s in self.OPEN} # update f_value of OPEN set 58 | 59 | self.INCONS = dict() 60 | self.CLOSED = set() 61 | self.ImprovePath() # improve path 62 | self.path.append(self.extract_path()) 63 | 64 | return self.path, self.visited 65 | 66 | def ImprovePath(self): 67 | """ 68 | :return: a e'-suboptimal path 69 | """ 70 | 71 | visited_each = [] 72 | 73 | while True: 74 | s, f_small = self.calc_smallest_f() 75 | 76 | if self.f_value(self.s_goal) <= f_small: 77 | break 78 | 79 | self.OPEN.pop(s) 80 | self.CLOSED.add(s) 81 | 82 | for s_n in self.get_neighbor(s): 83 | if s_n in self.obs: 84 | continue 85 | 86 | new_cost = self.g[s] + self.cost(s, s_n) 87 | 88 | if s_n not in self.g or new_cost < self.g[s_n]: 89 | self.g[s_n] = new_cost 90 | self.PARENT[s_n] = s 91 | visited_each.append(s_n) 92 | 93 | if s_n not in self.CLOSED: 94 | self.OPEN[s_n] = self.f_value(s_n) 95 | else: 96 | self.INCONS[s_n] = 0.0 97 | 98 | self.visited.append(visited_each) 99 | 100 | def calc_smallest_f(self): 101 | """ 102 | :return: node with smallest f_value in OPEN set. 103 | """ 104 | 105 | s_small = min(self.OPEN, key=self.OPEN.get) 106 | 107 | return s_small, self.OPEN[s_small] 108 | 109 | def get_neighbor(self, s): 110 | """ 111 | find neighbors of state s that not in obstacles. 112 | :param s: state 113 | :return: neighbors 114 | """ 115 | 116 | return {(s[0] + u[0], s[1] + u[1]) for u in self.u_set} 117 | 118 | def update_e(self): 119 | v = float("inf") 120 | 121 | if self.OPEN: 122 | v = min(self.g[s] + self.h(s) for s in self.OPEN) 123 | if self.INCONS: 124 | v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS)) 125 | 126 | return min(self.e, self.g[self.s_goal] / v) 127 | 128 | def f_value(self, x): 129 | """ 130 | f = g + e * h 131 | f = cost-to-come + weight * cost-to-go 132 | :param x: current state 133 | :return: f_value 134 | """ 135 | 136 | return self.g[x] + self.e * self.h(x) 137 | 138 | def extract_path(self): 139 | """ 140 | Extract the path based on the PARENT set. 141 | :return: The planning path 142 | """ 143 | 144 | path = [self.s_goal] 145 | s = self.s_goal 146 | 147 | while True: 148 | s = self.PARENT[s] 149 | path.append(s) 150 | 151 | if s == self.s_start: 152 | break 153 | 154 | return list(path) 155 | 156 | def h(self, s): 157 | """ 158 | Calculate heuristic. 159 | :param s: current node (state) 160 | :return: heuristic function value 161 | """ 162 | 163 | heuristic_type = self.heuristic_type # heuristic type 164 | goal = self.s_goal # goal node 165 | 166 | if heuristic_type == "manhattan": 167 | return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) 168 | else: 169 | return math.hypot(goal[0] - s[0], goal[1] - s[1]) 170 | 171 | def cost(self, s_start, s_goal): 172 | """ 173 | Calculate Cost for this motion 174 | :param s_start: starting node 175 | :param s_goal: end node 176 | :return: Cost for this motion 177 | :note: Cost function could be more complicate! 178 | """ 179 | 180 | if self.is_collision(s_start, s_goal): 181 | return math.inf 182 | 183 | return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) 184 | 185 | def is_collision(self, s_start, s_end): 186 | """ 187 | check if the line segment (s_start, s_end) is collision. 188 | :param s_start: start node 189 | :param s_end: end node 190 | :return: True: is collision / False: not collision 191 | """ 192 | 193 | if s_start in self.obs or s_end in self.obs: 194 | return True 195 | 196 | if s_start[0] != s_end[0] and s_start[1] != s_end[1]: 197 | if s_end[0] - s_start[0] == s_start[1] - s_end[1]: 198 | s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 199 | s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 200 | else: 201 | s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 202 | s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 203 | 204 | if s1 in self.obs or s2 in self.obs: 205 | return True 206 | 207 | return False 208 | 209 | 210 | def main(): 211 | s_start = (5, 5) 212 | s_goal = (45, 25) 213 | 214 | arastar = AraStar(s_start, s_goal, 2.5, "euclidean") 215 | plot = plotting.Plotting(s_start, s_goal) 216 | 217 | path, visited = arastar.searching() 218 | plot.animation_ara_star(path, visited, "Anytime Repairing A* (ARA*)") 219 | 220 | 221 | if __name__ == '__main__': 222 | main() 223 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/Astar.py: -------------------------------------------------------------------------------- 1 | """ 2 | A_star 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import heapq 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Search_based_Planning/") 13 | 14 | from Search_2D import plotting, env 15 | 16 | 17 | class AStar: 18 | """AStar set the cost + heuristics as the priority 19 | """ 20 | def __init__(self, s_start, s_goal, heuristic_type): 21 | self.s_start = s_start 22 | self.s_goal = s_goal 23 | self.heuristic_type = heuristic_type 24 | 25 | self.Env = env.Env() # class Env 26 | 27 | self.u_set = self.Env.motions # feasible input set 28 | self.obs = self.Env.obs # position of obstacles 29 | 30 | self.OPEN = [] # priority queue / OPEN set 31 | self.CLOSED = [] # CLOSED set / VISITED order 32 | self.PARENT = dict() # recorded parent 33 | self.g = dict() # cost to come 34 | 35 | def searching(self): 36 | """ 37 | A_star Searching. 38 | :return: path, visited order 39 | """ 40 | 41 | self.PARENT[self.s_start] = self.s_start 42 | self.g[self.s_start] = 0 43 | self.g[self.s_goal] = math.inf 44 | heapq.heappush(self.OPEN, 45 | (self.f_value(self.s_start), self.s_start)) 46 | 47 | while self.OPEN: 48 | _, s = heapq.heappop(self.OPEN) 49 | self.CLOSED.append(s) 50 | 51 | if s == self.s_goal: # stop condition 52 | break 53 | 54 | for s_n in self.get_neighbor(s): 55 | new_cost = self.g[s] + self.cost(s, s_n) 56 | 57 | if s_n not in self.g: 58 | self.g[s_n] = math.inf 59 | 60 | if new_cost < self.g[s_n]: # conditions for updating Cost 61 | self.g[s_n] = new_cost 62 | self.PARENT[s_n] = s 63 | heapq.heappush(self.OPEN, (self.f_value(s_n), s_n)) 64 | 65 | return self.extract_path(self.PARENT), self.CLOSED 66 | 67 | def searching_repeated_astar(self, e): 68 | """ 69 | repeated A*. 70 | :param e: weight of A* 71 | :return: path and visited order 72 | """ 73 | 74 | path, visited = [], [] 75 | 76 | while e >= 1: 77 | p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e) 78 | path.append(p_k) 79 | visited.append(v_k) 80 | e -= 0.5 81 | 82 | return path, visited 83 | 84 | def repeated_searching(self, s_start, s_goal, e): 85 | """ 86 | run A* with weight e. 87 | :param s_start: starting state 88 | :param s_goal: goal state 89 | :param e: weight of a* 90 | :return: path and visited order. 91 | """ 92 | 93 | g = {s_start: 0, s_goal: float("inf")} 94 | PARENT = {s_start: s_start} 95 | OPEN = [] 96 | CLOSED = [] 97 | heapq.heappush(OPEN, 98 | (g[s_start] + e * self.heuristic(s_start), s_start)) 99 | 100 | while OPEN: 101 | _, s = heapq.heappop(OPEN) 102 | CLOSED.append(s) 103 | 104 | if s == s_goal: 105 | break 106 | 107 | for s_n in self.get_neighbor(s): 108 | new_cost = g[s] + self.cost(s, s_n) 109 | 110 | if s_n not in g: 111 | g[s_n] = math.inf 112 | 113 | if new_cost < g[s_n]: # conditions for updating Cost 114 | g[s_n] = new_cost 115 | PARENT[s_n] = s 116 | heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n)) 117 | 118 | return self.extract_path(PARENT), CLOSED 119 | 120 | def get_neighbor(self, s): 121 | """ 122 | find neighbors of state s that not in obstacles. 123 | :param s: state 124 | :return: neighbors 125 | """ 126 | 127 | return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] 128 | 129 | def cost(self, s_start, s_goal): 130 | """ 131 | Calculate Cost for this motion 132 | :param s_start: starting node 133 | :param s_goal: end node 134 | :return: Cost for this motion 135 | :note: Cost function could be more complicate! 136 | """ 137 | 138 | if self.is_collision(s_start, s_goal): 139 | return math.inf 140 | 141 | return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) 142 | 143 | def is_collision(self, s_start, s_end): 144 | """ 145 | check if the line segment (s_start, s_end) is collision. 146 | :param s_start: start node 147 | :param s_end: end node 148 | :return: True: is collision / False: not collision 149 | """ 150 | 151 | if s_start in self.obs or s_end in self.obs: 152 | return True 153 | 154 | if s_start[0] != s_end[0] and s_start[1] != s_end[1]: 155 | if s_end[0] - s_start[0] == s_start[1] - s_end[1]: 156 | s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 157 | s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 158 | else: 159 | s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 160 | s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 161 | 162 | if s1 in self.obs or s2 in self.obs: 163 | return True 164 | 165 | return False 166 | 167 | def f_value(self, s): 168 | """ 169 | f = g + h. (g: Cost to come, h: heuristic value) 170 | :param s: current state 171 | :return: f 172 | """ 173 | 174 | return self.g[s] + self.heuristic(s) 175 | 176 | def extract_path(self, PARENT): 177 | """ 178 | Extract the path based on the PARENT set. 179 | :return: The planning path 180 | """ 181 | 182 | path = [self.s_goal] 183 | s = self.s_goal 184 | 185 | while True: 186 | s = PARENT[s] 187 | path.append(s) 188 | 189 | if s == self.s_start: 190 | break 191 | 192 | return list(path) 193 | 194 | def heuristic(self, s): 195 | """ 196 | Calculate heuristic. 197 | :param s: current node (state) 198 | :return: heuristic function value 199 | """ 200 | 201 | heuristic_type = self.heuristic_type # heuristic type 202 | goal = self.s_goal # goal node 203 | 204 | if heuristic_type == "manhattan": 205 | return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) 206 | else: 207 | return math.hypot(goal[0] - s[0], goal[1] - s[1]) 208 | 209 | 210 | def main(): 211 | s_start = (5, 5) 212 | s_goal = (45, 25) 213 | 214 | astar = AStar(s_start, s_goal, "euclidean") 215 | plot = plotting.Plotting(s_start, s_goal) 216 | 217 | path, visited = astar.searching() 218 | plot.animation(path, visited, "A*") # animation 219 | 220 | # path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5 221 | # plot.animation_ara_star(path, visited, "Repeated A*") 222 | 223 | 224 | if __name__ == '__main__': 225 | main() 226 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/Best_First.py: -------------------------------------------------------------------------------- 1 | """ 2 | Best-First Searching 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import heapq 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Search_based_Planning/") 13 | 14 | from Search_2D import plotting, env 15 | from Search_2D.Astar import AStar 16 | 17 | 18 | class BestFirst(AStar): 19 | """BestFirst set the heuristics as the priority 20 | """ 21 | def searching(self): 22 | """ 23 | Breadth-first Searching. 24 | :return: path, visited order 25 | """ 26 | 27 | self.PARENT[self.s_start] = self.s_start 28 | self.g[self.s_start] = 0 29 | self.g[self.s_goal] = math.inf 30 | heapq.heappush(self.OPEN, 31 | (self.heuristic(self.s_start), self.s_start)) 32 | 33 | while self.OPEN: 34 | _, s = heapq.heappop(self.OPEN) 35 | self.CLOSED.append(s) 36 | 37 | if s == self.s_goal: 38 | break 39 | 40 | for s_n in self.get_neighbor(s): 41 | new_cost = self.g[s] + self.cost(s, s_n) 42 | 43 | if s_n not in self.g: 44 | self.g[s_n] = math.inf 45 | 46 | if new_cost < self.g[s_n]: # conditions for updating Cost 47 | self.g[s_n] = new_cost 48 | self.PARENT[s_n] = s 49 | 50 | # best first set the heuristics as the priority 51 | heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n)) 52 | 53 | return self.extract_path(self.PARENT), self.CLOSED 54 | 55 | 56 | def main(): 57 | s_start = (5, 5) 58 | s_goal = (45, 25) 59 | 60 | BF = BestFirst(s_start, s_goal, 'euclidean') 61 | plot = plotting.Plotting(s_start, s_goal) 62 | 63 | path, visited = BF.searching() 64 | plot.animation(path, visited, "Best-first Searching") # animation 65 | 66 | 67 | if __name__ == '__main__': 68 | main() 69 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/Bidirectional_a_star.py: -------------------------------------------------------------------------------- 1 | """ 2 | Bidirectional_a_star 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import heapq 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Search_based_Planning/") 13 | 14 | from Search_2D import plotting, env 15 | 16 | 17 | class BidirectionalAStar: 18 | def __init__(self, s_start, s_goal, heuristic_type): 19 | self.s_start = s_start 20 | self.s_goal = s_goal 21 | self.heuristic_type = heuristic_type 22 | 23 | self.Env = env.Env() # class Env 24 | 25 | self.u_set = self.Env.motions # feasible input set 26 | self.obs = self.Env.obs # position of obstacles 27 | 28 | self.OPEN_fore = [] # OPEN set for forward searching 29 | self.OPEN_back = [] # OPEN set for backward searching 30 | self.CLOSED_fore = [] # CLOSED set for forward 31 | self.CLOSED_back = [] # CLOSED set for backward 32 | self.PARENT_fore = dict() # recorded parent for forward 33 | self.PARENT_back = dict() # recorded parent for backward 34 | self.g_fore = dict() # cost to come for forward 35 | self.g_back = dict() # cost to come for backward 36 | 37 | def init(self): 38 | """ 39 | initialize parameters 40 | """ 41 | 42 | self.g_fore[self.s_start] = 0.0 43 | self.g_fore[self.s_goal] = math.inf 44 | self.g_back[self.s_goal] = 0.0 45 | self.g_back[self.s_start] = math.inf 46 | self.PARENT_fore[self.s_start] = self.s_start 47 | self.PARENT_back[self.s_goal] = self.s_goal 48 | heapq.heappush(self.OPEN_fore, 49 | (self.f_value_fore(self.s_start), self.s_start)) 50 | heapq.heappush(self.OPEN_back, 51 | (self.f_value_back(self.s_goal), self.s_goal)) 52 | 53 | def searching(self): 54 | """ 55 | Bidirectional A* 56 | :return: connected path, visited order of forward, visited order of backward 57 | """ 58 | 59 | self.init() 60 | s_meet = self.s_start 61 | 62 | while self.OPEN_fore and self.OPEN_back: 63 | # solve foreward-search 64 | _, s_fore = heapq.heappop(self.OPEN_fore) 65 | 66 | if s_fore in self.PARENT_back: 67 | s_meet = s_fore 68 | break 69 | 70 | self.CLOSED_fore.append(s_fore) 71 | 72 | for s_n in self.get_neighbor(s_fore): 73 | new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n) 74 | 75 | if s_n not in self.g_fore: 76 | self.g_fore[s_n] = math.inf 77 | 78 | if new_cost < self.g_fore[s_n]: 79 | self.g_fore[s_n] = new_cost 80 | self.PARENT_fore[s_n] = s_fore 81 | heapq.heappush(self.OPEN_fore, 82 | (self.f_value_fore(s_n), s_n)) 83 | 84 | # solve backward-search 85 | _, s_back = heapq.heappop(self.OPEN_back) 86 | 87 | if s_back in self.PARENT_fore: 88 | s_meet = s_back 89 | break 90 | 91 | self.CLOSED_back.append(s_back) 92 | 93 | for s_n in self.get_neighbor(s_back): 94 | new_cost = self.g_back[s_back] + self.cost(s_back, s_n) 95 | 96 | if s_n not in self.g_back: 97 | self.g_back[s_n] = math.inf 98 | 99 | if new_cost < self.g_back[s_n]: 100 | self.g_back[s_n] = new_cost 101 | self.PARENT_back[s_n] = s_back 102 | heapq.heappush(self.OPEN_back, 103 | (self.f_value_back(s_n), s_n)) 104 | 105 | return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back 106 | 107 | def get_neighbor(self, s): 108 | """ 109 | find neighbors of state s that not in obstacles. 110 | :param s: state 111 | :return: neighbors 112 | """ 113 | 114 | return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set] 115 | 116 | def extract_path(self, s_meet): 117 | """ 118 | extract path from start and goal 119 | :param s_meet: meet point of bi-direction a* 120 | :return: path 121 | """ 122 | 123 | # extract path for foreward part 124 | path_fore = [s_meet] 125 | s = s_meet 126 | 127 | while True: 128 | s = self.PARENT_fore[s] 129 | path_fore.append(s) 130 | if s == self.s_start: 131 | break 132 | 133 | # extract path for backward part 134 | path_back = [] 135 | s = s_meet 136 | 137 | while True: 138 | s = self.PARENT_back[s] 139 | path_back.append(s) 140 | if s == self.s_goal: 141 | break 142 | 143 | return list(reversed(path_fore)) + list(path_back) 144 | 145 | def f_value_fore(self, s): 146 | """ 147 | forward searching: f = g + h. (g: Cost to come, h: heuristic value) 148 | :param s: current state 149 | :return: f 150 | """ 151 | 152 | return self.g_fore[s] + self.h(s, self.s_goal) 153 | 154 | def f_value_back(self, s): 155 | """ 156 | backward searching: f = g + h. (g: Cost to come, h: heuristic value) 157 | :param s: current state 158 | :return: f 159 | """ 160 | 161 | return self.g_back[s] + self.h(s, self.s_start) 162 | 163 | def h(self, s, goal): 164 | """ 165 | Calculate heuristic value. 166 | :param s: current node (state) 167 | :param goal: goal node (state) 168 | :return: heuristic value 169 | """ 170 | 171 | heuristic_type = self.heuristic_type 172 | 173 | if heuristic_type == "manhattan": 174 | return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) 175 | else: 176 | return math.hypot(goal[0] - s[0], goal[1] - s[1]) 177 | 178 | def cost(self, s_start, s_goal): 179 | """ 180 | Calculate Cost for this motion 181 | :param s_start: starting node 182 | :param s_goal: end node 183 | :return: Cost for this motion 184 | :note: Cost function could be more complicate! 185 | """ 186 | 187 | if self.is_collision(s_start, s_goal): 188 | return math.inf 189 | 190 | return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) 191 | 192 | def is_collision(self, s_start, s_end): 193 | """ 194 | check if the line segment (s_start, s_end) is collision. 195 | :param s_start: start node 196 | :param s_end: end node 197 | :return: True: is collision / False: not collision 198 | """ 199 | 200 | if s_start in self.obs or s_end in self.obs: 201 | return True 202 | 203 | if s_start[0] != s_end[0] and s_start[1] != s_end[1]: 204 | if s_end[0] - s_start[0] == s_start[1] - s_end[1]: 205 | s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 206 | s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 207 | else: 208 | s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 209 | s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 210 | 211 | if s1 in self.obs or s2 in self.obs: 212 | return True 213 | 214 | return False 215 | 216 | 217 | def main(): 218 | x_start = (5, 5) 219 | x_goal = (45, 25) 220 | 221 | bastar = BidirectionalAStar(x_start, x_goal, "euclidean") 222 | plot = plotting.Plotting(x_start, x_goal) 223 | 224 | path, visited_fore, visited_back = bastar.searching() 225 | plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation 226 | 227 | 228 | if __name__ == '__main__': 229 | main() 230 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/Dijkstra.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dijkstra 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import heapq 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Search_based_Planning/") 13 | 14 | from Search_2D import plotting, env 15 | 16 | from Search_2D.Astar import AStar 17 | 18 | 19 | class Dijkstra(AStar): 20 | """Dijkstra set the cost as the priority 21 | """ 22 | def searching(self): 23 | """ 24 | Breadth-first Searching. 25 | :return: path, visited order 26 | """ 27 | 28 | self.PARENT[self.s_start] = self.s_start 29 | self.g[self.s_start] = 0 30 | self.g[self.s_goal] = math.inf 31 | heapq.heappush(self.OPEN, 32 | (0, self.s_start)) 33 | 34 | while self.OPEN: 35 | _, s = heapq.heappop(self.OPEN) 36 | self.CLOSED.append(s) 37 | 38 | if s == self.s_goal: 39 | break 40 | 41 | for s_n in self.get_neighbor(s): 42 | new_cost = self.g[s] + self.cost(s, s_n) 43 | 44 | if s_n not in self.g: 45 | self.g[s_n] = math.inf 46 | 47 | if new_cost < self.g[s_n]: # conditions for updating Cost 48 | self.g[s_n] = new_cost 49 | self.PARENT[s_n] = s 50 | 51 | # best first set the heuristics as the priority 52 | heapq.heappush(self.OPEN, (new_cost, s_n)) 53 | 54 | return self.extract_path(self.PARENT), self.CLOSED 55 | 56 | 57 | def main(): 58 | s_start = (5, 5) 59 | s_goal = (45, 25) 60 | 61 | dijkstra = Dijkstra(s_start, s_goal, 'None') 62 | plot = plotting.Plotting(s_start, s_goal) 63 | 64 | path, visited = dijkstra.searching() 65 | plot.animation(path, visited, "Dijkstra's") # animation generate 66 | 67 | 68 | if __name__ == '__main__': 69 | main() 70 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/LRTAstar.py: -------------------------------------------------------------------------------- 1 | """ 2 | LRTA_star 2D (Learning Real-time A*) 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import copy 9 | import math 10 | 11 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 12 | "/../../Search_based_Planning/") 13 | 14 | from Search_2D import queue, plotting, env 15 | 16 | 17 | class LrtAStarN: 18 | def __init__(self, s_start, s_goal, N, heuristic_type): 19 | self.s_start, self.s_goal = s_start, s_goal 20 | self.heuristic_type = heuristic_type 21 | 22 | self.Env = env.Env() 23 | 24 | self.u_set = self.Env.motions # feasible input set 25 | self.obs = self.Env.obs # position of obstacles 26 | 27 | self.N = N # number of expand nodes each iteration 28 | self.visited = [] # order of visited nodes in planning 29 | self.path = [] # path of each iteration 30 | self.h_table = {} # h_value table 31 | 32 | def init(self): 33 | """ 34 | initialize the h_value of all nodes in the environment. 35 | it is a global table. 36 | """ 37 | 38 | for i in range(self.Env.x_range): 39 | for j in range(self.Env.y_range): 40 | self.h_table[(i, j)] = self.h((i, j)) 41 | 42 | def searching(self): 43 | self.init() 44 | s_start = self.s_start # initialize start node 45 | 46 | while True: 47 | OPEN, CLOSED = self.AStar(s_start, self.N) # OPEN, CLOSED sets in each iteration 48 | 49 | if OPEN == "FOUND": # reach the goal node 50 | self.path.append(CLOSED) 51 | break 52 | 53 | h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes 54 | 55 | for x in h_value: 56 | self.h_table[x] = h_value[x] 57 | 58 | s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set 59 | self.path.append(path_k) 60 | 61 | def extract_path_in_CLOSE(self, s_start, h_value): 62 | path = [s_start] 63 | s = s_start 64 | 65 | while True: 66 | h_list = {} 67 | 68 | for s_n in self.get_neighbor(s): 69 | if s_n in h_value: 70 | h_list[s_n] = h_value[s_n] 71 | else: 72 | h_list[s_n] = self.h_table[s_n] 73 | 74 | s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value 75 | path.append(s_key) # generate path 76 | s = s_key # use end of this iteration as the start of next 77 | 78 | if s_key not in h_value: # reach the expected node in OPEN set 79 | return s_key, path 80 | 81 | def iteration(self, CLOSED): 82 | h_value = {} 83 | 84 | for s in CLOSED: 85 | h_value[s] = float("inf") # initialize h_value of CLOSED nodes 86 | 87 | while True: 88 | h_value_rec = copy.deepcopy(h_value) 89 | for s in CLOSED: 90 | h_list = [] 91 | for s_n in self.get_neighbor(s): 92 | if s_n not in CLOSED: 93 | h_list.append(self.cost(s, s_n) + self.h_table[s_n]) 94 | else: 95 | h_list.append(self.cost(s, s_n) + h_value[s_n]) 96 | h_value[s] = min(h_list) # update h_value of current node 97 | 98 | if h_value == h_value_rec: # h_value table converged 99 | return h_value 100 | 101 | def AStar(self, x_start, N): 102 | OPEN = queue.QueuePrior() # OPEN set 103 | OPEN.put(x_start, self.h(x_start)) 104 | CLOSED = [] # CLOSED set 105 | g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come 106 | PARENT = {x_start: x_start} # relations 107 | count = 0 # counter 108 | 109 | while not OPEN.empty(): 110 | count += 1 111 | s = OPEN.get() 112 | CLOSED.append(s) 113 | 114 | if s == self.s_goal: # reach the goal node 115 | self.visited.append(CLOSED) 116 | return "FOUND", self.extract_path(x_start, PARENT) 117 | 118 | for s_n in self.get_neighbor(s): 119 | if s_n not in CLOSED: 120 | new_cost = g_table[s] + self.cost(s, s_n) 121 | if s_n not in g_table: 122 | g_table[s_n] = float("inf") 123 | if new_cost < g_table[s_n]: # conditions for updating Cost 124 | g_table[s_n] = new_cost 125 | PARENT[s_n] = s 126 | OPEN.put(s_n, g_table[s_n] + self.h_table[s_n]) 127 | 128 | if count == N: # expand needed CLOSED nodes 129 | break 130 | 131 | self.visited.append(CLOSED) # visited nodes in each iteration 132 | 133 | return OPEN, CLOSED 134 | 135 | def get_neighbor(self, s): 136 | """ 137 | find neighbors of state s that not in obstacles. 138 | :param s: state 139 | :return: neighbors 140 | """ 141 | 142 | s_list = [] 143 | 144 | for u in self.u_set: 145 | s_next = tuple([s[i] + u[i] for i in range(2)]) 146 | if s_next not in self.obs: 147 | s_list.append(s_next) 148 | 149 | return s_list 150 | 151 | def extract_path(self, x_start, parent): 152 | """ 153 | Extract the path based on the relationship of nodes. 154 | 155 | :return: The planning path 156 | """ 157 | 158 | path_back = [self.s_goal] 159 | x_current = self.s_goal 160 | 161 | while True: 162 | x_current = parent[x_current] 163 | path_back.append(x_current) 164 | 165 | if x_current == x_start: 166 | break 167 | 168 | return list(reversed(path_back)) 169 | 170 | def h(self, s): 171 | """ 172 | Calculate heuristic. 173 | :param s: current node (state) 174 | :return: heuristic function value 175 | """ 176 | 177 | heuristic_type = self.heuristic_type # heuristic type 178 | goal = self.s_goal # goal node 179 | 180 | if heuristic_type == "manhattan": 181 | return abs(goal[0] - s[0]) + abs(goal[1] - s[1]) 182 | else: 183 | return math.hypot(goal[0] - s[0], goal[1] - s[1]) 184 | 185 | def cost(self, s_start, s_goal): 186 | """ 187 | Calculate Cost for this motion 188 | :param s_start: starting node 189 | :param s_goal: end node 190 | :return: Cost for this motion 191 | :note: Cost function could be more complicate! 192 | """ 193 | 194 | if self.is_collision(s_start, s_goal): 195 | return float("inf") 196 | 197 | return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1]) 198 | 199 | def is_collision(self, s_start, s_end): 200 | if s_start in self.obs or s_end in self.obs: 201 | return True 202 | 203 | if s_start[0] != s_end[0] and s_start[1] != s_end[1]: 204 | if s_end[0] - s_start[0] == s_start[1] - s_end[1]: 205 | s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 206 | s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 207 | else: 208 | s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1])) 209 | s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1])) 210 | 211 | if s1 in self.obs or s2 in self.obs: 212 | return True 213 | 214 | return False 215 | 216 | 217 | def main(): 218 | s_start = (10, 5) 219 | s_goal = (45, 25) 220 | 221 | lrta = LrtAStarN(s_start, s_goal, 250, "euclidean") 222 | plot = plotting.Plotting(s_start, s_goal) 223 | 224 | lrta.searching() 225 | plot.animation_lrta(lrta.path, lrta.visited, 226 | "Learning Real-time A* (LRTA*)") 227 | 228 | 229 | if __name__ == '__main__': 230 | main() 231 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/Astar.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/Astar.cpython-38.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/env.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/env.cpython-38.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/plotting.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-38.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/__pycache__/queue.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_2D/__pycache__/queue.cpython-38.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/bfs.py: -------------------------------------------------------------------------------- 1 | """ 2 | Breadth-first Searching_2D (BFS) 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | from collections import deque 9 | 10 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 11 | "/../../Search_based_Planning/") 12 | 13 | from Search_2D import plotting, env 14 | from Search_2D.Astar import AStar 15 | import math 16 | import heapq 17 | 18 | class BFS(AStar): 19 | """BFS add the new visited node in the end of the openset 20 | """ 21 | def searching(self): 22 | """ 23 | Breadth-first Searching. 24 | :return: path, visited order 25 | """ 26 | 27 | self.PARENT[self.s_start] = self.s_start 28 | self.g[self.s_start] = 0 29 | self.g[self.s_goal] = math.inf 30 | heapq.heappush(self.OPEN, 31 | (0, self.s_start)) 32 | 33 | while self.OPEN: 34 | _, s = heapq.heappop(self.OPEN) 35 | self.CLOSED.append(s) 36 | 37 | if s == self.s_goal: 38 | break 39 | 40 | for s_n in self.get_neighbor(s): 41 | new_cost = self.g[s] + self.cost(s, s_n) 42 | 43 | if s_n not in self.g: 44 | self.g[s_n] = math.inf 45 | 46 | if new_cost < self.g[s_n]: # conditions for updating Cost 47 | self.g[s_n] = new_cost 48 | self.PARENT[s_n] = s 49 | 50 | # bfs, add new node to the end of the openset 51 | prior = self.OPEN[-1][0]+1 if len(self.OPEN)>0 else 0 52 | heapq.heappush(self.OPEN, (prior, s_n)) 53 | 54 | return self.extract_path(self.PARENT), self.CLOSED 55 | 56 | 57 | def main(): 58 | s_start = (5, 5) 59 | s_goal = (45, 25) 60 | 61 | bfs = BFS(s_start, s_goal, 'None') 62 | plot = plotting.Plotting(s_start, s_goal) 63 | 64 | path, visited = bfs.searching() 65 | plot.animation(path, visited, "Breadth-first Searching (BFS)") 66 | 67 | 68 | if __name__ == '__main__': 69 | main() 70 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/dfs.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import sys 4 | import math 5 | import heapq 6 | 7 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 8 | "/../../Search_based_Planning/") 9 | 10 | from Search_2D import plotting, env 11 | from Search_2D.Astar import AStar 12 | 13 | class DFS(AStar): 14 | """DFS add the new visited node in the front of the openset 15 | """ 16 | def searching(self): 17 | """ 18 | Breadth-first Searching. 19 | :return: path, visited order 20 | """ 21 | 22 | self.PARENT[self.s_start] = self.s_start 23 | self.g[self.s_start] = 0 24 | self.g[self.s_goal] = math.inf 25 | heapq.heappush(self.OPEN, 26 | (0, self.s_start)) 27 | 28 | while self.OPEN: 29 | _, s = heapq.heappop(self.OPEN) 30 | self.CLOSED.append(s) 31 | 32 | if s == self.s_goal: 33 | break 34 | 35 | for s_n in self.get_neighbor(s): 36 | new_cost = self.g[s] + self.cost(s, s_n) 37 | 38 | if s_n not in self.g: 39 | self.g[s_n] = math.inf 40 | 41 | if new_cost < self.g[s_n]: # conditions for updating Cost 42 | self.g[s_n] = new_cost 43 | self.PARENT[s_n] = s 44 | 45 | # dfs, add new node to the front of the openset 46 | prior = self.OPEN[0][0]-1 if len(self.OPEN)>0 else 0 47 | heapq.heappush(self.OPEN, (prior, s_n)) 48 | 49 | return self.extract_path(self.PARENT), self.CLOSED 50 | 51 | 52 | def main(): 53 | s_start = (5, 5) 54 | s_goal = (45, 25) 55 | 56 | dfs = DFS(s_start, s_goal, 'None') 57 | plot = plotting.Plotting(s_start, s_goal) 58 | 59 | path, visited = dfs.searching() 60 | visited = list(dict.fromkeys(visited)) 61 | plot.animation(path, visited, "Depth-first Searching (DFS)") # animation 62 | 63 | 64 | if __name__ == '__main__': 65 | main() 66 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Env 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | 7 | class Env: 8 | def __init__(self): 9 | self.x_range = 51 # size of background 10 | self.y_range = 31 11 | self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1), 12 | (1, 0), (1, -1), (0, -1), (-1, -1)] 13 | self.obs = self.obs_map() 14 | 15 | def update_obs(self, obs): 16 | self.obs = obs 17 | 18 | def obs_map(self): 19 | """ 20 | Initialize obstacles' positions 21 | :return: map of obstacles 22 | """ 23 | 24 | x = self.x_range 25 | y = self.y_range 26 | obs = set() 27 | 28 | for i in range(x): 29 | obs.add((i, 0)) 30 | for i in range(x): 31 | obs.add((i, y - 1)) 32 | 33 | for i in range(y): 34 | obs.add((0, i)) 35 | for i in range(y): 36 | obs.add((x - 1, i)) 37 | 38 | for i in range(10, 21): 39 | obs.add((i, 15)) 40 | for i in range(15): 41 | obs.add((20, i)) 42 | 43 | for i in range(15, 30): 44 | obs.add((30, i)) 45 | for i in range(16): 46 | obs.add((40, i)) 47 | 48 | return obs 49 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/plotting.py: -------------------------------------------------------------------------------- 1 | """ 2 | Plot tools 2D 3 | @author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import matplotlib.pyplot as plt 9 | 10 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 11 | "/../../Search_based_Planning/") 12 | 13 | from Search_2D import env 14 | 15 | 16 | class Plotting: 17 | def __init__(self, xI, xG): 18 | self.xI, self.xG = xI, xG 19 | self.env = env.Env() 20 | self.obs = self.env.obs_map() 21 | 22 | def update_obs(self, obs): 23 | self.obs = obs 24 | 25 | def animation(self, path, visited, name): 26 | self.plot_grid(name) 27 | self.plot_visited(visited) 28 | self.plot_path(path) 29 | plt.show() 30 | 31 | def animation_lrta(self, path, visited, name): 32 | self.plot_grid(name) 33 | cl = self.color_list_2() 34 | path_combine = [] 35 | 36 | for k in range(len(path)): 37 | self.plot_visited(visited[k], cl[k]) 38 | plt.pause(0.2) 39 | self.plot_path(path[k]) 40 | path_combine += path[k] 41 | plt.pause(0.2) 42 | if self.xI in path_combine: 43 | path_combine.remove(self.xI) 44 | self.plot_path(path_combine) 45 | plt.show() 46 | 47 | def animation_ara_star(self, path, visited, name): 48 | self.plot_grid(name) 49 | cl_v, cl_p = self.color_list() 50 | 51 | for k in range(len(path)): 52 | self.plot_visited(visited[k], cl_v[k]) 53 | self.plot_path(path[k], cl_p[k], True) 54 | plt.pause(0.5) 55 | 56 | plt.show() 57 | 58 | def animation_bi_astar(self, path, v_fore, v_back, name): 59 | self.plot_grid(name) 60 | self.plot_visited_bi(v_fore, v_back) 61 | self.plot_path(path) 62 | plt.show() 63 | 64 | def plot_grid(self, name): 65 | obs_x = [x[0] for x in self.obs] 66 | obs_y = [x[1] for x in self.obs] 67 | 68 | plt.plot(self.xI[0], self.xI[1], "bs") 69 | plt.plot(self.xG[0], self.xG[1], "gs") 70 | plt.plot(obs_x, obs_y, "sk") 71 | plt.title(name) 72 | plt.axis("equal") 73 | 74 | def plot_visited(self, visited, cl='gray'): 75 | if self.xI in visited: 76 | visited.remove(self.xI) 77 | 78 | if self.xG in visited: 79 | visited.remove(self.xG) 80 | 81 | count = 0 82 | 83 | for x in visited: 84 | count += 1 85 | plt.plot(x[0], x[1], color=cl, marker='o') 86 | plt.gcf().canvas.mpl_connect('key_release_event', 87 | lambda event: [exit(0) if event.key == 'escape' else None]) 88 | 89 | if count < len(visited) / 3: 90 | length = 20 91 | elif count < len(visited) * 2 / 3: 92 | length = 30 93 | else: 94 | length = 40 95 | # 96 | # length = 15 97 | 98 | if count % length == 0: 99 | plt.pause(0.001) 100 | plt.pause(0.01) 101 | 102 | def plot_path(self, path, cl='r', flag=False): 103 | path_x = [path[i][0] for i in range(len(path))] 104 | path_y = [path[i][1] for i in range(len(path))] 105 | 106 | if not flag: 107 | plt.plot(path_x, path_y, linewidth='3', color='r') 108 | else: 109 | plt.plot(path_x, path_y, linewidth='3', color=cl) 110 | 111 | plt.plot(self.xI[0], self.xI[1], "bs") 112 | plt.plot(self.xG[0], self.xG[1], "gs") 113 | 114 | plt.pause(0.01) 115 | 116 | def plot_visited_bi(self, v_fore, v_back): 117 | if self.xI in v_fore: 118 | v_fore.remove(self.xI) 119 | 120 | if self.xG in v_back: 121 | v_back.remove(self.xG) 122 | 123 | len_fore, len_back = len(v_fore), len(v_back) 124 | 125 | for k in range(max(len_fore, len_back)): 126 | if k < len_fore: 127 | plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o') 128 | if k < len_back: 129 | plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o') 130 | 131 | plt.gcf().canvas.mpl_connect('key_release_event', 132 | lambda event: [exit(0) if event.key == 'escape' else None]) 133 | 134 | if k % 10 == 0: 135 | plt.pause(0.001) 136 | plt.pause(0.01) 137 | 138 | @staticmethod 139 | def color_list(): 140 | cl_v = ['silver', 141 | 'wheat', 142 | 'lightskyblue', 143 | 'royalblue', 144 | 'slategray'] 145 | cl_p = ['gray', 146 | 'orange', 147 | 'deepskyblue', 148 | 'red', 149 | 'm'] 150 | return cl_v, cl_p 151 | 152 | @staticmethod 153 | def color_list_2(): 154 | cl = ['silver', 155 | 'steelblue', 156 | 'dimgray', 157 | 'cornflowerblue', 158 | 'dodgerblue', 159 | 'royalblue', 160 | 'plum', 161 | 'mediumslateblue', 162 | 'mediumpurple', 163 | 'blueviolet', 164 | ] 165 | return cl 166 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_2D/queue.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import heapq 3 | 4 | 5 | class QueueFIFO: 6 | """ 7 | Class: QueueFIFO 8 | Description: QueueFIFO is designed for First-in-First-out rule. 9 | """ 10 | 11 | def __init__(self): 12 | self.queue = collections.deque() 13 | 14 | def empty(self): 15 | return len(self.queue) == 0 16 | 17 | def put(self, node): 18 | self.queue.append(node) # enter from back 19 | 20 | def get(self): 21 | return self.queue.popleft() # leave from front 22 | 23 | 24 | class QueueLIFO: 25 | """ 26 | Class: QueueLIFO 27 | Description: QueueLIFO is designed for Last-in-First-out rule. 28 | """ 29 | 30 | def __init__(self): 31 | self.queue = collections.deque() 32 | 33 | def empty(self): 34 | return len(self.queue) == 0 35 | 36 | def put(self, node): 37 | self.queue.append(node) # enter from back 38 | 39 | def get(self): 40 | return self.queue.pop() # leave from back 41 | 42 | 43 | class QueuePrior: 44 | """ 45 | Class: QueuePrior 46 | Description: QueuePrior reorders elements using value [priority] 47 | """ 48 | 49 | def __init__(self): 50 | self.queue = [] 51 | 52 | def empty(self): 53 | return len(self.queue) == 0 54 | 55 | def put(self, item, priority): 56 | heapq.heappush(self.queue, (priority, item)) # reorder s using priority 57 | 58 | def get(self): 59 | return heapq.heappop(self.queue)[1] # pop out the smallest item 60 | 61 | def enumerate(self): 62 | return self.queue 63 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/Astar3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional A* algo 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | import os 11 | import sys 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 14 | from Search_3D.env3D import env 15 | from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ 16 | cost, children, StateSpace, heuristic_fun 17 | from Search_3D.plot_util3D import visualization 18 | import queue 19 | import time 20 | 21 | class Weighted_A_star(object): 22 | def __init__(self, resolution=0.5): 23 | self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ 24 | (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ 25 | (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ 26 | (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ 27 | (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ 28 | (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ 29 | (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ 30 | (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ 31 | (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} 32 | self.settings = 'NonCollisionChecking' # 'NonCollisionChecking' or 'CollisionChecking' 33 | self.env = env(resolution=resolution) 34 | self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) 35 | self.g = {self.start:0,self.goal:np.inf} 36 | self.Parent = {} 37 | self.CLOSED = set() 38 | self.V = [] 39 | self.done = False 40 | self.Path = [] 41 | self.ind = 0 42 | self.x0, self.xt = self.start, self.goal 43 | self.OPEN = queue.MinheapPQ() # store [point,priority] 44 | self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h 45 | self.lastpoint = self.x0 46 | 47 | def run(self, N=None): 48 | xt = self.xt 49 | xi = self.x0 50 | while self.OPEN: # while xt not reached and open is not empty 51 | xi = self.OPEN.get() 52 | if xi not in self.CLOSED: 53 | self.V.append(np.array(xi)) 54 | self.CLOSED.add(xi) # add the point in CLOSED set 55 | if getDist(xi,xt) < self.env.resolution: 56 | break 57 | # visualization(self) 58 | for xj in children(self,xi): 59 | # if xj not in self.CLOSED: 60 | if xj not in self.g: 61 | self.g[xj] = np.inf 62 | else: 63 | pass 64 | a = self.g[xi] + cost(self, xi, xj) 65 | if a < self.g[xj]: 66 | self.g[xj] = a 67 | self.Parent[xj] = xi 68 | # assign or update the priority in the open 69 | self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) 70 | # For specified expanded nodes, used primarily in LRTA* 71 | if N: 72 | if len(self.CLOSED) % N == 0: 73 | break 74 | if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V))) 75 | self.ind += 1 76 | 77 | self.lastpoint = xi 78 | # if the path finding is finished 79 | if self.lastpoint in self.CLOSED: 80 | self.done = True 81 | self.Path = self.path() 82 | if N is None: 83 | visualization(self) 84 | plt.show() 85 | return True 86 | 87 | return False 88 | 89 | def path(self): 90 | path = [] 91 | x = self.lastpoint 92 | start = self.x0 93 | while x != start: 94 | path.append([x, self.Parent[x]]) 95 | x = self.Parent[x] 96 | # path = np.flip(path, axis=0) 97 | return path 98 | 99 | # utility used in LRTA* 100 | def reset(self, xj): 101 | self.g = g_Space(self) # key is the point, store g value 102 | self.start = xj 103 | self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0 104 | self.x0 = xj 105 | self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h 106 | self.CLOSED = set() 107 | 108 | # self.h = h(self.Space, self.goal) 109 | 110 | 111 | if __name__ == '__main__': 112 | 113 | Astar = Weighted_A_star(0.5) 114 | sta = time.time() 115 | Astar.run() 116 | print(time.time() - sta) -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/Dstar3D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | import os 5 | import sys 6 | from collections import defaultdict 7 | 8 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 9 | from Search_3D.env3D import env 10 | from Search_3D import Astar3D 11 | from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \ 12 | initcost 13 | from Search_3D.plot_util3D import visualization 14 | 15 | 16 | class D_star(object): 17 | def __init__(self, resolution=1): 18 | self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ 19 | (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ 20 | (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ 21 | (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ 22 | (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ 23 | (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ 24 | (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ 25 | (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ 26 | (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} 27 | self.settings = 'CollisionChecking' 28 | self.env = env(resolution=resolution) 29 | self.X = StateSpace(self.env) 30 | self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal) 31 | # self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) 32 | self.b = defaultdict(lambda: defaultdict(dict)) # back pointers every state has one except xt. 33 | self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value 34 | self.h = {} # estimate from a point to the end point 35 | self.tag = {} # set all states to new 36 | self.V = set() # vertice in closed 37 | # for visualization 38 | self.ind = 0 39 | self.Path = [] 40 | self.done = False 41 | self.Obstaclemap = {} 42 | 43 | def checkState(self, y): 44 | if y not in self.h: 45 | self.h[y] = 0 46 | if y not in self.tag: 47 | self.tag[y] = 'New' 48 | 49 | def get_kmin(self): 50 | # get the minimum of the k val in OPEN 51 | # -1 if it does not exist 52 | if self.OPEN: 53 | return min(self.OPEN.values()) 54 | return -1 55 | 56 | def min_state(self): 57 | # returns the state in OPEN with min k(.) 58 | # if empty, returns None and -1 59 | # it also removes this min value form the OPEN set. 60 | if self.OPEN: 61 | minvalue = min(self.OPEN.values()) 62 | for k in self.OPEN.keys(): 63 | if self.OPEN[k] == minvalue: 64 | return k, self.OPEN.pop(k) 65 | return None, -1 66 | 67 | def insert(self, x, h_new): 68 | # inserting a key and value into OPEN list (s, kx) 69 | # depending on following situations 70 | if self.tag[x] == 'New': 71 | kx = h_new 72 | if self.tag[x] == 'Open': 73 | kx = min(self.OPEN[x], h_new) 74 | if self.tag[x] == 'Closed': 75 | kx = min(self.h[x], h_new) 76 | self.OPEN[x] = kx 77 | self.h[x], self.tag[x] = h_new, 'Open' 78 | 79 | def process_state(self): 80 | # main function of the D star algorithm, perform the process state 81 | # around the old path when needed. 82 | x, kold = self.min_state() 83 | self.tag[x] = 'Closed' 84 | self.V.add(x) 85 | if x is None: 86 | return -1 87 | # check if 1st timer s 88 | self.checkState(x) 89 | if kold < self.h[x]: # raised states 90 | for y in children(self, x): 91 | # check y 92 | self.checkState(y) 93 | a = self.h[y] + cost(self, y, x) 94 | if self.h[y] <= kold and self.h[x] > a: 95 | self.b[x], self.h[x] = y, a 96 | if kold == self.h[x]: # lower 97 | for y in children(self, x): 98 | # check y 99 | self.checkState(y) 100 | bb = self.h[x] + cost(self, x, y) 101 | if self.tag[y] == 'New' or \ 102 | (self.b[y] == x and self.h[y] != bb) or \ 103 | (self.b[y] != x and self.h[y] > bb): 104 | self.b[y] = x 105 | self.insert(y, bb) 106 | else: 107 | for y in children(self, x): 108 | # check y 109 | self.checkState(y) 110 | bb = self.h[x] + cost(self, x, y) 111 | if self.tag[y] == 'New' or \ 112 | (self.b[y] == x and self.h[y] != bb): 113 | self.b[y] = x 114 | self.insert(y, bb) 115 | else: 116 | if self.b[y] != x and self.h[y] > bb: 117 | self.insert(x, self.h[x]) 118 | else: 119 | if self.b[y] != x and self.h[y] > bb and \ 120 | self.tag[y] == 'Closed' and self.h[y] == kold: 121 | self.insert(y, self.h[y]) 122 | return self.get_kmin() 123 | 124 | def modify_cost(self, x): 125 | xparent = self.b[x] 126 | if self.tag[x] == 'Closed': 127 | self.insert(x, self.h[xparent] + cost(self, x, xparent)) 128 | 129 | def modify(self, x): 130 | self.modify_cost(x) 131 | while True: 132 | kmin = self.process_state() 133 | # visualization(self) 134 | if kmin >= self.h[x]: 135 | break 136 | 137 | def path(self, goal=None): 138 | path = [] 139 | if not goal: 140 | x = self.x0 141 | else: 142 | x = goal 143 | start = self.xt 144 | while x != start: 145 | path.append([np.array(x), np.array(self.b[x])]) 146 | x = self.b[x] 147 | return path 148 | 149 | def run(self): 150 | # put G (ending state) into the OPEN list 151 | self.OPEN[self.xt] = 0 152 | self.tag[self.x0] = 'New' 153 | # first run 154 | while True: 155 | # TODO: self.x0 = 156 | self.process_state() 157 | # visualization(self) 158 | if self.tag[self.x0] == "Closed": 159 | break 160 | self.ind += 1 161 | self.Path = self.path() 162 | self.done = True 163 | visualization(self) 164 | plt.pause(0.2) 165 | # plt.show() 166 | # when the environemnt changes over time 167 | 168 | for i in range(5): 169 | self.env.move_block(a=[0, -0.50, 0], s=0.5, block_to_move=1, mode='translation') 170 | self.env.move_block(a=[-0.25, 0, 0], s=0.5, block_to_move=0, mode='translation') 171 | # travel from end to start 172 | s = tuple(self.env.start) 173 | # self.V = set() 174 | while s != self.xt: 175 | if s == tuple(self.env.start): 176 | sparent = self.b[self.x0] 177 | else: 178 | sparent = self.b[s] 179 | # if there is a change of Cost, or a collision. 180 | if cost(self, s, sparent) == np.inf: 181 | self.modify(s) 182 | continue 183 | self.ind += 1 184 | s = sparent 185 | self.Path = self.path() 186 | visualization(self) 187 | plt.show() 188 | 189 | 190 | if __name__ == '__main__': 191 | D = D_star(1) 192 | D.run() 193 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/LP_Astar3D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | import os 5 | import sys 6 | 7 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 8 | from Search_3D.env3D import env 9 | from Search_3D import Astar3D 10 | from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \ 11 | cost, obstacleFree, isCollide 12 | from Search_3D.plot_util3D import visualization 13 | import queue 14 | import pyrr 15 | import time 16 | 17 | 18 | class Lifelong_Astar(object): 19 | def __init__(self,resolution = 1): 20 | self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ 21 | (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ 22 | (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ 23 | (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ 24 | (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ 25 | (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ 26 | (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ 27 | (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ 28 | (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} 29 | self.env = env(resolution=resolution) 30 | self.g = g_Space(self) 31 | self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) 32 | self.x0, self.xt = self.start, self.goal 33 | self.rhs = g_Space(self) # rhs(.) = g(.) = inf 34 | self.rhs[self.start] = 0 # rhs(x0) = 0 35 | self.h = Heuristic(self.g, self.goal) 36 | 37 | self.OPEN = queue.MinheapPQ() # store [point,priority] 38 | self.OPEN.put(self.x0, [self.h[self.x0],0]) 39 | self.CLOSED = set() 40 | 41 | # used for A* 42 | self.done = False 43 | self.Path = [] 44 | self.V = [] 45 | self.ind = 0 46 | 47 | # initialize children list 48 | self.CHILDREN = {} 49 | self.getCHILDRENset() 50 | 51 | # initialize Cost list 52 | self.COST = {} 53 | _ = self.costset() 54 | 55 | def costset(self): 56 | NodeToChange = set() 57 | for xi in self.CHILDREN.keys(): 58 | children = self.CHILDREN[xi] 59 | toUpdate = [self.cost(xj,xi) for xj in children] 60 | if xi in self.COST: 61 | # if the old Cost not equal to new Cost 62 | diff = np.not_equal(self.COST[xi],toUpdate) 63 | cd = np.array(children)[diff] 64 | for i in cd: 65 | NodeToChange.add(tuple(i)) 66 | self.COST[xi] = toUpdate 67 | else: 68 | self.COST[xi] = toUpdate 69 | return NodeToChange 70 | 71 | def getCOSTset(self,xi,xj): 72 | ind, children = 0, self.CHILDREN[xi] 73 | for i in children: 74 | if i == xj: 75 | return self.COST[xi][ind] 76 | ind += 1 77 | 78 | 79 | def children(self, x): 80 | allchild = [] 81 | resolution = self.env.resolution 82 | for direc in self.Alldirec: 83 | child = tuple(map(np.add,x,np.multiply(direc,resolution))) 84 | if isinbound(self.env.boundary,child): 85 | allchild.append(child) 86 | return allchild 87 | 88 | def getCHILDRENset(self): 89 | for xi in self.g.keys(): 90 | self.CHILDREN[xi] = self.children(xi) 91 | 92 | def isCollide(self, x, child): 93 | ray , dist = getRay(x, child) , getDist(x, child) 94 | if not isinbound(self.env.boundary,child): 95 | return True, dist 96 | for i in self.env.AABB_pyrr: 97 | shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) 98 | if shot is not None: 99 | dist_wall = getDist(x, shot) 100 | if dist_wall <= dist: # collide 101 | return True, dist 102 | for i in self.env.balls: 103 | if isinball(i, child): 104 | return True, dist 105 | shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) 106 | if shot != []: 107 | dists_ball = [getDist(x, j) for j in shot] 108 | if all(dists_ball <= dist): # collide 109 | return True, dist 110 | return False, dist 111 | 112 | def cost(self, x, y): 113 | collide, dist = isCollide(self, x, y) 114 | if collide: return np.inf 115 | else: return dist 116 | 117 | def key(self,xi,epsilion = 1): 118 | return [min(self.g[xi],self.rhs[xi]) + epsilion*self.h[xi],min(self.g[xi],self.rhs[xi])] 119 | 120 | def path(self): 121 | path = [] 122 | x = self.xt 123 | start = self.x0 124 | ind = 0 125 | while x != start: 126 | j = x 127 | nei = self.CHILDREN[x] 128 | gset = [self.g[xi] for xi in nei] 129 | # collision check and make g Cost inf 130 | for i in range(len(nei)): 131 | if isCollide(self, nei[i],j)[0]: 132 | gset[i] = np.inf 133 | parent = nei[np.argmin(gset)] 134 | path.append([x, parent]) 135 | x = parent 136 | if ind > 100: 137 | break 138 | ind += 1 139 | return path 140 | 141 | #------------------Lifelong Plannning A* 142 | def UpdateMembership(self, xi, xparent=None): 143 | if xi != self.x0: 144 | self.rhs[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]]) 145 | self.OPEN.check_remove(xi) 146 | if self.g[xi] != self.rhs[xi]: 147 | self.OPEN.put(xi,self.key(xi)) 148 | 149 | def ComputePath(self): 150 | print('computing path ...') 151 | while self.key(self.xt) > self.OPEN.top_key() or self.rhs[self.xt] != self.g[self.xt]: 152 | xi = self.OPEN.get() 153 | # if g > rhs, overconsistent 154 | if self.g[xi] > self.rhs[xi]: 155 | self.g[xi] = self.rhs[xi] 156 | # add xi to expanded node set 157 | if xi not in self.CLOSED: 158 | self.V.append(xi) 159 | self.CLOSED.add(xi) 160 | else: # underconsistent and consistent 161 | self.g[xi] = np.inf 162 | self.UpdateMembership(xi) 163 | for xj in self.CHILDREN[xi]: 164 | self.UpdateMembership(xj) 165 | 166 | # visualization(self) 167 | self.ind += 1 168 | self.Path = self.path() 169 | self.done = True 170 | visualization(self) 171 | plt.pause(1) 172 | 173 | def change_env(self): 174 | _, _ = self.env.move_block(block_to_move=1,a = [0,2,0]) 175 | self.done = False 176 | self.Path = [] 177 | self.CLOSED = set() 178 | N = self.costset() 179 | for xi in N: 180 | self.UpdateMembership(xi) 181 | 182 | if __name__ == '__main__': 183 | sta = time.time() 184 | Astar = Lifelong_Astar(1) 185 | Astar.ComputePath() 186 | for i in range(5): 187 | Astar.change_env() 188 | Astar.ComputePath() 189 | plt.pause(1) 190 | 191 | print(time.time() - sta) -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/LRT_Astar3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional N>1 LRTA* algo 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | import os 11 | import sys 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 14 | from Search_3D.env3D import env 15 | from Search_3D import Astar3D 16 | from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ 17 | cost, obstacleFree, children 18 | from Search_3D.plot_util3D import visualization 19 | import queue 20 | 21 | class LRT_A_star2: 22 | def __init__(self, resolution=0.5, N=7): 23 | self.N = N 24 | self.Astar = Astar3D.Weighted_A_star(resolution=resolution) 25 | self.path = [] 26 | 27 | def updateHeuristic(self): 28 | # Initialize hvalues at infinity 29 | for xi in self.Astar.CLOSED: 30 | self.Astar.h[xi] = np.inf 31 | Diff = True 32 | while Diff: # repeat DP until converge 33 | hvals, lasthvals = [], [] 34 | for xi in self.Astar.CLOSED: 35 | lasthvals.append(self.Astar.h[xi]) 36 | # update h values if they are smaller 37 | Children = children(self.Astar,xi) 38 | minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) 39 | # h(s) = h(s') if h(s) > cBest(s,s') + h(s') 40 | if self.Astar.h[xi] >= minfval: 41 | self.Astar.h[xi] = minfval 42 | hvals.append(self.Astar.h[xi]) 43 | if lasthvals == hvals: Diff = False 44 | 45 | def move(self): 46 | st = self.Astar.x0 47 | ind = 0 48 | # find the lowest path down hill 49 | while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop 50 | Children = children(self.Astar,st) 51 | minh, minchild = np.inf, None 52 | for child in Children: 53 | # check collision here, not a supper efficient 54 | collide, _ = isCollide(self.Astar,st, child) 55 | if collide: 56 | continue 57 | h = self.Astar.h[child] 58 | if h <= minh: 59 | minh, minchild = h, child 60 | self.path.append([st, minchild]) 61 | st = minchild 62 | for (_, p) in self.Astar.OPEN.enumerate(): 63 | if p == st: 64 | break 65 | ind += 1 66 | if ind > 1000: 67 | break 68 | self.Astar.reset(st) 69 | 70 | def run(self): 71 | while True: 72 | if self.Astar.run(N=self.N): 73 | self.Astar.Path = self.Astar.Path + self.path 74 | self.Astar.done = True 75 | visualization(self.Astar) 76 | plt.show() 77 | break 78 | self.updateHeuristic() 79 | self.move() 80 | 81 | 82 | if __name__ == '__main__': 83 | T = LRT_A_star2(resolution=0.5, N=100) 84 | T.run() 85 | -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/RTA_Astar3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional Real-time Adaptive LRTA* algo 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | import os 11 | import sys 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 14 | from Search_3D.env3D import env 15 | from Search_3D import Astar3D 16 | from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ 17 | cost, obstacleFree, children 18 | from Search_3D.plot_util3D import visualization 19 | import queue 20 | 21 | class RTA_A_star: 22 | def __init__(self, resolution=0.5, N=7): 23 | self.N = N # node to expand 24 | self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star 25 | self.path = [] # empty path 26 | self.st = [] 27 | self.localhvals = [] 28 | 29 | def updateHeuristic(self): 30 | # Initialize hvalues at infinity 31 | self.localhvals = [] 32 | nodeset, vals = [], [] 33 | for (_,_,xi) in self.Astar.OPEN.enumerate(): 34 | nodeset.append(xi) 35 | vals.append(self.Astar.g[xi] + self.Astar.h[xi]) 36 | j, fj = nodeset[np.argmin(vals)], min(vals) 37 | self.st = j 38 | # single pass update of hvals 39 | for xi in self.Astar.CLOSED: 40 | self.Astar.h[xi] = fj - self.Astar.g[xi] 41 | self.localhvals.append(self.Astar.h[xi]) 42 | 43 | def move(self): 44 | st, localhvals = self.st, self.localhvals 45 | maxhval = max(localhvals) 46 | sthval = self.Astar.h[st] 47 | # find the lowest path up hill 48 | while sthval < maxhval: 49 | parentsvals , parents = [] , [] 50 | # find the max child 51 | for xi in children(self.Astar,st): 52 | if xi in self.Astar.CLOSED: 53 | parents.append(xi) 54 | parentsvals.append(self.Astar.h[xi]) 55 | lastst = st 56 | st = parents[np.argmax(parentsvals)] 57 | self.path.append([st,lastst]) # add to path 58 | sthval = self.Astar.h[st] 59 | self.Astar.reset(self.st) 60 | 61 | def run(self): 62 | while True: 63 | if self.Astar.run(N=self.N): 64 | self.Astar.Path = self.Astar.Path + self.path 65 | self.Astar.done = True 66 | visualization(self.Astar) 67 | plt.show() 68 | break 69 | self.updateHeuristic() 70 | self.move() 71 | 72 | 73 | if __name__ == '__main__': 74 | T = RTA_A_star(resolution=1, N=100) 75 | T.run() -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/bidirectional_Astar3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional bidirectional A* algo 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from collections import defaultdict 10 | 11 | import os 12 | import sys 13 | 14 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") 15 | from Search_3D.env3D import env 16 | from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun 17 | from Search_3D.plot_util3D import visualization 18 | import queue 19 | 20 | 21 | class Weighted_A_star(object): 22 | def __init__(self,resolution=0.5): 23 | self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \ 24 | (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \ 25 | (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \ 26 | (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \ 27 | (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \ 28 | (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \ 29 | (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \ 30 | (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ 31 | (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} 32 | self.env = env(resolution = resolution) 33 | self.settings = 'NonCollisionChecking' 34 | self.start, self.goal = tuple(self.env.start), tuple(self.env.goal) 35 | self.g = {self.start:0,self.goal:0} 36 | self.OPEN1 = queue.MinheapPQ() # store [point,priority] 37 | self.OPEN2 = queue.MinheapPQ() 38 | self.Parent1, self.Parent2 = {}, {} 39 | self.CLOSED1, self.CLOSED2 = set(), set() 40 | self.V = [] 41 | self.done = False 42 | self.Path = [] 43 | 44 | def run(self): 45 | x0, xt = self.start, self.goal 46 | self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h 47 | self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h 48 | self.ind = 0 49 | while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty 50 | xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() 51 | self.CLOSED1.add(xi1) # add the point in CLOSED set 52 | self.CLOSED2.add(xi2) 53 | self.V.append(xi1) 54 | self.V.append(xi2) 55 | # visualization(self) 56 | allchild1, allchild2 = children(self,xi1), children(self,xi2) 57 | self.evaluation(allchild1,xi1,conf=1) 58 | self.evaluation(allchild2,xi2,conf=2) 59 | if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) 60 | self.ind += 1 61 | self.common = self.CLOSED1.intersection(self.CLOSED2) 62 | self.done = True 63 | self.Path = self.path() 64 | visualization(self) 65 | plt.show() 66 | 67 | def evaluation(self, allchild, xi, conf): 68 | for xj in allchild: 69 | if conf == 1: 70 | if xj not in self.CLOSED1: 71 | if xj not in self.g: 72 | self.g[xj] = np.inf 73 | else: 74 | pass 75 | gi = self.g[xi] 76 | a = gi + cost(self,xi,xj) 77 | if a < self.g[xj]: 78 | self.g[xj] = a 79 | self.Parent1[xj] = xi 80 | self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal)) 81 | if conf == 2: 82 | if xj not in self.CLOSED2: 83 | if xj not in self.g: 84 | self.g[xj] = np.inf 85 | else: 86 | pass 87 | gi = self.g[xi] 88 | a = gi + cost(self,xi,xj) 89 | if a < self.g[xj]: 90 | self.g[xj] = a 91 | self.Parent2[xj] = xi 92 | self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start)) 93 | 94 | def path(self): 95 | # TODO: fix path 96 | path = [] 97 | goal = self.goal 98 | start = self.start 99 | x = list(self.common)[0] 100 | while x != start: 101 | path.append([x,self.Parent1[x]]) 102 | x = self.Parent1[x] 103 | x = list(self.common)[0] 104 | while x != goal: 105 | path.append([x,self.Parent2[x]]) 106 | x = self.Parent2[x] 107 | path = np.flip(path,axis=0) 108 | return path 109 | 110 | if __name__ == '__main__': 111 | Astar = Weighted_A_star(0.5) 112 | Astar.run() -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/env3D.py: -------------------------------------------------------------------------------- 1 | # this is the three dimensional space 2 | # !/usr/bin/env python3 3 | # -*- coding: utf-8 -*- 4 | """ 5 | @author: yue qi 6 | """ 7 | import numpy as np 8 | # from utils3D import OBB2AABB 9 | 10 | def R_matrix(z_angle,y_angle,x_angle): 11 | # s angle: row; y angle: pitch; z angle: yaw 12 | # generate rotation matrix in SO3 13 | # RzRyRx = R, ZYX intrinsic rotation 14 | # also (r1,r2,r3) in R3*3 in {W} frame 15 | # used in obb.O 16 | # [[R p] 17 | # [0T 1]] gives transformation from body to world 18 | 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]])@ \ 19 | 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)]])@ \ 20 | 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)]]) 21 | 22 | def getblocks(): 23 | # AABBs 24 | block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00], 25 | [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00], 26 | [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00], 27 | [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00], 28 | [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]] 29 | Obstacles = [] 30 | for i in block: 31 | i = np.array(i) 32 | Obstacles.append([j for j in i]) 33 | return np.array(Obstacles) 34 | 35 | def getballs(): 36 | spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]] 37 | Obstacles = [] 38 | for i in spheres: 39 | Obstacles.append([j for j in i]) 40 | return np.array(Obstacles) 41 | 42 | def getAABB(blocks): 43 | # used for Pyrr package for detecting collision 44 | AABB = [] 45 | for i in blocks: 46 | AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger 47 | return AABB 48 | 49 | def getAABB2(blocks): 50 | # used in lineAABB 51 | AABB = [] 52 | for i in blocks: 53 | AABB.append(aabb(i)) 54 | return AABB 55 | 56 | def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): 57 | return block 58 | 59 | class aabb(object): 60 | # make AABB out of blocks, 61 | # P: center point 62 | # E: extents 63 | # O: Rotation matrix in SO(3), in {w} 64 | def __init__(self,AABB): 65 | self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point 66 | self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents 67 | self.O = [[1,0,0],[0,1,0],[0,0,1]] 68 | 69 | class obb(object): 70 | # P: center point 71 | # E: extents 72 | # O: Rotation matrix in SO(3), in {w} 73 | def __init__(self, P, E, O): 74 | self.P = P 75 | self.E = E 76 | self.O = O 77 | self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) 78 | 79 | class env(): 80 | def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1): 81 | # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): 82 | self.resolution = resolution 83 | self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 84 | self.blocks = getblocks() 85 | self.AABB = getAABB2(self.blocks) 86 | self.AABB_pyrr = getAABB(self.blocks) 87 | self.balls = getballs() 88 | self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)), 89 | obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))]) 90 | self.start = np.array([2.0, 2.0, 2.0]) 91 | self.goal = np.array([6.0, 16.0, 0.0]) 92 | self.t = 0 # time 93 | 94 | def New_block(self): 95 | newblock = add_block() 96 | self.blocks = np.vstack([self.blocks,newblock]) 97 | self.AABB = getAABB2(self.blocks) 98 | self.AABB_pyrr = getAABB(self.blocks) 99 | 100 | def move_start(self, x): 101 | self.start = x 102 | 103 | def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'): 104 | # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 105 | # R is an orthorgonal transform in R3*3, is the rotation matrix 106 | # (s',t') = (s + tv, t) is uniform transformation 107 | # (s',t') = (s + a, t + s) is a translation 108 | if mode == 'translation': 109 | ori = np.array(self.blocks[block_to_move]) 110 | self.blocks[block_to_move] = \ 111 | np.array([ori[0] + a[0],\ 112 | ori[1] + a[1],\ 113 | ori[2] + a[2],\ 114 | ori[3] + a[0],\ 115 | ori[4] + a[1],\ 116 | ori[5] + a[2]]) 117 | 118 | self.AABB[block_to_move].P = \ 119 | [self.AABB[block_to_move].P[0] + a[0], \ 120 | self.AABB[block_to_move].P[1] + a[1], \ 121 | self.AABB[block_to_move].P[2] + a[2]] 122 | self.t += s 123 | # return a range of block that the block might moved 124 | a = self.blocks[block_to_move] 125 | return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ 126 | a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ 127 | np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ 128 | ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) 129 | # return a,ori 130 | # (s',t') = (Rx, t) 131 | def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]): 132 | # theta stands for rotational angles around three principle axis in world frame 133 | # translation stands for translation in the world frame 134 | ori = [self.OBB[obb_to_move]] 135 | self.OBB[obb_to_move].P = \ 136 | [self.OBB[obb_to_move].P[0] + translation[0], 137 | self.OBB[obb_to_move].P[1] + translation[1], 138 | self.OBB[obb_to_move].P[2] + translation[2]] 139 | # Calculate orientation 140 | self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) 141 | # generating transformation matrix 142 | self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\ 143 | -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]]) 144 | return self.OBB[obb_to_move], ori[0] 145 | 146 | if __name__ == '__main__': 147 | newenv = env() -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/plot_util3D.py: -------------------------------------------------------------------------------- 1 | # plotting 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d import Axes3D 4 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection 5 | import mpl_toolkits.mplot3d as plt3d 6 | from mpl_toolkits.mplot3d import proj3d 7 | import numpy as np 8 | 9 | def CreateSphere(center,r): 10 | u = np.linspace(0,2* np.pi,30) 11 | v = np.linspace(0,np.pi,30) 12 | x = np.outer(np.cos(u),np.sin(v)) 13 | y = np.outer(np.sin(u),np.sin(v)) 14 | z = np.outer(np.ones(np.size(u)),np.cos(v)) 15 | x, y, z = r*x + center[0], r*y + center[1], r*z + center[2] 16 | return (x,y,z) 17 | 18 | def draw_Spheres(ax,balls): 19 | for i in balls: 20 | (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) 21 | ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") 22 | 23 | def draw_block_list(ax, blocks ,color=None,alpha=0.15): 24 | ''' 25 | drawing the blocks on the graph 26 | ''' 27 | 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]], 28 | dtype='float') 29 | 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]]) 30 | n = blocks.shape[0] 31 | d = blocks[:, 3:6] - blocks[:, :3] 32 | vl = np.zeros((8 * n, 3)) 33 | fl = np.zeros((6 * n, 4), dtype='int64') 34 | for k in range(n): 35 | vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] 36 | fl[k * 6:(k + 1) * 6, :] = f + k * 8 37 | if type(ax) is Poly3DCollection: 38 | ax.set_verts(vl[fl]) 39 | else: 40 | pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') 41 | pc.set_facecolor(color) 42 | h = ax.add_collection3d(pc) 43 | return h 44 | 45 | def obb_verts(obb): 46 | # 0.017004013061523438 for 1000 iters 47 | ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ 48 | [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) 49 | # P + (ori * E) 50 | ori_body = np.multiply(ori_body,obb.E) 51 | # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) 52 | verts = (obb.O@ori_body.T).T + obb.P 53 | return verts 54 | 55 | 56 | def draw_obb(ax, OBB, color=None,alpha=0.15): 57 | 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]]) 58 | n = OBB.shape[0] 59 | vl = np.zeros((8 * n, 3)) 60 | fl = np.zeros((6 * n, 4), dtype='int64') 61 | for k in range(n): 62 | vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) 63 | fl[k * 6:(k + 1) * 6, :] = f + k * 8 64 | if type(ax) is Poly3DCollection: 65 | ax.set_verts(vl[fl]) 66 | else: 67 | pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') 68 | pc.set_facecolor(color) 69 | h = ax.add_collection3d(pc) 70 | return h 71 | 72 | 73 | def draw_line(ax,SET,visibility=1,color=None): 74 | if SET != []: 75 | for i in SET: 76 | xs = i[0][0], i[1][0] 77 | ys = i[0][1], i[1][1] 78 | zs = i[0][2], i[1][2] 79 | line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) 80 | ax.add_line(line) 81 | 82 | def visualization(initparams): 83 | if initparams.ind % 20 == 0 or initparams.done: 84 | V = np.array(list(initparams.V)) 85 | # E = initparams.E 86 | Path = np.array(initparams.Path) 87 | start = initparams.env.start 88 | goal = initparams.env.goal 89 | # edges = E.get_edge() 90 | # generate axis objects 91 | ax = plt.subplot(111, projection='3d') 92 | #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) 93 | #ax.view_init(elev=0., azim=90.) 94 | ax.view_init(elev=90., azim=0.) 95 | #ax.view_init(elev=-8., azim=180) 96 | ax.clear() 97 | # drawing objects 98 | draw_Spheres(ax, initparams.env.balls) 99 | draw_block_list(ax, initparams.env.blocks) 100 | if initparams.env.OBB is not None: 101 | draw_obb(ax,initparams.env.OBB) 102 | draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) 103 | # draw_line(ax,edges,visibility=0.25) 104 | draw_line(ax,Path,color='r') 105 | if len(V) > 0: 106 | ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) 107 | ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') 108 | ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') 109 | # adjust the aspect ratio 110 | set_axes_equal(ax) 111 | make_transparent(ax) 112 | # plt.xlabel('s') 113 | # plt.ylabel('y') 114 | plt.pause(0.0001) 115 | 116 | def set_axes_equal(ax): 117 | '''Make axes of 3D plot have equal scale so that spheres appear as spheres, 118 | cubes as cubes, etc.. This is one possible solution to Matplotlib's 119 | ax.set_aspect('equal') and ax.axis('equal') not working for 3D. 120 | https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to 121 | Input 122 | ax: a matplotlib axis, e.g., as output from plt.gca(). 123 | ''' 124 | 125 | x_limits = ax.get_xlim3d() 126 | y_limits = ax.get_ylim3d() 127 | z_limits = ax.get_zlim3d() 128 | 129 | x_range = abs(x_limits[1] - x_limits[0]) 130 | x_middle = np.mean(x_limits) 131 | y_range = abs(y_limits[1] - y_limits[0]) 132 | y_middle = np.mean(y_limits) 133 | z_range = abs(z_limits[1] - z_limits[0]) 134 | z_middle = np.mean(z_limits) 135 | 136 | # The plot bounding box is a sphere in the sense of the infinity 137 | # norm, hence I call half the max range the plot radius. 138 | plot_radius = 0.5*max([x_range, y_range, z_range]) 139 | 140 | ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) 141 | ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) 142 | ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) 143 | 144 | def make_transparent(ax): 145 | # make the panes transparent 146 | ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 147 | ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 148 | ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0)) 149 | # make the grid lines transparent 150 | ax.xaxis._axinfo["grid"]['color'] = (1,1,1,0) 151 | ax.yaxis._axinfo["grid"]['color'] = (1,1,1,0) 152 | ax.zaxis._axinfo["grid"]['color'] = (1,1,1,0) 153 | 154 | if __name__ == '__main__': 155 | pass -------------------------------------------------------------------------------- /Search_based_Planning/Search_3D/queue.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import heapq 3 | import itertools 4 | 5 | class QueueFIFO: 6 | """ 7 | Class: QueueFIFO 8 | Description: QueueFIFO is designed for First-in-First-out rule. 9 | """ 10 | 11 | def __init__(self): 12 | self.queue = collections.deque() 13 | 14 | def empty(self): 15 | return len(self.queue) == 0 16 | 17 | def put(self, node): 18 | self.queue.append(node) # enter from back 19 | 20 | def get(self): 21 | return self.queue.popleft() # leave from front 22 | 23 | 24 | class QueueLIFO: 25 | """ 26 | Class: QueueLIFO 27 | Description: QueueLIFO is designed for Last-in-First-out rule. 28 | """ 29 | 30 | def __init__(self): 31 | self.queue = collections.deque() 32 | 33 | def empty(self): 34 | return len(self.queue) == 0 35 | 36 | def put(self, node): 37 | self.queue.append(node) # enter from back 38 | 39 | def get(self): 40 | return self.queue.pop() # leave from back 41 | 42 | 43 | class QueuePrior: 44 | """ 45 | Class: QueuePrior 46 | Description: QueuePrior reorders elements using value [priority] 47 | """ 48 | 49 | def __init__(self): 50 | self.queue = [] 51 | 52 | def empty(self): 53 | return len(self.queue) == 0 54 | 55 | def put(self, item, priority): 56 | heapq.heappush(self.queue, (priority, item)) # reorder s using priority 57 | 58 | def get(self): 59 | return heapq.heappop(self.queue)[1] # pop out the smallest item 60 | 61 | def enumerate(self): 62 | return self.queue 63 | 64 | def check_remove(self, item): 65 | for (p, x) in self.queue: 66 | if item == x: 67 | self.queue.remove((p, x)) 68 | 69 | def top_key(self): 70 | return self.queue[0][0] 71 | 72 | class MinheapPQ: 73 | """ 74 | A priority queue based on min heap, which takes O(logn) on element removal 75 | https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes 76 | """ 77 | def __init__(self): 78 | self.pq = [] # lis of the entries arranged in a heap 79 | self.nodes = set() 80 | self.entry_finder = {} # mapping of the item entries 81 | self.counter = itertools.count() # unique sequence count 82 | self.REMOVED = '' 83 | 84 | def put(self, item, priority): 85 | '''add a new task or update the priority of an existing item''' 86 | if item in self.entry_finder: 87 | self.check_remove(item) 88 | count = next(self.counter) 89 | entry = [priority, count, item] 90 | self.entry_finder[item] = entry 91 | heapq.heappush(self.pq, entry) 92 | self.nodes.add(item) 93 | 94 | def check_remove(self, item): 95 | if item not in self.entry_finder: 96 | return 97 | entry = self.entry_finder.pop(item) 98 | entry[-1] = self.REMOVED 99 | self.nodes.remove(item) 100 | 101 | def get(self): 102 | """Remove and return the lowest priority task. Raise KeyError if empty.""" 103 | while self.pq: 104 | priority, count, item = heapq.heappop(self.pq) 105 | if item is not self.REMOVED: 106 | del self.entry_finder[item] 107 | self.nodes.remove(item) 108 | return item 109 | raise KeyError('pop from an empty priority queue') 110 | 111 | def top_key(self): 112 | return self.pq[0][0] 113 | 114 | def enumerate(self): 115 | return self.pq 116 | 117 | def allnodes(self): 118 | return self.nodes 119 | 120 | # class QueuePrior: 121 | # """ 122 | # Class: QueuePrior 123 | # Description: QueuePrior reorders elements using value [priority] 124 | # """ 125 | 126 | # def __init__(self): 127 | # self.queue = [] 128 | 129 | # def empty(self): 130 | # return len(self.queue) == 0 131 | 132 | # def put(self, item, priority): 133 | # count = 0 134 | # for (p, s) in self.queue: 135 | # if s == item: 136 | # self.queue[count] = (priority, item) 137 | # break 138 | # count += 1 139 | # if count == len(self.queue): 140 | # heapq.heappush(self.queue, (priority, item)) # reorder s using priority 141 | 142 | # def get(self): 143 | # return heapq.heappop(self.queue)[1] # pop out the smallest item 144 | 145 | # def enumerate(self): 146 | # return self.queue 147 | 148 | # def check_remove(self, item): 149 | # for (p, s) in self.queue: 150 | # if item == s: 151 | # self.queue.remove((p, s)) 152 | 153 | # def top_key(self): 154 | # return self.queue[0][0] -------------------------------------------------------------------------------- /Search_based_Planning/gif/ADstar_sig.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/ADstar_sig.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/ADstar_small.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/ADstar_small.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/ARA_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/ARA_star.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/Astar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/Astar.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/BF.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/BF.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/BFS.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/BFS.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/Bi-Astar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/Bi-Astar.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/DFS.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/DFS.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/D_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/D_star.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/D_star_Lite.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/D_star_Lite.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/Dijkstra.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/Dijkstra.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/LPA_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/LPA_star.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/LPAstar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/LPAstar.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/LRTA_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/LRTA_star.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/RTAA_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/RTAA_star.gif -------------------------------------------------------------------------------- /Search_based_Planning/gif/RepeatedA_star.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/PathPlanning/89c200e0ab855010e2f892b6465b50dab002963c/Search_based_Planning/gif/RepeatedA_star.gif --------------------------------------------------------------------------------