From 38261ec67efd65256f44cd92b88dba58502fc2f7 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 10 Apr 2022 19:30:31 +0900 Subject: [PATCH] clean up clothoidal_paths.py (#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 --- .../ClothoidPath/clothoid_path_planner.py | 192 ++++++++++++++++++ .../ClothoidalPaths/clothoidal_paths.py | 144 ------------- .../inverted_pendulum_control.rst | 14 +- .../clothoid_path/clothoid_path.rst | 80 ++++++++ .../path_planning/path_planning_main.rst | 1 + tests/test_clothoidal_paths.py | 11 + 6 files changed, 293 insertions(+), 149 deletions(-) create mode 100644 PathPlanning/ClothoidPath/clothoid_path_planner.py delete mode 100644 PathPlanning/ClothoidalPaths/clothoidal_paths.py create mode 100644 docs/modules/path_planning/clothoid_path/clothoid_path.rst create mode 100644 tests/test_clothoidal_paths.py diff --git a/PathPlanning/ClothoidPath/clothoid_path_planner.py b/PathPlanning/ClothoidPath/clothoid_path_planner.py new file mode 100644 index 00000000..5e5fc6e9 --- /dev/null +++ b/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() diff --git a/PathPlanning/ClothoidalPaths/clothoidal_paths.py b/PathPlanning/ClothoidalPaths/clothoidal_paths.py deleted file mode 100644 index 6fbf46ed..00000000 --- a/PathPlanning/ClothoidalPaths/clothoidal_paths.py +++ /dev/null @@ -1,144 +0,0 @@ -""" -Clothoidal Path Planner -Author: Daniel Ingram (daniel-s-ingram) -Reference paper: https://www.researchgate.net/publication/237062806 -""" - -import matplotlib.pyplot as plt -import numpy as np -import scipy.integrate as integrate -from collections import namedtuple -from scipy.optimize import fsolve -from math import atan2, cos, hypot, pi, sin -from matplotlib import animation - -Point = namedtuple("Point", ["x", "y"]) - - -def draw_clothoids( - theta1_vals, - theta2_vals, - num_clothoids=75, - num_steps=100, - save_animation=False -): - p1 = Point(0, 0) - p2 = Point(10, 0) - clothoids = [] - for theta1 in theta1_vals: - for theta2 in theta2_vals: - clothoid = get_clothoid_points(p1, p2, theta1, theta2, num_steps) - clothoids.append(clothoid) - - fig = plt.figure(figsize=(10, 10)) - x_min, x_max, y_min, y_max = get_axes_limits(clothoids) - axes = plt.axes(xlim=(x_min, x_max), ylim=(y_min, y_max)) - - axes.plot(p1.x, p1.y, 'ro') - axes.plot(p2.x, p2.y, 'ro') - lines = [axes.plot([], [], 'b-')[0] for _ in range(len(clothoids))] - - def animate(i): - for line, clothoid in zip(lines, clothoids): - x = [p.x for p in clothoid[:i]] - y = [p.y for p in clothoid[: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 get_clothoid_points(p1, p2, theta1, theta2, num_steps=100): - dx = p2.x - p1.x - dy = p2.y - p1.y - r = hypot(dx, dy) - - phi = atan2(dy, dx) - phi1 = normalize_angle(theta1 - phi) - phi2 = normalize_angle(theta2 - phi) - delta = phi2 - phi1 - - try: - A = solve_for_root(phi1, phi2, delta) - L = compute_length(r, phi1, delta, A) - curv = compute_curvature(delta, A, L) - curv_rate = compute_curvature_rate(A, L) - except Exception as e: - print(f"Failed to generate clothoid points: {e}") - return None - - points = [] - for s in np.linspace(0, L, num_steps): - try: - x = p1.x + s*X(curv_rate*s**2, curv*s, theta1) - y = p1.y + s*Y(curv_rate*s**2, curv*s, theta1) - 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_for_root(theta1, theta2, delta): - initial_guess = 3*(theta1 + theta2) - return fsolve(lambda x: Y(2*x, delta - x, theta1), [initial_guess]) - - -def compute_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 - - -if __name__ == "__main__": - theta1_vals = [0] - theta2_vals = np.linspace(-pi, pi, 75) - draw_clothoids(theta1_vals, theta2_vals, save_animation=False) diff --git a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst b/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst index 5a447f7c..4c1f672b 100644 --- a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst +++ b/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst @@ -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,16 +68,20 @@ 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 `__ (DARE): + +and :math:`P` is the unique positive definite solution to the discrete time +`algebraic Riccati equation `__ (DARE): .. math:: P = A^T P A - A^T P B ( R + B^T P B )^{-1} B^T P A + Q @@ -85,13 +89,13 @@ and :math:`P` is the unique positive definite solution to the discrete time `alg 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 diff --git a/docs/modules/path_planning/clothoid_path/clothoid_path.rst b/docs/modules/path_planning/clothoid_path/clothoid_path.rst new file mode 100644 index 00000000..1e8cee69 --- /dev/null +++ b/docs/modules/path_planning/clothoid_path/clothoid_path.rst @@ -0,0 +1,80 @@ +.. _clothoid-path-planning: + +Clothoid path planning +-------------------------- + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation1.gif +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation2.gif +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation3.gif + +This is a clothoid path planning sample code. + +This can interpolate two 2D pose (x, y, yaw) with a clothoid path, +which its curvature is linearly continuous. +In other words, this is G1 Hermite interpolation with a single clothoid segment. + +This path planning algorithm as follows: + +Step1: Solve g function +~~~~~~~~~~~~~~~~~~~~~~~ + +Solve the g(A) function with a nonlinear optimization solver. + +.. math:: + + g(A):=Y(2A, \delta-A, \phi_{s}) + +Where + +* :math:`\delta`: the orientation difference between start and goal pose. +* :math:`\phi_{s}`: the orientation of the start pose. +* :math:`Y`: :math:`Y(a, b, c)=\int_{0}^{1} \sin \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau` + + +Step2: Calculate path parameters +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +We can calculate these path parameters using :math:`A`, + +:math:`L`: path length + +.. math:: + + L=\frac{R}{X\left(2 A, \delta-A, \phi_{s}\right)} + +where + +* :math:`R`: the distance between start and goal pose +* :math:`X`: :math:`X(a, b, c)=\int_{0}^{1} \cos \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau` + + +- :math:`\kappa`: curvature + +.. math:: + + \kappa=(\delta-A) / L + + +- :math:`\kappa'`: curvature rate + +.. math:: + + \kappa^{\prime}=2 A / L^{2} + + +Step3: Construct a path with Fresnel integral +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The final clothoid path can be calculated with the path parameters and Fresnel integrals. + +.. math:: + \begin{aligned} + &x(s)=x_{0}+\int_{0}^{s} \cos \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau \\ + &y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau + \end{aligned} + + +References +~~~~~~~~~~ + +- `Fast and accurate G1 fitting of clothoid curves `__ diff --git a/docs/modules/path_planning/path_planning_main.rst b/docs/modules/path_planning/path_planning_main.rst index c3c5c389..5c70592a 100644 --- a/docs/modules/path_planning/path_planning_main.rst +++ b/docs/modules/path_planning/path_planning_main.rst @@ -14,6 +14,7 @@ Path Planning .. include:: rrt/rrt.rst .. include:: cubic_spline/cubic_spline.rst .. include:: bspline_path/bspline_path.rst +.. include:: clothoid_path/clothoid_path.rst .. include:: eta3_spline/eta3_spline.rst .. include:: bezier_path/bezier_path.rst .. include:: quintic_polynomials_planner/quintic_polynomials_planner.rst diff --git a/tests/test_clothoidal_paths.py b/tests/test_clothoidal_paths.py new file mode 100644 index 00000000..0b038c03 --- /dev/null +++ b/tests/test_clothoidal_paths.py @@ -0,0 +1,11 @@ +import conftest +from PathPlanning.ClothoidPath import clothoid_path_planner as m + + +def test_1(): + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__)