update

CurvesGenerator/bezier_path.py

@@ -0,0 +1,145 @@
+"""
+bezier path
+
+author: Atsushi Sakai(@Atsushi_twi)
+modified: huiming zhou
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.special import comb
+import draw
+
+
+def calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset):
+
+    dist = np.hypot(sx - gx, sy - gy) / offset
+    control_points = np.array(
+        [[sx, sy],
+         [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)],
+         [gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)],
+         [gx, gy]])
+
+    path = calc_bezier_path(control_points, n_points=100)
+
+    return path, control_points
+
+
+def calc_bezier_path(control_points, n_points=100):
+    traj = []
+
+    for t in np.linspace(0, 1, n_points):
+        traj.append(bezier(t, control_points))
+
+    return np.array(traj)
+
+
+def Comb(n, i, t):
+    return comb(n, i) * t ** i * (1 - t) ** (n - i)
+
+
+def bezier(t, control_points):
+    n = len(control_points) - 1
+    return np.sum([Comb(n, i, t) * control_points[i] for i in range(n + 1)], axis=0)
+
+
+def bezier_derivatives_control_points(control_points, n_derivatives):
+    w = {0: control_points}
+
+    for i in range(n_derivatives):
+        n = len(w[i])
+        w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])
+                             for j in range(n - 1)])
+
+    return w
+
+
+def curvature(dx, dy, ddx, ddy):
+    return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
+
+
+def simulation():
+    sx = [-3, 0, 4, 6]
+    sy = [2, 0, 1.5, 6]
+
+    ratio = np.linspace(0, 1, 100)
+    pathx, pathy = [], []
+
+    for t in ratio:
+        x, y = [], []
+        for i in range(len(sx) - 1):
+            x.append(sx[i + 1] * t + sx[i] * (1 - t))
+            y.append(sy[i + 1] * t + sy[i] * (1 - t))
+
+        xx, yy = [], []
+        for i in range(len(x) - 1):
+            xx.append(x[i + 1] * t + x[i] * (1 - t))
+            yy.append(y[i + 1] * t + y[i] * (1 - t))
+
+        px = xx[1] * t + xx[0] * (1 - t)
+        py = yy[1] * t + yy[0] * (1 - t)
+        pathx.append(px)
+        pathy.append(py)
+
+        plt.cla()
+        plt.plot(sx, sy, linestyle='-', marker='o', color='dimgray', label="Control Points")
+        plt.plot(x, y, color='dodgerblue')
+        plt.plot(xx, yy, color='cyan')
+        plt.plot(pathx, pathy, color='darkorange', linewidth=2, label="Bezier Path")
+        plt.plot(px, py, marker='o')
+        plt.axis("equal")
+        plt.legend()
+        plt.title("Cubic Bezier Curve demo")
+        plt.grid(True)
+        plt.pause(0.001)
+
+    plt.show()
+
+
+def main():
+    sx, sy, syaw = 10.0, 1.0, np.deg2rad(180.0)
+    gx, gy, gyaw = 0.0, -3.0, np.deg2rad(-45.0)
+    offset = 3.0
+
+    path, control_points = calc_4points_bezier_path(sx, sy, syaw, gx, gy, gyaw, offset)
+
+    t = 0.8  # Number in [0, 1]
+    x_target, y_target = bezier(t, control_points)
+    derivatives_cp = bezier_derivatives_control_points(control_points, 2)
+    point = bezier(t, control_points)
+    dt = bezier(t, derivatives_cp[1])
+    ddt = bezier(t, derivatives_cp[2])
+    # Radius of curv
+    radius = 1 / curvature(dt[0], dt[1], ddt[0], ddt[1])
+    # Normalize derivative
+    dt /= np.linalg.norm(dt, 2)
+    tangent = np.array([point, point + dt])
+    normal = np.array([point, point + [- dt[1], dt[0]]])
+    curvature_center = point + np.array([- dt[1], dt[0]]) * radius
+    circle = plt.Circle(tuple(curvature_center), radius,
+                        color=(0, 0.8, 0.8), fill=False, linewidth=1)
+
+    assert path.T[0][0] == sx, "path is invalid"
+    assert path.T[1][0] == sy, "path is invalid"
+    assert path.T[0][-1] == gx, "path is invalid"
+    assert path.T[1][-1] == gy, "path is invalid"
+
+    fig, ax = plt.subplots()
+    ax.plot(path.T[0], path.T[1], label="Bezier Path")
+    ax.plot(control_points.T[0], control_points.T[1],
+            '--o', label="Control Points")
+    ax.plot(x_target, y_target)
+    ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
+    ax.plot(normal[:, 0], normal[:, 1], label="Normal")
+    ax.add_artist(circle)
+    draw.Arrow(sx, sy, syaw, 1, "darkorange")
+    draw.Arrow(gx, gy, gyaw, 1, "darkorange")
+    plt.grid(True)
+    plt.title("Bezier Path: from Atsushi's work")
+    ax.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()
+    # simulation()

CurvesGenerator/bspline_curve.py

@@ -0,0 +1,80 @@
+"""
+
+Path Planner with B-Spline
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.interpolate as scipy_interpolate
+import cubic_spline as cs
+
+
+def approximate_b_spline_path(x, y, n_path_points, degree=3):
+    t = range(len(x))
+    x_tup = scipy_interpolate.splrep(t, x, k=degree)
+    y_tup = scipy_interpolate.splrep(t, y, k=degree)
+
+    x_list = list(x_tup)
+    x_list[1] = x + [0.0, 0.0, 0.0, 0.0]
+
+    y_list = list(y_tup)
+    y_list[1] = y + [0.0, 0.0, 0.0, 0.0]
+
+    ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)
+    rx = scipy_interpolate.splev(ipl_t, x_list)
+    ry = scipy_interpolate.splev(ipl_t, y_list)
+
+    return rx, ry
+
+
+def interpolate_b_spline_path(x, y, n_path_points, degree=3):
+    ipl_t = np.linspace(0.0, len(x) - 1, len(x))
+    spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)
+    spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)
+
+    travel = np.linspace(0.0, len(x) - 1, n_path_points)
+    return spl_i_x(travel), spl_i_y(travel)
+
+
+def main():
+    print(__file__ + " start!!")
+    # way points
+    # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
+    # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
+    way_point_x = [-2, 2.0, 3.5, 5.5, 6.0, 8.0]
+    way_point_y = [0, 2.7, -0.5, 0.5, 3.0, 4.0]
+
+    sp = cs.Spline2D(way_point_x, way_point_y)
+    s = np.arange(0, sp.s[-1], 0.1)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    n_course_point = 100  # sampling number
+    rax, ray = approximate_b_spline_path(way_point_x, way_point_y,
+                                         n_course_point)
+    rix, riy = interpolate_b_spline_path(way_point_x, way_point_y,
+                                         n_course_point)
+
+    # show results
+    plt.plot(way_point_x, way_point_y, '-og', label="Control Points")
+    plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
+    plt.plot(rix, riy, '-b', label="Interpolated B-Spline path")
+    plt.plot(rx, ry, color='dimgray', label="Cubic Spline")
+    plt.grid(True)
+    plt.title("Curves Comparison")
+    plt.legend()
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

