Reopen: Added missing path types (#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 <videh.patel@fluxauto.xyz>

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -3,6 +3,7 @@
 Reeds Shepp path planner sample code
 
 author Atsushi Sakai(@Atsushi_twi)
+co-author Videh Patel(@videh25) : Added the missing RS paths
 
 """
 import math
@@ -54,21 +55,6 @@ def mod2pi(x):
             v -= 2.0 * math.pi
     return v
 
-def straight_left_straight(x, y, phi):
-    phi = mod2pi(phi)
-    # only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed.
-    # phi in (0, 0.01*math.pi) will make test2() in test_rrt_star_reeds_shepp.py
-    # extremely time-consuming, since the value of xd, t will be very large.
-    if math.pi * 0.01 < phi < math.pi * 0.99 and y != 0:
-        xd = - y / math.tan(phi) + x
-        t = xd - math.tan(phi / 2.0)
-        u = phi
-        v = np.sign(y) * math.hypot(x - xd, y) - math.tan(phi / 2.0)
-        return True, t, u, v
-
-    return False, 0.0, 0.0, 0.0
-
-
 def set_path(paths, lengths, ctypes, step_size):
     path = Path()
     path.ctypes = ctypes
@@ -90,18 +76,6 @@ def set_path(paths, lengths, ctypes, step_size):
     return paths
 
 
-def straight_curve_straight(x, y, phi, paths, step_size):
-    flag, t, u, v = straight_left_straight(x, y, phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["S", "L", "S"], step_size)
-
-    flag, t, u, v = straight_left_straight(x, -y, -phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["S", "R", "S"], step_size)
-
-    return paths
-
-
 def polar(x, y):
     r = math.hypot(x, y)
     theta = math.atan2(y, x)
@@ -110,102 +84,12 @@ def polar(x, y):
 
 def left_straight_left(x, y, phi):
     u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
-    if t >= 0.0:
+    if 0.0 <= t <= math.pi:
         v = mod2pi(phi - t)
-        if v >= 0.0:
-            return True, t, u, v
+        if 0.0 <= v <= math.pi:
+            return True, [t, u, v], ['L', 'S', 'L']
 
-    return False, 0.0, 0.0, 0.0
-
-
-def left_right_left(x, y, phi):
-    u1, t1 = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
-
-    if u1 <= 4.0:
-        u = -2.0 * math.asin(0.25 * u1)
-        t = mod2pi(t1 + 0.5 * u + math.pi)
-        v = mod2pi(phi - t + u)
-
-        if t >= 0.0 >= u:
-            return True, t, u, v
-
-    return False, 0.0, 0.0, 0.0
-
-
-def curve_curve_curve(x, y, phi, paths, step_size):
-    flag, t, u, v = left_right_left(x, y, phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["L", "R", "L"], step_size)
-
-    flag, t, u, v = left_right_left(-x, y, -phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"], step_size)
-
-    flag, t, u, v = left_right_left(x, -y, -phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["R", "L", "R"], step_size)
-
-    flag, t, u, v = left_right_left(-x, -y, phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"], step_size)
-
-    # backwards
-    xb = x * math.cos(phi) + y * math.sin(phi)
-    yb = x * math.sin(phi) - y * math.cos(phi)
-
-    flag, t, u, v = left_right_left(xb, yb, phi)
-    if flag:
-        paths = set_path(paths, [v, u, t], ["L", "R", "L"], step_size)
-
-    flag, t, u, v = left_right_left(-xb, yb, -phi)
-    if flag:
-        paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"], step_size)
-
-    flag, t, u, v = left_right_left(xb, -yb, -phi)
-    if flag:
-        paths = set_path(paths, [v, u, t], ["R", "L", "R"], step_size)
-
-    flag, t, u, v = left_right_left(-xb, -yb, phi)
-    if flag:
-        paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"], step_size)
-
-    return paths
-
-
-def curve_straight_curve(x, y, phi, paths, step_size):
-    flag, t, u, v = left_straight_left(x, y, phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["L", "S", "L"], step_size)
-
-    flag, t, u, v = left_straight_left(-x, y, -phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"], step_size)
-
-    flag, t, u, v = left_straight_left(x, -y, -phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["R", "S", "R"], step_size)
-
-    flag, t, u, v = left_straight_left(-x, -y, phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"], step_size)
-
-    flag, t, u, v = left_straight_right(x, y, phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["L", "S", "R"], step_size)
-
-    flag, t, u, v = left_straight_right(-x, y, -phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"], step_size)
-
-    flag, t, u, v = left_straight_right(x, -y, -phi)
-    if flag:
-        paths = set_path(paths, [t, u, v], ["R", "S", "L"], step_size)
-
-    flag, t, u, v = left_straight_right(-x, -y, phi)
-    if flag:
-        paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"], step_size)
-
-    return paths
+    return False, [], []
 
 
 def left_straight_right(x, y, phi):
@@ -217,10 +101,183 @@ def left_straight_right(x, y, phi):
         t = mod2pi(t1 + theta)
         v = mod2pi(t - phi)
 
-        if t >= 0.0 and v >= 0.0:
-            return True, t, u, v
+        if (t >= 0.0) and (v >= 0.0):
+            return True, [t, u, v], ['L', 'S', 'R']
 
-    return False, 0.0, 0.0, 0.0
+    return False, [], []
+
+
+def left_x_right_x_left(x, y, phi):
+    zeta = x - math.sin(phi)
+    eeta = y - 1 + math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 <= 4.0:
+        A = math.acos(0.25 * u1)
+        t = mod2pi(A + theta + math.pi/2)
+        u = mod2pi(math.pi - 2 * A)
+        v = mod2pi(phi - t - u)
+        return True, [t, -u, v], ['L', 'R', 'L']
+
+    return False, [], []
+
+
+def left_x_right_left(x, y, phi):
+    zeta = x - math.sin(phi)
+    eeta = y - 1 + math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 <= 4.0:
+        A = math.acos(0.25 * u1)
+        t = mod2pi(A + theta + math.pi/2)
+        u = mod2pi(math.pi - 2*A)
+        v = mod2pi(-phi + t + u)
+        return True, [t, -u, -v], ['L', 'R', 'L']
+
+    return False, [], []
+
+
+def left_right_x_left(x, y, phi):
+    zeta = x - math.sin(phi)
+    eeta = y - 1 + math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 <= 4.0:
+        u = math.acos(1 - u1**2 * 0.125)
+        A = math.asin(2 * math.sin(u) / u1)
+        t = mod2pi(-A + theta + math.pi/2)
+        v = mod2pi(t - u - phi)
+        return True, [t, u, -v], ['L', 'R', 'L']
+
+    return False, [], []
+
+
+def left_right_x_left_right(x, y, phi):
+    zeta = x + math.sin(phi)
+    eeta = y - 1 - math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    # Solutions refering to (2 < u1 <= 4) are considered sub-optimal in paper
+    # Solutions do not exist for u1 > 4
+    if u1 <= 2:
+        A = math.acos((u1 + 2) * 0.25)
+        t = mod2pi(theta + A + math.pi/2)
+        u = mod2pi(A)
+        v = mod2pi(phi - t + 2*u)
+        if ((t >= 0) and (u >= 0) and (v >= 0)):
+            return True, [t, u, -u, -v], ['L', 'R', 'L', 'R']
+
+    return False, [], []
+
+
+def left_x_right_left_x_right(x, y, phi):
+    zeta = x + math.sin(phi)
+    eeta = y - 1 - math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+    u2 = (20 - u1**2) / 16
+
+    if (0 <= u2 <= 1):
+        u = math.acos(u2)
+        A = math.asin(2 * math.sin(u) / u1)
+        t = mod2pi(theta + A + math.pi/2)
+        v = mod2pi(t - phi)
+        if (t >= 0) and (v >= 0):
+            return True, [t, -u, -u, v], ['L', 'R', 'L', 'R']
+
+    return False, [], []
+
+
+def left_x_right90_straight_left(x, y, phi):
+    zeta = x - math.sin(phi)
+    eeta = y - 1 + math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 >= 2.0:
+        u = math.sqrt(u1**2 - 4) - 2
+        A = math.atan2(2, math.sqrt(u1**2 - 4))
+        t = mod2pi(theta + A + math.pi/2)
+        v = mod2pi(t - phi + math.pi/2)
+        if (t >= 0) and (v >= 0):
+           return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'L']
+
+    return False, [], []
+
+
+def left_straight_right90_x_left(x, y, phi):
+    zeta = x - math.sin(phi)
+    eeta = y - 1 + math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 >= 2.0:
+        u = math.sqrt(u1**2 - 4) - 2
+        A = math.atan2(math.sqrt(u1**2 - 4), 2)
+        t = mod2pi(theta - A + math.pi/2)
+        v = mod2pi(t - phi - math.pi/2)
+        if (t >= 0) and (v >= 0):
+            return True, [t, u, math.pi/2, -v], ['L', 'S', 'R', 'L']
+
+    return False, [], []
+
+
+def left_x_right90_straight_right(x, y, phi):
+    zeta = x + math.sin(phi)
+    eeta = y - 1 - math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 >= 2.0:
+        t = mod2pi(theta + math.pi/2)
+        u = u1 - 2
+        v = mod2pi(phi - t - math.pi/2)
+        if (t >= 0) and (v >= 0):
+            return True, [t, -math.pi/2, -u, -v], ['L', 'R', 'S', 'R']
+
+    return False, [], []
+
+
+def left_straight_left90_x_right(x, y, phi):
+    zeta = x + math.sin(phi)
+    eeta = y - 1 - math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 >= 2.0:
+        t = mod2pi(theta)
+        u = u1 - 2
+        v = mod2pi(phi - t - math.pi/2)
+        if (t >= 0) and (v >= 0):
+            return True, [t, u, math.pi/2, -v], ['L', 'S', 'L', 'R']
+
+    return False, [], []
+
+
+def left_x_right90_straight_left90_x_right(x, y, phi):
+    zeta = x + math.sin(phi)
+    eeta = y - 1 - math.cos(phi)
+    u1, theta = polar(zeta, eeta)
+
+    if u1 >= 4.0:
+        u = math.sqrt(u1**2 - 4) - 4
+        A = math.atan2(2, math.sqrt(u1**2 - 4))
+        t = mod2pi(theta + A + math.pi/2)
+        v = mod2pi(t - phi)
+        if (t >= 0) and (v >= 0):
+            return True, [t, -math.pi/2, -u, -math.pi/2, v], ['L', 'R', 'S', 'L', 'R']
+
+    return False, [], []
+
+
+def timeflip(travel_distances):
+    return [-x for x in travel_distances]
+
+
+def reflect(steering_directions):
+    def switch_dir(dirn):
+        if dirn == 'L':
+            return 'R'
+        elif dirn == 'R':
+            return 'L'
+        else:
+            return 'S'
+    return[switch_dir(dirn) for dirn in steering_directions]
 
 
 def generate_path(q0, q1, max_curvature, step_size):
@@ -231,11 +288,52 @@ def generate_path(q0, q1, max_curvature, step_size):
     s = math.sin(q0[2])
     x = (c * dx + s * dy) * max_curvature
     y = (-s * dx + c * dy) * max_curvature
+    step_size *= max_curvature
 
     paths = []
-    paths = straight_curve_straight(x, y, dth, paths, step_size)
-    paths = curve_straight_curve(x, y, dth, paths, step_size)
-    paths = curve_curve_curve(x, y, dth, paths, step_size)
+    path_functions = [left_straight_left, left_straight_right,                          # CSC
+                      left_x_right_x_left, left_x_right_left, left_right_x_left,        # CCC
+                      left_right_x_left_right, left_x_right_left_x_right,               # CCCC
+                      left_x_right90_straight_left, left_x_right90_straight_right,      # CCSC
+                      left_straight_right90_x_left, left_straight_left90_x_right,       # CSCC
+                      left_x_right90_straight_left90_x_right]                           # CCSCC
+
+    for path_func in path_functions:
+        flag, travel_distances, steering_dirns = path_func(x, y, dth)
+        if flag:
+            for distance in travel_distances:
+                if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+                    print("Step size too large for Reeds-Shepp paths.")
+                    return []
+            paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+        flag, travel_distances, steering_dirns = path_func(-x, y, -dth)
+        if flag:
+            for distance in travel_distances:
+                if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+                    print("Step size too large for Reeds-Shepp paths.")
+                    return []
+            travel_distances = timeflip(travel_distances)
+            paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+        flag, travel_distances, steering_dirns = path_func(x, -y, -dth)
+        if flag:
+            for distance in travel_distances:
+                if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+                    print("Step size too large for Reeds-Shepp paths.")
+                    return []
+            steering_dirns = reflect(steering_dirns)
+            paths = set_path(paths, travel_distances, steering_dirns, step_size)
+
+        flag, travel_distances, steering_dirns = path_func(-x, -y, dth)
+        if flag:
+            for distance in travel_distances:
+                if (0.1*sum([abs(d) for d in travel_distances]) < abs(distance) < step_size):
+                    print("Step size too large for Reeds-Shepp paths.")
+                    return []
+            travel_distances = timeflip(travel_distances)
+            steering_dirns = reflect(steering_dirns)
+            paths = set_path(paths, travel_distances, steering_dirns, step_size)
 
     return paths
 
@@ -252,7 +350,7 @@ def calc_interpolate_dists_list(lengths, step_size):
 
 
 def generate_local_course(lengths, modes, max_curvature, step_size):
-    interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size)
+    interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature)
 
     origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0
 
@@ -307,7 +405,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
     for path in paths:
         xs, ys, yaws, directions = generate_local_course(path.lengths,
                                                          path.ctypes, maxc,
-                                                         step_size * maxc)
+                                                         step_size)
 
         # convert global coordinate
         path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
@@ -354,6 +452,9 @@ def main():
                                                              curvature,
                                                              step_size)
 
+    if not xs:
+        assert False, "No path"
+
     if show_animation:  # pragma: no cover
         plt.cla()
         plt.plot(xs, ys, label="final course " + str(modes))
@@ -368,9 +469,6 @@ def main():
         plt.axis("equal")
         plt.show()
 
-    if not xs:
-        assert False, "No path"
-
 
 if __name__ == '__main__':
     main()

docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst

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

tests/test_rrt_star_reeds_shepp.py

@@ -32,16 +32,14 @@ def test2():
         # + 0.00000000000001 for acceptable errors arising from the planning process
         assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
 
-def test3():
+def test_too_big_step_size():
     step_size = 20
     rrt_star_reeds_shepp = m.RRTStarReedsShepp(start, goal,
                                              obstacleList, [-2.0, 15.0],
                                              max_iter=100, step_size=step_size)
     rrt_star_reeds_shepp.set_random_seed(seed=8)
     path = rrt_star_reeds_shepp.planning(animation=False)
-    for i in range(len(path)-1):
-        # + 0.00000000000001 for acceptable errors arising from the planning process
-        assert m.math.dist(path[i][0:2], path[i+1][0:2]) < step_size + 0.00000000000001
+    assert path is None
 
 
 if __name__ == '__main__':