[ { "path": ".idea/.gitignore", "content": "\n# Default ignored files\n/workspace.xml" }, { "path": ".idea/PathPlanning.iml", "content": "\n\n \n \n \n \n \n \n \n" }, { "path": ".idea/inspectionProfiles/profiles_settings.xml", "content": "\n \n \n" }, { "path": ".idea/misc.xml", "content": "\n\n \n" }, { "path": ".idea/modules.xml", "content": "\n\n \n \n \n \n \n" }, { "path": ".idea/vcs.xml", "content": "\n\n \n \n \n" }, { "path": "CurvesGenerator/bezier_path.py", "content": "\"\"\"\nbezier path\n\nauthor: Atsushi Sakai(@Atsushi_twi)\nmodified: huiming zhou\n\"\"\"\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.special import comb\nimport draw\n\n\ndef calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset):\n\n dist = np.hypot(sx - gx, sy - gy) / offset\n control_points = np.array(\n [[sx, sy],\n [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],\n [gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)],\n [gx, gy]])\n\n path = calc_bezier_path(control_points, n_points=100)\n\n return path, control_points\n\n\ndef calc_bezier_path(control_points, n_points=100):\n traj = []\n\n for t in np.linspace(0, 1, n_points):\n traj.append(bezier(t, control_points))\n\n return np.array(traj)\n\n\ndef Comb(n, i, t):\n return comb(n, i) * t ** i * (1 - t) ** (n - i)\n\n\ndef bezier(t, control_points):\n n = len(control_points) - 1\n return np.sum([Comb(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)\n\n\ndef bezier_derivatives_control_points(control_points, n_derivatives):\n w = {0: control_points}\n\n for i in range(n_derivatives):\n n = len(w[i])\n w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])\n for j in range(n - 1)])\n\n return w\n\n\ndef curvature(dx, dy, ddx, ddy):\n return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)\n\n\ndef simulation():\n sx = [-3, 0, 4, 6]\n sy = [2, 0, 1.5, 6]\n\n ratio = np.linspace(0, 1, 100)\n pathx, pathy = [], []\n\n for t in ratio:\n x, y = [], []\n for i in range(len(sx) - 1):\n x.append(sx[i + 1] * t + sx[i] * (1 - t))\n y.append(sy[i + 1] * t + sy[i] * (1 - t))\n\n xx, yy = [], []\n for i in range(len(x) - 1):\n xx.append(x[i + 1] * t + x[i] * (1 - t))\n yy.append(y[i + 1] * t + y[i] * (1 - t))\n\n px = xx[1] * t + xx[0] * (1 - t)\n py = yy[1] * t + yy[0] * (1 - t)\n pathx.append(px)\n pathy.append(py)\n\n plt.cla()\n plt.plot(sx, sy, linestyle='-', marker='o', color='dimgray', label=\"Control Points\")\n plt.plot(x, y, color='dodgerblue')\n plt.plot(xx, yy, color='cyan')\n plt.plot(pathx, pathy, color='darkorange', linewidth=2, label=\"Bezier Path\")\n plt.plot(px, py, marker='o')\n plt.axis(\"equal\")\n plt.legend()\n plt.title(\"Cubic Bezier Curve demo\")\n plt.grid(True)\n plt.pause(0.001)\n\n plt.show()\n\n\ndef main():\n sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0)\n gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0)\n offset = 3.0\n\n path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset)\n\n t = 0.8 # Number in [0, 1]\n x_target, y_target = bezier(t, control_points)\n derivatives_cp = bezier_derivatives_control_points(control_points, 2)\n point = bezier(t, control_points)\n dt = bezier(t, derivatives_cp[1])\n ddt = bezier(t, derivatives_cp[2])\n # Radius of curv\n radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])\n # Normalize derivative\n dt /= np.linalg.norm(dt, 2)\n tangent = np.array([point, point + dt])\n normal = np.array([point, point + [- dt[1], dt[0]]])\n curvature_center = point + np.array([- dt[1], dt[0]]) * radius\n circle = plt.Circle(tuple(curvature_center), radius,\n color=(0, 0.8, 0.8), fill=False, linewidth=1)\n\n assert path.T[0][0] == sx, \"path is invalid\"\n assert path.T[1][0] == sy, \"path is invalid\"\n assert path.T[0][-1] == gx, \"path is invalid\"\n assert path.T[1][-1] == gy, \"path is invalid\"\n\n fig, ax = plt.subplots()\n ax.plot(path.T[0], path.T[1], label=\"Bezier Path\")\n ax.plot(control_points.T[0], control_points.T[1],\n '--o', label=\"Control Points\")\n ax.plot(x_target, y_target)\n ax.plot(tangent[:, 0], tangent[:, 1], label=\"Tangent\")\n ax.plot(normal[:, 0], normal[:, 1], label=\"Normal\")\n ax.add_artist(circle)\n draw.Arrow(sx, sy, syaw, 1, \"darkorange\")\n draw.Arrow(gx, gy, gyaw, 1, \"darkorange\")\n plt.grid(True)\n plt.title(\"Bezier Path: from Atsushi's work\")\n ax.axis(\"equal\")\n plt.show()\n\n\nif __name__ == '__main__':\n main()\n # simulation()\n" }, { "path": "CurvesGenerator/bspline_curve.py", "content": "\"\"\"\n\nPath Planner with B-Spline\n\nauthor: Atsushi Sakai (@Atsushi_twi)\n\n\"\"\"\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport scipy.interpolate as scipy_interpolate\nimport cubic_spline as cs\n\n\ndef approximate_b_spline_path(x, y, n_path_points, degree=3):\n t = range(len(x))\n x_tup = scipy_interpolate.splrep(t, x, k=degree)\n y_tup = scipy_interpolate.splrep(t, y, k=degree)\n\n x_list = list(x_tup)\n x_list[1] = x + [0.0, 0.0, 0.0, 0.0]\n\n y_list = list(y_tup)\n y_list[1] = y + [0.0, 0.0, 0.0, 0.0]\n\n ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)\n rx = scipy_interpolate.splev(ipl_t, x_list)\n ry = scipy_interpolate.splev(ipl_t, y_list)\n\n return rx, ry\n\n\ndef interpolate_b_spline_path(x, y, n_path_points, degree=3):\n ipl_t = np.linspace(0.0, len(x) - 1, len(x))\n spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)\n spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)\n\n travel = np.linspace(0.0, len(x) - 1, n_path_points)\n return spl_i_x(travel), spl_i_y(travel)\n\n\ndef main():\n print(__file__ + \" start!!\")\n # way points\n # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]\n # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]\n way_point_x = [-2, 2.0, 3.5, 5.5, 6.0, 8.0]\n way_point_y = [0, 2.7, -0.5, 0.5, 3.0, 4.0]\n\n sp = cs.Spline2D(way_point_x, way_point_y)\n s = np.arange(0, sp.s[-1], 0.1)\n\n rx, ry, ryaw, rk = [], [], [], []\n for i_s in s:\n ix, iy = sp.calc_position(i_s)\n rx.append(ix)\n ry.append(iy)\n ryaw.append(sp.calc_yaw(i_s))\n rk.append(sp.calc_curvature(i_s))\n\n n_course_point = 100 # sampling number\n rax, ray = approximate_b_spline_path(way_point_x, way_point_y,\n n_course_point)\n rix, riy = interpolate_b_spline_path(way_point_x, way_point_y,\n n_course_point)\n\n # show results\n plt.plot(way_point_x, way_point_y, '-og', label=\"Control Points\")\n plt.plot(rax, ray, '-r', label=\"Approximated B-Spline path\")\n plt.plot(rix, riy, '-b', label=\"Interpolated B-Spline path\")\n plt.plot(rx, ry, color='dimgray', label=\"Cubic Spline\")\n plt.grid(True)\n plt.title(\"Curves Comparison\")\n plt.legend()\n plt.axis(\"equal\")\n plt.show()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "CurvesGenerator/cubic_spline.py", "content": "#! /usr/bin/python\n# -*- coding: utf-8 -*-\nu\"\"\"\nCubic Spline library on python\n\nauthor Atsushi Sakai\n\nusage: see test codes as below\n\nlicense: MIT\n\"\"\"\nimport math\nimport numpy as np\nimport bisect\n\n\nclass Spline:\n u\"\"\"\n Cubic Spline class\n \"\"\"\n\n def __init__(self, x, y):\n self.b, self.c, self.d, self.w = [], [], [], []\n\n self.x = x\n self.y = y\n\n self.nx = len(x) # dimension of s\n h = np.diff(x)\n\n # calc coefficient cBest\n self.a = [iy for iy in y]\n\n # calc coefficient cBest\n A = self.__calc_A(h)\n B = self.__calc_B(h)\n self.c = np.linalg.solve(A, B)\n # print(self.c1)\n\n # calc spline coefficient b and d\n for i in range(self.nx - 1):\n self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))\n tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \\\n (self.c[i + 1] + 2.0 * self.c[i]) / 3.0\n self.b.append(tb)\n\n def calc(self, t):\n u\"\"\"\n Calc position\n\n if t is outside of the input s, return None\n\n \"\"\"\n\n if t < self.x[0]:\n return None\n elif t > self.x[-1]:\n return None\n\n i = self.__search_index(t)\n dx = t - self.x[i]\n result = self.a[i] + self.b[i] * dx + \\\n self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0\n\n return result\n\n def calcd(self, t):\n u\"\"\"\n Calc first derivative\n\n if t is outside of the input s, return None\n \"\"\"\n\n if t < self.x[0]:\n return None\n elif t > self.x[-1]:\n return None\n\n i = self.__search_index(t)\n dx = t - self.x[i]\n result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0\n return result\n\n def calcdd(self, t):\n u\"\"\"\n Calc second derivative\n \"\"\"\n\n if t < self.x[0]:\n return None\n elif t > self.x[-1]:\n return None\n\n i = self.__search_index(t)\n dx = t - self.x[i]\n result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx\n return result\n\n def __search_index(self, x):\n u\"\"\"\n search data segment index\n \"\"\"\n return bisect.bisect(self.x, x) - 1\n\n def __calc_A(self, h):\n u\"\"\"\n calc matrix A for spline coefficient cBest\n \"\"\"\n A = np.zeros((self.nx, self.nx))\n A[0, 0] = 1.0\n for i in range(self.nx - 1):\n if i != (self.nx - 2):\n A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])\n A[i + 1, i] = h[i]\n A[i, i + 1] = h[i]\n\n A[0, 1] = 0.0\n A[self.nx - 1, self.nx - 2] = 0.0\n A[self.nx - 1, self.nx - 1] = 1.0\n # print(A)\n return A\n\n def __calc_B(self, h):\n u\"\"\"\n calc matrix B for spline coefficient cBest\n \"\"\"\n B = np.zeros(self.nx)\n for i in range(self.nx - 2):\n B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \\\n h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]\n # print(B)\n return B\n\n\nclass Spline2D:\n u\"\"\"\n 2D Cubic Spline class\n\n \"\"\"\n\n def __init__(self, x, y):\n self.s = self.__calc_s(x, y)\n self.sx = Spline(self.s, x)\n self.sy = Spline(self.s, y)\n\n def __calc_s(self, x, y):\n dx = np.diff(x)\n dy = np.diff(y)\n self.ds = [math.sqrt(idx ** 2 + idy ** 2)\n for (idx, idy) in zip(dx, dy)]\n s = [0]\n s.extend(np.cumsum(self.ds))\n return s\n\n def calc_position(self, s):\n u\"\"\"\n calc position\n \"\"\"\n x = self.sx.calc(s)\n y = self.sy.calc(s)\n\n return x, y\n\n def calc_curvature(self, s):\n u\"\"\"\n calc curvature\n \"\"\"\n dx = self.sx.calcd(s)\n ddx = self.sx.calcdd(s)\n dy = self.sy.calcd(s)\n ddy = self.sy.calcdd(s)\n k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)\n return k\n\n def calc_yaw(self, s):\n u\"\"\"\n calc yaw\n \"\"\"\n dx = self.sx.calcd(s)\n dy = self.sy.calcd(s)\n yaw = math.atan2(dy, dx)\n return yaw\n\n\ndef calc_spline_course(x, y, ds=0.1):\n sp = Spline2D(x, y)\n s = np.arange(0, sp.s[-1], ds)\n\n rx, ry, ryaw, rk = [], [], [], []\n for i_s in s:\n ix, iy = sp.calc_position(i_s)\n rx.append(ix)\n ry.append(iy)\n ryaw.append(sp.calc_yaw(i_s))\n rk.append(sp.calc_curvature(i_s))\n\n return rx, ry, ryaw, rk, s\n\n\ndef test_spline2d():\n print(\"Spline 2D test\")\n import matplotlib.pyplot as plt\n x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]\n y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]\n\n sp = Spline2D(x, y)\n s = np.arange(0, sp.s[-1], 0.1)\n\n rx, ry, ryaw, rk = [], [], [], []\n for i_s in s:\n ix, iy = sp.calc_position(i_s)\n rx.append(ix)\n ry.append(iy)\n ryaw.append(sp.calc_yaw(i_s))\n rk.append(sp.calc_curvature(i_s))\n\n flg, ax = plt.subplots(1)\n plt.plot(x, y, \"xb\", label=\"input\")\n plt.plot(rx, ry, \"-r\", label=\"spline\")\n plt.grid(True)\n plt.axis(\"equal\")\n plt.xlabel(\"s[m]\")\n plt.ylabel(\"y[m]\")\n plt.legend()\n\n flg, ax = plt.subplots(1)\n plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], \"-r\", label=\"yaw\")\n plt.grid(True)\n plt.legend()\n plt.xlabel(\"line length[m]\")\n plt.ylabel(\"yaw angle[deg]\")\n\n flg, ax = plt.subplots(1)\n plt.plot(s, rk, \"-r\", label=\"curvature\")\n plt.grid(True)\n plt.legend()\n plt.xlabel(\"line length[m]\")\n plt.ylabel(\"curvature [1/m]\")\n\n plt.show()\n\n\ndef test_spline():\n print(\"Spline test\")\n import matplotlib.pyplot as plt\n x = [-0.5, 0.0, 0.5, 1.0, 1.5]\n y = [3.2, 2.7, 6, 5, 6.5]\n\n spline = Spline(x, y)\n rx = np.arange(-2.0, 4, 0.01)\n ry = [spline.calc(i) for i in rx]\n\n plt.plot(x, y, \"xb\")\n plt.plot(rx, ry, \"-r\")\n plt.grid(True)\n plt.axis(\"equal\")\n plt.show()\n\n\nif __name__ == '__main__':\n test_spline()\n # test_spline2d()\n" }, { "path": "CurvesGenerator/draw.py", "content": "import matplotlib.pyplot as plt\nimport numpy as np\nPI = np.pi\n\n\nclass Arrow:\n def __init__(self, x, y, theta, L, c):\n angle = np.deg2rad(30)\n d = 0.5 * L\n w = 2\n\n x_start = x\n y_start = y\n x_end = x + L * np.cos(theta)\n y_end = y + L * np.sin(theta)\n\n theta_hat_L = theta + PI - angle\n theta_hat_R = theta + PI + angle\n\n x_hat_start = x_end\n x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)\n x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)\n\n y_hat_start = y_end\n y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)\n y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)\n\n plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)\n plt.plot([x_hat_start, x_hat_end_L],\n [y_hat_start, y_hat_end_L], color=c, linewidth=w)\n plt.plot([x_hat_start, x_hat_end_R],\n [y_hat_start, y_hat_end_R], color=c, linewidth=w)\n\n\nclass Car:\n def __init__(self, x, y, yaw, w, L):\n theta_B = PI + yaw\n\n xB = x + L / 4 * np.cos(theta_B)\n yB = y + L / 4 * np.sin(theta_B)\n\n theta_BL = theta_B + PI / 2\n theta_BR = theta_B - PI / 2\n\n x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex\n y_BL = yB + w / 2 * np.sin(theta_BL)\n x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex\n y_BR = yB + w / 2 * np.sin(theta_BR)\n\n x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex\n y_FL = y_BL + L * np.sin(yaw)\n x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex\n y_FR = y_BR + L * np.sin(yaw)\n\n plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],\n [y_BL, y_BR, y_FR, y_FL, y_BL],\n linewidth=1, color='black')\n\n Arrow(x, y, yaw, L / 2, 'black')\n # plt.axis(\"equal\")\n # plt.show()\n\n\nif __name__ == '__main__':\n # Arrow(-1, 2, 60)\n Car(0, 0, 1, 2, 60)\n" }, { "path": "CurvesGenerator/dubins_path.py", "content": "\"\"\"\nDubins Path\n\"\"\"\n\nimport math\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.spatial.transform import Rotation as Rot\nimport CurvesGenerator.draw as draw\n\n\n# class for PATH element\nclass PATH:\n def __init__(self, L, mode, x, y, yaw):\n self.L = L # total path length [float]\n self.mode = mode # type of each part of the path [string]\n self.x = x # final s positions [m]\n self.y = y # final y positions [m]\n self.yaw = yaw # final yaw angles [rad]\n\n\n# utils\ndef pi_2_pi(theta):\n while theta > math.pi:\n theta -= 2.0 * math.pi\n\n while theta < -math.pi:\n theta += 2.0 * math.pi\n\n return theta\n\n\ndef mod2pi(theta):\n return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)\n\n\ndef LSL(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)\n\n if p_lsl < 0:\n return None, None, None, [\"L\", \"S\", \"L\"]\n else:\n p_lsl = math.sqrt(p_lsl)\n\n denominate = dist + sin_a - sin_b\n t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate))\n q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate))\n\n return t_lsl, p_lsl, q_lsl, [\"L\", \"S\", \"L\"]\n\n\ndef RSR(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)\n\n if p_rsr < 0:\n return None, None, None, [\"R\", \"S\", \"R\"]\n else:\n p_rsr = math.sqrt(p_rsr)\n\n denominate = dist - sin_a + sin_b\n t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate))\n q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate))\n\n return t_rsr, p_rsr, q_rsr, [\"R\", \"S\", \"R\"]\n\n\ndef LSR(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)\n\n if p_lsr < 0:\n return None, None, None, [\"L\", \"S\", \"R\"]\n else:\n p_lsr = math.sqrt(p_lsr)\n\n rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr)\n t_lsr = mod2pi(-alpha + rec)\n q_lsr = mod2pi(-mod2pi(beta) + rec)\n\n return t_lsr, p_lsr, q_lsr, [\"L\", \"S\", \"R\"]\n\n\ndef RSL(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)\n\n if p_rsl < 0:\n return None, None, None, [\"R\", \"S\", \"L\"]\n else:\n p_rsl = math.sqrt(p_rsl)\n\n rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl)\n t_rsl = mod2pi(alpha - rec)\n q_rsl = mod2pi(beta - rec)\n\n return t_rsl, p_rsl, q_rsl, [\"R\", \"S\", \"L\"]\n\n\ndef RLR(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0\n\n if abs(rec) > 1.0:\n return None, None, None, [\"R\", \"L\", \"R\"]\n\n p_rlr = mod2pi(2 * math.pi - math.acos(rec))\n t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))\n q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))\n\n return t_rlr, p_rlr, q_rlr, [\"R\", \"L\", \"R\"]\n\n\ndef LRL(alpha, beta, dist):\n sin_a = math.sin(alpha)\n sin_b = math.sin(beta)\n cos_a = math.cos(alpha)\n cos_b = math.cos(beta)\n cos_a_b = math.cos(alpha - beta)\n\n rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0\n\n if abs(rec) > 1.0:\n return None, None, None, [\"L\", \"R\", \"L\"]\n\n p_lrl = mod2pi(2 * math.pi - math.acos(rec))\n t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)\n q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))\n\n return t_lrl, p_lrl, q_lrl, [\"L\", \"R\", \"L\"]\n\n\ndef interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):\n if m == \"S\":\n px[ind] = ox + l / maxc * math.cos(oyaw)\n py[ind] = oy + l / maxc * math.sin(oyaw)\n pyaw[ind] = oyaw\n else:\n ldx = math.sin(l) / maxc\n if m == \"L\":\n ldy = (1.0 - math.cos(l)) / maxc\n elif m == \"R\":\n ldy = (1.0 - math.cos(l)) / (-maxc)\n\n gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy\n gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy\n px[ind] = ox + gdx\n py[ind] = oy + gdy\n\n if m == \"L\":\n pyaw[ind] = oyaw + l\n elif m == \"R\":\n pyaw[ind] = oyaw - l\n\n if l > 0.0:\n directions[ind] = 1\n else:\n directions[ind] = -1\n\n return px, py, pyaw, directions\n\n\ndef generate_local_course(L, lengths, mode, maxc, step_size):\n point_num = int(L / step_size) + len(lengths) + 3\n\n px = [0.0 for _ in range(point_num)]\n py = [0.0 for _ in range(point_num)]\n pyaw = [0.0 for _ in range(point_num)]\n directions = [0 for _ in range(point_num)]\n ind = 1\n\n if lengths[0] > 0.0:\n directions[0] = 1\n else:\n directions[0] = -1\n\n if lengths[0] > 0.0:\n d = step_size\n else:\n d = -step_size\n\n ll = 0.0\n\n for m, l, i in zip(mode, lengths, range(len(mode))):\n if l > 0.0:\n d = step_size\n else:\n d = -step_size\n\n ox, oy, oyaw = px[ind], py[ind], pyaw[ind]\n\n ind -= 1\n if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:\n pd = -d - ll\n else:\n pd = d - ll\n\n while abs(pd) <= abs(l):\n ind += 1\n px, py, pyaw, directions = \\\n interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)\n pd += d\n\n ll = l - pd - d # calc remain length\n\n ind += 1\n px, py, pyaw, directions = \\\n interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)\n\n if len(px) <= 1:\n return [], [], [], []\n\n # remove unused data\n while len(px) >= 1 and px[-1] == 0.0:\n px.pop()\n py.pop()\n pyaw.pop()\n directions.pop()\n\n return px, py, pyaw, directions\n\n\ndef planning_from_origin(gx, gy, gyaw, curv, step_size):\n D = math.hypot(gx, gy)\n d = D * curv\n\n theta = mod2pi(math.atan2(gy, gx))\n alpha = mod2pi(-theta)\n beta = mod2pi(gyaw - theta)\n\n planners = [LSL, RSR, LSR, RSL, RLR, LRL]\n\n best_cost = float(\"inf\")\n bt, bp, bq, best_mode = None, None, None, None\n\n for planner in planners:\n t, p, q, mode = planner(alpha, beta, d)\n\n if t is None:\n continue\n\n cost = (abs(t) + abs(p) + abs(q))\n if best_cost > cost:\n bt, bp, bq, best_mode = t, p, q, mode\n best_cost = cost\n lengths = [bt, bp, bq]\n\n x_list, y_list, yaw_list, directions = generate_local_course(\n sum(lengths), lengths, best_mode, curv, step_size)\n\n return x_list, y_list, yaw_list, best_mode, best_cost\n\n\ndef calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):\n gx = gx - sx\n gy = gy - sy\n\n l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]\n le_xy = np.stack([gx, gy]).T @ l_rot\n le_yaw = gyaw - syaw\n\n lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(\n le_xy[0], le_xy[1], le_yaw, curv, step_size)\n\n rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]\n converted_xy = np.stack([lp_x, lp_y]).T @ rot\n x_list = converted_xy[:, 0] + sx\n y_list = converted_xy[:, 1] + sy\n yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]\n\n return PATH(lengths, mode, x_list, y_list, yaw_list)\n\n\ndef main():\n # choose states pairs: (s, y, yaw)\n # simulation-1\n states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),\n (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]\n\n # simulation-2\n # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),\n # (35, 10, 180), (32, -10, 180), (5, -12, 90)]\n\n max_c = 0.25 # max curvature\n path_x, path_y, yaw = [], [], []\n\n for i in range(len(states) - 1):\n s_x = states[i][0]\n s_y = states[i][1]\n s_yaw = np.deg2rad(states[i][2])\n g_x = states[i + 1][0]\n g_y = states[i + 1][1]\n g_yaw = np.deg2rad(states[i + 1][2])\n\n path_i = calc_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c)\n\n for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):\n path_x.append(x)\n path_y.append(y)\n yaw.append(iyaw)\n\n # animation\n plt.ion()\n plt.figure(1)\n\n for i in range(len(path_x)):\n plt.clf()\n plt.plot(path_x, path_y, linewidth=1, color='gray')\n\n for x, y, theta in states:\n draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')\n\n draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)\n\n plt.axis(\"equal\")\n plt.title(\"Simulation of Dubins Path\")\n plt.axis([-10, 42, -20, 20])\n plt.draw()\n plt.pause(0.001)\n\n plt.pause(1)\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "CurvesGenerator/quartic_polynomial.py", "content": "\"\"\"\nQuartic Polynomial\n\"\"\"\n\nimport numpy as np\n\n\nclass QuarticPolynomial:\n def __init__(self, x0, v0, a0, v1, a1, T):\n A = np.array([[3 * T ** 2, 4 * T ** 3],\n [6 * T, 12 * T ** 2]])\n b = np.array([v1 - v0 - a0 * T,\n a1 - a0])\n X = np.linalg.solve(A, b)\n\n self.a0 = x0\n self.a1 = v0\n self.a2 = a0 / 2.0\n self.a3 = X[0]\n self.a4 = X[1]\n\n def calc_xt(self, t):\n xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \\\n self.a3 * t ** 3 + self.a4 * t ** 4\n\n return xt\n\n def calc_dxt(self, t):\n xt = self.a1 + 2 * self.a2 * t + \\\n 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3\n\n return xt\n\n def calc_ddxt(self, t):\n xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2\n\n return xt\n\n def calc_dddxt(self, t):\n xt = 6 * self.a3 + 24 * self.a4 * t\n\n return xt\n\n" }, { "path": "CurvesGenerator/quintic_polynomial.py", "content": "\"\"\"\nQuintic Polynomial\n\"\"\"\n\nimport math\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport draw\n\n\nclass QuinticPolynomial:\n def __init__(self, x0, v0, a0, x1, v1, a1, T):\n A = np.array([[T ** 3, T ** 4, T ** 5],\n [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],\n [6 * T, 12 * T ** 2, 20 * T ** 3]])\n b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2,\n v1 - v0 - a0 * T,\n a1 - a0])\n X = np.linalg.solve(A, b)\n\n self.a0 = x0\n self.a1 = v0\n self.a2 = a0 / 2.0\n self.a3 = X[0]\n self.a4 = X[1]\n self.a5 = X[2]\n\n def calc_xt(self, t):\n xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \\\n self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5\n\n return xt\n\n def calc_dxt(self, t):\n dxt = self.a1 + 2 * self.a2 * t + \\\n 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4\n\n return dxt\n\n def calc_ddxt(self, t):\n ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3\n\n return ddxt\n\n def calc_dddxt(self, t):\n dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2\n\n return dddxt\n\n\nclass Trajectory:\n def __init__(self):\n self.t = []\n self.x = []\n self.y = []\n self.yaw = []\n self.v = []\n self.a = []\n self.jerk = []\n\n\ndef simulation():\n sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1\n gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1\n\n MAX_ACCEL = 1.0 # max accel [m/s2]\n MAX_JERK = 0.5 # max jerk [m/s3]\n dt = 0.1 # T tick [s]\n\n MIN_T = 5\n MAX_T = 100\n T_STEP = 5\n\n sv_x = sv * math.cos(syaw)\n sv_y = sv * math.sin(syaw)\n gv_x = gv * math.cos(gyaw)\n gv_y = gv * math.sin(gyaw)\n\n sa_x = sa * math.cos(syaw)\n sa_y = sa * math.sin(syaw)\n ga_x = ga * math.cos(gyaw)\n ga_y = ga * math.sin(gyaw)\n\n path = Trajectory()\n\n for T in np.arange(MIN_T, MAX_T, T_STEP):\n path = Trajectory()\n xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T)\n yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T)\n\n for t in np.arange(0.0, T + dt, dt):\n path.t.append(t)\n path.x.append(xqp.calc_xt(t))\n path.y.append(yqp.calc_xt(t))\n\n vx = xqp.calc_dxt(t)\n vy = yqp.calc_dxt(t)\n path.v.append(np.hypot(vx, vy))\n path.yaw.append(math.atan2(vy, vx))\n\n ax = xqp.calc_ddxt(t)\n ay = yqp.calc_ddxt(t)\n a = np.hypot(ax, ay)\n\n if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:\n a *= -1\n path.a.append(a)\n\n jx = xqp.calc_dddxt(t)\n jy = yqp.calc_dddxt(t)\n j = np.hypot(jx, jy)\n\n if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0:\n j *= -1\n path.jerk.append(j)\n\n if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK:\n break\n\n print(\"t_len: \", path.t, \"s\")\n print(\"max_v: \", max(path.v), \"m/s\")\n print(\"max_a: \", max(np.abs(path.a)), \"m/s2\")\n print(\"max_jerk: \", max(np.abs(path.jerk)), \"m/s3\")\n\n for i in range(len(path.t)):\n plt.cla()\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n plt.axis(\"equal\")\n plt.plot(path.x, path.y, linewidth=2, color='gray')\n draw.Car(sx, sy, syaw, 1.5, 3)\n draw.Car(gx, gy, gyaw, 1.5, 3)\n draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)\n plt.title(\"Quintic Polynomial Curves\")\n plt.grid(True)\n plt.pause(0.001)\n\n plt.show()\n\n\nif __name__ == '__main__':\n simulation()\n" }, { "path": "CurvesGenerator/reeds_shepp.py", "content": "import math\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport draw\n\n# parameters initiation\nSTEP_SIZE = 0.2\nMAX_LENGTH = 1000.0\nPI = math.pi\n\n\n# class for PATH element\nclass PATH:\n def __init__(self, lengths, ctypes, L, x, y, yaw, directions):\n self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float]\n self.ctypes = ctypes # type of each part of the path [string]\n self.L = L # total path length [float]\n self.x = x # final s positions [m]\n self.y = y # final y positions [m]\n self.yaw = yaw # final yaw angles [rad]\n self.directions = directions # forward: 1, backward:-1\n\n\ndef calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):\n paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)\n\n minL = paths[0].L\n mini = 0\n\n for i in range(len(paths)):\n if paths[i].L <= minL:\n minL, mini = paths[i].L, i\n\n return paths[mini]\n\n\ndef calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):\n q0 = [sx, sy, syaw]\n q1 = [gx, gy, gyaw]\n\n paths = generate_path(q0, q1, maxc)\n\n for path in paths:\n x, y, yaw, directions = \\\n generate_local_course(path.L, path.lengths,\n path.ctypes, maxc, step_size * maxc)\n\n # convert global coordinate\n path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]\n path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]\n path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]\n path.directions = directions\n path.lengths = [l / maxc for l in path.lengths]\n path.L = path.L / maxc\n\n return paths\n\n\ndef set_path(paths, lengths, ctypes):\n path = PATH([], [], 0.0, [], [], [], [])\n path.ctypes = ctypes\n path.lengths = lengths\n\n # check same path exist\n for path_e in paths:\n if path_e.ctypes == path.ctypes:\n if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:\n return paths # not insert path\n\n path.L = sum([abs(i) for i in lengths])\n\n if path.L >= MAX_LENGTH:\n return paths\n\n assert path.L >= 0.01\n paths.append(path)\n\n return paths\n\n\ndef LSL(x, y, phi):\n u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))\n\n if t >= 0.0:\n v = M(phi - t)\n if v >= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef LSR(x, y, phi):\n u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))\n u1 = u1 ** 2\n\n if u1 >= 4.0:\n u = math.sqrt(u1 - 4.0)\n theta = math.atan2(2.0, u)\n t = M(t1 + theta)\n v = M(t - phi)\n\n if t >= 0.0 and v >= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef LRL(x, y, phi):\n u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))\n\n if u1 <= 4.0:\n u = -2.0 * math.asin(0.25 * u1)\n t = M(t1 + 0.5 * u + PI)\n v = M(phi - t + u)\n\n if t >= 0.0 and u <= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef SCS(x, y, phi, paths):\n flag, t, u, v = SLS(x, y, phi)\n\n if flag:\n paths = set_path(paths, [t, u, v], [\"S\", \"L\", \"S\"])\n\n flag, t, u, v = SLS(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"S\", \"R\", \"S\"])\n\n return paths\n\n\ndef SLS(x, y, phi):\n phi = M(phi)\n\n if y > 0.0 and 0.0 < phi < PI * 0.99:\n xd = -y / math.tan(phi) + x\n t = xd - math.tan(phi / 2.0)\n u = phi\n v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)\n return True, t, u, v\n elif y < 0.0 and 0.0 < phi < PI * 0.99:\n xd = -y / math.tan(phi) + x\n t = xd - math.tan(phi / 2.0)\n u = phi\n v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef CSC(x, y, phi, paths):\n flag, t, u, v = LSL(x, y, phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"L\", \"S\", \"L\"])\n\n flag, t, u, v = LSL(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"L\", \"S\", \"L\"])\n\n flag, t, u, v = LSL(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"R\", \"S\", \"R\"])\n\n flag, t, u, v = LSL(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"R\", \"S\", \"R\"])\n\n flag, t, u, v = LSR(x, y, phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"L\", \"S\", \"R\"])\n\n flag, t, u, v = LSR(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"L\", \"S\", \"R\"])\n\n flag, t, u, v = LSR(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"R\", \"S\", \"L\"])\n\n flag, t, u, v = LSR(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"R\", \"S\", \"L\"])\n\n return paths\n\n\ndef CCC(x, y, phi, paths):\n flag, t, u, v = LRL(x, y, phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRL(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRL(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, v], [\"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRL(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, -u, -v], [\"R\", \"L\", \"R\"])\n\n # backwards\n xb = x * math.cos(phi) + y * math.sin(phi)\n yb = x * math.sin(phi) - y * math.cos(phi)\n\n flag, t, u, v = LRL(xb, yb, phi)\n if flag:\n paths = set_path(paths, [v, u, t], [\"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRL(-xb, yb, -phi)\n if flag:\n paths = set_path(paths, [-v, -u, -t], [\"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRL(xb, -yb, -phi)\n if flag:\n paths = set_path(paths, [v, u, t], [\"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRL(-xb, -yb, phi)\n if flag:\n paths = set_path(paths, [-v, -u, -t], [\"R\", \"L\", \"R\"])\n\n return paths\n\n\ndef calc_tauOmega(u, v, xi, eta, phi):\n delta = M(u - v)\n A = math.sin(u) - math.sin(delta)\n B = math.cos(u) - math.cos(delta) - 1.0\n\n t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)\n t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0\n\n if t2 < 0:\n tau = M(t1 + PI)\n else:\n tau = M(t1)\n\n omega = M(tau - u + v - phi)\n\n return tau, omega\n\n\ndef LRLRn(x, y, phi):\n xi = x + math.sin(phi)\n eta = y - 1.0 - math.cos(phi)\n rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))\n\n if rho <= 1.0:\n u = math.acos(rho)\n t, v = calc_tauOmega(u, -u, xi, eta, phi)\n if t >= 0.0 and v <= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef LRLRp(x, y, phi):\n xi = x + math.sin(phi)\n eta = y - 1.0 - math.cos(phi)\n rho = (20.0 - xi * xi - eta * eta) / 16.0\n\n if 0.0 <= rho <= 1.0:\n u = -math.acos(rho)\n if u >= -0.5 * PI:\n t, v = calc_tauOmega(u, u, xi, eta, phi)\n if t >= 0.0 and v >= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef CCCC(x, y, phi, paths):\n flag, t, u, v = LRLRn(x, y, phi)\n if flag:\n paths = set_path(paths, [t, u, -u, v], [\"L\", \"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRLRn(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, -u, u, -v], [\"L\", \"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRLRn(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, -u, v], [\"R\", \"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRLRn(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, -u, u, -v], [\"R\", \"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRLRp(x, y, phi)\n if flag:\n paths = set_path(paths, [t, u, u, v], [\"L\", \"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRLRp(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, -u, -u, -v], [\"L\", \"R\", \"L\", \"R\"])\n\n flag, t, u, v = LRLRp(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, u, u, v], [\"R\", \"L\", \"R\", \"L\"])\n\n flag, t, u, v = LRLRp(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, -u, -u, -v], [\"R\", \"L\", \"R\", \"L\"])\n\n return paths\n\n\ndef LRSR(x, y, phi):\n xi = x + math.sin(phi)\n eta = y - 1.0 - math.cos(phi)\n rho, theta = R(-eta, xi)\n\n if rho >= 2.0:\n t = theta\n u = 2.0 - rho\n v = M(t + 0.5 * PI - phi)\n if t >= 0.0 and u <= 0.0 and v <= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef LRSL(x, y, phi):\n xi = x - math.sin(phi)\n eta = y - 1.0 + math.cos(phi)\n rho, theta = R(xi, eta)\n\n if rho >= 2.0:\n r = math.sqrt(rho * rho - 4.0)\n u = 2.0 - r\n t = M(theta + math.atan2(r, -2.0))\n v = M(phi - 0.5 * PI - t)\n if t >= 0.0 and u <= 0.0 and v <= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef CCSC(x, y, phi, paths):\n flag, t, u, v = LRSL(x, y, phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, v], [\"L\", \"R\", \"S\", \"L\"])\n\n flag, t, u, v = LRSL(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, -v], [\"L\", \"R\", \"S\", \"L\"])\n\n flag, t, u, v = LRSL(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, v], [\"R\", \"L\", \"S\", \"R\"])\n\n flag, t, u, v = LRSL(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, -v], [\"R\", \"L\", \"S\", \"R\"])\n\n flag, t, u, v = LRSR(x, y, phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, v], [\"L\", \"R\", \"S\", \"R\"])\n\n flag, t, u, v = LRSR(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, -v], [\"L\", \"R\", \"S\", \"R\"])\n\n flag, t, u, v = LRSR(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, v], [\"R\", \"L\", \"S\", \"L\"])\n\n flag, t, u, v = LRSR(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, -v], [\"R\", \"L\", \"S\", \"L\"])\n\n # backwards\n xb = x * math.cos(phi) + y * math.sin(phi)\n yb = x * math.sin(phi) - y * math.cos(phi)\n\n flag, t, u, v = LRSL(xb, yb, phi)\n if flag:\n paths = set_path(paths, [v, u, -0.5 * PI, t], [\"L\", \"S\", \"R\", \"L\"])\n\n flag, t, u, v = LRSL(-xb, yb, -phi)\n if flag:\n paths = set_path(paths, [-v, -u, 0.5 * PI, -t], [\"L\", \"S\", \"R\", \"L\"])\n\n flag, t, u, v = LRSL(xb, -yb, -phi)\n if flag:\n paths = set_path(paths, [v, u, -0.5 * PI, t], [\"R\", \"S\", \"L\", \"R\"])\n\n flag, t, u, v = LRSL(-xb, -yb, phi)\n if flag:\n paths = set_path(paths, [-v, -u, 0.5 * PI, -t], [\"R\", \"S\", \"L\", \"R\"])\n\n flag, t, u, v = LRSR(xb, yb, phi)\n if flag:\n paths = set_path(paths, [v, u, -0.5 * PI, t], [\"R\", \"S\", \"R\", \"L\"])\n\n flag, t, u, v = LRSR(-xb, yb, -phi)\n if flag:\n paths = set_path(paths, [-v, -u, 0.5 * PI, -t], [\"R\", \"S\", \"R\", \"L\"])\n\n flag, t, u, v = LRSR(xb, -yb, -phi)\n if flag:\n paths = set_path(paths, [v, u, -0.5 * PI, t], [\"L\", \"S\", \"L\", \"R\"])\n\n flag, t, u, v = LRSR(-xb, -yb, phi)\n if flag:\n paths = set_path(paths, [-v, -u, 0.5 * PI, -t], [\"L\", \"S\", \"L\", \"R\"])\n\n return paths\n\n\ndef LRSLR(x, y, phi):\n # formula 8.11 *** TYPO IN PAPER ***\n xi = x + math.sin(phi)\n eta = y - 1.0 - math.cos(phi)\n rho, theta = R(xi, eta)\n\n if rho >= 2.0:\n u = 4.0 - math.sqrt(rho * rho - 4.0)\n if u <= 0.0:\n t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))\n v = M(t - phi)\n\n if t >= 0.0 and v >= 0.0:\n return True, t, u, v\n\n return False, 0.0, 0.0, 0.0\n\n\ndef CCSCC(x, y, phi, paths):\n flag, t, u, v = LRSLR(x, y, phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], [\"L\", \"R\", \"S\", \"L\", \"R\"])\n\n flag, t, u, v = LRSLR(-x, y, -phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], [\"L\", \"R\", \"S\", \"L\", \"R\"])\n\n flag, t, u, v = LRSLR(x, -y, -phi)\n if flag:\n paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], [\"R\", \"L\", \"S\", \"R\", \"L\"])\n\n flag, t, u, v = LRSLR(-x, -y, phi)\n if flag:\n paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], [\"R\", \"L\", \"S\", \"R\", \"L\"])\n\n return paths\n\n\ndef generate_local_course(L, lengths, mode, maxc, step_size):\n point_num = int(L / step_size) + len(lengths) + 3\n\n px = [0.0 for _ in range(point_num)]\n py = [0.0 for _ in range(point_num)]\n pyaw = [0.0 for _ in range(point_num)]\n directions = [0 for _ in range(point_num)]\n ind = 1\n\n if lengths[0] > 0.0:\n directions[0] = 1\n else:\n directions[0] = -1\n\n if lengths[0] > 0.0:\n d = step_size\n else:\n d = -step_size\n\n pd = d\n ll = 0.0\n\n for m, l, i in zip(mode, lengths, range(len(mode))):\n if l > 0.0:\n d = step_size\n else:\n d = -step_size\n\n ox, oy, oyaw = px[ind], py[ind], pyaw[ind]\n\n ind -= 1\n if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:\n pd = -d - ll\n else:\n pd = d - ll\n\n while abs(pd) <= abs(l):\n ind += 1\n px, py, pyaw, directions = \\\n interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)\n pd += d\n\n ll = l - pd - d # calc remain length\n\n ind += 1\n px, py, pyaw, directions = \\\n interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)\n\n # remove unused data\n while px[-1] == 0.0:\n px.pop()\n py.pop()\n pyaw.pop()\n directions.pop()\n\n return px, py, pyaw, directions\n\n\ndef interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):\n if m == \"S\":\n px[ind] = ox + l / maxc * math.cos(oyaw)\n py[ind] = oy + l / maxc * math.sin(oyaw)\n pyaw[ind] = oyaw\n else:\n ldx = math.sin(l) / maxc\n if m == \"L\":\n ldy = (1.0 - math.cos(l)) / maxc\n elif m == \"R\":\n ldy = (1.0 - math.cos(l)) / (-maxc)\n\n gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy\n gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy\n px[ind] = ox + gdx\n py[ind] = oy + gdy\n\n if m == \"L\":\n pyaw[ind] = oyaw + l\n elif m == \"R\":\n pyaw[ind] = oyaw - l\n\n if l > 0.0:\n directions[ind] = 1\n else:\n directions[ind] = -1\n\n return px, py, pyaw, directions\n\n\ndef generate_path(q0, q1, maxc):\n dx = q1[0] - q0[0]\n dy = q1[1] - q0[1]\n dth = q1[2] - q0[2]\n c = math.cos(q0[2])\n s = math.sin(q0[2])\n x = (c * dx + s * dy) * maxc\n y = (-s * dx + c * dy) * maxc\n\n paths = []\n paths = SCS(x, y, dth, paths)\n paths = CSC(x, y, dth, paths)\n paths = CCC(x, y, dth, paths)\n paths = CCCC(x, y, dth, paths)\n paths = CCSC(x, y, dth, paths)\n paths = CCSCC(x, y, dth, paths)\n\n return paths\n\n\n# utils\ndef pi_2_pi(theta):\n while theta > PI:\n theta -= 2.0 * PI\n\n while theta < -PI:\n theta += 2.0 * PI\n\n return theta\n\n\ndef R(x, y):\n \"\"\"\n Return the polar coordinates (r, theta) of the point (s, y)\n \"\"\"\n r = math.hypot(x, y)\n theta = math.atan2(y, x)\n\n return r, theta\n\n\ndef M(theta):\n \"\"\"\n Regulate theta to -pi <= theta < pi\n \"\"\"\n phi = theta % (2.0 * PI)\n\n if phi < -PI:\n phi += 2.0 * PI\n if phi > PI:\n phi -= 2.0 * PI\n\n return phi\n\n\ndef get_label(path):\n label = \"\"\n\n for m, l in zip(path.ctypes, path.lengths):\n label = label + m\n if l > 0.0:\n label = label + \"+\"\n else:\n label = label + \"-\"\n\n return label\n\n\ndef calc_curvature(x, y, yaw, directions):\n c, ds = [], []\n\n for i in range(1, len(x) - 1):\n dxn = x[i] - x[i - 1]\n dxp = x[i + 1] - x[i]\n dyn = y[i] - y[i - 1]\n dyp = y[i + 1] - y[i]\n dn = math.hypot(dxn, dyn)\n dp = math.hypot(dxp, dyp)\n dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)\n ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)\n dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)\n ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)\n curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)\n d = (dn + dp) / 2.0\n\n if np.isnan(curvature):\n curvature = 0.0\n\n if directions[i] <= 0.0:\n curvature = -curvature\n\n if len(c) == 0:\n ds.append(d)\n c.append(curvature)\n\n ds.append(d)\n c.append(curvature)\n\n ds.append(ds[-1])\n c.append(c[-1])\n\n return c, ds\n\n\ndef check_path(sx, sy, syaw, gx, gy, gyaw, maxc):\n paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)\n\n assert len(paths) >= 1\n\n for path in paths:\n assert abs(path.x[0] - sx) <= 0.01\n assert abs(path.y[0] - sy) <= 0.01\n assert abs(path.yaw[0] - syaw) <= 0.01\n assert abs(path.x[-1] - gx) <= 0.01\n assert abs(path.y[-1] - gy) <= 0.01\n assert abs(path.yaw[-1] - gyaw) <= 0.01\n\n # course distance check\n d = [math.hypot(dx, dy)\n for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),\n np.diff(path.y[0:len(path.y) - 1]))]\n\n for i in range(len(d)):\n assert abs(d[i] - STEP_SIZE) <= 0.001\n\n\ndef main():\n # choose states pairs: (s, y, yaw)\n # simulation-1\n # states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),\n # (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]\n\n # simulation-2\n states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),\n (35, 10, 180), (32, -10, 180), (5, -12, 90)]\n\n max_c = 0.1 # max curvature\n path_x, path_y, yaw = [], [], []\n\n for i in range(len(states) - 1):\n s_x = states[i][0]\n s_y = states[i][1]\n s_yaw = np.deg2rad(states[i][2])\n g_x = states[i + 1][0]\n g_y = states[i + 1][1]\n g_yaw = np.deg2rad(states[i + 1][2])\n\n path_i = calc_optimal_path(s_x, s_y, s_yaw,\n g_x, g_y, g_yaw, max_c)\n\n path_x += path_i.x\n path_y += path_i.y\n yaw += path_i.yaw\n\n # animation\n plt.ion()\n plt.figure(1)\n\n for i in range(len(path_x)):\n plt.clf()\n plt.plot(path_x, path_y, linewidth=1, color='gray')\n\n for x, y, theta in states:\n draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')\n\n draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)\n\n plt.axis(\"equal\")\n plt.title(\"Simulation of Reeds-Shepp Curves\")\n plt.axis([-10, 42, -20, 20])\n plt.draw()\n plt.pause(0.001)\n\n plt.pause(1)\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "LICENSE", "content": "MIT License\n\nCopyright (c) 2020 Huiming Zhou\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n" }, { "path": "README.md", "content": "Overview\n------\nThis repository implements some common path planning algorithms used in robotics, including Search-based algorithms and Sampling-based algorithms. We designed animation for each algorithm to display the running process. The related papers are listed in [Papers](https://github.com/zhm-real/PathPlanning#papers).\n\nDirectory Structure\n------\n .\n └── Search-based Planning\n ├── Breadth-First Searching (BFS)\n ├── Depth-First Searching (DFS)\n ├── Best-First Searching\n ├── Dijkstra's\n ├── A*\n ├── Bidirectional A*\n ├── Anytime Repairing A*\n ├── Learning Real-time A* (LRTA*)\n ├── Real-time Adaptive A* (RTAA*)\n ├── Lifelong Planning A* (LPA*)\n ├── Dynamic A* (D*)\n ├── D* Lite\n └── Anytime D*\n └── Sampling-based Planning\n ├── RRT\n ├── RRT-Connect\n ├── Extended-RRT\n ├── Dynamic-RRT\n ├── RRT*\n ├── Informed RRT*\n ├── RRT* Smart\n ├── Anytime RRT*\n ├── Closed-Loop RRT*\n ├── Spline-RRT*\n ├── Fast Marching Trees (FMT*)\n └── Batch Informed Trees (BIT*)\n └── Papers\n\n## Animations - Search-Based\n### Best-First & Dijkstra\n
\n\n \n \n \n \n
\"dfs\"\"dijkstra\"
\n
\n\n### A* and A* Variants\n
\n\n \n \n \n \n
\"astar\"\"biastar\"
\n\n \n \n \n \n
\"repeatedastar\"\"arastar\"
\n\n \n \n \n \n
\"lrtastar\"\"rtaastar\"
\n\n \n \n \n \n
\"lpastar\"\"dstarlite\"
\n\n \n \n \n \n
\"lpastar\"\"dstarlite\"
\n
\n\n## Animation - Sampling-Based\n### RRT & Variants\n
\n\n \n \n \n \n
\"value\"value
\n\n \n \n \n \n
\"value\"value
\n\n \n \n \n \n
\"value\"value
\n\n \n \n \n \n
\"value\"value
\n\n \n \n \n \n
\"value\"value
\n
\n\n## Papers\n### Search-base Planning\n* [A*: ](https://ieeexplore.ieee.org/document/4082128) A Formal Basis for the heuristic Determination of Minimum Cost Paths\n* [Learning Real-Time A*: ](https://arxiv.org/pdf/1110.4076.pdf) Learning in Real-Time Search: A Unifying Framework\n* [Real-Time Adaptive A*: ](http://idm-lab.org/bib/abstracts/papers/aamas06.pdf) Real-Time Adaptive A*\n* [Lifelong Planning A*: ](https://www.cs.cmu.edu/~maxim/files/aij04.pdf) Lifelong Planning A*\n* [Anytime Repairing A*: ](https://papers.nips.cc/paper/2382-ara-anytime-a-with-provable-bounds-on-sub-optimality.pdf) ARA*: Anytime A* with Provable Bounds on Sub-Optimality\n* [D*: ](http://web.mit.edu/16.412j/www/html/papers/original_dstar_icra94.pdf) Optimal and Efficient Path Planning for Partially-Known Environments\n* [D* Lite: ](http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf) D* Lite\n* [Field D*: ](http://robots.stanford.edu/isrr-papers/draft/stentz.pdf) Field D*: An Interpolation-based Path Planner and Replanner\n* [Anytime D*: ](http://www.cs.cmu.edu/~ggordon/likhachev-etal.anytime-dstar.pdf) Anytime Dynamic A*: An Anytime, Replanning Algorithm\n* [Focussed D*: ](http://robotics.caltech.edu/~jwb/courses/ME132/handouts/Dstar_ijcai95.pdf) The Focussed D* Algorithm for Real-Time Replanning\n* [Potential Field, ](https://journals.sagepub.com/doi/abs/10.1177/027836498600500106) [[PPT]: ](https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf) Real-Time Obstacle Avoidance for Manipulators and Mobile Robots\n* [Hybrid A*: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving\n\n### Sampling-based Planning\n* [RRT: ](http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf) Rapidly-Exploring Random Trees: A New Tool for Path Planning\n* [RRT-Connect: ](http://www-cgi.cs.cmu.edu/afs/cs/academic/class/15494-s12/readings/kuffner_icra2000.pdf) RRT-Connect: An Efficient Approach to Single-Query Path Planning\n* [Extended-RRT: ](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.1.7617&rep=rep1&type=pdf) Real-Time Randomized Path Planning for Robot Navigation\n* [Dynamic-RRT: ](https://www.ri.cmu.edu/pub_files/pub4/ferguson_david_2006_2/ferguson_david_2006_2.pdf) Replanning with RRTs\n* [RRT*: ](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761) Sampling-based algorithms for optimal motion planning\n* [Anytime-RRT*: ](https://dspace.mit.edu/handle/1721.1/63170) Anytime Motion Planning using the RRT*\n* [Closed-loop RRT* (CL-RRT*): ](http://acl.mit.edu/papers/KuwataTCST09.pdf) Real-time Motion Planning with Applications to Autonomous Urban Driving\n* [Spline-RRT*: ](https://ieeexplore.ieee.org/abstract/document/6987895?casa_token=B9GUwVDbbncAAAAA:DWscGFLIa97ptgH7NpUQUL0A2ModiiBDBGklk1z7aDjI11Kyfzo8rpuFstdYcjOofJfCjR-mNw) Optimal path planning based on spline-RRT* for fixed-wing UAVs operating in three-dimensional environments\n* [LQR-RRT*: ](https://lis.csail.mit.edu/pubs/perez-icra12.pdf) Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics\n* [RRT#: ](http://dcsl.gatech.edu/papers/icra13.pdf) Use of Relaxation Methods in Sampling-Based Algorithms for Optimal Motion Planning\n* [RRT*-Smart: ](http://save.seecs.nust.edu.pk/pubs/ICMA2012.pdf) Rapid convergence implementation of RRT* towards optimal solution\n* [Informed RRT*: ](https://arxiv.org/abs/1404.2334) Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic\n* [Fast Marching Trees (FMT*): ](https://arxiv.org/abs/1306.3532) a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions\n* [Motion Planning using Lower Bounds (MPLB): ](https://ieeexplore.ieee.org/document/7139773) Asymptotically-optimal Motion Planning using lower bounds on cost\n* [Batch Informed Trees (BIT*): ](https://arxiv.org/abs/1405.5848) Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs\n* [Advanced Batch Informed Trees (ABIT*): ](https://arxiv.org/abs/2002.06589) Sampling-Based Planning with Advanced Graph-Search Techniques ((ICRA) 2020)\n* [Adaptively Informed Trees (AIT*): ](https://arxiv.org/abs/2002.06599) Fast Asymptotically Optimal Path Planning through Adaptive Heuristics ((ICRA) 2020)\n" }, { "path": "Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py", "content": "" }, { "path": "Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py", "content": "" }, { "path": "Sampling_based_Planning/rrt_2D/batch_informed_trees.py", "content": "\"\"\"\nBatch Informed Trees (BIT*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\nfrom scipy.spatial.transform import Rotation as Rot\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, x, y):\n self.x = x\n self.y = y\n self.parent = None\n\n\nclass Tree:\n def __init__(self, x_start, x_goal):\n self.x_start = x_start\n self.goal = x_goal\n\n self.r = 4.0\n self.V = set()\n self.E = set()\n self.QE = set()\n self.QV = set()\n\n self.V_old = set()\n\n\nclass BITStar:\n def __init__(self, x_start, x_goal, eta, iter_max):\n self.x_start = Node(x_start[0], x_start[1])\n self.x_goal = Node(x_goal[0], x_goal[1])\n self.eta = eta\n self.iter_max = iter_max\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(x_start, x_goal)\n self.utils = utils.Utils()\n\n self.fig, self.ax = plt.subplots()\n\n self.delta = self.utils.delta\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n self.Tree = Tree(self.x_start, self.x_goal)\n self.X_sample = set()\n self.g_T = dict()\n\n def init(self):\n self.Tree.V.add(self.x_start)\n self.X_sample.add(self.x_goal)\n\n self.g_T[self.x_start] = 0.0\n self.g_T[self.x_goal] = np.inf\n\n cMin, theta = self.calc_dist_and_angle(self.x_start, self.x_goal)\n C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)\n xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],\n [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])\n\n return theta, cMin, xCenter, C\n\n def planning(self):\n theta, cMin, xCenter, C = self.init()\n\n for k in range(500):\n if not self.Tree.QE and not self.Tree.QV:\n if k == 0:\n m = 350\n else:\n m = 200\n\n if self.x_goal.parent is not None:\n path_x, path_y = self.ExtractPath()\n plt.plot(path_x, path_y, linewidth=2, color='r')\n plt.pause(0.5)\n\n self.Prune(self.g_T[self.x_goal])\n self.X_sample.update(self.Sample(m, self.g_T[self.x_goal], cMin, xCenter, C))\n self.Tree.V_old = {v for v in self.Tree.V}\n self.Tree.QV = {v for v in self.Tree.V}\n # self.Tree.r = self.radius(len(self.Tree.V) + len(self.X_sample))\n\n while self.BestVertexQueueValue() <= self.BestEdgeQueueValue():\n self.ExpandVertex(self.BestInVertexQueue())\n\n vm, xm = self.BestInEdgeQueue()\n self.Tree.QE.remove((vm, xm))\n\n if self.g_T[vm] + self.calc_dist(vm, xm) + self.h_estimated(xm) < self.g_T[self.x_goal]:\n actual_cost = self.cost(vm, xm)\n if self.g_estimated(vm) + actual_cost + self.h_estimated(xm) < self.g_T[self.x_goal]:\n if self.g_T[vm] + actual_cost < self.g_T[xm]:\n if xm in self.Tree.V:\n # remove edges\n edge_delete = set()\n for v, x in self.Tree.E:\n if x == xm:\n edge_delete.add((v, x))\n\n for edge in edge_delete:\n self.Tree.E.remove(edge)\n else:\n self.X_sample.remove(xm)\n self.Tree.V.add(xm)\n self.Tree.QV.add(xm)\n\n self.g_T[xm] = self.g_T[vm] + actual_cost\n self.Tree.E.add((vm, xm))\n xm.parent = vm\n\n set_delete = set()\n for v, x in self.Tree.QE:\n if x == xm and self.g_T[v] + self.calc_dist(v, xm) >= self.g_T[xm]:\n set_delete.add((v, x))\n\n for edge in set_delete:\n self.Tree.QE.remove(edge)\n else:\n self.Tree.QE = set()\n self.Tree.QV = set()\n\n if k % 5 == 0:\n self.animation(xCenter, self.g_T[self.x_goal], cMin, theta)\n\n path_x, path_y = self.ExtractPath()\n plt.plot(path_x, path_y, linewidth=2, color='r')\n plt.pause(0.01)\n plt.show()\n\n def ExtractPath(self):\n node = self.x_goal\n path_x, path_y = [node.x], [node.y]\n\n while node.parent:\n node = node.parent\n path_x.append(node.x)\n path_y.append(node.y)\n\n return path_x, path_y\n\n def Prune(self, cBest):\n self.X_sample = {x for x in self.X_sample if self.f_estimated(x) < cBest}\n self.Tree.V = {v for v in self.Tree.V if self.f_estimated(v) <= cBest}\n self.Tree.E = {(v, w) for v, w in self.Tree.E\n if self.f_estimated(v) <= cBest and self.f_estimated(w) <= cBest}\n self.X_sample.update({v for v in self.Tree.V if self.g_T[v] == np.inf})\n self.Tree.V = {v for v in self.Tree.V if self.g_T[v] < np.inf}\n\n def cost(self, start, end):\n if self.utils.is_collision(start, end):\n return np.inf\n\n return self.calc_dist(start, end)\n\n def f_estimated(self, node):\n return self.g_estimated(node) + self.h_estimated(node)\n\n def g_estimated(self, node):\n return self.calc_dist(self.x_start, node)\n\n def h_estimated(self, node):\n return self.calc_dist(node, self.x_goal)\n\n def Sample(self, m, cMax, cMin, xCenter, C):\n if cMax < np.inf:\n return self.SampleEllipsoid(m, cMax, cMin, xCenter, C)\n else:\n return self.SampleFreeSpace(m)\n\n def SampleEllipsoid(self, m, cMax, cMin, xCenter, C):\n r = [cMax / 2.0,\n math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,\n math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]\n L = np.diag(r)\n\n ind = 0\n delta = self.delta\n Sample = set()\n\n while ind < m:\n xBall = self.SampleUnitNBall()\n x_rand = np.dot(np.dot(C, L), xBall) + xCenter\n node = Node(x_rand[(0, 0)], x_rand[(1, 0)])\n in_obs = self.utils.is_inside_obs(node)\n in_x_range = self.x_range[0] + delta <= node.x <= self.x_range[1] - delta\n in_y_range = self.y_range[0] + delta <= node.y <= self.y_range[1] - delta\n\n if not in_obs and in_x_range and in_y_range:\n Sample.add(node)\n ind += 1\n\n return Sample\n\n def SampleFreeSpace(self, m):\n delta = self.utils.delta\n Sample = set()\n\n ind = 0\n while ind < m:\n node = Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))\n if self.utils.is_inside_obs(node):\n continue\n else:\n Sample.add(node)\n ind += 1\n\n return Sample\n\n def radius(self, q):\n cBest = self.g_T[self.x_goal]\n lambda_X = len([1 for v in self.Tree.V if self.f_estimated(v) <= cBest])\n radius = 2 * self.eta * (1.5 * lambda_X / math.pi * math.log(q) / q) ** 0.5\n\n return radius\n\n def ExpandVertex(self, v):\n self.Tree.QV.remove(v)\n X_near = {x for x in self.X_sample if self.calc_dist(x, v) <= self.Tree.r}\n\n for x in X_near:\n if self.g_estimated(v) + self.calc_dist(v, x) + self.h_estimated(x) < self.g_T[self.x_goal]:\n self.g_T[x] = np.inf\n self.Tree.QE.add((v, x))\n\n if v not in self.Tree.V_old:\n V_near = {w for w in self.Tree.V if self.calc_dist(w, v) <= self.Tree.r}\n\n for w in V_near:\n if (v, w) not in self.Tree.E and \\\n self.g_estimated(v) + self.calc_dist(v, w) + self.h_estimated(w) < self.g_T[self.x_goal] and \\\n self.g_T[v] + self.calc_dist(v, w) < self.g_T[w]:\n self.Tree.QE.add((v, w))\n if w not in self.g_T:\n self.g_T[w] = np.inf\n\n def BestVertexQueueValue(self):\n if not self.Tree.QV:\n return np.inf\n\n return min(self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV)\n\n def BestEdgeQueueValue(self):\n if not self.Tree.QE:\n return np.inf\n\n return min(self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)\n for v, x in self.Tree.QE)\n\n def BestInVertexQueue(self):\n if not self.Tree.QV:\n print(\"QV is Empty!\")\n return None\n\n v_value = {v: self.g_T[v] + self.h_estimated(v) for v in self.Tree.QV}\n\n return min(v_value, key=v_value.get)\n\n def BestInEdgeQueue(self):\n if not self.Tree.QE:\n print(\"QE is Empty!\")\n return None\n\n e_value = {(v, x): self.g_T[v] + self.calc_dist(v, x) + self.h_estimated(x)\n for v, x in self.Tree.QE}\n\n return min(e_value, key=e_value.get)\n\n @staticmethod\n def SampleUnitNBall():\n while True:\n x, y = random.uniform(-1, 1), random.uniform(-1, 1)\n if x ** 2 + y ** 2 < 1:\n return np.array([[x], [y], [0.0]])\n\n @staticmethod\n def RotationToWorldFrame(x_start, x_goal, L):\n a1 = np.array([[(x_goal.x - x_start.x) / L],\n [(x_goal.y - x_start.y) / L], [0.0]])\n e1 = np.array([[1.0], [0.0], [0.0]])\n M = a1 @ e1.T\n U, _, V_T = np.linalg.svd(M, True, True)\n C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T\n\n return C\n\n @staticmethod\n def calc_dist(start, end):\n return math.hypot(start.x - end.x, start.y - end.y)\n\n @staticmethod\n def calc_dist_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def animation(self, xCenter, cMax, cMin, theta):\n plt.cla()\n self.plot_grid(\"Batch Informed Trees (BIT*)\")\n\n plt.gcf().canvas.mpl_connect(\n 'key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n for v in self.X_sample:\n plt.plot(v.x, v.y, marker='.', color='lightgrey', markersize='2')\n\n if cMax < np.inf:\n self.draw_ellipse(xCenter, cMax, cMin, theta)\n\n for v, w in self.Tree.E:\n plt.plot([v.x, w.x], [v.y, w.y], '-g')\n\n plt.pause(0.001)\n\n def plot_grid(self, name):\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.x_start.x, self.x_start.y, \"bs\", linewidth=3)\n plt.plot(self.x_goal.x, self.x_goal.y, \"rs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n @staticmethod\n def draw_ellipse(x_center, c_best, dist, theta):\n a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0\n b = c_best / 2.0\n angle = math.pi / 2.0 - theta\n cx = x_center[0]\n cy = x_center[1]\n t = np.arange(0, 2 * math.pi + 0.1, 0.2)\n x = [a * math.cos(it) for it in t]\n y = [b * math.sin(it) for it in t]\n rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]\n fx = rot @ np.array([x, y])\n px = np.array(fx[0, :] + cx).flatten()\n py = np.array(fx[1, :] + cy).flatten()\n plt.plot(cx, cy, marker='.', color='darkorange')\n plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)\n\n\ndef main():\n x_start = (18, 8) # Starting node\n x_goal = (37, 18) # Goal node\n eta = 2\n iter_max = 200\n print(\"start!!!\")\n bit = BITStar(x_start, x_goal, eta, iter_max)\n # bit.animation(\"Batch Informed Trees (BIT*)\")\n bit.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/dubins_rrt_star.py", "content": "\"\"\"\nDUBINS_RRT_STAR 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\nfrom scipy.spatial.transform import Rotation as Rot\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\nimport CurvesGenerator.dubins_path as dubins\nimport CurvesGenerator.draw as draw\n\n\nclass Node:\n def __init__(self, x, y, yaw):\n self.x = x\n self.y = y\n self.yaw = yaw\n self.parent = None\n self.cost = 0.0\n self.path_x = []\n self.path_y = []\n self.paty_yaw = []\n\n\nclass DubinsRRTStar:\n def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,\n goal_sample_rate, search_radius, iter_max):\n self.s_start = Node(sx, sy, syaw)\n self.s_goal = Node(gx, gy, gyaw)\n self.vr = vehicle_radius\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.search_radius = search_radius\n self.iter_max = iter_max\n self.curv = 1\n\n self.env = env.Env()\n self.utils = utils.Utils()\n\n self.fig, self.ax = plt.subplots()\n self.delta = self.utils.delta\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.obs_circle()\n self.obs_boundary = self.env.obs_boundary\n self.utils.update_obs(self.obs_circle, self.obs_boundary, [])\n\n self.V = [self.s_start]\n self.path = None\n\n def planning(self):\n\n for i in range(self.iter_max):\n print(\"Iter:\", i, \", number of nodes:\", len(self.V))\n rnd = self.Sample()\n node_nearest = self.Nearest(self.V, rnd)\n new_node = self.Steer(node_nearest, rnd)\n\n if new_node and not self.is_collision(new_node):\n near_indexes = self.Near(self.V, new_node)\n new_node = self.choose_parent(new_node, near_indexes)\n\n if new_node:\n self.V.append(new_node)\n self.rewire(new_node, near_indexes)\n\n if i % 5 == 0:\n self.draw_graph()\n\n last_index = self.search_best_goal_node()\n\n path = self.generate_final_course(last_index)\n print(\"get!\")\n px = [s[0] for s in path]\n py = [s[1] for s in path]\n plt.plot(px, py, '-r')\n plt.pause(0.01)\n plt.show()\n\n def draw_graph(self, rnd=None):\n plt.cla()\n # for stopping simulation with the esc key.\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n for node in self.V:\n if node.parent:\n plt.plot(node.path_x, node.path_y, \"-g\")\n\n self.plot_grid(\"dubins rrt*\")\n plt.plot(self.s_start.x, self.s_start.y, \"xr\")\n plt.plot(self.s_goal.x, self.s_goal.y, \"xr\")\n plt.grid(True)\n self.plot_start_goal_arrow()\n plt.pause(0.01)\n\n def plot_start_goal_arrow(self):\n draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2, \"darkorange\")\n draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2, \"darkorange\")\n\n def generate_final_course(self, goal_index):\n print(\"final\")\n path = [[self.s_goal.x, self.s_goal.y]]\n node = self.V[goal_index]\n while node.parent:\n for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):\n path.append([ix, iy])\n node = node.parent\n path.append([self.s_start.x, self.s_start.y])\n return path\n\n def calc_dist_to_goal(self, x, y):\n dx = x - self.s_goal.x\n dy = y - self.s_goal.y\n return math.hypot(dx, dy)\n\n def search_best_goal_node(self):\n dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.V]\n goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.step_len]\n\n safe_goal_inds = []\n for goal_ind in goal_inds:\n t_node = self.Steer(self.V[goal_ind], self.s_goal)\n if t_node and not self.is_collision(t_node):\n safe_goal_inds.append(goal_ind)\n\n if not safe_goal_inds:\n return None\n\n min_cost = min([self.V[i].cost for i in safe_goal_inds])\n for i in safe_goal_inds:\n if self.V[i].cost == min_cost:\n return i\n\n return None\n\n def rewire(self, new_node, near_inds):\n for i in near_inds:\n near_node = self.V[i]\n edge_node = self.Steer(new_node, near_node)\n if not edge_node:\n continue\n edge_node.cost = self.calc_new_cost(new_node, near_node)\n\n no_collision = ~self.is_collision(edge_node)\n improved_cost = near_node.cost > edge_node.cost\n\n if no_collision and improved_cost:\n self.V[i] = edge_node\n self.propagate_cost_to_leaves(new_node)\n\n def choose_parent(self, new_node, near_inds):\n if not near_inds:\n return None\n\n costs = []\n for i in near_inds:\n near_node = self.V[i]\n t_node = self.Steer(near_node, new_node)\n if t_node and not self.is_collision(t_node):\n costs.append(self.calc_new_cost(near_node, new_node))\n else:\n costs.append(float(\"inf\")) # the cost of collision node\n min_cost = min(costs)\n\n if min_cost == float(\"inf\"):\n print(\"There is no good path.(min_cost is inf)\")\n return None\n\n min_ind = near_inds[costs.index(min_cost)]\n new_node = self.Steer(self.V[min_ind], new_node)\n\n return new_node\n\n def calc_new_cost(self, from_node, to_node):\n d, _ = self.get_distance_and_angle(from_node, to_node)\n return from_node.cost + d\n\n def propagate_cost_to_leaves(self, parent_node):\n for node in self.V:\n if node.parent == parent_node:\n node.cost = self.calc_new_cost(parent_node, node)\n self.propagate_cost_to_leaves(node)\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def Near(self, nodelist, node):\n n = len(nodelist) + 1\n r = min(self.search_radius * math.sqrt((math.log(n)) / n), self.step_len)\n\n dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]\n node_near_ind = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2]\n\n return node_near_ind\n\n def Steer(self, node_start, node_end):\n sx, sy, syaw = node_start.x, node_start.y, node_start.yaw\n gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw\n maxc = self.curv\n\n path = dubins.calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, maxc)\n\n if len(path.x) <= 1:\n return None\n\n node_new = Node(path.x[-1], path.y[-1], path.yaw[-1])\n node_new.path_x = path.x\n node_new.path_y = path.y\n node_new.path_yaw = path.yaw\n node_new.cost = node_start.cost + path.L\n node_new.parent = node_start\n\n return node_new\n\n def Sample(self):\n delta = self.utils.delta\n\n if random.random() > self.goal_sample_rate:\n return Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n random.uniform(self.y_range[0] + delta, self.y_range[1] - delta),\n random.uniform(-math.pi, math.pi))\n else:\n return self.s_goal\n\n @staticmethod\n def Nearest(nodelist, n):\n return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2\n for nd in nodelist]))]\n\n def is_collision(self, node):\n for ox, oy, r in self.obs_circle:\n dx = [ox - x for x in node.path_x]\n dy = [oy - y for y in node.path_y]\n dist = np.hypot(dx, dy)\n\n if min(dist) < r + self.delta:\n return True\n\n return False\n\n def animation(self):\n self.plot_grid(\"dubins rrt*\")\n self.plot_arrow()\n plt.show()\n\n def plot_arrow(self):\n draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2.5, \"darkorange\")\n draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2.5, \"darkorange\")\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.s_start.x, self.s_start.y, \"bs\", linewidth=3)\n plt.plot(self.s_goal.x, self.s_goal.y, \"gs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n @staticmethod\n def obs_circle():\n obs_cir = [\n [10, 10, 3],\n [15, 22, 3],\n [22, 8, 2.5],\n [26, 16, 2],\n [37, 10, 3],\n [37, 23, 3],\n [45, 15, 2]\n ]\n\n return obs_cir\n\n\ndef main():\n sx, sy, syaw = 5, 5, np.deg2rad(90)\n gx, gy, gyaw = 45, 25, np.deg2rad(0)\n goal_sample_rate = 0.1\n search_radius = 50.0\n step_len = 30.0\n iter_max = 250\n vehicle_radius = 2.0\n\n drrtstar = DubinsRRTStar(sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,\n goal_sample_rate, search_radius, iter_max)\n drrtstar.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/dynamic_rrt.py", "content": "\"\"\"\nDYNAMIC_RRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport copy\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n self.flag = \"VALID\"\n\n\nclass Edge:\n def __init__(self, n_p, n_c):\n self.parent = n_p\n self.child = n_c\n self.flag = \"VALID\"\n\n\nclass DynamicRrt:\n def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max):\n self.s_start = Node(s_start)\n self.s_goal = Node(s_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.waypoint_sample_rate = waypoint_sample_rate\n self.iter_max = iter_max\n self.vertex = [self.s_start]\n self.vertex_old = []\n self.vertex_new = []\n self.edges = []\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(s_start, s_goal)\n self.utils = utils.Utils()\n self.fig, self.ax = plt.subplots()\n\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n self.obs_add = [0, 0, 0]\n\n self.path = []\n self.waypoint = []\n\n def planning(self):\n for i in range(self.iter_max):\n node_rand = self.generate_random_node(self.goal_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.vertex.append(node_new)\n self.edges.append(Edge(node_near, node_new))\n dist, _ = self.get_distance_and_angle(node_new, self.s_goal)\n\n if dist <= self.step_len:\n self.new_state(node_new, self.s_goal)\n\n path = self.extract_path(node_new)\n self.plot_grid(\"Dynamic_RRT\")\n self.plot_visited()\n self.plot_path(path)\n self.path = path\n self.waypoint = self.extract_waypoint(node_new)\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n plt.show()\n\n return\n\n return None\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > 50 or y < 0 or y > 30:\n print(\"Please choose right area!\")\n else:\n x, y = int(x), int(y)\n print(\"Add circle obstacle at: s =\", x, \",\", \"y =\", y)\n self.obs_add = [x, y, 2]\n self.obs_circle.append([x, y, 2])\n self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)\n self.InvalidateNodes()\n\n if self.is_path_invalid():\n print(\"Path is Replanning ...\")\n path, waypoint = self.replanning()\n\n print(\"len_vertex: \", len(self.vertex))\n print(\"len_vertex_old: \", len(self.vertex_old))\n print(\"len_vertex_new: \", len(self.vertex_new))\n\n plt.cla()\n self.plot_grid(\"Dynamic_RRT\")\n self.plot_vertex_old()\n self.plot_path(self.path, color='blue')\n self.plot_vertex_new()\n self.vertex_new = []\n self.plot_path(path)\n self.path = path\n self.waypoint = waypoint\n else:\n print(\"Trimming Invalid Nodes ...\")\n self.TrimRRT()\n\n plt.cla()\n self.plot_grid(\"Dynamic_RRT\")\n self.plot_visited(animation=False)\n self.plot_path(self.path)\n\n self.fig.canvas.draw_idle()\n\n def InvalidateNodes(self):\n for edge in self.edges:\n if self.is_collision_obs_add(edge.parent, edge.child):\n edge.child.flag = \"INVALID\"\n\n def is_path_invalid(self):\n for node in self.waypoint:\n if node.flag == \"INVALID\":\n return True\n\n def is_collision_obs_add(self, start, end):\n delta = self.utils.delta\n obs_add = self.obs_add\n\n if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta:\n return True\n\n if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta:\n return True\n\n o, d = self.utils.get_ray(start, end)\n if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]):\n return True\n\n return False\n\n def replanning(self):\n self.TrimRRT()\n\n for i in range(self.iter_max):\n node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.vertex.append(node_new)\n self.vertex_new.append(node_new)\n self.edges.append(Edge(node_near, node_new))\n dist, _ = self.get_distance_and_angle(node_new, self.s_goal)\n\n if dist <= self.step_len:\n self.new_state(node_new, self.s_goal)\n path = self.extract_path(node_new)\n waypoint = self.extract_waypoint(node_new)\n print(\"path: \", len(path))\n print(\"waypoint: \", len(waypoint))\n\n return path, waypoint\n\n return None\n\n def TrimRRT(self):\n for i in range(1, len(self.vertex)):\n node = self.vertex[i]\n node_p = node.parent\n if node_p.flag == \"INVALID\":\n node.flag = \"INVALID\"\n\n self.vertex = [node for node in self.vertex if node.flag == \"VALID\"]\n self.vertex_old = copy.deepcopy(self.vertex)\n self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]]\n\n def generate_random_node(self, goal_sample_rate):\n delta = self.utils.delta\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.s_goal\n\n def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate):\n delta = self.utils.delta\n p = np.random.random()\n\n if p < goal_sample_rate:\n return self.s_goal\n elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:\n return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)]\n else:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n @staticmethod\n def nearest_neighbor(node_list, n):\n return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)\n for nd in node_list]))]\n\n def new_state(self, node_start, node_end):\n dist, theta = self.get_distance_and_angle(node_start, node_end)\n\n dist = min(self.step_len, dist)\n node_new = Node((node_start.x + dist * math.cos(theta),\n node_start.y + dist * math.sin(theta)))\n node_new.parent = node_start\n\n return node_new\n\n def extract_path(self, node_end):\n path = [(self.s_goal.x, self.s_goal.y)]\n node_now = node_end\n\n while node_now.parent is not None:\n node_now = node_now.parent\n path.append((node_now.x, node_now.y))\n\n return path\n\n def extract_waypoint(self, node_end):\n waypoint = [self.s_goal]\n node_now = node_end\n\n while node_now.parent is not None:\n node_now = node_now.parent\n waypoint.append(node_now)\n\n return waypoint\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.s_start.x, self.s_start.y, \"bs\", linewidth=3)\n plt.plot(self.s_goal.x, self.s_goal.y, \"gs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n def plot_visited(self, animation=True):\n if animation:\n count = 0\n for node in self.vertex:\n count += 1\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event:\n [exit(0) if event.key == 'escape' else None])\n if count % 10 == 0:\n plt.pause(0.001)\n else:\n for node in self.vertex:\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n\n def plot_vertex_old(self):\n for node in self.vertex_old:\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n\n def plot_vertex_new(self):\n count = 0\n\n for node in self.vertex_new:\n count += 1\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange')\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event:\n [exit(0) if event.key == 'escape' else None])\n if count % 10 == 0:\n plt.pause(0.001)\n\n @staticmethod\n def plot_path(path, color='red'):\n plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)\n plt.pause(0.01)\n\n\ndef main():\n x_start = (2, 2) # Starting node\n x_goal = (49, 24) # Goal node\n\n drrt = DynamicRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000)\n drrt.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/env.py", "content": "\"\"\"\nEnvironment for rrt_2D\n@author: huiming zhou\n\"\"\"\n\n\nclass Env:\n def __init__(self):\n self.x_range = (0, 50)\n self.y_range = (0, 30)\n self.obs_boundary = self.obs_boundary()\n self.obs_circle = self.obs_circle()\n self.obs_rectangle = self.obs_rectangle()\n\n @staticmethod\n def obs_boundary():\n obs_boundary = [\n [0, 0, 1, 30],\n [0, 30, 50, 1],\n [1, 0, 50, 1],\n [50, 1, 1, 30]\n ]\n return obs_boundary\n\n @staticmethod\n def obs_rectangle():\n obs_rectangle = [\n [14, 12, 8, 2],\n [18, 22, 8, 3],\n [26, 7, 2, 12],\n [32, 14, 10, 2]\n ]\n return obs_rectangle\n\n @staticmethod\n def obs_circle():\n obs_cir = [\n [7, 12, 3],\n [46, 20, 2],\n [15, 5, 2],\n [37, 7, 3],\n [37, 23, 3]\n ]\n\n return obs_cir\n" }, { "path": "Sampling_based_Planning/rrt_2D/extended_rrt.py", "content": "\"\"\"\nEXTENDED_RRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass ExtendedRrt:\n def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_sample_rate, iter_max):\n self.s_start = Node(s_start)\n self.s_goal = Node(s_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.waypoint_sample_rate = waypoint_sample_rate\n self.iter_max = iter_max\n self.vertex = [self.s_start]\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(s_start, s_goal)\n self.utils = utils.Utils()\n self.fig, self.ax = plt.subplots()\n\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n self.path = []\n self.waypoint = []\n\n def planning(self):\n for i in range(self.iter_max):\n node_rand = self.generate_random_node(self.goal_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.vertex.append(node_new)\n dist, _ = self.get_distance_and_angle(node_new, self.s_goal)\n\n if dist <= self.step_len:\n self.new_state(node_new, self.s_goal)\n\n path = self.extract_path(node_new)\n self.plot_grid(\"Extended_RRT\")\n self.plot_visited()\n self.plot_path(path)\n self.path = path\n self.waypoint = self.extract_waypoint(node_new)\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n plt.show()\n\n return\n\n return None\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > 50 or y < 0 or y > 30:\n print(\"Please choose right area!\")\n else:\n x, y = int(x), int(y)\n print(\"Add circle obstacle at: s =\", x, \",\", \"y =\", y)\n self.obs_circle.append([x, y, 2])\n self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)\n path, waypoint = self.replanning()\n\n plt.cla()\n self.plot_grid(\"Extended_RRT\")\n self.plot_path(self.path, color='blue')\n self.plot_visited()\n self.plot_path(path)\n self.path = path\n self.waypoint = waypoint\n self.fig.canvas.draw_idle()\n\n def replanning(self):\n self.vertex = [self.s_start]\n\n for i in range(self.iter_max):\n node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.vertex.append(node_new)\n dist, _ = self.get_distance_and_angle(node_new, self.s_goal)\n\n if dist <= self.step_len:\n self.new_state(node_new, self.s_goal)\n path = self.extract_path(node_new)\n waypoint = self.extract_waypoint(node_new)\n\n return path, waypoint\n\n return None\n\n def generate_random_node(self, goal_sample_rate):\n delta = self.utils.delta\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.s_goal\n\n def generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate):\n delta = self.utils.delta\n p = np.random.random()\n\n if p < goal_sample_rate:\n return self.s_goal\n elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:\n return self.waypoint[np.random.randint(0, len(self.path) - 1)]\n else:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n\n @staticmethod\n def nearest_neighbor(node_list, n):\n return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)\n for nd in node_list]))]\n\n def new_state(self, node_start, node_end):\n dist, theta = self.get_distance_and_angle(node_start, node_end)\n\n dist = min(self.step_len, dist)\n node_new = Node((node_start.x + dist * math.cos(theta),\n node_start.y + dist * math.sin(theta)))\n node_new.parent = node_start\n\n return node_new\n\n def extract_path(self, node_end):\n path = [(self.s_goal.x, self.s_goal.y)]\n node_now = node_end\n\n while node_now.parent is not None:\n node_now = node_now.parent\n path.append((node_now.x, node_now.y))\n\n return path\n\n def extract_waypoint(self, node_end):\n waypoint = [self.s_goal]\n node_now = node_end\n\n while node_now.parent is not None:\n node_now = node_now.parent\n waypoint.append(node_now)\n\n return waypoint\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.s_start.x, self.s_start.y, \"bs\", linewidth=3)\n plt.plot(self.s_goal.x, self.s_goal.y, \"gs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n def plot_visited(self):\n animation = True\n if animation:\n count = 0\n for node in self.vertex:\n count += 1\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event:\n [exit(0) if event.key == 'escape' else None])\n if count % 10 == 0:\n plt.pause(0.001)\n else:\n for node in self.vertex:\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n\n @staticmethod\n def plot_path(path, color='red'):\n plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)\n plt.pause(0.01)\n\n\ndef main():\n x_start = (2, 2) # Starting node\n x_goal = (49, 24) # Goal node\n\n errt = ExtendedRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000)\n errt.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/fast_marching_trees.py", "content": "\"\"\"\nFast Marching Trees (FMT*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n self.cost = np.inf\n\n\nclass FMT:\n def __init__(self, x_start, x_goal, search_radius):\n self.x_init = Node(x_start)\n self.x_goal = Node(x_goal)\n self.search_radius = search_radius\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(x_start, x_goal)\n self.utils = utils.Utils()\n\n self.fig, self.ax = plt.subplots()\n self.delta = self.utils.delta\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n self.V = set()\n self.V_unvisited = set()\n self.V_open = set()\n self.V_closed = set()\n self.sample_numbers = 1000\n\n def Init(self):\n samples = self.SampleFree()\n\n self.x_init.cost = 0.0\n self.V.add(self.x_init)\n self.V.update(samples)\n self.V_unvisited.update(samples)\n self.V_unvisited.add(self.x_goal)\n self.V_open.add(self.x_init)\n\n def Planning(self):\n self.Init()\n z = self.x_init\n n = self.sample_numbers\n rn = self.search_radius * math.sqrt((math.log(n) / n))\n Visited = []\n\n while z is not self.x_goal:\n V_open_new = set()\n X_near = self.Near(self.V_unvisited, z, rn)\n Visited.append(z)\n\n for x in X_near:\n Y_near = self.Near(self.V_open, x, rn)\n cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near}\n y_min = min(cost_list, key=cost_list.get)\n\n if not self.utils.is_collision(y_min, x):\n x.parent = y_min\n V_open_new.add(x)\n self.V_unvisited.remove(x)\n x.cost = y_min.cost + self.Cost(y_min, x)\n\n self.V_open.update(V_open_new)\n self.V_open.remove(z)\n self.V_closed.add(z)\n\n if not self.V_open:\n print(\"open set empty!\")\n break\n\n cost_open = {y: y.cost for y in self.V_open}\n z = min(cost_open, key=cost_open.get)\n\n # node_end = self.ChooseGoalPoint()\n path_x, path_y = self.ExtractPath()\n self.animation(path_x, path_y, Visited[1: len(Visited)])\n\n def ChooseGoalPoint(self):\n Near = self.Near(self.V, self.x_goal, 2.0)\n cost = {y: y.cost + self.Cost(y, self.x_goal) for y in Near}\n\n return min(cost, key=cost.get)\n\n def ExtractPath(self):\n path_x, path_y = [], []\n node = self.x_goal\n\n while node.parent:\n path_x.append(node.x)\n path_y.append(node.y)\n node = node.parent\n\n path_x.append(self.x_init.x)\n path_y.append(self.x_init.y)\n\n return path_x, path_y\n\n def Cost(self, x_start, x_end):\n if self.utils.is_collision(x_start, x_end):\n return np.inf\n else:\n return self.calc_dist(x_start, x_end)\n\n @staticmethod\n def calc_dist(x_start, x_end):\n return math.hypot(x_start.x - x_end.x, x_start.y - x_end.y)\n\n @staticmethod\n def Near(nodelist, z, rn):\n return {nd for nd in nodelist\n if 0 < (nd.x - z.x) ** 2 + (nd.y - z.y) ** 2 <= rn ** 2}\n\n def SampleFree(self):\n n = self.sample_numbers\n delta = self.utils.delta\n Sample = set()\n\n ind = 0\n while ind < n:\n node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n if self.utils.is_inside_obs(node):\n continue\n else:\n Sample.add(node)\n ind += 1\n\n return Sample\n\n def animation(self, path_x, path_y, visited):\n self.plot_grid(\"Fast Marching Trees (FMT*)\")\n\n for node in self.V:\n plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3)\n\n count = 0\n for node in visited:\n count += 1\n plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')\n plt.gcf().canvas.mpl_connect(\n 'key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n if count % 10 == 0:\n plt.pause(0.001)\n\n plt.plot(path_x, path_y, linewidth=2, color='red')\n plt.pause(0.01)\n plt.show()\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.x_init.x, self.x_init.y, \"bs\", linewidth=3)\n plt.plot(self.x_goal.x, self.x_goal.y, \"rs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n\ndef main():\n x_start = (18, 8) # Starting node\n x_goal = (37, 18) # Goal node\n\n fmt = FMT(x_start, x_goal, 40)\n fmt.Planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/informed_rrt_star.py", "content": "\"\"\"\nINFORMED_RRT_STAR 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom scipy.spatial.transform import Rotation as Rot\nimport matplotlib.patches as patches\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass IRrtStar:\n def __init__(self, x_start, x_goal, step_len,\n goal_sample_rate, search_radius, iter_max):\n self.x_start = Node(x_start)\n self.x_goal = Node(x_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.search_radius = search_radius\n self.iter_max = iter_max\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(x_start, x_goal)\n self.utils = utils.Utils()\n\n self.fig, self.ax = plt.subplots()\n self.delta = self.utils.delta\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n self.V = [self.x_start]\n self.X_soln = set()\n self.path = None\n\n def init(self):\n cMin, theta = self.get_distance_and_angle(self.x_start, self.x_goal)\n C = self.RotationToWorldFrame(self.x_start, self.x_goal, cMin)\n xCenter = np.array([[(self.x_start.x + self.x_goal.x) / 2.0],\n [(self.x_start.y + self.x_goal.y) / 2.0], [0.0]])\n x_best = self.x_start\n\n return theta, cMin, xCenter, C, x_best\n\n def planning(self):\n theta, dist, x_center, C, x_best = self.init()\n c_best = np.inf\n\n for k in range(self.iter_max):\n if self.X_soln:\n cost = {node: self.Cost(node) for node in self.X_soln}\n x_best = min(cost, key=cost.get)\n c_best = cost[x_best]\n\n x_rand = self.Sample(c_best, dist, x_center, C)\n x_nearest = self.Nearest(self.V, x_rand)\n x_new = self.Steer(x_nearest, x_rand)\n\n if x_new and not self.utils.is_collision(x_nearest, x_new):\n X_near = self.Near(self.V, x_new)\n c_min = self.Cost(x_nearest) + self.Line(x_nearest, x_new)\n self.V.append(x_new)\n\n # choose parent\n for x_near in X_near:\n c_new = self.Cost(x_near) + self.Line(x_near, x_new)\n if c_new < c_min:\n x_new.parent = x_near\n c_min = c_new\n\n # rewire\n for x_near in X_near:\n c_near = self.Cost(x_near)\n c_new = self.Cost(x_new) + self.Line(x_new, x_near)\n if c_new < c_near:\n x_near.parent = x_new\n\n if self.InGoalRegion(x_new):\n if not self.utils.is_collision(x_new, self.x_goal):\n self.X_soln.add(x_new)\n # new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal)\n # if new_cost < c_best:\n # c_best = new_cost\n # x_best = x_new\n\n if k % 20 == 0:\n self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)\n\n self.path = self.ExtractPath(x_best)\n self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta)\n plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')\n plt.pause(0.01)\n plt.show()\n\n def Steer(self, x_start, x_goal):\n dist, theta = self.get_distance_and_angle(x_start, x_goal)\n dist = min(self.step_len, dist)\n node_new = Node((x_start.x + dist * math.cos(theta),\n x_start.y + dist * math.sin(theta)))\n node_new.parent = x_start\n\n return node_new\n\n def Near(self, nodelist, node):\n n = len(nodelist) + 1\n r = 50 * math.sqrt((math.log(n) / n))\n\n dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]\n X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and\n not self.utils.is_collision(nodelist[ind], node)]\n\n return X_near\n\n def Sample(self, c_max, c_min, x_center, C):\n if c_max < np.inf:\n r = [c_max / 2.0,\n math.sqrt(c_max ** 2 - c_min ** 2) / 2.0,\n math.sqrt(c_max ** 2 - c_min ** 2) / 2.0]\n L = np.diag(r)\n\n while True:\n x_ball = self.SampleUnitBall()\n x_rand = np.dot(np.dot(C, L), x_ball) + x_center\n if self.x_range[0] + self.delta <= x_rand[0] <= self.x_range[1] - self.delta and \\\n self.y_range[0] + self.delta <= x_rand[1] <= self.y_range[1] - self.delta:\n break\n x_rand = Node((x_rand[(0, 0)], x_rand[(1, 0)]))\n else:\n x_rand = self.SampleFreeSpace()\n\n return x_rand\n\n @staticmethod\n def SampleUnitBall():\n while True:\n x, y = random.uniform(-1, 1), random.uniform(-1, 1)\n if x ** 2 + y ** 2 < 1:\n return np.array([[x], [y], [0.0]])\n\n def SampleFreeSpace(self):\n delta = self.delta\n\n if np.random.random() > self.goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.x_goal\n\n def ExtractPath(self, node):\n path = [[self.x_goal.x, self.x_goal.y]]\n\n while node.parent:\n path.append([node.x, node.y])\n node = node.parent\n\n path.append([self.x_start.x, self.x_start.y])\n\n return path\n\n def InGoalRegion(self, node):\n if self.Line(node, self.x_goal) < self.step_len:\n return True\n\n return False\n\n @staticmethod\n def RotationToWorldFrame(x_start, x_goal, L):\n a1 = np.array([[(x_goal.x - x_start.x) / L],\n [(x_goal.y - x_start.y) / L], [0.0]])\n e1 = np.array([[1.0], [0.0], [0.0]])\n M = a1 @ e1.T\n U, _, V_T = np.linalg.svd(M, True, True)\n C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T\n\n return C\n\n @staticmethod\n def Nearest(nodelist, n):\n return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2\n for nd in nodelist]))]\n\n @staticmethod\n def Line(x_start, x_goal):\n return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)\n\n def Cost(self, node):\n if node == self.x_start:\n return 0.0\n\n if node.parent is None:\n return np.inf\n\n cost = 0.0\n while node.parent:\n cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)\n node = node.parent\n\n return cost\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def animation(self, x_center=None, c_best=None, dist=None, theta=None):\n plt.cla()\n self.plot_grid(\"Informed rrt*, N = \" + str(self.iter_max))\n plt.gcf().canvas.mpl_connect(\n 'key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n for node in self.V:\n if node.parent:\n plt.plot([node.x, node.parent.x], [node.y, node.parent.y], \"-g\")\n\n if c_best != np.inf:\n self.draw_ellipse(x_center, c_best, dist, theta)\n\n plt.pause(0.01)\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.x_start.x, self.x_start.y, \"bs\", linewidth=3)\n plt.plot(self.x_goal.x, self.x_goal.y, \"rs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n @staticmethod\n def draw_ellipse(x_center, c_best, dist, theta):\n a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0\n b = c_best / 2.0\n angle = math.pi / 2.0 - theta\n cx = x_center[0]\n cy = x_center[1]\n t = np.arange(0, 2 * math.pi + 0.1, 0.1)\n x = [a * math.cos(it) for it in t]\n y = [b * math.sin(it) for it in t]\n rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]\n fx = rot @ np.array([x, y])\n px = np.array(fx[0, :] + cx).flatten()\n py = np.array(fx[1, :] + cy).flatten()\n plt.plot(cx, cy, \".b\")\n plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2)\n\n\ndef main():\n x_start = (18, 8) # Starting node\n x_goal = (37, 18) # Goal node\n\n rrt_star = IRrtStar(x_start, x_goal, 1, 0.10, 12, 1000)\n rrt_star.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/plotting.py", "content": "\"\"\"\nPlotting tools for Sampling-based algorithms\n@author: huiming zhou\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env\n\n\nclass Plotting:\n def __init__(self, x_start, x_goal):\n self.xI, self.xG = x_start, x_goal\n self.env = env.Env()\n self.obs_bound = self.env.obs_boundary\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n\n def animation(self, nodelist, path, name, animation=False):\n self.plot_grid(name)\n self.plot_visited(nodelist, animation)\n self.plot_path(path)\n\n def animation_connect(self, V1, V2, path, name):\n self.plot_grid(name)\n self.plot_visited_connect(V1, V2)\n self.plot_path(path)\n\n def plot_grid(self, name):\n fig, ax = plt.subplots()\n\n for (ox, oy, w, h) in self.obs_bound:\n ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.xI[0], self.xI[1], \"bs\", linewidth=3)\n plt.plot(self.xG[0], self.xG[1], \"gs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n @staticmethod\n def plot_visited(nodelist, animation):\n if animation:\n count = 0\n for node in nodelist:\n count += 1\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event:\n [exit(0) if event.key == 'escape' else None])\n if count % 10 == 0:\n plt.pause(0.001)\n else:\n for node in nodelist:\n if node.parent:\n plt.plot([node.parent.x, node.x], [node.parent.y, node.y], \"-g\")\n\n @staticmethod\n def plot_visited_connect(V1, V2):\n len1, len2 = len(V1), len(V2)\n\n for k in range(max(len1, len2)):\n if k < len1:\n if V1[k].parent:\n plt.plot([V1[k].x, V1[k].parent.x], [V1[k].y, V1[k].parent.y], \"-g\")\n if k < len2:\n if V2[k].parent:\n plt.plot([V2[k].x, V2[k].parent.x], [V2[k].y, V2[k].parent.y], \"-g\")\n\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n if k % 2 == 0:\n plt.pause(0.001)\n\n plt.pause(0.01)\n\n @staticmethod\n def plot_path(path):\n if len(path) != 0:\n plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)\n plt.pause(0.01)\n plt.show()\n" }, { "path": "Sampling_based_Planning/rrt_2D/queue.py", "content": "import collections\nimport heapq\n\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFIFO is designed for First-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.popleft() # leave from front\n\n\nclass QueueLIFO:\n \"\"\"\n Class: QueueLIFO\n Description: QueueLIFO is designed for Last-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.pop() # leave from back\n\n\nclass QueuePrior:\n \"\"\"\n Class: QueuePrior\n Description: QueuePrior reorders elements using value [priority]\n \"\"\"\n\n def __init__(self):\n self.queue = []\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, item, priority):\n heapq.heappush(self.queue, (priority, item)) # reorder s using priority\n\n def get(self):\n return heapq.heappop(self.queue)[1] # pop out the smallest item\n\n def enumerate(self):\n return self.queue\n" }, { "path": "Sampling_based_Planning/rrt_2D/rrt.py", "content": "\"\"\"\nRRT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass Rrt:\n def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):\n self.s_start = Node(s_start)\n self.s_goal = Node(s_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.iter_max = iter_max\n self.vertex = [self.s_start]\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(s_start, s_goal)\n self.utils = utils.Utils()\n\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n def planning(self):\n for i in range(self.iter_max):\n node_rand = self.generate_random_node(self.goal_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.vertex.append(node_new)\n dist, _ = self.get_distance_and_angle(node_new, self.s_goal)\n\n if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal):\n self.new_state(node_new, self.s_goal)\n return self.extract_path(node_new)\n\n return None\n\n def generate_random_node(self, goal_sample_rate):\n delta = self.utils.delta\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.s_goal\n\n @staticmethod\n def nearest_neighbor(node_list, n):\n return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)\n for nd in node_list]))]\n\n def new_state(self, node_start, node_end):\n dist, theta = self.get_distance_and_angle(node_start, node_end)\n\n dist = min(self.step_len, dist)\n node_new = Node((node_start.x + dist * math.cos(theta),\n node_start.y + dist * math.sin(theta)))\n node_new.parent = node_start\n\n return node_new\n\n def extract_path(self, node_end):\n path = [(self.s_goal.x, self.s_goal.y)]\n node_now = node_end\n\n while node_now.parent is not None:\n node_now = node_now.parent\n path.append((node_now.x, node_now.y))\n\n return path\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n\ndef main():\n x_start = (2, 2) # Starting node\n x_goal = (49, 24) # Goal node\n\n rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000)\n path = rrt.planning()\n\n if path:\n rrt.plotting.animation(rrt.vertex, path, \"RRT\", True)\n else:\n print(\"No Path Found!\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/rrt_connect.py", "content": "\"\"\"\nRRT_CONNECT_2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport copy\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass RrtConnect:\n def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):\n self.s_start = Node(s_start)\n self.s_goal = Node(s_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.iter_max = iter_max\n self.V1 = [self.s_start]\n self.V2 = [self.s_goal]\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(s_start, s_goal)\n self.utils = utils.Utils()\n\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n def planning(self):\n for i in range(self.iter_max):\n node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate)\n node_near = self.nearest_neighbor(self.V1, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n self.V1.append(node_new)\n node_near_prim = self.nearest_neighbor(self.V2, node_new)\n node_new_prim = self.new_state(node_near_prim, node_new)\n\n if node_new_prim and not self.utils.is_collision(node_new_prim, node_near_prim):\n self.V2.append(node_new_prim)\n\n while True:\n node_new_prim2 = self.new_state(node_new_prim, node_new)\n if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim):\n self.V2.append(node_new_prim2)\n node_new_prim = self.change_node(node_new_prim, node_new_prim2)\n else:\n break\n\n if self.is_node_same(node_new_prim, node_new):\n break\n\n if self.is_node_same(node_new_prim, node_new):\n return self.extract_path(node_new, node_new_prim)\n\n if len(self.V2) < len(self.V1):\n list_mid = self.V2\n self.V2 = self.V1\n self.V1 = list_mid\n\n return None\n\n @staticmethod\n def change_node(node_new_prim, node_new_prim2):\n node_new = Node((node_new_prim2.x, node_new_prim2.y))\n node_new.parent = node_new_prim\n\n return node_new\n\n @staticmethod\n def is_node_same(node_new_prim, node_new):\n if node_new_prim.x == node_new.x and \\\n node_new_prim.y == node_new.y:\n return True\n\n return False\n\n def generate_random_node(self, sample_goal, goal_sample_rate):\n delta = self.utils.delta\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return sample_goal\n\n @staticmethod\n def nearest_neighbor(node_list, n):\n return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)\n for nd in node_list]))]\n\n def new_state(self, node_start, node_end):\n dist, theta = self.get_distance_and_angle(node_start, node_end)\n\n dist = min(self.step_len, dist)\n node_new = Node((node_start.x + dist * math.cos(theta),\n node_start.y + dist * math.sin(theta)))\n node_new.parent = node_start\n\n return node_new\n\n @staticmethod\n def extract_path(node_new, node_new_prim):\n path1 = [(node_new.x, node_new.y)]\n node_now = node_new\n\n while node_now.parent is not None:\n node_now = node_now.parent\n path1.append((node_now.x, node_now.y))\n\n path2 = [(node_new_prim.x, node_new_prim.y)]\n node_now = node_new_prim\n\n while node_now.parent is not None:\n node_now = node_now.parent\n path2.append((node_now.x, node_now.y))\n\n return list(list(reversed(path1)) + path2)\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n\ndef main():\n x_start = (2, 2) # Starting node\n x_goal = (49, 24) # Goal node\n\n rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.05, 5000)\n path = rrt_conn.planning()\n\n rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, \"RRT_CONNECT\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/rrt_sharp.py", "content": "" }, { "path": "Sampling_based_Planning/rrt_2D/rrt_star.py", "content": "\"\"\"\nRRT_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport numpy as np\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils, queue\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass RrtStar:\n def __init__(self, x_start, x_goal, step_len,\n goal_sample_rate, search_radius, iter_max):\n self.s_start = Node(x_start)\n self.s_goal = Node(x_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.search_radius = search_radius\n self.iter_max = iter_max\n self.vertex = [self.s_start]\n self.path = []\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(x_start, x_goal)\n self.utils = utils.Utils()\n\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n def planning(self):\n for k in range(self.iter_max):\n node_rand = self.generate_random_node(self.goal_sample_rate)\n node_near = self.nearest_neighbor(self.vertex, node_rand)\n node_new = self.new_state(node_near, node_rand)\n\n if k % 500 == 0:\n print(k)\n\n if node_new and not self.utils.is_collision(node_near, node_new):\n neighbor_index = self.find_near_neighbor(node_new)\n self.vertex.append(node_new)\n\n if neighbor_index:\n self.choose_parent(node_new, neighbor_index)\n self.rewire(node_new, neighbor_index)\n\n index = self.search_goal_parent()\n self.path = self.extract_path(self.vertex[index])\n\n self.plotting.animation(self.vertex, self.path, \"rrt*, N = \" + str(self.iter_max))\n\n def new_state(self, node_start, node_goal):\n dist, theta = self.get_distance_and_angle(node_start, node_goal)\n\n dist = min(self.step_len, dist)\n node_new = Node((node_start.x + dist * math.cos(theta),\n node_start.y + dist * math.sin(theta)))\n\n node_new.parent = node_start\n\n return node_new\n\n def choose_parent(self, node_new, neighbor_index):\n cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index]\n\n cost_min_index = neighbor_index[int(np.argmin(cost))]\n node_new.parent = self.vertex[cost_min_index]\n\n def rewire(self, node_new, neighbor_index):\n for i in neighbor_index:\n node_neighbor = self.vertex[i]\n\n if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):\n node_neighbor.parent = node_new\n\n def search_goal_parent(self):\n dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]\n node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]\n\n if len(node_index) > 0:\n cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index\n if not self.utils.is_collision(self.vertex[i], self.s_goal)]\n return node_index[int(np.argmin(cost_list))]\n\n return len(self.vertex) - 1\n\n def get_new_cost(self, node_start, node_end):\n dist, _ = self.get_distance_and_angle(node_start, node_end)\n\n return self.cost(node_start) + dist\n\n def generate_random_node(self, goal_sample_rate):\n delta = self.utils.delta\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.s_goal\n\n def find_near_neighbor(self, node_new):\n n = len(self.vertex) + 1\n r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)\n\n dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]\n dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and\n not self.utils.is_collision(node_new, self.vertex[ind])]\n\n return dist_table_index\n\n @staticmethod\n def nearest_neighbor(node_list, n):\n return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)\n for nd in node_list]))]\n\n @staticmethod\n def cost(node_p):\n node = node_p\n cost = 0.0\n\n while node.parent:\n cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)\n node = node.parent\n\n return cost\n\n def update_cost(self, parent_node):\n OPEN = queue.QueueFIFO()\n OPEN.put(parent_node)\n\n while not OPEN.empty():\n node = OPEN.get()\n\n if len(node.child) == 0:\n continue\n\n for node_c in node.child:\n node_c.Cost = self.get_new_cost(node, node_c)\n OPEN.put(node_c)\n\n def extract_path(self, node_end):\n path = [[self.s_goal.x, self.s_goal.y]]\n node = node_end\n\n while node.parent is not None:\n path.append([node.x, node.y])\n node = node.parent\n path.append([node.x, node.y])\n\n return path\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n\ndef main():\n x_start = (18, 8) # Starting node\n x_goal = (37, 18) # Goal node\n\n rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)\n rrt_star.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/rrt_star_smart.py", "content": "\"\"\"\nRRT_STAR_SMART 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport random\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\nfrom scipy.spatial.transform import Rotation as Rot\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env, plotting, utils\n\n\nclass Node:\n def __init__(self, n):\n self.x = n[0]\n self.y = n[1]\n self.parent = None\n\n\nclass RrtStarSmart:\n def __init__(self, x_start, x_goal, step_len,\n goal_sample_rate, search_radius, iter_max):\n self.x_start = Node(x_start)\n self.x_goal = Node(x_goal)\n self.step_len = step_len\n self.goal_sample_rate = goal_sample_rate\n self.search_radius = search_radius\n self.iter_max = iter_max\n\n self.env = env.Env()\n self.plotting = plotting.Plotting(x_start, x_goal)\n self.utils = utils.Utils()\n\n self.fig, self.ax = plt.subplots()\n self.delta = self.utils.delta\n self.x_range = self.env.x_range\n self.y_range = self.env.y_range\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n self.V = [self.x_start]\n self.beacons = []\n self.beacons_radius = 2\n self.direct_cost_old = np.inf\n self.obs_vertex = self.utils.get_obs_vertex()\n self.path = None\n\n def planning(self):\n n = 0\n b = 2\n InitPathFlag = False\n self.ReformObsVertex()\n\n for k in range(self.iter_max):\n if k % 200 == 0:\n print(k)\n\n if (k - n) % b == 0 and len(self.beacons) > 0:\n x_rand = self.Sample(self.beacons)\n else:\n x_rand = self.Sample()\n\n x_nearest = self.Nearest(self.V, x_rand)\n x_new = self.Steer(x_nearest, x_rand)\n\n if x_new and not self.utils.is_collision(x_nearest, x_new):\n X_near = self.Near(self.V, x_new)\n self.V.append(x_new)\n\n if X_near:\n # choose parent\n cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]\n x_new.parent = X_near[int(np.argmin(cost_list))]\n\n # rewire\n c_min = self.Cost(x_new)\n for x_near in X_near:\n c_near = self.Cost(x_near)\n c_new = c_min + self.Line(x_new, x_near)\n if c_new < c_near:\n x_near.parent = x_new\n\n if not InitPathFlag and self.InitialPathFound(x_new):\n InitPathFlag = True\n n = k\n\n if InitPathFlag:\n self.PathOptimization(x_new)\n if k % 5 == 0:\n self.animation()\n\n self.path = self.ExtractPath()\n self.animation()\n plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')\n plt.pause(0.01)\n plt.show()\n\n def PathOptimization(self, node):\n direct_cost_new = 0.0\n node_end = self.x_goal\n\n while node.parent:\n node_parent = node.parent\n if not self.utils.is_collision(node_parent, node_end):\n node_end.parent = node_parent\n else:\n direct_cost_new += self.Line(node, node_end)\n node_end = node\n\n node = node_parent\n\n if direct_cost_new < self.direct_cost_old:\n self.direct_cost_old = direct_cost_new\n self.UpdateBeacons()\n\n def UpdateBeacons(self):\n node = self.x_goal\n beacons = []\n\n while node.parent:\n near_vertex = [v for v in self.obs_vertex\n if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]\n if len(near_vertex) > 0:\n for v in near_vertex:\n beacons.append(v)\n\n node = node.parent\n\n self.beacons = beacons\n\n def ReformObsVertex(self):\n obs_vertex = []\n\n for obs in self.obs_vertex:\n for vertex in obs:\n obs_vertex.append(vertex)\n\n self.obs_vertex = obs_vertex\n\n def Steer(self, x_start, x_goal):\n dist, theta = self.get_distance_and_angle(x_start, x_goal)\n dist = min(self.step_len, dist)\n node_new = Node((x_start.x + dist * math.cos(theta),\n x_start.y + dist * math.sin(theta)))\n node_new.parent = x_start\n\n return node_new\n\n def Near(self, nodelist, node):\n n = len(self.V) + 1\n r = 50 * math.sqrt((math.log(n) / n))\n\n dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]\n X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and\n not self.utils.is_collision(node, nodelist[ind])]\n\n return X_near\n\n def Sample(self, goal=None):\n if goal is None:\n delta = self.utils.delta\n goal_sample_rate = self.goal_sample_rate\n\n if np.random.random() > goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.x_goal\n else:\n R = self.beacons_radius\n r = random.uniform(0, R)\n theta = random.uniform(0, 2 * math.pi)\n ind = random.randint(0, len(goal) - 1)\n\n return Node((goal[ind][0] + r * math.cos(theta),\n goal[ind][1] + r * math.sin(theta)))\n\n def SampleFreeSpace(self):\n delta = self.delta\n\n if np.random.random() > self.goal_sample_rate:\n return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),\n np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))\n\n return self.x_goal\n\n def ExtractPath(self):\n path = []\n node = self.x_goal\n\n while node.parent:\n path.append([node.x, node.y])\n node = node.parent\n\n path.append([self.x_start.x, self.x_start.y])\n\n return path\n\n def InitialPathFound(self, node):\n if self.Line(node, self.x_goal) < self.step_len:\n return True\n\n return False\n\n @staticmethod\n def Nearest(nodelist, n):\n return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2\n for nd in nodelist]))]\n\n @staticmethod\n def Line(x_start, x_goal):\n return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)\n\n @staticmethod\n def Cost(node):\n cost = 0.0\n if node.parent is None:\n return cost\n\n while node.parent:\n cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)\n node = node.parent\n\n return cost\n\n @staticmethod\n def get_distance_and_angle(node_start, node_end):\n dx = node_end.x - node_start.x\n dy = node_end.y - node_start.y\n return math.hypot(dx, dy), math.atan2(dy, dx)\n\n def animation(self):\n plt.cla()\n self.plot_grid(\"rrt*-Smart, N = \" + str(self.iter_max))\n plt.gcf().canvas.mpl_connect(\n 'key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n for node in self.V:\n if node.parent:\n plt.plot([node.x, node.parent.x], [node.y, node.parent.y], \"-g\")\n\n if self.beacons:\n theta = np.arange(0, 2 * math.pi, 0.1)\n r = self.beacons_radius\n\n for v in self.beacons:\n x = v[0] + r * np.cos(theta)\n y = v[1] + r * np.sin(theta)\n plt.plot(x, y, linestyle='--', linewidth=2, color='darkorange')\n\n plt.pause(0.01)\n\n def plot_grid(self, name):\n\n for (ox, oy, w, h) in self.obs_boundary:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='black',\n fill=True\n )\n )\n\n for (ox, oy, w, h) in self.obs_rectangle:\n self.ax.add_patch(\n patches.Rectangle(\n (ox, oy), w, h,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n for (ox, oy, r) in self.obs_circle:\n self.ax.add_patch(\n patches.Circle(\n (ox, oy), r,\n edgecolor='black',\n facecolor='gray',\n fill=True\n )\n )\n\n plt.plot(self.x_start.x, self.x_start.y, \"bs\", linewidth=3)\n plt.plot(self.x_goal.x, self.x_goal.y, \"rs\", linewidth=3)\n\n plt.title(name)\n plt.axis(\"equal\")\n\n\ndef main():\n x_start = (18, 8) # Starting node\n x_goal = (37, 18) # Goal node\n\n rrt = RrtStarSmart(x_start, x_goal, 1.5, 0.10, 0, 1000)\n rrt.planning()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Sampling_based_Planning/rrt_2D/utils.py", "content": "\"\"\"\nutils for collision check\n@author: huiming zhou\n\"\"\"\n\nimport math\nimport numpy as np\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Sampling_based_Planning/\")\n\nfrom Sampling_based_Planning.rrt_2D import env\nfrom Sampling_based_Planning.rrt_2D.rrt import Node\n\n\nclass Utils:\n def __init__(self):\n self.env = env.Env()\n\n self.delta = 0.5\n self.obs_circle = self.env.obs_circle\n self.obs_rectangle = self.env.obs_rectangle\n self.obs_boundary = self.env.obs_boundary\n\n def update_obs(self, obs_cir, obs_bound, obs_rec):\n self.obs_circle = obs_cir\n self.obs_boundary = obs_bound\n self.obs_rectangle = obs_rec\n\n def get_obs_vertex(self):\n delta = self.delta\n obs_list = []\n\n for (ox, oy, w, h) in self.obs_rectangle:\n vertex_list = [[ox - delta, oy - delta],\n [ox + w + delta, oy - delta],\n [ox + w + delta, oy + h + delta],\n [ox - delta, oy + h + delta]]\n obs_list.append(vertex_list)\n\n return obs_list\n\n def is_intersect_rec(self, start, end, o, d, a, b):\n v1 = [o[0] - a[0], o[1] - a[1]]\n v2 = [b[0] - a[0], b[1] - a[1]]\n v3 = [-d[1], d[0]]\n\n div = np.dot(v2, v3)\n\n if div == 0:\n return False\n\n t1 = np.linalg.norm(np.cross(v2, v1)) / div\n t2 = np.dot(v1, v3) / div\n\n if t1 >= 0 and 0 <= t2 <= 1:\n shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))\n dist_obs = self.get_dist(start, shot)\n dist_seg = self.get_dist(start, end)\n if dist_obs <= dist_seg:\n return True\n\n return False\n\n def is_intersect_circle(self, o, d, a, r):\n d2 = np.dot(d, d)\n delta = self.delta\n\n if d2 == 0:\n return False\n\n t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2\n\n if 0 <= t <= 1:\n shot = Node((o[0] + t * d[0], o[1] + t * d[1]))\n if self.get_dist(shot, Node(a)) <= r + delta:\n return True\n\n return False\n\n def is_collision(self, start, end):\n if self.is_inside_obs(start) or self.is_inside_obs(end):\n return True\n\n o, d = self.get_ray(start, end)\n obs_vertex = self.get_obs_vertex()\n\n for (v1, v2, v3, v4) in obs_vertex:\n if self.is_intersect_rec(start, end, o, d, v1, v2):\n return True\n if self.is_intersect_rec(start, end, o, d, v2, v3):\n return True\n if self.is_intersect_rec(start, end, o, d, v3, v4):\n return True\n if self.is_intersect_rec(start, end, o, d, v4, v1):\n return True\n\n for (x, y, r) in self.obs_circle:\n if self.is_intersect_circle(o, d, [x, y], r):\n return True\n\n return False\n\n def is_inside_obs(self, node):\n delta = self.delta\n\n for (x, y, r) in self.obs_circle:\n if math.hypot(node.x - x, node.y - y) <= r + delta:\n return True\n\n for (x, y, w, h) in self.obs_rectangle:\n if 0 <= node.x - (x - delta) <= w + 2 * delta \\\n and 0 <= node.y - (y - delta) <= h + 2 * delta:\n return True\n\n for (x, y, w, h) in self.obs_boundary:\n if 0 <= node.x - (x - delta) <= w + 2 * delta \\\n and 0 <= node.y - (y - delta) <= h + 2 * delta:\n return True\n\n return False\n\n @staticmethod\n def get_ray(start, end):\n orig = [start.x, start.y]\n direc = [end.x - start.x, end.y - start.y]\n return orig, direc\n\n @staticmethod\n def get_dist(start, end):\n return math.hypot(end.x - start.x, end.y - start.y)\n" }, { "path": "Sampling_based_Planning/rrt_3D/ABIT_star3D.py", "content": "# This is Advanced Batched Informed Tree star 3D algorithm \n# implementation\n\"\"\"\nThis is ABIT* code for 3D\n@author: yue qi \nsource: M.P.Strub, J.D.Gammel. \"Advanced BIT* (ABIT*):\n Sampling-Based Planning with Advanced Graph-Search Techniques\" \n\"\"\"\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport time\nimport copy\n\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide\nfrom rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\nfrom rrt_3D.queue import MinheapPQ\n\nclass ABIT_star:\n\n def __init__(self):\n self.env = env()\n self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)\n self.maxiter = 1000\n self.done = False\n self.n = 1000# used in radius calc r(q)\n self.lam = 10 # used in radius calc r(q)\n\n def run(self):\n V, E = {self.xstart}, set()\n T = (V,E)\n Xunconnected = {self.xgoal}\n q = len(V) + len(Xunconnected)\n eps_infl, eps_trunc = np.inf, np.inf\n Vclosed, Vinconsistent = set(), set()\n Q = self.expand(self.xstart, T, Xunconnected, np.inf)\n\n ind = 0\n while True:\n if self.is_search_marked_finished():\n if self.update_approximation(eps_infl, eps_trunc):\n T, Xunconnected = self.prune(T, Xunconnected, self.xgoal)\n Xunconnected.update(self.sample(m, self.xgoal))\n q = len(V) + len(Xunconnected)\n Q = self.expand({self.xstart}, T, Xunconnected, self.r(q))\n else:\n Q.update(self.expand(Vinconsistent, T, Xunconnected, self.r(q)))\n eps_infl = self.update_inflation_factor()\n eps_trunc = self.update_truncation_factor()\n Vclosed = set()\n Vinconsistent = set()\n self.mark_search_unfinished()\n else:\n state_tuple = list(Q)\n (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] )]\n Q = Q.difference({(xp, xc)})\n if (xp, xc) in E:\n if xc in Vclosed:\n Vinconsistent.add(xc)\n else:\n Q.update(self.expand({xc}, T, Xunconnected, self.r(q)))\n Vclosed.add(xc)\n elif eps_trunc * (self.g_T(xp) + self.c_hat(xi, xj) + self.h_hat(xc)) <= self.g_T(self.xgoal):\n if self.g_T(xp) + self.c_hat(xp, xc) < self.g_T(xc):\n if self.g_T(xp) + self.c(xp, xc) + self.h_hat(xc) < self.g_T(self.xgoal):\n if self.g_T(xp) + self.c(xp, xc) < self.g_T(xc):\n if xc in V:\n E = E.difference({(xprev, xc)})\n else:\n Xunconnected.difference_update({xc})\n V.add(xc)\n E.add((xp, xc))\n if xc in Vclosed:\n Vinconsistent.add(xc)\n else:\n Q.update(self.expand({xc}, T, Xunconnected, self.r(q)))\n Vclosed.add(xc)\n else: \n self.mark_search_finished()\n ind += 1\n # until stop\n if ind > self.maxiter:\n break\n\n def sample(self, m, xgoal):\n pass\n\n def expand(self, set_xi, T, Xunconnected, r):\n V, E = T\n Eout = set()\n for xp in set_xi:\n Eout.update({(x1, x2) for (x1, x2) in E if x1 == xp})\n for xc in {x for x in Xunconnected.union(V) if getDist(xp, x) <= r}:\n if self.g_hat(xp) + self.c_hat(xp, xc) + self.h_hat(xc) <= self.g_T(self.xgoal):\n if self.g_hat(xp) + self.c_hat(xp, xc) <= self.g_hat(xc):\n Eout.add((xp,xc))\n return Eout\n\n def prune(self, T, Xunconnected, xgoal):\n V, E = T\n Xunconnected.difference_update({x for x in Xunconnected if self.f_hat(x) >= self.g_T(xgoal)})\n V.difference_update({x for x in V if self.f_hat(x) > self.g_T(xgoal)})\n 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)})\n Xunconnected.update({xc for (xp, xc) in E if (xp not in V) and (xc in V)})\n V.difference_update({xc for (xp, xc) in E if (xp not in V) and (xc in V)})\n T = (V,E)\n return T, Xunconnected\n\n def g_hat(self, x):\n pass\n \n def h_hat(self, x):\n pass\n\n def c_hat(self, x1, x2):\n pass\n\n def f_hat(self, x):\n pass\n\n def g_T(self, x):\n pass\n\n def r(self, q):\n return self.eta * (2 * (1 + 1/self.n) * (self.Lambda(self.Xf_hat) / self.Zeta) * (np.log(q) / q)) ** (1 / self.n)\n\n def Lambda(self, inputset):\n pass\n\n def Zeta(self):\n pass\n\n def is_search_marked_finished(self):\n return self.done\n\n def mark_search_unfinished(self):\n self.done = False\n return self.done\n\n def mark_search_finished(self):\n self.done = True\n return self.done\n" }, { "path": "Sampling_based_Planning/rrt_3D/BIT_star3D.py", "content": "# This is Batched Informed Tree star 3D algorithm \n# implementation\n\"\"\"\nThis is ABIT* code for 3D\n@author: yue qi \nAlgorithm 1\nsource: Gammell, Jonathan D., Siddhartha S. Srinivasa, and Timothy D. Barfoot. \"Batch informed trees (BIT*): \n Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs.\" \n 2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015. \nand \nsource: Gammell, Jonathan D., Timothy D. Barfoot, and Siddhartha S. Srinivasa. \n \"Batch Informed Trees (BIT*): Informed asymptotically optimal anytime search.\" \n The International Journal of Robotics Research 39.5 (2020): 543-567.\n\"\"\"\n\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom numpy.matlib import repmat\nimport time\nimport copy\n\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, isinbound\nfrom rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\nfrom rrt_3D.queue import MinheapPQ\n\n#---------methods to draw ellipse during sampling\ndef CreateUnitSphere(r = 1):\n phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane\n theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle\n radius = r\n\n # Transformation formulae for a spherical coordinate system.\n x = radius*np.sin(theta)*np.cos(phi)\n y = radius*np.sin(theta)*np.sin(phi)\n z = radius*np.cos(theta)\n return (x, y, z)\n\ndef draw_ellipsoid(ax, C, L, xcenter):\n (xs, ys, zs) = CreateUnitSphere()\n pts = np.array([xs, ys, zs])\n pts_in_world_frame = C@L@pts + xcenter\n ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color=\"g\")\n\nclass BIT_star:\n# ---------initialize and run\n def __init__(self, show_ellipse=False):\n self.env = env()\n self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.maxiter = 1000 # used for determining how many batches needed\n \n # radius calc parameter:\n # larger value makes better 1-time-performance, but longer time trade off\n self.eta = 7 # bigger or equal to 1\n\n # sampling \n self.m = 400 # number of samples for one time sample\n self.d = 3 # dimension we work with\n \n # instance of the cost to come gT\n self.g = {self.xstart:0, self.xgoal:np.inf}\n\n # draw ellipse\n self.show_ellipse = show_ellipse\n\n # denote if the path is found \n self.done = False\n self.Path = []\n \n # for drawing the ellipse\n self.C = np.zeros([3,3])\n self.L = np.zeros([3,3])\n self.xcenter = np.zeros(3)\n self.show_ellipse = show_ellipse\n\n def run(self):\n self.V = {self.xstart} # node expanded\n self.E = set() # edge set\n self.Parent = {} # Parent relation\n # self.T = (self.V, self.E) # tree\n self.Xsamples = {self.xgoal} # sampled set\n self.QE = set() # edges in queue\n self.QV = set() # nodes in queue\n self.r = np.inf # radius for evaluation\n self.ind = 0\n num_resample = 0\n while True:\n # for the first round\n print('round '+str(self.ind))\n self.visualization()\n # print(len(self.V))\n if len(self.QE) == 0 and len(self.QV) == 0:\n self.Prune(self.g_T(self.xgoal))\n self.Xsamples = self.Sample(self.m, self.g_T(self.xgoal)) # sample function\n self.Xsamples.add(self.xgoal) # adding goal into the sample\n self.Vold = {v for v in self.V}\n self.QV = {v for v in self.V}\n # setting the radius \n if self.done:\n self.r = 2 # sometimes the original radius criteria makes the radius too small to improve existing tree\n num_resample += 1\n else:\n self.r = self.radius(len(self.V) + len(self.Xsamples)) # radius determined with the sample size and dimension of conf space\n while self.BestQueueValue(self.QV, mode = 'QV') <= self.BestQueueValue(self.QE, mode = 'QE'):\n self.ExpandVertex(self.BestInQueue(self.QV, mode = 'QV'))\n (vm, xm) = self.BestInQueue(self.QE, mode = 'QE')\n self.QE.remove((vm, xm))\n if self.g_T(vm) + self.c_hat(vm, xm) + self.h_hat(xm) < self.g_T(self.xgoal):\n cost = self.c(vm, xm)\n if self.g_hat(vm) + cost + self.h_hat(xm) < self.g_T(self.xgoal):\n if self.g_T(vm) + cost < self.g_T(xm):\n if xm in self.V:\n self.E.difference_update({(v, x) for (v, x) in self.E if x == xm})\n else:\n self.Xsamples.remove(xm)\n self.V.add(xm)\n self.QV.add(xm)\n self.g[xm] = self.g[vm] + cost\n self.E.add((vm, xm))\n self.Parent[xm] = vm # add parent or update parent\n self.QE.difference_update({(v, x) for (v, x) in self.QE if x == xm and (self.g_T(v) + self.c_hat(v, xm)) >= self.g_T(xm)})\n \n # reinitializing sampling\n else:\n self.QE = set()\n self.QV = set()\n self.ind += 1\n \n # if the goal is reached\n if self.xgoal in self.Parent:\n print('locating path...')\n self.done = True\n self.Path = self.path()\n\n # if the iteration is bigger\n if self.ind > self.maxiter:\n break\n\n print('complete')\n print('number of times resampling ' + str(num_resample))\n\n# ---------IRRT utils\n def Sample(self, m, cmax, bias = 0.05, xrand = set()):\n # sample within a eclipse \n print('new sample')\n if cmax < np.inf:\n cmin = getDist(self.xgoal, self.xstart)\n xcenter = np.array([(self.xgoal[0] + self.xstart[0]) / 2, (self.xgoal[1] + self.xstart[1]) / 2, (self.xgoal[2] + self.xstart[2]) / 2])\n C = self.RotationToWorldFrame(self.xstart, self.xgoal)\n r = np.zeros(3)\n r[0] = cmax /2\n for i in range(1,3):\n r[i] = np.sqrt(cmax**2 - cmin**2) / 2\n L = np.diag(r) # R3*3 \n xball = self.SampleUnitBall(m) # np.array\n x = (C@L@xball).T + repmat(xcenter, len(xball.T), 1)\n # x2 = set(map(tuple, x))\n self.C = C # save to global var\n self.xcenter = xcenter\n self.L = L\n x2 = set(map(tuple, x[np.array([not isinside(self, state) and isinbound(self.env.boundary, state) for state in x])])) # intersection with the state space\n xrand.update(x2)\n # if there are samples inside obstacle: recursion\n if len(x2) < m:\n return self.Sample(m - len(x2), cmax, bias=bias, xrand=xrand)\n else:\n for i in range(m):\n xrand.add(tuple(sampleFree(self, bias = bias)))\n return xrand\n\n def SampleUnitBall(self, n):\n # uniform sampling in spherical coordinate system in 3D\n # sample radius\n r = np.random.uniform(0.0, 1.0, size = n)\n theta = np.random.uniform(0, np.pi, size = n)\n phi = np.random.uniform(0, 2 * np.pi, size = n)\n x = r * np.sin(theta) * np.cos(phi)\n y = r * np.sin(theta) * np.sin(phi)\n z = r * np.cos(theta)\n return np.array([x,y,z])\n\n def RotationToWorldFrame(self, xstart, xgoal):\n # S0(n): such that the xstart and xgoal are the center points\n d = getDist(xstart, xgoal)\n xstart, xgoal = np.array(xstart), np.array(xgoal)\n a1 = (xgoal - xstart) / d\n M = np.outer(a1,[1,0,0])\n U, S, V = np.linalg.svd(M)\n C = U@np.diag([1, 1, np.linalg.det(U)*np.linalg.det(V)])@V.T\n return C\n\n#----------BIT_star particular\n def ExpandVertex(self, v):\n self.QV.remove(v)\n Xnear = {x for x in self.Xsamples if getDist(x, v) <= self.r}\n self.QE.update({(v, x) for x in Xnear if self.g_hat(v) + self.c_hat(v, x) + self.h_hat(x) < self.g_T(self.xgoal)})\n if v not in self.Vold:\n Vnear = {w for w in self.V if getDist(w, v) <= self.r}\n self.QE.update({(v,w) for w in Vnear if \\\n ((v,w) not in self.E) and \\\n (self.g_hat(v) + self.c_hat(v, w) + self.h_hat(w) < self.g_T(self.xgoal)) and \\\n (self.g_T(v) + self.c_hat(v, w) < self.g_T(w))})\n\n def Prune(self, c):\n self.Xsamples = {x for x in self.Xsamples if self.f_hat(x) >= c}\n self.V.difference_update({v for v in self.V if self.f_hat(v) >= c})\n self.E.difference_update({(v, w) for (v, w) in self.E if (self.f_hat(v) > c) or (self.f_hat(w) > c)})\n self.Xsamples.update({v for v in self.V if self.g_T(v) == np.inf})\n self.V.difference_update({v for v in self.V if self.g_T(v) == np.inf})\n\n def radius(self, q):\n return 2 * self.eta * (1 + 1/self.d) ** (1/self.d) * \\\n (self.Lambda(self.Xf_hat(self.V)) / self.Zeta() ) ** (1/self.d) * \\\n (np.log(q) / q) ** (1/self.d)\n\n def Lambda(self, inputset):\n # lebesgue measure of a set, defined as \n # mu: L(Rn) --> [0, inf], e.g. volume\n return len(inputset)\n\n def Xf_hat(self, X):\n # the X is a set, defined as {x in X | fhat(x) <= cbest}\n # where cbest is current best cost.\n cbest = self.g_T(self.xgoal)\n return {x for x in X if self.f_hat(x) <= cbest}\n\n def Zeta(self):\n # Lebesgue measure of a n dimensional unit ball\n # since it's the 3D, use volume\n return 4/3 * np.pi\n\n def BestInQueue(self, inputset, mode):\n # returns the best vertex in the vertex queue given this ordering\n # mode = 'QE' or 'QV'\n if mode == 'QV':\n V = {state: self.g_T(state) + self.h_hat(state) for state in self.QV}\n if mode == 'QE':\n V = {state: self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE}\n if len(V) == 0:\n print(mode + 'empty')\n return None\n return min(V, key = V.get)\n\n def BestQueueValue(self, inputset, mode):\n # returns the best value in the vertex queue given this ordering\n # mode = 'QE' or 'QV'\n if mode == 'QV':\n V = {self.g_T(state) + self.h_hat(state) for state in self.QV}\n if mode == 'QE':\n V = {self.g_T(state[0]) + self.c_hat(state[0], state[1]) + self.h_hat(state[1]) for state in self.QE}\n if len(V) == 0:\n return np.inf\n return min(V)\n\n def g_hat(self, v):\n return getDist(self.xstart, v)\n\n def h_hat(self, v):\n return getDist(self.xgoal, v)\n\n def f_hat(self, v):\n # f = g + h: estimate cost\n return self.g_hat(v) + self.h_hat(v)\n\n def c(self, v, w):\n # admissible estimate of the cost of an edge between state v, w\n collide, dist = isCollide(self, v, w)\n if collide:\n return np.inf\n else: \n return dist\n\n def c_hat(self, v, w):\n # c_hat < c < np.inf\n # heuristic estimate of the edge cost, since c is expensive\n return getDist(v, w)\n\n def g_T(self, v):\n # represent cost-to-come from the start in the tree, \n # if the state is not in tree, or unreachable, return inf\n if v not in self.g:\n self.g[v] = np.inf\n return self.g[v]\n\n def path(self):\n path = []\n s = self.xgoal\n i = 0\n while s != self.xstart:\n path.append((s, self.Parent[s]))\n s = self.Parent[s]\n if i > self.m:\n break\n i += 1\n return path\n \n def visualization(self):\n if self.ind % 20 == 0:\n V = np.array(list(self.V))\n Xsample = np.array(list(self.Xsamples))\n edges = list(map(list, self.E))\n Path = np.array(self.Path)\n start = self.env.start\n goal = self.env.goal\n # edges = E.get_edge()\n #----------- list structure\n # edges = []\n # for i in self.Parent:\n # edges.append([i,self.Parent[i]])\n #----------- end\n # generate axis objects\n ax = plt.subplot(111, projection='3d')\n \n # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))\n # ax.view_init(elev=0., azim=90.)\n ax.view_init(elev=90., azim=60.)\n # ax.view_init(elev=-8., azim=180)\n ax.clear()\n # drawing objects\n draw_Spheres(ax, self.env.balls)\n draw_block_list(ax, self.env.blocks)\n if self.env.OBB is not None:\n draw_obb(ax, self.env.OBB)\n draw_block_list(ax, np.array([self.env.boundary]), alpha=0)\n draw_line(ax, edges, visibility=0.75, color='g')\n draw_line(ax, Path, color='r')\n if self.show_ellipse:\n draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis\n if len(V) > 0:\n ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )\n if len(Xsample) > 0: # plot the sampled points\n ax.scatter3D(Xsample[:, 0], Xsample[:, 1], Xsample[:, 2], s=1, color='b',)\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\n # adjust the aspect ratio\n ax.dist = 11\n set_axes_equal(ax)\n make_transparent(ax)\n #plt.xlabel('s')\n #plt.ylabel('y')\n ax.set_axis_off()\n plt.pause(0.0001)\n\n\nif __name__ == '__main__':\n Newprocess = BIT_star(show_ellipse=False)\n Newprocess.run()" }, { "path": "Sampling_based_Planning/rrt_3D/FMT_star3D.py", "content": "\"\"\"\nThis is fast marching tree* code for 3D\n@author: yue qi \nsource: Janson, Lucas, et al. \"Fast marching tree: A fast marching sampling-based method \n for optimal motion planning in many dimensions.\" \n The International journal of robotics research 34.7 (2015): 883-921.\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport time\nimport copy\n\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide\nfrom rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\nfrom rrt_3D.queue import MinheapPQ\n\nclass FMT_star:\n\n def __init__(self, radius = 1, n = 1000):\n self.env = env()\n # init start and goal\n # note that the xgoal could be a region since this algorithm is a multiquery method\n self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal)\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # used for sample free\n self.n = n # number of samples\n self.radius = radius # radius of the ball\n # self.radius = 40 * np.sqrt((np.log(self.n) / self.n))\n # sets\n self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets()\n # make space for save \n self.neighbors = {}\n # additional\n self.done = True\n self.Path = []\n self.Parent = {}\n\n def generateSampleSet(self, n):\n V = set()\n for i in range(n):\n V.add(tuple(sampleFree(self, bias = 0.0)))\n return V\n\n def initNodeSets(self):\n # open set\n Vopen = {self.xinit} # open set\n # closed set\n closed = set()\n # V, Vunvisited set \n V = self.generateSampleSet(self.n - 2) # set of all nodes\n Vunvisited = copy.deepcopy(V) # unvisited set\n Vunvisited.add(self.xgoal)\n V.add(self.xinit)\n V.add(self.xgoal)\n # initialize all cost to come at inf\n c = {node : np.inf for node in V}\n c[self.xinit] = 0\n # use a min heap to speed up\n Vopen_queue = MinheapPQ()\n Vopen_queue.put(self.xinit, c[self.xinit]) # priority organized as the cost to come\n return Vopen, Vopen_queue, closed, V, Vunvisited, c\n\n def Near(self, nodeset, node, rn):\n if node in self.neighbors:\n return self.neighbors[node]\n validnodes = {i for i in nodeset if getDist(i, node) < rn}\n return validnodes\n\n def Save(self, V_associated, node):\n self.neighbors[node] = V_associated\n\n def path(self, z, initT):\n path = []\n s = self.xgoal\n i = 0\n while s != self.xinit:\n path.append((s, self.Parent[s]))\n s = self.Parent[s]\n if i > self.n:\n break\n i += 1\n return path\n\n def Cost(self, x, y):\n # collide, dist = isCollide(self, x, y)\n # if collide:\n # return np.inf\n # return dist\n return getDist(x, y)\n\n def FMTrun(self):\n z = self.xinit\n rn = self.radius\n Nz = self.Near(self.Vunvisited, z, rn)\n E = set()\n self.Save(Nz, z)\n ind = 0\n while z != self.xgoal:\n Vopen_new = set()\n #Nz = self.Near(self.Vunvisited, z, rn)\n #self.Save(Nz, z)\n #Xnear = Nz.intersection(self.Vunvisited)\n Xnear = self.Near(self.Vunvisited, z ,rn)\n self.Save(Xnear, z)\n for x in Xnear:\n #Nx = self.Near(self.V.difference({x}), x, rn)\n #self.Save(Nx, x)\n #Ynear = list(Nx.intersection(self.Vopen))\n Ynear = list(self.Near(self.Vopen, x, rn))\n # self.Save(set(Ynear), x)\n ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation\n collide, _ = isCollide(self, ymin, x)\n if not collide:\n E.add((ymin, x)) # straight line joining ymin and x is collision free\n Vopen_new.add(x)\n self.Parent[x] = z\n self.Vunvisited = self.Vunvisited.difference({x})\n self.c[x] = self.c[ymin] + self.Cost(ymin, x) # estimated cost-to-arrive from xinit in tree T = (VopenUVclosed, E)\n # update open set\n self.Vopen = self.Vopen.union(Vopen_new).difference({z})\n self.Vclosed.add(z)\n if len(self.Vopen) == 0:\n print('Failure')\n return \n ind += 1\n print(str(ind) + ' node expanded')\n self.visualization(ind, E)\n # update current node\n Vopenlist = list(self.Vopen)\n z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])]\n # creating the tree\n T = (self.Vopen.union(self.Vclosed), E)\n self.done = True\n self.Path = self.path(z, T)\n self.visualization(ind, E)\n plt.show()\n # return self.path(z, T)\n\n def visualization(self, ind, E):\n if ind % 100 == 0 or self.done:\n #----------- list structure\n # V = np.array(list(initparams.V))\n # E = initparams.E\n #----------- end\n # edges = initparams.E\n Path = np.array(self.Path)\n start = self.env.start\n goal = self.env.goal\n # edges = E.get_edge()\n #----------- list structure\n edges = np.array(list(E))\n #----------- end\n # generate axis objects\n ax = plt.subplot(111, projection='3d')\n \n # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))\n # ax.view_init(elev=0., azim=90.)\n ax.view_init(elev=65., azim=60.)\n ax.dist = 15\n # ax.view_init(elev=-8., azim=180)\n ax.clear()\n # drawing objects\n draw_Spheres(ax, self.env.balls)\n draw_block_list(ax, self.env.blocks)\n if self.env.OBB is not None:\n draw_obb(ax, self.env.OBB)\n draw_block_list(ax, np.array([self.env.boundary]), alpha=0)\n draw_line(ax, edges, visibility=0.75, color='g')\n draw_line(ax, Path, color='r')\n # if len(V) > 0:\n # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\n # adjust the aspect ratio\n set_axes_equal(ax)\n make_transparent(ax)\n #plt.xlabel('x')\n #plt.ylabel('y')\n ax.set_axis_off()\n plt.pause(0.0001)\n\nif __name__ == '__main__':\n A = FMT_star(radius = 1, n = 3000)\n A.FMTrun()\n\n\n\n" }, { "path": "Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py", "content": "\"\"\"\nThis is dynamic rrt code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide\nfrom rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\n\n\nclass dynamic_rrt_3D:\n\n def __init__(self):\n self.env = env()\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.qrobot = self.x0\n self.current = tuple(self.env.start)\n self.stepsize = 0.25\n self.maxiter = 10000\n self.GoalProb = 0.05 # probability biased to the goal\n self.WayPointProb = 0.02 # probability falls back on to the way points\n self.done = False\n self.invalid = False\n\n self.V = [] # vertices\n self.Parent = {} # parent child relation\n self.Edge = set() # edge relation (node, parent node) tuple\n self.Path = []\n self.flag = {} # flag dictionary\n self.ind = 0\n self.i = 0\n\n # --------Dynamic RRT algorithm\n def RegrowRRT(self):\n self.TrimRRT()\n self.GrowRRT()\n\n def TrimRRT(self):\n S = []\n i = 1\n print('trimming...')\n while i < len(self.V):\n qi = self.V[i]\n qp = self.Parent[qi]\n if self.flag[qp] == 'Invalid':\n self.flag[qi] = 'Invalid'\n if self.flag[qi] != 'Invalid':\n S.append(qi)\n i += 1\n self.CreateTreeFromNodes(S)\n\n def InvalidateNodes(self, obstacle):\n Edges = self.FindAffectedEdges(obstacle)\n for edge in Edges:\n qe = self.ChildEndpointNode(edge)\n self.flag[qe] = 'Invalid'\n\n # --------Extend RRT algorithm-----\n def initRRT(self):\n self.V.append(self.x0)\n self.flag[self.x0] = 'Valid'\n\n def GrowRRT(self):\n print('growing...')\n qnew = self.x0\n distance_threshold = self.stepsize\n self.ind = 0\n while self.ind <= self.maxiter:\n qtarget = self.ChooseTarget()\n qnearest = self.Nearest(qtarget)\n qnew, collide = self.Extend(qnearest, qtarget)\n if not collide:\n self.AddNode(qnearest, qnew)\n if getDist(qnew, self.xt) < distance_threshold:\n self.AddNode(qnearest, self.xt)\n self.flag[self.xt] = 'Valid'\n break\n self.i += 1\n self.ind += 1\n # self.visualization()\n\n def ChooseTarget(self):\n # return the goal, or randomly choose a state in the waypoints based on probs\n p = np.random.uniform()\n if len(self.V) == 1:\n i = 0\n else:\n i = np.random.randint(0, high=len(self.V) - 1)\n if 0 < p < self.GoalProb:\n return self.xt\n elif self.GoalProb < p < self.GoalProb + self.WayPointProb:\n return self.V[i]\n elif self.GoalProb + self.WayPointProb < p < 1:\n return tuple(self.RandomState())\n\n def RandomState(self):\n # generate a random, obstacle free state\n xrand = sampleFree(self, bias=0)\n return xrand\n\n def AddNode(self, nearest, extended):\n self.V.append(extended)\n self.Parent[extended] = nearest\n self.Edge.add((extended, nearest))\n self.flag[extended] = 'Valid'\n\n def Nearest(self, target):\n # TODO use kdTree to speed up search\n return nearest(self, target, isset=True)\n\n def Extend(self, nearest, target):\n extended, dist = steer(self, nearest, target, DIST=True)\n collide, _ = isCollide(self, nearest, target, dist)\n return extended, collide\n\n # --------Main function\n def Main(self):\n # qstart = qgoal\n self.x0 = tuple(self.env.goal)\n # qgoal = qrobot\n self.xt = tuple(self.env.start)\n self.initRRT()\n self.GrowRRT()\n self.Path, D = self.path()\n self.done = True\n self.visualization()\n t = 0\n while True:\n # move the block while the robot is moving\n new, _ = self.env.move_block(a=[0.2, 0, -0.2], mode='translation')\n self.InvalidateNodes(new)\n self.TrimRRT()\n # if solution path contains invalid node\n self.visualization()\n self.invalid = self.PathisInvalid(self.Path)\n if self.invalid:\n self.done = False\n self.RegrowRRT()\n self.Path = []\n self.Path, D = self.path()\n self.done = True\n self.visualization()\n if t == 8:\n break\n t += 1\n self.visualization()\n plt.show()\n\n # --------Additional utility functions\n def FindAffectedEdges(self, obstacle):\n # scan the graph for the changed edges in the tree.\n # return the end point and the affected\n print('finding affected edges...')\n Affectededges = []\n for e in self.Edge:\n child, parent = e\n collide, _ = isCollide(self, child, parent)\n if collide:\n Affectededges.append(e)\n return Affectededges\n\n def ChildEndpointNode(self, edge):\n return edge[0]\n\n def CreateTreeFromNodes(self, Nodes):\n print('creating tree...')\n # self.Parent = {node: self.Parent[node] for node in Nodes}\n self.V = [node for node in Nodes]\n self.Edge = {(node, self.Parent[node]) for node in Nodes}\n # if self.invalid:\n # del self.Parent[self.xt]\n\n def PathisInvalid(self, path):\n for edge in path:\n if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid':\n return True\n\n def path(self, dist=0):\n Path=[]\n x = self.xt\n i = 0\n while x != self.x0:\n x2 = self.Parent[x]\n Path.append(np.array([x, x2]))\n dist += getDist(x, x2)\n x = x2\n if i > 10000:\n print('Path is not found')\n return \n i+= 1\n return Path, dist\n\n # --------Visualization specialized for dynamic RRT\n def visualization(self):\n if self.ind % 100 == 0 or self.done:\n V = np.array(self.V)\n Path = np.array(self.Path)\n start = self.env.start\n goal = self.env.goal\n # edges = []\n # for i in self.Parent:\n # edges.append([i, self.Parent[i]])\n edges = np.array([list(i) for i in self.Edge])\n ax = plt.subplot(111, projection='3d')\n # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))\n # ax.view_init(elev=0., azim=90.)\n ax.view_init(elev=90., azim=0.)\n ax.clear()\n # drawing objects\n draw_Spheres(ax, self.env.balls)\n draw_block_list(ax, self.env.blocks)\n if self.env.OBB is not None:\n draw_obb(ax, self.env.OBB)\n draw_block_list(ax, np.array([self.env.boundary]), alpha=0)\n draw_line(ax, edges, visibility=0.75, color='g')\n draw_line(ax, Path, color='r')\n # if len(V) > 0:\n # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\n # adjust the aspect ratio\n set_axes_equal(ax)\n make_transparent(ax)\n # plt.xlabel('s')\n # plt.ylabel('y')\n ax.set_axis_off()\n plt.pause(0.0001)\n\n\nif __name__ == '__main__':\n rrt = dynamic_rrt_3D()\n rrt.Main()\n" }, { "path": "Sampling_based_Planning/rrt_3D/env3D.py", "content": "# this is the three dimensional configuration space for rrt\r\n# !/usr/bin/env python3\r\n# -*- coding: utf-8 -*-\r\n\"\"\"\r\n@author: yue qi\r\n\"\"\"\r\nimport numpy as np\r\n# from utils3D import OBB2AABB\r\n\r\ndef R_matrix(z_angle,y_angle,x_angle):\r\n # s angle: row; y angle: pitch; z angle: yaw\r\n # generate rotation matrix in SO3\r\n # RzRyRx = R, ZYX intrinsic rotation\r\n # also (r1,r2,r3) in R3*3 in {W} frame\r\n # used in obb.O\r\n # [[R p]\r\n # [0T 1]] gives transformation from body to world \r\n 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]])@ \\\r\n 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)]])@ \\\r\n 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)]])\r\n\r\ndef getblocks():\r\n # AABBs\r\n block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00],\r\n [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],\r\n [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00],\r\n [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00],\r\n [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]]\r\n Obstacles = []\r\n for i in block:\r\n i = np.array(i)\r\n Obstacles.append([j for j in i])\r\n return np.array(Obstacles)\r\n\r\ndef getballs():\r\n spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]]\r\n Obstacles = []\r\n for i in spheres:\r\n Obstacles.append([j for j in i])\r\n return np.array(Obstacles)\r\n\r\ndef getAABB(blocks):\r\n # used for Pyrr package for detecting collision\r\n AABB = []\r\n for i in blocks:\r\n AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger\r\n return AABB\r\n\r\ndef getAABB2(blocks):\r\n # used in lineAABB\r\n AABB = []\r\n for i in blocks:\r\n AABB.append(aabb(i))\r\n return AABB\r\n\r\ndef add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):\r\n return block\r\n\r\nclass aabb(object):\r\n # make AABB out of blocks, \r\n # P: center point\r\n # E: extents\r\n # O: Rotation matrix in SO(3), in {w}\r\n def __init__(self,AABB):\r\n self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point\r\n self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents\r\n self.O = [[1,0,0],[0,1,0],[0,0,1]]\r\n\r\nclass obb(object):\r\n # P: center point\r\n # E: extents\r\n # O: Rotation matrix in SO(3), in {w}\r\n def __init__(self, P, E, O):\r\n self.P = P\r\n self.E = E\r\n self.O = O\r\n self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]])\r\n\r\nclass env():\r\n def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1):\r\n # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): \r\n self.resolution = resolution\r\n self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) \r\n self.blocks = getblocks()\r\n self.AABB = getAABB2(self.blocks)\r\n self.AABB_pyrr = getAABB(self.blocks)\r\n self.balls = getballs()\r\n self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)),\r\n obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))])\r\n self.start = np.array([2.0, 2.0, 2.0])\r\n self.goal = np.array([6.0, 16.0, 0.0])\r\n self.t = 0 # time \r\n\r\n def New_block(self):\r\n newblock = add_block()\r\n self.blocks = np.vstack([self.blocks,newblock])\r\n self.AABB = getAABB2(self.blocks)\r\n self.AABB_pyrr = getAABB(self.blocks)\r\n\r\n def move_start(self, x):\r\n self.start = x\r\n\r\n def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'):\r\n # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, \r\n # R is an orthorgonal transform in R3*3, is the rotation matrix\r\n # (s',t') = (s + tv, t) is uniform transformation\r\n # (s',t') = (s + a, t + s) is a translation\r\n if mode == 'translation':\r\n ori = np.array(self.blocks[block_to_move])\r\n self.blocks[block_to_move] = \\\r\n np.array([ori[0] + a[0],\\\r\n ori[1] + a[1],\\\r\n ori[2] + a[2],\\\r\n ori[3] + a[0],\\\r\n ori[4] + a[1],\\\r\n ori[5] + a[2]])\r\n\r\n self.AABB[block_to_move].P = \\\r\n [self.AABB[block_to_move].P[0] + a[0], \\\r\n self.AABB[block_to_move].P[1] + a[1], \\\r\n self.AABB[block_to_move].P[2] + a[2]]\r\n self.t += s\r\n # return a range of block that the block might moved\r\n a = self.blocks[block_to_move]\r\n return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \\\r\n a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \\\r\n np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \\\r\n ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])\r\n # return a,ori\r\n # (s',t') = (Rx, t)\r\n def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):\r\n # theta stands for rotational angles around three principle axis in world frame\r\n # translation stands for translation in the world frame\r\n ori = [self.OBB[obb_to_move]]\r\n # move obb position\r\n self.OBB[obb_to_move].P = \\\r\n [self.OBB[obb_to_move].P[0] + translation[0], \r\n self.OBB[obb_to_move].P[1] + translation[1], \r\n self.OBB[obb_to_move].P[2] + translation[2]]\r\n # Calculate orientation\r\n self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])\r\n # generating transformation matrix\r\n self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\\\r\n -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]])\r\n return self.OBB[obb_to_move], ori[0]\r\n \r\nif __name__ == '__main__':\r\n newenv = env()\r\n" }, { "path": "Sampling_based_Planning/rrt_3D/extend_rrt3D.py", "content": "# Real-Time Randomized Path Planning for Robot Navigation∗\n\"\"\"\nThis is rrt extend code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections import defaultdict\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path\n\n# here attempt to use a KD tree for the data structure implementation\nimport scipy.spatial.kdtree as KDtree\n\n\nclass extend_rrt(object):\n def __init__(self):\n self.env = env()\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.current = tuple(self.env.start)\n self.stepsize = 0.5\n self.maxiter = 10000\n self.GoalProb = 0.05 # probability biased to the goal\n self.WayPointProb = 0.05 # probability falls back on to the way points\n\n self.done = False\n self.V = [] # vertices\n self.Parent = {}\n self.Path = []\n self.ind = 0\n self.i = 0\n\n #--------- basic rrt algorithm----------\n def RRTplan(self, env, initial, goal):\n threshold = self.stepsize\n nearest = initial # state structure\n self.V.append(initial)\n rrt_tree = initial # TODO KDtree structure\n while self.ind <= self.maxiter:\n target = self.ChooseTarget(goal)\n nearest = self.Nearest(rrt_tree, target)\n extended, collide = self.Extend(env, nearest, target)\n if not collide:\n self.AddNode(rrt_tree, nearest, extended)\n if getDist(nearest, goal) <= threshold:\n self.AddNode(rrt_tree, nearest, self.xt)\n break\n self.i += 1\n self.ind += 1\n visualization(self)\n \n # return rrt_tree\n self.done = True\n self.Path, _ = path(self)\n visualization(self)\n plt.show()\n \n\n def Nearest(self, tree, target):\n # TODO use kdTree to speed up search\n return nearest(self, target, isset=True)\n\n def Extend(self, env, nearest, target):\n extended, dist = steer(self, nearest, target, DIST = True)\n collide, _ = isCollide(self, nearest, target, dist)\n return extended, collide\n\n def AddNode(self, tree, nearest, extended):\n # TODO use a kdtree\n self.V.append(extended)\n self.Parent[extended] = nearest\n\n def RandomState(self):\n # generate a random, obstacle free state\n xrand = sampleFree(self, bias=0)\n return xrand\n\n #--------- insight to the rrt_extend\n def ChooseTarget(self, state):\n # return the goal, or randomly choose a state in the waypoints based on probs\n p = np.random.uniform()\n if len(self.V) == 1:\n i = 0\n else:\n i = np.random.randint(0, high = len(self.V) - 1)\n if 0 < p < self.GoalProb:\n return self.xt\n elif self.GoalProb < p < self.GoalProb + self.WayPointProb:\n return self.V[i]\n elif self.GoalProb + self.WayPointProb < p < 1:\n return tuple(self.RandomState())\n \nif __name__ == '__main__':\n t = extend_rrt()\n _ = t.RRTplan(t.env, t.x0, t.xt)" }, { "path": "Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py", "content": "# informed RRT star in 3D\n\"\"\"\nThis is IRRT* code for 3D\n@author: yue qi \nsource: J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT*:\n Optimal sampling-based path planning focused via direct sampling of\n an admissible ellipsoidal heuristic,” in IROS, 2997–3004, 2014.\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\nimport time\nimport copy\n\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, near, nearest, path\nfrom rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\nfrom rrt_3D.queue import MinheapPQ\n\n\ndef CreateUnitSphere(r = 1):\n phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane\n theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle\n radius = r\n\n # Transformation formulae for a spherical coordinate system.\n x = radius*np.sin(theta)*np.cos(phi)\n y = radius*np.sin(theta)*np.sin(phi)\n z = radius*np.cos(theta)\n return (x, y, z)\n\ndef draw_ellipsoid(ax, C, L, xcenter):\n (xs, ys, zs) = CreateUnitSphere()\n pts = np.array([xs, ys, zs])\n pts_in_world_frame = C@L@pts + xcenter\n ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color=\"g\")\n\nclass IRRT:\n\n def __init__(self, show_ellipse = False):\n self.env = env()\n self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.Parent = {}\n self.Path = []\n self.N = 10000 # used for determining how many batches needed\n self.ind = 0\n self.i = 0\n # rrt* near and other utils\n self.stepsize = 1\n self.gamma = 500\n self.eta = self.stepsize\n self.rgoal = self.stepsize\n self.done = False\n # for drawing the ellipse\n self.C = np.zeros([3,3])\n self.L = np.zeros([3,3])\n self.xcenter = np.zeros(3)\n self.show_ellipse = show_ellipse\n\n def Informed_rrt(self):\n self.V = [self.xstart]\n self.E = set()\n self.Xsoln = set()\n self.T = (self.V, self.E)\n \n c = 1\n while self.ind <= self.N:\n print(self.ind)\n self.visualization()\n # print(self.i)\n if len(self.Xsoln) == 0:\n cbest = np.inf\n else:\n cbest = min({self.cost(xsln) for xsln in self.Xsoln})\n xrand = self.Sample(self.xstart, self.xgoal, cbest)\n xnearest = nearest(self, xrand)\n xnew, dist = steer(self, xnearest, xrand)\n # print(xnew)\n collide, _ = isCollide(self, xnearest, xnew, dist=dist)\n if not collide:\n self.V.append(xnew)\n Xnear = near(self, xnew)\n xmin = xnearest\n cmin = self.cost(xmin) + c * self.line(xnearest, xnew)\n for xnear in Xnear:\n xnear = tuple(xnear)\n cnew = self.cost(xnear) + c * self.line(xnear, xnew)\n if cnew < cmin:\n collide, _ = isCollide(self, xnear, xnew)\n if not collide:\n xmin = xnear\n cmin = cnew\n self.E.add((xmin, xnew))\n self.Parent[xnew] = xmin\n \n for xnear in Xnear:\n xnear = tuple(xnear)\n cnear = self.cost(xnear)\n cnew = self.cost(xnew) + c * self.line(xnew, xnear)\n # rewire\n if cnew < cnear:\n collide, _ = isCollide(self, xnew, xnear)\n if not collide:\n xparent = self.Parent[xnear]\n self.E.difference_update((xparent, xnear))\n self.E.add((xnew, xnear))\n self.Parent[xnear] = xnew\n self.i += 1\n if self.InGoalRegion(xnew):\n print('reached')\n self.done = True\n self.Parent[self.xgoal] = xnew\n self.Path, _ = path(self)\n self.Xsoln.add(xnew)\n # update path\n if self.done:\n self.Path, _ = path(self, Path = [])\n self.ind += 1\n # return tree\n return self.T\n \n def Sample(self, xstart, xgoal, cmax, bias = 0.05):\n # sample within a eclipse \n if cmax < np.inf:\n cmin = getDist(xgoal, xstart)\n xcenter = np.array([(xgoal[0] + xstart[0]) / 2, (xgoal[1] + xstart[1]) / 2, (xgoal[2] + xstart[2]) / 2])\n C = self.RotationToWorldFrame(xstart, xgoal)\n r = np.zeros(3)\n r[0] = cmax /2\n for i in range(1,3):\n r[i] = np.sqrt(cmax**2 - cmin**2) / 2\n L = np.diag(r) # R3*3 \n xball = self.SampleUnitBall() # np.array\n x = C@L@xball + xcenter\n self.C = C # save to global var\n self.xcenter = xcenter\n self.L = L\n if not isinside(self, x): # intersection with the state space\n xrand = x\n else:\n return self.Sample(xstart, xgoal, cmax)\n else:\n xrand = sampleFree(self, bias = bias)\n return xrand\n\n def SampleUnitBall(self):\n # uniform sampling in spherical coordinate system in 3D\n # sample radius\n r = np.random.uniform(0.0, 1.0)\n theta = np.random.uniform(0, np.pi)\n phi = np.random.uniform(0, 2 * np.pi)\n x = r * np.sin(theta) * np.cos(phi)\n y = r * np.sin(theta) * np.sin(phi)\n z = r * np.cos(theta)\n return np.array([x,y,z])\n\n def RotationToWorldFrame(self, xstart, xgoal):\n # S0(n): such that the xstart and xgoal are the center points\n d = getDist(xstart, xgoal)\n xstart, xgoal = np.array(xstart), np.array(xgoal)\n a1 = (xgoal - xstart) / d\n M = np.outer(a1,[1,0,0])\n U, S, V = np.linalg.svd(M)\n C = U@np.diag([1, 1, np.linalg.det(U)*np.linalg.det(V)])@V.T\n return C\n\n def InGoalRegion(self, x):\n # Xgoal = {x in Xfree | \\\\x-xgoal\\\\2 <= rgoal}\n return getDist(x, self.xgoal) <= self.rgoal\n\n def cost(self, x):\n # actual cost \n '''here use the additive recursive cost function'''\n if x == self.xstart:\n return 0.0\n if x not in self.Parent:\n return np.inf\n return self.cost(self.Parent[x]) + getDist(x, self.Parent[x])\n\n def line(self, x, y):\n return getDist(x, y)\n\n def visualization(self):\n if self.ind % 500 == 0:\n V = np.array(self.V)\n edges = list(map(list, self.E))\n Path = np.array(self.Path)\n start = self.env.start\n goal = self.env.goal\n # edges = E.get_edge()\n #----------- list structure\n # edges = []\n # for i in self.Parent:\n # edges.append([i,self.Parent[i]])\n #----------- end\n # generate axis objects\n ax = plt.subplot(111, projection='3d')\n \n # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))\n # ax.view_init(elev=0., azim=90.)\n ax.view_init(elev=90., azim=0.)\n # ax.view_init(elev=-8., azim=180)\n ax.clear()\n # drawing objects\n draw_Spheres(ax, self.env.balls)\n draw_block_list(ax, self.env.blocks)\n if self.env.OBB is not None:\n draw_obb(ax, self.env.OBB)\n draw_block_list(ax, np.array([self.env.boundary]), alpha=0)\n draw_line(ax, edges, visibility=0.75, color='g')\n draw_line(ax, Path, color='r')\n if self.show_ellipse:\n draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis\n if len(V) > 0:\n ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\n # adjust the aspect ratio\n ax.dist = 5\n set_axes_equal(ax)\n make_transparent(ax)\n #plt.xlabel('s')\n #plt.ylabel('y')\n ax.set_axis_off()\n plt.pause(0.0001)\n\nif __name__ == '__main__':\n A = IRRT(show_ellipse=False)\n A.Informed_rrt()\n" }, { "path": "Sampling_based_Planning/rrt_3D/plot_util3D.py", "content": "# plotting\r\nimport matplotlib.pyplot as plt\r\nfrom mpl_toolkits.mplot3d import Axes3D\r\nfrom mpl_toolkits.mplot3d.art3d import Poly3DCollection\r\nimport mpl_toolkits.mplot3d as plt3d\r\nfrom mpl_toolkits.mplot3d import proj3d\r\nimport numpy as np\r\n\r\n\r\ndef CreateSphere(center, r):\r\n u = np.linspace(0, 2 * np.pi, 30)\r\n v = np.linspace(0, np.pi, 30)\r\n x = np.outer(np.cos(u), np.sin(v))\r\n y = np.outer(np.sin(u), np.sin(v))\r\n z = np.outer(np.ones(np.size(u)), np.cos(v))\r\n x, y, z = r * x + center[0], r * y + center[1], r * z + center[2]\r\n return (x, y, z)\r\n\r\n\r\ndef draw_Spheres(ax, balls):\r\n for i in balls:\r\n (xs, ys, zs) = CreateSphere(i[0:3], i[-1])\r\n ax.plot_wireframe(xs, ys, zs, alpha=0.15, color=\"b\")\r\n\r\n\r\ndef draw_block_list(ax, blocks, color=None, alpha=0.15):\r\n '''\r\n drawing the blocks on the graph\r\n '''\r\n 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]],\r\n dtype='float')\r\n 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]])\r\n n = blocks.shape[0]\r\n d = blocks[:, 3:6] - blocks[:, :3]\r\n vl = np.zeros((8 * n, 3))\r\n fl = np.zeros((6 * n, 4), dtype='int64')\r\n for k in range(n):\r\n vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]\r\n fl[k * 6:(k + 1) * 6, :] = f + k * 8\r\n if type(ax) is Poly3DCollection:\r\n ax.set_verts(vl[fl])\r\n else:\r\n h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k'))\r\n return h\r\n\r\n\r\ndef obb_verts(obb):\r\n # 0.017004013061523438 for 1000 iters\r\n ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \\\r\n [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]])\r\n # P + (ori * E)\r\n ori_body = np.multiply(ori_body, obb.E)\r\n # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)\r\n verts = (obb.O @ ori_body.T).T + obb.P\r\n return verts\r\n\r\n\r\ndef draw_obb(ax, OBB, color=None, alpha=0.15):\r\n 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]])\r\n n = OBB.shape[0]\r\n vl = np.zeros((8 * n, 3))\r\n fl = np.zeros((6 * n, 4), dtype='int64')\r\n for k in range(n):\r\n vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])\r\n fl[k * 6:(k + 1) * 6, :] = f + k * 8\r\n if type(ax) is Poly3DCollection:\r\n ax.set_verts(vl[fl])\r\n else:\r\n h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k'))\r\n return h\r\n\r\n\r\ndef draw_line(ax, SET, visibility=1, color=None):\r\n if SET != []:\r\n for i in SET:\r\n xs = i[0][0], i[1][0]\r\n ys = i[0][1], i[1][1]\r\n zs = i[0][2], i[1][2]\r\n line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color)\r\n ax.add_line(line)\r\n\r\n\r\ndef visualization(initparams):\r\n if initparams.ind % 100 == 0 or initparams.done:\r\n #----------- list structure\r\n # V = np.array(list(initparams.V))\r\n # E = initparams.E\r\n #----------- end\r\n # edges = initparams.E\r\n Path = np.array(initparams.Path)\r\n start = initparams.env.start\r\n goal = initparams.env.goal\r\n # edges = E.get_edge()\r\n #----------- list structure\r\n edges = []\r\n for i in initparams.Parent:\r\n edges.append([i,initparams.Parent[i]])\r\n #----------- end\r\n # generate axis objects\r\n ax = plt.subplot(111, projection='3d')\r\n \r\n # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))\r\n # ax.view_init(elev=0., azim=90.)\r\n ax.view_init(elev=65., azim=60.)\r\n # ax.view_init(elev=-8., azim=180)\r\n ax.clear()\r\n # drawing objects\r\n draw_Spheres(ax, initparams.env.balls)\r\n draw_block_list(ax, initparams.env.blocks)\r\n if initparams.env.OBB is not None:\r\n draw_obb(ax, initparams.env.OBB)\r\n draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0)\r\n draw_line(ax, edges, visibility=0.75, color='g')\r\n draw_line(ax, Path, color='r')\r\n # if len(V) > 0:\r\n # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )\r\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\r\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\r\n # adjust the aspect ratio\r\n ax.dist = 15\r\n set_axes_equal(ax)\r\n make_transparent(ax)\r\n #plt.xlabel('s')\r\n #plt.ylabel('y')\r\n ax.set_axis_off()\r\n plt.pause(0.0001)\r\n\r\ndef set_axes_equal(ax):\r\n '''Make axes of 3D plot have equal scale so that spheres appear as spheres,\r\n cubes as cubes, etc.. This is one possible solution to Matplotlib's\r\n ax.set_aspect('equal') and ax.axis('equal') not working for 3D.\r\n https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to\r\n Input\r\n ax: a matplotlib axis, e.g., as output from plt.gca().\r\n '''\r\n\r\n x_limits = ax.get_xlim3d()\r\n y_limits = ax.get_ylim3d()\r\n z_limits = ax.get_zlim3d()\r\n\r\n x_range = abs(x_limits[1] - x_limits[0])\r\n x_middle = np.mean(x_limits)\r\n y_range = abs(y_limits[1] - y_limits[0])\r\n y_middle = np.mean(y_limits)\r\n z_range = abs(z_limits[1] - z_limits[0])\r\n z_middle = np.mean(z_limits)\r\n\r\n # The plot bounding box is a sphere in the sense of the infinity\r\n # norm, hence I call half the max range the plot radius.\r\n plot_radius = 0.5*max([x_range, y_range, z_range])\r\n\r\n ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])\r\n ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])\r\n ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])\r\n\r\ndef make_transparent(ax):\r\n # make the panes transparent\r\n ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\r\n ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\r\n ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\r\n # make the grid lines transparent\r\n ax.xaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\r\n ax.yaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\r\n ax.zaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\r\n\r\nif __name__ == '__main__':\r\n pass\r\n" }, { "path": "Sampling_based_Planning/rrt_3D/queue.py", "content": "# min heap used in the FMT*\n\nimport collections\nimport heapq\nimport itertools\n\nclass MinheapPQ:\n \"\"\"\n A priority queue based on min heap, which takes O(logn) on element removal\n https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes\n \"\"\"\n def __init__(self):\n self.pq = [] # lis of the entries arranged in a heap\n self.nodes = set()\n self.entry_finder = {} # mapping of the item entries\n self.counter = itertools.count() # unique sequence count\n self.REMOVED = ''\n \n def put(self, item, priority):\n '''add a new task or update the priority of an existing item'''\n if item in self.entry_finder:\n self.check_remove(item)\n count = next(self.counter)\n entry = [priority, count, item]\n self.entry_finder[item] = entry\n heapq.heappush(self.pq, entry)\n self.nodes.add(item)\n\n def put_set(self, dictin):\n '''add a new dict into the priority queue'''\n for item, priority in enumerate(dictin):\n self.put(item, priority)\n\n def check_remove(self, item):\n if item not in self.entry_finder:\n return\n entry = self.entry_finder.pop(item)\n entry[-1] = self.REMOVED\n self.nodes.remove(item)\n\n def check_remove_set(self, set_input):\n if len(set_input) == 0:\n return\n for item in set_input:\n if item not in self.entry_finder:\n continue\n entry = self.entry_finder.pop(item)\n entry[-1] = self.REMOVED\n self.nodes.remove(item)\n\n def priority_filtering(self, threshold, mode):\n # mode: bigger: check and remove those key vals bigger than threshold\n if mode == 'lowpass':\n for entry in self.enumerate():\n item = entry[2]\n if entry[0] >= threshold: # priority\n _ = self.entry_finder.pop(item)\n entry[-1] = self.REMOVED\n self.nodes.remove(item)\n # mode: smaller: check and remove those key vals smaller than threshold\n elif mode == 'highpass':\n for entry in self.enumerate():\n item = entry[2]\n if entry[0] <= threshold: # priority\n _ = self.entry_finder.pop(item)\n entry[-1] = self.REMOVED\n self.nodes.remove(item)\n \n def get(self):\n \"\"\"Remove and return the lowest priority task. Raise KeyError if empty.\"\"\"\n while self.pq:\n priority, count, item = heapq.heappop(self.pq)\n if item is not self.REMOVED:\n del self.entry_finder[item]\n self.nodes.remove(item)\n return item\n raise KeyError('pop from an empty priority queue')\n\n def top_key(self):\n return self.pq[0][0]\n \n def enumerate(self):\n return self.pq\n\n def allnodes(self):\n return self.nodes\n" }, { "path": "Sampling_based_Planning/rrt_3D/rrt3D.py", "content": "\"\"\"\nThis is rrt star code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections import defaultdict\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\n\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path\n\n\nclass rrt():\n def __init__(self):\n self.env = env()\n self.Parent = {}\n self.V = []\n # self.E = edgeset()\n self.i = 0\n self.maxiter = 10000\n self.stepsize = 0.5\n self.Path = []\n self.done = False\n self.x0 = tuple(self.env.start)\n self.xt = tuple(self.env.goal)\n\n \n self.ind = 0\n # self.fig = plt.figure(figsize=(10, 8))\n\n def wireup(self, x, y):\n # self.E.add_edge([s, y]) # add edge\n self.Parent[x] = y\n\n def run(self):\n self.V.append(self.x0)\n while self.ind < self.maxiter:\n xrand = sampleFree(self)\n xnearest = nearest(self, xrand)\n xnew, dist = steer(self, xnearest, xrand)\n collide, _ = isCollide(self, xnearest, xnew, dist=dist)\n if not collide:\n self.V.append(xnew) # add point\n self.wireup(xnew, xnearest)\n\n if getDist(xnew, self.xt) <= self.stepsize:\n self.wireup(self.xt, xnew)\n self.Path, D = path(self)\n print('Total distance = ' + str(D))\n break\n visualization(self)\n self.i += 1\n self.ind += 1\n # if the goal is really reached\n \n self.done = True\n visualization(self)\n plt.show()\n\n\nif __name__ == '__main__':\n p = rrt()\n starttime = time.time()\n p.run()\n print('time used = ' + str(time.time() - starttime))\n" }, { "path": "Sampling_based_Planning/rrt_3D/rrt_connect3D.py", "content": "# rrt connect algorithm\n\"\"\"\nThis is rrt connect implementation for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections import defaultdict\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\n\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset\nfrom rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent\n\n\nclass Tree():\n def __init__(self, node):\n self.V = []\n self.Parent = {}\n self.V.append(node)\n # self.Parent[node] = None\n\n def add_vertex(self, node):\n if node not in self.V:\n self.V.append(node)\n \n def add_edge(self, parent, child):\n # here edge is defined a tuple of (parent, child) (qnear, qnew)\n self.Parent[child] = parent\n\n\nclass rrt_connect():\n def __init__(self):\n self.env = env()\n self.Parent = {}\n self.V = []\n self.E = set()\n self.i = 0\n self.maxiter = 10000\n self.stepsize = 0.5\n self.Path = []\n self.done = False\n self.qinit = tuple(self.env.start)\n self.qgoal = tuple(self.env.goal)\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.qnew = None\n self.done = False\n \n self.ind = 0\n self.fig = plt.figure(figsize=(10, 8))\n\n\n#----------Normal RRT algorithm\n def BUILD_RRT(self, qinit):\n tree = Tree(qinit)\n for k in range(self.maxiter):\n qrand = self.RANDOM_CONFIG()\n self.EXTEND(tree, qrand)\n return tree\n\n def EXTEND(self, tree, q):\n qnear = tuple(self.NEAREST_NEIGHBOR(q, tree))\n qnew, dist = steer(self, qnear, q)\n self.qnew = qnew # store qnew outside\n if self.NEW_CONFIG(q, qnear, qnew, dist=None):\n tree.add_vertex(qnew)\n tree.add_edge(qnear, qnew)\n if qnew == q:\n return 'Reached'\n else:\n return 'Advanced'\n return 'Trapped'\n\n def NEAREST_NEIGHBOR(self, q, tree):\n # find the nearest neighbor in the tree\n V = np.array(tree.V)\n if len(V) == 1:\n return V[0]\n xr = repmat(q, len(V), 1)\n dists = np.linalg.norm(xr - V, axis=1)\n return tuple(tree.V[np.argmin(dists)])\n\n def RANDOM_CONFIG(self):\n return tuple(sampleFree(self))\n\n def NEW_CONFIG(self, q, qnear, qnew, dist = None):\n # to check if the new configuration is valid or not by \n # making a move is used for steer\n # check in bound\n collide, _ = isCollide(self, qnear, qnew, dist = dist)\n return not collide\n\n#----------RRT connect algorithm\n def CONNECT(self, Tree, q):\n print('in connect')\n while True:\n S = self.EXTEND(Tree, q)\n if S != 'Advanced':\n break\n return S\n\n def RRT_CONNECT_PLANNER(self, qinit, qgoal):\n Tree_A = Tree(qinit)\n Tree_B = Tree(qgoal)\n for k in range(self.maxiter):\n print(k)\n qrand = self.RANDOM_CONFIG()\n if self.EXTEND(Tree_A, qrand) != 'Trapped':\n qnew = self.qnew # get qnew from outside\n if self.CONNECT(Tree_B, qnew) == 'Reached':\n print('reached')\n self.done = True\n self.Path = self.PATH(Tree_A, Tree_B)\n self.visualization(Tree_A, Tree_B, k)\n plt.show()\n return\n # return\n Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B)\n self.visualization(Tree_A, Tree_B, k)\n return 'Failure'\n\n # def PATH(self, tree_a, tree_b):\n def SWAP(self, tree_a, tree_b):\n tree_a, tree_b = tree_b, tree_a\n return tree_a, tree_b\n\n def PATH(self, tree_a, tree_b):\n qnew = self.qnew\n patha = []\n pathb = []\n while True:\n patha.append((tree_a.Parent[qnew], qnew))\n qnew = tree_a.Parent[qnew]\n if qnew == self.qinit or qnew == self.qgoal:\n break\n qnew = self.qnew\n while True:\n pathb.append((tree_b.Parent[qnew], qnew))\n qnew = tree_b.Parent[qnew]\n if qnew == self.qinit or qnew == self.qgoal:\n break\n return patha + pathb\n\n#----------RRT connect algorithm \n def visualization(self, tree_a, tree_b, index):\n if (index % 20 == 0 and index != 0) or self.done:\n # a_V = np.array(tree_a.V)\n # b_V = np.array(tree_b.V)\n Path = self.Path\n start = self.env.start\n goal = self.env.goal\n a_edges, b_edges = [], []\n for i in tree_a.Parent:\n a_edges.append([i,tree_a.Parent[i]])\n for i in tree_b.Parent:\n b_edges.append([i,tree_b.Parent[i]])\n ax = plt.subplot(111, projection='3d')\n ax.view_init(elev=90., azim=0.)\n ax.clear()\n draw_Spheres(ax, self.env.balls)\n draw_block_list(ax, self.env.blocks)\n if self.env.OBB is not None:\n draw_obb(ax, self.env.OBB)\n draw_block_list(ax, np.array([self.env.boundary]), alpha=0)\n draw_line(ax, a_edges, visibility=0.75, color='g')\n draw_line(ax, b_edges, visibility=0.75, color='y')\n draw_line(ax, Path, color='r')\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')\n set_axes_equal(ax)\n make_transparent(ax)\n ax.set_axis_off()\n plt.pause(0.0001)\n\n\n\nif __name__ == '__main__':\n p = rrt_connect()\n starttime = time.time()\n p.RRT_CONNECT_PLANNER(p.qinit, p.qgoal)\n print('time used = ' + str(time.time() - starttime))\n" }, { "path": "Sampling_based_Planning/rrt_3D/rrt_star3D.py", "content": "\"\"\"\nThis is rrt star code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections import defaultdict\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path\n\n\nclass rrtstar():\n def __init__(self):\n self.env = env()\n\n self.Parent = {}\n self.V = []\n # self.E = edgeset()\n self.COST = {}\n\n self.i = 0\n self.maxiter = 4000 # at least 2000 in this env\n self.stepsize = 2\n self.gamma = 7\n self.eta = self.stepsize\n self.Path = []\n self.done = False\n self.x0 = tuple(self.env.start)\n self.xt = tuple(self.env.goal)\n\n self.V.append(self.x0)\n self.ind = 0\n def wireup(self,x,y):\n # self.E.add_edge([s,y]) # add edge\n self.Parent[x] = y\n\n def removewire(self,xnear):\n xparent = self.Parent[xnear]\n a = [xnear,xparent]\n # self.E.remove_edge(a) # remove and replace old the connection\n\n def reached(self):\n self.done = True\n goal = self.xt\n xn = near(self,self.env.goal)\n c = [cost(self,tuple(x)) for x in xn]\n xncmin = xn[np.argmin(c)]\n self.wireup(goal , tuple(xncmin))\n self.V.append(goal)\n self.Path,self.D = path(self)\n\n def run(self):\n xnew = self.x0\n print('start rrt*... ')\n self.fig = plt.figure(figsize = (10,8))\n while self.ind < self.maxiter:\n xrand = sampleFree(self)\n xnearest = nearest(self,xrand)\n xnew, dist = steer(self,xnearest,xrand)\n collide, _ = isCollide(self,xnearest,xnew,dist=dist)\n if not collide:\n Xnear = near(self,xnew)\n self.V.append(xnew) # add point\n visualization(self)\n plt.title('rrt*')\n # minimal path and minimal cost\n xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)\n # connecting along minimal cost path\n Collide = []\n for xnear in Xnear:\n xnear = tuple(xnear)\n c1 = cost(self, xnear) + getDist(xnew, xnear)\n collide, _ = isCollide(self, xnew, xnear)\n Collide.append(collide)\n if not collide and c1 < cmin:\n xmin, cmin = xnear, c1\n self.wireup(xnew, xmin)\n # rewire\n for i in range(len(Xnear)):\n collide = Collide[i]\n xnear = tuple(Xnear[i])\n c2 = cost(self, xnew) + getDist(xnew, xnear)\n if not collide and c2 < cost(self, xnear):\n # self.removewire(xnear)\n self.wireup(xnear, xnew)\n self.i += 1\n self.ind += 1\n # max sample reached\n self.reached()\n print('time used = ' + str(time.time()-starttime))\n print('Total distance = '+str(self.D))\n visualization(self)\n plt.show()\n \n\nif __name__ == '__main__':\n p = rrtstar()\n starttime = time.time()\n p.run()\n \n" }, { "path": "Sampling_based_Planning/rrt_3D/rrt_star_smart3D.py", "content": "" }, { "path": "Sampling_based_Planning/rrt_3D/utils3D.py", "content": "import numpy as np\r\nfrom numpy.matlib import repmat\r\nimport pyrr as pyrr\r\nfrom collections import deque\r\n\r\nimport os\r\nimport sys\r\n\r\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling_based_Planning/\")\r\nfrom rrt_3D.plot_util3D import visualization\r\n\r\n\r\ndef getRay(x, y):\r\n direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]\r\n return np.array([x, direc])\r\n\r\n\r\ndef getAABB(blocks):\r\n AABB = []\r\n for i in blocks:\r\n AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger\r\n return AABB\r\n\r\n\r\ndef getDist(pos1, pos2):\r\n return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))\r\n\r\n\r\n''' The following utils can be used for rrt or rrt*,\r\n required param initparams should have\r\n env, environement generated from env3D\r\n V, node set\r\n E, edge set\r\n i, nodes added\r\n maxiter, maximum iteration allowed\r\n stepsize, leaf growth restriction\r\n\r\n'''\r\n\r\n\r\ndef sampleFree(initparams, bias = 0.1):\r\n '''biased sampling'''\r\n x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])\r\n i = np.random.random()\r\n if isinside(initparams, x):\r\n return sampleFree(initparams)\r\n else:\r\n if i < bias:\r\n return np.array(initparams.xt) + 1\r\n else:\r\n return x\r\n return x\r\n\r\n# ---------------------- Collision checking algorithms\r\ndef isinside(initparams, x):\r\n '''see if inside obstacle'''\r\n for i in initparams.env.blocks:\r\n if isinbound(i, x):\r\n return True\r\n for i in initparams.env.OBB:\r\n if isinbound(i, x, mode = 'obb'):\r\n return True\r\n for i in initparams.env.balls:\r\n if isinball(i, x):\r\n return True\r\n return False\r\n\r\ndef isinbound(i, x, mode = False, factor = 0, isarray = False):\r\n if mode == 'obb':\r\n return isinobb(i, x, isarray)\r\n if isarray:\r\n compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) \r\n compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) \r\n compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) \r\n return compx & compy & compz\r\n else: \r\n return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5]\r\n\r\ndef isinobb(i, x, isarray = False):\r\n # transform the point from {W} to {body}\r\n if isarray:\r\n pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3]\r\n block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]\r\n return isinbound(block, pts, isarray = isarray)\r\n else:\r\n pt = i.T@np.append(x,1)\r\n block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]\r\n return isinbound(block, pt)\r\n\r\ndef isinball(i, x, factor = 0):\r\n if getDist(i[0:3], x) <= i[3] + factor:\r\n return True\r\n return False\r\n\r\ndef lineSphere(p0, p1, ball):\r\n # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf\r\n c, r = ball[0:3], ball[-1]\r\n line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]]\r\n d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]]\r\n t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * (\r\n line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2])\r\n if t <= 0:\r\n if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True\r\n elif t >= 1:\r\n d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]]\r\n if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True\r\n elif 0 < t < 1:\r\n x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]]\r\n k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]]\r\n if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True\r\n return False\r\n\r\ndef lineAABB(p0, p1, dist, aabb):\r\n # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1\r\n # aabb should have the attributes of P, E as center point and extents\r\n mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point\r\n I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction\r\n hl = dist / 2 # radius\r\n T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]]\r\n # do any of the principal axis form a separting axis?\r\n if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False\r\n if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False\r\n if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False\r\n # I.cross(s axis) ?\r\n r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])\r\n if abs(T[1] * I[2] - T[2] * I[1]) > r: return False\r\n # I.cross(y axis) ?\r\n r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0])\r\n if abs(T[2] * I[0] - T[0] * I[2]) > r: return False\r\n # I.cross(z axis) ?\r\n r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0])\r\n if abs(T[0] * I[1] - T[1] * I[0]) > r: return False\r\n\r\n return True\r\n\r\ndef lineOBB(p0, p1, dist, obb):\r\n # transform points to obb frame\r\n res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T \r\n # record old position and set the position to origin\r\n oldP, obb.P= obb.P, [0,0,0] \r\n # calculate segment-AABB testing\r\n ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb)\r\n # reset the position\r\n obb.P = oldP \r\n return ans\r\n\r\ndef isCollide(initparams, x, child, dist=None):\r\n '''see if line intersects obstacle'''\r\n '''specified for expansion in A* 3D lookup table'''\r\n if dist==None:\r\n dist = getDist(x, child)\r\n # check in bound\r\n if not isinbound(initparams.env.boundary, child): \r\n return True, dist\r\n # check collision in AABB\r\n for i in range(len(initparams.env.AABB)):\r\n if lineAABB(x, child, dist, initparams.env.AABB[i]): \r\n return True, dist\r\n # check collision in ball\r\n for i in initparams.env.balls:\r\n if lineSphere(x, child, i): \r\n return True, dist\r\n # check collision with obb\r\n for i in initparams.env.OBB:\r\n if lineOBB(x, child, dist, i):\r\n return True, dist\r\n return False, dist\r\n\r\n# ---------------------- leaf node extending algorithms\r\ndef nearest(initparams, x, isset=False):\r\n V = np.array(initparams.V)\r\n if initparams.i == 0:\r\n return initparams.V[0]\r\n xr = repmat(x, len(V), 1)\r\n dists = np.linalg.norm(xr - V, axis=1)\r\n return tuple(initparams.V[np.argmin(dists)])\r\n\r\ndef near(initparams, x):\r\n # s = np.array(s)\r\n V = np.array(initparams.V)\r\n if initparams.i == 0:\r\n return [initparams.V[0]]\r\n cardV = len(initparams.V)\r\n eta = initparams.eta\r\n gamma = initparams.gamma\r\n # min{γRRT∗ (log(card (V ))/ card (V ))1/d, η}\r\n r = min(gamma * ((np.log(cardV) / cardV) ** (1/3)), eta)\r\n if initparams.done: \r\n r = 1\r\n xr = repmat(x, len(V), 1)\r\n inside = np.linalg.norm(xr - V, axis=1) < r\r\n nearpoints = V[inside]\r\n return np.array(nearpoints)\r\n\r\ndef steer(initparams, x, y, DIST=False):\r\n # steer from s to y\r\n if np.equal(x, y).all():\r\n return x, 0.0\r\n dist, step = getDist(y, x), initparams.stepsize\r\n step = min(dist, step)\r\n increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)\r\n xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])\r\n # direc = (y - s) / np.linalg.norm(y - s)\r\n # xnew = s + initparams.stepsize * direc\r\n if DIST:\r\n return xnew, dist\r\n return xnew, dist\r\n\r\ndef cost(initparams, x):\r\n '''here use the additive recursive cost function'''\r\n if x == initparams.x0:\r\n return 0\r\n return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x])\r\n\r\ndef cost_from_set(initparams, x):\r\n '''here use a incremental cost set function'''\r\n if x == initparams.x0:\r\n return 0\r\n return initparams.COST[initparams.Parent[x]] + getDist(x, initparams.Parent[x])\r\n\r\ndef path(initparams, Path=[], dist=0):\r\n x = initparams.xt\r\n while x != initparams.x0:\r\n x2 = initparams.Parent[x]\r\n Path.append(np.array([x, x2]))\r\n dist += getDist(x, x2)\r\n x = x2\r\n return Path, dist\r\n\r\nclass edgeset(object):\r\n def __init__(self):\r\n self.E = {}\r\n\r\n def add_edge(self, edge):\r\n x, y = edge[0], edge[1]\r\n if x in self.E:\r\n self.E[x].add(y)\r\n else:\r\n self.E[x] = set()\r\n self.E[x].add(y)\r\n\r\n def remove_edge(self, edge):\r\n x, y = edge[0], edge[1]\r\n self.E[x].remove(y)\r\n\r\n def get_edge(self, nodes = None):\r\n edges = []\r\n if nodes is None:\r\n for v in self.E:\r\n for n in self.E[v]:\r\n # if (n,v) not in edges:\r\n edges.append((v, n))\r\n else: \r\n for v in nodes:\r\n for n in self.E[tuple(v)]:\r\n edges.append((v, n))\r\n return edges\r\n\r\n def isEndNode(self, node):\r\n return node not in self.E\r\n\r\n#------------------------ use a linked list to express the tree \r\nclass Node:\r\n def __init__(self, data):\r\n self.pos = data\r\n self.Parent = None\r\n self.child = set()\r\n\r\ndef tree_add_edge(node_in_tree, x):\r\n # add an edge at the specified parent\r\n node_to_add = Node(x)\r\n # node_in_tree = tree_bfs(head, xparent)\r\n node_in_tree.child.add(node_to_add)\r\n node_to_add.Parent = node_in_tree\r\n return node_to_add\r\n\r\ndef tree_bfs(head, x):\r\n # searches s in order of bfs\r\n node = head\r\n Q = []\r\n Q.append(node)\r\n while Q:\r\n curr = Q.pop()\r\n if curr.pos == x:\r\n return curr\r\n for child_node in curr.child:\r\n Q.append(child_node)\r\n\r\ndef tree_nearest(head, x):\r\n # find the node nearest to s\r\n D = np.inf\r\n min_node = None\r\n\r\n Q = []\r\n Q.append(head)\r\n while Q:\r\n curr = Q.pop()\r\n dist = getDist(curr.pos, x)\r\n # record the current best\r\n if dist < D:\r\n D, min_node = dist, curr\r\n # bfs\r\n for child_node in curr.child:\r\n Q.append(child_node)\r\n return min_node\r\n\r\ndef tree_steer(initparams, node, x):\r\n # steer from node to s\r\n dist, step = getDist(node.pos, x), initparams.stepsize\r\n increment = ((node.pos[0] - x[0]) / dist * step, (node.pos[1] - x[1]) / dist * step, (node.pos[2] - x[2]) / dist * step)\r\n xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])\r\n return xnew\r\n\r\ndef tree_print(head):\r\n Q = []\r\n Q.append(head)\r\n verts = []\r\n edge = []\r\n while Q:\r\n curr = Q.pop()\r\n # print(curr.pos)\r\n verts.append(curr.pos)\r\n if curr.Parent == None:\r\n pass\r\n else:\r\n edge.append([curr.pos, curr.Parent.pos])\r\n for child in curr.child:\r\n Q.append(child)\r\n return verts, edge\r\n\r\ndef tree_path(initparams, end_node):\r\n path = []\r\n curr = end_node\r\n while curr.pos != initparams.x0:\r\n path.append([curr.pos, curr.Parent.pos])\r\n curr = curr.Parent\r\n return path\r\n\r\n\r\n#---------------KD tree, used for nearest neighbor search\r\nclass kdTree:\r\n def __init__(self):\r\n pass\r\n\r\n def R1_dist(self, q, p):\r\n return abs(q-p)\r\n\r\n def S1_dist(self, q, p):\r\n return min(abs(q-p), 1- abs(q-p))\r\n\r\n def P3_dist(self, q, p):\r\n # cubes with antipodal points\r\n q1, q2, q3 = q\r\n p1, p2, p3 = p\r\n d1 = np.sqrt((q1-p1)**2 + (q2-p2)**2 + (q3-p3)**2)\r\n d2 = np.sqrt((1-abs(q1-p1))**2 + (1-abs(q2-p2))**2 + (1-abs(q3-p3))**2)\r\n d3 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3+1-p3)**2)\r\n d4 = np.sqrt((-q1-p1)**2 + (-q2-p2)**2 + (q3-1-p3)**2)\r\n d5 = np.sqrt((-q1-p1)**2 + (q2+1-p2)**2 + (-q3-p3)**2)\r\n d6 = np.sqrt((-q1-p1)**2 + (q2-1-p2)**2 + (-q3-p3)**2)\r\n d7 = np.sqrt((q1+1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2)\r\n d8 = np.sqrt((q1-1-p1)**2 + (-q2-p2)**2 + (-q3-p3)**2)\r\n return min(d1,d2,d3,d4,d5,d6,d7,d8)\r\n\r\n\r\n\r\nif __name__ == '__main__':\r\n from rrt_3D.env3D import env\r\n import time\r\n import matplotlib.pyplot as plt\r\n class rrt_demo:\r\n def __init__(self):\r\n self.env = env()\r\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\r\n self.stepsize = 0.5\r\n self.maxiter = 10000\r\n self.ind, self.i = 0, 0\r\n self.done = False\r\n self.Path = []\r\n self.V = []\r\n\r\n self.head = Node(self.x0)\r\n \r\n def run(self):\r\n while self.ind < self.maxiter:\r\n xrand = sampleFree(self) # O(1)\r\n nearest_node = tree_nearest(self.head, xrand) # O(N)\r\n xnew = tree_steer(self, nearest_node, xrand) # O(1)\r\n collide, _ = isCollide(self, nearest_node.pos, xnew) # O(num obs)\r\n if not collide:\r\n new_node = tree_add_edge(nearest_node, xnew) # O(1)\r\n # if the path is found\r\n if getDist(xnew, self.xt) <= self.stepsize:\r\n end_node = tree_add_edge(new_node, self.xt)\r\n self.Path = tree_path(self, end_node)\r\n break\r\n self.i += 1\r\n self.ind += 1\r\n \r\n self.done = True\r\n self.V, self.E = tree_print(self.head)\r\n print(self.E)\r\n visualization(self)\r\n plt.show()\r\n \r\n\r\n \r\n A = rrt_demo()\r\n st = time.time()\r\n A.run()\r\n print(time.time() - st)\r\n\r\n \r\n \r\n\r\n\r\n" }, { "path": "Search_based_Planning/Search_2D/ARAstar.py", "content": "\"\"\"\nARA_star 2D (Anytime Repairing A*)\n@author: huiming zhou\n\n@description: local inconsistency: g-value decreased.\ng(s) decreased introduces a local inconsistency between s and its successors.\n\n\"\"\"\n\nimport os\nimport sys\nimport math\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass AraStar:\n def __init__(self, s_start, s_goal, e, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n self.e = e # weight\n\n self.g = dict() # Cost to come\n self.OPEN = dict() # priority queue / OPEN set\n self.CLOSED = set() # CLOSED set\n self.INCONS = {} # INCONSISTENT set\n self.PARENT = dict() # relations\n self.path = [] # planning path\n self.visited = [] # order of visited nodes\n\n def init(self):\n \"\"\"\n initialize each set.\n \"\"\"\n\n self.g[self.s_start] = 0.0\n self.g[self.s_goal] = math.inf\n self.OPEN[self.s_start] = self.f_value(self.s_start)\n self.PARENT[self.s_start] = self.s_start\n\n def searching(self):\n self.init()\n self.ImprovePath()\n self.path.append(self.extract_path())\n\n while self.update_e() > 1: # continue condition\n self.e -= 0.4 # increase weight\n self.OPEN.update(self.INCONS)\n self.OPEN = {s: self.f_value(s) for s in self.OPEN} # update f_value of OPEN set\n\n self.INCONS = dict()\n self.CLOSED = set()\n self.ImprovePath() # improve path\n self.path.append(self.extract_path())\n\n return self.path, self.visited\n\n def ImprovePath(self):\n \"\"\"\n :return: a e'-suboptimal path\n \"\"\"\n\n visited_each = []\n\n while True:\n s, f_small = self.calc_smallest_f()\n\n if self.f_value(self.s_goal) <= f_small:\n break\n\n self.OPEN.pop(s)\n self.CLOSED.add(s)\n\n for s_n in self.get_neighbor(s):\n if s_n in self.obs:\n continue\n\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g or new_cost < self.g[s_n]:\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n visited_each.append(s_n)\n\n if s_n not in self.CLOSED:\n self.OPEN[s_n] = self.f_value(s_n)\n else:\n self.INCONS[s_n] = 0.0\n\n self.visited.append(visited_each)\n\n def calc_smallest_f(self):\n \"\"\"\n :return: node with smallest f_value in OPEN set.\n \"\"\"\n\n s_small = min(self.OPEN, key=self.OPEN.get)\n\n return s_small, self.OPEN[s_small]\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n return {(s[0] + u[0], s[1] + u[1]) for u in self.u_set}\n\n def update_e(self):\n v = float(\"inf\")\n\n if self.OPEN:\n v = min(self.g[s] + self.h(s) for s in self.OPEN)\n if self.INCONS:\n v = min(v, min(self.g[s] + self.h(s) for s in self.INCONS))\n\n return min(self.e, self.g[self.s_goal] / v)\n\n def f_value(self, x):\n \"\"\"\n f = g + e * h\n f = cost-to-come + weight * cost-to-go\n :param x: current state\n :return: f_value\n \"\"\"\n\n return self.g[x] + self.e * self.h(x)\n\n def extract_path(self):\n \"\"\"\n Extract the path based on the PARENT set.\n :return: The planning path\n \"\"\"\n\n path = [self.s_goal]\n s = self.s_goal\n\n while True:\n s = self.PARENT[s]\n path.append(s)\n\n if s == self.s_start:\n break\n\n return list(path)\n\n def h(self, s):\n \"\"\"\n Calculate heuristic.\n :param s: current node (state)\n :return: heuristic function value\n \"\"\"\n\n heuristic_type = self.heuristic_type # heuristic type\n goal = self.s_goal # goal node\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return math.inf\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n \"\"\"\n check if the line segment (s_start, s_end) is collision.\n :param s_start: start node\n :param s_end: end node\n :return: True: is collision / False: not collision\n \"\"\"\n\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n arastar = AraStar(s_start, s_goal, 2.5, \"euclidean\")\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = arastar.searching()\n plot.animation_ara_star(path, visited, \"Anytime Repairing A* (ARA*)\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/Anytime_D_star.py", "content": "\"\"\"\nAnytime_D_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting\nfrom Search_2D import env\n\n\nclass ADStar:\n def __init__(self, s_start, s_goal, eps, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n self.Plot = plotting.Plotting(s_start, s_goal)\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n self.x = self.Env.x_range\n self.y = self.Env.y_range\n\n self.g, self.rhs, self.OPEN = {}, {}, {}\n\n for i in range(1, self.Env.x_range - 1):\n for j in range(1, self.Env.y_range - 1):\n self.rhs[(i, j)] = float(\"inf\")\n self.g[(i, j)] = float(\"inf\")\n\n self.rhs[self.s_goal] = 0.0\n self.eps = eps\n self.OPEN[self.s_goal] = self.Key(self.s_goal)\n self.CLOSED, self.INCONS = set(), dict()\n\n self.visited = set()\n self.count = 0\n self.count_env_change = 0\n self.obs_add = set()\n self.obs_remove = set()\n self.title = \"Anytime D*: Small changes\" # Significant changes\n self.fig = plt.figure()\n\n def run(self):\n self.Plot.plot_grid(self.title)\n self.ComputeOrImprovePath()\n self.plot_visited()\n self.plot_path(self.extract_path())\n self.visited = set()\n\n while True:\n if self.eps <= 1.0:\n break\n self.eps -= 0.5\n self.OPEN.update(self.INCONS)\n for s in self.OPEN:\n self.OPEN[s] = self.Key(s)\n self.CLOSED = set()\n self.ComputeOrImprovePath()\n self.plot_visited()\n self.plot_path(self.extract_path())\n self.visited = set()\n plt.pause(0.5)\n\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n plt.show()\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:\n print(\"Please choose right area!\")\n else:\n self.count_env_change += 1\n x, y = int(x), int(y)\n print(\"Change position: s =\", x, \",\", \"y =\", y)\n\n # for small changes\n if self.title == \"Anytime D*: Small changes\":\n if (x, y) not in self.obs:\n self.obs.add((x, y))\n self.g[(x, y)] = float(\"inf\")\n self.rhs[(x, y)] = float(\"inf\")\n else:\n self.obs.remove((x, y))\n self.UpdateState((x, y))\n\n self.Plot.update_obs(self.obs)\n\n for sn in self.get_neighbor((x, y)):\n self.UpdateState(sn)\n\n plt.cla()\n self.Plot.plot_grid(self.title)\n\n while True:\n if len(self.INCONS) == 0:\n break\n self.OPEN.update(self.INCONS)\n for s in self.OPEN:\n self.OPEN[s] = self.Key(s)\n self.CLOSED = set()\n self.ComputeOrImprovePath()\n self.plot_visited()\n self.plot_path(self.extract_path())\n # plt.plot(self.title)\n self.visited = set()\n\n if self.eps <= 1.0:\n break\n\n else:\n if (x, y) not in self.obs:\n self.obs.add((x, y))\n self.obs_add.add((x, y))\n plt.plot(x, y, 'sk')\n if (x, y) in self.obs_remove:\n self.obs_remove.remove((x, y))\n else:\n self.obs.remove((x, y))\n self.obs_remove.add((x, y))\n plt.plot(x, y, marker='s', color='white')\n if (x, y) in self.obs_add:\n self.obs_add.remove((x, y))\n\n self.Plot.update_obs(self.obs)\n\n if self.count_env_change >= 15:\n self.count_env_change = 0\n self.eps += 2.0\n for s in self.obs_add:\n self.g[(x, y)] = float(\"inf\")\n self.rhs[(x, y)] = float(\"inf\")\n\n for sn in self.get_neighbor(s):\n self.UpdateState(sn)\n\n for s in self.obs_remove:\n for sn in self.get_neighbor(s):\n self.UpdateState(sn)\n self.UpdateState(s)\n\n plt.cla()\n self.Plot.plot_grid(self.title)\n\n while True:\n if self.eps <= 1.0:\n break\n self.eps -= 0.5\n self.OPEN.update(self.INCONS)\n for s in self.OPEN:\n self.OPEN[s] = self.Key(s)\n self.CLOSED = set()\n self.ComputeOrImprovePath()\n self.plot_visited()\n self.plot_path(self.extract_path())\n plt.title(self.title)\n self.visited = set()\n plt.pause(0.5)\n\n self.fig.canvas.draw_idle()\n\n def ComputeOrImprovePath(self):\n while True:\n s, v = self.TopKey()\n if v >= self.Key(self.s_start) and \\\n self.rhs[self.s_start] == self.g[self.s_start]:\n break\n\n self.OPEN.pop(s)\n self.visited.add(s)\n\n if self.g[s] > self.rhs[s]:\n self.g[s] = self.rhs[s]\n self.CLOSED.add(s)\n for sn in self.get_neighbor(s):\n self.UpdateState(sn)\n else:\n self.g[s] = float(\"inf\")\n for sn in self.get_neighbor(s):\n self.UpdateState(sn)\n self.UpdateState(s)\n\n def UpdateState(self, s):\n if s != self.s_goal:\n self.rhs[s] = float(\"inf\")\n for x in self.get_neighbor(s):\n self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x))\n if s in self.OPEN:\n self.OPEN.pop(s)\n\n if self.g[s] != self.rhs[s]:\n if s not in self.CLOSED:\n self.OPEN[s] = self.Key(s)\n else:\n self.INCONS[s] = 0\n\n def Key(self, s):\n if self.g[s] > self.rhs[s]:\n return [self.rhs[s] + self.eps * self.h(self.s_start, s), self.rhs[s]]\n else:\n return [self.g[s] + self.h(self.s_start, s), self.g[s]]\n\n def TopKey(self):\n \"\"\"\n :return: return the min key and its value.\n \"\"\"\n\n s = min(self.OPEN, key=self.OPEN.get)\n return s, self.OPEN[s]\n\n def h(self, s_start, s_goal):\n heuristic_type = self.heuristic_type # heuristic type\n\n if heuristic_type == \"manhattan\":\n return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])\n else:\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n def get_neighbor(self, s):\n nei_list = set()\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n nei_list.add(s_next)\n\n return nei_list\n\n def extract_path(self):\n \"\"\"\n Extract the path based on the PARENT set.\n :return: The planning path\n \"\"\"\n\n path = [self.s_start]\n s = self.s_start\n\n for k in range(100):\n g_list = {}\n for x in self.get_neighbor(s):\n if not self.is_collision(s, x):\n g_list[x] = self.g[x]\n s = min(g_list, key=g_list.get)\n path.append(s)\n if s == self.s_goal:\n break\n\n return list(path)\n\n def plot_path(self, path):\n px = [x[0] for x in path]\n py = [x[1] for x in path]\n plt.plot(px, py, linewidth=2)\n plt.plot(self.s_start[0], self.s_start[1], \"bs\")\n plt.plot(self.s_goal[0], self.s_goal[1], \"gs\")\n\n def plot_visited(self):\n self.count += 1\n\n color = ['gainsboro', 'lightgray', 'silver', 'darkgray',\n 'bisque', 'navajowhite', 'moccasin', 'wheat',\n 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']\n\n if self.count >= len(color) - 1:\n self.count = 0\n\n for x in self.visited:\n plt.plot(x[0], x[1], marker='s', color=color[self.count])\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n dstar = ADStar(s_start, s_goal, 2.5, \"euclidean\")\n dstar.run()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/Astar.py", "content": "\"\"\"\nA_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass AStar:\n \"\"\"AStar set the cost + heuristics as the priority\n \"\"\"\n def __init__(self, s_start, s_goal, heuristic_type):\n self.s_start = s_start\n self.s_goal = s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n\n self.OPEN = [] # priority queue / OPEN set\n self.CLOSED = [] # CLOSED set / VISITED order\n self.PARENT = dict() # recorded parent\n self.g = dict() # cost to come\n\n def searching(self):\n \"\"\"\n A_star Searching.\n :return: path, visited order\n \"\"\"\n\n self.PARENT[self.s_start] = self.s_start\n self.g[self.s_start] = 0\n self.g[self.s_goal] = math.inf\n heapq.heappush(self.OPEN,\n (self.f_value(self.s_start), self.s_start))\n\n while self.OPEN:\n _, s = heapq.heappop(self.OPEN)\n self.CLOSED.append(s)\n\n if s == self.s_goal: # stop condition\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g:\n self.g[s_n] = math.inf\n\n if new_cost < self.g[s_n]: # conditions for updating Cost\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))\n\n return self.extract_path(self.PARENT), self.CLOSED\n\n def searching_repeated_astar(self, e):\n \"\"\"\n repeated A*.\n :param e: weight of A*\n :return: path and visited order\n \"\"\"\n\n path, visited = [], []\n\n while e >= 1:\n p_k, v_k = self.repeated_searching(self.s_start, self.s_goal, e)\n path.append(p_k)\n visited.append(v_k)\n e -= 0.5\n\n return path, visited\n\n def repeated_searching(self, s_start, s_goal, e):\n \"\"\"\n run A* with weight e.\n :param s_start: starting state\n :param s_goal: goal state\n :param e: weight of a*\n :return: path and visited order.\n \"\"\"\n\n g = {s_start: 0, s_goal: float(\"inf\")}\n PARENT = {s_start: s_start}\n OPEN = []\n CLOSED = []\n heapq.heappush(OPEN,\n (g[s_start] + e * self.heuristic(s_start), s_start))\n\n while OPEN:\n _, s = heapq.heappop(OPEN)\n CLOSED.append(s)\n\n if s == s_goal:\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = g[s] + self.cost(s, s_n)\n\n if s_n not in g:\n g[s_n] = math.inf\n\n if new_cost < g[s_n]: # conditions for updating Cost\n g[s_n] = new_cost\n PARENT[s_n] = s\n heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n))\n\n return self.extract_path(PARENT), CLOSED\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return math.inf\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n \"\"\"\n check if the line segment (s_start, s_end) is collision.\n :param s_start: start node\n :param s_end: end node\n :return: True: is collision / False: not collision\n \"\"\"\n\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n def f_value(self, s):\n \"\"\"\n f = g + h. (g: Cost to come, h: heuristic value)\n :param s: current state\n :return: f\n \"\"\"\n\n return self.g[s] + self.heuristic(s)\n\n def extract_path(self, PARENT):\n \"\"\"\n Extract the path based on the PARENT set.\n :return: The planning path\n \"\"\"\n\n path = [self.s_goal]\n s = self.s_goal\n\n while True:\n s = PARENT[s]\n path.append(s)\n\n if s == self.s_start:\n break\n\n return list(path)\n\n def heuristic(self, s):\n \"\"\"\n Calculate heuristic.\n :param s: current node (state)\n :return: heuristic function value\n \"\"\"\n\n heuristic_type = self.heuristic_type # heuristic type\n goal = self.s_goal # goal node\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n astar = AStar(s_start, s_goal, \"euclidean\")\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = astar.searching()\n plot.animation(path, visited, \"A*\") # animation\n\n # path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5\n # plot.animation_ara_star(path, visited, \"Repeated A*\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/Best_First.py", "content": "\"\"\"\nBest-First Searching\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\nfrom Search_2D.Astar import AStar\n\n\nclass BestFirst(AStar):\n \"\"\"BestFirst set the heuristics as the priority \n \"\"\"\n def searching(self):\n \"\"\"\n Breadth-first Searching.\n :return: path, visited order\n \"\"\"\n\n self.PARENT[self.s_start] = self.s_start\n self.g[self.s_start] = 0\n self.g[self.s_goal] = math.inf\n heapq.heappush(self.OPEN,\n (self.heuristic(self.s_start), self.s_start))\n\n while self.OPEN:\n _, s = heapq.heappop(self.OPEN)\n self.CLOSED.append(s)\n\n if s == self.s_goal:\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g:\n self.g[s_n] = math.inf\n\n if new_cost < self.g[s_n]: # conditions for updating Cost\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n\n # best first set the heuristics as the priority \n heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))\n\n return self.extract_path(self.PARENT), self.CLOSED\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n BF = BestFirst(s_start, s_goal, 'euclidean')\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = BF.searching()\n plot.animation(path, visited, \"Best-first Searching\") # animation\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/Bidirectional_a_star.py", "content": "\"\"\"\nBidirectional_a_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass BidirectionalAStar:\n def __init__(self, s_start, s_goal, heuristic_type):\n self.s_start = s_start\n self.s_goal = s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n\n self.OPEN_fore = [] # OPEN set for forward searching\n self.OPEN_back = [] # OPEN set for backward searching\n self.CLOSED_fore = [] # CLOSED set for forward\n self.CLOSED_back = [] # CLOSED set for backward\n self.PARENT_fore = dict() # recorded parent for forward\n self.PARENT_back = dict() # recorded parent for backward\n self.g_fore = dict() # cost to come for forward\n self.g_back = dict() # cost to come for backward\n\n def init(self):\n \"\"\"\n initialize parameters\n \"\"\"\n\n self.g_fore[self.s_start] = 0.0\n self.g_fore[self.s_goal] = math.inf\n self.g_back[self.s_goal] = 0.0\n self.g_back[self.s_start] = math.inf\n self.PARENT_fore[self.s_start] = self.s_start\n self.PARENT_back[self.s_goal] = self.s_goal\n heapq.heappush(self.OPEN_fore,\n (self.f_value_fore(self.s_start), self.s_start))\n heapq.heappush(self.OPEN_back,\n (self.f_value_back(self.s_goal), self.s_goal))\n\n def searching(self):\n \"\"\"\n Bidirectional A*\n :return: connected path, visited order of forward, visited order of backward\n \"\"\"\n\n self.init()\n s_meet = self.s_start\n\n while self.OPEN_fore and self.OPEN_back:\n # solve foreward-search\n _, s_fore = heapq.heappop(self.OPEN_fore)\n\n if s_fore in self.PARENT_back:\n s_meet = s_fore\n break\n\n self.CLOSED_fore.append(s_fore)\n\n for s_n in self.get_neighbor(s_fore):\n new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n)\n\n if s_n not in self.g_fore:\n self.g_fore[s_n] = math.inf\n\n if new_cost < self.g_fore[s_n]:\n self.g_fore[s_n] = new_cost\n self.PARENT_fore[s_n] = s_fore\n heapq.heappush(self.OPEN_fore,\n (self.f_value_fore(s_n), s_n))\n\n # solve backward-search\n _, s_back = heapq.heappop(self.OPEN_back)\n\n if s_back in self.PARENT_fore:\n s_meet = s_back\n break\n\n self.CLOSED_back.append(s_back)\n\n for s_n in self.get_neighbor(s_back):\n new_cost = self.g_back[s_back] + self.cost(s_back, s_n)\n\n if s_n not in self.g_back:\n self.g_back[s_n] = math.inf\n\n if new_cost < self.g_back[s_n]:\n self.g_back[s_n] = new_cost\n self.PARENT_back[s_n] = s_back\n heapq.heappush(self.OPEN_back,\n (self.f_value_back(s_n), s_n))\n\n return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]\n\n def extract_path(self, s_meet):\n \"\"\"\n extract path from start and goal\n :param s_meet: meet point of bi-direction a*\n :return: path\n \"\"\"\n\n # extract path for foreward part\n path_fore = [s_meet]\n s = s_meet\n\n while True:\n s = self.PARENT_fore[s]\n path_fore.append(s)\n if s == self.s_start:\n break\n\n # extract path for backward part\n path_back = []\n s = s_meet\n\n while True:\n s = self.PARENT_back[s]\n path_back.append(s)\n if s == self.s_goal:\n break\n\n return list(reversed(path_fore)) + list(path_back)\n\n def f_value_fore(self, s):\n \"\"\"\n forward searching: f = g + h. (g: Cost to come, h: heuristic value)\n :param s: current state\n :return: f\n \"\"\"\n\n return self.g_fore[s] + self.h(s, self.s_goal)\n\n def f_value_back(self, s):\n \"\"\"\n backward searching: f = g + h. (g: Cost to come, h: heuristic value)\n :param s: current state\n :return: f\n \"\"\"\n\n return self.g_back[s] + self.h(s, self.s_start)\n\n def h(self, s, goal):\n \"\"\"\n Calculate heuristic value.\n :param s: current node (state)\n :param goal: goal node (state)\n :return: heuristic value\n \"\"\"\n\n heuristic_type = self.heuristic_type\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return math.inf\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n \"\"\"\n check if the line segment (s_start, s_end) is collision.\n :param s_start: start node\n :param s_end: end node\n :return: True: is collision / False: not collision\n \"\"\"\n\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n\ndef main():\n x_start = (5, 5)\n x_goal = (45, 25)\n\n bastar = BidirectionalAStar(x_start, x_goal, \"euclidean\")\n plot = plotting.Plotting(x_start, x_goal)\n\n path, visited_fore, visited_back = bastar.searching()\n plot.animation_bi_astar(path, visited_fore, visited_back, \"Bidirectional-A*\") # animation\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/D_star.py", "content": "\"\"\"\nD_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass DStar:\n def __init__(self, s_start, s_goal):\n self.s_start, self.s_goal = s_start, s_goal\n\n self.Env = env.Env()\n self.Plot = plotting.Plotting(self.s_start, self.s_goal)\n\n self.u_set = self.Env.motions\n self.obs = self.Env.obs\n self.x = self.Env.x_range\n self.y = self.Env.y_range\n\n self.fig = plt.figure()\n\n self.OPEN = set()\n self.t = dict()\n self.PARENT = dict()\n self.h = dict()\n self.k = dict()\n self.path = []\n self.visited = set()\n self.count = 0\n\n def init(self):\n for i in range(self.Env.x_range):\n for j in range(self.Env.y_range):\n self.t[(i, j)] = 'NEW'\n self.k[(i, j)] = 0.0\n self.h[(i, j)] = float(\"inf\")\n self.PARENT[(i, j)] = None\n\n self.h[self.s_goal] = 0.0\n\n def run(self, s_start, s_end):\n self.init()\n self.insert(s_end, 0)\n\n while True:\n self.process_state()\n if self.t[s_start] == 'CLOSED':\n break\n\n self.path = self.extract_path(s_start, s_end)\n self.Plot.plot_grid(\"Dynamic A* (D*)\")\n self.plot_path(self.path)\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n plt.show()\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:\n print(\"Please choose right area!\")\n else:\n x, y = int(x), int(y)\n if (x, y) not in self.obs:\n print(\"Add obstacle at: s =\", x, \",\", \"y =\", y)\n self.obs.add((x, y))\n self.Plot.update_obs(self.obs)\n\n s = self.s_start\n self.visited = set()\n self.count += 1\n\n while s != self.s_goal:\n if self.is_collision(s, self.PARENT[s]):\n self.modify(s)\n continue\n s = self.PARENT[s]\n\n self.path = self.extract_path(self.s_start, self.s_goal)\n\n plt.cla()\n self.Plot.plot_grid(\"Dynamic A* (D*)\")\n self.plot_visited(self.visited)\n self.plot_path(self.path)\n\n self.fig.canvas.draw_idle()\n\n def extract_path(self, s_start, s_end):\n path = [s_start]\n s = s_start\n while True:\n s = self.PARENT[s]\n path.append(s)\n if s == s_end:\n return path\n\n def process_state(self):\n s = self.min_state() # get node in OPEN set with min k value\n self.visited.add(s)\n\n if s is None:\n return -1 # OPEN set is empty\n\n k_old = self.get_k_min() # record the min k value of this iteration (min path cost)\n self.delete(s) # move state s from OPEN set to CLOSED set\n\n # k_min < h[s] --> s: RAISE state (increased cost)\n if k_old < self.h[s]:\n for s_n in self.get_neighbor(s):\n if self.h[s_n] <= k_old and \\\n self.h[s] > self.h[s_n] + self.cost(s_n, s):\n\n # update h_value and choose parent\n self.PARENT[s] = s_n\n self.h[s] = self.h[s_n] + self.cost(s_n, s)\n\n # s: k_min >= h[s] -- > s: LOWER state (cost reductions)\n if k_old == self.h[s]:\n for s_n in self.get_neighbor(s):\n if self.t[s_n] == 'NEW' or \\\n (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \\\n (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)):\n\n # Condition:\n # 1) t[s_n] == 'NEW': not visited\n # 2) s_n's parent: cost reduction\n # 3) s_n find a better parent\n self.PARENT[s_n] = s\n self.insert(s_n, self.h[s] + self.cost(s, s_n))\n else:\n for s_n in self.get_neighbor(s):\n if self.t[s_n] == 'NEW' or \\\n (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)):\n\n # Condition:\n # 1) t[s_n] == 'NEW': not visited\n # 2) s_n's parent: cost reduction\n self.PARENT[s_n] = s\n self.insert(s_n, self.h[s] + self.cost(s, s_n))\n else:\n if self.PARENT[s_n] != s and \\\n self.h[s_n] > self.h[s] + self.cost(s, s_n):\n\n # Condition: LOWER happened in OPEN set (s), s should be explored again\n self.insert(s, self.h[s])\n else:\n if self.PARENT[s_n] != s and \\\n self.h[s] > self.h[s_n] + self.cost(s_n, s) and \\\n self.t[s_n] == 'CLOSED' and \\\n self.h[s_n] > k_old:\n\n # Condition: LOWER happened in CLOSED set (s_n), s_n should be explored again\n self.insert(s_n, self.h[s_n])\n\n return self.get_k_min()\n\n def min_state(self):\n \"\"\"\n choose the node with the minimum k value in OPEN set.\n :return: state\n \"\"\"\n\n if not self.OPEN:\n return None\n\n return min(self.OPEN, key=lambda x: self.k[x])\n\n def get_k_min(self):\n \"\"\"\n calc the min k value for nodes in OPEN set.\n :return: k value\n \"\"\"\n\n if not self.OPEN:\n return -1\n\n return min([self.k[x] for x in self.OPEN])\n\n def insert(self, s, h_new):\n \"\"\"\n insert node into OPEN set.\n :param s: node\n :param h_new: new or better cost to come value\n \"\"\"\n\n if self.t[s] == 'NEW':\n self.k[s] = h_new\n elif self.t[s] == 'OPEN':\n self.k[s] = min(self.k[s], h_new)\n elif self.t[s] == 'CLOSED':\n self.k[s] = min(self.h[s], h_new)\n\n self.h[s] = h_new\n self.t[s] = 'OPEN'\n self.OPEN.add(s)\n\n def delete(self, s):\n \"\"\"\n delete: move state s from OPEN set to CLOSED set.\n :param s: state should be deleted\n \"\"\"\n\n if self.t[s] == 'OPEN':\n self.t[s] = 'CLOSED'\n\n self.OPEN.remove(s)\n\n def modify(self, s):\n \"\"\"\n start processing from state s.\n :param s: is a node whose status is RAISE or LOWER.\n \"\"\"\n\n self.modify_cost(s)\n\n while True:\n k_min = self.process_state()\n\n if k_min >= self.h[s]:\n break\n\n def modify_cost(self, s):\n # if node in CLOSED set, put it into OPEN set.\n # Since cost may be changed between s - s.parent, calc cost(s, s.p) again\n\n if self.t[s] == 'CLOSED':\n self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[s]))\n\n def get_neighbor(self, s):\n nei_list = set()\n\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n nei_list.add(s_next)\n\n return nei_list\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n def plot_path(self, path):\n px = [x[0] for x in path]\n py = [x[1] for x in path]\n plt.plot(px, py, linewidth=2)\n plt.plot(self.s_start[0], self.s_start[1], \"bs\")\n plt.plot(self.s_goal[0], self.s_goal[1], \"gs\")\n\n def plot_visited(self, visited):\n color = ['gainsboro', 'lightgray', 'silver', 'darkgray',\n 'bisque', 'navajowhite', 'moccasin', 'wheat',\n 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']\n\n if self.count >= len(color) - 1:\n self.count = 0\n\n for x in visited:\n plt.plot(x[0], x[1], marker='s', color=color[self.count])\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n dstar = DStar(s_start, s_goal)\n dstar.run(s_start, s_goal)\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/D_star_Lite.py", "content": "\"\"\"\nD_star_Lite 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass DStar:\n def __init__(self, s_start, s_goal, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n self.Plot = plotting.Plotting(s_start, s_goal)\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n self.x = self.Env.x_range\n self.y = self.Env.y_range\n\n self.g, self.rhs, self.U = {}, {}, {}\n self.km = 0\n\n for i in range(1, self.Env.x_range - 1):\n for j in range(1, self.Env.y_range - 1):\n self.rhs[(i, j)] = float(\"inf\")\n self.g[(i, j)] = float(\"inf\")\n\n self.rhs[self.s_goal] = 0.0\n self.U[self.s_goal] = self.CalculateKey(self.s_goal)\n self.visited = set()\n self.count = 0\n self.fig = plt.figure()\n\n def run(self):\n self.Plot.plot_grid(\"D* Lite\")\n self.ComputePath()\n self.plot_path(self.extract_path())\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n plt.show()\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:\n print(\"Please choose right area!\")\n else:\n x, y = int(x), int(y)\n print(\"Change position: s =\", x, \",\", \"y =\", y)\n\n s_curr = self.s_start\n s_last = self.s_start\n i = 0\n path = [self.s_start]\n\n while s_curr != self.s_goal:\n s_list = {}\n\n for s in self.get_neighbor(s_curr):\n s_list[s] = self.g[s] + self.cost(s_curr, s)\n s_curr = min(s_list, key=s_list.get)\n path.append(s_curr)\n\n if i < 1:\n self.km += self.h(s_last, s_curr)\n s_last = s_curr\n if (x, y) not in self.obs:\n self.obs.add((x, y))\n plt.plot(x, y, 'sk')\n self.g[(x, y)] = float(\"inf\")\n self.rhs[(x, y)] = float(\"inf\")\n else:\n self.obs.remove((x, y))\n plt.plot(x, y, marker='s', color='white')\n self.UpdateVertex((x, y))\n for s in self.get_neighbor((x, y)):\n self.UpdateVertex(s)\n i += 1\n\n self.count += 1\n self.visited = set()\n self.ComputePath()\n\n self.plot_visited(self.visited)\n self.plot_path(path)\n self.fig.canvas.draw_idle()\n\n def ComputePath(self):\n while True:\n s, v = self.TopKey()\n if v >= self.CalculateKey(self.s_start) and \\\n self.rhs[self.s_start] == self.g[self.s_start]:\n break\n\n k_old = v\n self.U.pop(s)\n self.visited.add(s)\n\n if k_old < self.CalculateKey(s):\n self.U[s] = self.CalculateKey(s)\n elif self.g[s] > self.rhs[s]:\n self.g[s] = self.rhs[s]\n for x in self.get_neighbor(s):\n self.UpdateVertex(x)\n else:\n self.g[s] = float(\"inf\")\n self.UpdateVertex(s)\n for x in self.get_neighbor(s):\n self.UpdateVertex(x)\n\n def UpdateVertex(self, s):\n if s != self.s_goal:\n self.rhs[s] = float(\"inf\")\n for x in self.get_neighbor(s):\n self.rhs[s] = min(self.rhs[s], self.g[x] + self.cost(s, x))\n if s in self.U:\n self.U.pop(s)\n\n if self.g[s] != self.rhs[s]:\n self.U[s] = self.CalculateKey(s)\n\n def CalculateKey(self, s):\n return [min(self.g[s], self.rhs[s]) + self.h(self.s_start, s) + self.km,\n min(self.g[s], self.rhs[s])]\n\n def TopKey(self):\n \"\"\"\n :return: return the min key and its value.\n \"\"\"\n\n s = min(self.U, key=self.U.get)\n return s, self.U[s]\n\n def h(self, s_start, s_goal):\n heuristic_type = self.heuristic_type # heuristic type\n\n if heuristic_type == \"manhattan\":\n return abs(s_goal[0] - s_start[0]) + abs(s_goal[1] - s_start[1])\n else:\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n def get_neighbor(self, s):\n nei_list = set()\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n nei_list.add(s_next)\n\n return nei_list\n\n def extract_path(self):\n \"\"\"\n Extract the path based on the PARENT set.\n :return: The planning path\n \"\"\"\n\n path = [self.s_start]\n s = self.s_start\n\n for k in range(100):\n g_list = {}\n for x in self.get_neighbor(s):\n if not self.is_collision(s, x):\n g_list[x] = self.g[x]\n s = min(g_list, key=g_list.get)\n path.append(s)\n if s == self.s_goal:\n break\n\n return list(path)\n\n def plot_path(self, path):\n px = [x[0] for x in path]\n py = [x[1] for x in path]\n plt.plot(px, py, linewidth=2)\n plt.plot(self.s_start[0], self.s_start[1], \"bs\")\n plt.plot(self.s_goal[0], self.s_goal[1], \"gs\")\n\n def plot_visited(self, visited):\n color = ['gainsboro', 'lightgray', 'silver', 'darkgray',\n 'bisque', 'navajowhite', 'moccasin', 'wheat',\n 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']\n\n if self.count >= len(color) - 1:\n self.count = 0\n\n for x in visited:\n plt.plot(x[0], x[1], marker='s', color=color[self.count])\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n dstar = DStar(s_start, s_goal, \"euclidean\")\n dstar.run()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/Dijkstra.py", "content": "\"\"\"\nDijkstra 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\nfrom Search_2D.Astar import AStar\n\n\nclass Dijkstra(AStar):\n \"\"\"Dijkstra set the cost as the priority \n \"\"\"\n def searching(self):\n \"\"\"\n Breadth-first Searching.\n :return: path, visited order\n \"\"\"\n\n self.PARENT[self.s_start] = self.s_start\n self.g[self.s_start] = 0\n self.g[self.s_goal] = math.inf\n heapq.heappush(self.OPEN,\n (0, self.s_start))\n\n while self.OPEN:\n _, s = heapq.heappop(self.OPEN)\n self.CLOSED.append(s)\n\n if s == self.s_goal:\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g:\n self.g[s_n] = math.inf\n\n if new_cost < self.g[s_n]: # conditions for updating Cost\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n\n # best first set the heuristics as the priority \n heapq.heappush(self.OPEN, (new_cost, s_n))\n\n return self.extract_path(self.PARENT), self.CLOSED\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n dijkstra = Dijkstra(s_start, s_goal, 'None')\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = dijkstra.searching()\n plot.animation(path, visited, \"Dijkstra's\") # animation generate\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/LPAstar.py", "content": "\"\"\"\nLPA_star 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport math\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\n\n\nclass LPAStar:\n def __init__(self, s_start, s_goal, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env()\n self.Plot = plotting.Plotting(self.s_start, self.s_goal)\n\n self.u_set = self.Env.motions\n self.obs = self.Env.obs\n self.x = self.Env.x_range\n self.y = self.Env.y_range\n\n self.g, self.rhs, self.U = {}, {}, {}\n\n for i in range(self.Env.x_range):\n for j in range(self.Env.y_range):\n self.rhs[(i, j)] = float(\"inf\")\n self.g[(i, j)] = float(\"inf\")\n\n self.rhs[self.s_start] = 0\n self.U[self.s_start] = self.CalculateKey(self.s_start)\n self.visited = set()\n self.count = 0\n\n self.fig = plt.figure()\n\n def run(self):\n self.Plot.plot_grid(\"Lifelong Planning A*\")\n\n self.ComputeShortestPath()\n self.plot_path(self.extract_path())\n self.fig.canvas.mpl_connect('button_press_event', self.on_press)\n\n plt.show()\n\n def on_press(self, event):\n x, y = event.xdata, event.ydata\n if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:\n print(\"Please choose right area!\")\n else:\n x, y = int(x), int(y)\n print(\"Change position: s =\", x, \",\", \"y =\", y)\n\n self.visited = set()\n self.count += 1\n\n if (x, y) not in self.obs:\n self.obs.add((x, y))\n else:\n self.obs.remove((x, y))\n self.UpdateVertex((x, y))\n\n self.Plot.update_obs(self.obs)\n\n for s_n in self.get_neighbor((x, y)):\n self.UpdateVertex(s_n)\n\n self.ComputeShortestPath()\n\n plt.cla()\n self.Plot.plot_grid(\"Lifelong Planning A*\")\n self.plot_visited(self.visited)\n self.plot_path(self.extract_path())\n self.fig.canvas.draw_idle()\n\n def ComputeShortestPath(self):\n while True:\n s, v = self.TopKey()\n\n if v >= self.CalculateKey(self.s_goal) and \\\n self.rhs[self.s_goal] == self.g[self.s_goal]:\n break\n\n self.U.pop(s)\n self.visited.add(s)\n\n if self.g[s] > self.rhs[s]:\n\n # Condition: over-consistent (eg: deleted obstacles)\n # So, rhs[s] decreased -- > rhs[s] < g[s]\n self.g[s] = self.rhs[s]\n else:\n\n # Condition: # under-consistent (eg: added obstacles)\n # So, rhs[s] increased --> rhs[s] > g[s]\n self.g[s] = float(\"inf\")\n self.UpdateVertex(s)\n\n for s_n in self.get_neighbor(s):\n self.UpdateVertex(s_n)\n\n def UpdateVertex(self, s):\n \"\"\"\n update the status and the current cost to come of state s.\n :param s: state s\n \"\"\"\n\n if s != self.s_start:\n\n # Condition: cost of parent of s changed\n # Since we do not record the children of a state, we need to enumerate its neighbors\n self.rhs[s] = min(self.g[s_n] + self.cost(s_n, s)\n for s_n in self.get_neighbor(s))\n\n if s in self.U:\n self.U.pop(s)\n\n if self.g[s] != self.rhs[s]:\n\n # Condition: current cost to come is different to that of last time\n # state s should be added into OPEN set (set U)\n self.U[s] = self.CalculateKey(s)\n\n def TopKey(self):\n \"\"\"\n :return: return the min key and its value.\n \"\"\"\n\n s = min(self.U, key=self.U.get)\n\n return s, self.U[s]\n\n def CalculateKey(self, s):\n\n return [min(self.g[s], self.rhs[s]) + self.h(s),\n min(self.g[s], self.rhs[s])]\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n s_list = set()\n\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n s_list.add(s_next)\n\n return s_list\n\n def h(self, s):\n \"\"\"\n Calculate heuristic.\n :param s: current node (state)\n :return: heuristic function value\n \"\"\"\n\n heuristic_type = self.heuristic_type # heuristic type\n goal = self.s_goal # goal node\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n def extract_path(self):\n \"\"\"\n Extract the path based on the PARENT set.\n :return: The planning path\n \"\"\"\n\n path = [self.s_goal]\n s = self.s_goal\n\n for k in range(100):\n g_list = {}\n for x in self.get_neighbor(s):\n if not self.is_collision(s, x):\n g_list[x] = self.g[x]\n s = min(g_list, key=g_list.get)\n path.append(s)\n if s == self.s_start:\n break\n\n return list(reversed(path))\n\n def plot_path(self, path):\n px = [x[0] for x in path]\n py = [x[1] for x in path]\n plt.plot(px, py, linewidth=2)\n plt.plot(self.s_start[0], self.s_start[1], \"bs\")\n plt.plot(self.s_goal[0], self.s_goal[1], \"gs\")\n\n def plot_visited(self, visited):\n color = ['gainsboro', 'lightgray', 'silver', 'darkgray',\n 'bisque', 'navajowhite', 'moccasin', 'wheat',\n 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']\n\n if self.count >= len(color) - 1:\n self.count = 0\n\n for x in visited:\n plt.plot(x[0], x[1], marker='s', color=color[self.count])\n\n\ndef main():\n x_start = (5, 5)\n x_goal = (45, 25)\n\n lpastar = LPAStar(x_start, x_goal, \"Euclidean\")\n lpastar.run()\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/LRTAstar.py", "content": "\"\"\"\nLRTA_star 2D (Learning Real-time A*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport copy\nimport math\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import queue, plotting, env\n\n\nclass LrtAStarN:\n def __init__(self, s_start, s_goal, N, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env()\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n\n self.N = N # number of expand nodes each iteration\n self.visited = [] # order of visited nodes in planning\n self.path = [] # path of each iteration\n self.h_table = {} # h_value table\n\n def init(self):\n \"\"\"\n initialize the h_value of all nodes in the environment.\n it is a global table.\n \"\"\"\n\n for i in range(self.Env.x_range):\n for j in range(self.Env.y_range):\n self.h_table[(i, j)] = self.h((i, j))\n\n def searching(self):\n self.init()\n s_start = self.s_start # initialize start node\n\n while True:\n OPEN, CLOSED = self.AStar(s_start, self.N) # OPEN, CLOSED sets in each iteration\n\n if OPEN == \"FOUND\": # reach the goal node\n self.path.append(CLOSED)\n break\n\n h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes\n\n for x in h_value:\n self.h_table[x] = h_value[x]\n\n s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set\n self.path.append(path_k)\n\n def extract_path_in_CLOSE(self, s_start, h_value):\n path = [s_start]\n s = s_start\n\n while True:\n h_list = {}\n\n for s_n in self.get_neighbor(s):\n if s_n in h_value:\n h_list[s_n] = h_value[s_n]\n else:\n h_list[s_n] = self.h_table[s_n]\n\n s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value\n path.append(s_key) # generate path\n s = s_key # use end of this iteration as the start of next\n\n if s_key not in h_value: # reach the expected node in OPEN set\n return s_key, path\n\n def iteration(self, CLOSED):\n h_value = {}\n\n for s in CLOSED:\n h_value[s] = float(\"inf\") # initialize h_value of CLOSED nodes\n\n while True:\n h_value_rec = copy.deepcopy(h_value)\n for s in CLOSED:\n h_list = []\n for s_n in self.get_neighbor(s):\n if s_n not in CLOSED:\n h_list.append(self.cost(s, s_n) + self.h_table[s_n])\n else:\n h_list.append(self.cost(s, s_n) + h_value[s_n])\n h_value[s] = min(h_list) # update h_value of current node\n\n if h_value == h_value_rec: # h_value table converged\n return h_value\n\n def AStar(self, x_start, N):\n OPEN = queue.QueuePrior() # OPEN set\n OPEN.put(x_start, self.h(x_start))\n CLOSED = [] # CLOSED set\n g_table = {x_start: 0, self.s_goal: float(\"inf\")} # Cost to come\n PARENT = {x_start: x_start} # relations\n count = 0 # counter\n\n while not OPEN.empty():\n count += 1\n s = OPEN.get()\n CLOSED.append(s)\n\n if s == self.s_goal: # reach the goal node\n self.visited.append(CLOSED)\n return \"FOUND\", self.extract_path(x_start, PARENT)\n\n for s_n in self.get_neighbor(s):\n if s_n not in CLOSED:\n new_cost = g_table[s] + self.cost(s, s_n)\n if s_n not in g_table:\n g_table[s_n] = float(\"inf\")\n if new_cost < g_table[s_n]: # conditions for updating Cost\n g_table[s_n] = new_cost\n PARENT[s_n] = s\n OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])\n\n if count == N: # expand needed CLOSED nodes\n break\n\n self.visited.append(CLOSED) # visited nodes in each iteration\n\n return OPEN, CLOSED\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n s_list = []\n\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n s_list.append(s_next)\n\n return s_list\n\n def extract_path(self, x_start, parent):\n \"\"\"\n Extract the path based on the relationship of nodes.\n\n :return: The planning path\n \"\"\"\n\n path_back = [self.s_goal]\n x_current = self.s_goal\n\n while True:\n x_current = parent[x_current]\n path_back.append(x_current)\n\n if x_current == x_start:\n break\n\n return list(reversed(path_back))\n\n def h(self, s):\n \"\"\"\n Calculate heuristic.\n :param s: current node (state)\n :return: heuristic function value\n \"\"\"\n\n heuristic_type = self.heuristic_type # heuristic type\n goal = self.s_goal # goal node\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n\ndef main():\n s_start = (10, 5)\n s_goal = (45, 25)\n\n lrta = LrtAStarN(s_start, s_goal, 250, \"euclidean\")\n plot = plotting.Plotting(s_start, s_goal)\n\n lrta.searching()\n plot.animation_lrta(lrta.path, lrta.visited,\n \"Learning Real-time A* (LRTA*)\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/RTAAStar.py", "content": "\"\"\"\nRTAAstar 2D (Real-time Adaptive A*)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport copy\nimport math\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import queue, plotting, env\n\n\nclass RTAAStar:\n def __init__(self, s_start, s_goal, N, heuristic_type):\n self.s_start, self.s_goal = s_start, s_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env()\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n\n self.N = N # number of expand nodes each iteration\n self.visited = [] # order of visited nodes in planning\n self.path = [] # path of each iteration\n self.h_table = {} # h_value table\n\n def init(self):\n \"\"\"\n initialize the h_value of all nodes in the environment.\n it is a global table.\n \"\"\"\n\n for i in range(self.Env.x_range):\n for j in range(self.Env.y_range):\n self.h_table[(i, j)] = self.h((i, j))\n\n def searching(self):\n self.init()\n s_start = self.s_start # initialize start node\n\n while True:\n OPEN, CLOSED, g_table, PARENT = \\\n self.Astar(s_start, self.N)\n\n if OPEN == \"FOUND\": # reach the goal node\n self.path.append(CLOSED)\n break\n\n s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT)\n\n for x in h_value:\n self.h_table[x] = h_value[x]\n\n s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value)\n self.path.append(path_k)\n\n def cal_h_value(self, OPEN, CLOSED, g_table, PARENT):\n v_open = {}\n h_value = {}\n for (_, x) in OPEN.enumerate():\n v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x]\n s_open = min(v_open, key=v_open.get)\n f_min = v_open[s_open]\n for x in CLOSED:\n h_value[x] = f_min - g_table[x]\n\n return s_open, h_value\n\n def iteration(self, CLOSED):\n h_value = {}\n\n for s in CLOSED:\n h_value[s] = float(\"inf\") # initialize h_value of CLOSED nodes\n\n while True:\n h_value_rec = copy.deepcopy(h_value)\n for s in CLOSED:\n h_list = []\n for s_n in self.get_neighbor(s):\n if s_n not in CLOSED:\n h_list.append(self.cost(s, s_n) + self.h_table[s_n])\n else:\n h_list.append(self.cost(s, s_n) + h_value[s_n])\n h_value[s] = min(h_list) # update h_value of current node\n\n if h_value == h_value_rec: # h_value table converged\n return h_value\n\n def Astar(self, x_start, N):\n OPEN = queue.QueuePrior() # OPEN set\n OPEN.put(x_start, self.h_table[x_start])\n CLOSED = [] # CLOSED set\n g_table = {x_start: 0, self.s_goal: float(\"inf\")} # Cost to come\n PARENT = {x_start: x_start} # relations\n count = 0 # counter\n\n while not OPEN.empty():\n count += 1\n s = OPEN.get()\n CLOSED.append(s)\n\n if s == self.s_goal: # reach the goal node\n self.visited.append(CLOSED)\n return \"FOUND\", self.extract_path(x_start, PARENT), [], []\n\n for s_n in self.get_neighbor(s):\n if s_n not in CLOSED:\n new_cost = g_table[s] + self.cost(s, s_n)\n if s_n not in g_table:\n g_table[s_n] = float(\"inf\")\n if new_cost < g_table[s_n]: # conditions for updating Cost\n g_table[s_n] = new_cost\n PARENT[s_n] = s\n OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])\n\n if count == N: # expand needed CLOSED nodes\n break\n\n self.visited.append(CLOSED) # visited nodes in each iteration\n\n return OPEN, CLOSED, g_table, PARENT\n\n def get_neighbor(self, s):\n \"\"\"\n find neighbors of state s that not in obstacles.\n :param s: state\n :return: neighbors\n \"\"\"\n\n s_list = set()\n\n for u in self.u_set:\n s_next = tuple([s[i] + u[i] for i in range(2)])\n if s_next not in self.obs:\n s_list.add(s_next)\n\n return s_list\n\n def extract_path_in_CLOSE(self, s_end, s_start, h_value):\n path = [s_start]\n s = s_start\n\n while True:\n h_list = {}\n for s_n in self.get_neighbor(s):\n if s_n in h_value:\n h_list[s_n] = h_value[s_n]\n s_key = max(h_list, key=h_list.get) # move to the smallest node with min h_value\n path.append(s_key) # generate path\n s = s_key # use end of this iteration as the start of next\n\n if s_key == s_end: # reach the expected node in OPEN set\n return s_start, list(reversed(path))\n\n def extract_path(self, x_start, parent):\n \"\"\"\n Extract the path based on the relationship of nodes.\n :return: The planning path\n \"\"\"\n\n path = [self.s_goal]\n s = self.s_goal\n\n while True:\n s = parent[s]\n path.append(s)\n if s == x_start:\n break\n\n return list(reversed(path))\n\n def h(self, s):\n \"\"\"\n Calculate heuristic.\n :param s: current node (state)\n :return: heuristic function value\n \"\"\"\n\n heuristic_type = self.heuristic_type # heuristic type\n goal = self.s_goal # goal node\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - s[0]) + abs(goal[1] - s[1])\n else:\n return math.hypot(goal[0] - s[0], goal[1] - s[1])\n\n def cost(self, s_start, s_goal):\n \"\"\"\n Calculate Cost for this motion\n :param s_start: starting node\n :param s_goal: end node\n :return: Cost for this motion\n :note: Cost function could be more complicate!\n \"\"\"\n\n if self.is_collision(s_start, s_goal):\n return float(\"inf\")\n\n return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])\n\n def is_collision(self, s_start, s_end):\n if s_start in self.obs or s_end in self.obs:\n return True\n\n if s_start[0] != s_end[0] and s_start[1] != s_end[1]:\n if s_end[0] - s_start[0] == s_start[1] - s_end[1]:\n s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n else:\n s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))\n s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))\n\n if s1 in self.obs or s2 in self.obs:\n return True\n\n return False\n\n\ndef main():\n s_start = (10, 5)\n s_goal = (45, 25)\n\n rtaa = RTAAStar(s_start, s_goal, 240, \"euclidean\")\n plot = plotting.Plotting(s_start, s_goal)\n\n rtaa.searching()\n plot.animation_lrta(rtaa.path, rtaa.visited,\n \"Real-time Adaptive A* (RTAA*)\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/bfs.py", "content": "\"\"\"\nBreadth-first Searching_2D (BFS)\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nfrom collections import deque\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\nfrom Search_2D.Astar import AStar\nimport math\nimport heapq\n\nclass BFS(AStar):\n \"\"\"BFS add the new visited node in the end of the openset\n \"\"\"\n def searching(self):\n \"\"\"\n Breadth-first Searching.\n :return: path, visited order\n \"\"\"\n\n self.PARENT[self.s_start] = self.s_start\n self.g[self.s_start] = 0\n self.g[self.s_goal] = math.inf\n heapq.heappush(self.OPEN,\n (0, self.s_start))\n\n while self.OPEN:\n _, s = heapq.heappop(self.OPEN)\n self.CLOSED.append(s)\n\n if s == self.s_goal:\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g:\n self.g[s_n] = math.inf\n\n if new_cost < self.g[s_n]: # conditions for updating Cost\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n\n # bfs, add new node to the end of the openset\n prior = self.OPEN[-1][0]+1 if len(self.OPEN)>0 else 0\n heapq.heappush(self.OPEN, (prior, s_n))\n\n return self.extract_path(self.PARENT), self.CLOSED\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n bfs = BFS(s_start, s_goal, 'None')\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = bfs.searching()\n plot.animation(path, visited, \"Breadth-first Searching (BFS)\")\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/dfs.py", "content": "\nimport os\nimport sys\nimport math\nimport heapq\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import plotting, env\nfrom Search_2D.Astar import AStar\n\nclass DFS(AStar):\n \"\"\"DFS add the new visited node in the front of the openset\n \"\"\"\n def searching(self):\n \"\"\"\n Breadth-first Searching.\n :return: path, visited order\n \"\"\"\n\n self.PARENT[self.s_start] = self.s_start\n self.g[self.s_start] = 0\n self.g[self.s_goal] = math.inf\n heapq.heappush(self.OPEN,\n (0, self.s_start))\n\n while self.OPEN:\n _, s = heapq.heappop(self.OPEN)\n self.CLOSED.append(s)\n\n if s == self.s_goal:\n break\n\n for s_n in self.get_neighbor(s):\n new_cost = self.g[s] + self.cost(s, s_n)\n\n if s_n not in self.g:\n self.g[s_n] = math.inf\n\n if new_cost < self.g[s_n]: # conditions for updating Cost\n self.g[s_n] = new_cost\n self.PARENT[s_n] = s\n\n # dfs, add new node to the front of the openset\n prior = self.OPEN[0][0]-1 if len(self.OPEN)>0 else 0\n heapq.heappush(self.OPEN, (prior, s_n))\n\n return self.extract_path(self.PARENT), self.CLOSED\n\n\ndef main():\n s_start = (5, 5)\n s_goal = (45, 25)\n\n dfs = DFS(s_start, s_goal, 'None')\n plot = plotting.Plotting(s_start, s_goal)\n\n path, visited = dfs.searching()\n visited = list(dict.fromkeys(visited))\n plot.animation(path, visited, \"Depth-first Searching (DFS)\") # animation\n\n\nif __name__ == '__main__':\n main()\n" }, { "path": "Search_based_Planning/Search_2D/env.py", "content": "\"\"\"\nEnv 2D\n@author: huiming zhou\n\"\"\"\n\n\nclass Env:\n def __init__(self):\n self.x_range = 51 # size of background\n self.y_range = 31\n self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),\n (1, 0), (1, -1), (0, -1), (-1, -1)]\n self.obs = self.obs_map()\n\n def update_obs(self, obs):\n self.obs = obs\n\n def obs_map(self):\n \"\"\"\n Initialize obstacles' positions\n :return: map of obstacles\n \"\"\"\n\n x = self.x_range\n y = self.y_range\n obs = set()\n\n for i in range(x):\n obs.add((i, 0))\n for i in range(x):\n obs.add((i, y - 1))\n\n for i in range(y):\n obs.add((0, i))\n for i in range(y):\n obs.add((x - 1, i))\n\n for i in range(10, 21):\n obs.add((i, 15))\n for i in range(15):\n obs.add((20, i))\n\n for i in range(15, 30):\n obs.add((30, i))\n for i in range(16):\n obs.add((40, i))\n\n return obs\n" }, { "path": "Search_based_Planning/Search_2D/plotting.py", "content": "\"\"\"\nPlot tools 2D\n@author: huiming zhou\n\"\"\"\n\nimport os\nimport sys\nimport matplotlib.pyplot as plt\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) +\n \"/../../Search_based_Planning/\")\n\nfrom Search_2D import env\n\n\nclass Plotting:\n def __init__(self, xI, xG):\n self.xI, self.xG = xI, xG\n self.env = env.Env()\n self.obs = self.env.obs_map()\n\n def update_obs(self, obs):\n self.obs = obs\n\n def animation(self, path, visited, name):\n self.plot_grid(name)\n self.plot_visited(visited)\n self.plot_path(path)\n plt.show()\n\n def animation_lrta(self, path, visited, name):\n self.plot_grid(name)\n cl = self.color_list_2()\n path_combine = []\n\n for k in range(len(path)):\n self.plot_visited(visited[k], cl[k])\n plt.pause(0.2)\n self.plot_path(path[k])\n path_combine += path[k]\n plt.pause(0.2)\n if self.xI in path_combine:\n path_combine.remove(self.xI)\n self.plot_path(path_combine)\n plt.show()\n\n def animation_ara_star(self, path, visited, name):\n self.plot_grid(name)\n cl_v, cl_p = self.color_list()\n\n for k in range(len(path)):\n self.plot_visited(visited[k], cl_v[k])\n self.plot_path(path[k], cl_p[k], True)\n plt.pause(0.5)\n\n plt.show()\n\n def animation_bi_astar(self, path, v_fore, v_back, name):\n self.plot_grid(name)\n self.plot_visited_bi(v_fore, v_back)\n self.plot_path(path)\n plt.show()\n\n def plot_grid(self, name):\n obs_x = [x[0] for x in self.obs]\n obs_y = [x[1] for x in self.obs]\n\n plt.plot(self.xI[0], self.xI[1], \"bs\")\n plt.plot(self.xG[0], self.xG[1], \"gs\")\n plt.plot(obs_x, obs_y, \"sk\")\n plt.title(name)\n plt.axis(\"equal\")\n\n def plot_visited(self, visited, cl='gray'):\n if self.xI in visited:\n visited.remove(self.xI)\n\n if self.xG in visited:\n visited.remove(self.xG)\n\n count = 0\n\n for x in visited:\n count += 1\n plt.plot(x[0], x[1], color=cl, marker='o')\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n if count < len(visited) / 3:\n length = 20\n elif count < len(visited) * 2 / 3:\n length = 30\n else:\n length = 40\n #\n # length = 15\n\n if count % length == 0:\n plt.pause(0.001)\n plt.pause(0.01)\n\n def plot_path(self, path, cl='r', flag=False):\n path_x = [path[i][0] for i in range(len(path))]\n path_y = [path[i][1] for i in range(len(path))]\n\n if not flag:\n plt.plot(path_x, path_y, linewidth='3', color='r')\n else:\n plt.plot(path_x, path_y, linewidth='3', color=cl)\n\n plt.plot(self.xI[0], self.xI[1], \"bs\")\n plt.plot(self.xG[0], self.xG[1], \"gs\")\n\n plt.pause(0.01)\n\n def plot_visited_bi(self, v_fore, v_back):\n if self.xI in v_fore:\n v_fore.remove(self.xI)\n\n if self.xG in v_back:\n v_back.remove(self.xG)\n\n len_fore, len_back = len(v_fore), len(v_back)\n\n for k in range(max(len_fore, len_back)):\n if k < len_fore:\n plt.plot(v_fore[k][0], v_fore[k][1], linewidth='3', color='gray', marker='o')\n if k < len_back:\n plt.plot(v_back[k][0], v_back[k][1], linewidth='3', color='cornflowerblue', marker='o')\n\n plt.gcf().canvas.mpl_connect('key_release_event',\n lambda event: [exit(0) if event.key == 'escape' else None])\n\n if k % 10 == 0:\n plt.pause(0.001)\n plt.pause(0.01)\n\n @staticmethod\n def color_list():\n cl_v = ['silver',\n 'wheat',\n 'lightskyblue',\n 'royalblue',\n 'slategray']\n cl_p = ['gray',\n 'orange',\n 'deepskyblue',\n 'red',\n 'm']\n return cl_v, cl_p\n\n @staticmethod\n def color_list_2():\n cl = ['silver',\n 'steelblue',\n 'dimgray',\n 'cornflowerblue',\n 'dodgerblue',\n 'royalblue',\n 'plum',\n 'mediumslateblue',\n 'mediumpurple',\n 'blueviolet',\n ]\n return cl\n" }, { "path": "Search_based_Planning/Search_2D/queue.py", "content": "import collections\nimport heapq\n\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFIFO is designed for First-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.popleft() # leave from front\n\n\nclass QueueLIFO:\n \"\"\"\n Class: QueueLIFO\n Description: QueueLIFO is designed for Last-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.pop() # leave from back\n\n\nclass QueuePrior:\n \"\"\"\n Class: QueuePrior\n Description: QueuePrior reorders elements using value [priority]\n \"\"\"\n\n def __init__(self):\n self.queue = []\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, item, priority):\n heapq.heappush(self.queue, (priority, item)) # reorder s using priority\n\n def get(self):\n return heapq.heappop(self.queue)[1] # pop out the smallest item\n\n def enumerate(self):\n return self.queue\n" }, { "path": "Search_based_Planning/Search_3D/Anytime_Dstar3D.py", "content": "# check paper of \n# [Likhachev2005]\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collections import defaultdict\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, isinobb, \\\n cost, children, StateSpace\nfrom Search_3D.plot_util3D import visualization\nfrom Search_3D import queue\nimport time\n\n\nclass Anytime_Dstar(object):\n\n def __init__(self, resolution=1):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1): np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.env = env(resolution=resolution)\n self.settings = 'CollisionChecking' # for collision checking\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.OPEN = queue.MinheapPQ()\n self.g = {} # all g initialized at inf\n self.h = {}\n self.rhs = {self.xt: 0} # rhs(x0) = 0\n self.OPEN.put(self.xt, self.key(self.xt))\n self.INCONS = set()\n self.CLOSED = set()\n\n # init children set:\n self.CHILDREN = {}\n # init Cost set\n self.COST = defaultdict(lambda: defaultdict(dict))\n\n # for visualization\n self.V = set() # vertice in closed\n self.ind = 0\n self.Path = []\n self.done = False\n\n # epsilon in the key caculation\n self.epsilon = 1\n self.increment = 0.1\n self.decrement = 0.2\n\n def getcost(self, xi, xj):\n # use a LUT for getting the costd\n if xi not in self.COST:\n for (xj, xjcost) in children(self, xi, settings=1):\n self.COST[xi][xj] = cost(self, xi, xj, xjcost)\n # this might happen when there is a node changed. \n if xj not in self.COST[xi]:\n self.COST[xi][xj] = cost(self, xi, xj)\n return self.COST[xi][xj]\n\n def getchildren(self, xi):\n if xi not in self.CHILDREN:\n allchild = children(self, xi)\n self.CHILDREN[xi] = set(allchild)\n return self.CHILDREN[xi]\n\n def geth(self, xi):\n # when the heurisitic is first calculated\n if xi not in self.h:\n self.h[xi] = heuristic_fun(self, xi, self.x0)\n return self.h[xi]\n\n def getg(self, xi):\n if xi not in self.g:\n self.g[xi] = np.inf\n return self.g[xi]\n\n def getrhs(self, xi):\n if xi not in self.rhs:\n self.rhs[xi] = np.inf\n return self.rhs[xi]\n\n def updatecost(self, range_changed=None, new=None, old=None, mode=False):\n # scan graph for changed Cost, if Cost is changed update it\n CHANGED = set()\n for xi in self.CLOSED:\n if self.isinobs(old, xi, mode) or self.isinobs(new, xi, mode):\n # if self.isinobs(new, xi, mode):\n self.V.remove(xi)\n # self.V.difference_update({i for i in children(self, xi)})\n newchildren = set(children(self, xi)) # B\n self.CHILDREN[xi] = newchildren\n for xj in newchildren:\n self.COST[xi][xj] = cost(self, xi, xj)\n CHANGED.add(xi)\n return CHANGED\n\n def isinobs(self, obs, x, mode):\n if mode == 'obb':\n return isinobb(obs, x)\n elif mode == 'aabb':\n return isinbound(obs, x, mode)\n\n # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False):\n # # TODO scan graph for changed Cost, if Cost is changed update it\n # # make the graph Cost via vectorization\n # CHANGED = set()\n # Allnodes = np.array(list(self.CLOSED))\n # isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \\\n # isinbound(new, Allnodes, mode = mode, isarray = True)\n # Changednodes = Allnodes[isChanged]\n # for xi in Changednodes:\n # xi = tuple(xi)\n # CHANGED.add(xi)\n # self.CHILDREN[xi] = set(children(self, xi))\n # for xj in self.CHILDREN:\n # self.COST[xi][xj] = Cost(self, xi, xj)\n \n\n # --------------main functions for Anytime D star\n\n def key(self, s, epsilon=1):\n if self.getg(s) > self.getrhs(s):\n return [self.rhs[s] + epsilon * heuristic_fun(self, s, self.x0), self.rhs[s]]\n else:\n return [self.getg(s) + heuristic_fun(self, s, self.x0), self.getg(s)]\n\n def UpdateState(self, s):\n if s not in self.CLOSED:\n # TODO if s is not visited before\n self.g[s] = np.inf\n if s != self.xt:\n self.rhs[s] = min([self.getcost(s, s_p) + self.getg(s_p) for s_p in self.getchildren(s)])\n self.OPEN.check_remove(s)\n if self.getg(s) != self.getrhs(s):\n if s not in self.CLOSED:\n self.OPEN.put(s, self.key(s))\n else:\n self.INCONS.add(s)\n\n def ComputeorImprovePath(self):\n while self.OPEN.top_key() < self.key(self.x0, self.epsilon) or self.rhs[self.x0] != self.g[self.x0]:\n s = self.OPEN.get()\n\n if getDist(s, tuple(self.env.start)) < self.env.resolution:\n break\n\n if self.g[s] > self.rhs[s]:\n self.g[s] = self.rhs[s]\n self.CLOSED.add(s)\n self.V.add(s)\n for s_p in self.getchildren(s):\n self.UpdateState(s_p)\n else:\n self.g[s] = np.inf\n self.UpdateState(s)\n for s_p in self.getchildren(s):\n self.UpdateState(s_p)\n self.ind += 1\n\n def Main(self):\n ischanged = False\n islargelychanged = False\n t = 0\n self.ComputeorImprovePath()\n # TODO publish current epsilon sub-optimal solution\n self.done = True\n self.ind = 0\n self.Path = self.path()\n visualization(self)\n while True:\n visualization(self)\n if t == 20:\n break\n # change environment\n # new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation')\n # new2, old2 = self.env.move_block(a=[0, 0, -0.2], mode='translation')\n new2, old2 = self.env.move_OBB(theta=[10*t, 0, 0], translation=[0, 0.1*t, 0])\n mmode = 'obb' # obb or aabb\n ischanged = True\n # islargelychanged = True\n self.Path = []\n\n # update Cost with changed environment\n if ischanged:\n # CHANGED = self.updatecost(True, new2, old2, mode='obb')\n CHANGED = self.updatecost(True, new2, old2, mode=mmode)\n for u in CHANGED:\n self.UpdateState(u)\n self.ComputeorImprovePath()\n ischanged = False\n\n if islargelychanged:\n self.epsilon += self.increment # or replan from scratch\n elif self.epsilon > 1:\n self.epsilon -= self.decrement\n\n # move states from the INCONS to OPEN\n # update priorities in OPEN\n Allnodes = self.INCONS.union(self.OPEN.allnodes())\n for node in Allnodes:\n self.OPEN.put(node, self.key(node, self.epsilon))\n self.INCONS = set()\n self.CLOSED = set()\n self.ComputeorImprovePath()\n # publish current epsilon sub optimal solution\n self.Path = self.path()\n # if epsilon == 1:\n # wait for change to occur\n t += 1\n\n def path(self, s_start=None):\n '''After ComputeShortestPath()\n returns, one can then follow a shortest path from x_init to\n x_goal by always moving from the current vertex s, starting\n at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')\n until x_goal is reached (ties can be broken arbitrarily).'''\n path = []\n s_goal = self.xt\n s = self.x0\n ind = 0\n while getDist(s, s_goal) > self.env.resolution:\n if s == self.x0:\n children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution * np.sqrt(3)]\n else:\n children = list(self.CHILDREN[s])\n snext = children[np.argmin([self.getcost(s, s_p) + self.getg(s_p) for s_p in children])]\n path.append([s, snext])\n s = snext\n if ind > 100:\n break\n ind += 1\n return path\n\n\nif __name__ == '__main__':\n AD = Anytime_Dstar(resolution=1)\n AD.Main()\n" }, { "path": "Search_based_Planning/Search_3D/Astar3D.py", "content": "# this is the three dimensional A* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \\\n cost, children, StateSpace, heuristic_fun\nfrom Search_3D.plot_util3D import visualization\nimport queue\nimport time\n\nclass Weighted_A_star(object):\n def __init__(self, resolution=0.5):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.settings = 'NonCollisionChecking' # 'NonCollisionChecking' or 'CollisionChecking' \n self.env = env(resolution=resolution)\n self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)\n self.g = {self.start:0,self.goal:np.inf}\n self.Parent = {}\n self.CLOSED = set()\n self.V = []\n self.done = False\n self.Path = []\n self.ind = 0\n self.x0, self.xt = self.start, self.goal\n self.OPEN = queue.MinheapPQ() # store [point,priority]\n self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h\n self.lastpoint = self.x0\n\n def run(self, N=None):\n xt = self.xt\n xi = self.x0\n while self.OPEN: # while xt not reached and open is not empty\n xi = self.OPEN.get()\n if xi not in self.CLOSED:\n self.V.append(np.array(xi))\n self.CLOSED.add(xi) # add the point in CLOSED set\n if getDist(xi,xt) < self.env.resolution:\n break\n # visualization(self)\n for xj in children(self,xi):\n # if xj not in self.CLOSED:\n if xj not in self.g:\n self.g[xj] = np.inf\n else:\n pass\n a = self.g[xi] + cost(self, xi, xj)\n if a < self.g[xj]:\n self.g[xj] = a\n self.Parent[xj] = xi\n # assign or update the priority in the open\n self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj))\n # For specified expanded nodes, used primarily in LRTA*\n if N:\n if len(self.CLOSED) % N == 0:\n break\n if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V)))\n self.ind += 1\n\n self.lastpoint = xi\n # if the path finding is finished\n if self.lastpoint in self.CLOSED:\n self.done = True\n self.Path = self.path()\n if N is None:\n visualization(self)\n plt.show()\n return True\n\n return False\n\n def path(self):\n path = []\n x = self.lastpoint\n start = self.x0\n while x != start:\n path.append([x, self.Parent[x]])\n x = self.Parent[x]\n # path = np.flip(path, axis=0)\n return path\n\n # utility used in LRTA*\n def reset(self, xj):\n self.g = g_Space(self) # key is the point, store g value\n self.start = xj\n self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0\n self.x0 = xj\n self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h\n self.CLOSED = set()\n\n # self.h = h(self.Space, self.goal)\n\n\nif __name__ == '__main__':\n \n Astar = Weighted_A_star(0.5)\n sta = time.time()\n Astar.run()\n print(time.time() - sta)" }, { "path": "Search_based_Planning/Search_3D/Dstar3D.py", "content": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collections import defaultdict\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D import Astar3D\nfrom Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \\\n initcost\nfrom Search_3D.plot_util3D import visualization\n\n\nclass D_star(object):\n def __init__(self, resolution=1):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.settings = 'CollisionChecking'\n self.env = env(resolution=resolution)\n self.X = StateSpace(self.env)\n self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)\n # self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n self.b = defaultdict(lambda: defaultdict(dict)) # back pointers every state has one except xt.\n self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value\n self.h = {} # estimate from a point to the end point\n self.tag = {} # set all states to new\n self.V = set() # vertice in closed\n # for visualization\n self.ind = 0\n self.Path = []\n self.done = False\n self.Obstaclemap = {}\n\n def checkState(self, y):\n if y not in self.h:\n self.h[y] = 0\n if y not in self.tag:\n self.tag[y] = 'New'\n\n def get_kmin(self):\n # get the minimum of the k val in OPEN\n # -1 if it does not exist\n if self.OPEN:\n return min(self.OPEN.values())\n return -1\n\n def min_state(self):\n # returns the state in OPEN with min k(.)\n # if empty, returns None and -1\n # it also removes this min value form the OPEN set.\n if self.OPEN:\n minvalue = min(self.OPEN.values())\n for k in self.OPEN.keys():\n if self.OPEN[k] == minvalue:\n return k, self.OPEN.pop(k)\n return None, -1\n\n def insert(self, x, h_new):\n # inserting a key and value into OPEN list (s, kx)\n # depending on following situations\n if self.tag[x] == 'New':\n kx = h_new\n if self.tag[x] == 'Open':\n kx = min(self.OPEN[x], h_new)\n if self.tag[x] == 'Closed':\n kx = min(self.h[x], h_new)\n self.OPEN[x] = kx\n self.h[x], self.tag[x] = h_new, 'Open'\n\n def process_state(self):\n # main function of the D star algorithm, perform the process state \n # around the old path when needed.\n x, kold = self.min_state()\n self.tag[x] = 'Closed'\n self.V.add(x)\n if x is None:\n return -1\n # check if 1st timer s\n self.checkState(x)\n if kold < self.h[x]: # raised states\n for y in children(self, x):\n # check y\n self.checkState(y)\n a = self.h[y] + cost(self, y, x)\n if self.h[y] <= kold and self.h[x] > a:\n self.b[x], self.h[x] = y, a\n if kold == self.h[x]: # lower\n for y in children(self, x):\n # check y\n self.checkState(y)\n bb = self.h[x] + cost(self, x, y)\n if self.tag[y] == 'New' or \\\n (self.b[y] == x and self.h[y] != bb) or \\\n (self.b[y] != x and self.h[y] > bb):\n self.b[y] = x\n self.insert(y, bb)\n else:\n for y in children(self, x):\n # check y\n self.checkState(y)\n bb = self.h[x] + cost(self, x, y)\n if self.tag[y] == 'New' or \\\n (self.b[y] == x and self.h[y] != bb):\n self.b[y] = x\n self.insert(y, bb)\n else:\n if self.b[y] != x and self.h[y] > bb:\n self.insert(x, self.h[x])\n else:\n if self.b[y] != x and self.h[y] > bb and \\\n self.tag[y] == 'Closed' and self.h[y] == kold:\n self.insert(y, self.h[y])\n return self.get_kmin()\n\n def modify_cost(self, x):\n xparent = self.b[x]\n if self.tag[x] == 'Closed':\n self.insert(x, self.h[xparent] + cost(self, x, xparent))\n\n def modify(self, x):\n self.modify_cost(x)\n while True:\n kmin = self.process_state()\n # visualization(self)\n if kmin >= self.h[x]:\n break\n\n def path(self, goal=None):\n path = []\n if not goal:\n x = self.x0\n else:\n x = goal\n start = self.xt\n while x != start:\n path.append([np.array(x), np.array(self.b[x])])\n x = self.b[x]\n return path\n\n def run(self):\n # put G (ending state) into the OPEN list\n self.OPEN[self.xt] = 0\n self.tag[self.x0] = 'New'\n # first run\n while True:\n # TODO: self.x0 =\n self.process_state()\n # visualization(self)\n if self.tag[self.x0] == \"Closed\":\n break\n self.ind += 1\n self.Path = self.path()\n self.done = True\n visualization(self)\n plt.pause(0.2)\n # plt.show()\n # when the environemnt changes over time\n\n for i in range(5):\n self.env.move_block(a=[0, -0.50, 0], s=0.5, block_to_move=1, mode='translation')\n self.env.move_block(a=[-0.25, 0, 0], s=0.5, block_to_move=0, mode='translation')\n # travel from end to start\n s = tuple(self.env.start)\n # self.V = set()\n while s != self.xt:\n if s == tuple(self.env.start):\n sparent = self.b[self.x0]\n else:\n sparent = self.b[s]\n # if there is a change of Cost, or a collision.\n if cost(self, s, sparent) == np.inf:\n self.modify(s)\n continue\n self.ind += 1\n s = sparent\n self.Path = self.path()\n visualization(self)\n plt.show()\n\n\nif __name__ == '__main__':\n D = D_star(1)\n D.run()\n" }, { "path": "Search_based_Planning/Search_3D/DstarLite3D.py", "content": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\nfrom collections import defaultdict\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \\\n cost, children, StateSpace\nfrom Search_3D.plot_util3D import visualization\nfrom Search_3D import queue\nimport time\n\nclass D_star_Lite(object):\n # Original version of the D*lite\n def __init__(self, resolution = 1):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.env = env(resolution=resolution)\n #self.X = StateSpace(self.env)\n #self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)\n self.settings = 'CollisionChecking' # for collision checking\n self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)\n # self.OPEN = queue.QueuePrior()\n self.OPEN = queue.MinheapPQ()\n self.km = 0\n self.g = {} # all g initialized at inf\n self.rhs = {self.xt:0} # rhs(x0) = 0\n self.h = {}\n self.OPEN.put(self.xt, self.CalculateKey(self.xt))\n self.CLOSED = set()\n \n # init children set:\n self.CHILDREN = {}\n # init Cost set\n self.COST = defaultdict(lambda: defaultdict(dict))\n \n # for visualization\n self.V = set() # vertice in closed\n self.ind = 0\n self.Path = []\n self.done = False\n\n def updatecost(self, range_changed=None, new=None, old=None, mode=False):\n # scan graph for changed Cost, if Cost is changed update it\n CHANGED = set()\n for xi in self.CLOSED:\n if isinbound(old, xi, mode) or isinbound(new, xi, mode):\n newchildren = set(children(self, xi)) # B\n self.CHILDREN[xi] = newchildren\n for xj in newchildren:\n self.COST[xi][xj] = cost(self, xi, xj)\n CHANGED.add(xi)\n return CHANGED\n\n def getcost(self, xi, xj):\n # use a LUT for getting the costd\n if xi not in self.COST:\n for (xj,xjcost) in children(self, xi, settings=1):\n self.COST[xi][xj] = cost(self, xi, xj, xjcost)\n # this might happen when there is a node changed. \n if xj not in self.COST[xi]:\n self.COST[xi][xj] = cost(self, xi, xj)\n return self.COST[xi][xj]\n\n def getchildren(self, xi):\n if xi not in self.CHILDREN:\n allchild = children(self, xi)\n self.CHILDREN[xi] = set(allchild)\n return self.CHILDREN[xi]\n\n def geth(self, xi):\n # when the heurisitic is first calculated\n if xi not in self.h:\n self.h[xi] = heuristic_fun(self, xi, self.x0)\n return self.h[xi]\n\n def getg(self, xi):\n if xi not in self.g:\n self.g[xi] = np.inf\n return self.g[xi]\n\n def getrhs(self, xi):\n if xi not in self.rhs:\n self.rhs[xi] = np.inf\n return self.rhs[xi]\n#-------------main functions for D*Lite-------------\n\n def CalculateKey(self, s, epsilion = 1):\n return [min(self.getg(s), self.getrhs(s)) + epsilion * self.geth(s) + self.km, min(self.getg(s), self.getrhs(s))]\n\n def UpdateVertex(self, u):\n # if still in the hunt\n if not getDist(self.xt, u) <= self.env.resolution: # originally: u != x_goal\n if u in self.CHILDREN and len(self.CHILDREN[u]) == 0:\n self.rhs[u] = np.inf\n else:\n self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)])\n # if u is in OPEN, remove it\n self.OPEN.check_remove(u)\n # if rhs(u) not equal to g(u)\n if self.getg(u) != self.getrhs(u):\n self.OPEN.put(u, self.CalculateKey(u))\n \n def ComputeShortestPath(self):\n while self.OPEN.top_key() < self.CalculateKey(self.x0) or self.getrhs(self.x0) != self.getg(self.x0) :\n kold = self.OPEN.top_key()\n u = self.OPEN.get()\n self.V.add(u)\n self.CLOSED.add(u)\n if not self.done: # first time running, we need to stop on this condition\n if getDist(self.x0,u) < 1*self.env.resolution:\n self.x0 = u\n break\n if kold < self.CalculateKey(u):\n self.OPEN.put(u, self.CalculateKey(u))\n if self.getg(u) > self.getrhs(u):\n self.g[u] = self.rhs[u]\n else:\n self.g[u] = np.inf\n self.UpdateVertex(u)\n for s in self.getchildren(u):\n self.UpdateVertex(s)\n # visualization(self)\n self.ind += 1\n\n def main(self):\n s_last = self.x0\n print('first run ...')\n self.ComputeShortestPath()\n self.Path = self.path()\n self.done = True\n visualization(self)\n plt.pause(0.5)\n # plt.show()\n print('running with map update ...')\n t = 0 # count time\n ischanged = False\n self.V = set()\n while getDist(self.x0, self.xt) > 2*self.env.resolution:\n #---------------------------------- at specific times, the environment is changed and Cost is updated\n if t % 2 == 0: \n new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation')\n new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation')\n new2,old2 = self.env.move_OBB(theta = [0,0.1*t,0])\n #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation')\n ischanged = True\n self.Path = []\n #----------------------------------- traverse the route as originally planned\n if t == 0:\n children_new = [i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution*np.sqrt(3)]\n else:\n children_new = list(children(self,self.x0))\n self.x0 = children_new[np.argmin([self.getcost(self.x0,s_p) + self.getg(s_p) for s_p in children_new])]\n # TODO add the moving robot position codes\n self.env.start = self.x0\n # ---------------------------------- if any Cost changed, update km, reset slast,\n # for all directed edgees (u,v) with chaged edge costs, \n # update the edge Cost cBest(u,v) and update vertex u. then replan\n if ischanged:\n self.km += heuristic_fun(self, self.x0, s_last)\n s_last = self.x0\n CHANGED = self.updatecost(True, new0, old0)\n CHANGED1 = self.updatecost(True, new1, old1)\n CHANGED2 = self.updatecost(True, new2, old2, mode='obb')\n CHANGED = CHANGED.union(CHANGED1, CHANGED2)\n # self.V = set()\n for u in CHANGED:\n self.UpdateVertex(u)\n self.ComputeShortestPath()\n \n ischanged = False\n self.Path = self.path(self.x0)\n visualization(self)\n t += 1\n plt.show()\n\n def path(self, s_start=None):\n '''After ComputeShortestPath()\n returns, one can then follow a shortest path from x_init to\n x_goal by always moving from the current vertex s, starting\n at x_init. , to any successor s' that minimizes cBest(s,s') + g(s')\n until x_goal is reached (ties can be broken arbitrarily).'''\n path = []\n s_goal = self.xt\n if not s_start:\n s = self.x0\n else:\n s= s_start\n ind = 0\n while s != s_goal:\n if s == self.x0:\n children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)]\n else: \n children = list(self.CHILDREN[s])\n snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])]\n path.append([s, snext])\n s = snext\n if ind > 100:\n break\n ind += 1\n return path\n\nif __name__ == '__main__':\n \n D_lite = D_star_Lite(1)\n a = time.time()\n D_lite.main()\n print('used time (s) is ' + str(time.time() - a))\n " }, { "path": "Search_based_Planning/Search_3D/LP_Astar3D.py", "content": "import numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D import Astar3D\nfrom Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \\\n cost, obstacleFree, isCollide\nfrom Search_3D.plot_util3D import visualization\nimport queue\nimport pyrr\nimport time\n\n\nclass Lifelong_Astar(object):\n def __init__(self,resolution = 1):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.env = env(resolution=resolution)\n self.g = g_Space(self)\n self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)\n self.x0, self.xt = self.start, self.goal\n self.rhs = g_Space(self) # rhs(.) = g(.) = inf\n self.rhs[self.start] = 0 # rhs(x0) = 0\n self.h = Heuristic(self.g, self.goal)\n \n self.OPEN = queue.MinheapPQ() # store [point,priority]\n self.OPEN.put(self.x0, [self.h[self.x0],0])\n self.CLOSED = set()\n\n # used for A*\n self.done = False\n self.Path = []\n self.V = []\n self.ind = 0\n\n # initialize children list\n self.CHILDREN = {}\n self.getCHILDRENset()\n\n # initialize Cost list\n self.COST = {}\n _ = self.costset()\n\n def costset(self):\n NodeToChange = set()\n for xi in self.CHILDREN.keys():\n children = self.CHILDREN[xi]\n toUpdate = [self.cost(xj,xi) for xj in children]\n if xi in self.COST:\n # if the old Cost not equal to new Cost\n diff = np.not_equal(self.COST[xi],toUpdate)\n cd = np.array(children)[diff]\n for i in cd:\n NodeToChange.add(tuple(i))\n self.COST[xi] = toUpdate\n else:\n self.COST[xi] = toUpdate\n return NodeToChange\n\n def getCOSTset(self,xi,xj):\n ind, children = 0, self.CHILDREN[xi]\n for i in children:\n if i == xj:\n return self.COST[xi][ind]\n ind += 1\n \n\n def children(self, x):\n allchild = []\n resolution = self.env.resolution\n for direc in self.Alldirec:\n child = tuple(map(np.add,x,np.multiply(direc,resolution)))\n if isinbound(self.env.boundary,child):\n allchild.append(child)\n return allchild\n\n def getCHILDRENset(self):\n for xi in self.g.keys():\n self.CHILDREN[xi] = self.children(xi)\n \n def isCollide(self, x, child):\n ray , dist = getRay(x, child) , getDist(x, child)\n if not isinbound(self.env.boundary,child):\n return True, dist\n for i in self.env.AABB_pyrr:\n shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)\n if shot is not None:\n dist_wall = getDist(x, shot)\n if dist_wall <= dist: # collide\n return True, dist\n for i in self.env.balls:\n if isinball(i, child):\n return True, dist\n shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)\n if shot != []:\n dists_ball = [getDist(x, j) for j in shot]\n if all(dists_ball <= dist): # collide\n return True, dist\n return False, dist\n\n def cost(self, x, y):\n collide, dist = isCollide(self, x, y)\n if collide: return np.inf\n else: return dist\n \n def key(self,xi,epsilion = 1):\n return [min(self.g[xi],self.rhs[xi]) + epsilion*self.h[xi],min(self.g[xi],self.rhs[xi])]\n\n def path(self):\n path = []\n x = self.xt\n start = self.x0\n ind = 0\n while x != start:\n j = x\n nei = self.CHILDREN[x]\n gset = [self.g[xi] for xi in nei]\n # collision check and make g Cost inf\n for i in range(len(nei)):\n if isCollide(self, nei[i],j)[0]:\n gset[i] = np.inf\n parent = nei[np.argmin(gset)]\n path.append([x, parent])\n x = parent\n if ind > 100:\n break\n ind += 1\n return path\n\n #------------------Lifelong Plannning A* \n def UpdateMembership(self, xi, xparent=None):\n if xi != self.x0:\n self.rhs[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]])\n self.OPEN.check_remove(xi)\n if self.g[xi] != self.rhs[xi]:\n self.OPEN.put(xi,self.key(xi))\n \n def ComputePath(self):\n print('computing path ...')\n while self.key(self.xt) > self.OPEN.top_key() or self.rhs[self.xt] != self.g[self.xt]:\n xi = self.OPEN.get()\n # if g > rhs, overconsistent\n if self.g[xi] > self.rhs[xi]: \n self.g[xi] = self.rhs[xi]\n # add xi to expanded node set\n if xi not in self.CLOSED:\n self.V.append(xi)\n self.CLOSED.add(xi)\n else: # underconsistent and consistent\n self.g[xi] = np.inf\n self.UpdateMembership(xi)\n for xj in self.CHILDREN[xi]:\n self.UpdateMembership(xj)\n\n # visualization(self)\n self.ind += 1\n self.Path = self.path()\n self.done = True\n visualization(self)\n plt.pause(1)\n\n def change_env(self):\n _, _ = self.env.move_block(block_to_move=1,a = [0,2,0])\n self.done = False\n self.Path = []\n self.CLOSED = set()\n N = self.costset()\n for xi in N:\n self.UpdateMembership(xi)\n\nif __name__ == '__main__':\n sta = time.time()\n Astar = Lifelong_Astar(1)\n Astar.ComputePath()\n for i in range(5):\n Astar.change_env()\n Astar.ComputePath()\n plt.pause(1)\n \n print(time.time() - sta)" }, { "path": "Search_based_Planning/Search_3D/LRT_Astar3D.py", "content": "# this is the three dimensional N>1 LRTA* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D import Astar3D\nfrom Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \\\n cost, obstacleFree, children\nfrom Search_3D.plot_util3D import visualization\nimport queue\n\nclass LRT_A_star2:\n def __init__(self, resolution=0.5, N=7):\n self.N = N\n self.Astar = Astar3D.Weighted_A_star(resolution=resolution)\n self.path = []\n\n def updateHeuristic(self):\n # Initialize hvalues at infinity\n for xi in self.Astar.CLOSED:\n self.Astar.h[xi] = np.inf\n Diff = True\n while Diff: # repeat DP until converge\n hvals, lasthvals = [], []\n for xi in self.Astar.CLOSED:\n lasthvals.append(self.Astar.h[xi])\n # update h values if they are smaller\n Children = children(self.Astar,xi)\n minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])\n # h(s) = h(s') if h(s) > cBest(s,s') + h(s') \n if self.Astar.h[xi] >= minfval:\n self.Astar.h[xi] = minfval\n hvals.append(self.Astar.h[xi])\n if lasthvals == hvals: Diff = False\n\n def move(self):\n st = self.Astar.x0\n ind = 0\n # find the lowest path down hill\n while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop\n Children = children(self.Astar,st)\n minh, minchild = np.inf, None\n for child in Children:\n # check collision here, not a supper efficient\n collide, _ = isCollide(self.Astar,st, child)\n if collide:\n continue\n h = self.Astar.h[child]\n if h <= minh:\n minh, minchild = h, child\n self.path.append([st, minchild])\n st = minchild\n for (_, p) in self.Astar.OPEN.enumerate():\n if p == st:\n break\n ind += 1\n if ind > 1000:\n break\n self.Astar.reset(st)\n\n def run(self):\n while True:\n if self.Astar.run(N=self.N):\n self.Astar.Path = self.Astar.Path + self.path\n self.Astar.done = True\n visualization(self.Astar)\n plt.show()\n break\n self.updateHeuristic()\n self.move()\n\n\nif __name__ == '__main__':\n T = LRT_A_star2(resolution=0.5, N=100)\n T.run()\n" }, { "path": "Search_based_Planning/Search_3D/RTA_Astar3D.py", "content": "# this is the three dimensional Real-time Adaptive LRTA* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D import Astar3D\nfrom Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \\\n cost, obstacleFree, children\nfrom Search_3D.plot_util3D import visualization\nimport queue\n\nclass RTA_A_star:\n def __init__(self, resolution=0.5, N=7):\n self.N = N # node to expand \n self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star\n self.path = [] # empty path\n self.st = []\n self.localhvals = []\n\n def updateHeuristic(self):\n # Initialize hvalues at infinity\n self.localhvals = []\n nodeset, vals = [], []\n for (_,_,xi) in self.Astar.OPEN.enumerate():\n nodeset.append(xi)\n vals.append(self.Astar.g[xi] + self.Astar.h[xi])\n j, fj = nodeset[np.argmin(vals)], min(vals)\n self.st = j\n # single pass update of hvals\n for xi in self.Astar.CLOSED:\n self.Astar.h[xi] = fj - self.Astar.g[xi]\n self.localhvals.append(self.Astar.h[xi])\n \n def move(self):\n st, localhvals = self.st, self.localhvals\n maxhval = max(localhvals)\n sthval = self.Astar.h[st]\n # find the lowest path up hill\n while sthval < maxhval:\n parentsvals , parents = [] , []\n # find the max child\n for xi in children(self.Astar,st):\n if xi in self.Astar.CLOSED:\n parents.append(xi)\n parentsvals.append(self.Astar.h[xi])\n lastst = st \n st = parents[np.argmax(parentsvals)]\n self.path.append([st,lastst]) # add to path\n sthval = self.Astar.h[st]\n self.Astar.reset(self.st)\n\n def run(self):\n while True:\n if self.Astar.run(N=self.N):\n self.Astar.Path = self.Astar.Path + self.path\n self.Astar.done = True\n visualization(self.Astar)\n plt.show()\n break\n self.updateHeuristic()\n self.move()\n\n\nif __name__ == '__main__':\n T = RTA_A_star(resolution=1, N=100)\n T.run()" }, { "path": "Search_based_Planning/Search_3D/bidirectional_Astar3D.py", "content": "# this is the three dimensional bidirectional A* algo\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom collections import defaultdict\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Search_based_Planning/\")\nfrom Search_3D.env3D import env\nfrom Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun\nfrom Search_3D.plot_util3D import visualization\nimport queue\n\n\nclass Weighted_A_star(object):\n def __init__(self,resolution=0.5):\n self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \\\n (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \\\n (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \\\n (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \\\n (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \\\n (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \\\n (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \\\n (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \\\n (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}\n self.env = env(resolution = resolution)\n self.settings = 'NonCollisionChecking'\n self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)\n self.g = {self.start:0,self.goal:0}\n self.OPEN1 = queue.MinheapPQ() # store [point,priority]\n self.OPEN2 = queue.MinheapPQ()\n self.Parent1, self.Parent2 = {}, {}\n self.CLOSED1, self.CLOSED2 = set(), set()\n self.V = []\n self.done = False\n self.Path = []\n\n def run(self):\n x0, xt = self.start, self.goal\n self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h\n self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h\n self.ind = 0\n while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty\n xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() \n self.CLOSED1.add(xi1) # add the point in CLOSED set\n self.CLOSED2.add(xi2)\n self.V.append(xi1)\n self.V.append(xi2)\n # visualization(self)\n allchild1, allchild2 = children(self,xi1), children(self,xi2)\n self.evaluation(allchild1,xi1,conf=1)\n self.evaluation(allchild2,xi2,conf=2)\n if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))\n self.ind += 1\n self.common = self.CLOSED1.intersection(self.CLOSED2)\n self.done = True\n self.Path = self.path()\n visualization(self)\n plt.show()\n\n def evaluation(self, allchild, xi, conf):\n for xj in allchild:\n if conf == 1:\n if xj not in self.CLOSED1:\n if xj not in self.g:\n self.g[xj] = np.inf\n else:\n pass\n gi = self.g[xi]\n a = gi + cost(self,xi,xj)\n if a < self.g[xj]:\n self.g[xj] = a\n self.Parent1[xj] = xi\n self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))\n if conf == 2:\n if xj not in self.CLOSED2:\n if xj not in self.g:\n self.g[xj] = np.inf\n else:\n pass\n gi = self.g[xi]\n a = gi + cost(self,xi,xj)\n if a < self.g[xj]:\n self.g[xj] = a\n self.Parent2[xj] = xi\n self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))\n \n def path(self):\n # TODO: fix path\n path = []\n goal = self.goal\n start = self.start\n x = list(self.common)[0]\n while x != start:\n path.append([x,self.Parent1[x]])\n x = self.Parent1[x]\n x = list(self.common)[0]\n while x != goal:\n path.append([x,self.Parent2[x]])\n x = self.Parent2[x]\n path = np.flip(path,axis=0)\n return path\n\nif __name__ == '__main__':\n Astar = Weighted_A_star(0.5)\n Astar.run()" }, { "path": "Search_based_Planning/Search_3D/env3D.py", "content": "# this is the three dimensional space\n# !/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\n@author: yue qi\n\"\"\"\nimport numpy as np\n# from utils3D import OBB2AABB\n\ndef R_matrix(z_angle,y_angle,x_angle):\n # s angle: row; y angle: pitch; z angle: yaw\n # generate rotation matrix in SO3\n # RzRyRx = R, ZYX intrinsic rotation\n # also (r1,r2,r3) in R3*3 in {W} frame\n # used in obb.O\n # [[R p]\n # [0T 1]] gives transformation from body to world \n 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]])@ \\\n 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)]])@ \\\n 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)]])\n\ndef getblocks():\n # AABBs\n block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00],\n [5.5e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],\n [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00],\n [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00],\n [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]]\n Obstacles = []\n for i in block:\n i = np.array(i)\n Obstacles.append([j for j in i])\n return np.array(Obstacles)\n\ndef getballs():\n spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]]\n Obstacles = []\n for i in spheres:\n Obstacles.append([j for j in i])\n return np.array(Obstacles)\n\ndef getAABB(blocks):\n # used for Pyrr package for detecting collision\n AABB = []\n for i in blocks:\n AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger\n return AABB\n\ndef getAABB2(blocks):\n # used in lineAABB\n AABB = []\n for i in blocks:\n AABB.append(aabb(i))\n return AABB\n\ndef add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):\n return block\n\nclass aabb(object):\n # make AABB out of blocks, \n # P: center point\n # E: extents\n # O: Rotation matrix in SO(3), in {w}\n def __init__(self,AABB):\n self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point\n self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents\n self.O = [[1,0,0],[0,1,0],[0,0,1]]\n\nclass obb(object):\n # P: center point\n # E: extents\n # O: Rotation matrix in SO(3), in {w}\n def __init__(self, P, E, O):\n self.P = P\n self.E = E\n self.O = O\n self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]])\n\nclass env():\n def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1):\n # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1): \n self.resolution = resolution\n self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) \n self.blocks = getblocks()\n self.AABB = getAABB2(self.blocks)\n self.AABB_pyrr = getAABB(self.blocks)\n self.balls = getballs()\n self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)),\n obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))])\n self.start = np.array([2.0, 2.0, 2.0])\n self.goal = np.array([6.0, 16.0, 0.0])\n self.t = 0 # time \n\n def New_block(self):\n newblock = add_block()\n self.blocks = np.vstack([self.blocks,newblock])\n self.AABB = getAABB2(self.blocks)\n self.AABB_pyrr = getAABB(self.blocks)\n\n def move_start(self, x):\n self.start = x\n\n def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'):\n # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, \n # R is an orthorgonal transform in R3*3, is the rotation matrix\n # (s',t') = (s + tv, t) is uniform transformation\n # (s',t') = (s + a, t + s) is a translation\n if mode == 'translation':\n ori = np.array(self.blocks[block_to_move])\n self.blocks[block_to_move] = \\\n np.array([ori[0] + a[0],\\\n ori[1] + a[1],\\\n ori[2] + a[2],\\\n ori[3] + a[0],\\\n ori[4] + a[1],\\\n ori[5] + a[2]])\n\n self.AABB[block_to_move].P = \\\n [self.AABB[block_to_move].P[0] + a[0], \\\n self.AABB[block_to_move].P[1] + a[1], \\\n self.AABB[block_to_move].P[2] + a[2]]\n self.t += s\n # return a range of block that the block might moved\n a = self.blocks[block_to_move]\n return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \\\n a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \\\n np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \\\n ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])\n # return a,ori\n # (s',t') = (Rx, t)\n def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):\n # theta stands for rotational angles around three principle axis in world frame\n # translation stands for translation in the world frame\n ori = [self.OBB[obb_to_move]]\n self.OBB[obb_to_move].P = \\\n [self.OBB[obb_to_move].P[0] + translation[0], \n self.OBB[obb_to_move].P[1] + translation[1], \n self.OBB[obb_to_move].P[2] + translation[2]]\n # Calculate orientation\n self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])\n # generating transformation matrix\n self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\\\n -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]])\n return self.OBB[obb_to_move], ori[0]\n \nif __name__ == '__main__':\n newenv = env()" }, { "path": "Search_based_Planning/Search_3D/plot_util3D.py", "content": "# plotting\nimport matplotlib.pyplot as plt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom mpl_toolkits.mplot3d.art3d import Poly3DCollection\nimport mpl_toolkits.mplot3d as plt3d\nfrom mpl_toolkits.mplot3d import proj3d\nimport numpy as np\n\ndef CreateSphere(center,r):\n u = np.linspace(0,2* np.pi,30)\n v = np.linspace(0,np.pi,30)\n x = np.outer(np.cos(u),np.sin(v))\n y = np.outer(np.sin(u),np.sin(v))\n z = np.outer(np.ones(np.size(u)),np.cos(v))\n x, y, z = r*x + center[0], r*y + center[1], r*z + center[2]\n return (x,y,z)\n\ndef draw_Spheres(ax,balls):\n for i in balls:\n (xs,ys,zs) = CreateSphere(i[0:3],i[-1])\n ax.plot_wireframe(xs, ys, zs, alpha=0.15,color=\"b\")\n\ndef draw_block_list(ax, blocks ,color=None,alpha=0.15):\n '''\n drawing the blocks on the graph\n '''\n 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]],\n dtype='float')\n f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])\n n = blocks.shape[0]\n d = blocks[:, 3:6] - blocks[:, :3]\n vl = np.zeros((8 * n, 3))\n fl = np.zeros((6 * n, 4), dtype='int64')\n for k in range(n):\n vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]\n fl[k * 6:(k + 1) * 6, :] = f + k * 8\n if type(ax) is Poly3DCollection:\n ax.set_verts(vl[fl])\n else:\n pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')\n pc.set_facecolor(color)\n h = ax.add_collection3d(pc)\n return h\n\ndef obb_verts(obb):\n # 0.017004013061523438 for 1000 iters\n ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\\\n [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]])\n # P + (ori * E)\n ori_body = np.multiply(ori_body,obb.E)\n # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3)\n verts = (obb.O@ori_body.T).T + obb.P\n return verts\n\n\ndef draw_obb(ax, OBB, color=None,alpha=0.15):\n f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])\n n = OBB.shape[0]\n vl = np.zeros((8 * n, 3))\n fl = np.zeros((6 * n, 4), dtype='int64')\n for k in range(n):\n vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k])\n fl[k * 6:(k + 1) * 6, :] = f + k * 8\n if type(ax) is Poly3DCollection:\n ax.set_verts(vl[fl])\n else:\n pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')\n pc.set_facecolor(color)\n h = ax.add_collection3d(pc)\n return h\n\n\ndef draw_line(ax,SET,visibility=1,color=None):\n if SET != []:\n for i in SET:\n xs = i[0][0], i[1][0]\n ys = i[0][1], i[1][1]\n zs = i[0][2], i[1][2]\n line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color)\n ax.add_line(line)\n\ndef visualization(initparams):\n if initparams.ind % 20 == 0 or initparams.done:\n V = np.array(list(initparams.V))\n # E = initparams.E\n Path = np.array(initparams.Path)\n start = initparams.env.start\n goal = initparams.env.goal\n # edges = E.get_edge()\n # generate axis objects\n ax = plt.subplot(111, projection='3d')\n #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))\n #ax.view_init(elev=0., azim=90.)\n ax.view_init(elev=90., azim=0.)\n #ax.view_init(elev=-8., azim=180)\n ax.clear()\n # drawing objects\n draw_Spheres(ax, initparams.env.balls)\n draw_block_list(ax, initparams.env.blocks)\n if initparams.env.OBB is not None:\n draw_obb(ax,initparams.env.OBB)\n draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)\n # draw_line(ax,edges,visibility=0.25)\n draw_line(ax,Path,color='r')\n if len(V) > 0:\n ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)\n ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')\n ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') \n # adjust the aspect ratio\n set_axes_equal(ax)\n make_transparent(ax)\n # plt.xlabel('s')\n # plt.ylabel('y')\n plt.pause(0.0001)\n\ndef set_axes_equal(ax):\n '''Make axes of 3D plot have equal scale so that spheres appear as spheres,\n cubes as cubes, etc.. This is one possible solution to Matplotlib's\n ax.set_aspect('equal') and ax.axis('equal') not working for 3D.\n https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to\n Input\n ax: a matplotlib axis, e.g., as output from plt.gca().\n '''\n\n x_limits = ax.get_xlim3d()\n y_limits = ax.get_ylim3d()\n z_limits = ax.get_zlim3d()\n\n x_range = abs(x_limits[1] - x_limits[0])\n x_middle = np.mean(x_limits)\n y_range = abs(y_limits[1] - y_limits[0])\n y_middle = np.mean(y_limits)\n z_range = abs(z_limits[1] - z_limits[0])\n z_middle = np.mean(z_limits)\n\n # The plot bounding box is a sphere in the sense of the infinity\n # norm, hence I call half the max range the plot radius.\n plot_radius = 0.5*max([x_range, y_range, z_range])\n\n ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])\n ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])\n ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])\n\ndef make_transparent(ax):\n # make the panes transparent\n ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\n ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\n ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))\n # make the grid lines transparent\n ax.xaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\n ax.yaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\n ax.zaxis._axinfo[\"grid\"]['color'] = (1,1,1,0)\n\nif __name__ == '__main__':\n pass" }, { "path": "Search_based_Planning/Search_3D/queue.py", "content": "import collections\nimport heapq\nimport itertools\n\nclass QueueFIFO:\n \"\"\"\n Class: QueueFIFO\n Description: QueueFIFO is designed for First-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.popleft() # leave from front\n\n\nclass QueueLIFO:\n \"\"\"\n Class: QueueLIFO\n Description: QueueLIFO is designed for Last-in-First-out rule.\n \"\"\"\n\n def __init__(self):\n self.queue = collections.deque()\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, node):\n self.queue.append(node) # enter from back\n\n def get(self):\n return self.queue.pop() # leave from back\n\n\nclass QueuePrior:\n \"\"\"\n Class: QueuePrior\n Description: QueuePrior reorders elements using value [priority]\n \"\"\"\n\n def __init__(self):\n self.queue = []\n\n def empty(self):\n return len(self.queue) == 0\n\n def put(self, item, priority):\n heapq.heappush(self.queue, (priority, item)) # reorder s using priority\n\n def get(self):\n return heapq.heappop(self.queue)[1] # pop out the smallest item\n\n def enumerate(self):\n return self.queue\n\n def check_remove(self, item):\n for (p, x) in self.queue:\n if item == x:\n self.queue.remove((p, x))\n\n def top_key(self):\n return self.queue[0][0]\n\nclass MinheapPQ:\n \"\"\"\n A priority queue based on min heap, which takes O(logn) on element removal\n https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes\n \"\"\"\n def __init__(self):\n self.pq = [] # lis of the entries arranged in a heap\n self.nodes = set()\n self.entry_finder = {} # mapping of the item entries\n self.counter = itertools.count() # unique sequence count\n self.REMOVED = ''\n \n def put(self, item, priority):\n '''add a new task or update the priority of an existing item'''\n if item in self.entry_finder:\n self.check_remove(item)\n count = next(self.counter)\n entry = [priority, count, item]\n self.entry_finder[item] = entry\n heapq.heappush(self.pq, entry)\n self.nodes.add(item)\n\n def check_remove(self, item):\n if item not in self.entry_finder:\n return\n entry = self.entry_finder.pop(item)\n entry[-1] = self.REMOVED\n self.nodes.remove(item)\n\n def get(self):\n \"\"\"Remove and return the lowest priority task. Raise KeyError if empty.\"\"\"\n while self.pq:\n priority, count, item = heapq.heappop(self.pq)\n if item is not self.REMOVED:\n del self.entry_finder[item]\n self.nodes.remove(item)\n return item\n raise KeyError('pop from an empty priority queue')\n\n def top_key(self):\n return self.pq[0][0]\n \n def enumerate(self):\n return self.pq\n\n def allnodes(self):\n return self.nodes\n\n# class QueuePrior:\n# \"\"\"\n# Class: QueuePrior\n# Description: QueuePrior reorders elements using value [priority]\n# \"\"\"\n\n# def __init__(self):\n# self.queue = []\n\n# def empty(self):\n# return len(self.queue) == 0\n\n# def put(self, item, priority):\n# count = 0\n# for (p, s) in self.queue:\n# if s == item:\n# self.queue[count] = (priority, item)\n# break\n# count += 1\n# if count == len(self.queue):\n# heapq.heappush(self.queue, (priority, item)) # reorder s using priority\n\n# def get(self):\n# return heapq.heappop(self.queue)[1] # pop out the smallest item\n\n# def enumerate(self):\n# return self.queue\n\n# def check_remove(self, item):\n# for (p, s) in self.queue:\n# if item == s:\n# self.queue.remove((p, s))\n\n# def top_key(self):\n# return self.queue[0][0]" }, { "path": "Search_based_Planning/Search_3D/utils3D.py", "content": "import numpy as np\nimport pyrr\nfrom collections import defaultdict\nimport copy\n\n\ndef getRay(x, y):\n direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]\n return np.array([x, direc])\n\n\ndef getDist(pos1, pos2):\n return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))\n\n\ndef getManDist(pos1, pos2):\n return sum([abs(pos1[0] - pos2[0]), abs(pos1[1] - pos2[1]), abs(pos1[2] - pos2[2])])\n\n\ndef getNearest(Space, pt):\n '''get the nearest point on the grid'''\n mindis, minpt = 1000, None\n for pts in Space:\n dis = getDist(pts, pt)\n if dis < mindis:\n mindis, minpt = dis, pts\n return minpt\n\n\ndef Heuristic(Space, t):\n '''Max norm distance'''\n h = {}\n for k in Space.keys():\n h[k] = max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])\n return h\n\ndef heuristic_fun(initparams, k, t=None):\n if t is None:\n t = initparams.goal\n return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])\n\ndef isinbound(i, x, mode = False, factor = 0, isarray = False):\n if mode == 'obb':\n return isinobb(i, x, isarray)\n if isarray:\n compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) \n compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) \n compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) \n return compx & compy & compz\n else: \n return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor\n\ndef isinball(i, x, factor = 0):\n if getDist(i[0:3], x) <= i[3] + factor:\n return True\n return False\n\ndef isinobb(i, x, isarray = False):\n # transform the point from {W} to {body}\n if isarray:\n pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3]\n block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]\n return isinbound(block, pts, isarray = isarray)\n else:\n pt = i.T@np.append(x,1)\n block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]]\n return isinbound(block, pt)\n\ndef OBB2AABB(obb):\n # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1\n aabb = copy.deepcopy(obb)\n P = obb.P\n a = obb.E\n A = obb.O\n # a1(A1 dot s) + a2(A2 dot s) + a3(A3 dot s)\n Ex = a[0]*abs(A[0][0]) + a[1]*abs(A[1][0]) + a[2]*abs(A[2][0])\n Ey = a[0]*abs(A[0][1]) + a[1]*abs(A[1][1]) + a[2]*abs(A[2][1])\n Ez = a[0]*abs(A[0][2]) + a[1]*abs(A[1][2]) + a[2]*abs(A[2][2])\n E = np.array([Ex, Ey, Ez])\n aabb.P = P\n aabb.E = E\n aabb.O = np.array([[1,0,0],[0,1,0],[0,0,1]])\n return aabb\n\ndef lineSphere(p0, p1, ball):\n # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf\n c, r = ball[0:3], ball[-1]\n line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]]\n d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]]\n t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * (\n line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2])\n if t <= 0:\n if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True\n elif t >= 1:\n d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]]\n if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True\n elif 0 < t < 1:\n x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]]\n k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]]\n if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True\n return False\n\ndef lineAABB(p0, p1, dist, aabb):\n # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1\n # aabb should have the attributes of P, E as center point and extents\n mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point\n I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction\n hl = dist / 2 # radius\n T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]]\n # do any of the principal axis form a separting axis?\n if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False\n if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False\n if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False\n # I.cross(s axis) ?\n r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])\n if abs(T[1] * I[2] - T[2] * I[1]) > r: return False\n # I.cross(y axis) ?\n r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0])\n if abs(T[2] * I[0] - T[0] * I[2]) > r: return False\n # I.cross(z axis) ?\n r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0])\n if abs(T[0] * I[1] - T[1] * I[0]) > r: return False\n\n return True\n\ndef lineOBB(p0, p1, dist, obb):\n # transform points to obb frame\n res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T \n # record old position and set the position to origin\n oldP, obb.P= obb.P, [0,0,0] \n # calculate segment-AABB testing\n ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb)\n # reset the position\n obb.P = oldP \n return ans\n\ndef OBBOBB(obb1, obb2):\n # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1\n # each obb class should contain attributes:\n # E: extents along three principle axis in R3\n # P: position of the center axis in R3\n # O: orthornormal basis in R3*3\n a , b = np.array(obb1.E), np.array(obb2.E)\n Pa, Pb = np.array(obb1.P), np.array(obb2.P)\n A , B = np.array(obb1.O), np.array(obb2.O)\n # check if two oriented bounding boxes overlap\n # translation, in parent frame\n v = Pb - Pa\n # translation, in A's frame\n # vdotA[0],vdotA[1],vdotA[2]\n T = [v@B[0], v@B[1], v@B[2]]\n R = np.zeros([3,3])\n for i in range(0,3):\n for k in range(0,3):\n R[i][k] = A[i]@B[k]\n # use separating axis thm for all 15 separating axes\n # if the separating axis cannot be found, then overlap\n # A's basis vector\n for i in range(0,3):\n ra = a[i]\n rb = b[0]*abs(R[i][0]) + b[1]*abs(R[i][1]) + b[2]*abs(R[i][2])\n t = abs(T[i])\n if t > ra + rb:\n return False\n for k in range(0,3):\n ra = a[0]*abs(R[0][k]) + a[1]*abs(R[1][k]) + a[2]*abs(R[2][k])\n rb = b[k]\n t = abs(T[0]*R[0][k] + T[1]*R[1][k] + T[2]*R[2][k])\n if t > ra + rb:\n return False\n\n #9 cross products\n #L = A0 s B0\n ra = a[1]*abs(R[2][0]) + a[2]*abs(R[1][0])\n rb = b[1]*abs(R[0][2]) + b[2]*abs(R[0][1])\n t = abs(T[2]*R[1][0] - T[1]*R[2][0])\n if t > ra + rb:\n return False\n\n #L = A0 s B1\n ra = a[1]*abs(R[2][1]) + a[2]*abs(R[1][1])\n rb = b[0]*abs(R[0][2]) + b[2]*abs(R[0][0])\n t = abs(T[2]*R[1][1] - T[1]*R[2][1])\n if t > ra + rb:\n return False\n\n #L = A0 s B2\n ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2])\n rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0])\n t = abs(T[2]*R[1][2] - T[1]*R[2][2])\n if t > ra + rb:\n return False\n\n #L = A1 s B0\n ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0])\n rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1])\n t = abs( T[0]*R[2][0] - T[2]*R[0][0] )\n if t > ra + rb:\n return False\n\n # L = A1 s B1\n ra = a[0]*abs(R[2][1]) + a[2]*abs(R[0][1])\n rb = b[0]*abs(R[1][2]) + b[2]*abs(R[1][0])\n t = abs( T[0]*R[2][1] - T[2]*R[0][1] )\n if t > ra + rb:\n return False\n\n #L = A1 s B2\n ra = a[0]*abs(R[2][2]) + a[2]*abs(R[0][2])\n rb = b[0]*abs(R[1][1]) + b[1]*abs(R[1][0])\n t = abs( T[0]*R[2][2] - T[2]*R[0][2] )\n if t > ra + rb:\n return False\n\n #L = A2 s B0\n ra = a[0]*abs(R[1][0]) + a[1]*abs(R[0][0])\n rb = b[1]*abs(R[2][2]) + b[2]*abs(R[2][1])\n t = abs( T[1]*R[0][0] - T[0]*R[1][0] )\n if t > ra + rb:\n return False\n\n # L = A2 s B1\n ra = a[0]*abs(R[1][1]) + a[1]*abs(R[0][1])\n rb = b[0] *abs(R[2][2]) + b[2]*abs(R[2][0])\n t = abs( T[1]*R[0][1] - T[0]*R[1][1] )\n if t > ra + rb:\n return False\n\n #L = A2 s B2\n ra = a[0]*abs(R[1][2]) + a[1]*abs(R[0][2])\n rb = b[0]*abs(R[2][1]) + b[1]*abs(R[2][0])\n t = abs( T[1]*R[0][2] - T[0]*R[1][2] )\n if t > ra + rb:\n return False\n\n # no separating axis found,\n # the two boxes overlap \n return True\n\ndef StateSpace(env, factor=0):\n boundary = env.boundary\n resolution = env.resolution\n xmin, xmax = boundary[0] + factor * resolution, boundary[3] - factor * resolution\n ymin, ymax = boundary[1] + factor * resolution, boundary[4] - factor * resolution\n zmin, zmax = boundary[2] + factor * resolution, boundary[5] - factor * resolution\n xarr = np.arange(xmin, xmax, resolution).astype(float)\n yarr = np.arange(ymin, ymax, resolution).astype(float)\n zarr = np.arange(zmin, zmax, resolution).astype(float)\n Space = set()\n for x in xarr:\n for y in yarr:\n for z in zarr:\n Space.add((x, y, z))\n return Space\n\ndef g_Space(initparams):\n '''This function is used to get nodes and discretize the space.\n State space is by s*y*z,3 where each 3 is a point in 3D.'''\n g = {}\n Space = StateSpace(initparams.env)\n for v in Space:\n g[v] = np.inf # this hashmap initialize all g values at inf\n return g\n\ndef isCollide(initparams, x, child, dist=None):\n '''see if line intersects obstacle'''\n '''specified for expansion in A* 3D lookup table'''\n if dist==None:\n dist = getDist(x, child)\n # check in bound\n if not isinbound(initparams.env.boundary, child): \n return True, dist\n # check collision in AABB\n for i in range(len(initparams.env.AABB)):\n if lineAABB(x, child, dist, initparams.env.AABB[i]): \n return True, dist\n # check collision in ball\n for i in initparams.env.balls:\n if lineSphere(x, child, i): \n return True, dist\n # check collision with obb\n for i in initparams.env.OBB:\n if lineOBB(x, child, dist, i):\n return True, dist\n return False, dist\n\ndef children(initparams, x, settings = 0):\n # get the neighbor of a specific state\n allchild = []\n allcost = []\n resolution = initparams.env.resolution\n for direc in initparams.Alldirec:\n child = tuple(map(np.add, x, np.multiply(direc, resolution)))\n if any([isinobb(i, child) for i in initparams.env.OBB]):\n continue\n if any([isinball(i ,child) for i in initparams.env.balls]):\n continue\n if any([isinbound(i ,child) for i in initparams.env.blocks]):\n continue\n if isinbound(initparams.env.boundary, child):\n allchild.append(child)\n allcost.append((child,initparams.Alldirec[direc]*resolution))\n if settings == 0:\n return allchild\n if settings == 1:\n return allcost\n\ndef obstacleFree(initparams, x):\n for i in initparams.env.blocks:\n if isinbound(i, x):\n return False\n for i in initparams.env.balls:\n if isinball(i, x):\n return False\n return True\n\n\ndef cost(initparams, i, j, dist=None, settings='Euclidean'):\n if initparams.settings == 'NonCollisionChecking':\n if dist==None:\n dist = getDist(i,j)\n collide = False\n else:\n collide, dist = isCollide(initparams, i, j, dist)\n # collide, dist= False, getDist(i, j)\n if settings == 'Euclidean':\n if collide:\n return np.inf\n else:\n return dist\n if settings == 'Manhattan':\n if collide:\n return np.inf\n else:\n return getManDist(i, j)\n\n\ndef initcost(initparams):\n # initialize Cost dictionary, could be modifed lateron\n c = defaultdict(lambda: defaultdict(dict)) # two key dicionary\n for xi in initparams.X:\n cdren = children(initparams, xi)\n for child in cdren:\n c[xi][child] = cost(initparams, xi, child)\n return c\n\nif __name__ == \"__main__\":\n import time\n from env3D import R_matrix, obb\n obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45))\n # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]])\n p0, p1 = [2.9,2.5,1],[1.9,2.5,1]\n pts = np.array([[1,2,3],[4,5,6],[7,8,9],[2,2,2],[1,1,1],[3,3,3]])\n start = time.time()\n isinbound(obb1, pts, mode='obb', factor = 0, isarray = True)\n print(time.time() - start)\n \n\n\n" } ]