reformat

Sampling-based Planning/rrt_2D/CurvesGenerator/reeds_shepp.py

@@ -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()

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

Sampling-based Planning/rrt_2D/dynamic_rrt.py → 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:

Sampling-based Planning/rrt_2D/extended_rrt.py → 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:

Sampling-based Planning/rrt_2D/informed_rrt_star.py → 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:

Sampling-based Planning/rrt_2D/optimal_bi_rrt.py → 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)
 

Sampling-based Planning/rrt_2D/plotting.py → 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:

Sampling-based Planning/rrt_2D/rrt.py → 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:

Sampling-based Planning/rrt_2D/rrt_connect.py → 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:

Sampling-based Planning/rrt_2D/rrt_star.py → 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:

Sampling-based Planning/rrt_2D/rrt_star_smart.py → 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:

Sampling-based Planning/rrt_2D/rs_rrt_star.py → 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:

Sampling-based Planning/rrt_2D/utils.py → 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:

Sampling-based Planning/rrt_3D/dynamic_rrt3D.py → 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

Sampling-based Planning/rrt_3D/env3D.py → 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()

Sampling-based Planning/rrt_3D/plot_util3D.py → 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

Sampling-based Planning/rrt_3D/rrt3D.py → 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

Sampling-based Planning/rrt_3D/rrtstar3D.py → 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
 

Sampling-based Planning/rrt_3D/utils3D.py → 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]
+
+        
+        
+
+

Search-based Planning/Search_2D/ARAstar.py → 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

Search-based Planning/Search_2D/Anytime_D_star.py → 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

Search-based Planning/Search_2D/Astar.py → 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

Search-based Planning/Search_2D/Best_First.py → 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

Search-based Planning/Search_2D/Bidirectional_a_star.py → 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

Search-based Planning/Search_2D/D_star.py → 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

Search-based Planning/Search_2D/D_star_Lite.py → 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

Search-based Planning/Search_2D/Dijkstra.py → 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

Search-based Planning/Search_2D/LPAstar.py → 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

Search-based Planning/Search_2D/LRTAstar.py → 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

Search-based Planning/Search_2D/PotentialField.py → 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

Search-based Planning/Search_2D/RTAAstar.py → 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

Search-based Planning/Search_2D/bfs.py → 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

Search-based Planning/Search_2D/dfs.py → 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

Search-based Planning/Search_2D/plotting.py → 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
 

Search-based Planning/Search_3D/Anytime_Dstar3D.py → 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

Search-based Planning/Search_3D/Astar3D.py → 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

Search-based Planning/Search_3D/Dstar3D.py → 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, \

Search-based Planning/Search_3D/DstarLite3D.py → 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

Search-based Planning/Search_3D/LP_Astar3D.py → 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, \

Search-based Planning/Search_3D/LRT_Astar3D.py → 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, \

Search-based Planning/Search_3D/RTA_Astar3D.py → 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, \

Search-based Planning/Search_3D/bidirectional_Astar3D.py → 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

Search-based Planning/Search_3D/env3D.py → 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()

Search-based Planning/Search_3D/plot_util3D.py → 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