diff --git a/PathPlanning/Catmull_RomSplinePath/blending_functions.py b/PathPlanning/Catmull_RomSplinePath/blending_functions.py new file mode 100644 index 00000000..f3ef5dd3 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py b/PathPlanning/Catmull_RomSplinePath/catmull_rom_spline_path.py new file mode 100644 index 00000000..79916330 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/docs/modules/path_planning/catmull_rom_spline/blending_functions.png b/docs/modules/path_planning/catmull_rom_spline/blending_functions.png new file mode 100644 index 00000000..928946df Binary files /dev/null and b/docs/modules/path_planning/catmull_rom_spline/blending_functions.png differ diff --git a/docs/modules/path_planning/catmull_rom_spline/catmull_rom_path_planning.png b/docs/modules/path_planning/catmull_rom_spline/catmull_rom_path_planning.png new file mode 100644 index 00000000..1d0ff001 Binary files /dev/null and b/docs/modules/path_planning/catmull_rom_spline/catmull_rom_path_planning.png differ diff --git a/docs/modules/path_planning/catmull_rom_spline/catmull_rom_spline_main.rst b/docs/modules/path_planning/catmull_rom_spline/catmull_rom_spline_main.rst new file mode 100644 index 00000000..4d8d3bde --- /dev/null +++ b/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 `__ +- `Catmull-Rom Splines `__ \ No newline at end of file diff --git a/tests/test_catmull_rom_spline.py b/tests/test_catmull_rom_spline.py new file mode 100644 index 00000000..41a73588 --- /dev/null +++ b/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__)