pathplanning 0.1.0__py3-none-any.whl

CurvesGenerator/reeds_shepp.py

@@ -0,0 +1,720 @@
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+import draw
+
+# parameters initiation
+STEP_SIZE = 0.2
+MAX_LENGTH = 1000.0
+PI = math.pi
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
+        self.lengths = lengths  # lengths of each part of path (+: forward, -: backward) [float]
+        self.ctypes = ctypes  # type of each part of the path [string]
+        self.L = L  # total path length [float]
+        self.x = x  # final s positions [m]
+        self.y = y  # final y positions [m]
+        self.yaw = yaw  # final yaw angles [rad]
+        self.directions = directions  # forward: 1, backward:-1
+
+
+def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)
+
+    minL = paths[0].L
+    mini = 0
+
+    for i in range(len(paths)):
+        if paths[i].L <= minL:
+            minL, mini = paths[i].L, i
+
+    return paths[mini]
+
+
+def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
+    q0 = [sx, sy, syaw]
+    q1 = [gx, gy, gyaw]
+
+    paths = generate_path(q0, q1, maxc)
+
+    for path in paths:
+        x, y, yaw, directions = \
+            generate_local_course(path.L, path.lengths,
+                                  path.ctypes, maxc, step_size * maxc)
+
+        # convert global coordinate
+        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]
+        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]
+        path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
+        path.directions = directions
+        path.lengths = [l / maxc for l in path.lengths]
+        path.L = path.L / maxc
+
+    return paths
+
+
+def set_path(paths, lengths, ctypes):
+    path = PATH([], [], 0.0, [], [], [], [])
+    path.ctypes = ctypes
+    path.lengths = lengths
+
+    # check same path exist
+    for path_e in paths:
+        if path_e.ctypes == path.ctypes:
+            if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:
+                return paths  # not insert path
+
+    path.L = sum([abs(i) for i in lengths])
+
+    if path.L >= MAX_LENGTH:
+        return paths
+
+    assert path.L >= 0.01
+    paths.append(path)
+
+    return paths
+
+
+def LSL(x, y, phi):
+    u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if t >= 0.0:
+        v = M(phi - t)
+        if v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LSR(x, y, phi):
+    u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
+    u1 = u1 ** 2
+
+    if u1 >= 4.0:
+        u = math.sqrt(u1 - 4.0)
+        theta = math.atan2(2.0, u)
+        t = M(t1 + theta)
+        v = M(t - phi)
+
+        if t >= 0.0 and v >= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRL(x, y, phi):
+    u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
+
+    if u1 <= 4.0:
+        u = -2.0 * math.asin(0.25 * u1)
+        t = M(t1 + 0.5 * u + PI)
+        v = M(phi - t + u)
+
+        if t >= 0.0 and u <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def SCS(x, y, phi, paths):
+    flag, t, u, v = SLS(x, y, phi)
+
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "L", "S"])
+
+    flag, t, u, v = SLS(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["S", "R", "S"])
+
+    return paths
+
+
+def SLS(x, y, phi):
+    phi = M(phi)
+
+    if y > 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+    elif y < 0.0 and 0.0 < phi < PI * 0.99:
+        xd = -y / math.tan(phi) + x
+        t = xd - math.tan(phi / 2.0)
+        u = phi
+        v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
+        return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CSC(x, y, phi, paths):
+    flag, t, u, v = LSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "S", "L"])
+
+    flag, t, u, v = LSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])
+
+    flag, t, u, v = LSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "R"])
+
+    flag, t, u, v = LSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
+
+    flag, t, u, v = LSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "S", "R"])
+
+    flag, t, u, v = LSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])
+
+    flag, t, u, v = LSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "S", "L"])
+
+    flag, t, u, v = LSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])
+
+    return paths
+
+
+def CCC(x, y, phi, paths):
+    flag, t, u, v = LRL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, v], ["R", "L", "R"])
+
+    flag, t, u, v = LRL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])
+
+    flag, t, u, v = LRL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, t], ["R", "L", "R"])
+
+    flag, t, u, v = LRL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])
+
+    return paths
+
+
+def calc_tauOmega(u, v, xi, eta, phi):
+    delta = M(u - v)
+    A = math.sin(u) - math.sin(delta)
+    B = math.cos(u) - math.cos(delta) - 1.0
+
+    t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
+    t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
+
+    if t2 < 0:
+        tau = M(t1 + PI)
+    else:
+        tau = M(t1)
+
+    omega = M(tau - u + v - phi)
+
+    return tau, omega
+
+
+def LRLRn(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
+
+    if rho <= 1.0:
+        u = math.acos(rho)
+        t, v = calc_tauOmega(u, -u, xi, eta, phi)
+        if t >= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRLRp(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho = (20.0 - xi * xi - eta * eta) / 16.0
+
+    if 0.0 <= rho <= 1.0:
+        u = -math.acos(rho)
+        if u >= -0.5 * PI:
+            t, v = calc_tauOmega(u, u, xi, eta, phi)
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCCC(x, y, phi, paths):
+    flag, t, u, v = LRLRn(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRn(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRn(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRn(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRp(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRp(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"])
+
+    flag, t, u, v = LRLRp(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"])
+
+    flag, t, u, v = LRLRp(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"])
+
+    return paths
+
+
+def LRSR(x, y, phi):
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(-eta, xi)
+
+    if rho >= 2.0:
+        t = theta
+        u = 2.0 - rho
+        v = M(t + 0.5 * PI - phi)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def LRSL(x, y, phi):
+    xi = x - math.sin(phi)
+    eta = y - 1.0 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        r = math.sqrt(rho * rho - 4.0)
+        u = 2.0 - r
+        t = M(theta + math.atan2(r, -2.0))
+        v = M(phi - 0.5 * PI - t)
+        if t >= 0.0 and u <= 0.0 and v <= 0.0:
+            return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSC(x, y, phi, paths):
+    flag, t, u, v = LRSL(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"])
+
+    flag, t, u, v = LRSL(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"])
+
+    flag, t, u, v = LRSL(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"])
+
+    flag, t, u, v = LRSL(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"])
+
+    flag, t, u, v = LRSR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"])
+
+    flag, t, u, v = LRSR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"])
+
+    flag, t, u, v = LRSR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"])
+
+    # backwards
+    xb = x * math.cos(phi) + y * math.sin(phi)
+    yb = x * math.sin(phi) - y * math.cos(phi)
+
+    flag, t, u, v = LRSL(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"])
+
+    flag, t, u, v = LRSL(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"])
+
+    flag, t, u, v = LRSL(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"])
+
+    flag, t, u, v = LRSL(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"])
+
+    flag, t, u, v = LRSR(xb, yb, phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"])
+
+    flag, t, u, v = LRSR(-xb, yb, -phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"])
+
+    flag, t, u, v = LRSR(xb, -yb, -phi)
+    if flag:
+        paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"])
+
+    flag, t, u, v = LRSR(-xb, -yb, phi)
+    if flag:
+        paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"])
+
+    return paths
+
+
+def LRSLR(x, y, phi):
+    # formula 8.11 *** TYPO IN PAPER ***
+    xi = x + math.sin(phi)
+    eta = y - 1.0 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2.0:
+        u = 4.0 - math.sqrt(rho * rho - 4.0)
+        if u <= 0.0:
+            t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
+            v = M(t - phi)
+
+            if t >= 0.0 and v >= 0.0:
+                return True, t, u, v
+
+    return False, 0.0, 0.0, 0.0
+
+
+def CCSCC(x, y, phi, paths):
+    flag, t, u, v = LRSLR(x, y, phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"])
+
+    flag, t, u, v = LRSLR(-x, y, -phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"])
+
+    flag, t, u, v = LRSLR(x, -y, -phi)
+    if flag:
+        paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"])
+
+    flag, t, u, v = LRSLR(-x, -y, phi)
+    if flag:
+        paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"])
+
+    return paths
+
+
+def generate_local_course(L, lengths, mode, maxc, step_size):
+    point_num = int(L / step_size) + len(lengths) + 3
+
+    px = [0.0 for _ in range(point_num)]
+    py = [0.0 for _ in range(point_num)]
+    pyaw = [0.0 for _ in range(point_num)]
+    directions = [0 for _ in range(point_num)]
+    ind = 1
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    if lengths[0] > 0.0:
+        d = step_size
+    else:
+        d = -step_size
+
+    pd = d
+    ll = 0.0
+
+    for m, l, i in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
+
+        ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
+
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = -d - ll
+        else:
+            pd = d - ll
+
+        while abs(pd) <= abs(l):
+            ind += 1
+            px, py, pyaw, directions = \
+                interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+            pd += d
+
+        ll = l - pd - d  # calc remain length
+
+        ind += 1
+        px, py, pyaw, directions = \
+            interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
+
+    # remove unused data
+    while px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
+    if m == "S":
+        px[ind] = ox + l / maxc * math.cos(oyaw)
+        py[ind] = oy + l / maxc * math.sin(oyaw)
+        pyaw[ind] = oyaw
+    else:
+        ldx = math.sin(l) / maxc
+        if m == "L":
+            ldy = (1.0 - math.cos(l)) / maxc
+        elif m == "R":
+            ldy = (1.0 - math.cos(l)) / (-maxc)
+
+        gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
+        gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
+        px[ind] = ox + gdx
+        py[ind] = oy + gdy
+
+    if m == "L":
+        pyaw[ind] = oyaw + l
+    elif m == "R":
+        pyaw[ind] = oyaw - l
+
+    if l > 0.0:
+        directions[ind] = 1
+    else:
+        directions[ind] = -1
+
+    return px, py, pyaw, directions
+
+
+def generate_path(q0, q1, maxc):
+    dx = q1[0] - q0[0]
+    dy = q1[1] - q0[1]
+    dth = q1[2] - q0[2]
+    c = math.cos(q0[2])
+    s = math.sin(q0[2])
+    x = (c * dx + s * dy) * maxc
+    y = (-s * dx + c * dy) * maxc
+
+    paths = []
+    paths = SCS(x, y, dth, paths)
+    paths = CSC(x, y, dth, paths)
+    paths = CCC(x, y, dth, paths)
+    paths = CCCC(x, y, dth, paths)
+    paths = CCSC(x, y, dth, paths)
+    paths = CCSCC(x, y, dth, paths)
+
+    return paths
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > PI:
+        theta -= 2.0 * PI
+
+    while theta < -PI:
+        theta += 2.0 * PI
+
+    return theta
+
+
+def R(x, y):
+    """
+    Return the polar coordinates (r, theta) of the point (s, y)
+    """
+    r = math.hypot(x, y)
+    theta = math.atan2(y, x)
+
+    return r, theta
+
+
+def M(theta):
+    """
+    Regulate theta to -pi <= theta < pi
+    """
+    phi = theta % (2.0 * PI)
+
+    if phi < -PI:
+        phi += 2.0 * PI
+    if phi > PI:
+        phi -= 2.0 * PI
+
+    return phi
+
+
+def get_label(path):
+    label = ""
+
+    for m, l in zip(path.ctypes, path.lengths):
+        label = label + m
+        if l > 0.0:
+            label = label + "+"
+        else:
+            label = label + "-"
+
+    return label
+
+
+def calc_curvature(x, y, yaw, directions):
+    c, ds = [], []
+
+    for i in range(1, len(x) - 1):
+        dxn = x[i] - x[i - 1]
+        dxp = x[i + 1] - x[i]
+        dyn = y[i] - y[i - 1]
+        dyp = y[i + 1] - y[i]
+        dn = math.hypot(dxn, dyn)
+        dp = math.hypot(dxp, dyp)
+        dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)
+        ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)
+        dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)
+        ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)
+        curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        d = (dn + dp) / 2.0
+
+        if np.isnan(curvature):
+            curvature = 0.0
+
+        if directions[i] <= 0.0:
+            curvature = -curvature
+
+        if len(c) == 0:
+            ds.append(d)
+            c.append(curvature)
+
+        ds.append(d)
+        c.append(curvature)
+
+    ds.append(ds[-1])
+    c.append(c[-1])
+
+    return c, ds
+
+
+def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
+    paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+    assert len(paths) >= 1
+
+    for path in paths:
+        assert abs(path.x[0] - sx) <= 0.01
+        assert abs(path.y[0] - sy) <= 0.01
+        assert abs(path.yaw[0] - syaw) <= 0.01
+        assert abs(path.x[-1] - gx) <= 0.01
+        assert abs(path.y[-1] - gy) <= 0.01
+        assert abs(path.yaw[-1] - gyaw) <= 0.01
+
+        # course distance check
+        d = [math.hypot(dx, dy)
+             for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),
+                               np.diff(path.y[0:len(path.y) - 1]))]
+
+        for i in range(len(d)):
+            assert abs(d[i] - STEP_SIZE) <= 0.001
+
+
+def main():
+    # choose states pairs: (s, y, yaw)
+    # simulation-1
+    # states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
+    #           (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
+
+    # simulation-2
+    states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
+              (35, 10, 180), (32, -10, 180), (5, -12, 90)]
+
+    max_c = 0.1  # max curvature
+    path_x, path_y, yaw = [], [], []
+
+    for i in range(len(states) - 1):
+        s_x = states[i][0]
+        s_y = states[i][1]
+        s_yaw = np.deg2rad(states[i][2])
+        g_x = states[i + 1][0]
+        g_y = states[i + 1][1]
+        g_yaw = np.deg2rad(states[i + 1][2])
+
+        path_i = calc_optimal_path(s_x, s_y, s_yaw,
+                                   g_x, g_y, g_yaw, max_c)
+
+        path_x += path_i.x
+        path_y += path_i.y
+        yaw += path_i.yaw
+
+    # animation
+    plt.ion()
+    plt.figure(1)
+
+    for i in range(len(path_x)):
+        plt.clf()
+        plt.plot(path_x, path_y, linewidth=1, color='gray')
+
+        for x, y, theta in states:
+            draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
+
+        draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
+
+        plt.axis("equal")
+        plt.title("Simulation of Reeds-Shepp Curves")
+        plt.axis([-10, 42, -20, 20])
+        plt.draw()
+        plt.pause(0.001)
+
+    plt.pause(1)
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/__init__.py

@@ -0,0 +1 @@
+"""Sampling-based planning algorithms package."""

Sampling_based_Planning/rrt_2D/__init__.py

@@ -0,0 +1 @@
+"""2D sampling-based planners."""