CurvesGenerator/cubic_spline.py

@@ -0,0 +1,262 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+u"""
+Cubic Spline library on python
+
+author Atsushi Sakai
+
+usage: see test codes as below
+
+license: MIT
+"""
+import math
+import numpy as np
+import bisect
+
+
+class Spline:
+    u"""
+    Cubic Spline class
+    """
+
+    def __init__(self, x, y):
+        self.b, self.c, self.d, self.w = [], [], [], []
+
+        self.x = x
+        self.y = y
+
+        self.nx = len(x)  # dimension of x
+        h = np.diff(x)
+
+        # calc coefficient c
+        self.a = [iy for iy in y]
+
+        # calc coefficient c
+        A = self.__calc_A(h)
+        B = self.__calc_B(h)
+        self.c = np.linalg.solve(A, B)
+        #  print(self.c1)
+
+        # calc spline coefficient b and d
+        for i in range(self.nx - 1):
+            self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
+            tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
+                (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
+            self.b.append(tb)
+
+    def calc(self, t):
+        u"""
+        Calc position
+
+        if t is outside of the input x, return None
+
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = self.a[i] + self.b[i] * dx + \
+            self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
+
+        return result
+
+    def calcd(self, t):
+        u"""
+        Calc first derivative
+
+        if t is outside of the input x, return None
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
+        return result
+
+    def calcdd(self, t):
+        u"""
+        Calc second derivative
+        """
+
+        if t < self.x[0]:
+            return None
+        elif t > self.x[-1]:
+            return None
+
+        i = self.__search_index(t)
+        dx = t - self.x[i]
+        result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
+        return result
+
+    def __search_index(self, x):
+        u"""
+        search data segment index
+        """
+        return bisect.bisect(self.x, x) - 1
+
+    def __calc_A(self, h):
+        u"""
+        calc matrix A for spline coefficient c
+        """
+        A = np.zeros((self.nx, self.nx))
+        A[0, 0] = 1.0
+        for i in range(self.nx - 1):
+            if i != (self.nx - 2):
+                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
+            A[i + 1, i] = h[i]
+            A[i, i + 1] = h[i]
+
+        A[0, 1] = 0.0
+        A[self.nx - 1, self.nx - 2] = 0.0
+        A[self.nx - 1, self.nx - 1] = 1.0
+        #  print(A)
+        return A
+
+    def __calc_B(self, h):
+        u"""
+        calc matrix B for spline coefficient c
+        """
+        B = np.zeros(self.nx)
+        for i in range(self.nx - 2):
+            B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
+                h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
+        #  print(B)
+        return B
+
+
+class Spline2D:
+    u"""
+    2D Cubic Spline class
+
+    """
+
+    def __init__(self, x, y):
+        self.s = self.__calc_s(x, y)
+        self.sx = Spline(self.s, x)
+        self.sy = Spline(self.s, y)
+
+    def __calc_s(self, x, y):
+        dx = np.diff(x)
+        dy = np.diff(y)
+        self.ds = [math.sqrt(idx ** 2 + idy ** 2)
+                   for (idx, idy) in zip(dx, dy)]
+        s = [0]
+        s.extend(np.cumsum(self.ds))
+        return s
+
+    def calc_position(self, s):
+        u"""
+        calc position
+        """
+        x = self.sx.calc(s)
+        y = self.sy.calc(s)
+
+        return x, y
+
+    def calc_curvature(self, s):
+        u"""
+        calc curvature
+        """
+        dx = self.sx.calcd(s)
+        ddx = self.sx.calcdd(s)
+        dy = self.sy.calcd(s)
+        ddy = self.sy.calcdd(s)
+        k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
+        return k
+
+    def calc_yaw(self, s):
+        u"""
+        calc yaw
+        """
+        dx = self.sx.calcd(s)
+        dy = self.sy.calcd(s)
+        yaw = math.atan2(dy, dx)
+        return yaw
+
+
+def calc_spline_course(x, y, ds=0.1):
+    sp = Spline2D(x, y)
+    s = np.arange(0, sp.s[-1], ds)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    return rx, ry, ryaw, rk, s
+
+
+def test_spline2d():
+    print("Spline 2D test")
+    import matplotlib.pyplot as plt
+    x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
+    y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
+
+    sp = Spline2D(x, y)
+    s = np.arange(0, sp.s[-1], 0.1)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = sp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(sp.calc_yaw(i_s))
+        rk.append(sp.calc_curvature(i_s))
+
+    flg, ax = plt.subplots(1)
+    plt.plot(x, y, "xb", label="input")
+    plt.plot(rx, ry, "-r", label="spline")
+    plt.grid(True)
+    plt.axis("equal")
+    plt.xlabel("x[m]")
+    plt.ylabel("y[m]")
+    plt.legend()
+
+    flg, ax = plt.subplots(1)
+    plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw")
+    plt.grid(True)
+    plt.legend()
+    plt.xlabel("line length[m]")
+    plt.ylabel("yaw angle[deg]")
+
+    flg, ax = plt.subplots(1)
+    plt.plot(s, rk, "-r", label="curvature")
+    plt.grid(True)
+    plt.legend()
+    plt.xlabel("line length[m]")
+    plt.ylabel("curvature [1/m]")
+
+    plt.show()
+
+
+def test_spline():
+    print("Spline test")
+    import matplotlib.pyplot as plt
+    x = [-0.5, 0.0, 0.5, 1.0, 1.5]
+    y = [3.2, 2.7, 6, 5, 6.5]
+
+    spline = Spline(x, y)
+    rx = np.arange(-2.0, 4, 0.01)
+    ry = [spline.calc(i) for i in rx]
+
+    plt.plot(x, y, "xb")
+    plt.plot(rx, ry, "-r")
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+
+if __name__ == '__main__':
+    test_spline()
+    # test_spline2d()

CurvesGenerator/draw.py

@@ -0,0 +1,66 @@
+import matplotlib.pyplot as plt
+import numpy as np
+PI = np.pi
+
+
+class Arrow:
+    def __init__(self, x, y, theta, L, c):
+        angle = np.deg2rad(30)
+        d = 0.5 * L
+        w = 2
+
+        x_start = x
+        y_start = y
+        x_end = x + L * np.cos(theta)
+        y_end = y + L * np.sin(theta)
+
+        theta_hat_L = theta + PI - angle
+        theta_hat_R = theta + PI + angle
+
+        x_hat_start = x_end
+        x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L)
+        x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R)
+
+        y_hat_start = y_end
+        y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L)
+        y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R)
+
+        plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_L],
+                 [y_hat_start, y_hat_end_L], color=c, linewidth=w)
+        plt.plot([x_hat_start, x_hat_end_R],
+                 [y_hat_start, y_hat_end_R], color=c, linewidth=w)
+
+
+class Car:
+    def __init__(self, x, y, yaw, w, L):
+        theta_B = PI + yaw
+
+        xB = x + L / 4 * np.cos(theta_B)
+        yB = y + L / 4 * np.sin(theta_B)
+
+        theta_BL = theta_B + PI / 2
+        theta_BR = theta_B - PI / 2
+
+        x_BL = xB + w / 2 * np.cos(theta_BL)        # Bottom-Left vertex
+        y_BL = yB + w / 2 * np.sin(theta_BL)
+        x_BR = xB + w / 2 * np.cos(theta_BR)        # Bottom-Right vertex
+        y_BR = yB + w / 2 * np.sin(theta_BR)
+
+        x_FL = x_BL + L * np.cos(yaw)               # Front-Left vertex
+        y_FL = y_BL + L * np.sin(yaw)
+        x_FR = x_BR + L * np.cos(yaw)               # Front-Right vertex
+        y_FR = y_BR + L * np.sin(yaw)
+
+        plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL],
+                 [y_BL, y_BR, y_FR, y_FL, y_BL],
+                 linewidth=1, color='black')
+
+        Arrow(x, y, yaw, L / 2, 'black')
+        # plt.axis("equal")
+        # plt.show()
+
+
+if __name__ == '__main__':
+    # Arrow(-1, 2, 60)
+    Car(0, 0, 1, 2, 60)

