clean up clothoidal_paths.py (AtsushiSakai#638)

* clean up clothoidal_paths.py

* add unit tests for clothoid_paths

* add unit tests for clothoid_paths

* add unit tests for clothoid_paths

* code clean up

* code clean up

* code clean up

* code clean up

* code clean up

* code clean up

* code clean up

* code clean up

Author: AtsushiSakai

PathPlanning/ClothoidPath/clothoid_path_planner.py

@@ -0,0 +1,192 @@
+"""
+Clothoid Path Planner
+Author: Daniel Ingram (daniel-s-ingram)
+        Atsushi Sakai (AtsushiSakai)
+Reference paper: Fast and accurate G1 fitting of clothoid curves
+https://www.researchgate.net/publication/237062806
+"""
+
+from collections import namedtuple
+import matplotlib.pyplot as plt
+import numpy as np
+import scipy.integrate as integrate
+from scipy.optimize import fsolve
+from math import atan2, cos, hypot, pi, sin
+from matplotlib import animation
+
+Point = namedtuple("Point", ["x", "y"])
+
+show_animation = True
+
+
+def generate_clothoid_paths(start_point, start_yaw_list,
+                            goal_point, goal_yaw_list,
+                            n_path_points):
+    """
+    Generate clothoid path list. This function generate multiple clothoid paths
+    from multiple orientations(yaw) at start points to multiple orientations
+    (yaw) at goal point.
+
+    :param start_point: Start point of the path
+    :param start_yaw_list: Orientation list at start point in radian
+    :param goal_point: Goal point of the path
+    :param goal_yaw_list: Orientation list at goal point in radian
+    :param n_path_points: number of path points
+    :return: clothoid path list
+    """
+    clothoids = []
+    for start_yaw in start_yaw_list:
+        for goal_yaw in goal_yaw_list:
+            clothoid = generate_clothoid_path(start_point, start_yaw,
+                                              goal_point, goal_yaw,
+                                              n_path_points)
+            clothoids.append(clothoid)
+    return clothoids
+
+
+def generate_clothoid_path(start_point, start_yaw,
+                           goal_point, goal_yaw, n_path_points):
+    """
+    Generate a clothoid path list.
+
+    :param start_point: Start point of the path
+    :param start_yaw: Orientation at start point in radian
+    :param goal_point: Goal point of the path
+    :param goal_yaw: Orientation at goal point in radian
+    :param n_path_points: number of path points
+    :return: a clothoid path
+    """
+    dx = goal_point.x - start_point.x
+    dy = goal_point.y - start_point.y
+    r = hypot(dx, dy)
+
+    phi = atan2(dy, dx)
+    phi1 = normalize_angle(start_yaw - phi)
+    phi2 = normalize_angle(goal_yaw - phi)
+    delta = phi2 - phi1
+
+    try:
+        # Step1: Solve g function
+        A = solve_g_for_root(phi1, phi2, delta)
+
+        # Step2: Calculate path parameters
+        L = compute_path_length(r, phi1, delta, A)
+        curvature = compute_curvature(delta, A, L)
+        curvature_rate = compute_curvature_rate(A, L)
+    except Exception as e:
+        print(f"Failed to generate clothoid points: {e}")
+        return None
+
+    # Step3: Construct a path with Fresnel integral
+    points = []
+    for s in np.linspace(0, L, n_path_points):
+        try:
+            x = start_point.x + s * X(curvature_rate * s ** 2, curvature * s,
+                                      start_yaw)
+            y = start_point.y + s * Y(curvature_rate * s ** 2, curvature * s,
+                                      start_yaw)
+            points.append(Point(x, y))
+        except Exception as e:
+            print(f"Skipping failed clothoid point: {e}")
+
+    return points
+
+
+def X(a, b, c):
+    return integrate.quad(lambda t: cos((a/2)*t**2 + b*t + c), 0, 1)[0]
+
+
+def Y(a, b, c):
+    return integrate.quad(lambda t: sin((a/2)*t**2 + b*t + c), 0, 1)[0]
+
+
+def solve_g_for_root(theta1, theta2, delta):
+    initial_guess = 3*(theta1 + theta2)
+    return fsolve(lambda A: Y(2*A, delta - A, theta1), [initial_guess])
+
+
+def compute_path_length(r, theta1, delta, A):
+    return r / X(2*A, delta - A, theta1)
+
+
+def compute_curvature(delta, A, L):
+    return (delta - A) / L
+
+
+def compute_curvature_rate(A, L):
+    return 2 * A / (L**2)
+
+
+def normalize_angle(angle_rad):
+    return (angle_rad + pi) % (2 * pi) - pi
+
+
+def get_axes_limits(clothoids):
+    x_vals = [p.x for clothoid in clothoids for p in clothoid]
+    y_vals = [p.y for clothoid in clothoids for p in clothoid]
+
+    x_min = min(x_vals)
+    x_max = max(x_vals)
+    y_min = min(y_vals)
+    y_max = max(y_vals)
+
+    x_offset = 0.1*(x_max - x_min)
+    y_offset = 0.1*(y_max - y_min)
+
+    x_min = x_min - x_offset
+    x_max = x_max + x_offset
+    y_min = y_min - y_offset
+    y_max = y_max + y_offset
+
+    return x_min, x_max, y_min, y_max
+
+
+def draw_clothoids(start, goal, num_steps, clothoidal_paths,
+                   save_animation=False):
+
+    fig = plt.figure(figsize=(10, 10))
+    x_min, x_max, y_min, y_max = get_axes_limits(clothoidal_paths)
+    axes = plt.axes(xlim=(x_min, x_max), ylim=(y_min, y_max))
+
+    axes.plot(start.x, start.y, 'ro')
+    axes.plot(goal.x, goal.y, 'ro')
+    lines = [axes.plot([], [], 'b-')[0] for _ in range(len(clothoidal_paths))]
+
+    def animate(i):
+        for line, clothoid_path in zip(lines, clothoidal_paths):
+            x = [p.x for p in clothoid_path[:i]]
+            y = [p.y for p in clothoid_path[:i]]
+            line.set_data(x, y)
+
+        return lines
+
+    anim = animation.FuncAnimation(
+        fig,
+        animate,
+        frames=num_steps,
+        interval=25,
+        blit=True
+    )
+    if save_animation:
+        anim.save('clothoid.gif', fps=30, writer="imagemagick")
+    plt.show()
+
+
+def main():
+    start_point = Point(0, 0)
+    start_orientation_list = [0.0]
+    goal_point = Point(10, 0)
+    goal_orientation_list = np.linspace(-pi, pi, 75)
+    num_path_points = 100
+    clothoid_paths = generate_clothoid_paths(
+        start_point, start_orientation_list,
+        goal_point, goal_orientation_list,
+        num_path_points)
+    if show_animation:
+        draw_clothoids(start_point, goal_point,
+                       num_path_points, clothoid_paths,
+                       save_animation=False)
+
+
+if __name__ == "__main__":
+    main()

PathPlanning/ClothoidalPaths/clothoidal_paths.py

@@ -34,7 +34,7 @@ So
     & \ddot{\theta} = \frac{g(M + m)sin{\theta} - \dot{\theta}^2lmsin{\theta}cos{\theta} + ucos{\theta}}{l(M + m - mcos^2{\theta})}
 
 
-Linearlied model when :math:`\theta` small, :math:`cos{\theta} \approx 1`, :math:`sin{\theta} \approx \theta`, :math:`\dot{\theta}^2 \approx 0`.
+Linearized model when :math:`\theta` small, :math:`cos{\theta} \approx 1`, :math:`sin{\theta} \approx \theta`, :math:`\dot{\theta}^2 \approx 0`.
 
 .. math::
     & \ddot{x} =  \frac{gm}{M}\theta + \frac{1}{M}u\\
@@ -68,30 +68,34 @@ If control x and \theta
 LQR control
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-The LQR cotroller minimize this cost function defined as:
+The LQR controller minimize this cost function defined as:
 
 .. math::  J = x^T Q x + u^T R u
+
 the feedback control law that minimizes the value of the cost is:
 
 .. math::  u = -K x
+
 where:
 
 .. math::  K = (B^T P B + R)^{-1} B^T P A
-and :math:`P` is the unique positive definite solution to the discrete time `algebraic Riccati equation <https://en.wikipedia.org/wiki/Inverted_pendulum#From_Lagrange's_equations>`__  (DARE):
+
+and :math:`P` is the unique positive definite solution to the discrete time
+`algebraic Riccati equation <https://en.wikipedia.org/wiki/Inverted_pendulum#From_Lagrange's_equations>`__  (DARE):
 
 .. math::  P = A^T P A - A^T P B ( R + B^T P B )^{-1} B^T P A + Q
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation_lqr.gif
 
 MPC control
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~
-The MPC cotroller minimize this cost function defined as:
+The MPC controller minimize this cost function defined as:
 
 .. math:: J = x^T Q x + u^T R u
 
 subject to:
 
-- Linearlied Inverted Pendulum model
+- Linearized Inverted Pendulum model
 - Initial state
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif