Reopen: Added missing path types (AtsushiSakai#949)

* added missing RS path types

* modified assert condition in test3

* removed linting errors

* added sign check to avoid non RS paths

* Mathematical description of RS curves

* Undid debugging changes

* Added large step_size consideration

* renamed test3 to test_too_big_step_size

---------

Co-authored-by: Videh Patel <[email protected]>

Author: videh25

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -5,6 +5,381 @@ A sample code with Reeds Shepp path planning.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true
 
+Mathematical Description of Individual Path Types
+=================================================
+Here is an overview of mathematical derivations of formulae for individual path types.
+
+In all the derivations below, radius of curvature of the vehicle is assumed to be of unit length and start pose is considered to be at origin.  (*In code we are removing the offset due to start position and normalising the lengths before passing the values to these functions.*)
+
+Also, (t, u, v) respresent the measure of each motion requried. Thus, in case of a turning maneuver, they represent the angle inscribed at the centre of turning circle and in case of straight maneuver, they represent the distance to be travelled. 
+
+1. **Left-Straight-Left**
+
+.. image:: LSL.png
+
+We can deduce the following facts using geometry.
+
+- AGHC is a rectangle.
+- :math:`∠LAC = ∠BAG = t`
+- :math:`t + v = φ`
+- :math:`C(x - sin(φ), y + cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u, t = polar(vector<AC>)`
+
+Hence, we have:
+
+- :math:`u, t = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`v = φ - t`
+
+
+2. **Left-Straight-Right**
+
+.. image:: LSR.png
+
+With followng notations:
+
+- :math:`∠MBD = t1`
+- :math:`∠BDF = θ`
+- :math:`BC = u1`
+
+We can deduce the following facts using geometry.
+
+- D is mid-point of BC and FG.
+- :math:`t - v = φ`
+- :math:`C(x + sin(φ), y - cos(φ))`
+- :math:`A(0, 1)`
+- :math:`u1, t1 = polar(vector<AC>)`
+- :math:`\frac{u1^2}{4} = 1 + \frac{u^2}{4}`
+- :math:`BF = 1` [Radius Of Curvature]
+- :math:`FD = \frac{u}{2}`
+- :math:`θ = arctan(\frac{BF}{FD})`
+- :math:`t1 + θ = t`
+
+Hence, we have:
+
+- :math:`u1, t1 = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = \sqrt{u1^2 - 4}`
+- :math:`θ = arctan(\frac{2}{u})`
+- :math:`t = t1 + θ`
+- :math:`v = t - φ`
+
+3. **LeftxRightxLeft**
+
+.. image:: L_R_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u + v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BD>)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (φ - t - u)`
+
+4. **LeftxRight-Left**
+
+.. image:: L_RL.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BD>)`
+- :math:`A = arccos(\frac{BD/2}{CD})`
+- :math:`u = (π - 2*A)`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD + ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`A = arccos(\frac{u1/2}{2})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`u = (π - 2*A)`
+- :math:`v = (-φ + t + u)`
+
+5. **Left-RightxLeft**
+
+.. image:: LR_L.png
+
+With followng notations:
+
+- :math:`∠CBD = ∠CDB = A` [BCD is an isoceles triangle]
+- :math:`∠DBK = θ`
+- :math:`BD = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - u - v = φ`
+- :math:`D(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BD>)`
+- :math:`BC = CD = 2` [2 * radius of curvature]
+- :math:`cos(2π - u) = \frac{BC^2 + CD^2 - BD^2}{2 * BC * CD}` [Cosine Rule]
+- :math:`\frac{sin(A)}{BC} = \frac{sin(u)}{u1}` [Sine Rule]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`∠KBD = θ`
+- :math:`t = ∠ABK + ∠KBD - ∠DBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(1 - \frac{u1^2}{8})`
+- :math:`A = arcsin(\frac{sin(u)}{u1}*2)`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - u - φ)`
+
+6. **Left-RightxLeft-Right**
+
+.. image:: LR_LR.png
+
+With followng notations:
+
+- :math:`∠CLG = ∠BCL = ∠CBG = ∠LGB = A = u` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - 2u + v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BG>)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- :math:`CG^2 = CL^2 + LG^2 - 2*CL*LG*cos(A)` [Cosine rule in LGC]
+- From the previous two equations: :math:`A = arccos(\frac{u1 + 2}{4})`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{u1 + 2}{4})`
+- :math:`t = \frac{π}{2} + θ + u`
+- :math:`v = (φ - t + 2u)`
+
+7. **LeftxRight-LeftxRight**
+
+.. image:: L_RL_R.png
+
+With followng notations:
+
+- :math:`∠GBC = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BG>)`
+- :math:`BC = CL = LG = 2` [2 * radius of curvature]
+- :math:`CD = 1` [radius of curvature]
+- D is midpoint of BG
+- :math:`BD = \frac{u1}{2}`
+- :math:`cos(u) = \frac{BC^2 + CD^2 - BD^2}{2*BC*CD}` [Cosine rule in BCD]
+- :math:`sin(A) = CD*\frac{sin(u)}{BD}` [Sine rule in BCD]
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG + ∠GBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = arccos(\frac{20 - u1^2}{16})`
+- :math:`A = arcsin(2*\frac{sin(u)}{u1})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
+8. **LeftxRight90-Straight-Left**
+
+.. image:: L_R90SL.png
+
+With followng notations:
+
+- :math:`∠FBM = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBF = θ`
+- :math:`BF = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} - v = φ`
+- :math:`F(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BF>)`
+- :math:`BM = CB = 2` [2 * radius of curvature]
+- :math:`MD = CD = 1` [CGDM is a rectangle]
+- :math:`MC = GD = u` [CGDM is a rectangle]
+- :math:`MF = MD + DF = 2`
+- :math:`BM = \sqrt{BF^2 - MF^2}` [Pythagoras theorem on BFM]
+- :math:`tan(A) = \frac{MF}{BM}`
+- :math:`u = MC = BM - CB` 
+- :math:`t = ∠ABK + ∠KBF + ∠FBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ + \frac{π}{2})`
+
+
+9. **Left-Straight-Right90xLeft**
+
+.. image:: LSR90_L.png
+
+With followng notations:
+
+- :math:`∠MBH = A` [BGCL is an isoceles trapezium]
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`H(x - sin(φ), y + cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BH>)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`CM = DG = 1` [CGDM is a rectangle]
+- :math:`CD = MG = u` [CGDM is a rectangle]
+- :math:`BM = BC + CM = 2`
+- :math:`MH = \sqrt{BH^2 - BM^2}` [Pythagoras theorem on BHM]
+- :math:`tan(A) = \frac{HM}{BM}`
+- :math:`u = MC = BM - CB` 
+- :math:`t = ∠ABK + ∠KBH - ∠HBC`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x - sin(φ), y + cos(φ) - 1)`
+- :math:`u = arccos(\sqrt{u1^2 - 4} - 2)`
+- :math:`A = arctan(\frac{2}{\sqrt{u1^2 - 4}})`
+- :math:`t = \frac{π}{2} + θ - A`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+10. **LeftxRight90-Straight-Right**
+
+.. image:: L_R90SR.png
+
+With followng notations:
+
+- :math:`∠KBG = θ`
+- :math:`BG = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - \frac{π}{2} - v = φ`
+- :math:`G(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BG>)`
+- :math:`BD = 2` [2 * radius of curvature]
+- :math:`DG = EF = u` [DGFE is a rectangle]
+- :math:`DG = BG - BD = 2`
+- :math:`∠ABK = \frac{π}{2}`
+- :math:`t = ∠ABK + ∠KBG`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = \frac{π}{2} + θ`
+- :math:`v = (t - φ - \frac{π}{2})`
+
+
+11. **Left-Straight-Left90xRight**
+
+.. image:: LSL90xR.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t + \frac{π}{2} + v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BH>)`
+- :math:`GH = 2` [2 * radius of curvature]
+- :math:`DC = BG = u` [DGBC is a rectangle]
+- :math:`BG = BH - GH`
+- :math:`∠ABC= ∠KBH`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = u1 - 2`
+- :math:`t = θ`
+- :math:`v = (φ - t - \frac{π}{2})`
+
+
+12. **LeftxRight90-Straight-Left90xRight**
+
+.. image:: L_R90SL90_R.png
+
+With followng notations:
+
+- :math:`∠KBH = θ`
+- :math:`∠HBM = A`
+- :math:`BH = u1`
+
+We can deduce the following facts using geometry.
+
+- :math:`t - v = φ`
+- :math:`H(x + sin(φ), y - cos(φ))`
+- :math:`B(0, 1)`
+- :math:`u1, θ = polar(vector<BH>)`
+- :math:`GF = ED = 1` [radius of curvature]
+- :math:`BD = GH = 2` [2 * radius of curvature]
+- :math:`FN = GH = 2` [ENMD is a rectangle]
+- :math:`NH = GF = 1` [FNHG is a rectangle]
+- :math:`MN = ED = 1` [ENMD is a rectangle]
+- :math:`DO = EF = u` [DOFE is a rectangle]
+- :math:`MH = MN + NH = 2`
+- :math:`BM = \sqrt{BH^2 - MH^2}` [Pythagoras theorem on BHM]
+- :math:`DO = BM - BD - OM`
+- :math:`tan(A) = \frac{MH}{BM}`
+- :math:`∠ABC = ∠ABK + ∠KBH + ∠HBM`
+
+Hence, we have:
+
+- :math:`u1, θ = polar(x + sin(φ), y - cos(φ) - 1)`
+- :math:`u = /sqrt{u1^2 - 4} - 4`
+- :math:`A = arctan(\frac{2}{u1^2 - 4})`
+- :math:`t = \frac{π}{2} + θ + A`
+- :math:`v = (t - φ)`
+
+
 Ref:
 
 -  `15.3.2 Reeds-Shepp