fix(mobile): wrap curvature planner heading residuals

src/roboticstoolbox/mobile/CurvaturePolyPlanner.py

@@ -10,6 +10,7 @@
 import numpy as np
 import scipy.integrate
 import scipy.optimize
+from spatialmath.base import angdiff
 from spatialmath.base.types import ArrayLike3
 
 from roboticstoolbox.mobile.PlannerBase import PlannerBase
@@ -70,6 +71,8 @@ def costfunc(x: ArrayLike, start: ArrayLike3, goal: ArrayLike3) -> float:
     :param start: start configuration
     :param goal: goal configuration
     :return: distance of the end of the path from ``goal``
+
+    The heading error is the shortest angular difference.
     """
     # p[0:4] is polynomial: k0, a, b, c
     # p[4] is s_f
@@ -79,7 +82,9 @@ def costfunc(x: ArrayLike, start: ArrayLike3, goal: ArrayLike3) -> float:
     path, _ = solvepath(x, q0=start)
 
     # cost is configuration error at end of path
-    e = np.linalg.norm(path[:, -1] - np.r_[goal])
+    error = path[:, -1] - np.r_[goal]
+    error[2] = angdiff(path[2, -1], goal[2])
+    e = np.linalg.norm(error)
 
     return e
 

tests/test_curvature_heading_error.py

@@ -0,0 +1,38 @@
+"""Curvature-planner objectives measure heading error on the circle."""
+
+import numpy as np
+import numpy.testing as nt
+import pytest
+
+from roboticstoolbox import CurvaturePolyPlanner
+from roboticstoolbox.mobile.CurvaturePolyPlanner import costfunc
+
+
+@pytest.mark.parametrize("turns", [-2, -1, 1, 2])
+def test_cost_is_invariant_to_full_heading_turns(turns: int) -> None:
+    start = np.array([1.0, 2.0, 0.3])
+    goal = start + [2 * np.cos(0.3), 2 * np.sin(0.3), turns * 2 * np.pi]
+
+    cost = costfunc([0, 0, 0, 0, 2], start, goal)
+
+    nt.assert_allclose(cost, 0, atol=1e-12)
+
+
+def test_cost_uses_short_heading_error_across_angle_cut() -> None:
+    start = np.array([0.0, 0.0, np.pi - 0.02])
+    goal = np.array([2 * np.cos(start[2]), 2 * np.sin(start[2]), -np.pi + 0.01])
+
+    cost = costfunc([0, 0, 0, 0, 2], start, goal)
+
+    nt.assert_allclose(cost, 0.03, atol=1e-12)
+
+
+@pytest.mark.timeout(15)
+def test_query_does_not_add_a_loop_for_equivalent_goal_heading() -> None:
+    planner = CurvaturePolyPlanner()
+
+    path, status = planner.query((0, 0, 0), (2, 0, 2 * np.pi))
+
+    nt.assert_allclose(status.length, 2, atol=1e-8)
+    nt.assert_allclose(path[:, 1:], 0, atol=1e-8)
+    nt.assert_allclose(path[-1, :2], [2, 0], atol=1e-8)