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

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

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 <https://www.researchgate.net/publication/237062806>`__

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