From 1cfa5ab22559f3457c293a94edacb6ce3eafa66a Mon Sep 17 00:00:00 2001
From: Afloat <132809511+Afloat16@users.noreply.github.com>
Date: Fri, 30 Oct 2026 13:53:47 +0800
Subject: [PATCH] fix: stop quintic paths at their terminal time

Co-Authored-By: ChatGPT (GPT-6 Astra Pro)
---
 .../mobile/QuinticPolyPlanner.py              |  7 ++-
 tests/test_quintic_planner_terminal_sample.py | 62 +++++++++++++++++++
 2 files changed, 68 insertions(+), 1 deletion(-)
 create mode 100644 tests/test_quintic_planner_terminal_sample.py

diff --git a/src/roboticstoolbox/mobile/QuinticPolyPlanner.py b/src/roboticstoolbox/mobile/QuinticPolyPlanner.py
index a34e9fc59..9c9fc44dc 100644
--- a/src/roboticstoolbox/mobile/QuinticPolyPlanner.py
+++ b/src/roboticstoolbox/mobile/QuinticPolyPlanner.py
@@ -131,6 +131,7 @@ def quintic_polynomials_planner(
     :param MAX_T: maximum path time [s]
     :raises NoPathError: no path time in the range meets the acceleration
         and jerk limits
+    :raises ValueError: ``dt`` is not positive and finite
     :return: time, path, velocity, acceleration and jerk
     :rtype: tuple(list, ndarray(N,3), list, list, list)
 
@@ -140,6 +141,9 @@ def quintic_polynomials_planner(
     step.
     """
 
+    if not math.isfinite(dt) or dt <= 0:
+        raise ValueError("dt must be a positive finite number")
+
     vxs = sv * math.cos(syaw)
     vys = sv * math.sin(syaw)
     vxg = gv * math.cos(gyaw)
@@ -164,7 +168,8 @@ def quintic_polynomials_planner(
 
         time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
 
-        for t in np.arange(0.0, T + dt, dt):
+        samples = np.arange(0.0, T, dt)
+        for t in np.r_[samples[samples < T], T]:
             time.append(t)
             rx.append(xqp.calc_point(t))
             ry.append(yqp.calc_point(t))
diff --git a/tests/test_quintic_planner_terminal_sample.py b/tests/test_quintic_planner_terminal_sample.py
new file mode 100644
index 000000000..335d20ff1
--- /dev/null
+++ b/tests/test_quintic_planner_terminal_sample.py
@@ -0,0 +1,62 @@
+"""Quintic sampling preserves its terminal boundary conditions."""
+
+import numpy as np
+import numpy.testing as npt
+import pytest
+
+from roboticstoolbox.mobile.QuinticPolyPlanner import quintic_polynomials_planner
+
+
+@pytest.mark.parametrize(
+    "duration,step",
+    [(1.0, 0.3), (1.1, 0.3), (1.0, 2.0), (1.0, 0.25), (3 * 0.1, 0.1)],
+)
+@pytest.mark.parametrize("heading", [-0.4, 0.7])
+def test_terminal_boundary_is_sampled_once(
+    duration: float, step: float, heading: float
+) -> None:
+    times, path, velocity, acceleration, jerk = quintic_polynomials_planner(
+        sx=0.2,
+        sy=-0.3,
+        syaw=0.1,
+        sv=0.6,
+        sa=0.0,
+        gx=1.2,
+        gy=0.8,
+        gyaw=heading,
+        gv=0.4,
+        ga=0.0,
+        max_accel=np.inf,
+        max_jerk=np.inf,
+        dt=step,
+        MIN_T=duration,
+        MAX_T=2 * duration,
+    )
+
+    assert times[0] == 0.0
+    assert times[-1] == duration
+    assert np.count_nonzero(np.asarray(times) == duration) == 1
+    assert np.all(np.diff(times) > 0)
+    assert np.all(np.asarray(times) <= duration)
+    assert np.all(np.diff(times) <= step + 1e-15)
+    assert path.shape == (len(times), 3)
+    assert len(velocity) == len(acceleration) == len(jerk) == len(times)
+    npt.assert_allclose(path[0], [0.2, -0.3, 0.1], atol=1e-12)
+    npt.assert_allclose(path[-1], [1.2, 0.8, heading], atol=1e-10)
+    npt.assert_allclose(velocity[-1], 0.4, atol=1e-10)
+    npt.assert_allclose(acceleration[-1], 0.0, atol=1e-9)
+
+
+def test_exact_grid_spacing_is_preserved() -> None:
+    times, *_ = quintic_polynomials_planner(
+        0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 100, 100, 0.25, 1.0, 2.0
+    )
+    npt.assert_array_equal(times, [0.0, 0.25, 0.5, 0.75, 1.0])
+
+
+@pytest.mark.parametrize("step", [-1.0, -0.1, 0.0, np.inf, -np.inf, np.nan])
+def test_invalid_time_step_is_rejected(step: float) -> None:
+    with pytest.raises(ValueError):
+        quintic_polynomials_planner(
+            0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 100, 100, step, 1.0, 2.0
+        )