pathplanning 0.1.0__py3-none-any.whl

CurvesGenerator/__init__.py

@@ -0,0 +1 @@
+"""Curve generation utilities."""

CurvesGenerator/bezier_path.py

@@ -0,0 +1,145 @@
+"""
+bezier path
+
+author: Atsushi Sakai(@Atsushi_twi)
+modified: damminhtien
+"""
+
+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 s
+        h = np.diff(x)
+
+        # calc coefficient cBest
+        self.a = [iy for iy in y]
+
+        # calc coefficient cBest
+        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 s, 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 s, 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 cBest
+        """
+        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 cBest
+        """
+        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("s[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)