diff --git a/src/simulations/course/cubic_spline/cubic_spline_2d.png b/src/simulations/course/cubic_spline/cubic_spline_2d.png new file mode 100644 index 0000000..58be878 Binary files /dev/null and b/src/simulations/course/cubic_spline/cubic_spline_2d.png differ diff --git a/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py b/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py index f69f632..37def8e 100644 --- a/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py +++ b/src/simulations/course/cubic_spline/cubic_spline_2d_plot.py @@ -1,452 +1,58 @@ -# """ -# cubic_spline_2d_plot.py - -# Author: Shisato Yano -# """ - -# # import path setting -# import numpy as np -# import sys -# import matplotlib.pyplot as plt -# from pathlib import Path - -# abs_dir_path = str(Path(__file__).absolute().parent) -# relative_path = "/../../../components/" - -# sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course") - -# # import component module -# from cubic_spline_2d import CubicSpline2D - - -# # flag to show plot figure -# # when executed as unit test, this flag is set as false -# show_plot = True - - -# def main(): -# """ -# Main process function -# """ - -# x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] -# y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - -# ds = 0.1 # distance between 2 interpolated points - -# cs = CubicSpline2D(x_points, y_points) -# s = np.arange(0, cs.s[-1], ds) - -# xs, ys, yaws, curvs = [], [], [], [] -# for i_s in s: -# i_x, i_y = cs.calc_interpolated_xy(i_s) -# xs.append(i_x) -# ys.append(i_y) - -# plt.subplots(1) -# plt.plot(x_points, y_points, "xb", label="Input points") -# # plt.plot(xs, ys, "-r", label="Cubic spline path") -# plt.grid(True) -# plt.axis("equal") -# plt.xlabel("X[m]") -# plt.ylabel("Y[m]") -# plt.legend() - -# if show_plot: plt.savefig("cubic_spline_2d.png") - - -# if __name__ == "__main__": -# main() - """ -Cubic spline planner - -Author: Atsushi Sakai(@Atsushi_twi) +cubic_spline_2d_plot.py +Author: Shisato Yano """ -import math -import numpy as np -import bisect - - -class CubicSpline1D: - """ - 1D Cubic Spline class - - Parameters - ---------- - x : list - x coordinates for data points. This x coordinates must be - sorted - in ascending order. - y : list - y coordinates for data points - - Examples - -------- - You can interpolate 1D data points. - - >>> import numpy as np - >>> import matplotlib.pyplot as plt - >>> x = np.arange(5) - >>> y = [1.7, -6, 5, 6.5, 0.0] - >>> sp = CubicSpline1D(x, y) - >>> xi = np.linspace(0.0, 5.0) - >>> yi = [sp.calc_position(x) for x in xi] - >>> plt.plot(x, y, "xb", label="Data points") - >>> plt.plot(xi, yi , "r", label="Cubic spline interpolation") - >>> plt.grid(True) - >>> plt.legend() - >>> plt.show() - - .. image:: cubic_spline_1d.png - - """ - - def __init__(self, x, y): - - h = np.diff(x) - if np.any(h < 0): - raise ValueError("x coordinates must be sorted in ascending order") - - self.a, self.b, self.c, self.d = [], [], [], [] - self.x = x - self.y = y - self.nx = len(x) # dimension of x - - # calc coefficient a - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h, self.a) - self.c = np.linalg.solve(A, B) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i]) - b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \ - - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1]) - self.d.append(d) - self.b.append(b) - - def calc_position(self, x): - """ - Calc `y` position for given `x`. - - if `x` is outside the data point's `x` range, return None. - - Returns - ------- - y : float - y position for given x. - """ - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - position = self.a[i] + self.b[i] * dx + \ - self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 - - return position - - def calc_first_derivative(self, x): - """ - Calc first derivative at given x. - - if x is outside the input x, return None - - Returns - ------- - dy : float - first derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None - - i = self.__search_index(x) - dx = x - self.x[i] - dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 - return dy - - def calc_second_derivative(self, x): - """ - Calc second derivative at given x. - if x is outside the input x, return None - - Returns - ------- - ddy : float - second derivative for given x. - """ - - if x < self.x[0]: - return None - elif x > self.x[-1]: - return None +# import path setting +import numpy as np +import sys +import matplotlib.pyplot as plt +from pathlib import Path - i = self.__search_index(x) - dx = x - self.x[i] - ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return ddy +abs_dir_path = str(Path(__file__).absolute().parent) +relative_path = "/../../../components/" - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x, x) - 1 +sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course") - def __calc_A(self, h): - """ - 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] +# import component module +from cubic_spline_2d import CubicSpline2D - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - return A - def __calc_B(self, h, a): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\ - - 3.0 * (a[i + 1] - a[i]) / h[i] - return B +# flag to show plot figure +# when executed as unit test, this flag is set as false +show_plot = True -class CubicSpline2D: +def main(): """ - Cubic CubicSpline2D class - - Parameters - ---------- - x : list - x coordinates for data points. - y : list - y coordinates for data points. - - Examples - -------- - You can interpolate a 2D data points. - - >>> 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] - >>> ds = 0.1 # [m] distance of each interpolated points - >>> sp = CubicSpline2D(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)) - >>> plt.subplots(1) - >>> plt.plot(x, y, "xb", label="Data points") - >>> plt.plot(rx, ry, "-r", label="Cubic spline path") - >>> plt.grid(True) - >>> plt.axis("equal") - >>> plt.xlabel("x[m]") - >>> plt.ylabel("y[m]") - >>> plt.legend() - >>> plt.show() - - .. image:: cubic_spline_2d_path.png - - >>> plt.subplots(1) - >>> plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - >>> plt.grid(True) - >>> plt.legend() - >>> plt.xlabel("line length[m]") - >>> plt.ylabel("yaw angle[deg]") - - .. image:: cubic_spline_2d_yaw.png - - >>> 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]") - - .. image:: cubic_spline_2d_curvature.png + Main process function """ - def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = CubicSpline1D(self.s, x) - self.sy = CubicSpline1D(self.s, y) - - def __calc_s(self, x, y): - dx = np.diff(x) - dy = np.diff(y) - self.ds = np.hypot(dx, dy) - s = [0] - s.extend(np.cumsum(self.ds)) - return s - - def calc_position(self, s): - """ - calc position - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - x : float - x position for given s. - y : float - y position for given s. - """ - x = self.sx.calc_position(s) - y = self.sy.calc_position(s) - - return x, y - - def calc_curvature(self, s): - """ - calc curvature - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - k : float - curvature for given s. - """ - dx = self.sx.calc_first_derivative(s) - ddx = self.sx.calc_second_derivative(s) - dy = self.sy.calc_first_derivative(s) - ddy = self.sy.calc_second_derivative(s) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - return k - - def calc_yaw(self, s): - """ - calc yaw - - Parameters - ---------- - s : float - distance from the start point. if `s` is outside the data point's - range, return None. - - Returns - ------- - yaw : float - yaw angle (tangent vector) for given s. - """ - dx = self.sx.calc_first_derivative(s) - dy = self.sy.calc_first_derivative(s) - yaw = math.atan2(dy, dx) - return yaw - + x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] -def calc_spline_course(x, y, ds=0.1): - sp = CubicSpline2D(x, y) - s = list(np.arange(0, sp.s[-1], ds)) + ds = 0.1 # distance between 2 interpolated points - 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 main_1d(): - print("CubicSpline1D test") - import matplotlib.pyplot as plt - x = np.arange(5) - y = [1.7, -6, 5, 6.5, 0.0] - sp = CubicSpline1D(x, y) - xi = np.linspace(0.0, 5.0) - - plt.plot(x, y, "xb", label="Data points") - plt.plot(xi, [sp.calc_position(x) for x in xi], "r", - label="Cubic spline interpolation") - plt.grid(True) - plt.legend() - plt.show() - - -def main_2d(): # pragma: no cover - print("CubicSpline1D 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] - ds = 0.1 # [m] distance of each interpolated points + cs = CubicSpline2D(x_points, y_points) + s = np.arange(0, cs.s[-1], ds) - sp = CubicSpline2D(x, y) - s = np.arange(0, sp.s[-1], ds) - - rx, ry, ryaw, rk = [], [], [], [] + xs, ys, yaws, curvs = [], [], [], [] 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)) - + i_x, i_y = cs.calc_interpolated_xy(i_s) + xs.append(i_x) + ys.append(i_y) + plt.subplots(1) - plt.plot(x, y, "xb", label="Data points") - plt.plot(rx, ry, "-r", label="Cubic spline path") + plt.plot(x_points, y_points, "xb", label="Input points") + plt.plot(xs, ys, "-r", label="Cubic spline path") plt.grid(True) plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") + plt.xlabel("X[m]") + plt.ylabel("Y[m]") plt.legend() - - plt.savefig("cubic_spline_2d.png") - - plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("yaw angle[deg]") - - plt.savefig("cubic_spline_yaw_angle.png") - - 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.savefig("cubic_spline_curvature.png") + if show_plot: plt.savefig("cubic_spline_2d.png") -if __name__ == '__main__': - # main_1d() - main_2d() \ No newline at end of file +if __name__ == "__main__": + main()