From 5f0698983750571e7d6e45bbfbfd108d312470ad Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 30 Jul 2020 10:02:26 -0700 Subject: [PATCH] reformat --- .../rrt_2D/CurvesGenerator/reeds_shepp.py | 687 ------------------ .../.idea/.gitignore | 0 .../.idea/Sampling-based Planning.iml | 0 .../.idea/dictionaries/zhou.xml | 0 .../inspectionProfiles/profiles_settings.xml | 0 .../.idea/misc.xml | 0 .../.idea/modules.xml | 0 .../.idea/vcs.xml | 0 .../gif/Dynamic_RRT_2D.gif | Bin .../gif/Extended_RRT_2D.gif | Bin .../gif/Goal_biasd_RRT_2D.gif | Bin .../gif/INFORMED_RRT_STAR_2D.gif | Bin .../gif/RRT_CONNECT_2D.gif | Bin .../gif/RRT_STAR2_2D.gif | Bin .../gif/RRT_STAR_2D.gif | Bin .../gif/RRT_STAR_SMART_2D.gif | Bin .../rrt_2D/__pycache__/env.cpython-37.pyc | Bin .../__pycache__/plotting.cpython-37.pyc | Bin .../rrt_2D/__pycache__/queue.cpython-37.pyc | Bin .../rrt_2D/__pycache__/rrt.cpython-37.pyc | Bin .../rrt_2D/__pycache__/utils.cpython-37.pyc | Bin .../rrt_2D/dubins_rrt_star.py | 57 ++ .../rrt_2D/dynamic_rrt.py | 6 +- .../rrt_2D/env.py | 0 .../rrt_2D/extended_rrt.py | 6 +- .../rrt_2D/informed_rrt_star.py | 6 +- .../rrt_2D/optimal_bi_rrt.py | 11 +- .../rrt_2D/plotting.py | 4 +- .../rrt_2D/queue.py | 0 .../rrt_2D/rrt.py | 6 +- .../rrt_2D/rrt_connect.py | 6 +- .../rrt_2D/rrt_star.py | 7 +- .../rrt_2D/rrt_star_smart.py | 8 +- .../rrt_2D/rs_rrt_star.py | 8 +- .../rrt_2D/utils.py | 6 +- .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin .../__pycache__/plot_util3D.cpython-37.pyc | Bin .../rrt_3D/__pycache__/rrt3D.cpython-37.pyc | Bin .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin .../rrt_3D/dynamic_rrt3D.py | 2 +- .../rrt_3D/env3D.py | 328 ++++----- .../rrt_3D/plot_util3D.py | 368 +++++----- .../rrt_3D/rrt3D.py | 2 +- .../rrt_3D/rrtstar3D.py | 2 +- .../rrt_3D/utils3D.py | 538 +++++++------- .../.idea/Search-based Planning.iml | 0 .../.idea/dictionaries/Huiming_Zhou.xml | 0 .../inspectionProfiles/profiles_settings.xml | 0 .../.idea/misc.xml | 0 .../.idea/modules.xml | 0 .../.idea/vcs.xml | 0 .../.idea/workspace.xml | 0 .../Search_2D/ARAstar.py | 2 +- .../Search_2D/Anytime_D_star.py | 2 +- .../Search_2D/Astar.py | 2 +- .../Search_2D/Best_First.py | 2 +- .../Search_2D/Bidirectional_a_star.py | 2 +- .../Search_2D/D_star.py | 2 +- .../Search_2D/D_star_Lite.py | 2 +- .../Search_2D/Dijkstra.py | 2 +- .../Search_2D/LPAstar.py | 2 +- .../Search_2D/LRTAstar.py | 2 +- .../Search_2D/PotentialField.py | 2 +- .../Search_2D/RTAAstar.py | 2 +- .../Search_2D/__pycache__/env.cpython-37.pyc | Bin .../__pycache__/plotting.cpython-37.pyc | Bin .../__pycache__/queue.cpython-37.pyc | Bin .../Search_2D/bfs.py | 2 +- .../Search_2D/dfs.py | 2 +- .../Search_2D/env.py | 0 .../Search_2D/plotting.py | 2 +- .../Search_2D/queue.py | 0 .../Search_3D/Anytime_Dstar3D.py | 2 +- .../Search_3D/Astar3D.py | 2 +- .../Search_3D/Dstar3D.py | 2 +- .../Search_3D/DstarLite3D.py | 2 +- .../Search_3D/LP_Astar3D.py | 2 +- .../Search_3D/LRT_Astar3D.py | 2 +- .../Search_3D/RTA_Astar3D.py | 2 +- .../__pycache__/Astar3D.cpython-37.pyc | Bin .../__pycache__/env3D.cpython-37.pyc | Bin .../__pycache__/plot_util3D.cpython-37.pyc | Bin .../__pycache__/queue.cpython-37.pyc | Bin .../__pycache__/utils3D.cpython-37.pyc | Bin .../Search_3D/bidirectional_Astar3D.py | 2 +- .../Search_3D/env3D.py | 358 ++++----- .../Search_3D/plot_util3D.py | 346 ++++----- .../Search_3D/queue.py | 0 .../Search_3D/utils3D.py | 0 .../__pycache__/env.cpython-37.pyc | Bin .../__pycache__/plotting.cpython-35.pyc | Bin .../__pycache__/plotting.cpython-37.pyc | Bin .../__pycache__/queue.cpython-35.pyc | Bin .../__pycache__/queue.cpython-37.pyc | Bin .../gif/ADstar_sig.gif | Bin .../gif/ADstar_small.gif | Bin .../gif/ARA_star.gif | Bin .../gif/Astar.gif | Bin .../gif/BF.gif | Bin .../gif/BFS.gif | Bin .../gif/Bi-Astar.gif | Bin .../gif/DFS.gif | Bin .../gif/D_star.gif | Bin .../gif/D_star_Lite.gif | Bin .../gif/Dijkstra.gif | Bin .../gif/LPA_star.gif | Bin .../gif/LPAstar.gif | Bin .../gif/LRTA_star.gif | Bin .../gif/RTAA_star.gif | Bin .../gif/RepeatedA_star.gif | Bin 110 files changed, 1077 insertions(+), 1731 deletions(-) delete mode 100644 Sampling-based Planning/rrt_2D/CurvesGenerator/reeds_shepp.py rename {Sampling-based Planning => Sampling_based_Planning}/.idea/.gitignore (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/Sampling-based Planning.iml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/dictionaries/zhou.xml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/inspectionProfiles/profiles_settings.xml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/misc.xml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/modules.xml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/.idea/vcs.xml (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/Dynamic_RRT_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/Extended_RRT_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/Goal_biasd_RRT_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/INFORMED_RRT_STAR_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/RRT_CONNECT_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/RRT_STAR2_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/RRT_STAR_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/gif/RRT_STAR_SMART_2D.gif (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/__pycache__/env.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/__pycache__/plotting.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/__pycache__/queue.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/__pycache__/rrt.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/__pycache__/utils.cpython-37.pyc (100%) create mode 100644 Sampling_based_Planning/rrt_2D/dubins_rrt_star.py rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/dynamic_rrt.py (98%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/env.py (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/extended_rrt.py (98%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/informed_rrt_star.py (98%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/optimal_bi_rrt.py (93%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/plotting.py (97%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/queue.py (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/rrt.py (96%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/rrt_connect.py (97%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/rrt_star.py (97%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/rrt_star_smart.py (98%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/rs_rrt_star.py (93%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_2D/utils.py (96%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/__pycache__/env3D.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/__pycache__/rrt3D.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/__pycache__/utils3D.cpython-37.pyc (100%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/dynamic_rrt3D.py (99%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/env3D.py (97%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/plot_util3D.py (97%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/rrt3D.py (98%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/rrtstar3D.py (99%) rename {Sampling-based Planning => Sampling_based_Planning}/rrt_3D/utils3D.py (96%) rename {Search-based Planning => Search_based_Planning}/.idea/Search-based Planning.iml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/dictionaries/Huiming_Zhou.xml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/inspectionProfiles/profiles_settings.xml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/misc.xml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/modules.xml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/vcs.xml (100%) rename {Search-based Planning => Search_based_Planning}/.idea/workspace.xml (100%) rename {Search-based Planning => Search_based_Planning}/Search_2D/ARAstar.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/Anytime_D_star.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/Astar.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/Best_First.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/Bidirectional_a_star.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/D_star.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/D_star_Lite.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/Dijkstra.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/LPAstar.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/LRTAstar.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/PotentialField.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/RTAAstar.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_2D/__pycache__/env.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_2D/__pycache__/plotting.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_2D/__pycache__/queue.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_2D/bfs.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/dfs.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/env.py (100%) rename {Search-based Planning => Search_based_Planning}/Search_2D/plotting.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_2D/queue.py (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/Anytime_Dstar3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/Astar3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/Dstar3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/DstarLite3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/LP_Astar3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/LRT_Astar3D.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_3D/RTA_Astar3D.py (98%) rename {Search-based Planning => Search_based_Planning}/Search_3D/__pycache__/Astar3D.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/__pycache__/env3D.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/__pycache__/plot_util3D.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/__pycache__/queue.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/__pycache__/utils3D.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/bidirectional_Astar3D.py (99%) rename {Search-based Planning => Search_based_Planning}/Search_3D/env3D.py (97%) rename {Search-based Planning => Search_based_Planning}/Search_3D/plot_util3D.py (97%) rename {Search-based Planning => Search_based_Planning}/Search_3D/queue.py (100%) rename {Search-based Planning => Search_based_Planning}/Search_3D/utils3D.py (100%) rename {Search-based Planning => Search_based_Planning}/__pycache__/env.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/__pycache__/plotting.cpython-35.pyc (100%) rename {Search-based Planning => Search_based_Planning}/__pycache__/plotting.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/__pycache__/queue.cpython-35.pyc (100%) rename {Search-based Planning => Search_based_Planning}/__pycache__/queue.cpython-37.pyc (100%) rename {Search-based Planning => Search_based_Planning}/gif/ADstar_sig.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/ADstar_small.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/ARA_star.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/Astar.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/BF.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/BFS.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/Bi-Astar.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/DFS.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/D_star.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/D_star_Lite.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/Dijkstra.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/LPA_star.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/LPAstar.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/LRTA_star.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/RTAA_star.gif (100%) rename {Search-based Planning => Search_based_Planning}/gif/RepeatedA_star.gif (100%) diff --git a/Sampling-based Planning/rrt_2D/CurvesGenerator/reeds_shepp.py b/Sampling-based Planning/rrt_2D/CurvesGenerator/reeds_shepp.py deleted file mode 100644 index b83ade2..0000000 --- a/Sampling-based Planning/rrt_2D/CurvesGenerator/reeds_shepp.py +++ /dev/null @@ -1,687 +0,0 @@ -import time -import math -import numpy as np - - -# parameters initiation -STEP_SIZE = 0.2 -MAX_LENGTH = 1000.0 -PI = math.pi - - -# class for PATH element -class PATH: - def __init__(self, lengths, ctypes, L, x, y, yaw, directions): - self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float] - self.ctypes = ctypes # type of each part of the path [string] - self.L = L # total path length [float] - self.x = x # final x positions [m] - self.y = y # final y positions [m] - self.yaw = yaw # final yaw angles [rad] - self.directions = directions # forward: 1, backward:-1 - - -def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): - paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size) - - minL = paths[0].L - mini = 0 - - for i in range(len(paths)): - if paths[i].L <= minL: - minL, mini = paths[i].L, i - - return paths[mini] - - -def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): - q0 = [sx, sy, syaw] - q1 = [gx, gy, gyaw] - - paths = generate_path(q0, q1, maxc) - - for path in paths: - x, y, yaw, directions = \ - generate_local_course(path.L, path.lengths, - path.ctypes, maxc, step_size * maxc) - - # convert global coordinate - path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)] - path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)] - path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw] - path.directions = directions - path.lengths = [l / maxc for l in path.lengths] - path.L = path.L / maxc - - return paths - - -def set_path(paths, lengths, ctypes): - path = PATH([], [], 0.0, [], [], [], []) - path.ctypes = ctypes - path.lengths = lengths - - # check same path exist - for path_e in paths: - if path_e.ctypes == path.ctypes: - if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01: - return paths # not insert path - - path.L = sum([abs(i) for i in lengths]) - - if path.L >= MAX_LENGTH: - return paths - - assert path.L >= 0.01 - paths.append(path) - - return paths - - -def LSL(x, y, phi): - u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) - - if t >= 0.0: - v = M(phi - t) - if v >= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def LSR(x, y, phi): - u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi)) - u1 = u1 ** 2 - - if u1 >= 4.0: - u = math.sqrt(u1 - 4.0) - theta = math.atan2(2.0, u) - t = M(t1 + theta) - v = M(t - phi) - - if t >= 0.0 and v >= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def LRL(x, y, phi): - u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) - - if u1 <= 4.0: - u = -2.0 * math.asin(0.25 * u1) - t = M(t1 + 0.5 * u + PI) - v = M(phi - t + u) - - if t >= 0.0 and u <= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def SCS(x, y, phi, paths): - flag, t, u, v = SLS(x, y, phi) - - if flag: - paths = set_path(paths, [t, u, v], ["S", "L", "S"]) - - flag, t, u, v = SLS(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, v], ["S", "R", "S"]) - - return paths - - -def SLS(x, y, phi): - phi = M(phi) - - if y > 0.0 and 0.0 < phi < PI * 0.99: - xd = -y / math.tan(phi) + x - t = xd - math.tan(phi / 2.0) - u = phi - v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) - return True, t, u, v - elif y < 0.0 and 0.0 < phi < PI * 0.99: - xd = -y / math.tan(phi) + x - t = xd - math.tan(phi / 2.0) - u = phi - v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def CSC(x, y, phi, paths): - flag, t, u, v = LSL(x, y, phi) - if flag: - paths = set_path(paths, [t, u, v], ["L", "S", "L"]) - - flag, t, u, v = LSL(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"]) - - flag, t, u, v = LSL(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, v], ["R", "S", "R"]) - - flag, t, u, v = LSL(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"]) - - flag, t, u, v = LSR(x, y, phi) - if flag: - paths = set_path(paths, [t, u, v], ["L", "S", "R"]) - - flag, t, u, v = LSR(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"]) - - flag, t, u, v = LSR(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, v], ["R", "S", "L"]) - - flag, t, u, v = LSR(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"]) - - return paths - - -def CCC(x, y, phi, paths): - flag, t, u, v = LRL(x, y, phi) - if flag: - paths = set_path(paths, [t, u, v], ["L", "R", "L"]) - - flag, t, u, v = LRL(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"]) - - flag, t, u, v = LRL(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, v], ["R", "L", "R"]) - - flag, t, u, v = LRL(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"]) - - # backwards - xb = x * math.cos(phi) + y * math.sin(phi) - yb = x * math.sin(phi) - y * math.cos(phi) - - flag, t, u, v = LRL(xb, yb, phi) - if flag: - paths = set_path(paths, [v, u, t], ["L", "R", "L"]) - - flag, t, u, v = LRL(-xb, yb, -phi) - if flag: - paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"]) - - flag, t, u, v = LRL(xb, -yb, -phi) - if flag: - paths = set_path(paths, [v, u, t], ["R", "L", "R"]) - - flag, t, u, v = LRL(-xb, -yb, phi) - if flag: - paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"]) - - return paths - - -def calc_tauOmega(u, v, xi, eta, phi): - delta = M(u - v) - A = math.sin(u) - math.sin(delta) - B = math.cos(u) - math.cos(delta) - 1.0 - - t1 = math.atan2(eta * A - xi * B, xi * A + eta * B) - t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0 - - if t2 < 0: - tau = M(t1 + PI) - else: - tau = M(t1) - - omega = M(tau - u + v - phi) - - return tau, omega - - -def LRLRn(x, y, phi): - xi = x + math.sin(phi) - eta = y - 1.0 - math.cos(phi) - rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta)) - - if rho <= 1.0: - u = math.acos(rho) - t, v = calc_tauOmega(u, -u, xi, eta, phi) - if t >= 0.0 and v <= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def LRLRp(x, y, phi): - xi = x + math.sin(phi) - eta = y - 1.0 - math.cos(phi) - rho = (20.0 - xi * xi - eta * eta) / 16.0 - - if 0.0 <= rho <= 1.0: - u = -math.acos(rho) - if u >= -0.5 * PI: - t, v = calc_tauOmega(u, u, xi, eta, phi) - if t >= 0.0 and v >= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def CCCC(x, y, phi, paths): - flag, t, u, v = LRLRn(x, y, phi) - if flag: - paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"]) - - flag, t, u, v = LRLRn(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"]) - - flag, t, u, v = LRLRn(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"]) - - flag, t, u, v = LRLRn(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"]) - - flag, t, u, v = LRLRp(x, y, phi) - if flag: - paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"]) - - flag, t, u, v = LRLRp(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"]) - - flag, t, u, v = LRLRp(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"]) - - flag, t, u, v = LRLRp(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"]) - - return paths - - -def LRSR(x, y, phi): - xi = x + math.sin(phi) - eta = y - 1.0 - math.cos(phi) - rho, theta = R(-eta, xi) - - if rho >= 2.0: - t = theta - u = 2.0 - rho - v = M(t + 0.5 * PI - phi) - if t >= 0.0 and u <= 0.0 and v <= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def LRSL(x, y, phi): - xi = x - math.sin(phi) - eta = y - 1.0 + math.cos(phi) - rho, theta = R(xi, eta) - - if rho >= 2.0: - r = math.sqrt(rho * rho - 4.0) - u = 2.0 - r - t = M(theta + math.atan2(r, -2.0)) - v = M(phi - 0.5 * PI - t) - if t >= 0.0 and u <= 0.0 and v <= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def CCSC(x, y, phi, paths): - flag, t, u, v = LRSL(x, y, phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"]) - - flag, t, u, v = LRSL(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"]) - - flag, t, u, v = LRSL(x, -y, -phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"]) - - flag, t, u, v = LRSL(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"]) - - flag, t, u, v = LRSR(x, y, phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"]) - - flag, t, u, v = LRSR(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"]) - - flag, t, u, v = LRSR(x, -y, -phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"]) - - flag, t, u, v = LRSR(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"]) - - # backwards - xb = x * math.cos(phi) + y * math.sin(phi) - yb = x * math.sin(phi) - y * math.cos(phi) - - flag, t, u, v = LRSL(xb, yb, phi) - if flag: - paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"]) - - flag, t, u, v = LRSL(-xb, yb, -phi) - if flag: - paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"]) - - flag, t, u, v = LRSL(xb, -yb, -phi) - if flag: - paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"]) - - flag, t, u, v = LRSL(-xb, -yb, phi) - if flag: - paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"]) - - flag, t, u, v = LRSR(xb, yb, phi) - if flag: - paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"]) - - flag, t, u, v = LRSR(-xb, yb, -phi) - if flag: - paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"]) - - flag, t, u, v = LRSR(xb, -yb, -phi) - if flag: - paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"]) - - flag, t, u, v = LRSR(-xb, -yb, phi) - if flag: - paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"]) - - return paths - - -def LRSLR(x, y, phi): - # formula 8.11 *** TYPO IN PAPER *** - xi = x + math.sin(phi) - eta = y - 1.0 - math.cos(phi) - rho, theta = R(xi, eta) - - if rho >= 2.0: - u = 4.0 - math.sqrt(rho * rho - 4.0) - if u <= 0.0: - t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta)) - v = M(t - phi) - - if t >= 0.0 and v >= 0.0: - return True, t, u, v - - return False, 0.0, 0.0, 0.0 - - -def CCSCC(x, y, phi, paths): - flag, t, u, v = LRSLR(x, y, phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"]) - - flag, t, u, v = LRSLR(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"]) - - flag, t, u, v = LRSLR(x, -y, -phi) - if flag: - paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"]) - - flag, t, u, v = LRSLR(-x, -y, phi) - if flag: - paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"]) - - return paths - - -def generate_local_course(L, lengths, mode, maxc, step_size): - point_num = int(L / step_size) + len(lengths) + 3 - - px = [0.0 for _ in range(point_num)] - py = [0.0 for _ in range(point_num)] - pyaw = [0.0 for _ in range(point_num)] - directions = [0 for _ in range(point_num)] - ind = 1 - - if lengths[0] > 0.0: - directions[0] = 1 - else: - directions[0] = -1 - - if lengths[0] > 0.0: - d = step_size - else: - d = -step_size - - pd = d - ll = 0.0 - - for m, l, i in zip(mode, lengths, range(len(mode))): - if l > 0.0: - d = step_size - else: - d = -step_size - - ox, oy, oyaw = px[ind], py[ind], pyaw[ind] - - ind -= 1 - if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: - pd = -d - ll - else: - pd = d - ll - - while abs(pd) <= abs(l): - ind += 1 - px, py, pyaw, directions = \ - interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) - pd += d - - ll = l - pd - d # calc remain length - - ind += 1 - px, py, pyaw, directions = \ - interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) - - # remove unused data - while px[-1] == 0.0: - px.pop() - py.pop() - pyaw.pop() - directions.pop() - - return px, py, pyaw, directions - - -def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions): - if m == "S": - px[ind] = ox + l / maxc * math.cos(oyaw) - py[ind] = oy + l / maxc * math.sin(oyaw) - pyaw[ind] = oyaw - else: - ldx = math.sin(l) / maxc - if m == "L": - ldy = (1.0 - math.cos(l)) / maxc - elif m == "R": - ldy = (1.0 - math.cos(l)) / (-maxc) - - gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy - gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy - px[ind] = ox + gdx - py[ind] = oy + gdy - - if m == "L": - pyaw[ind] = oyaw + l - elif m == "R": - pyaw[ind] = oyaw - l - - if l > 0.0: - directions[ind] = 1 - else: - directions[ind] = -1 - - return px, py, pyaw, directions - - -def generate_path(q0, q1, maxc): - dx = q1[0] - q0[0] - dy = q1[1] - q0[1] - dth = q1[2] - q0[2] - c = math.cos(q0[2]) - s = math.sin(q0[2]) - x = (c * dx + s * dy) * maxc - y = (-s * dx + c * dy) * maxc - - paths = [] - paths = SCS(x, y, dth, paths) - paths = CSC(x, y, dth, paths) - paths = CCC(x, y, dth, paths) - paths = CCCC(x, y, dth, paths) - paths = CCSC(x, y, dth, paths) - paths = CCSCC(x, y, dth, paths) - - return paths - - -# utils -def pi_2_pi(theta): - while theta > PI: - theta -= 2.0 * PI - - while theta < -PI: - theta += 2.0 * PI - - return theta - - -def R(x, y): - """ - Return the polar coordinates (r, theta) of the point (x, y) - """ - r = math.hypot(x, y) - theta = math.atan2(y, x) - - return r, theta - - -def M(theta): - """ - Regulate theta to -pi <= theta < pi - """ - phi = theta % (2.0 * PI) - - if phi < -PI: - phi += 2.0 * PI - if phi > PI: - phi -= 2.0 * PI - - return phi - - -def get_label(path): - label = "" - - for m, l in zip(path.ctypes, path.lengths): - label = label + m - if l > 0.0: - label = label + "+" - else: - label = label + "-" - - return label - - -def calc_curvature(x, y, yaw, directions): - c, ds = [], [] - - for i in range(1, len(x) - 1): - dxn = x[i] - x[i - 1] - dxp = x[i + 1] - x[i] - dyn = y[i] - y[i - 1] - dyp = y[i + 1] - y[i] - dn = math.hypot(dxn, dyn) - dp = math.hypot(dxp, dyp) - dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp) - ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn) - dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp) - ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn) - curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) - d = (dn + dp) / 2.0 - - if np.isnan(curvature): - curvature = 0.0 - - if directions[i] <= 0.0: - curvature = -curvature - - if len(c) == 0: - ds.append(d) - c.append(curvature) - - ds.append(d) - c.append(curvature) - - ds.append(ds[-1]) - c.append(c[-1]) - - return c, ds - - -def check_path(sx, sy, syaw, gx, gy, gyaw, maxc): - paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc) - - assert len(paths) >= 1 - - for path in paths: - assert abs(path.x[0] - sx) <= 0.01 - assert abs(path.y[0] - sy) <= 0.01 - assert abs(path.yaw[0] - syaw) <= 0.01 - assert abs(path.x[-1] - gx) <= 0.01 - assert abs(path.y[-1] - gy) <= 0.01 - assert abs(path.yaw[-1] - gyaw) <= 0.01 - - # course distance check - d = [math.hypot(dx, dy) - for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]), - np.diff(path.y[0:len(path.y) - 1]))] - - for i in range(len(d)): - assert abs(d[i] - STEP_SIZE) <= 0.001 - - -def main(): - start_x = 3.0 # [m] - start_y = 10.0 # [m] - start_yaw = np.deg2rad(40.0) # [rad] - end_x = 0.0 # [m] - end_y = 1.0 # [m] - end_yaw = np.deg2rad(0.0) # [rad] - max_curvature = 0.1 - - t0 = time.time() - - for i in range(1000): - _ = calc_optimal_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature) - - t1 = time.time() - print(t1 - t0) - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/Sampling-based Planning/.idea/.gitignore b/Sampling_based_Planning/.idea/.gitignore similarity index 100% rename from Sampling-based Planning/.idea/.gitignore rename to Sampling_based_Planning/.idea/.gitignore diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling_based_Planning/.idea/Sampling-based Planning.iml similarity index 100% rename from Sampling-based Planning/.idea/Sampling-based Planning.iml rename to Sampling_based_Planning/.idea/Sampling-based Planning.iml diff --git a/Sampling-based Planning/.idea/dictionaries/zhou.xml b/Sampling_based_Planning/.idea/dictionaries/zhou.xml similarity index 100% rename from Sampling-based Planning/.idea/dictionaries/zhou.xml rename to Sampling_based_Planning/.idea/dictionaries/zhou.xml diff --git a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml similarity index 100% rename from Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml rename to Sampling_based_Planning/.idea/inspectionProfiles/profiles_settings.xml diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling_based_Planning/.idea/misc.xml similarity index 100% rename from Sampling-based Planning/.idea/misc.xml rename to Sampling_based_Planning/.idea/misc.xml diff --git a/Sampling-based Planning/.idea/modules.xml b/Sampling_based_Planning/.idea/modules.xml similarity index 100% rename from Sampling-based Planning/.idea/modules.xml rename to Sampling_based_Planning/.idea/modules.xml diff --git a/Sampling-based Planning/.idea/vcs.xml b/Sampling_based_Planning/.idea/vcs.xml similarity index 100% rename from Sampling-based Planning/.idea/vcs.xml rename to Sampling_based_Planning/.idea/vcs.xml diff --git a/Sampling-based Planning/gif/Dynamic_RRT_2D.gif b/Sampling_based_Planning/gif/Dynamic_RRT_2D.gif similarity index 100% rename from Sampling-based Planning/gif/Dynamic_RRT_2D.gif rename to Sampling_based_Planning/gif/Dynamic_RRT_2D.gif diff --git a/Sampling-based Planning/gif/Extended_RRT_2D.gif b/Sampling_based_Planning/gif/Extended_RRT_2D.gif similarity index 100% rename from Sampling-based Planning/gif/Extended_RRT_2D.gif rename to Sampling_based_Planning/gif/Extended_RRT_2D.gif diff --git a/Sampling-based Planning/gif/Goal_biasd_RRT_2D.gif b/Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif similarity index 100% rename from Sampling-based Planning/gif/Goal_biasd_RRT_2D.gif rename to Sampling_based_Planning/gif/Goal_biasd_RRT_2D.gif diff --git a/Sampling-based Planning/gif/INFORMED_RRT_STAR_2D.gif b/Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif similarity index 100% rename from Sampling-based Planning/gif/INFORMED_RRT_STAR_2D.gif rename to Sampling_based_Planning/gif/INFORMED_RRT_STAR_2D.gif diff --git a/Sampling-based Planning/gif/RRT_CONNECT_2D.gif b/Sampling_based_Planning/gif/RRT_CONNECT_2D.gif similarity index 100% rename from Sampling-based Planning/gif/RRT_CONNECT_2D.gif rename to Sampling_based_Planning/gif/RRT_CONNECT_2D.gif diff --git a/Sampling-based Planning/gif/RRT_STAR2_2D.gif b/Sampling_based_Planning/gif/RRT_STAR2_2D.gif similarity index 100% rename from Sampling-based Planning/gif/RRT_STAR2_2D.gif rename to Sampling_based_Planning/gif/RRT_STAR2_2D.gif diff --git a/Sampling-based Planning/gif/RRT_STAR_2D.gif b/Sampling_based_Planning/gif/RRT_STAR_2D.gif similarity index 100% rename from Sampling-based Planning/gif/RRT_STAR_2D.gif rename to Sampling_based_Planning/gif/RRT_STAR_2D.gif diff --git a/Sampling-based Planning/gif/RRT_STAR_SMART_2D.gif b/Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif similarity index 100% rename from Sampling-based Planning/gif/RRT_STAR_SMART_2D.gif rename to Sampling_based_Planning/gif/RRT_STAR_SMART_2D.gif diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc rename to Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc rename to Sampling_based_Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_2D/__pycache__/queue.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_2D/__pycache__/queue.cpython-37.pyc rename to Sampling_based_Planning/rrt_2D/__pycache__/queue.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc rename to Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc rename to Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc diff --git a/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py b/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py new file mode 100644 index 0000000..08337b7 --- /dev/null +++ b/Sampling_based_Planning/rrt_2D/dubins_rrt_star.py @@ -0,0 +1,57 @@ +""" +DUBINS_RRT_STAR 2D +@author: huiming zhou +""" + +import os +import sys +import math +import random +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +from scipy.spatial.transform import Rotation as Rot + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../../Sampling_based_Planning/") + +from Sampling_based_Planning.rrt_2D import env, plotting, utils +import CurvesGenerator.dubins_path as dubins + + +class Node: + def __init__(self, x, y, yaw): + self.x = x + self.y = y + self.yaw = yaw + self.path_x = [] + self.path_y = [] + self.paty_yaw = [] + self.parent = None + self.cost = 0.0 + + +class RrtStarSmart: + def __init__(self, sx, sy, syaw, gx, gy, gyaw, step_len, + goal_sample_rate, search_radius, iter_max): + self.s_start = Node(sx, sy, syaw) + self.s_goal = Node(gx, gy, gyaw) + self.step_len = step_len + self.goal_sample_rate = goal_sample_rate + self.search_radius = search_radius + self.iter_max = iter_max + self.curv = 1.0 + + self.env = env.Env() + self.utils = utils.Utils() + + self.fig, self.ax = plt.subplots() + self.delta = self.utils.delta + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.V = [self.s_start] + self.path = None diff --git a/Sampling-based Planning/rrt_2D/dynamic_rrt.py b/Sampling_based_Planning/rrt_2D/dynamic_rrt.py similarity index 98% rename from Sampling-based Planning/rrt_2D/dynamic_rrt.py rename to Sampling_based_Planning/rrt_2D/dynamic_rrt.py index 1eef63d..e365971 100644 --- a/Sampling-based Planning/rrt_2D/dynamic_rrt.py +++ b/Sampling_based_Planning/rrt_2D/dynamic_rrt.py @@ -12,11 +12,9 @@ import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/env.py b/Sampling_based_Planning/rrt_2D/env.py similarity index 100% rename from Sampling-based Planning/rrt_2D/env.py rename to Sampling_based_Planning/rrt_2D/env.py diff --git a/Sampling-based Planning/rrt_2D/extended_rrt.py b/Sampling_based_Planning/rrt_2D/extended_rrt.py similarity index 98% rename from Sampling-based Planning/rrt_2D/extended_rrt.py rename to Sampling_based_Planning/rrt_2D/extended_rrt.py index 12c6a4e..746e365 100644 --- a/Sampling-based Planning/rrt_2D/extended_rrt.py +++ b/Sampling_based_Planning/rrt_2D/extended_rrt.py @@ -11,11 +11,9 @@ import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/informed_rrt_star.py b/Sampling_based_Planning/rrt_2D/informed_rrt_star.py similarity index 98% rename from Sampling-based Planning/rrt_2D/informed_rrt_star.py rename to Sampling_based_Planning/rrt_2D/informed_rrt_star.py index bbf64fb..20c244c 100644 --- a/Sampling-based Planning/rrt_2D/informed_rrt_star.py +++ b/Sampling_based_Planning/rrt_2D/informed_rrt_star.py @@ -13,11 +13,9 @@ from scipy.spatial.transform import Rotation as Rot import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py b/Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py similarity index 93% rename from Sampling-based Planning/rrt_2D/optimal_bi_rrt.py rename to Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py index 3aa2b41..59630fb 100644 --- a/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py +++ b/Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py @@ -11,12 +11,9 @@ import matplotlib.pyplot as plt import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils -from rrt_2D import queue +from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue class Node: @@ -70,10 +67,6 @@ class OBiRrt: L_near.sort() - for c_near, x_near, sigma_near in L_near: - if c_near + self.cost_to_go() - - def cost_to_go(self, x_start, x_goal): return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling_based_Planning/rrt_2D/plotting.py similarity index 97% rename from Sampling-based Planning/rrt_2D/plotting.py rename to Sampling_based_Planning/rrt_2D/plotting.py index f4e4eb5..1df752b 100644 --- a/Sampling-based Planning/rrt_2D/plotting.py +++ b/Sampling_based_Planning/rrt_2D/plotting.py @@ -9,9 +9,9 @@ import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env +from Sampling_based_Planning.rrt_2D import env class Plotting: diff --git a/Sampling-based Planning/rrt_2D/queue.py b/Sampling_based_Planning/rrt_2D/queue.py similarity index 100% rename from Sampling-based Planning/rrt_2D/queue.py rename to Sampling_based_Planning/rrt_2D/queue.py diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling_based_Planning/rrt_2D/rrt.py similarity index 96% rename from Sampling-based Planning/rrt_2D/rrt.py rename to Sampling_based_Planning/rrt_2D/rrt.py index d7a0269..e8f0f54 100644 --- a/Sampling-based Planning/rrt_2D/rrt.py +++ b/Sampling_based_Planning/rrt_2D/rrt.py @@ -9,11 +9,9 @@ import math import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/rrt_connect.py b/Sampling_based_Planning/rrt_2D/rrt_connect.py similarity index 97% rename from Sampling-based Planning/rrt_2D/rrt_connect.py rename to Sampling_based_Planning/rrt_2D/rrt_connect.py index eb7c033..24a8a99 100644 --- a/Sampling-based Planning/rrt_2D/rrt_connect.py +++ b/Sampling_based_Planning/rrt_2D/rrt_connect.py @@ -11,11 +11,9 @@ import numpy as np import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling_based_Planning/rrt_2D/rrt_star.py similarity index 97% rename from Sampling-based Planning/rrt_2D/rrt_star.py rename to Sampling_based_Planning/rrt_2D/rrt_star.py index 0f6b851..bc84c84 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star.py +++ b/Sampling_based_Planning/rrt_2D/rrt_star.py @@ -9,12 +9,9 @@ import math import numpy as np sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils -from rrt_2D import queue +from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue class Node: diff --git a/Sampling-based Planning/rrt_2D/rrt_star_smart.py b/Sampling_based_Planning/rrt_2D/rrt_star_smart.py similarity index 98% rename from Sampling-based Planning/rrt_2D/rrt_star_smart.py rename to Sampling_based_Planning/rrt_2D/rrt_star_smart.py index 85d8c14..a847fc4 100644 --- a/Sampling-based Planning/rrt_2D/rrt_star_smart.py +++ b/Sampling_based_Planning/rrt_2D/rrt_star_smart.py @@ -9,15 +9,13 @@ import math import random import numpy as np import matplotlib.pyplot as plt -from scipy.spatial.transform import Rotation as Rot import matplotlib.patches as patches +from scipy.spatial.transform import Rotation as Rot sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils +from Sampling_based_Planning.rrt_2D import env, plotting, utils class Node: diff --git a/Sampling-based Planning/rrt_2D/rs_rrt_star.py b/Sampling_based_Planning/rrt_2D/rs_rrt_star.py similarity index 93% rename from Sampling-based Planning/rrt_2D/rs_rrt_star.py rename to Sampling_based_Planning/rrt_2D/rs_rrt_star.py index d1d90d6..fbbe5ff 100644 --- a/Sampling-based Planning/rrt_2D/rs_rrt_star.py +++ b/Sampling_based_Planning/rrt_2D/rs_rrt_star.py @@ -13,12 +13,10 @@ from scipy.spatial.transform import Rotation as Rot import matplotlib.patches as patches sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D import plotting -from rrt_2D import utils -import rrt_2D.CurvesGenerator.reeds_shepp as rs +from Sampling_based_Planning.rrt_2D import env, plotting, utils +import CurvesGenerator.reeds_shepp as rs class Node: diff --git a/Sampling-based Planning/rrt_2D/utils.py b/Sampling_based_Planning/rrt_2D/utils.py similarity index 96% rename from Sampling-based Planning/rrt_2D/utils.py rename to Sampling_based_Planning/rrt_2D/utils.py index ca230a6..62dacb0 100644 --- a/Sampling-based Planning/rrt_2D/utils.py +++ b/Sampling_based_Planning/rrt_2D/utils.py @@ -9,10 +9,10 @@ import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling-based Planning/") + "/../../Sampling_based_Planning/") -from rrt_2D import env -from rrt_2D.rrt import Node +from Sampling_based_Planning.rrt_2D import env +from Sampling_based_Planning.rrt_2D.rrt import Node class Utils: diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc rename to Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc rename to Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc rename to Sampling_based_Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc similarity index 100% rename from Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc rename to Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py similarity index 99% rename from Sampling-based Planning/rrt_3D/dynamic_rrt3D.py rename to Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py index c1654aa..a9a99a9 100644 --- a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py +++ b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, isinbound from rrt_3D.rrt3D import rrt diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling_based_Planning/rrt_3D/env3D.py similarity index 97% rename from Sampling-based Planning/rrt_3D/env3D.py rename to Sampling_based_Planning/rrt_3D/env3D.py index 1204413..36d4dab 100644 --- a/Sampling-based Planning/rrt_3D/env3D.py +++ b/Sampling_based_Planning/rrt_3D/env3D.py @@ -1,164 +1,164 @@ -# this is the three dimensional configuration space for rrt -# !/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: yue qi -""" -import numpy as np -# from utils3D import OBB2AABB - -def R_matrix(z_angle,y_angle,x_angle): - # x angle: row; y angle: pitch; z angle: yaw - # generate rotation matrix in SO3 - # RzRyRx = R, ZYX intrinsic rotation - # also (r1,r2,r3) in R3*3 in {W} frame - # used in obb.O - # [[R p] - # [0T 1]] gives transformation from body to world - return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ - np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ - np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) - -def getblocks(): - # AABBs - block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], - [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], - #[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], - #[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], - #[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], - [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], - [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] - Obstacles = [] - for i in block: - i = np.array(i) - Obstacles.append([j for j in i]) - return np.array(Obstacles) - -def getAABB(blocks): - # used for Pyrr package for detecting collision - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - -class aabb(object): - # make AABB out of blocks, - # P: center point - # E: extents - # O: Rotation matrix in SO(3), in {w} - def __init__(self,AABB): - self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point - self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents - self.O = [[1,0,0],[0,1,0],[0,0,1]] - -class obb(object): - # P: center point - # E: extents - # O: Rotation matrix in SO(3), in {w} - def __init__(self, P, E, O): - self.P = P - self.E = E - self.O = O - self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) - -def getAABB2(blocks): - # used in lineAABB - AABB = [] - for i in blocks: - AABB.append(aabb(i)) - return AABB - -def getballs(): - spheres = [[16,2.5,4,2],[10,2.5,1,1]] - Obstacles = [] - for i in spheres: - Obstacles.append([j for j in i]) - return np.array(Obstacles) - -def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): - return block - -class env(): - def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): - self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) - self.blocks = getblocks() - self.AABB = getAABB2(self.blocks) - self.AABB_pyrr = getAABB(self.blocks) - self.balls = getballs() - self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))]) - #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) - #print(self.OBB) - # self.OBB = [] - self.start = np.array([0.5, 2.5, 5.5]) - self.goal = np.array([19.0, 2.5, 5.5]) - self.t = 0 # time - - def New_block(self): - newblock = add_block() - self.blocks = np.vstack([self.blocks,newblock]) - self.AABB = getAABB2(self.blocks) - self.AABB_pyrr = getAABB(self.blocks) - - def move_start(self, x): - self.start = x - - def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'): - # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, - # R is an orthorgonal transform in R3*3, is the rotation matrix - # (x',t') = (x + tv, t) is uniform transformation - if mode == 'uniform': - ori = np.array(self.blocks[block_to_move]) - self.blocks[block_to_move] = \ - np.array([ori[0] + self.t * v[0],\ - ori[1] + self.t * v[1],\ - ori[2] + self.t * v[2],\ - ori[3] + self.t * v[0],\ - ori[4] + self.t * v[1],\ - ori[5] + self.t * v[2]]) - - self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + self.t * v[0], \ - self.AABB[block_to_move].P[1] + self.t * v[1], \ - self.AABB[block_to_move].P[2] + self.t * v[2]] - # return a range of block that the block might moved - a = self.blocks[block_to_move] - # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ - # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ - # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ - # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) - return a,ori - # (x',t') = (x + a, t + s) is a translation - if mode == 'translation': - ori = np.array(self.blocks[block_to_move]) - self.blocks[block_to_move] = \ - np.array([ori[0] + a[0],\ - ori[1] + a[1],\ - ori[2] + a[2],\ - ori[3] + a[0],\ - ori[4] + a[1],\ - ori[5] + a[2]]) - - self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + a[0], \ - self.AABB[block_to_move].P[1] + a[1], \ - self.AABB[block_to_move].P[2] + a[2]] - self.t += s - # return a range of block that the block might moved - a = self.blocks[block_to_move] - return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ - a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ - np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ - ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) - # return a,ori - # (x',t') = (Rx, t) - if mode == 'rotation': # this makes an OBB rotate - ori = [self.OBB[obb_to_move]] - self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) - self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]]) - return self.OBB[obb_to_move], ori[0] - - - -if __name__ == '__main__': - newenv = env() +# this is the three dimensional configuration space for rrt +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: yue qi +""" +import numpy as np +# from utils3D import OBB2AABB + +def R_matrix(z_angle,y_angle,x_angle): + # x angle: row; y angle: pitch; z angle: yaw + # generate rotation matrix in SO3 + # RzRyRx = R, ZYX intrinsic rotation + # also (r1,r2,r3) in R3*3 in {W} frame + # used in obb.O + # [[R p] + # [0T 1]] gives transformation from body to world + return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ + np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ + np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) + +def getblocks(): + # AABBs + block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], + [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], + #[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], + #[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], + #[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], + [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], + [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] + Obstacles = [] + for i in block: + i = np.array(i) + Obstacles.append([j for j in i]) + return np.array(Obstacles) + +def getAABB(blocks): + # used for Pyrr package for detecting collision + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + +class aabb(object): + # make AABB out of blocks, + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self,AABB): + self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point + self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents + self.O = [[1,0,0],[0,1,0],[0,0,1]] + +class obb(object): + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self, P, E, O): + self.P = P + self.E = E + self.O = O + self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) + +def getAABB2(blocks): + # used in lineAABB + AABB = [] + for i in blocks: + AABB.append(aabb(i)) + return AABB + +def getballs(): + spheres = [[16,2.5,4,2],[10,2.5,1,1]] + Obstacles = [] + for i in spheres: + Obstacles.append([j for j in i]) + return np.array(Obstacles) + +def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): + return block + +class env(): + def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): + self.resolution = resolution + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + self.blocks = getblocks() + self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) + self.balls = getballs() + self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))]) + #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) + #print(self.OBB) + # self.OBB = [] + self.start = np.array([0.5, 2.5, 5.5]) + self.goal = np.array([19.0, 2.5, 5.5]) + self.t = 0 # time + + def New_block(self): + newblock = add_block() + self.blocks = np.vstack([self.blocks,newblock]) + self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) + + def move_start(self, x): + self.start = x + + def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'): + # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, + # R is an orthorgonal transform in R3*3, is the rotation matrix + # (x',t') = (x + tv, t) is uniform transformation + if mode == 'uniform': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + self.t * v[0],\ + ori[1] + self.t * v[1],\ + ori[2] + self.t * v[2],\ + ori[3] + self.t * v[0],\ + ori[4] + self.t * v[1],\ + ori[5] + self.t * v[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + self.t * v[0], \ + self.AABB[block_to_move].P[1] + self.t * v[1], \ + self.AABB[block_to_move].P[2] + self.t * v[2]] + # return a range of block that the block might moved + a = self.blocks[block_to_move] + # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ + # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ + # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + return a,ori + # (x',t') = (x + a, t + s) is a translation + if mode == 'translation': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + a[0],\ + ori[1] + a[1],\ + ori[2] + a[2],\ + ori[3] + a[0],\ + ori[4] + a[1],\ + ori[5] + a[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + a[0], \ + self.AABB[block_to_move].P[1] + a[1], \ + self.AABB[block_to_move].P[2] + a[2]] + self.t += s + # return a range of block that the block might moved + a = self.blocks[block_to_move] + return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ + a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ + np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + # return a,ori + # (x',t') = (Rx, t) + if mode == 'rotation': # this makes an OBB rotate + ori = [self.OBB[obb_to_move]] + self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) + self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]]) + return self.OBB[obb_to_move], ori[0] + + + +if __name__ == '__main__': + newenv = env() diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py similarity index 97% rename from Sampling-based Planning/rrt_3D/plot_util3D.py rename to Sampling_based_Planning/rrt_3D/plot_util3D.py index 5aea9a6..976ca0b 100644 --- a/Sampling-based Planning/rrt_3D/plot_util3D.py +++ b/Sampling_based_Planning/rrt_3D/plot_util3D.py @@ -1,184 +1,184 @@ -# plotting -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from mpl_toolkits.mplot3d.art3d import Poly3DCollection -import mpl_toolkits.mplot3d as plt3d -from mpl_toolkits.mplot3d import proj3d -import numpy as np - - -def CreateSphere(center, r): - u = np.linspace(0, 2 * np.pi, 30) - v = np.linspace(0, np.pi, 30) - x = np.outer(np.cos(u), np.sin(v)) - y = np.outer(np.sin(u), np.sin(v)) - z = np.outer(np.ones(np.size(u)), np.cos(v)) - x, y, z = r * x + center[0], r * y + center[1], r * z + center[2] - return (x, y, z) - - -def draw_Spheres(ax, balls): - for i in balls: - (xs, ys, zs) = CreateSphere(i[0:3], i[-1]) - ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b") - - -def draw_block_list(ax, blocks, color=None, alpha=0.15): - ''' - drawing the blocks on the graph - ''' - v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], - dtype='float') - f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) - n = blocks.shape[0] - d = blocks[:, 3:6] - blocks[:, :3] - vl = np.zeros((8 * n, 3)) - fl = np.zeros((6 * n, 4), dtype='int64') - for k in range(n): - vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] - fl[k * 6:(k + 1) * 6, :] = f + k * 8 - if type(ax) is Poly3DCollection: - ax.set_verts(vl[fl]) - else: - pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') - pc.set_facecolor(color) - h = ax.add_collection3d(pc) - return h - - -def obb_verts(obb): - # 0.017004013061523438 for 1000 iters - ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \ - [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]]) - # P + (ori * E) - ori_body = np.multiply(ori_body, obb.E) - # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) - verts = (obb.O @ ori_body.T).T + obb.P - return verts - - -def draw_obb(ax, OBB, color=None, alpha=0.15): - f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) - n = OBB.shape[0] - vl = np.zeros((8 * n, 3)) - fl = np.zeros((6 * n, 4), dtype='int64') - for k in range(n): - vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) - fl[k * 6:(k + 1) * 6, :] = f + k * 8 - if type(ax) is Poly3DCollection: - ax.set_verts(vl[fl]) - else: - pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') - pc.set_facecolor(color) - h = ax.add_collection3d(pc) - return h - - -def draw_line(ax, SET, visibility=1, color=None): - if SET != []: - for i in SET: - xs = i[0][0], i[1][0] - ys = i[0][1], i[1][1] - zs = i[0][2], i[1][2] - line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) - ax.add_line(line) - - -def visualization(initparams): - if initparams.ind % 100 == 0 or initparams.done: - V = np.array(list(initparams.V)) - E = initparams.E - Path = np.array(initparams.Path) - start = initparams.env.start - goal = initparams.env.goal - edges = E.get_edge() - # generate axis objects - ax = plt.subplot(111, projection='3d') - # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) - # ax.view_init(elev=0., azim=90.) - ax.view_init(elev=8., azim=120.) - # ax.view_init(elev=-8., azim=180) - ax.clear() - # drawing objects - draw_Spheres(ax, initparams.env.balls) - draw_block_list(ax, initparams.env.blocks) - if initparams.env.OBB is not None: - draw_obb(ax, initparams.env.OBB) - draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0) - draw_line(ax, edges, visibility=0.25) - draw_line(ax, Path, color='r') - if len(V) > 0: - ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) - ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') - ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') - # adjust the aspect ratio - xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] - ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] - zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] - dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin - ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) - plt.xlabel('x') - plt.ylabel('y') - plt.pause(0.0001) - - -def make_get_proj(self, rx, ry, rz): - ''' - Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that - makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. - ''' - - rm = max(rx, ry, rz) - kx = rm / rx; - ky = rm / ry; - kz = rm / rz - - # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are - # marked by ## - def get_proj(): - relev, razim = np.pi * self.elev / 180, np.pi * self.azim / 180 - - xmin, xmax = self.get_xlim3d() - ymin, ymax = self.get_ylim3d() - zmin, zmax = self.get_zlim3d() - - # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 - worldM = proj3d.world_transformation(xmin, xmax, - ymin, ymax, - zmin, zmax) - ratio = 0.5 - # adjust the aspect ratio ## - aspectM = proj3d.world_transformation(-kx + 1, kx, ## - -ky + 1, ky, ## - -kz + 1, kz) ## - - # look into the middle of the new coordinates - R = np.array([0.5, 0.5, 0.5]) - - xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist * ratio - yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist * ratio - zp = R[2] + np.sin(relev) * self.dist * ratio - E = np.array((xp, yp, zp)) - - self.eye = E - self.vvec = R - E - self.vvec = self.vvec / np.linalg.norm(self.vvec) - - if abs(relev) > np.pi / 2: - # upside down - V = np.array((0, 0, -1)) - else: - V = np.array((0, 0, 1)) - zfront, zback = -self.dist * ratio, self.dist * ratio - - viewM = proj3d.view_transformation(E, R, V) - perspM = proj3d.persp_transformation(zfront, zback) - M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## - M = np.dot(perspM, M0) - return M - - return get_proj - - -if __name__ == '__main__': - pass +# plotting +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import mpl_toolkits.mplot3d as plt3d +from mpl_toolkits.mplot3d import proj3d +import numpy as np + + +def CreateSphere(center, r): + u = np.linspace(0, 2 * np.pi, 30) + v = np.linspace(0, np.pi, 30) + x = np.outer(np.cos(u), np.sin(v)) + y = np.outer(np.sin(u), np.sin(v)) + z = np.outer(np.ones(np.size(u)), np.cos(v)) + x, y, z = r * x + center[0], r * y + center[1], r * z + center[2] + return (x, y, z) + + +def draw_Spheres(ax, balls): + for i in balls: + (xs, ys, zs) = CreateSphere(i[0:3], i[-1]) + ax.plot_wireframe(xs, ys, zs, alpha=0.15, color="b") + + +def draw_block_list(ax, blocks, color=None, alpha=0.15): + ''' + drawing the blocks on the graph + ''' + v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], + dtype='float') + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + n = blocks.shape[0] + d = blocks[:, 3:6] - blocks[:, :3] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') + pc.set_facecolor(color) + h = ax.add_collection3d(pc) + return h + + +def obb_verts(obb): + # 0.017004013061523438 for 1000 iters + ori_body = np.array([[1, 1, 1], [-1, 1, 1], [-1, -1, 1], [1, -1, 1], \ + [1, 1, -1], [-1, 1, -1], [-1, -1, -1], [1, -1, -1]]) + # P + (ori * E) + ori_body = np.multiply(ori_body, obb.E) + # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) + verts = (obb.O @ ori_body.T).T + obb.P + return verts + + +def draw_obb(ax, OBB, color=None, alpha=0.15): + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + n = OBB.shape[0] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') + pc.set_facecolor(color) + h = ax.add_collection3d(pc) + return h + + +def draw_line(ax, SET, visibility=1, color=None): + if SET != []: + for i in SET: + xs = i[0][0], i[1][0] + ys = i[0][1], i[1][1] + zs = i[0][2], i[1][2] + line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) + ax.add_line(line) + + +def visualization(initparams): + if initparams.ind % 100 == 0 or initparams.done: + V = np.array(list(initparams.V)) + E = initparams.E + Path = np.array(initparams.Path) + start = initparams.env.start + goal = initparams.env.goal + edges = E.get_edge() + # generate axis objects + ax = plt.subplot(111, projection='3d') + # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) + # ax.view_init(elev=0., azim=90.) + ax.view_init(elev=8., azim=120.) + # ax.view_init(elev=-8., azim=180) + ax.clear() + # drawing objects + draw_Spheres(ax, initparams.env.balls) + draw_block_list(ax, initparams.env.blocks) + if initparams.env.OBB is not None: + draw_obb(ax, initparams.env.OBB) + draw_block_list(ax, np.array([initparams.env.boundary]), alpha=0) + draw_line(ax, edges, visibility=0.25) + draw_line(ax, Path, color='r') + if len(V) > 0: + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) + ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') + ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') + # adjust the aspect ratio + xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] + ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] + zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] + dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin + ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) + plt.xlabel('x') + plt.ylabel('y') + plt.pause(0.0001) + + +def make_get_proj(self, rx, ry, rz): + ''' + Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that + makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. + ''' + + rm = max(rx, ry, rz) + kx = rm / rx; + ky = rm / ry; + kz = rm / rz + + # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are + # marked by ## + def get_proj(): + relev, razim = np.pi * self.elev / 180, np.pi * self.azim / 180 + + xmin, xmax = self.get_xlim3d() + ymin, ymax = self.get_ylim3d() + zmin, zmax = self.get_zlim3d() + + # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 + worldM = proj3d.world_transformation(xmin, xmax, + ymin, ymax, + zmin, zmax) + ratio = 0.5 + # adjust the aspect ratio ## + aspectM = proj3d.world_transformation(-kx + 1, kx, ## + -ky + 1, ky, ## + -kz + 1, kz) ## + + # look into the middle of the new coordinates + R = np.array([0.5, 0.5, 0.5]) + + xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist * ratio + yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist * ratio + zp = R[2] + np.sin(relev) * self.dist * ratio + E = np.array((xp, yp, zp)) + + self.eye = E + self.vvec = R - E + self.vvec = self.vvec / np.linalg.norm(self.vvec) + + if abs(relev) > np.pi / 2: + # upside down + V = np.array((0, 0, -1)) + else: + V = np.array((0, 0, 1)) + zfront, zback = -self.dist * ratio, self.dist * ratio + + viewM = proj3d.view_transformation(E, R, V) + perspM = proj3d.persp_transformation(zfront, zback) + M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## + M = np.dot(perspM, M0) + return M + + return get_proj + + +if __name__ == '__main__': + pass diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling_based_Planning/rrt_3D/rrt3D.py similarity index 98% rename from Sampling-based Planning/rrt_3D/rrt3D.py rename to Sampling_based_Planning/rrt_3D/rrt3D.py index c128965..1db1f37 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling_based_Planning/rrt_3D/rrt3D.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling_based_Planning/rrt_3D/rrtstar3D.py similarity index 99% rename from Sampling-based Planning/rrt_3D/rrtstar3D.py rename to Sampling_based_Planning/rrt_3D/rrtstar3D.py index 61b457c..0f43cfa 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling_based_Planning/rrt_3D/rrtstar3D.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling_based_Planning/rrt_3D/utils3D.py similarity index 96% rename from Sampling-based Planning/rrt_3D/utils3D.py rename to Sampling_based_Planning/rrt_3D/utils3D.py index 0adab8f..ec8bb2d 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling_based_Planning/rrt_3D/utils3D.py @@ -1,269 +1,269 @@ -import numpy as np -from numpy.matlib import repmat -import pyrr as pyrr - -import os -import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") -from rrt_3D.plot_util3D import visualization - - -def getRay(x, y): - direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] - return np.array([x, direc]) - - -def getAABB(blocks): - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - - -def getDist(pos1, pos2): - return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) - - -''' The following utils can be used for rrt or rrt*, - required param initparams should have - env, environement generated from env3D - V, node set - E, edge set - i, nodes added - maxiter, maximum iteration allowed - stepsize, leaf growth restriction - -''' - - -def sampleFree(initparams, bias = 0.1): - '''biased sampling''' - x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) - i = np.random.random() - if isinside(initparams, x): - return sampleFree(initparams) - else: - if i < bias: - return np.array(initparams.xt) + 1 - else: - return x - return x - -# ---------------------- Collision checking algorithms -def isinside(initparams, x): - '''see if inside obstacle''' - for i in initparams.env.blocks: - if isinbound(i, x): - return True - for i in initparams.env.OBB: - if isinbound(i, x, mode = 'obb'): - return True - for i in initparams.env.balls: - if isinball(i, x): - return True - return False - -def isinbound(i, x, mode = False, factor = 0, isarray = False): - if mode == 'obb': - return isinobb(i, x, isarray) - if isarray: - compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) - compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) - compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) - return compx & compy & compz - else: - return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] - -def isinobb(i, x, isarray = False): - # transform the point from {W} to {body} - if isarray: - pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3] - block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] - return isinbound(block, pts, isarray = isarray) - else: - pt = i.T@np.append(x,1) - block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] - return isinbound(block, pt) - -def isinball(i, x, factor = 0): - if getDist(i[0:3], x) <= i[3] + factor: - return True - return False - -def lineSphere(p0, p1, ball): - # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf - c, r = ball[0:3], ball[-1] - line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]] - d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]] - t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * ( - line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2]) - if t <= 0: - if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True - elif t >= 1: - d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] - if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True - elif 0 < t < 1: - x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] - k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] - if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True - return False - -def lineAABB(p0, p1, dist, aabb): - # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 - # aabb should have the attributes of P, E as center point and extents - mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point - I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction - hl = dist / 2 # radius - T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] - # do any of the principal axis form a separting axis? - if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False - if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False - if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False - # I.cross(x axis) ? - r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) - if abs(T[1] * I[2] - T[2] * I[1]) > r: return False - # I.cross(y axis) ? - r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) - if abs(T[2] * I[0] - T[0] * I[2]) > r: return False - # I.cross(z axis) ? - r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) - if abs(T[0] * I[1] - T[1] * I[0]) > r: return False - - return True - -def lineOBB(p0, p1, dist, obb): - # transform points to obb frame - res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T - # record old position and set the position to origin - oldP, obb.P= obb.P, [0,0,0] - # calculate segment-AABB testing - ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) - # reset the position - obb.P = oldP - return ans - -def isCollide(initparams, x, child, dist=None): - '''see if line intersects obstacle''' - '''specified for expansion in A* 3D lookup table''' - if dist==None: - dist = getDist(x, child) - # check in bound - if not isinbound(initparams.env.boundary, child): - return True, dist - # check collision in AABB - for i in range(len(initparams.env.AABB)): - if lineAABB(x, child, dist, initparams.env.AABB[i]): - return True, dist - # check collision in ball - for i in initparams.env.balls: - if lineSphere(x, child, i): - return True, dist - # check collision with obb - for i in initparams.env.OBB: - if lineOBB(x, child, dist, i): - return True, dist - return False, dist - -# ---------------------- leaf node extending algorithms -def nearest(initparams, x): - V = np.array(initparams.V) - if initparams.i == 0: - return initparams.V[0] - xr = repmat(x, len(V), 1) - dists = np.linalg.norm(xr - V, axis=1) - return tuple(initparams.V[np.argmin(dists)]) - -def near(initparams, x): - x = np.array(x) - V = np.array(initparams.V) - cardV = len(initparams.V) - eta = initparams.eta - gamma = initparams.gamma - r = min(gamma * (np.log(cardV) / cardV), eta) - if initparams.done: - r = 1 - if initparams.i == 0: - return [initparams.V[0]] - xr = repmat(x, len(V), 1) - inside = np.linalg.norm(xr - V, axis=1) < r - nearpoints = V[inside] - return np.array(nearpoints) - -def steer(initparams, x, y): - dist, step = getDist(y, x), initparams.stepsize - increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step) - xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) - # direc = (y - x) / np.linalg.norm(y - x) - # xnew = x + initparams.stepsize * direc - return xnew - -def cost(initparams, x): - '''here use the additive recursive Cost function''' - if x == initparams.x0: - return 0 - return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x]) - - -def path(initparams, Path=[], dist=0): - x = initparams.xt - while x != initparams.x0: - x2 = initparams.Parent[x] - Path.append(np.array([x, x2])) - dist += getDist(x, x2) - x = x2 - return Path, dist - -class edgeset(object): - def __init__(self): - self.E = {} - - def add_edge(self, edge): - x, y = edge[0], edge[1] - if x in self.E: - self.E[x].add(y) - else: - self.E[x] = set() - self.E[x].add(y) - - def remove_edge(self, edge): - x, y = edge[0], edge[1] - self.E[x].remove(y) - - def get_edge(self, nodes = None): - edges = [] - if nodes is None: - for v in self.E: - for n in self.E[v]: - # if (n,v) not in edges: - edges.append((v, n)) - else: - for v in nodes: - for n in self.E[tuple(v)]: - edges.append((v, n)) - return edges - - def isEndNode(self, node): - return node not in self.E - - -class Node: - def __init__(self, data): - self.data = data - self.sibling = None - self.child = None - -class Tree: - def __init__(self, start): - self.root = Node(start) - self.ind = 0 - self.index = {start:self.ind} - - def add_edge(self, edge): - # y exists in the tree while x does not - x, y = edge[0], edge[1] - - - - - +import numpy as np +from numpy.matlib import repmat +import pyrr as pyrr + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") +from rrt_3D.plot_util3D import visualization + + +def getRay(x, y): + direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] + return np.array([x, direc]) + + +def getAABB(blocks): + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + + +def getDist(pos1, pos2): + return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) + + +''' The following utils can be used for rrt or rrt*, + required param initparams should have + env, environement generated from env3D + V, node set + E, edge set + i, nodes added + maxiter, maximum iteration allowed + stepsize, leaf growth restriction + +''' + + +def sampleFree(initparams, bias = 0.1): + '''biased sampling''' + x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) + i = np.random.random() + if isinside(initparams, x): + return sampleFree(initparams) + else: + if i < bias: + return np.array(initparams.xt) + 1 + else: + return x + return x + +# ---------------------- Collision checking algorithms +def isinside(initparams, x): + '''see if inside obstacle''' + for i in initparams.env.blocks: + if isinbound(i, x): + return True + for i in initparams.env.OBB: + if isinbound(i, x, mode = 'obb'): + return True + for i in initparams.env.balls: + if isinball(i, x): + return True + return False + +def isinbound(i, x, mode = False, factor = 0, isarray = False): + if mode == 'obb': + return isinobb(i, x, isarray) + if isarray: + compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) + compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,2] < i[5] + factor) + return compx & compy & compz + else: + return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + +def isinobb(i, x, isarray = False): + # transform the point from {W} to {body} + if isarray: + pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3] + block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] + return isinbound(block, pts, isarray = isarray) + else: + pt = i.T@np.append(x,1) + block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] + return isinbound(block, pt) + +def isinball(i, x, factor = 0): + if getDist(i[0:3], x) <= i[3] + factor: + return True + return False + +def lineSphere(p0, p1, ball): + # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf + c, r = ball[0:3], ball[-1] + line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]] + d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]] + t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * ( + line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2]) + if t <= 0: + if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True + elif t >= 1: + d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] + if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True + elif 0 < t < 1: + x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] + k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] + if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True + return False + +def lineAABB(p0, p1, dist, aabb): + # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 + # aabb should have the attributes of P, E as center point and extents + mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point + I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction + hl = dist / 2 # radius + T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] + # do any of the principal axis form a separting axis? + if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False + if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False + if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False + # I.cross(x axis) ? + r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) + if abs(T[1] * I[2] - T[2] * I[1]) > r: return False + # I.cross(y axis) ? + r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) + if abs(T[2] * I[0] - T[0] * I[2]) > r: return False + # I.cross(z axis) ? + r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) + if abs(T[0] * I[1] - T[1] * I[0]) > r: return False + + return True + +def lineOBB(p0, p1, dist, obb): + # transform points to obb frame + res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T + # record old position and set the position to origin + oldP, obb.P= obb.P, [0,0,0] + # calculate segment-AABB testing + ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) + # reset the position + obb.P = oldP + return ans + +def isCollide(initparams, x, child, dist=None): + '''see if line intersects obstacle''' + '''specified for expansion in A* 3D lookup table''' + if dist==None: + dist = getDist(x, child) + # check in bound + if not isinbound(initparams.env.boundary, child): + return True, dist + # check collision in AABB + for i in range(len(initparams.env.AABB)): + if lineAABB(x, child, dist, initparams.env.AABB[i]): + return True, dist + # check collision in ball + for i in initparams.env.balls: + if lineSphere(x, child, i): + return True, dist + # check collision with obb + for i in initparams.env.OBB: + if lineOBB(x, child, dist, i): + return True, dist + return False, dist + +# ---------------------- leaf node extending algorithms +def nearest(initparams, x): + V = np.array(initparams.V) + if initparams.i == 0: + return initparams.V[0] + xr = repmat(x, len(V), 1) + dists = np.linalg.norm(xr - V, axis=1) + return tuple(initparams.V[np.argmin(dists)]) + +def near(initparams, x): + x = np.array(x) + V = np.array(initparams.V) + cardV = len(initparams.V) + eta = initparams.eta + gamma = initparams.gamma + r = min(gamma * (np.log(cardV) / cardV), eta) + if initparams.done: + r = 1 + if initparams.i == 0: + return [initparams.V[0]] + xr = repmat(x, len(V), 1) + inside = np.linalg.norm(xr - V, axis=1) < r + nearpoints = V[inside] + return np.array(nearpoints) + +def steer(initparams, x, y): + dist, step = getDist(y, x), initparams.stepsize + increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step) + xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) + # direc = (y - x) / np.linalg.norm(y - x) + # xnew = x + initparams.stepsize * direc + return xnew + +def cost(initparams, x): + '''here use the additive recursive Cost function''' + if x == initparams.x0: + return 0 + return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x]) + + +def path(initparams, Path=[], dist=0): + x = initparams.xt + while x != initparams.x0: + x2 = initparams.Parent[x] + Path.append(np.array([x, x2])) + dist += getDist(x, x2) + x = x2 + return Path, dist + +class edgeset(object): + def __init__(self): + self.E = {} + + def add_edge(self, edge): + x, y = edge[0], edge[1] + if x in self.E: + self.E[x].add(y) + else: + self.E[x] = set() + self.E[x].add(y) + + def remove_edge(self, edge): + x, y = edge[0], edge[1] + self.E[x].remove(y) + + def get_edge(self, nodes = None): + edges = [] + if nodes is None: + for v in self.E: + for n in self.E[v]: + # if (n,v) not in edges: + edges.append((v, n)) + else: + for v in nodes: + for n in self.E[tuple(v)]: + edges.append((v, n)) + return edges + + def isEndNode(self, node): + return node not in self.E + + +class Node: + def __init__(self, data): + self.data = data + self.sibling = None + self.child = None + +class Tree: + def __init__(self, start): + self.root = Node(start) + self.ind = 0 + self.index = {start:self.ind} + + def add_edge(self, edge): + # y exists in the tree while x does not + x, y = edge[0], edge[1] + + + + + diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search_based_Planning/.idea/Search-based Planning.iml similarity index 100% rename from Search-based Planning/.idea/Search-based Planning.iml rename to Search_based_Planning/.idea/Search-based Planning.iml diff --git a/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml b/Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml similarity index 100% rename from Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml rename to Search_based_Planning/.idea/dictionaries/Huiming_Zhou.xml diff --git a/Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml similarity index 100% rename from Search-based Planning/.idea/inspectionProfiles/profiles_settings.xml rename to Search_based_Planning/.idea/inspectionProfiles/profiles_settings.xml diff --git a/Search-based Planning/.idea/misc.xml b/Search_based_Planning/.idea/misc.xml similarity index 100% rename from Search-based Planning/.idea/misc.xml rename to Search_based_Planning/.idea/misc.xml diff --git a/Search-based Planning/.idea/modules.xml b/Search_based_Planning/.idea/modules.xml similarity index 100% rename from Search-based Planning/.idea/modules.xml rename to Search_based_Planning/.idea/modules.xml diff --git a/Search-based Planning/.idea/vcs.xml b/Search_based_Planning/.idea/vcs.xml similarity index 100% rename from Search-based Planning/.idea/vcs.xml rename to Search_based_Planning/.idea/vcs.xml diff --git a/Search-based Planning/.idea/workspace.xml b/Search_based_Planning/.idea/workspace.xml similarity index 100% rename from Search-based Planning/.idea/workspace.xml rename to Search_based_Planning/.idea/workspace.xml diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search_based_Planning/Search_2D/ARAstar.py similarity index 99% rename from Search-based Planning/Search_2D/ARAstar.py rename to Search_based_Planning/Search_2D/ARAstar.py index 0867f89..b034665 100644 --- a/Search-based Planning/Search_2D/ARAstar.py +++ b/Search_based_Planning/Search_2D/ARAstar.py @@ -8,7 +8,7 @@ import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/Anytime_D_star.py b/Search_based_Planning/Search_2D/Anytime_D_star.py similarity index 99% rename from Search-based Planning/Search_2D/Anytime_D_star.py rename to Search_based_Planning/Search_2D/Anytime_D_star.py index 4cebb48..7d0b093 100644 --- a/Search-based Planning/Search_2D/Anytime_D_star.py +++ b/Search_based_Planning/Search_2D/Anytime_D_star.py @@ -9,7 +9,7 @@ import math import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/Astar.py b/Search_based_Planning/Search_2D/Astar.py similarity index 99% rename from Search-based Planning/Search_2D/Astar.py rename to Search_based_Planning/Search_2D/Astar.py index 444a4d4..54b5919 100644 --- a/Search-based Planning/Search_2D/Astar.py +++ b/Search_based_Planning/Search_2D/Astar.py @@ -8,7 +8,7 @@ import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/Best_First.py b/Search_based_Planning/Search_2D/Best_First.py similarity index 98% rename from Search-based Planning/Search_2D/Best_First.py rename to Search_based_Planning/Search_2D/Best_First.py index 326e73b..8c6b4c3 100644 --- a/Search-based Planning/Search_2D/Best_First.py +++ b/Search_based_Planning/Search_2D/Best_First.py @@ -8,7 +8,7 @@ import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py similarity index 99% rename from Search-based Planning/Search_2D/Bidirectional_a_star.py rename to Search_based_Planning/Search_2D/Bidirectional_a_star.py index 8ffafc1..6da37b4 100644 --- a/Search-based Planning/Search_2D/Bidirectional_a_star.py +++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py @@ -8,7 +8,7 @@ import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py similarity index 99% rename from Search-based Planning/Search_2D/D_star.py rename to Search_based_Planning/Search_2D/D_star.py index 23522e4..45bf9b7 100644 --- a/Search-based Planning/Search_2D/D_star.py +++ b/Search_based_Planning/Search_2D/D_star.py @@ -9,7 +9,7 @@ import math import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/D_star_Lite.py b/Search_based_Planning/Search_2D/D_star_Lite.py similarity index 99% rename from Search-based Planning/Search_2D/D_star_Lite.py rename to Search_based_Planning/Search_2D/D_star_Lite.py index 6277b68..ed0fe04 100644 --- a/Search-based Planning/Search_2D/D_star_Lite.py +++ b/Search_based_Planning/Search_2D/D_star_Lite.py @@ -9,7 +9,7 @@ import math import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py similarity index 98% rename from Search-based Planning/Search_2D/Dijkstra.py rename to Search_based_Planning/Search_2D/Dijkstra.py index 9b4d4fb..5cd7325 100644 --- a/Search-based Planning/Search_2D/Dijkstra.py +++ b/Search_based_Planning/Search_2D/Dijkstra.py @@ -8,7 +8,7 @@ import sys import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search_based_Planning/Search_2D/LPAstar.py similarity index 99% rename from Search-based Planning/Search_2D/LPAstar.py rename to Search_based_Planning/Search_2D/LPAstar.py index 92b2656..cb23701 100644 --- a/Search-based Planning/Search_2D/LPAstar.py +++ b/Search_based_Planning/Search_2D/LPAstar.py @@ -9,7 +9,7 @@ import math import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search_based_Planning/Search_2D/LRTAstar.py similarity index 99% rename from Search-based Planning/Search_2D/LRTAstar.py rename to Search_based_Planning/Search_2D/LRTAstar.py index f3594cb..cd91bd3 100644 --- a/Search-based Planning/Search_2D/LRTAstar.py +++ b/Search_based_Planning/Search_2D/LRTAstar.py @@ -9,7 +9,7 @@ import copy import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/PotentialField.py b/Search_based_Planning/Search_2D/PotentialField.py similarity index 98% rename from Search-based Planning/Search_2D/PotentialField.py rename to Search_based_Planning/Search_2D/PotentialField.py index 44d0c85..dfa82e7 100644 --- a/Search-based Planning/Search_2D/PotentialField.py +++ b/Search_based_Planning/Search_2D/PotentialField.py @@ -10,7 +10,7 @@ import matplotlib.pyplot as plt from collections import deque sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import plotting from Search_2D import env diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search_based_Planning/Search_2D/RTAAstar.py similarity index 99% rename from Search-based Planning/Search_2D/RTAAstar.py rename to Search_based_Planning/Search_2D/RTAAstar.py index b73be99..8c59d91 100644 --- a/Search-based Planning/Search_2D/RTAAstar.py +++ b/Search_based_Planning/Search_2D/RTAAstar.py @@ -9,7 +9,7 @@ import copy import math sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc rename to Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc rename to Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc rename to Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc diff --git a/Search-based Planning/Search_2D/bfs.py b/Search_based_Planning/Search_2D/bfs.py similarity index 98% rename from Search-based Planning/Search_2D/bfs.py rename to Search_based_Planning/Search_2D/bfs.py index efadfbb..dffa661 100644 --- a/Search-based Planning/Search_2D/bfs.py +++ b/Search_based_Planning/Search_2D/bfs.py @@ -7,7 +7,7 @@ import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/dfs.py b/Search_based_Planning/Search_2D/dfs.py similarity index 98% rename from Search-based Planning/Search_2D/dfs.py rename to Search_based_Planning/Search_2D/dfs.py index 6e45d72..820680b 100644 --- a/Search-based Planning/Search_2D/dfs.py +++ b/Search_based_Planning/Search_2D/dfs.py @@ -7,7 +7,7 @@ import os import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import queue from Search_2D import plotting diff --git a/Search-based Planning/Search_2D/env.py b/Search_based_Planning/Search_2D/env.py similarity index 100% rename from Search-based Planning/Search_2D/env.py rename to Search_based_Planning/Search_2D/env.py diff --git a/Search-based Planning/Search_2D/plotting.py b/Search_based_Planning/Search_2D/plotting.py similarity index 98% rename from Search-based Planning/Search_2D/plotting.py rename to Search_based_Planning/Search_2D/plotting.py index c818a64..f952876 100644 --- a/Search-based Planning/Search_2D/plotting.py +++ b/Search_based_Planning/Search_2D/plotting.py @@ -8,7 +8,7 @@ import sys import matplotlib.pyplot as plt sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Search-based Planning/") + "/../../Search_based_Planning/") from Search_2D import env diff --git a/Search-based Planning/Search_2D/queue.py b/Search_based_Planning/Search_2D/queue.py similarity index 100% rename from Search-based Planning/Search_2D/queue.py rename to Search_based_Planning/Search_2D/queue.py diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py similarity index 99% rename from Search-based Planning/Search_3D/Anytime_Dstar3D.py rename to Search_based_Planning/Search_3D/Anytime_Dstar3D.py index 42075c6..10b8eb8 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py @@ -7,7 +7,7 @@ import os import sys from collections import defaultdict -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ cost, children, StateSpace diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search_based_Planning/Search_3D/Astar3D.py similarity index 99% rename from Search-based Planning/Search_3D/Astar3D.py rename to Search_based_Planning/Search_3D/Astar3D.py index 961e2ba..4b7ad68 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search_based_Planning/Search_3D/Astar3D.py @@ -10,7 +10,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ cost, children, StateSpace, heuristic_fun diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search_based_Planning/Search_3D/Dstar3D.py similarity index 99% rename from Search-based Planning/Search_3D/Dstar3D.py rename to Search_based_Planning/Search_3D/Dstar3D.py index 2953e16..9332d4e 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search_based_Planning/Search_3D/Dstar3D.py @@ -5,7 +5,7 @@ import os import sys from collections import defaultdict -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \ diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search_based_Planning/Search_3D/DstarLite3D.py similarity index 99% rename from Search-based Planning/Search_3D/DstarLite3D.py rename to Search_based_Planning/Search_3D/DstarLite3D.py index 278fb60..3621eda 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search_based_Planning/Search_3D/DstarLite3D.py @@ -5,7 +5,7 @@ import os import sys from collections import defaultdict -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \ cost, children, StateSpace diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search_based_Planning/Search_3D/LP_Astar3D.py similarity index 99% rename from Search-based Planning/Search_3D/LP_Astar3D.py rename to Search_based_Planning/Search_3D/LP_Astar3D.py index 497fbb1..7a5ca1d 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/Search_based_Planning/Search_3D/LP_Astar3D.py @@ -4,7 +4,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \ diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search_based_Planning/Search_3D/LRT_Astar3D.py similarity index 98% rename from Search-based Planning/Search_3D/LRT_Astar3D.py rename to Search_based_Planning/Search_3D/LRT_Astar3D.py index dac6c0e..cb04dd6 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search_based_Planning/Search_3D/LRT_Astar3D.py @@ -10,7 +10,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search_based_Planning/Search_3D/RTA_Astar3D.py similarity index 98% rename from Search-based Planning/Search_3D/RTA_Astar3D.py rename to Search_based_Planning/Search_3D/RTA_Astar3D.py index 5e93ebf..26548b0 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/Search_based_Planning/Search_3D/RTA_Astar3D.py @@ -10,7 +10,7 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc rename to Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc rename to Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc rename to Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc rename to Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc similarity index 100% rename from Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc rename to Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/bidirectional_Astar3D.py b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py similarity index 99% rename from Search-based Planning/Search_3D/bidirectional_Astar3D.py rename to Search_based_Planning/Search_3D/bidirectional_Astar3D.py index da41482..2f1c48d 100644 --- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py +++ b/Search_based_Planning/Search_3D/bidirectional_Astar3D.py @@ -11,7 +11,7 @@ from collections import defaultdict import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search_based_Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun from Search_3D.plot_util3D import visualization diff --git a/Search-based Planning/Search_3D/env3D.py b/Search_based_Planning/Search_3D/env3D.py similarity index 97% rename from Search-based Planning/Search_3D/env3D.py rename to Search_based_Planning/Search_3D/env3D.py index 1d1a59a..c8805ab 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search_based_Planning/Search_3D/env3D.py @@ -1,179 +1,179 @@ -# this is the three dimensional configuration space for rrt -# !/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: yue qi -""" -import numpy as np - - -# from utils3D import OBB2AABB - -def R_matrix(z_angle, y_angle, x_angle): - # x angle: row; y angle: pitch; z angle: yaw - # generate rotation matrix in SO3 - # RzRyRx = R, ZYX intrinsic rotation - # also (r1,r2,r3) in R3*3 in {W} frame - # used in obb.O - # [[R p] - # [0T 1]] gives transformation from body to world - return np.array( - [[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]]) @ \ - np.array( - [[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]]) @ \ - np.array( - [[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) - - -def getblocks(): - # AABBs - block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], - [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], - # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], - # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], - # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], - [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], - [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] - Obstacles = [] - for i in block: - i = np.array(i) - Obstacles.append([j for j in i]) - return np.array(Obstacles) - - -def getAABB(blocks): - # used for Pyrr package for detecting collision - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - - -class aabb(object): - # make AABB out of blocks, - # P: center point - # E: extents - # O: Rotation matrix in SO(3), in {w} - def __init__(self, AABB): - self.P = [(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, (AABB[5] + AABB[2]) / 2] # center point - self.E = [(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, (AABB[5] - AABB[2]) / 2] # extents - self.O = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] - - -class obb(object): - # P: center point - # E: extents - # O: Rotation matrix in SO(3), in {w} - def __init__(self, P, E, O): - self.P = P - self.E = E - self.O = O - self.T = np.vstack([np.column_stack([self.O.T, -self.O.T @ self.P]), [0, 0, 0, 1]]) - - -def getAABB2(blocks): - # used in lineAABB - AABB = [] - for i in blocks: - AABB.append(aabb(i)) - return AABB - - -def getballs(): - spheres = [[16, 2.5, 4, 2], [10, 2.5, 1, 1]] - Obstacles = [] - for i in spheres: - Obstacles.append([j for j in i]) - return np.array(Obstacles) - - -def add_block(block=[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): - return block - - -class env(): - def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): - self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) - self.blocks = getblocks() - self.AABB = getAABB2(self.blocks) - self.AABB_pyrr = getAABB(self.blocks) - self.balls = getballs() - self.OBB = np.array([obb([2.6, 2.5, 1], [0.2, 2, 1], R_matrix(0, 0, 45))]) - # self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) - # print(self.OBB) - # self.OBB = [] - self.start = np.array([0.5, 2.5, 5.5]) - self.goal = np.array([19.0, 2.5, 5.5]) - self.t = 0 # time - - def New_block(self): - newblock = add_block() - self.blocks = np.vstack([self.blocks, newblock]) - self.AABB = getAABB2(self.blocks) - self.AABB_pyrr = getAABB(self.blocks) - - def move_start(self, x): - self.start = x - - def move_block(self, a=[0, 0, 0], s=0, v=[0.1, 0, 0], theta=[0, 0, 0], block_to_move=0, obb_to_move=0, - mode='uniform'): - # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, - # R is an orthorgonal transform in R3*3, is the rotation matrix - # (x',t') = (x + tv, t) is uniform transformation - if mode == 'uniform': - ori = np.array(self.blocks[block_to_move]) - self.blocks[block_to_move] = \ - np.array([ori[0] + self.t * v[0], - ori[1] + self.t * v[1], - ori[2] + self.t * v[2], - ori[3] + self.t * v[0], - ori[4] + self.t * v[1], - ori[5] + self.t * v[2]]) - - self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + self.t * v[0], - self.AABB[block_to_move].P[1] + self.t * v[1], - self.AABB[block_to_move].P[2] + self.t * v[2]] - # return a range of block that the block might moved - a = self.blocks[block_to_move] - # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ - # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ - # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ - # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) - return a, ori - # (x',t') = (x + a, t + s) is a translation - if mode == 'translation': - ori = np.array(self.blocks[block_to_move]) - self.blocks[block_to_move] = \ - np.array([ori[0] + a[0], - ori[1] + a[1], - ori[2] + a[2], - ori[3] + a[0], - ori[4] + a[1], - ori[5] + a[2]]) - - self.AABB[block_to_move].P = \ - [self.AABB[block_to_move].P[0] + a[0], - self.AABB[block_to_move].P[1] + a[1], - self.AABB[block_to_move].P[2] + a[2]] - self.t += s - # return a range of block that the block might moved - a = self.blocks[block_to_move] - return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, - a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ - np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, - ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) - # return a,ori - # (x',t') = (Rx, t) - if mode == 'rotation': # this makes an OBB rotate - ori = [self.OBB[obb_to_move]] - self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2]) - self.OBB[obb_to_move].T = np.vstack( - [np.column_stack([self.OBB[obb_to_move].O.T, -self.OBB[obb_to_move].O.T @ self.OBB[obb_to_move].P]), - [0, 0, 0, 1]]) - return self.OBB[obb_to_move], ori[0] - - -if __name__ == '__main__': - newenv = env() +# this is the three dimensional configuration space for rrt +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: yue qi +""" +import numpy as np + + +# from utils3D import OBB2AABB + +def R_matrix(z_angle, y_angle, x_angle): + # x angle: row; y angle: pitch; z angle: yaw + # generate rotation matrix in SO3 + # RzRyRx = R, ZYX intrinsic rotation + # also (r1,r2,r3) in R3*3 in {W} frame + # used in obb.O + # [[R p] + # [0T 1]] gives transformation from body to world + return np.array( + [[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]]) @ \ + np.array( + [[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]]) @ \ + np.array( + [[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) + + +def getblocks(): + # AABBs + block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], + [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], + # [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], + # [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], + # [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], + [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], + [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] + Obstacles = [] + for i in block: + i = np.array(i) + Obstacles.append([j for j in i]) + return np.array(Obstacles) + + +def getAABB(blocks): + # used for Pyrr package for detecting collision + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + + +class aabb(object): + # make AABB out of blocks, + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self, AABB): + self.P = [(AABB[3] + AABB[0]) / 2, (AABB[4] + AABB[1]) / 2, (AABB[5] + AABB[2]) / 2] # center point + self.E = [(AABB[3] - AABB[0]) / 2, (AABB[4] - AABB[1]) / 2, (AABB[5] - AABB[2]) / 2] # extents + self.O = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + + +class obb(object): + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self, P, E, O): + self.P = P + self.E = E + self.O = O + self.T = np.vstack([np.column_stack([self.O.T, -self.O.T @ self.P]), [0, 0, 0, 1]]) + + +def getAABB2(blocks): + # used in lineAABB + AABB = [] + for i in blocks: + AABB.append(aabb(i)) + return AABB + + +def getballs(): + spheres = [[16, 2.5, 4, 2], [10, 2.5, 1, 1]] + Obstacles = [] + for i in spheres: + Obstacles.append([j for j in i]) + return np.array(Obstacles) + + +def add_block(block=[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): + return block + + +class env(): + def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): + self.resolution = resolution + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + self.blocks = getblocks() + self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) + self.balls = getballs() + self.OBB = np.array([obb([2.6, 2.5, 1], [0.2, 2, 1], R_matrix(0, 0, 45))]) + # self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) + # print(self.OBB) + # self.OBB = [] + self.start = np.array([0.5, 2.5, 5.5]) + self.goal = np.array([19.0, 2.5, 5.5]) + self.t = 0 # time + + def New_block(self): + newblock = add_block() + self.blocks = np.vstack([self.blocks, newblock]) + self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) + + def move_start(self, x): + self.start = x + + def move_block(self, a=[0, 0, 0], s=0, v=[0.1, 0, 0], theta=[0, 0, 0], block_to_move=0, obb_to_move=0, + mode='uniform'): + # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, + # R is an orthorgonal transform in R3*3, is the rotation matrix + # (x',t') = (x + tv, t) is uniform transformation + if mode == 'uniform': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + self.t * v[0], + ori[1] + self.t * v[1], + ori[2] + self.t * v[2], + ori[3] + self.t * v[0], + ori[4] + self.t * v[1], + ori[5] + self.t * v[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + self.t * v[0], + self.AABB[block_to_move].P[1] + self.t * v[1], + self.AABB[block_to_move].P[2] + self.t * v[2]] + # return a range of block that the block might moved + a = self.blocks[block_to_move] + # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ + # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ + # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + return a, ori + # (x',t') = (x + a, t + s) is a translation + if mode == 'translation': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + a[0], + ori[1] + a[1], + ori[2] + a[2], + ori[3] + a[0], + ori[4] + a[1], + ori[5] + a[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + a[0], + self.AABB[block_to_move].P[1] + a[1], + self.AABB[block_to_move].P[2] + a[2]] + self.t += s + # return a range of block that the block might moved + a = self.blocks[block_to_move] + return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, + a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ + np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, + ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + # return a,ori + # (x',t') = (Rx, t) + if mode == 'rotation': # this makes an OBB rotate + ori = [self.OBB[obb_to_move]] + self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0], y_angle=theta[1], x_angle=theta[2]) + self.OBB[obb_to_move].T = np.vstack( + [np.column_stack([self.OBB[obb_to_move].O.T, -self.OBB[obb_to_move].O.T @ self.OBB[obb_to_move].P]), + [0, 0, 0, 1]]) + return self.OBB[obb_to_move], ori[0] + + +if __name__ == '__main__': + newenv = env() diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search_based_Planning/Search_3D/plot_util3D.py similarity index 97% rename from Search-based Planning/Search_3D/plot_util3D.py rename to Search_based_Planning/Search_3D/plot_util3D.py index 6bf565b..ad4c5f0 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/Search_based_Planning/Search_3D/plot_util3D.py @@ -1,174 +1,174 @@ -# plotting -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from mpl_toolkits.mplot3d.art3d import Poly3DCollection -import mpl_toolkits.mplot3d as plt3d -from mpl_toolkits.mplot3d import proj3d -import numpy as np - -def CreateSphere(center,r): - u = np.linspace(0,2* np.pi,30) - v = np.linspace(0,np.pi,30) - x = np.outer(np.cos(u),np.sin(v)) - y = np.outer(np.sin(u),np.sin(v)) - z = np.outer(np.ones(np.size(u)),np.cos(v)) - x, y, z = r*x + center[0], r*y + center[1], r*z + center[2] - return (x,y,z) - -def draw_Spheres(ax,balls): - for i in balls: - (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) - ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") - -def draw_block_list(ax, blocks ,color=None,alpha=0.15): - ''' - drawing the blocks on the graph - ''' - v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], - dtype='float') - f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) - n = blocks.shape[0] - d = blocks[:, 3:6] - blocks[:, :3] - vl = np.zeros((8 * n, 3)) - fl = np.zeros((6 * n, 4), dtype='int64') - for k in range(n): - vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] - fl[k * 6:(k + 1) * 6, :] = f + k * 8 - if type(ax) is Poly3DCollection: - ax.set_verts(vl[fl]) - else: - pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') - pc.set_facecolor(color) - h = ax.add_collection3d(pc) - return h - -def obb_verts(obb): - # 0.017004013061523438 for 1000 iters - ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ - [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) - # P + (ori * E) - ori_body = np.multiply(ori_body,obb.E) - # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) - verts = (obb.O@ori_body.T).T + obb.P - return verts - - -def draw_obb(ax, OBB, color=None,alpha=0.15): - f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) - n = OBB.shape[0] - vl = np.zeros((8 * n, 3)) - fl = np.zeros((6 * n, 4), dtype='int64') - for k in range(n): - vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) - fl[k * 6:(k + 1) * 6, :] = f + k * 8 - if type(ax) is Poly3DCollection: - ax.set_verts(vl[fl]) - else: - pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') - pc.set_facecolor(color) - h = ax.add_collection3d(pc) - return h - - -def draw_line(ax,SET,visibility=1,color=None): - if SET != []: - for i in SET: - xs = i[0][0], i[1][0] - ys = i[0][1], i[1][1] - zs = i[0][2], i[1][2] - line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) - ax.add_line(line) - -def visualization(initparams): - if initparams.ind % 20 == 0 or initparams.done: - V = np.array(list(initparams.V)) - # E = initparams.E - Path = np.array(initparams.Path) - start = initparams.env.start - goal = initparams.env.goal - # edges = E.get_edge() - # generate axis objects - ax = plt.subplot(111, projection='3d') - #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) - #ax.view_init(elev=0., azim=90.) - ax.view_init(elev=8., azim=120.) - #ax.view_init(elev=-8., azim=180) - ax.clear() - # drawing objects - draw_Spheres(ax, initparams.env.balls) - draw_block_list(ax, initparams.env.blocks) - if initparams.env.OBB is not None: - draw_obb(ax,initparams.env.OBB) - draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) - # draw_line(ax,edges,visibility=0.25) - draw_line(ax,Path,color='r') - if len(V) > 0: - ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) - ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') - ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') - # adjust the aspect ratio - xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] - ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] - zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] - dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin - ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) - plt.xlabel('x') - plt.ylabel('y') - plt.pause(0.0001) - -def make_get_proj(self, rx, ry, rz): - ''' - Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that - makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. - ''' - - rm = max(rx, ry, rz) - kx = rm / rx; ky = rm / ry; kz = rm / rz - - # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are - # marked by ## - def get_proj(): - relev, razim = np.pi * self.elev/180, np.pi * self.azim/180 - - xmin, xmax = self.get_xlim3d() - ymin, ymax = self.get_ylim3d() - zmin, zmax = self.get_zlim3d() - - # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 - worldM = proj3d.world_transformation(xmin, xmax, - ymin, ymax, - zmin, zmax) - ratio = 0.5 - # adjust the aspect ratio ## - aspectM = proj3d.world_transformation(-kx + 1, kx, ## - -ky + 1, ky, ## - -kz + 1, kz) ## - - # look into the middle of the new coordinates - R = np.array([0.5, 0.5, 0.5]) - - xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio - yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio - zp = R[2] + np.sin(relev) * self.dist *ratio - E = np.array((xp, yp, zp)) - - self.eye = E - self.vvec = R - E - self.vvec = self.vvec / np.linalg.norm(self.vvec) - - if abs(relev) > np.pi/2: - # upside down - V = np.array((0, 0, -1)) - else: - V = np.array((0, 0, 1)) - zfront, zback = -self.dist *ratio, self.dist *ratio - - viewM = proj3d.view_transformation(E, R, V) - perspM = proj3d.persp_transformation(zfront, zback) - M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## - M = np.dot(perspM, M0) - return M - return get_proj - -if __name__ == '__main__': +# plotting +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import mpl_toolkits.mplot3d as plt3d +from mpl_toolkits.mplot3d import proj3d +import numpy as np + +def CreateSphere(center,r): + u = np.linspace(0,2* np.pi,30) + v = np.linspace(0,np.pi,30) + x = np.outer(np.cos(u),np.sin(v)) + y = np.outer(np.sin(u),np.sin(v)) + z = np.outer(np.ones(np.size(u)),np.cos(v)) + x, y, z = r*x + center[0], r*y + center[1], r*z + center[2] + return (x,y,z) + +def draw_Spheres(ax,balls): + for i in balls: + (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) + ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") + +def draw_block_list(ax, blocks ,color=None,alpha=0.15): + ''' + drawing the blocks on the graph + ''' + v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], + dtype='float') + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + n = blocks.shape[0] + d = blocks[:, 3:6] - blocks[:, :3] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') + pc.set_facecolor(color) + h = ax.add_collection3d(pc) + return h + +def obb_verts(obb): + # 0.017004013061523438 for 1000 iters + ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ + [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) + # P + (ori * E) + ori_body = np.multiply(ori_body,obb.E) + # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) + verts = (obb.O@ori_body.T).T + obb.P + return verts + + +def draw_obb(ax, OBB, color=None,alpha=0.15): + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + n = OBB.shape[0] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') + pc.set_facecolor(color) + h = ax.add_collection3d(pc) + return h + + +def draw_line(ax,SET,visibility=1,color=None): + if SET != []: + for i in SET: + xs = i[0][0], i[1][0] + ys = i[0][1], i[1][1] + zs = i[0][2], i[1][2] + line = plt3d.art3d.Line3D(xs, ys, zs, alpha=visibility, color=color) + ax.add_line(line) + +def visualization(initparams): + if initparams.ind % 20 == 0 or initparams.done: + V = np.array(list(initparams.V)) + # E = initparams.E + Path = np.array(initparams.Path) + start = initparams.env.start + goal = initparams.env.goal + # edges = E.get_edge() + # generate axis objects + ax = plt.subplot(111, projection='3d') + #ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) + #ax.view_init(elev=0., azim=90.) + ax.view_init(elev=8., azim=120.) + #ax.view_init(elev=-8., azim=180) + ax.clear() + # drawing objects + draw_Spheres(ax, initparams.env.balls) + draw_block_list(ax, initparams.env.blocks) + if initparams.env.OBB is not None: + draw_obb(ax,initparams.env.OBB) + draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) + # draw_line(ax,edges,visibility=0.25) + draw_line(ax,Path,color='r') + if len(V) > 0: + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) + ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') + ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') + # adjust the aspect ratio + xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] + ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] + zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] + dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin + ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) + plt.xlabel('x') + plt.ylabel('y') + plt.pause(0.0001) + +def make_get_proj(self, rx, ry, rz): + ''' + Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that + makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. + ''' + + rm = max(rx, ry, rz) + kx = rm / rx; ky = rm / ry; kz = rm / rz + + # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are + # marked by ## + def get_proj(): + relev, razim = np.pi * self.elev/180, np.pi * self.azim/180 + + xmin, xmax = self.get_xlim3d() + ymin, ymax = self.get_ylim3d() + zmin, zmax = self.get_zlim3d() + + # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 + worldM = proj3d.world_transformation(xmin, xmax, + ymin, ymax, + zmin, zmax) + ratio = 0.5 + # adjust the aspect ratio ## + aspectM = proj3d.world_transformation(-kx + 1, kx, ## + -ky + 1, ky, ## + -kz + 1, kz) ## + + # look into the middle of the new coordinates + R = np.array([0.5, 0.5, 0.5]) + + xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio + yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio + zp = R[2] + np.sin(relev) * self.dist *ratio + E = np.array((xp, yp, zp)) + + self.eye = E + self.vvec = R - E + self.vvec = self.vvec / np.linalg.norm(self.vvec) + + if abs(relev) > np.pi/2: + # upside down + V = np.array((0, 0, -1)) + else: + V = np.array((0, 0, 1)) + zfront, zback = -self.dist *ratio, self.dist *ratio + + viewM = proj3d.view_transformation(E, R, V) + perspM = proj3d.persp_transformation(zfront, zback) + M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## + M = np.dot(perspM, M0) + return M + return get_proj + +if __name__ == '__main__': pass \ No newline at end of file diff --git a/Search-based Planning/Search_3D/queue.py b/Search_based_Planning/Search_3D/queue.py similarity index 100% rename from Search-based Planning/Search_3D/queue.py rename to Search_based_Planning/Search_3D/queue.py diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search_based_Planning/Search_3D/utils3D.py similarity index 100% rename from Search-based Planning/Search_3D/utils3D.py rename to Search_based_Planning/Search_3D/utils3D.py diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search_based_Planning/__pycache__/env.cpython-37.pyc similarity index 100% rename from Search-based Planning/__pycache__/env.cpython-37.pyc rename to Search_based_Planning/__pycache__/env.cpython-37.pyc diff --git a/Search-based Planning/__pycache__/plotting.cpython-35.pyc b/Search_based_Planning/__pycache__/plotting.cpython-35.pyc similarity index 100% rename from Search-based Planning/__pycache__/plotting.cpython-35.pyc rename to Search_based_Planning/__pycache__/plotting.cpython-35.pyc diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search_based_Planning/__pycache__/plotting.cpython-37.pyc similarity index 100% rename from Search-based Planning/__pycache__/plotting.cpython-37.pyc rename to Search_based_Planning/__pycache__/plotting.cpython-37.pyc diff --git a/Search-based Planning/__pycache__/queue.cpython-35.pyc b/Search_based_Planning/__pycache__/queue.cpython-35.pyc similarity index 100% rename from Search-based Planning/__pycache__/queue.cpython-35.pyc rename to Search_based_Planning/__pycache__/queue.cpython-35.pyc diff --git a/Search-based Planning/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/__pycache__/queue.cpython-37.pyc similarity index 100% rename from Search-based Planning/__pycache__/queue.cpython-37.pyc rename to Search_based_Planning/__pycache__/queue.cpython-37.pyc diff --git a/Search-based Planning/gif/ADstar_sig.gif b/Search_based_Planning/gif/ADstar_sig.gif similarity index 100% rename from Search-based Planning/gif/ADstar_sig.gif rename to Search_based_Planning/gif/ADstar_sig.gif diff --git a/Search-based Planning/gif/ADstar_small.gif b/Search_based_Planning/gif/ADstar_small.gif similarity index 100% rename from Search-based Planning/gif/ADstar_small.gif rename to Search_based_Planning/gif/ADstar_small.gif diff --git a/Search-based Planning/gif/ARA_star.gif b/Search_based_Planning/gif/ARA_star.gif similarity index 100% rename from Search-based Planning/gif/ARA_star.gif rename to Search_based_Planning/gif/ARA_star.gif diff --git a/Search-based Planning/gif/Astar.gif b/Search_based_Planning/gif/Astar.gif similarity index 100% rename from Search-based Planning/gif/Astar.gif rename to Search_based_Planning/gif/Astar.gif diff --git a/Search-based Planning/gif/BF.gif b/Search_based_Planning/gif/BF.gif similarity index 100% rename from Search-based Planning/gif/BF.gif rename to Search_based_Planning/gif/BF.gif diff --git a/Search-based Planning/gif/BFS.gif b/Search_based_Planning/gif/BFS.gif similarity index 100% rename from Search-based Planning/gif/BFS.gif rename to Search_based_Planning/gif/BFS.gif diff --git a/Search-based Planning/gif/Bi-Astar.gif b/Search_based_Planning/gif/Bi-Astar.gif similarity index 100% rename from Search-based Planning/gif/Bi-Astar.gif rename to Search_based_Planning/gif/Bi-Astar.gif diff --git a/Search-based Planning/gif/DFS.gif b/Search_based_Planning/gif/DFS.gif similarity index 100% rename from Search-based Planning/gif/DFS.gif rename to Search_based_Planning/gif/DFS.gif diff --git a/Search-based Planning/gif/D_star.gif b/Search_based_Planning/gif/D_star.gif similarity index 100% rename from Search-based Planning/gif/D_star.gif rename to Search_based_Planning/gif/D_star.gif diff --git a/Search-based Planning/gif/D_star_Lite.gif b/Search_based_Planning/gif/D_star_Lite.gif similarity index 100% rename from Search-based Planning/gif/D_star_Lite.gif rename to Search_based_Planning/gif/D_star_Lite.gif diff --git a/Search-based Planning/gif/Dijkstra.gif b/Search_based_Planning/gif/Dijkstra.gif similarity index 100% rename from Search-based Planning/gif/Dijkstra.gif rename to Search_based_Planning/gif/Dijkstra.gif diff --git a/Search-based Planning/gif/LPA_star.gif b/Search_based_Planning/gif/LPA_star.gif similarity index 100% rename from Search-based Planning/gif/LPA_star.gif rename to Search_based_Planning/gif/LPA_star.gif diff --git a/Search-based Planning/gif/LPAstar.gif b/Search_based_Planning/gif/LPAstar.gif similarity index 100% rename from Search-based Planning/gif/LPAstar.gif rename to Search_based_Planning/gif/LPAstar.gif diff --git a/Search-based Planning/gif/LRTA_star.gif b/Search_based_Planning/gif/LRTA_star.gif similarity index 100% rename from Search-based Planning/gif/LRTA_star.gif rename to Search_based_Planning/gif/LRTA_star.gif diff --git a/Search-based Planning/gif/RTAA_star.gif b/Search_based_Planning/gif/RTAA_star.gif similarity index 100% rename from Search-based Planning/gif/RTAA_star.gif rename to Search_based_Planning/gif/RTAA_star.gif diff --git a/Search-based Planning/gif/RepeatedA_star.gif b/Search_based_Planning/gif/RepeatedA_star.gif similarity index 100% rename from Search-based Planning/gif/RepeatedA_star.gif rename to Search_based_Planning/gif/RepeatedA_star.gif