CurvesGenerator/dubins_path.py

@@ -0,0 +1,351 @@
+"""
+Dubins Path
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation as Rot
+import draw
+
+
+# class for PATH element
+class PATH:
+    def __init__(self, L, mode, x, y, yaw):
+        self.L = L  # total path length [float]
+        self.mode = mode  # type of each part of the path [string]
+        self.x = x  # final x positions [m]
+        self.y = y  # final y positions [m]
+        self.yaw = yaw  # final yaw angles [rad]
+
+
+# utils
+def pi_2_pi(theta):
+    while theta > math.pi:
+        theta -= 2.0 * math.pi
+
+    while theta < -math.pi:
+        theta += 2.0 * math.pi
+
+    return theta
+
+
+def mod2pi(theta):
+    return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0)
+
+
+def LSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
+
+    if p_lsl < 0:
+        return None, None, None, ["L", "S", "L"]
+    else:
+        p_lsl = math.sqrt(p_lsl)
+
+    denominate = dist + sin_a - sin_b
+    t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate))
+    q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate))
+
+    return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
+
+
+def RSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
+
+    if p_rsr < 0:
+        return None, None, None, ["R", "S", "R"]
+    else:
+        p_rsr = math.sqrt(p_rsr)
+
+    denominate = dist - sin_a + sin_b
+    t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate))
+    q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate))
+
+    return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]
+
+
+def LSR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
+
+    if p_lsr < 0:
+        return None, None, None, ["L", "S", "R"]
+    else:
+        p_lsr = math.sqrt(p_lsr)
+
+    rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr)
+    t_lsr = mod2pi(-alpha + rec)
+    q_lsr = mod2pi(-mod2pi(beta) + rec)
+
+    return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]
+
+
+def RSL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
+
+    if p_rsl < 0:
+        return None, None, None, ["R", "S", "L"]
+    else:
+        p_rsl = math.sqrt(p_rsl)
+
+    rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl)
+    t_rsl = mod2pi(alpha - rec)
+    q_rsl = mod2pi(beta - rec)
+
+    return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]
+
+
+def RLR(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["R", "L", "R"]
+
+    p_rlr = mod2pi(2 * math.pi - math.acos(rec))
+    t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0))
+    q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr))
+
+    return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
+
+
+def LRL(alpha, beta, dist):
+    sin_a = math.sin(alpha)
+    sin_b = math.sin(beta)
+    cos_a = math.cos(alpha)
+    cos_b = math.cos(beta)
+    cos_a_b = math.cos(alpha - beta)
+
+    rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0
+
+    if abs(rec) > 1.0:
+        return None, None, None, ["L", "R", "L"]
+
+    p_lrl = mod2pi(2 * math.pi - math.acos(rec))
+    t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
+    q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl))
+
+    return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
+
+
+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_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
+
+    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)
+
+    if len(px) <= 1:
+        return [], [], [], []
+
+    # remove unused data
+    while len(px) >= 1 and px[-1] == 0.0:
+        px.pop()
+        py.pop()
+        pyaw.pop()
+        directions.pop()
+
+    return px, py, pyaw, directions
+
+
+def planning_from_origin(gx, gy, gyaw, curv, step_size):
+    D = math.hypot(gx, gy)
+    d = D * curv
+
+    theta = mod2pi(math.atan2(gy, gx))
+    alpha = mod2pi(-theta)
+    beta = mod2pi(gyaw - theta)
+
+    planners = [LSL, RSR, LSR, RSL, RLR, LRL]
+
+    best_cost = float("inf")
+    bt, bp, bq, best_mode = None, None, None, None
+
+    for planner in planners:
+        t, p, q, mode = planner(alpha, beta, d)
+
+        if t is None:
+            continue
+
+        cost = (abs(t) + abs(p) + abs(q))
+        if best_cost > cost:
+            bt, bp, bq, best_mode = t, p, q, mode
+            best_cost = cost
+    lengths = [bt, bp, bq]
+
+    x_list, y_list, yaw_list, directions = generate_local_course(
+        sum(lengths), lengths, best_mode, curv, step_size)
+
+    return x_list, y_list, yaw_list, best_mode, best_cost
+
+
+def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1):
+    gx = gx - sx
+    gy = gy - sy
+
+    l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2]
+    le_xy = np.stack([gx, gy]).T @ l_rot
+    le_yaw = gyaw - syaw
+
+    lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin(
+        le_xy[0], le_xy[1], le_yaw, curv, step_size)
+
+    rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2]
+    converted_xy = np.stack([lp_x, lp_y]).T @ rot
+    x_list = converted_xy[:, 0] + sx
+    y_list = converted_xy[:, 1] + sy
+    yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw]
+
+    return PATH(lengths, mode, x_list, y_list, yaw_list)
+
+
+def main():
+    # choose states pairs: (x, 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.25  # 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_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c)
+
+        for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw):
+            path_x.append(x)
+            path_y.append(y)
+            yaw.append(iyaw)
+
+    # 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 Dubins Path")
+        plt.axis([-10, 42, -20, 20])
+        plt.draw()
+        plt.pause(0.001)
+
+    plt.pause(1)
+
+
+if __name__ == '__main__':
+    main()

CurvesGenerator/quartic_polynomial.py

@@ -0,0 +1,43 @@
+"""
+Quartic Polynomial
+"""
+
+import numpy as np
+
+
+class QuarticPolynomial:
+    def __init__(self, x0, v0, a0, v1, a1, T):
+        A = np.array([[3 * T ** 2, 4 * T ** 3],
+                      [6 * T, 12 * T ** 2]])
+        b = np.array([v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+             self.a3 * t ** 3 + self.a4 * t ** 4
+
+        return xt
+
+    def calc_dxt(self, t):
+        xt = self.a1 + 2 * self.a2 * t + \
+             3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
+
+        return xt
+
+    def calc_ddxt(self, t):
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+
+        return xt
+
+    def calc_dddxt(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t
+
+        return xt
+

CurvesGenerator/quintic_polynomial.py

@@ -0,0 +1,143 @@
+"""
+Quintic Polynomial
+"""
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+
+import draw
+
+
+class QuinticPolynomial:
+    def __init__(self, x0, v0, a0, x1, v1, a1, T):
+        A = np.array([[T ** 3, T ** 4, T ** 5],
+                      [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
+                      [6 * T, 12 * T ** 2, 20 * T ** 3]])
+        b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2,
+                      v1 - v0 - a0 * T,
+                      a1 - a0])
+        X = np.linalg.solve(A, b)
+
+        self.a0 = x0
+        self.a1 = v0
+        self.a2 = a0 / 2.0
+        self.a3 = X[0]
+        self.a4 = X[1]
+        self.a5 = X[2]
+
+    def calc_xt(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+                self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5
+
+        return xt
+
+    def calc_dxt(self, t):
+        dxt = self.a1 + 2 * self.a2 * t + \
+            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4
+
+        return dxt
+
+    def calc_ddxt(self, t):
+        ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3
+
+        return ddxt
+
+    def calc_dddxt(self, t):
+        dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2
+
+        return dddxt
+
+
+class Trajectory:
+    def __init__(self):
+        self.t = []
+        self.x = []
+        self.y = []
+        self.yaw = []
+        self.v = []
+        self.a = []
+        self.jerk = []
+
+
+def simulation():
+    sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1
+    gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1
+
+    MAX_ACCEL = 1.0  # max accel [m/s2]
+    MAX_JERK = 0.5  # max jerk [m/s3]
+    dt = 0.1  # T tick [s]
+
+    MIN_T = 5
+    MAX_T = 100
+    T_STEP = 5
+
+    sv_x = sv * math.cos(syaw)
+    sv_y = sv * math.sin(syaw)
+    gv_x = gv * math.cos(gyaw)
+    gv_y = gv * math.sin(gyaw)
+
+    sa_x = sa * math.cos(syaw)
+    sa_y = sa * math.sin(syaw)
+    ga_x = ga * math.cos(gyaw)
+    ga_y = ga * math.sin(gyaw)
+
+    path = Trajectory()
+
+    for T in np.arange(MIN_T, MAX_T, T_STEP):
+        path = Trajectory()
+        xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T)
+        yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T)
+
+        for t in np.arange(0.0, T + dt, dt):
+            path.t.append(t)
+            path.x.append(xqp.calc_xt(t))
+            path.y.append(yqp.calc_xt(t))
+
+            vx = xqp.calc_dxt(t)
+            vy = yqp.calc_dxt(t)
+            path.v.append(np.hypot(vx, vy))
+            path.yaw.append(math.atan2(vy, vx))
+
+            ax = xqp.calc_ddxt(t)
+            ay = yqp.calc_ddxt(t)
+            a = np.hypot(ax, ay)
+
+            if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0:
+                a *= -1
+            path.a.append(a)
+
+            jx = xqp.calc_dddxt(t)
+            jy = yqp.calc_dddxt(t)
+            j = np.hypot(jx, jy)
+
+            if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0:
+                j *= -1
+            path.jerk.append(j)
+
+        if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK:
+            break
+
+    print("t_len: ", path.t, "s")
+    print("max_v: ", max(path.v), "m/s")
+    print("max_a: ", max(np.abs(path.a)), "m/s2")
+    print("max_jerk: ", max(np.abs(path.jerk)), "m/s3")
+
+    for i in range(len(path.t)):
+        plt.cla()
+        plt.gcf().canvas.mpl_connect('key_release_event',
+                                     lambda event: [exit(0) if event.key == 'escape' else None])
+        plt.axis("equal")
+        plt.plot(path.x, path.y, linewidth=2, color='gray')
+        draw.Car(sx, sy, syaw, 1.5, 3)
+        draw.Car(gx, gy, gyaw, 1.5, 3)
+        draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3)
+        plt.title("Quintic Polynomial Curves")
+        plt.grid(True)
+        plt.pause(0.001)
+
+    plt.show()
+
+
+if __name__ == '__main__':
+    simulation()

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 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():
+    # choose states pairs: (x, 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()