Fix Reeds-Shepp planning at singular poses

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -72,8 +72,9 @@ def set_path(paths, lengths, ctypes, step_size):
         if type_is_same and length_is_close:
             return paths  # same path found, so do not insert path
 
-    # check path is long enough
-    if path.L <= step_size:
+    # Keep a stationary solution, but discard nonzero paths that are shorter
+    # than the interpolation step.
+    if 0.0 < path.L <= step_size:
         return paths  # too short, so do not insert path
 
     paths.append(path)
@@ -147,8 +148,8 @@ def left_right_x_left(x, y, 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)
+        u = 2.0 * math.asin(0.25 * u1)
+        A = math.acos(0.25 * u1)
         t = mod2pi(-A + theta + math.pi/2)
         v = mod2pi(t - u - phi)
         return True, [t, u, -v], ['L', 'R', 'L']

tests/test_reeds_shepp_path_planning.py

@@ -53,5 +53,33 @@ def test2():
         check_path_length(px, py, lengths)
 
 
+def test_identical_start_and_goal_pose():
+    for pose in [(0.0, 0.0, 0.0), (1.2, -3.4, 0.7)]:
+        px, py, pyaw, _, lengths = m.reeds_shepp_path_planning(
+            *pose, *pose, maxc=1.0
+        )
+
+        assert sum(np.abs(lengths)) == 0.0
+        assert np.all(np.isfinite(px))
+        assert np.all(np.isfinite(py))
+        assert np.all(np.isfinite(pyaw))
+        assert np.all(np.isfinite(lengths))
+        np.testing.assert_allclose(px, pose[0], atol=1e-10)
+        np.testing.assert_allclose(py, pose[1], atol=1e-10)
+        np.testing.assert_allclose(pyaw, pose[2], atol=1e-10)
+
+
+def test_near_singular_path_formula():
+    for distance in [1e-6, 1e-12]:
+        is_valid, lengths, modes = m.left_right_x_left(
+            distance, 0.0, 0.0
+        )
+
+        assert is_valid
+        assert np.all(np.isfinite(lengths))
+        np.testing.assert_allclose(lengths[1], distance / 2.0, rtol=1e-12)
+        assert modes == ["L", "R", "L"]
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)