mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-11 18:17:53 -05:00
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
This commit is contained in:
Atsushi Sakai
192
PathPlanning/ClothoidPath/clothoid_path_planner.py
Normal file
192
PathPlanning/ClothoidPath/clothoid_path_planner.py
Normal file
@@ -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()
|
||||
@@ -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)
|
||||
@@ -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 <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
|
||||
|
||||
@@ -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
|
||||
|
||||
80
docs/modules/path_planning/clothoid_path/clothoid_path.rst
Normal file
80
docs/modules/path_planning/clothoid_path/clothoid_path.rst
Normal file
@@ -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 <https://www.researchgate.net/publication/237062806>`__
|
||||
@@ -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
|
||||
|
||||
11
tests/test_clothoidal_paths.py
Normal file
11
tests/test_clothoidal_paths.py
Normal file
@@ -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__)
|
||||
Reference in New Issue
Block a user