diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 4d8fb7d9b8..8e134ff38b 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/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,117 +84,200 @@ 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, [], [] + + +def left_straight_right(x, y, phi): + u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi)) + u1 = u1 ** 2 + if u1 >= 4.0: + u = math.sqrt(u1 - 4.0) + theta = math.atan2(2.0, u) + t = mod2pi(t1 + theta) + v = mod2pi(t - phi) + + 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_right_left(x, y, phi): - u1, t1 = polar(x - math.sin(phi), y - 1.0 + math.cos(phi)) +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: - u = -2.0 * math.asin(0.25 * u1) - t = mod2pi(t1 + 0.5 * u + math.pi) - v = mod2pi(phi - t + u) + 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'] - if t >= 0.0 >= u: - return True, t, u, v + return False, [], [] - return False, 0.0, 0.0, 0.0 +def left_x_right_left(x, y, phi): + zeta = x - math.sin(phi) + eeta = y - 1 + math.cos(phi) + u1, theta = polar(zeta, eeta) -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) + 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'] - flag, t, u, v = left_right_left(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"], step_size) + return False, [], [] - 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) +def left_right_x_left(x, y, phi): + zeta = x - math.sin(phi) + eeta = y - 1 + math.cos(phi) + u1, theta = polar(zeta, eeta) - # backwards - xb = x * math.cos(phi) + y * math.sin(phi) - yb = x * math.sin(phi) - y * math.cos(phi) + 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'] - flag, t, u, v = left_right_left(xb, yb, phi) - if flag: - paths = set_path(paths, [v, u, t], ["L", "R", "L"], step_size) + return False, [], [] - 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) +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) - flag, t, u, v = left_right_left(-xb, -yb, phi) - if flag: - paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"], step_size) + 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 paths + return False, [], [] -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) +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) - flag, t, u, v = left_straight_left(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"], step_size) + 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'] - flag, t, u, v = left_straight_left(x, -y, -phi) - if flag: - paths = set_path(paths, [t, u, v], ["R", "S", "R"], step_size) + return False, [], [] - 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) +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) - flag, t, u, v = left_straight_right(-x, y, -phi) - if flag: - paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"], step_size) + 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'] - 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 False, [], [] - 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 +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) -def left_straight_right(x, y, phi): - u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi)) - u1 = u1 ** 2 if u1 >= 4.0: - u = math.sqrt(u1 - 4.0) - theta = math.atan2(2.0, u) - t = mod2pi(t1 + theta) + 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, [], [] + - if t >= 0.0 and v >= 0.0: - return True, t, u, v +def timeflip(travel_distances): + return [-x for x in travel_distances] - return False, 0.0, 0.0, 0.0 + +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() diff --git a/docs/modules/path_planning/reeds_shepp_path/LR_L.png b/docs/modules/path_planning/reeds_shepp_path/LR_L.png new file mode 100644 index 0000000000..1e64da57f2 Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LR_L.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/LR_LR.png b/docs/modules/path_planning/reeds_shepp_path/LR_LR.png new file mode 100644 index 0000000000..e2c9883d8e Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LR_LR.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/LSL.png b/docs/modules/path_planning/reeds_shepp_path/LSL.png new file mode 100644 index 0000000000..6785ad3f8c Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LSL.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/LSL90xR.png b/docs/modules/path_planning/reeds_shepp_path/LSL90xR.png new file mode 100644 index 0000000000..54e892ba46 Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LSL90xR.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/LSR.png b/docs/modules/path_planning/reeds_shepp_path/LSR.png new file mode 100644 index 0000000000..8acc0de69f Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LSR.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/LSR90_L.png b/docs/modules/path_planning/reeds_shepp_path/LSR90_L.png new file mode 100644 index 0000000000..58d381010d Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/LSR90_L.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_R90SL.png b/docs/modules/path_planning/reeds_shepp_path/L_R90SL.png new file mode 100644 index 0000000000..c305c0be6e Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_R90SL.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_R90SL90_R.png b/docs/modules/path_planning/reeds_shepp_path/L_R90SL90_R.png new file mode 100644 index 0000000000..f2b38329da Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_R90SL90_R.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_R90SR.png b/docs/modules/path_planning/reeds_shepp_path/L_R90SR.png new file mode 100644 index 0000000000..4323a9fe3b Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_R90SR.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_RL.png b/docs/modules/path_planning/reeds_shepp_path/L_RL.png new file mode 100644 index 0000000000..ad58b8ffea Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_RL.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_RL_R.png b/docs/modules/path_planning/reeds_shepp_path/L_RL_R.png new file mode 100644 index 0000000000..db4aaf6af3 Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_RL_R.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/L_R_L.png b/docs/modules/path_planning/reeds_shepp_path/L_R_L.png new file mode 100644 index 0000000000..0d3082aeaf Binary files /dev/null and b/docs/modules/path_planning/reeds_shepp_path/L_R_L.png differ diff --git a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst index 81e565888e..6d456417fe 100644 --- a/docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst +++ b/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)` + +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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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)` +- :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 diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 20d4916f96..11157aa57a 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/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__':