Implement Catmull-Rom Spline with test and documentation (#1085)

PathPlanning/Catmull_RomSplinePath/blending_functions.py

@@ -0,0 +1,34 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+def blending_function_1(t):
+    return -t + 2*t**2 - t**3
+
+def blending_function_2(t):
+    return 2 - 5*t**2 + 3*t**3
+
+def blending_function_3(t):
+    return t + 4*t**2 - 3*t**3
+
+def blending_function_4(t):
+    return -t**2 + t**3
+
+def plot_blending_functions():
+    t = np.linspace(0, 1, 100)
+
+    plt.plot(t, blending_function_1(t), label='b1')
+    plt.plot(t, blending_function_2(t), label='b2')
+    plt.plot(t, blending_function_3(t), label='b3')
+    plt.plot(t, blending_function_4(t), label='b4')
+
+    plt.title("Catmull-Rom Blending Functions")
+    plt.xlabel("t")
+    plt.ylabel("Value")
+    plt.legend()
+    plt.grid(True)
+    plt.axhline(y=0, color='k', linestyle='--')
+    plt.axvline(x=0, color='k', linestyle='--')
+    plt.show()
+
+if __name__ == "__main__":
+    plot_blending_functions()

PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py

@@ -0,0 +1,86 @@
+"""
+Path Planner with Catmull-Rom Spline
+Author: Surabhi Gupta (@this_is_surabhi)
+Source: http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf
+"""
+
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+def catmull_rom_point(t, p0, p1, p2, p3):
+    """
+    Parameters
+    ----------
+    t : float
+        Parameter value (0 <= t <= 1) (0 <= t <= 1)
+    p0, p1, p2, p3 : np.ndarray
+        Control points for the spline segment
+
+    Returns
+    -------
+    np.ndarray
+        Calculated point on the spline
+    """
+    return 0.5 * ((2 * p1) +
+                  (-p0 + p2) * t +
+                  (2 * p0 - 5 * p1 + 4 * p2 - p3) * t**2 +
+                  (-p0 + 3 * p1 - 3 * p2 + p3) * t**3)
+
+
+def catmull_rom_spline(control_points, num_points):
+    """
+    Parameters
+    ----------
+    control_points : list
+        List of control points
+    num_points : int
+        Number of points to generate on the spline
+
+    Returns
+    -------
+    tuple
+        x and y coordinates of the spline points
+    """
+    t_vals = np.linspace(0, 1, num_points)
+    spline_points = []
+
+    control_points = np.array(control_points)
+
+    for i in range(len(control_points) - 1):
+        if i == 0:
+            p1, p2, p3 = control_points[i:i+3]
+            p0 = p1
+        elif i == len(control_points) - 2:
+            p0, p1, p2 = control_points[i-1:i+2]
+            p3 = p2
+        else:
+            p0, p1, p2, p3 = control_points[i-1:i+3]
+
+        for t in t_vals:
+            point = catmull_rom_point(t, p0, p1, p2, p3)
+            spline_points.append(point)
+
+    return np.array(spline_points).T
+
+
+def main():
+    print(__file__ + " start!!")
+
+    way_points = [[-1.0, -2.0], [1.0, -1.0], [3.0, -2.0], [4.0, -1.0], [3.0, 1.0], [1.0, 2.0], [0.0, 2.0]]
+    n_course_point = 100  
+    spline_x, spline_y = catmull_rom_spline(way_points, n_course_point)
+
+    plt.plot(spline_x,spline_y, '-r', label="Catmull-Rom Spline Path")
+    plt.plot(np.array(way_points).T[0], np.array(way_points).T[1], '-og', label="Way points")
+    plt.title("Catmull-Rom Spline Path")
+    plt.grid(True)
+    plt.legend()
+    plt.axis("equal")
+    plt.show()
+
+if __name__ == '__main__':
+    main()

docs/modules/path_planning/catmull_rom_spline/catmull_rom_spline_main.rst

@@ -0,0 +1,103 @@
+Catmull-Rom Spline Planning
+-----------------
+
+.. image:: catmull_rom_path_planning.png
+
+This is a Catmull-Rom spline path planning routine.
+
+If you provide waypoints, the Catmull-Rom spline generates a smooth path that always passes through the control points, 
+exhibits local control, and maintains 𝐶1 continuity.
+
+
+Catmull-Rom Spline Fundamentals
+~~~~~~~~~~~~~~
+
+Catmull-Rom splines are a type of cubic spline that passes through a given set of points, known as control points. 
+
+They are defined by the following equation for calculating a point on the spline:
+
+:math:`P(t) = 0.5 \times \left( 2P_1 + (-P_0 + P_2)t + (2P_0 - 5P_1 + 4P_2 - P_3)t^2 + (-P_0 + 3P_1 - 3P_2 + P_3)t^3 \right)`
+
+Where:
+
+* :math:`P(t)` is the point on the spline at parameter :math:`t`.
+* :math:`P_0, P_1, P_2, P_3` are the control points surrounding the parameter :math:`t`.
+
+Types of Catmull-Rom Splines
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+There are different types of Catmull-Rom splines based on the choice of the **tau** parameter, which influences how the curve 
+behaves in relation to the control points:
+
+1. **Uniform Catmull-Rom Spline**: 
+   The standard implementation where the parameterization is uniform. Each segment of the spline is treated equally, 
+   regardless of the distances between control points.
+
+
+2. **Chordal Catmull-Rom Spline**: 
+   This spline type takes into account the distance between control points. The parameterization is based on the actual distance 
+   along the spline, ensuring smoother transitions. The equation can be modified to include the chord length :math:`L_i` between 
+   points :math:`P_i` and :math:`P_{i+1}`:
+
+   .. math::
+       \tau_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+3. **Centripetal Catmull-Rom Spline**: 
+   This variation improves upon the chordal spline by using the square root of the distance to determine the parameterization, 
+   which avoids oscillations and creates a more natural curve. The parameter :math:`t_i` is adjusted using the following relation:
+
+   .. math::
+       t_i = \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}
+
+
+Blending Functions
+~~~~~~~~~~~~~~~~~~
+
+In Catmull-Rom spline interpolation, blending functions are used to calculate the influence of each control point on the spline at a 
+given parameter :math:`t`. The blending functions ensure that the spline is smooth and passes through the control points while 
+maintaining continuity. The four blending functions used in Catmull-Rom splines are defined as follows:
+
+1. **Blending Function 1**:
+
+   .. math::
+       b_1(t) = -t + 2t^2 - t^3
+
+2. **Blending Function 2**:
+
+   .. math::
+       b_2(t) = 2 - 5t^2 + 3t^3
+
+3. **Blending Function 3**:
+
+   .. math::
+       b_3(t) = t + 4t^2 - 3t^3
+
+4. **Blending Function 4**:
+
+   .. math::
+       b_4(t) = -t^2 + t^3
+
+The blending functions are combined in the spline equation to create a smooth curve that reflects the influence of each control point.
+
+The following figure illustrates the blending functions over the interval :math:`[0, 1]`:
+
+.. image:: blending_functions.png
+
+Catmull-Rom Spline API
+~~~~~~~~~~~~~~~~~~~~~~~
+
+This section provides an overview of the functions used for Catmull-Rom spline path planning.
+
+API
+++++
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point
+
+.. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_spline
+
+
+References
+~~~~~~~~~~
+
+-  `Catmull-Rom Spline - Wikipedia <https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline>`__
+-  `Catmull-Rom Splines <http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf>`__

tests/test_catmull_rom_spline.py

@@ -0,0 +1,16 @@
+import conftest
+from PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path import catmull_rom_spline
+
+def test_catmull_rom_spline():
+    way_points = [[0, 0], [1, 2], [2, 0], [3, 3]]
+    num_points = 100
+
+    spline_x, spline_y = catmull_rom_spline(way_points, num_points)
+
+    assert spline_x.size > 0, "Spline X coordinates should not be empty"
+    assert spline_y.size > 0, "Spline Y coordinates should not be empty"
+
+    assert spline_x.shape == spline_y.shape, "Spline X and Y coordinates should have the same shape"
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)