Merge branch 'master' into dependabot/pip/requirements/cvxpy-1.6.0

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py

@@ -203,10 +203,10 @@ def calc_heuristic_map(M, goal_node):
     for i in range(heuristic_map.shape[0]):
         for j in range(heuristic_map.shape[1]):
             heuristic_map[i, j] = min(heuristic_map[i, j],
-                                      i + 1 + heuristic_map[M - 1, j],
-                                      M - i + heuristic_map[0, j],
-                                      j + 1 + heuristic_map[i, M - 1],
-                                      M - j + heuristic_map[i, 0]
+                                      M - i - 1 + heuristic_map[M - 1, j],
+                                      i + heuristic_map[0, j],
+                                      M - j - 1 + heuristic_map[i, M - 1],
+                                      j + heuristic_map[i, 0]
                                       )
 
     return heuristic_map

ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py

@@ -105,7 +105,7 @@ def get_occupancy_grid(arm, obstacles):
     Args:
         arm: An instance of NLinkArm
         obstacles: A list of obstacles, with each obstacle defined as a list
-                   of xy coordinates and a radius. 
+                   of xy coordinates and a radius.
 
     Returns:
         Occupancy grid in joint space
@@ -234,10 +234,10 @@ def calc_heuristic_map(M, goal_node):
     for i in range(heuristic_map.shape[0]):
         for j in range(heuristic_map.shape[1]):
             heuristic_map[i, j] = min(heuristic_map[i, j],
-                                      i + 1 + heuristic_map[M - 1, j],
-                                      M - i + heuristic_map[0, j],
-                                      j + 1 + heuristic_map[i, M - 1],
-                                      M - j + heuristic_map[i, 0]
+                                      M - i - 1 + heuristic_map[M - 1, j],
+                                      i + heuristic_map[0, j],
+                                      M - j - 1 + heuristic_map[i, M - 1],
+                                      j + heuristic_map[i, 0]
                                       )
 
     return heuristic_map

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

PathPlanning/HybridAStar/car.py

@@ -97,7 +97,7 @@ def pi_2_pi(angle):
 def move(x, y, yaw, distance, steer, L=WB):
     x += distance * cos(yaw)
     y += distance * sin(yaw)
-    yaw += pi_2_pi(distance * tan(steer) / L)  # distance/2
+    yaw = pi_2_pi(yaw + distance * tan(steer) / L)  # distance/2
 
     return x, y, yaw
 

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -105,12 +105,13 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
     x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1]
 
     arc_l = XY_GRID_RESOLUTION * 1.5
-    x_list, y_list, yaw_list = [], [], []
+    x_list, y_list, yaw_list, direction_list = [], [], [], []
     for _ in np.arange(0, arc_l, MOTION_RESOLUTION):
         x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
         x_list.append(x)
         y_list.append(y)
         yaw_list.append(yaw)
+        direction_list.append(direction == 1)
 
     if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
         return None
@@ -134,7 +135,7 @@ def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
     cost = current.cost + added_cost + arc_l
 
     node = Node(x_ind, y_ind, yaw_ind, d, x_list,
-                y_list, yaw_list, [d],
+                y_list, yaw_list, direction_list,
                 parent_index=calc_index(current, config),
                 cost=cost, steer=steer)
 
@@ -281,7 +282,7 @@ def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
     while True:
         if not openList:
             print("Error: Cannot find path, No open set")
-            return [], [], []
+            return Path([], [], [], [], 0)
 
         cost, c_id = heapq.heappop(pq)
         if c_id in openList:

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -6,6 +6,10 @@
 co-author Videh Patel(@videh25) : Added the missing RS paths
 
 """
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+
 import math
 
 import matplotlib.pyplot as plt

PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py

@@ -64,7 +64,7 @@ def transform(
     is_visited = np.zeros_like(transform_matrix, dtype=bool)
     is_visited[src[0], src[1]] = True
     traversal_queue = [src]
-    calculated = [(src[0] - 1) * n_cols + src[1]]
+    calculated = set([(src[0] - 1) * n_cols + src[1]])
 
     def is_valid_neighbor(g_i, g_j):
         return 0 <= g_i < n_rows and 0 <= g_j < n_cols \
@@ -86,7 +86,7 @@ def is_valid_neighbor(g_i, g_j):
                 if not is_visited[ni][nj] \
                         and ((ni - 1) * n_cols + nj) not in calculated:
                     traversal_queue.append((ni, nj))
-                    calculated.append((ni - 1) * n_cols + nj)
+                    calculated.add((ni - 1) * n_cols + nj)
 
     return transform_matrix
 

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

docs/modules/path_planning/path_planning_main.rst

@@ -18,6 +18,7 @@ Path Planning
    rrt/rrt
    cubic_spline/cubic_spline
    bspline_path/bspline_path
+   catmull_rom_spline/catmull_rom_spline
    clothoid_path/clothoid_path
    eta3_spline/eta3_spline
    bezier_path/bezier_path

requirements/requirements.txt

@@ -1,8 +1,8 @@
 numpy == 2.2.0
 scipy == 1.14.1
-matplotlib == 3.9.3
+matplotlib == 3.10.0
 cvxpy == 1.6.0
 pytest == 8.3.4 # For unit test
 pytest-xdist == 3.6.1 # For unit test
 mypy == 1.13.0 # For unit test
-ruff == 0.8.2 # For unit test
+ruff == 0.8.3 # For unit test

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