diff --git a/.github/workflows/Linux_CI.yml b/.github/workflows/Linux_CI.yml index 73f8aa5c..83c55b17 100644 --- a/.github/workflows/Linux_CI.yml +++ b/.github/workflows/Linux_CI.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - python-version: ['3.10'] + python-version: [ '3.10.6' ] # For mypy error Ref: https://github.com/python/mypy/issues/13627 name: Python ${{ matrix.python-version }} CI diff --git a/.github/workflows/MacOS_CI.yml b/.github/workflows/MacOS_CI.yml index 5c796120..a3459f25 100644 --- a/.github/workflows/MacOS_CI.yml +++ b/.github/workflows/MacOS_CI.yml @@ -16,7 +16,7 @@ jobs: runs-on: macos-latest strategy: matrix: - python-version: [ '3.10' ] + python-version: [ '3.10.6' ] # For mypy error Ref: https://github.com/python/mypy/issues/13627 name: Python ${{ matrix.python-version }} CI steps: - uses: actions/checkout@v2 diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py index d4b24c76..a2a396ef 100644 --- a/PathPlanning/BSplinePath/bspline_path.py +++ b/PathPlanning/BSplinePath/bspline_path.py @@ -5,57 +5,114 @@ Path Planner with B-Spline author: Atsushi Sakai (@Atsushi_twi) """ +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.parent.parent)) import numpy as np import matplotlib.pyplot as plt -import scipy.interpolate as scipy_interpolate +import scipy.interpolate as interpolate + +from utils.plot import plot_curvature -def approximate_b_spline_path(x: list, y: list, n_path_points: int, +def approximate_b_spline_path(x: list, + y: list, + n_path_points: int, + degree: int = 3, + s=None, + ) -> tuple: + """ + Approximate points with a B-Spline path + + Parameters + ---------- + x : array_like + x position list of approximated points + y : array_like + y position list of approximated points + n_path_points : int + number of path points + degree : int, optional + B Spline curve degree. Must be 2<= k <= 5. Default: 3. + s : int, optional + smoothing parameter. If this value is bigger, the path will be + smoother, but it will be less accurate. If this value is smaller, + the path will be more accurate, but it will be less smooth. + When `s` is 0, it is equivalent to the interpolation. Default is None, + in this case `s` will be `len(x)`. + + Returns + ------- + x : array + x positions of the result path + y : array + y positions of the result path + heading : array + heading of the result path + curvature : array + curvature of the result path + + """ + distances = _calc_distance_vector(x, y) + + spl_i_x = interpolate.UnivariateSpline(distances, x, k=degree, s=s) + spl_i_y = interpolate.UnivariateSpline(distances, y, k=degree, s=s) + + sampled = np.linspace(0.0, distances[-1], n_path_points) + return _evaluate_spline(sampled, spl_i_x, spl_i_y) + + +def interpolate_b_spline_path(x, y, + n_path_points: int, degree: int = 3) -> tuple: """ - approximate points with a B-Spline path + Interpolate x-y points with a B-Spline path + + Parameters + ---------- + x : array_like + x positions of interpolated points + y : array_like + y positions of interpolated points + n_path_points : int + number of path points + degree : int, optional + B-Spline degree. Must be 2<= k <= 5. Default: 3 + + Returns + ------- + x : array + x positions of the result path + y : array + y positions of the result path + heading : array + heading of the result path + curvature : array + curvature of the result path - :param x: x position list of approximated points - :param y: y position list of approximated points - :param n_path_points: number of path points - :param degree: (Optional) B Spline curve degree - :return: x and y position list of the result path """ - 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 + return approximate_b_spline_path(x, y, n_path_points, degree, s=0.0) -def interpolate_b_spline_path(x: list, y: list, n_path_points: int, - degree: int = 3) -> tuple: - """ - interpolate points with a B-Spline path +def _calc_distance_vector(x, y): + dx, dy = np.diff(x), np.diff(y) + distances = np.cumsum([np.hypot(idx, idy) for idx, idy in zip(dx, dy)]) + distances = np.concatenate(([0.0], distances)) + distances /= distances[-1] + return distances - :param x: x positions of interpolated points - :param y: y positions of interpolated points - :param n_path_points: number of path points - :param degree: B-Spline degree - :return: x and y position list of the result path - """ - 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 _evaluate_spline(sampled, spl_i_x, spl_i_y): + x = spl_i_x(sampled) + y = spl_i_y(sampled) + dx = spl_i_x.derivative(1)(sampled) + dy = spl_i_y.derivative(1)(sampled) + heading = np.arctan2(dy, dx) + ddx = spl_i_x.derivative(2)(sampled) + ddy = spl_i_y.derivative(2)(sampled) + curvature = (ddy * dx - ddx * dy) / np.power(dx * dx + dy * dy, 2.0 / 3.0) + return np.array(x), y, heading, curvature, def main(): @@ -63,17 +120,28 @@ def main(): # 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] - n_course_point = 100 # sampling number + n_course_point = 50 # 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="way points") + plt.subplots() + rax, ray, heading, curvature = approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5) plt.plot(rax, ray, '-r', label="Approximated B-Spline path") + plot_curvature(rax, ray, heading, curvature) + + plt.title("B-Spline approximation") + plt.plot(way_point_x, way_point_y, '-og', label="way points") + plt.grid(True) + plt.legend() + plt.axis("equal") + + plt.subplots() + rix, riy, heading, curvature = interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point) plt.plot(rix, riy, '-b', label="Interpolated B-Spline path") + plot_curvature(rix, riy, heading, curvature) + + plt.title("B-Spline interpolation") + plt.plot(way_point_x, way_point_y, '-og', label="way points") plt.grid(True) plt.legend() plt.axis("equal") diff --git a/docs/index_main.rst b/docs/index_main.rst index 15175928..67bd9889 100644 --- a/docs/index_main.rst +++ b/docs/index_main.rst @@ -44,6 +44,7 @@ See this paper for more details: modules/aerial_navigation/aerial_navigation modules/bipedal/bipedal modules/control/control + modules/utils/utils modules/appendix/appendix how_to_contribute diff --git a/docs/modules/path_planning/bspline_path/Figure_1.png b/docs/modules/path_planning/bspline_path/Figure_1.png deleted file mode 100644 index 539854ac..00000000 Binary files a/docs/modules/path_planning/bspline_path/Figure_1.png and /dev/null differ diff --git a/docs/modules/path_planning/bspline_path/approx_and_curvature.png b/docs/modules/path_planning/bspline_path/approx_and_curvature.png new file mode 100644 index 00000000..49f260bb Binary files /dev/null and b/docs/modules/path_planning/bspline_path/approx_and_curvature.png differ diff --git a/docs/modules/path_planning/bspline_path/approximation1.png b/docs/modules/path_planning/bspline_path/approximation1.png new file mode 100644 index 00000000..d5f00551 Binary files /dev/null and b/docs/modules/path_planning/bspline_path/approximation1.png differ diff --git a/docs/modules/path_planning/bspline_path/basis_functions.png b/docs/modules/path_planning/bspline_path/basis_functions.png new file mode 100644 index 00000000..65075c80 Binary files /dev/null and b/docs/modules/path_planning/bspline_path/basis_functions.png differ diff --git a/docs/modules/path_planning/bspline_path/bspline_path_main.rst b/docs/modules/path_planning/bspline_path/bspline_path_main.rst index d3523a19..e734352e 100644 --- a/docs/modules/path_planning/bspline_path/bspline_path_main.rst +++ b/docs/modules/path_planning/bspline_path/bspline_path_main.rst @@ -1,14 +1,146 @@ B-Spline planning ----------------- -.. image:: Figure_1.png +.. image:: bspline_path_planning.png -This is a path planning with B-Spline curse. +This is a B-Spline path planning routines. If you input waypoints, it generates a smooth path with B-Spline curve. -The final course should be on the first and last waypoints. +This codes provide two types of B-Spline curve generations: -Ref: +1. Interpolation: generate a curve that passes through all waypoints. + +2. Approximation: generate a curve that approximates the waypoints. (Not passing through all waypoints) + +Bspline basics +~~~~~~~~~~~~~~ + +BSpline (Basis-Spline) is a piecewise polynomial spline curve. + +It is expressed by the following equation. + +:math:`\mathbf{S}(x)=\sum_{i=k-p}^k \mathbf{c}_i B_{i, p}(x)` + +here: + +* :math:`S(x)` is the curve point on the spline at x. +* :math:`c_i` is the representative point generating the spline, called the control point. +* :math:`p+1` is the dimension of the BSpline. +* :math:`k` is the number of knots. +* :math:`B_{i,p}(x)` is a function called Basis Function. + +The the basis function can be calculated by the following `De Boor recursion formula `_: + +:math:`B_{i, 0}(x):= \begin{cases}1 & \text { if } \quad t_i \leq x`_. + +.. code-block:: python + + from scipy.interpolate import BSpline + + def B_orig(x, k, i, t): + if k == 0: + return 1.0 if t[i] <= x < t[i + 1] else 0.0 + if t[i + k] == t[i]: + c1 = 0.0 + else: + c1 = (x - t[i]) / (t[i + k] - t[i]) * B(x, k - 1, i, t) + + if t[i + k + 1] == t[i + 1]: + c2 = 0.0 + else: + c2 = (t[i + k + 1] - x) / (t[i + k + 1] - t[i + 1]) * B(x, k - 1, i + 1, t) + return c1 + c2 + + + def B(x, k, i, t): + c = np.zeros_like(t) + c[i] = 1 + return BSpline(t, c, k)(x) + + + def main(): + k = 3 # degree of the spline + t = [0, 1, 2, 3, 4, 5] # knots vector + + x = np.linspace(0, 5, 1000, endpoint=False) + t = np.r_[[np.min(t)]*k, t, [np.max(t)]*k] + + n = len(t) - k - 1 + for i in range(n): + y = np.array([B(ix, k, i, t) for ix in x]) + plt.plot(x, y, label=f'i = {i}') + + plt.title(f'Basis functions (k = {k}, knots = {t})') + plt.show() + +Bspline interpolation planning +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +:meth:`PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path` generates a curve that passes through all waypoints. + +This is a simple example of the interpolation planning: + +.. image:: interpolation1.png + +This figure also shows curvatures of each path point using :ref:`utils.plot.plot_curvature `. + +The default spline degree is 3, so curvature changes smoothly. + +.. image:: interp_and_curvature.png + +API +++++ + +.. autofunction:: PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path + + +Bspline approximation planning +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +:meth:`PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path` +generates a curve that approximates the waypoints, which means that +the curve might not pass through waypoints. + +Users can adjust path smoothness by the smoothing parameter `s`. If this +value is bigger, the path will be smoother, but it will be less accurate. +If this value is smaller, the path will be more accurate, but it will be +less smooth. + +This is a simple example of the approximation planning: + +.. image:: approximation1.png + +This figure also shows curvatures of each path point using :ref:`utils.plot.plot_curvature `. + +The default spline degree is 3, so curvature changes smoothly. + +.. image:: approx_and_curvature.png + +API +++++ + +.. autofunction:: PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path + + +References +~~~~~~~~~~ - `B-spline - Wikipedia `__ +- `scipy.interpolate.UnivariateSpline `__ \ No newline at end of file diff --git a/docs/modules/path_planning/bspline_path/bspline_path_planning.png b/docs/modules/path_planning/bspline_path/bspline_path_planning.png new file mode 100644 index 00000000..a3c1e621 Binary files /dev/null and b/docs/modules/path_planning/bspline_path/bspline_path_planning.png differ diff --git a/docs/modules/path_planning/bspline_path/interp_and_curvature.png b/docs/modules/path_planning/bspline_path/interp_and_curvature.png new file mode 100644 index 00000000..54e29174 Binary files /dev/null and b/docs/modules/path_planning/bspline_path/interp_and_curvature.png differ diff --git a/docs/modules/path_planning/bspline_path/interpolation1.png b/docs/modules/path_planning/bspline_path/interpolation1.png new file mode 100644 index 00000000..fb280343 Binary files /dev/null and b/docs/modules/path_planning/bspline_path/interpolation1.png differ diff --git a/docs/modules/utils/plot/curvature_plot.png b/docs/modules/utils/plot/curvature_plot.png new file mode 100644 index 00000000..891d3008 Binary files /dev/null and b/docs/modules/utils/plot/curvature_plot.png differ diff --git a/docs/modules/utils/plot/plot_main.rst b/docs/modules/utils/plot/plot_main.rst new file mode 100644 index 00000000..b5fef0c4 --- /dev/null +++ b/docs/modules/utils/plot/plot_main.rst @@ -0,0 +1,16 @@ +.. _plot_utils: + +Plotting Utilities +------------------- + +This module contains a number of utility functions for plotting data. + +.. _plot_curvature: + +plot_curvature +~~~~~~~~~~~~~~~ + +.. autofunction:: utils.plot.plot_curvature + +.. image:: curvature_plot.png + diff --git a/docs/modules/utils/utils_main.rst b/docs/modules/utils/utils_main.rst new file mode 100644 index 00000000..ff79a262 --- /dev/null +++ b/docs/modules/utils/utils_main.rst @@ -0,0 +1,12 @@ +.. _utils: + +Utilities +========== + +Common utilities for PythonRobotics. + +.. toctree:: + :maxdepth: 2 + :caption: Contents + + plot/plot diff --git a/tests/test_bspline_path.py b/tests/test_bspline_path.py new file mode 100644 index 00000000..4918c981 --- /dev/null +++ b/tests/test_bspline_path.py @@ -0,0 +1,74 @@ +import conftest +import numpy as np +import pytest +from PathPlanning.BSplinePath import bspline_path + + +def test_list_input(): + 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] + n_course_point = 50 # sampling number + + rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5) + + assert len(rax) == len(ray) == len(heading) == len(curvature) + + rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point) + + assert len(rix) == len(riy) == len(heading) == len(curvature) + + +def test_array_input(): + way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) + way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) + n_course_point = 50 # sampling number + + rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5) + + assert len(rax) == len(ray) == len(heading) == len(curvature) + + rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point) + + assert len(rix) == len(riy) == len(heading) == len(curvature) + + +def test_degree_change(): + way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) + way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) + n_course_point = 50 # sampling number + + rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5, degree=4) + + assert len(rax) == len(ray) == len(heading) == len(curvature) + + rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point, degree=4) + + assert len(rix) == len(riy) == len(heading) == len(curvature) + + rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5, degree=2) + + assert len(rax) == len(ray) == len(heading) == len(curvature) + + rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point, degree=2) + + assert len(rix) == len(riy) == len(heading) == len(curvature) + + with pytest.raises(ValueError): + bspline_path.approximate_b_spline_path( + way_point_x, way_point_y, n_course_point, s=0.5, degree=1) + + with pytest.raises(ValueError): + bspline_path.interpolate_b_spline_path( + way_point_x, way_point_y, n_course_point, degree=1) + + +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/utils/plot.py b/utils/plot.py index 2c70a60b..34996948 100644 --- a/utils/plot.py +++ b/utils/plot.py @@ -3,6 +3,7 @@ Matplotlib based plotting utilities """ import math import matplotlib.pyplot as plt +import numpy as np def plot_arrow(x, y, yaw, arrow_length=1.0, @@ -47,3 +48,39 @@ def plot_arrow(x, y, yaw, arrow_length=1.0, **kwargs) if origin_point_plot_style is not None: plt.plot(x, y, origin_point_plot_style) + + +def plot_curvature(x_list, y_list, heading_list, curvature, + k=0.01, c="-c", label="Curvature"): + """ + Plot curvature on 2D path. This plot is a line from the original path, + the lateral distance from the original path shows curvature magnitude. + Left turning shows right side plot, right turning shows left side plot. + For straight path, the curvature plot will be on the path, because + curvature is 0 on the straight path. + + Parameters + ---------- + x_list : array_like + x position list of the path + y_list : array_like + y position list of the path + heading_list : array_like + heading list of the path + curvature : array_like + curvature list of the path + k : float + curvature scale factor to calculate distance from the original path + c : string + color of the plot + label : string + label of the plot + """ + cx = [x + d * k * np.cos(yaw - np.pi / 2.0) for x, y, yaw, d in + zip(x_list, y_list, heading_list, curvature)] + cy = [y + d * k * np.sin(yaw - np.pi / 2.0) for x, y, yaw, d in + zip(x_list, y_list, heading_list, curvature)] + + plt.plot(cx, cy, c, label=label) + for ix, iy, icx, icy in zip(x_list, y_list, cx, cy): + plt.plot([ix, icx], [iy, icy], c)