Enhance bspline code and doc clean up (#720)

* Start bspline code and doc cluean up

* code clean up

* code clean up

* code clean up

* code clean up

* improve doc

* improve doc

* improve doc

* improve doc

* improve doc

* improve doc

* improve doc

* improve doc

* improve codes.

* improve codes.

* improve codes.

.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
 

.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

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")

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
 

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 <https://en.wikipedia.org/wiki/De_Boor%27s_algorithm>`_:
+
+:math:`B_{i, 0}(x):= \begin{cases}1 & \text { if } \quad t_i \leq x<t_{i+1} \\ 0 & \text { otherwise }\end{cases}`
+
+:math:`B_{i, p}(x):=\frac{x-t_i}{t_{i+p}-t_i} B_{i, p-1}(x)+\frac{t_{i+p+1}-x}{t_{i+p+1}-t_{i+1}} B_{i+1, p-1}(x)`
+
+here:
+
+* :math:`t_i` is each element of the knot vector.
+
+This figure shows the BSpline basis functions for each of :math:`i`:
+
+.. image:: basis_functions.png
+
+Note that when all the basis functions are added together, summation is 1.0 for any x value.
+
+This means that the result curve is smooth when each control point is weighted addition by this basis function,
+
+This code is for generating the upper basis function graph using `scipy <https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.BSpline.html>`_.
+
+.. 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 <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 <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 <https://en.wikipedia.org/wiki/B-spline>`__
+-  `scipy.interpolate.UnivariateSpline <https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.UnivariateSpline.html>`__

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
+

docs/modules/utils/utils_main.rst

@@ -0,0 +1,12 @@
+.. _utils:
+
+Utilities
+==========
+
+Common utilities for PythonRobotics.
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents
+
+   plot/plot

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__)

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)