From 92c028ccaa4e3fb8ecbf03715089b40f212fb67c Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Sun, 11 Oct 2026 20:03:09 +0800 Subject: [PATCH 1/5] fix(trajectory): normalize quintic profile time --- src/roboticstoolbox/tools/trajectory.py | 88 +++++--- tests/test_quintic_conditioning.py | 268 ++++++++++++++++++++++++ 2 files changed, 322 insertions(+), 34 deletions(-) create mode 100644 tests/test_quintic_conditioning.py diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index cff845ec6..63ef5929d 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -1,7 +1,7 @@ import math import warnings from collections import namedtuple - +from collections.abc import Callable import matplotlib.pyplot as plt import numpy as np from numpy.typing import ArrayLike @@ -364,25 +364,24 @@ def quintic( return Trajectory("quintic", t, p, pd, pdd, istime) -def quintic_func(q0, qf, T, qd0=0, qdf=0): +def quintic_func( + q0: float, qf: float, T: float, qd0: float = 0, qdf: float = 0 +) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: r""" Quintic scalar polynomial as a function :param q0: initial value - :type q0: float :param qf: final value - :type qf: float - :param T: trajectory time - :type T: float + :param T: trajectory duration :param qd0: initial velocity, defaults to 0 - :type q0: float, optional :param qdf: final velocity, defaults to 0 - :type q0: float, optional - :return: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` - :rtype: callable + :returns: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` Returns a function which computes the specific quintic polynomial, and its - derivatives, as described by the parameters. + derivatives, as described by the parameters. The polynomial is solved and + evaluated in normalized time to avoid ill-conditioning for short or long + durations. Returned derivatives are with respect to the supplied time, with + zero acceleration at both endpoints. Example: @@ -397,30 +396,51 @@ def quintic_func(q0, qf, T, qd0=0, qdf=0): :seealso: :func:`quintic` :func:`trapezoidal_func` """ - # solve for the polynomial coefficients using least squares - # fmt: off - X = [ - [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], - [ T**5, T**4, T**3, T**2, T, 1.0], - [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], - [ 5.0 * T**4, 4.0 * T**3, 3.0 * T**2, 2.0 * T, 1.0, 0.0], - [ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0], - [20.0 * T**3, 12.0 * T**2, 6.0 * T, 2.0, 0.0, 0.0], - ] - # fmt: on - coeffs, resid, rank, s = np.linalg.lstsq( - X, np.r_[q0, qf, qd0, qdf, 0, 0], rcond=None - ) - - # coefficients of derivatives - coeffs_d = coeffs[0:5] * np.arange(5, 0, -1) - coeffs_dd = coeffs_d[0:4] * np.arange(4, 0, -1) + # Hermite basis functions use normalized time. Factor the derivatives so + # zero endpoint acceleration is not lost through polynomial cancellation. + tscal = T if T != 0 else 1.0 + boundary = np.r_[q0, qf, qd0, qdf, 0, 0] + if boundary.shape != (6,): + raise np.linalg.LinAlgError("Incompatible dimensions") + coefficient_dtype = np.result_type(boundary.dtype, np.asarray(T).dtype, np.float64) + if coefficient_dtype not in (np.float64, np.complex128): + raise TypeError(f"array type {coefficient_dtype} is unsupported in linalg") + q0, qf, v0, vf, _, _ = boundary * np.r_[1.0, 1.0, tscal, tscal, 1.0, 1.0] + delta = qf - q0 + + def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: + samples = np.asarray(x) + sample_dtype = np.result_type(samples.dtype, coefficient_dtype) + tau = np.asarray(samples, dtype=sample_dtype) / tscal + if T == 0: + # Preserve the existing degenerate least-squares linear result. + coeffs = np.r_[0, 0, 0, 0, (v0 + vf) / 2, (q0 + qf) / 2] + return ( + np.polyval(coeffs, tau), + np.polyval(np.r_[0, 0, 0, 0, (v0 + vf) / 2], tau), + np.polyval(np.zeros(4), tau), + ) + remaining = 1 - tau + blend = tau**3 * (10 + tau * (-15 + 6 * tau)) + return ( + q0 * (1 - blend) + + qf * blend + + v0 * tau * remaining**3 * (1 + 3 * tau) + - vf * tau**3 * remaining * (4 - 3 * tau), + ( + 30 * delta * tau**2 * remaining**2 + + v0 * remaining**2 * (1 + 2 * tau - 15 * tau**2) + + vf * tau**2 * (1 + 2 * remaining - 15 * remaining**2) + ) + / tscal, + 12 + * tau + * remaining + * (5 * delta * (1 - 2 * tau) - v0 * (3 - 5 * tau) - vf * (2 - 5 * tau)) + / tscal**2, + ) - return lambda x: ( - np.polyval(coeffs, x), - np.polyval(coeffs_d, x), - np.polyval(coeffs_dd, x), - ) + return polynomial # -------------------------------------------------------------------------- # diff --git a/tests/test_quintic_conditioning.py b/tests/test_quintic_conditioning.py new file mode 100644 index 000000000..04548ead7 --- /dev/null +++ b/tests/test_quintic_conditioning.py @@ -0,0 +1,268 @@ +"""Quintic boundary conditions across trajectory time scales.""" + +from collections.abc import Callable +from decimal import Decimal, localcontext + +import numpy as np +import numpy.testing as nt +import pytest +from numpy.polynomial import Polynomial +from numpy.typing import ArrayLike + +from roboticstoolbox import jtraj, mtraj, quintic, quintic_func + + +@pytest.mark.parametrize("duration", [0.001, 0.01, 1.0, 10.0, 999.0, 1000.0, 1e6]) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_func_analytic_polynomial( + duration: float, moving_endpoints: bool +) -> None: + # These fixed polynomials independently specify all six boundary conditions. + # Both have p(0)=1, p(1)=2, p''(0)=p''(1)=0. The second has p'(0)=1, + # p'(1)=2; the first has zero endpoint velocities. + polynomial = Polynomial( + [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] + ) + velocity = polynomial.deriv() + acceleration = velocity.deriv() + samples = np.array([0.0, 0.125, 0.25, 0.5, 0.875, 1.0]) + function = quintic_func( + 1, 2, duration, qd0=velocity(0) / duration, qdf=velocity(1) / duration + ) + position, speed, accel = function(samples * duration) + + nt.assert_allclose(position, polynomial(samples), rtol=1e-12, atol=1e-12) + nt.assert_allclose(speed * duration, velocity(samples), rtol=1e-12, atol=1e-12) + nt.assert_allclose( + accel * duration**2, acceleration(samples), rtol=1e-12, atol=1e-12 + ) + nt.assert_allclose(position[[0, -1]], [1, 2], atol=1e-12) + nt.assert_allclose( + speed[[0, -1]] * duration, [velocity(0), velocity(1)], atol=1e-12 + ) + nt.assert_allclose(accel[[0, -1]] * duration**2, [0, 0], atol=1e-12) + + +@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) +def test_quintic_func_physical_endpoint_velocities(duration: float) -> None: + # Independent Hermite basis functions, with fixed velocities in time units. + position_basis = Polynomial([0, 0, 0, 10, -15, 6]) + initial_velocity_basis = Polynomial([0, 1, 0, -6, 8, -3]) + final_velocity_basis = Polynomial([0, 0, 0, -4, 7, -3]) + samples = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) + actual = quintic_func(-2, 3, duration, qd0=0.2, qdf=-0.3)(samples * duration) + for order, values in enumerate(actual): + # Evaluate each integer-coefficient basis before scaling so exact zero + # endpoint derivatives are not replaced by oracle cancellation noise. + reference = ( + 5 * position_basis.deriv(order)(samples) + + (0.2 * duration) * initial_velocity_basis.deriv(order)(samples) + - (0.3 * duration) * final_velocity_basis.deriv(order)(samples) + ) + if order == 0: + reference -= 2 + nt.assert_allclose( + values * duration**order, + reference, + rtol=1e-12, + atol=1e-12, + ) + nt.assert_allclose(actual[0][[0, -1]], [-2, 3], atol=1e-12) + nt.assert_allclose(actual[1][[0, -1]], [0.2, -0.3], atol=1e-12) + nt.assert_allclose(actual[2][[0, -1]], [0, 0], atol=1e-9) + + +@pytest.mark.parametrize("steps", [11, 1000]) +def test_quintic_integer_samples(steps: int) -> None: + trajectory = quintic(1, 2, steps) + samples = np.arange(steps) / (steps - 1) + polynomial = Polynomial([1, 0, 0, 10, -15, 6]) + + nt.assert_array_equal(trajectory.t, np.arange(steps)) + assert not trajectory.istime + assert trajectory.q.shape == trajectory.qd.shape == trajectory.qdd.shape == (steps,) + nt.assert_allclose(trajectory.q, polynomial(samples), rtol=1e-12, atol=1e-12) + nt.assert_allclose( + trajectory.qd * (steps - 1), polynomial.deriv()(samples), atol=1e-12 + ) + nt.assert_allclose( + trajectory.qdd * (steps - 1) ** 2, + polynomial.deriv(2)(samples), + atol=1e-12, + ) + + +@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) +@pytest.mark.parametrize("origin", [-5.0, 0.0, 5.0]) +def test_quintic_sampled_time_origin(duration: float, origin: float) -> None: + times = origin + duration * np.array([0.0, 0.125, 0.5, 0.875, 1.0]) + actual_duration = times[-1] - times[0] + # Use the represented elapsed times, including floating-point timestamp noise. + samples = (times - times[0]) / actual_duration + polynomial = Polynomial([1, 1, 0, -4, 7, -3]) + trajectory = quintic(1, 2, times, qd0=1 / actual_duration, qdf=2 / actual_duration) + + nt.assert_array_equal(trajectory.t, times) + assert trajectory.istime + for values, derivative, scale in ( + (trajectory.q, polynomial, 1), + (trajectory.qd, polynomial.deriv(), actual_duration), + (trajectory.qdd, polynomial.deriv(2), actual_duration**2), + ): + nt.assert_allclose(values * scale, derivative(samples), rtol=1e-12, atol=1e-12) + + +@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) +def test_quintic_multi_axis_native_control(duration: float) -> None: + times = np.linspace(0, duration, 11) + initial = np.array([1, -2, 3]) + final = np.array([2, 4, 3]) + actual = mtraj(quintic, initial, final, times) + expected = jtraj(initial, final, times) + + assert actual.q.shape == actual.qd.shape == actual.qdd.shape == (11, 3) + nt.assert_array_equal(actual.t, times) + assert actual.istime + for values, reference, scale in ( + (actual.q, expected.q, 1), + (actual.qd, expected.qd, duration), + (actual.qdd, expected.qdd, duration**2), + ): + assert np.issubdtype(values.dtype, np.floating) + nt.assert_allclose(values * scale, reference * scale, rtol=1e-12, atol=1e-12) + nt.assert_allclose(actual.q[[0, -1]], [initial, final], atol=1e-12) + + +@pytest.mark.parametrize("samples", [0.5, [0, 0.5, 1], [[0, 0.5], [0.75, 1]]]) +def test_quintic_func_sample_shapes(samples: ArrayLike) -> None: + function = quintic_func(np.int64(1), np.float32(2), 1) + position, velocity, acceleration = function(samples) + polynomial = Polynomial([1, 0, 0, 10, -15, 6]) + for actual, expected in zip( + (position, velocity, acceleration), + ( + polynomial(samples), + polynomial.deriv()(samples), + polynomial.deriv(2)(samples), + ), + ): + assert np.shape(actual) == np.shape(samples) + assert np.issubdtype(np.asarray(actual).dtype, np.floating) + nt.assert_allclose(actual, expected, rtol=1e-12, atol=1e-12) + + +@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) +@pytest.mark.parametrize( + "samples", + [ + np.float32(0.3), + np.array([0, 0.125, 0.3, 0.5, 0.7, 0.875, 1], dtype=np.float32), + np.array([[0, 0.3], [0.7, 1]], dtype=np.float32), + ], +) +def test_quintic_func_float32_sample_precision( + duration: float, samples: ArrayLike +) -> None: + times = np.asarray(samples) * np.float32(duration) + # Evaluate independent integer-coefficient polynomials at the exactly + # represented samples, including the rounding of the physical timestamps. + expected = [] + with localcontext() as context: + context.prec = 70 + for time in np.asarray(times).flat: + u = Decimal.from_float(float(time)) / Decimal.from_float(duration) + expected.append( + [ + 1 + 10 * u**3 - 15 * u**4 + 6 * u**5, + 30 * u**2 - 60 * u**3 + 30 * u**4, + 60 * u - 180 * u**2 + 120 * u**3, + ] + ) + reference = np.asarray(expected, dtype=float).T.reshape((3, *np.shape(times))) + for order, actual in enumerate(quintic_func(1, 2, duration)(times)): + assert np.shape(actual) == np.shape(times) + assert np.asarray(actual).dtype == np.dtype(np.float64) + nt.assert_allclose( + actual * duration**order, reference[order], rtol=1e-12, atol=1e-12 + ) + + +@pytest.mark.parametrize( + "dtype", + [ + np.float16, + np.float32, + np.float64, + np.int64, + np.complex64, + np.complex128, + np.longdouble, + np.clongdouble, + ], +) +def test_quintic_func_sample_dtype_compatibility(dtype: type) -> None: + values = ( + [0.3 + 0.2j, 0.7 - 0.1j] + if np.issubdtype(dtype, np.complexfloating) + else [0, 0.3, 1] + ) + samples = np.asarray(values, dtype=dtype) + result_dtype = np.result_type(samples.dtype, np.float64) + polynomial = Polynomial([1, 0, 0, 10, -15, 6]) + for order, actual in enumerate(quintic_func(1, 2, 1)(samples)): + assert isinstance(actual, np.ndarray) + assert actual.shape == samples.shape + assert actual.dtype == result_dtype + nt.assert_allclose( + actual, + polynomial.deriv(order)(samples.astype(result_dtype)), + rtol=1e-12, + atol=1e-12, + ) + + +@pytest.mark.parametrize("dtype", [np.longdouble, np.clongdouble]) +@pytest.mark.parametrize("argument", ["q0", "T", "qd0"]) +def test_quintic_func_extended_boundary_dtype_compatibility( + dtype: type, argument: str +) -> None: + arguments = {"q0": 1, "qf": 2, "T": 1, "qd0": 0} + arguments[argument] = dtype(arguments[argument]) + result_dtype = np.result_type(dtype, np.float64) + if result_dtype in (np.dtype(np.float64), np.dtype(np.complex128)): + # Some platforms give longdouble the same precision as float64. + nt.assert_allclose(quintic_func(**arguments)(0.5), [1.5, 1.875, 0], atol=1e-12) + else: + with pytest.raises( + TypeError, match=f"array type {result_dtype} is unsupported" + ): + quintic_func(**arguments) + + +def test_quintic_zero_duration_compatibility() -> None: + # Preserve the existing least-squares result for the degenerate duration. + function = quintic_func(1, 2, 0, qd0=0.2, qdf=-0.1) + position, velocity, acceleration = function(np.array([0, 0.5, 1])) + nt.assert_allclose(position, [1.5, 1.525, 1.55], atol=1e-12) + nt.assert_allclose(velocity, [0.05, 0.05, 0.05], atol=1e-12) + nt.assert_allclose(acceleration, [0, 0, 0], atol=1e-12) + + +@pytest.mark.parametrize("container", [list, np.array]) +def test_quintic_func_single_element_endpoint_arrays( + container: Callable[[list[int]], ArrayLike], +) -> None: + actual = quintic_func(container([1]), container([2]), 1)([0, 0.5, 1]) + nt.assert_allclose(actual[0], [1, 1.5, 2], atol=1e-12) + nt.assert_allclose(actual[1], [0, 1.875, 0], atol=1e-12) + nt.assert_allclose(actual[2], [0, 0, 0], atol=1e-12) + + +def test_quintic_func_rejects_multi_axis_endpoints() -> None: + with pytest.raises(np.linalg.LinAlgError, match="Incompatible dimensions"): + quintic_func([1, 2], [3, 4], 1) + + +def test_quintic_invalid_sampling_argument() -> None: + with pytest.raises(TypeError, match="bad argument for time"): + quintic(1, 2, "not time") From 9d322ab2b376586896df496b45b675a37947e8f0 Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Sun, 11 Oct 2026 20:59:02 +0800 Subject: [PATCH 2/5] Keep quintic regression checks active under Python optimization --- tests/test_quintic_conditioning.py | 31 +++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/tests/test_quintic_conditioning.py b/tests/test_quintic_conditioning.py index 04548ead7..14aaa1d31 100644 --- a/tests/test_quintic_conditioning.py +++ b/tests/test_quintic_conditioning.py @@ -79,8 +79,11 @@ def test_quintic_integer_samples(steps: int) -> None: polynomial = Polynomial([1, 0, 0, 10, -15, 6]) nt.assert_array_equal(trajectory.t, np.arange(steps)) - assert not trajectory.istime - assert trajectory.q.shape == trajectory.qd.shape == trajectory.qdd.shape == (steps,) + nt.assert_equal(not trajectory.istime, True) + nt.assert_equal( + trajectory.q.shape == trajectory.qd.shape == trajectory.qdd.shape == (steps,), + True, + ) nt.assert_allclose(trajectory.q, polynomial(samples), rtol=1e-12, atol=1e-12) nt.assert_allclose( trajectory.qd * (steps - 1), polynomial.deriv()(samples), atol=1e-12 @@ -103,7 +106,7 @@ def test_quintic_sampled_time_origin(duration: float, origin: float) -> None: trajectory = quintic(1, 2, times, qd0=1 / actual_duration, qdf=2 / actual_duration) nt.assert_array_equal(trajectory.t, times) - assert trajectory.istime + nt.assert_equal(trajectory.istime, True) for values, derivative, scale in ( (trajectory.q, polynomial, 1), (trajectory.qd, polynomial.deriv(), actual_duration), @@ -120,15 +123,17 @@ def test_quintic_multi_axis_native_control(duration: float) -> None: actual = mtraj(quintic, initial, final, times) expected = jtraj(initial, final, times) - assert actual.q.shape == actual.qd.shape == actual.qdd.shape == (11, 3) + nt.assert_equal( + actual.q.shape == actual.qd.shape == actual.qdd.shape == (11, 3), True + ) nt.assert_array_equal(actual.t, times) - assert actual.istime + nt.assert_equal(actual.istime, True) for values, reference, scale in ( (actual.q, expected.q, 1), (actual.qd, expected.qd, duration), (actual.qdd, expected.qdd, duration**2), ): - assert np.issubdtype(values.dtype, np.floating) + nt.assert_equal(np.issubdtype(values.dtype, np.floating), True) nt.assert_allclose(values * scale, reference * scale, rtol=1e-12, atol=1e-12) nt.assert_allclose(actual.q[[0, -1]], [initial, final], atol=1e-12) @@ -146,8 +151,8 @@ def test_quintic_func_sample_shapes(samples: ArrayLike) -> None: polynomial.deriv(2)(samples), ), ): - assert np.shape(actual) == np.shape(samples) - assert np.issubdtype(np.asarray(actual).dtype, np.floating) + nt.assert_equal(np.shape(actual) == np.shape(samples), True) + nt.assert_equal(np.issubdtype(np.asarray(actual).dtype, np.floating), True) nt.assert_allclose(actual, expected, rtol=1e-12, atol=1e-12) @@ -180,8 +185,8 @@ def test_quintic_func_float32_sample_precision( ) reference = np.asarray(expected, dtype=float).T.reshape((3, *np.shape(times))) for order, actual in enumerate(quintic_func(1, 2, duration)(times)): - assert np.shape(actual) == np.shape(times) - assert np.asarray(actual).dtype == np.dtype(np.float64) + nt.assert_equal(np.shape(actual) == np.shape(times), True) + nt.assert_equal(np.asarray(actual).dtype == np.dtype(np.float64), True) nt.assert_allclose( actual * duration**order, reference[order], rtol=1e-12, atol=1e-12 ) @@ -210,9 +215,9 @@ def test_quintic_func_sample_dtype_compatibility(dtype: type) -> None: result_dtype = np.result_type(samples.dtype, np.float64) polynomial = Polynomial([1, 0, 0, 10, -15, 6]) for order, actual in enumerate(quintic_func(1, 2, 1)(samples)): - assert isinstance(actual, np.ndarray) - assert actual.shape == samples.shape - assert actual.dtype == result_dtype + nt.assert_equal(isinstance(actual, np.ndarray), True) + nt.assert_equal(actual.shape == samples.shape, True) + nt.assert_equal(actual.dtype == result_dtype, True) nt.assert_allclose( actual, polynomial.deriv(order)(samples.astype(result_dtype)), From ca40038f56bd17885f9e0d866184ff4cdd20d15e Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Wed, 14 Oct 2026 13:02:49 +0800 Subject: [PATCH 3/5] fix(trajectory): guard quintic solve and add Hermite alternative Restore the six-equation teaching solver with a numerical-rank guard. Export the normalized-time Hermite function and use it for sampled quintic trajectories. Cover caller routing, endpoint derivatives and representable duration scaling. --- src/roboticstoolbox/__init__.py | 1 + src/roboticstoolbox/tools/__init__.py | 2 + src/roboticstoolbox/tools/trajectory.py | 115 +++++++++- tests/test_quintic_conditioning.py | 292 ++++++++++++++++++++++-- 4 files changed, 381 insertions(+), 29 deletions(-) diff --git a/src/roboticstoolbox/__init__.py b/src/roboticstoolbox/__init__.py index 384cf3007..c988eced3 100644 --- a/src/roboticstoolbox/__init__.py +++ b/src/roboticstoolbox/__init__.py @@ -45,6 +45,7 @@ "Ticker", "quintic", "quintic_func", + "quintic_hermite_func", "jtraj", "ctraj", "trapezoidal", diff --git a/src/roboticstoolbox/tools/__init__.py b/src/roboticstoolbox/tools/__init__.py index 0d5ee9011..78622dc19 100644 --- a/src/roboticstoolbox/tools/__init__.py +++ b/src/roboticstoolbox/tools/__init__.py @@ -5,6 +5,7 @@ from roboticstoolbox.tools.trajectory import ( quintic, quintic_func, + quintic_hermite_func, jtraj, mtraj, ctraj, @@ -33,6 +34,7 @@ "Ticker", "quintic", "quintic_func", + "quintic_hermite_func", "jtraj", "ctraj", "trapezoidal", diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 63ef5929d..62943255a 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -336,6 +336,10 @@ def quintic( time scaling is based on the first and last element. The returned trajectory retains the supplied time vector. + The samples are evaluated using :func:`quintic_hermite_func`, which works in + normalized time. The linear-system derivation is available separately in + :func:`quintic_func`. + :References: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. @@ -353,7 +357,7 @@ def quintic( raise TypeError("bad argument for time, must be int or vector") tf = t[-1] - t[0] - polyfunc = quintic_func(q0, qf, tf, qd0, qdf) + polyfunc = quintic_hermite_func(q0, qf, tf, qd0, qdf) # evaluate the polynomials traj = polyfunc(t - t[0]) @@ -368,7 +372,7 @@ def quintic_func( q0: float, qf: float, T: float, qd0: float = 0, qdf: float = 0 ) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: r""" - Quintic scalar polynomial as a function + Quintic scalar polynomial from a linear system :param q0: initial value :param qf: final value @@ -376,35 +380,125 @@ def quintic_func( :param qd0: initial velocity, defaults to 0 :param qdf: final velocity, defaults to 0 :returns: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` + :raises numpy.linalg.LinAlgError: the boundary-condition matrix is numerically + singular or contains non-finite values + + The six polynomial coefficients are found by solving a linear system for + the initial and final position, velocity and zero acceleration. Derivatives + are with respect to the supplied time units. This formulation is useful for + illustrating how those boundary conditions determine a quintic polynomial. + + Very short or long durations make the powers of ``T`` in the matrix differ + greatly in magnitude. The least-squares solver can then discard a boundary + condition: for example, ``quintic_func(1, 2, 1000)`` previously returned a + function whose initial position was close to zero instead of one. + + The matrix condition number is checked against ``1 / (6 * eps)``, where + ``eps`` is float64 machine precision. This is the numerical-rank cutoff of + ``numpy.linalg.lstsq(..., rcond=None)`` for this six-by-six system. At or above + this cutoff, including zero duration, an error is raised. Passing the check + does not guarantee accurate results for every ill-conditioned system; use + :func:`quintic_hermite_func` to avoid this duration-dependent conditioning. + + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import quintic_func + >>> f = quintic_func(1, 2, 5) + >>> f(0) + >>> f(5) + >>> f(2.5) + + :seealso: :func:`quintic_hermite_func`, :func:`quintic`, :func:`trapezoidal_func` + """ + + # solve for the polynomial coefficients using least squares + # fmt: off + X = [ + [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [ T**5, T**4, T**3, T**2, T, 1.0], + [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], + [ 5.0 * T**4, 4.0 * T**3, 3.0 * T**2, 2.0 * T, 1.0, 0.0], + [ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0], + [20.0 * T**3, 12.0 * T**2, 6.0 * T, 2.0, 0.0, 0.0], + ] + # fmt: on + # Match lstsq's default singular-value cutoff for this square system. + if not np.all(np.isfinite(X)) or np.linalg.cond(X) >= 1 / ( + len(X) * np.finfo(float).eps + ): + raise np.linalg.LinAlgError( + "quintic boundary-condition matrix is ill-conditioned; " + "use quintic_hermite_func instead" + ) + coeffs, resid, rank, s = np.linalg.lstsq( + X, np.r_[q0, qf, qd0, qdf, 0, 0], rcond=None + ) + + # coefficients of derivatives + coeffs_d = coeffs[0:5] * np.arange(5, 0, -1) + coeffs_dd = coeffs_d[0:4] * np.arange(4, 0, -1) + + return lambda x: ( + np.polyval(coeffs, x), + np.polyval(coeffs_d, x), + np.polyval(coeffs_dd, x), + ) + + +def quintic_hermite_func( + q0: float, qf: float, T: float, qd0: float = 0, qdf: float = 0 +) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: + r""" + Quintic scalar polynomial using a Hermite basis + + :param q0: initial value + :param qf: final value + :param T: trajectory duration + :param qd0: initial velocity, defaults to 0 + :param qdf: final velocity, defaults to 0 + :returns: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` + :raises ValueError: trajectory duration is not finite Returns a function which computes the specific quintic polynomial, and its - derivatives, as described by the parameters. The polynomial is solved and + derivatives, as described by the parameters. The polynomial is constructed and evaluated in normalized time to avoid ill-conditioning for short or long durations. Returned derivatives are with respect to the supplied time, with zero acceleration at both endpoints. + The returned function accepts scalar or array-like elapsed times, preserving + the sample shape. ``qd0`` and ``qdf`` use position units per supplied time + unit. The duration is promoted to the coefficient dtype before scaling. + For ``T == 0``, the degenerate linear least-squares result is retained: + the position starts at the mean of ``q0`` and ``qf``, velocity is the mean + of ``qd0`` and ``qdf``, and acceleration is zero. This zero-duration case + cannot satisfy distinct endpoint conditions. + Example: .. runblock:: pycon - >>> from roboticstoolbox import quintic_func - >>> f = quintic_func(1, 2, 5) + >>> from roboticstoolbox import quintic_hermite_func + >>> f = quintic_hermite_func(1, 2, 1000) >>> f(0) - >>> f(5) - >>> f(2.5) + >>> f(1000) + >>> f(500) - :seealso: :func:`quintic` :func:`trapezoidal_func` + :seealso: :func:`quintic_func`, :func:`quintic`, :func:`trapezoidal_func` """ # Hermite basis functions use normalized time. Factor the derivatives so # zero endpoint acceleration is not lost through polynomial cancellation. - tscal = T if T != 0 else 1.0 + if not np.isfinite(T): + raise ValueError("trajectory duration must be finite") boundary = np.r_[q0, qf, qd0, qdf, 0, 0] if boundary.shape != (6,): raise np.linalg.LinAlgError("Incompatible dimensions") coefficient_dtype = np.result_type(boundary.dtype, np.asarray(T).dtype, np.float64) if coefficient_dtype not in (np.float64, np.complex128): raise TypeError(f"array type {coefficient_dtype} is unsupported in linalg") + tscal = coefficient_dtype.type(T) if T != 0 else 1.0 q0, qf, v0, vf, _, _ = boundary * np.r_[1.0, 1.0, tscal, tscal, 1.0, 1.0] delta = qf - q0 @@ -437,7 +531,8 @@ def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: * tau * remaining * (5 * delta * (1 - 2 * tau) - v0 * (3 - 5 * tau) - vf * (2 - 5 * tau)) - / tscal**2, + / tscal + / tscal, ) return polynomial diff --git a/tests/test_quintic_conditioning.py b/tests/test_quintic_conditioning.py index 14aaa1d31..e5f2f0de0 100644 --- a/tests/test_quintic_conditioning.py +++ b/tests/test_quintic_conditioning.py @@ -2,6 +2,7 @@ from collections.abc import Callable from decimal import Decimal, localcontext +from types import SimpleNamespace import numpy as np import numpy.testing as nt @@ -9,12 +10,261 @@ from numpy.polynomial import Polynomial from numpy.typing import ArrayLike -from roboticstoolbox import jtraj, mtraj, quintic, quintic_func +from roboticstoolbox import jtraj, mtraj, quintic, quintic_func, quintic_hermite_func + + +@pytest.mark.parametrize("duration", [1.0, 5.0, 10.0, 49.0, -1.0, -10.0]) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_func_linear_system_control( + duration: float, moving_endpoints: bool +) -> None: + # Independent integer-coefficient polynomials, including nonzero velocities. + polynomial = Polynomial( + [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] + ) + samples = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) + actual = quintic_func( + 1, + 2, + duration, + qd0=polynomial.deriv()(0) / duration, + qdf=polynomial.deriv()(1) / duration, + )(samples * duration) + for order, values in enumerate(actual): + nt.assert_allclose( + values * duration**order, + polynomial.deriv(order)(samples), + rtol=1e-10, + atol=1e-10, + ) + + +@pytest.mark.parametrize("duration", [0.0, 0.001, -0.001, 1000.0, -1000.0, 1e6]) +def test_quintic_func_rejects_numerically_singular_system(duration: float) -> None: + with pytest.raises(np.linalg.LinAlgError, match="use quintic_hermite_func"): + quintic_func(1, 2, duration) + + +@pytest.mark.parametrize("duration", [0.0019, 0.0021, 920.0, 970.0]) +def test_quintic_func_condition_cutoff(duration: float) -> None: + # Build the boundary operator from independently differentiated monomials, + # using ascending powers rather than copying the production matrix. + matrix = np.array( + [ + [Polynomial.basis(power).deriv(order)(time) for power in range(6)] + for order in range(3) + for time in (0, duration) + ] + ) + cutoff = 1 / (6 * np.finfo(float).eps) + condition = np.linalg.cond(matrix) + # These samples straddle each cutoff with a margin against SVD round-off. + nt.assert_equal(0.7 < condition / cutoff < 1.4, True) + if duration in (0.0019, 970.0): + nt.assert_equal(condition > cutoff, True) + with pytest.raises(np.linalg.LinAlgError, match="ill-conditioned"): + quintic_func(1, 2, duration) + else: + nt.assert_equal(condition < cutoff, True) + values = quintic_func(1, 2, duration)([0, duration / 2, duration]) + nt.assert_equal(np.all(np.isfinite(values)), True) + + +@pytest.mark.parametrize("duration", [np.nan, np.inf, -np.inf]) +def test_quintic_func_nonfinite_duration(duration: float) -> None: + with pytest.raises(np.linalg.LinAlgError, match="ill-conditioned"): + quintic_func(1, 2, duration) + with pytest.raises(ValueError, match="duration must be finite"): + quintic_hermite_func(1, 2, duration) + + +def test_quintic_hermite_func_public_exports() -> None: + import roboticstoolbox + import roboticstoolbox.tools + + nt.assert_equal( + roboticstoolbox.tools.quintic_hermite_func is quintic_hermite_func, True + ) + for module in (roboticstoolbox, roboticstoolbox.tools): + nt.assert_equal("quintic_hermite_func" in module.__all__, True) + + +@pytest.mark.parametrize("duration", [-0.001, -1.0, -1000.0, 1e-6]) +def test_quintic_hermite_func_signed_and_short_duration(duration: float) -> None: + polynomial = Polynomial([1, 1, 0, -4, 7, -3]) + samples = np.array([0, 0.25, 0.5, 0.75, 1]) + actual = quintic_hermite_func(1, 2, duration, 1 / duration, 2 / duration)( + samples * duration + ) + for order, values in enumerate(actual): + nt.assert_allclose( + values * duration**order, + polynomial.deriv(order)(samples), + rtol=1e-12, + atol=1e-12, + ) + + +@pytest.mark.parametrize( + "duration", + [ + np.float16(0.001), + np.float32(1e-25), + np.float32(1e-20), + np.float32(1e20), + np.float32(1e30), + np.float32(-1e-25), + np.int64(10**10), + np.complex64(1e-25 + 1e-25j), + np.complex64(1e20 + 1e20j), + ], +) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_hermite_func_promotes_duration( + duration: complex, moving_endpoints: bool +) -> None: + # Use the represented duration, then scale in float64/complex128. In + # particular, a float32 square must not overflow or underflow before the + # derivative is divided by it. + represented_duration = ( + complex(duration) if np.iscomplexobj(duration) else float(duration) + ) + polynomial = Polynomial( + [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] + ) + samples = np.array([0, 0.25, 0.5, 0.75, 1]) + actual = quintic_hermite_func( + 1, + 2, + duration, + qd0=polynomial.deriv()(0) / represented_duration, + qdf=polynomial.deriv()(1) / represented_duration, + )(samples * represented_duration) + nt.assert_equal(np.all(np.isfinite(actual)), True) + for order, values in enumerate(actual): + nt.assert_allclose( + values * represented_duration**order, + polynomial.deriv(order)(samples), + rtol=1e-12, + atol=1e-12, + ) + + +@pytest.mark.parametrize("duration", [1e155, -1e155]) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_hermite_func_large_duration_acceleration( + duration: float, moving_endpoints: bool +) -> None: + polynomial = Polynomial( + [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] + ) + samples = np.array([0, 0.25, 0.5, 0.75, 1]) + actual = quintic_hermite_func( + 1, + 2, + duration, + qd0=polynomial.deriv()(0) / duration, + qdf=polynomial.deriv()(1) / duration, + )(samples * duration) + nt.assert_equal(np.all(np.isfinite(actual)), True) + for order, values in enumerate(actual): + # Scale sequentially: duration**2 overflows although the small physical + # acceleration and its normalized reference are both representable. + scaled = values + for _ in range(order): + scaled = scaled * duration + nt.assert_allclose( + scaled, polynomial.deriv(order)(samples), rtol=1e-12, atol=1e-12 + ) + + +@pytest.mark.parametrize("duration", [1e-200, -1e-200]) +def test_quintic_hermite_func_tiny_duration_endpoints(duration: float) -> None: + # Interior acceleration need not fit float64 at this scale, but the exact + # zero endpoint accelerations must not turn into 0/0 when T**2 underflows. + actual = quintic_hermite_func(1, 2, duration, 0.2, -0.3)([0, duration]) + nt.assert_allclose(actual[0], [1, 2], atol=1e-12) + nt.assert_allclose(actual[1], [0.2, -0.3], atol=1e-12) + nt.assert_array_equal(actual[2], [0, 0]) + + +@pytest.mark.parametrize("duration", [1e-200, -1e-200]) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_hermite_func_tiny_duration_small_displacement( + duration: float, moving_endpoints: bool +) -> None: + displacement = 1e-200 + polynomial = Polynomial( + [0, 1, 0, -4, 7, -3] if moving_endpoints else [0, 0, 0, 10, -15, 6] + ) + samples = np.array([0, 0.25, 0.5, 0.75, 1]) + actual = quintic_hermite_func( + 0, + displacement, + duration, + qd0=polynomial.deriv()(0) * displacement / duration, + qdf=polynomial.deriv()(1) * displacement / duration, + )(samples * duration) + nt.assert_equal(np.all(np.isfinite(actual)), True) + for order, values in enumerate(actual): + # A tiny displacement gives representable interior acceleration even + # though duration**2 is zero in float64. Keep the oracle scaling safe. + scaled = values + for _ in range(order): + scaled = scaled * duration + nt.assert_allclose( + scaled / displacement, + polynomial.deriv(order)(samples), + rtol=1e-12, + atol=1e-12, + ) + + +def test_quintic_single_sample_zero_duration() -> None: + trajectory = quintic(1, 2, 1, 0.2, -0.1) + nt.assert_array_equal(trajectory.t, [0]) + nt.assert_allclose(trajectory.q, [1.5]) + nt.assert_allclose(trajectory.qd, [0.05]) + nt.assert_allclose(trajectory.qdd, [0]) + + +def test_quintic_hermite_routing(monkeypatch: pytest.MonkeyPatch) -> None: + from roboticstoolbox.tools import trajectory + + calls = [] + hermite = trajectory.quintic_hermite_func + + def record_hermite(*args: float) -> Callable: + calls.append(args) + return hermite(*args) + + def unexpected_linear_system(*args: float) -> Callable: + pytest.fail("quintic() must evaluate with the Hermite function") + + monkeypatch.setattr(trajectory, "quintic_hermite_func", record_hermite) + monkeypatch.setattr(trajectory, "quintic_func", unexpected_linear_system) + actual = trajectory.quintic(1, 2, [5, 505, 1005], qd0=0.2, qdf=-0.3) + nt.assert_equal(calls, [(1, 2, 1000, 0.2, -0.3)]) + nt.assert_array_equal(actual.t, [5, 505, 1005]) + nt.assert_allclose(actual.q[[0, -1]], [1, 2]) + nt.assert_allclose(actual.qd[[0, -1]], [0.2, -0.3]) + nt.assert_allclose(actual.qdd[[0, -1]], [0, 0]) + + +def test_bdsim_quintic_retains_linear_system() -> None: + from roboticstoolbox.blocks.arm import Traj + + ordinary = Traj(y0=1, yf=2, T=5, time=True, traj="quintic") + ordinary.start(SimpleNamespace(T=5)) + nt.assert_allclose(ordinary.output(2.5, [], []), [[1.5], [0.375], [0]], atol=1e-12) + long_duration = Traj(y0=1, yf=2, T=1000, time=True, traj="quintic") + with pytest.raises(np.linalg.LinAlgError, match="use quintic_hermite_func"): + long_duration.start(SimpleNamespace(T=1000)) @pytest.mark.parametrize("duration", [0.001, 0.01, 1.0, 10.0, 999.0, 1000.0, 1e6]) @pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_func_analytic_polynomial( +def test_quintic_hermite_func_analytic_polynomial( duration: float, moving_endpoints: bool ) -> None: # These fixed polynomials independently specify all six boundary conditions. @@ -26,7 +276,7 @@ def test_quintic_func_analytic_polynomial( velocity = polynomial.deriv() acceleration = velocity.deriv() samples = np.array([0.0, 0.125, 0.25, 0.5, 0.875, 1.0]) - function = quintic_func( + function = quintic_hermite_func( 1, 2, duration, qd0=velocity(0) / duration, qdf=velocity(1) / duration ) position, speed, accel = function(samples * duration) @@ -44,13 +294,15 @@ def test_quintic_func_analytic_polynomial( @pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) -def test_quintic_func_physical_endpoint_velocities(duration: float) -> None: +def test_quintic_hermite_func_physical_endpoint_velocities(duration: float) -> None: # Independent Hermite basis functions, with fixed velocities in time units. position_basis = Polynomial([0, 0, 0, 10, -15, 6]) initial_velocity_basis = Polynomial([0, 1, 0, -6, 8, -3]) final_velocity_basis = Polynomial([0, 0, 0, -4, 7, -3]) samples = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) - actual = quintic_func(-2, 3, duration, qd0=0.2, qdf=-0.3)(samples * duration) + actual = quintic_hermite_func(-2, 3, duration, qd0=0.2, qdf=-0.3)( + samples * duration + ) for order, values in enumerate(actual): # Evaluate each integer-coefficient basis before scaling so exact zero # endpoint derivatives are not replaced by oracle cancellation noise. @@ -139,8 +391,8 @@ def test_quintic_multi_axis_native_control(duration: float) -> None: @pytest.mark.parametrize("samples", [0.5, [0, 0.5, 1], [[0, 0.5], [0.75, 1]]]) -def test_quintic_func_sample_shapes(samples: ArrayLike) -> None: - function = quintic_func(np.int64(1), np.float32(2), 1) +def test_quintic_hermite_func_sample_shapes(samples: ArrayLike) -> None: + function = quintic_hermite_func(np.int64(1), np.float32(2), 1) position, velocity, acceleration = function(samples) polynomial = Polynomial([1, 0, 0, 10, -15, 6]) for actual, expected in zip( @@ -165,7 +417,7 @@ def test_quintic_func_sample_shapes(samples: ArrayLike) -> None: np.array([[0, 0.3], [0.7, 1]], dtype=np.float32), ], ) -def test_quintic_func_float32_sample_precision( +def test_quintic_hermite_func_float32_sample_precision( duration: float, samples: ArrayLike ) -> None: times = np.asarray(samples) * np.float32(duration) @@ -184,7 +436,7 @@ def test_quintic_func_float32_sample_precision( ] ) reference = np.asarray(expected, dtype=float).T.reshape((3, *np.shape(times))) - for order, actual in enumerate(quintic_func(1, 2, duration)(times)): + for order, actual in enumerate(quintic_hermite_func(1, 2, duration)(times)): nt.assert_equal(np.shape(actual) == np.shape(times), True) nt.assert_equal(np.asarray(actual).dtype == np.dtype(np.float64), True) nt.assert_allclose( @@ -205,7 +457,7 @@ def test_quintic_func_float32_sample_precision( np.clongdouble, ], ) -def test_quintic_func_sample_dtype_compatibility(dtype: type) -> None: +def test_quintic_hermite_func_sample_dtype_compatibility(dtype: type) -> None: values = ( [0.3 + 0.2j, 0.7 - 0.1j] if np.issubdtype(dtype, np.complexfloating) @@ -214,7 +466,7 @@ def test_quintic_func_sample_dtype_compatibility(dtype: type) -> None: samples = np.asarray(values, dtype=dtype) result_dtype = np.result_type(samples.dtype, np.float64) polynomial = Polynomial([1, 0, 0, 10, -15, 6]) - for order, actual in enumerate(quintic_func(1, 2, 1)(samples)): + for order, actual in enumerate(quintic_hermite_func(1, 2, 1)(samples)): nt.assert_equal(isinstance(actual, np.ndarray), True) nt.assert_equal(actual.shape == samples.shape, True) nt.assert_equal(actual.dtype == result_dtype, True) @@ -228,7 +480,7 @@ def test_quintic_func_sample_dtype_compatibility(dtype: type) -> None: @pytest.mark.parametrize("dtype", [np.longdouble, np.clongdouble]) @pytest.mark.parametrize("argument", ["q0", "T", "qd0"]) -def test_quintic_func_extended_boundary_dtype_compatibility( +def test_quintic_hermite_func_extended_boundary_dtype_compatibility( dtype: type, argument: str ) -> None: arguments = {"q0": 1, "qf": 2, "T": 1, "qd0": 0} @@ -236,17 +488,19 @@ def test_quintic_func_extended_boundary_dtype_compatibility( result_dtype = np.result_type(dtype, np.float64) if result_dtype in (np.dtype(np.float64), np.dtype(np.complex128)): # Some platforms give longdouble the same precision as float64. - nt.assert_allclose(quintic_func(**arguments)(0.5), [1.5, 1.875, 0], atol=1e-12) + nt.assert_allclose( + quintic_hermite_func(**arguments)(0.5), [1.5, 1.875, 0], atol=1e-12 + ) else: with pytest.raises( TypeError, match=f"array type {result_dtype} is unsupported" ): - quintic_func(**arguments) + quintic_hermite_func(**arguments) def test_quintic_zero_duration_compatibility() -> None: # Preserve the existing least-squares result for the degenerate duration. - function = quintic_func(1, 2, 0, qd0=0.2, qdf=-0.1) + function = quintic_hermite_func(1, 2, 0, qd0=0.2, qdf=-0.1) position, velocity, acceleration = function(np.array([0, 0.5, 1])) nt.assert_allclose(position, [1.5, 1.525, 1.55], atol=1e-12) nt.assert_allclose(velocity, [0.05, 0.05, 0.05], atol=1e-12) @@ -254,18 +508,18 @@ def test_quintic_zero_duration_compatibility() -> None: @pytest.mark.parametrize("container", [list, np.array]) -def test_quintic_func_single_element_endpoint_arrays( +def test_quintic_hermite_func_single_element_endpoint_arrays( container: Callable[[list[int]], ArrayLike], ) -> None: - actual = quintic_func(container([1]), container([2]), 1)([0, 0.5, 1]) + actual = quintic_hermite_func(container([1]), container([2]), 1)([0, 0.5, 1]) nt.assert_allclose(actual[0], [1, 1.5, 2], atol=1e-12) nt.assert_allclose(actual[1], [0, 1.875, 0], atol=1e-12) nt.assert_allclose(actual[2], [0, 0, 0], atol=1e-12) -def test_quintic_func_rejects_multi_axis_endpoints() -> None: +def test_quintic_hermite_func_rejects_multi_axis_endpoints() -> None: with pytest.raises(np.linalg.LinAlgError, match="Incompatible dimensions"): - quintic_func([1, 2], [3, 4], 1) + quintic_hermite_func([1, 2], [3, 4], 1) def test_quintic_invalid_sampling_argument() -> None: From 709632a4fb8910df09237acf09958324a863fe41 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 16 Oct 2026 09:01:06 +1000 Subject: [PATCH 4/5] fix(trajectory): solve the quintic in normalized time, Hermite form behind method= The quintic boundary-condition matrix has entries from 1 to T**5, so its condition number is 1e15 at T=1000 and 1e16 at T=0.001, and lstsq silently drops a boundary condition: quintic(1, 2, 1000) started near zero. quintic_func now solves the same linear system in normalized time tau = t/T, where the matrix is that of the docstring with T = 1 (condition number 762, the same for every trajectory), scales the velocity boundary values by T and the derivatives by 1/T and 1/T**2. The closed-form Hermite basis contributed in #736 is kept, as method="hermite", and is used as an independent check. Raise ValueError for a zero or non-finite duration, and for a trajectory of fewer than two samples, instead of returning a degenerate result. quintic_hermite_func is not a public name. Triggered by #736 from Afloat16. Co-authored-by: Afloat <132809511+Afloat16@users.noreply.github.com> Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/__init__.py | 1 - src/roboticstoolbox/tools/__init__.py | 2 - src/roboticstoolbox/tools/trajectory.py | 226 +++++----- tests/test_quintic_conditioning.py | 561 +++++------------------- 4 files changed, 209 insertions(+), 581 deletions(-) diff --git a/src/roboticstoolbox/__init__.py b/src/roboticstoolbox/__init__.py index c988eced3..384cf3007 100644 --- a/src/roboticstoolbox/__init__.py +++ b/src/roboticstoolbox/__init__.py @@ -45,7 +45,6 @@ "Ticker", "quintic", "quintic_func", - "quintic_hermite_func", "jtraj", "ctraj", "trapezoidal", diff --git a/src/roboticstoolbox/tools/__init__.py b/src/roboticstoolbox/tools/__init__.py index 78622dc19..0d5ee9011 100644 --- a/src/roboticstoolbox/tools/__init__.py +++ b/src/roboticstoolbox/tools/__init__.py @@ -5,7 +5,6 @@ from roboticstoolbox.tools.trajectory import ( quintic, quintic_func, - quintic_hermite_func, jtraj, mtraj, ctraj, @@ -34,7 +33,6 @@ "Ticker", "quintic", "quintic_func", - "quintic_hermite_func", "jtraj", "ctraj", "trapezoidal", diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 62943255a..0488ddbf2 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -2,6 +2,7 @@ import warnings from collections import namedtuple from collections.abc import Callable +from typing import Literal import matplotlib.pyplot as plt import numpy as np from numpy.typing import ArrayLike @@ -336,10 +337,6 @@ def quintic( time scaling is based on the first and last element. The returned trajectory retains the supplied time vector. - The samples are evaluated using :func:`quintic_hermite_func`, which works in - normalized time. The linear-system derivation is available separately in - :func:`quintic_func`. - :References: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. @@ -355,9 +352,11 @@ def quintic( istime = True else: raise TypeError("bad argument for time, must be int or vector") + if len(t) < 2: + raise ValueError("a trajectory needs at least two time samples") tf = t[-1] - t[0] - polyfunc = quintic_hermite_func(q0, qf, tf, qd0, qdf) + polyfunc = quintic_func(q0, qf, tf, qd0, qdf) # evaluate the polynomials traj = polyfunc(t - t[0]) @@ -369,151 +368,138 @@ def quintic( def quintic_func( - q0: float, qf: float, T: float, qd0: float = 0, qdf: float = 0 + q0: float, + qf: float, + T: float, + qd0: float = 0, + qdf: float = 0, + method: Literal["lstsq", "hermite"] = "lstsq", ) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: r""" - Quintic scalar polynomial from a linear system + Quintic scalar polynomial as a function :param q0: initial value :param qf: final value - :param T: trajectory duration + :param T: trajectory duration, a finite non-zero value :param qd0: initial velocity, defaults to 0 :param qdf: final velocity, defaults to 0 + :param method: how the polynomial is found, ``"lstsq"`` (default) solves the + linear system, ``"hermite"`` uses the closed-form Hermite basis :returns: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` - :raises numpy.linalg.LinAlgError: the boundary-condition matrix is numerically - singular or contains non-finite values - - The six polynomial coefficients are found by solving a linear system for - the initial and final position, velocity and zero acceleration. Derivatives - are with respect to the supplied time units. This formulation is useful for - illustrating how those boundary conditions determine a quintic polynomial. - - Very short or long durations make the powers of ``T`` in the matrix differ - greatly in magnitude. The least-squares solver can then discard a boundary - condition: for example, ``quintic_func(1, 2, 1000)`` previously returned a - function whose initial position was close to zero instead of one. - - The matrix condition number is checked against ``1 / (6 * eps)``, where - ``eps`` is float64 machine precision. This is the numerical-rank cutoff of - ``numpy.linalg.lstsq(..., rcond=None)`` for this six-by-six system. At or above - this cutoff, including zero duration, an error is raised. Passing the check - does not guarantee accurate results for every ill-conditioned system; use - :func:`quintic_hermite_func` to avoid this duration-dependent conditioning. + :raises ValueError: ``T`` is zero or not finite, ``method`` is unknown, or an + endpoint value is not a scalar + + Returns a function which computes the specific quintic polynomial, and its + derivatives, as described by the parameters. Velocity and acceleration are + with respect to the units of ``T``, and the acceleration is zero at both ends. + + The polynomial :math:`q(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0` + is found from the position, velocity and acceleration at the start and end + + .. math:: + + \begin{pmatrix} + 0 & 0 & 0 & 0 & 0 & 1 \\ + T^5 & T^4 & T^3 & T^2 & T & 1 \\ + 0 & 0 & 0 & 0 & 1 & 0 \\ + 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\ + 0 & 0 & 0 & 2 & 0 & 0 \\ + 20T^3 & 12T^2 & 6T & 2 & 0 & 0 + \end{pmatrix} + \begin{pmatrix} a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix} + = + \begin{pmatrix} q_0 \\ q_f \\ \dot{q}_0 \\ \dot{q}_f \\ 0 \\ 0 \end{pmatrix} + + This matrix is badly scaled, since its entries range from 1 to :math:`T^5`. + Its condition number is about 10\ :sup:`3` for :math:`T=1` but 10\ :sup:`15` + for :math:`T=1000` and 10\ :sup:`16` for :math:`T=0.001`, at which point a + least-squares solver silently discards a boundary condition: the initial + position of ``quintic(1, 2, 1000)`` came out near zero rather than one. + + For ``method="lstsq"`` the system is therefore solved in normalized time + :math:`\tau = t/T`, for which the matrix above is used with :math:`T=1`. It + is the same for every trajectory, with a condition number of 762, and the + velocity boundary values are multiplied by :math:`T`. The derivatives are + divided by :math:`T` and :math:`T^2` to return to the units of ``t``. + + For ``method="hermite"`` the solution of the same system is written down + directly, as a sum of Hermite basis polynomials in :math:`\tau`. It is + equivalent, and is useful as an independent check. Example: .. runblock:: pycon >>> from roboticstoolbox import quintic_func - >>> f = quintic_func(1, 2, 5) + >>> f = quintic_func(1, 2, 1000) >>> f(0) - >>> f(5) - >>> f(2.5) + >>> f(1000) + >>> f(500) - :seealso: :func:`quintic_hermite_func`, :func:`quintic`, :func:`trapezoidal_func` - """ + .. note:: The Hermite form is due to Afloat16, who found the loss of + accuracy for long and short durations in PR #736. - # solve for the polynomial coefficients using least squares + :seealso: :func:`quintic` :func:`trapezoidal_func` + """ + if method not in ("lstsq", "hermite"): + raise ValueError(f"unknown method {method!r}, must be 'lstsq' or 'hermite'") + if not np.isfinite(T) or T == 0: + raise ValueError("trajectory duration must be finite and non-zero") + boundary = np.r_[q0, qf, qd0, qdf] + if boundary.shape != (4,): + raise ValueError("q0, qf, qd0 and qdf must be scalars") + + if method == "hermite": + return _quintic_hermite_func(q0, qf, T, qd0, qdf) + + # solve for the polynomial coefficients in normalized time, tau = t / T, + # where the matrix is that of the docstring with T = 1 # fmt: off - X = [ - [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], - [ T**5, T**4, T**3, T**2, T, 1.0], - [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], - [ 5.0 * T**4, 4.0 * T**3, 3.0 * T**2, 2.0 * T, 1.0, 0.0], - [ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0], - [20.0 * T**3, 12.0 * T**2, 6.0 * T, 2.0, 0.0, 0.0], - ] + X = np.array([ + [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], + [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], + [ 5.0, 4.0, 3.0, 2.0, 1.0, 0.0], + [ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0], + [20.0, 12.0, 6.0, 2.0, 0.0, 0.0], + ]) # fmt: on - # Match lstsq's default singular-value cutoff for this square system. - if not np.all(np.isfinite(X)) or np.linalg.cond(X) >= 1 / ( - len(X) * np.finfo(float).eps - ): - raise np.linalg.LinAlgError( - "quintic boundary-condition matrix is ill-conditioned; " - "use quintic_hermite_func instead" - ) - coeffs, resid, rank, s = np.linalg.lstsq( - X, np.r_[q0, qf, qd0, qdf, 0, 0], rcond=None - ) + # velocities with respect to tau are T times those with respect to t + coeffs = np.linalg.solve(X, np.r_[q0, qf, qd0 * T, qdf * T, 0, 0]) # coefficients of derivatives coeffs_d = coeffs[0:5] * np.arange(5, 0, -1) coeffs_dd = coeffs_d[0:4] * np.arange(4, 0, -1) - return lambda x: ( - np.polyval(coeffs, x), - np.polyval(coeffs_d, x), - np.polyval(coeffs_dd, x), - ) + def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: + tau = np.asarray(x) / T + return ( + np.polyval(coeffs, tau), + np.polyval(coeffs_d, tau) / T, + np.polyval(coeffs_dd, tau) / T**2, + ) + + return polynomial -def quintic_hermite_func( - q0: float, qf: float, T: float, qd0: float = 0, qdf: float = 0 +def _quintic_hermite_func( + q0: float, qf: float, T: float, qd0: float, qdf: float ) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: - r""" + """ Quintic scalar polynomial using a Hermite basis - :param q0: initial value - :param qf: final value - :param T: trajectory duration - :param qd0: initial velocity, defaults to 0 - :param qdf: final velocity, defaults to 0 - :returns: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` - :raises ValueError: trajectory duration is not finite - - Returns a function which computes the specific quintic polynomial, and its - derivatives, as described by the parameters. The polynomial is constructed and - evaluated in normalized time to avoid ill-conditioning for short or long - durations. Returned derivatives are with respect to the supplied time, with - zero acceleration at both endpoints. - - The returned function accepts scalar or array-like elapsed times, preserving - the sample shape. ``qd0`` and ``qdf`` use position units per supplied time - unit. The duration is promoted to the coefficient dtype before scaling. - For ``T == 0``, the degenerate linear least-squares result is retained: - the position starts at the mean of ``q0`` and ``qf``, velocity is the mean - of ``qd0`` and ``qdf``, and acceleration is zero. This zero-duration case - cannot satisfy distinct endpoint conditions. - - Example: - - .. runblock:: pycon - - >>> from roboticstoolbox import quintic_hermite_func - >>> f = quintic_hermite_func(1, 2, 1000) - >>> f(0) - >>> f(1000) - >>> f(500) - - :seealso: :func:`quintic_func`, :func:`quintic`, :func:`trapezoidal_func` + The polynomial is constructed and evaluated in normalized time to avoid + ill-conditioning for short or long durations. The derivatives are factored + so that zero endpoint acceleration is not lost through polynomial + cancellation. Contributed by Afloat16 in PR #736. """ - - # Hermite basis functions use normalized time. Factor the derivatives so - # zero endpoint acceleration is not lost through polynomial cancellation. - if not np.isfinite(T): - raise ValueError("trajectory duration must be finite") - boundary = np.r_[q0, qf, qd0, qdf, 0, 0] - if boundary.shape != (6,): - raise np.linalg.LinAlgError("Incompatible dimensions") - coefficient_dtype = np.result_type(boundary.dtype, np.asarray(T).dtype, np.float64) - if coefficient_dtype not in (np.float64, np.complex128): - raise TypeError(f"array type {coefficient_dtype} is unsupported in linalg") - tscal = coefficient_dtype.type(T) if T != 0 else 1.0 - q0, qf, v0, vf, _, _ = boundary * np.r_[1.0, 1.0, tscal, tscal, 1.0, 1.0] + # velocities with respect to normalized time + v0 = qd0 * T + vf = qdf * T delta = qf - q0 def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: - samples = np.asarray(x) - sample_dtype = np.result_type(samples.dtype, coefficient_dtype) - tau = np.asarray(samples, dtype=sample_dtype) / tscal - if T == 0: - # Preserve the existing degenerate least-squares linear result. - coeffs = np.r_[0, 0, 0, 0, (v0 + vf) / 2, (q0 + qf) / 2] - return ( - np.polyval(coeffs, tau), - np.polyval(np.r_[0, 0, 0, 0, (v0 + vf) / 2], tau), - np.polyval(np.zeros(4), tau), - ) + tau = np.asarray(x, dtype=float) / T remaining = 1 - tau blend = tau**3 * (10 + tau * (-15 + 6 * tau)) return ( @@ -526,13 +512,13 @@ def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: + v0 * remaining**2 * (1 + 2 * tau - 15 * tau**2) + vf * tau**2 * (1 + 2 * remaining - 15 * remaining**2) ) - / tscal, + / T, 12 * tau * remaining * (5 * delta * (1 - 2 * tau) - v0 * (3 - 5 * tau) - vf * (2 - 5 * tau)) - / tscal - / tscal, + / T + / T, ) return polynomial diff --git a/tests/test_quintic_conditioning.py b/tests/test_quintic_conditioning.py index e5f2f0de0..797f10c96 100644 --- a/tests/test_quintic_conditioning.py +++ b/tests/test_quintic_conditioning.py @@ -1,327 +1,109 @@ -"""Quintic boundary conditions across trajectory time scales.""" +"""Quintic boundary conditions across trajectory time scales + +The reference is always an independent integer-coefficient polynomial in +normalized time, so these tests share no code with the implementation. +""" -from collections.abc import Callable -from decimal import Decimal, localcontext from types import SimpleNamespace import numpy as np import numpy.testing as nt import pytest from numpy.polynomial import Polynomial -from numpy.typing import ArrayLike - -from roboticstoolbox import jtraj, mtraj, quintic, quintic_func, quintic_hermite_func - - -@pytest.mark.parametrize("duration", [1.0, 5.0, 10.0, 49.0, -1.0, -10.0]) -@pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_func_linear_system_control( - duration: float, moving_endpoints: bool -) -> None: - # Independent integer-coefficient polynomials, including nonzero velocities. - polynomial = Polynomial( - [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] - ) - samples = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) - actual = quintic_func( - 1, - 2, - duration, - qd0=polynomial.deriv()(0) / duration, - qdf=polynomial.deriv()(1) / duration, - )(samples * duration) - for order, values in enumerate(actual): - nt.assert_allclose( - values * duration**order, - polynomial.deriv(order)(samples), - rtol=1e-10, - atol=1e-10, - ) - -@pytest.mark.parametrize("duration", [0.0, 0.001, -0.001, 1000.0, -1000.0, 1e6]) -def test_quintic_func_rejects_numerically_singular_system(duration: float) -> None: - with pytest.raises(np.linalg.LinAlgError, match="use quintic_hermite_func"): - quintic_func(1, 2, duration) +from roboticstoolbox import jtraj, mtraj, quintic, quintic_func - -@pytest.mark.parametrize("duration", [0.0019, 0.0021, 920.0, 970.0]) -def test_quintic_func_condition_cutoff(duration: float) -> None: - # Build the boundary operator from independently differentiated monomials, - # using ascending powers rather than copying the production matrix. - matrix = np.array( - [ - [Polynomial.basis(power).deriv(order)(time) for power in range(6)] - for order in range(3) - for time in (0, duration) - ] - ) - cutoff = 1 / (6 * np.finfo(float).eps) - condition = np.linalg.cond(matrix) - # These samples straddle each cutoff with a margin against SVD round-off. - nt.assert_equal(0.7 < condition / cutoff < 1.4, True) - if duration in (0.0019, 970.0): - nt.assert_equal(condition > cutoff, True) - with pytest.raises(np.linalg.LinAlgError, match="ill-conditioned"): - quintic_func(1, 2, duration) - else: - nt.assert_equal(condition < cutoff, True) - values = quintic_func(1, 2, duration)([0, duration / 2, duration]) - nt.assert_equal(np.all(np.isfinite(values)), True) - - -@pytest.mark.parametrize("duration", [np.nan, np.inf, -np.inf]) -def test_quintic_func_nonfinite_duration(duration: float) -> None: - with pytest.raises(np.linalg.LinAlgError, match="ill-conditioned"): - quintic_func(1, 2, duration) - with pytest.raises(ValueError, match="duration must be finite"): - quintic_hermite_func(1, 2, duration) - - -def test_quintic_hermite_func_public_exports() -> None: - import roboticstoolbox - import roboticstoolbox.tools - - nt.assert_equal( - roboticstoolbox.tools.quintic_hermite_func is quintic_hermite_func, True - ) - for module in (roboticstoolbox, roboticstoolbox.tools): - nt.assert_equal("quintic_hermite_func" in module.__all__, True) - - -@pytest.mark.parametrize("duration", [-0.001, -1.0, -1000.0, 1e-6]) -def test_quintic_hermite_func_signed_and_short_duration(duration: float) -> None: - polynomial = Polynomial([1, 1, 0, -4, 7, -3]) - samples = np.array([0, 0.25, 0.5, 0.75, 1]) - actual = quintic_hermite_func(1, 2, duration, 1 / duration, 2 / duration)( - samples * duration - ) - for order, values in enumerate(actual): - nt.assert_allclose( - values * duration**order, - polynomial.deriv(order)(samples), - rtol=1e-12, - atol=1e-12, - ) +DURATIONS = [0.001, 0.01, 1.0, 10.0, 1000.0, 1e6, -1.0, -1000.0] +SAMPLES = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) -@pytest.mark.parametrize( - "duration", - [ - np.float16(0.001), - np.float32(1e-25), - np.float32(1e-20), - np.float32(1e20), - np.float32(1e30), - np.float32(-1e-25), - np.int64(10**10), - np.complex64(1e-25 + 1e-25j), - np.complex64(1e20 + 1e20j), - ], -) +@pytest.mark.parametrize("method", ["lstsq", "hermite"]) +@pytest.mark.parametrize("duration", DURATIONS) @pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_hermite_func_promotes_duration( - duration: complex, moving_endpoints: bool +def test_quintic_func_matches_polynomial( + method: str, duration: float, moving_endpoints: bool ) -> None: - # Use the represented duration, then scale in float64/complex128. In - # particular, a float32 square must not overflow or underflow before the - # derivative is divided by it. - represented_duration = ( - complex(duration) if np.iscomplexobj(duration) else float(duration) - ) + # q(tau) from 1 to 2 with zero acceleration at both ends polynomial = Polynomial( [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] ) - samples = np.array([0, 0.25, 0.5, 0.75, 1]) - actual = quintic_hermite_func( - 1, - 2, - duration, - qd0=polynomial.deriv()(0) / represented_duration, - qdf=polynomial.deriv()(1) / represented_duration, - )(samples * represented_duration) - nt.assert_equal(np.all(np.isfinite(actual)), True) - for order, values in enumerate(actual): - nt.assert_allclose( - values * represented_duration**order, - polynomial.deriv(order)(samples), - rtol=1e-12, - atol=1e-12, - ) - - -@pytest.mark.parametrize("duration", [1e155, -1e155]) -@pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_hermite_func_large_duration_acceleration( - duration: float, moving_endpoints: bool -) -> None: - polynomial = Polynomial( - [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] + # velocities are given in position units per time unit, not per tau + qd0 = polynomial.deriv()(0) / duration + qdf = polynomial.deriv()(1) / duration + actual = quintic_func(1, 2, duration, qd0=qd0, qdf=qdf, method=method)( + SAMPLES * duration ) - samples = np.array([0, 0.25, 0.5, 0.75, 1]) - actual = quintic_hermite_func( - 1, - 2, - duration, - qd0=polynomial.deriv()(0) / duration, - qdf=polynomial.deriv()(1) / duration, - )(samples * duration) - nt.assert_equal(np.all(np.isfinite(actual)), True) for order, values in enumerate(actual): - # Scale sequentially: duration**2 overflows although the small physical - # acceleration and its normalized reference are both representable. - scaled = values - for _ in range(order): - scaled = scaled * duration nt.assert_allclose( - scaled, polynomial.deriv(order)(samples), rtol=1e-12, atol=1e-12 + values * duration**order, + polynomial.deriv(order)(SAMPLES), + rtol=1e-9, + atol=1e-9, ) -@pytest.mark.parametrize("duration", [1e-200, -1e-200]) -def test_quintic_hermite_func_tiny_duration_endpoints(duration: float) -> None: - # Interior acceleration need not fit float64 at this scale, but the exact - # zero endpoint accelerations must not turn into 0/0 when T**2 underflows. - actual = quintic_hermite_func(1, 2, duration, 0.2, -0.3)([0, duration]) - nt.assert_allclose(actual[0], [1, 2], atol=1e-12) - nt.assert_allclose(actual[1], [0.2, -0.3], atol=1e-12) - nt.assert_array_equal(actual[2], [0, 0]) - - -@pytest.mark.parametrize("duration", [1e-200, -1e-200]) -@pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_hermite_func_tiny_duration_small_displacement( - duration: float, moving_endpoints: bool -) -> None: - displacement = 1e-200 - polynomial = Polynomial( - [0, 1, 0, -4, 7, -3] if moving_endpoints else [0, 0, 0, 10, -15, 6] - ) - samples = np.array([0, 0.25, 0.5, 0.75, 1]) - actual = quintic_hermite_func( - 0, - displacement, - duration, - qd0=polynomial.deriv()(0) * displacement / duration, - qdf=polynomial.deriv()(1) * displacement / duration, - )(samples * duration) - nt.assert_equal(np.all(np.isfinite(actual)), True) - for order, values in enumerate(actual): - # A tiny displacement gives representable interior acceleration even - # though duration**2 is zero in float64. Keep the oracle scaling safe. - scaled = values - for _ in range(order): - scaled = scaled * duration - nt.assert_allclose( - scaled / displacement, - polynomial.deriv(order)(samples), - rtol=1e-12, - atol=1e-12, +@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) +def test_quintic_func_physical_endpoint_velocities(duration: float) -> None: + # Hermite basis polynomials in tau, endpoint velocities in time units + position = Polynomial([0, 0, 0, 10, -15, 6]) + initial_velocity = Polynomial([0, 1, 0, -6, 8, -3]) + final_velocity = Polynomial([0, 0, 0, -4, 7, -3]) + for method in ("lstsq", "hermite"): + actual = quintic_func(-2, 3, duration, qd0=0.2, qdf=-0.3, method=method)( + SAMPLES * duration ) + for order, values in enumerate(actual): + reference = ( + 5 * position.deriv(order)(SAMPLES) + + (0.2 * duration) * initial_velocity.deriv(order)(SAMPLES) + - (0.3 * duration) * final_velocity.deriv(order)(SAMPLES) + ) + if order == 0: + reference -= 2 + nt.assert_allclose(values * duration**order, reference, atol=1e-9) + nt.assert_allclose(actual[0][[0, -1]], [-2, 3], atol=1e-9) + nt.assert_allclose(actual[1][[0, -1]], [0.2, -0.3], atol=1e-9) + nt.assert_allclose(actual[2][[0, -1]], [0, 0], atol=1e-6) + + +def test_quintic_func_long_duration_regression() -> None: + # PR #736: the unscaled least-squares system started this near zero + f = quintic_func(1, 2, 1000) + nt.assert_allclose([f(0)[0], f(500)[0], f(1000)[0]], [1, 1.5, 2], atol=1e-12) + nt.assert_allclose(quintic(1, 2, 1000).q[[0, -1]], [1, 2], atol=1e-12) + + +def test_quintic_func_methods_agree() -> None: + rng = np.random.default_rng(0) + for duration in (0.001, 0.5, 7.0, 1000.0, 1e5): + q0, qf, qd0, qdf = rng.uniform(-5, 5, 4) + t = np.linspace(0, duration, 25) + a = quintic_func(q0, qf, duration, qd0, qdf, method="lstsq")(t) + b = quintic_func(q0, qf, duration, qd0, qdf, method="hermite")(t) + for order, (x, y) in enumerate(zip(a, b)): + nt.assert_allclose( + x * duration**order, y * duration**order, rtol=1e-8, atol=1e-8 + ) -def test_quintic_single_sample_zero_duration() -> None: - trajectory = quintic(1, 2, 1, 0.2, -0.1) - nt.assert_array_equal(trajectory.t, [0]) - nt.assert_allclose(trajectory.q, [1.5]) - nt.assert_allclose(trajectory.qd, [0.05]) - nt.assert_allclose(trajectory.qdd, [0]) - - -def test_quintic_hermite_routing(monkeypatch: pytest.MonkeyPatch) -> None: - from roboticstoolbox.tools import trajectory - - calls = [] - hermite = trajectory.quintic_hermite_func - - def record_hermite(*args: float) -> Callable: - calls.append(args) - return hermite(*args) - - def unexpected_linear_system(*args: float) -> Callable: - pytest.fail("quintic() must evaluate with the Hermite function") - - monkeypatch.setattr(trajectory, "quintic_hermite_func", record_hermite) - monkeypatch.setattr(trajectory, "quintic_func", unexpected_linear_system) - actual = trajectory.quintic(1, 2, [5, 505, 1005], qd0=0.2, qdf=-0.3) - nt.assert_equal(calls, [(1, 2, 1000, 0.2, -0.3)]) - nt.assert_array_equal(actual.t, [5, 505, 1005]) - nt.assert_allclose(actual.q[[0, -1]], [1, 2]) - nt.assert_allclose(actual.qd[[0, -1]], [0.2, -0.3]) - nt.assert_allclose(actual.qdd[[0, -1]], [0, 0]) - - -def test_bdsim_quintic_retains_linear_system() -> None: - from roboticstoolbox.blocks.arm import Traj - - ordinary = Traj(y0=1, yf=2, T=5, time=True, traj="quintic") - ordinary.start(SimpleNamespace(T=5)) - nt.assert_allclose(ordinary.output(2.5, [], []), [[1.5], [0.375], [0]], atol=1e-12) - long_duration = Traj(y0=1, yf=2, T=1000, time=True, traj="quintic") - with pytest.raises(np.linalg.LinAlgError, match="use quintic_hermite_func"): - long_duration.start(SimpleNamespace(T=1000)) - +@pytest.mark.parametrize("duration", [0, np.nan, np.inf, -np.inf]) +@pytest.mark.parametrize("method", ["lstsq", "hermite"]) +def test_quintic_func_bad_duration(duration: float, method: str) -> None: + with pytest.raises(ValueError, match="finite and non-zero"): + quintic_func(1, 2, duration, method=method) -@pytest.mark.parametrize("duration", [0.001, 0.01, 1.0, 10.0, 999.0, 1000.0, 1e6]) -@pytest.mark.parametrize("moving_endpoints", [False, True]) -def test_quintic_hermite_func_analytic_polynomial( - duration: float, moving_endpoints: bool -) -> None: - # These fixed polynomials independently specify all six boundary conditions. - # Both have p(0)=1, p(1)=2, p''(0)=p''(1)=0. The second has p'(0)=1, - # p'(1)=2; the first has zero endpoint velocities. - polynomial = Polynomial( - [1, 1, 0, -4, 7, -3] if moving_endpoints else [1, 0, 0, 10, -15, 6] - ) - velocity = polynomial.deriv() - acceleration = velocity.deriv() - samples = np.array([0.0, 0.125, 0.25, 0.5, 0.875, 1.0]) - function = quintic_hermite_func( - 1, 2, duration, qd0=velocity(0) / duration, qdf=velocity(1) / duration - ) - position, speed, accel = function(samples * duration) - nt.assert_allclose(position, polynomial(samples), rtol=1e-12, atol=1e-12) - nt.assert_allclose(speed * duration, velocity(samples), rtol=1e-12, atol=1e-12) - nt.assert_allclose( - accel * duration**2, acceleration(samples), rtol=1e-12, atol=1e-12 - ) - nt.assert_allclose(position[[0, -1]], [1, 2], atol=1e-12) - nt.assert_allclose( - speed[[0, -1]] * duration, [velocity(0), velocity(1)], atol=1e-12 - ) - nt.assert_allclose(accel[[0, -1]] * duration**2, [0, 0], atol=1e-12) +def test_quintic_func_bad_arguments() -> None: + with pytest.raises(ValueError, match="unknown method"): + quintic_func(1, 2, 1, method="nope") # type: ignore[arg-type] + with pytest.raises(ValueError, match="must be scalars"): + quintic_func([1, 2], 3, 1) -@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) -def test_quintic_hermite_func_physical_endpoint_velocities(duration: float) -> None: - # Independent Hermite basis functions, with fixed velocities in time units. - position_basis = Polynomial([0, 0, 0, 10, -15, 6]) - initial_velocity_basis = Polynomial([0, 1, 0, -6, 8, -3]) - final_velocity_basis = Polynomial([0, 0, 0, -4, 7, -3]) - samples = np.array([0.0, 0.125, 0.5, 0.875, 1.0]) - actual = quintic_hermite_func(-2, 3, duration, qd0=0.2, qdf=-0.3)( - samples * duration - ) - for order, values in enumerate(actual): - # Evaluate each integer-coefficient basis before scaling so exact zero - # endpoint derivatives are not replaced by oracle cancellation noise. - reference = ( - 5 * position_basis.deriv(order)(samples) - + (0.2 * duration) * initial_velocity_basis.deriv(order)(samples) - - (0.3 * duration) * final_velocity_basis.deriv(order)(samples) - ) - if order == 0: - reference -= 2 - nt.assert_allclose( - values * duration**order, - reference, - rtol=1e-12, - atol=1e-12, - ) - nt.assert_allclose(actual[0][[0, -1]], [-2, 3], atol=1e-12) - nt.assert_allclose(actual[1][[0, -1]], [0.2, -0.3], atol=1e-12) - nt.assert_allclose(actual[2][[0, -1]], [0, 0], atol=1e-9) +@pytest.mark.parametrize("samples", [0.5, [0, 0.5, 1], [[0, 0.5], [0.75, 1]]]) +def test_quintic_func_sample_shapes(samples: object) -> None: + for values in quintic_func(1, 2, 1)(samples): + nt.assert_equal(np.shape(values), np.shape(samples)) @pytest.mark.parametrize("steps", [11, 1000]) @@ -332,196 +114,59 @@ def test_quintic_integer_samples(steps: int) -> None: nt.assert_array_equal(trajectory.t, np.arange(steps)) nt.assert_equal(not trajectory.istime, True) - nt.assert_equal( - trajectory.q.shape == trajectory.qd.shape == trajectory.qdd.shape == (steps,), - True, - ) - nt.assert_allclose(trajectory.q, polynomial(samples), rtol=1e-12, atol=1e-12) + nt.assert_allclose(trajectory.q, polynomial(samples), atol=1e-12) nt.assert_allclose( trajectory.qd * (steps - 1), polynomial.deriv()(samples), atol=1e-12 ) nt.assert_allclose( - trajectory.qdd * (steps - 1) ** 2, - polynomial.deriv(2)(samples), - atol=1e-12, + trajectory.qdd * (steps - 1) ** 2, polynomial.deriv(2)(samples), atol=1e-12 ) @pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) @pytest.mark.parametrize("origin", [-5.0, 0.0, 5.0]) def test_quintic_sampled_time_origin(duration: float, origin: float) -> None: - times = origin + duration * np.array([0.0, 0.125, 0.5, 0.875, 1.0]) - actual_duration = times[-1] - times[0] - # Use the represented elapsed times, including floating-point timestamp noise. - samples = (times - times[0]) / actual_duration + times = origin + duration * SAMPLES + elapsed = times[-1] - times[0] polynomial = Polynomial([1, 1, 0, -4, 7, -3]) - trajectory = quintic(1, 2, times, qd0=1 / actual_duration, qdf=2 / actual_duration) + trajectory = quintic(1, 2, times, qd0=1 / elapsed, qdf=2 / elapsed) nt.assert_array_equal(trajectory.t, times) nt.assert_equal(trajectory.istime, True) - for values, derivative, scale in ( + tau = (times - times[0]) / elapsed + for values, reference, scale in ( (trajectory.q, polynomial, 1), - (trajectory.qd, polynomial.deriv(), actual_duration), - (trajectory.qdd, polynomial.deriv(2), actual_duration**2), + (trajectory.qd, polynomial.deriv(), elapsed), + (trajectory.qdd, polynomial.deriv(2), elapsed**2), ): - nt.assert_allclose(values * scale, derivative(samples), rtol=1e-12, atol=1e-12) + nt.assert_allclose(values * scale, reference(tau), rtol=1e-9, atol=1e-9) @pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) -def test_quintic_multi_axis_native_control(duration: float) -> None: +def test_quintic_multi_axis_matches_jtraj(duration: float) -> None: times = np.linspace(0, duration, 11) initial = np.array([1, -2, 3]) final = np.array([2, 4, 3]) actual = mtraj(quintic, initial, final, times) expected = jtraj(initial, final, times) - nt.assert_equal( - actual.q.shape == actual.qd.shape == actual.qdd.shape == (11, 3), True - ) - nt.assert_array_equal(actual.t, times) - nt.assert_equal(actual.istime, True) - for values, reference, scale in ( - (actual.q, expected.q, 1), - (actual.qd, expected.qd, duration), - (actual.qdd, expected.qdd, duration**2), - ): - nt.assert_equal(np.issubdtype(values.dtype, np.floating), True) - nt.assert_allclose(values * scale, reference * scale, rtol=1e-12, atol=1e-12) - nt.assert_allclose(actual.q[[0, -1]], [initial, final], atol=1e-12) + nt.assert_equal(actual.q.shape, (11, 3)) + nt.assert_allclose(actual.q, expected.q, atol=1e-9) + nt.assert_allclose(actual.qd * duration, expected.qd * duration, atol=1e-9) + nt.assert_allclose(actual.qdd * duration**2, expected.qdd * duration**2, atol=1e-9) -@pytest.mark.parametrize("samples", [0.5, [0, 0.5, 1], [[0, 0.5], [0.75, 1]]]) -def test_quintic_hermite_func_sample_shapes(samples: ArrayLike) -> None: - function = quintic_hermite_func(np.int64(1), np.float32(2), 1) - position, velocity, acceleration = function(samples) - polynomial = Polynomial([1, 0, 0, 10, -15, 6]) - for actual, expected in zip( - (position, velocity, acceleration), - ( - polynomial(samples), - polynomial.deriv()(samples), - polynomial.deriv(2)(samples), - ), - ): - nt.assert_equal(np.shape(actual) == np.shape(samples), True) - nt.assert_equal(np.issubdtype(np.asarray(actual).dtype, np.floating), True) - nt.assert_allclose(actual, expected, rtol=1e-12, atol=1e-12) - - -@pytest.mark.parametrize("duration", [0.001, 1.0, 1000.0]) -@pytest.mark.parametrize( - "samples", - [ - np.float32(0.3), - np.array([0, 0.125, 0.3, 0.5, 0.7, 0.875, 1], dtype=np.float32), - np.array([[0, 0.3], [0.7, 1]], dtype=np.float32), - ], -) -def test_quintic_hermite_func_float32_sample_precision( - duration: float, samples: ArrayLike -) -> None: - times = np.asarray(samples) * np.float32(duration) - # Evaluate independent integer-coefficient polynomials at the exactly - # represented samples, including the rounding of the physical timestamps. - expected = [] - with localcontext() as context: - context.prec = 70 - for time in np.asarray(times).flat: - u = Decimal.from_float(float(time)) / Decimal.from_float(duration) - expected.append( - [ - 1 + 10 * u**3 - 15 * u**4 + 6 * u**5, - 30 * u**2 - 60 * u**3 + 30 * u**4, - 60 * u - 180 * u**2 + 120 * u**3, - ] - ) - reference = np.asarray(expected, dtype=float).T.reshape((3, *np.shape(times))) - for order, actual in enumerate(quintic_hermite_func(1, 2, duration)(times)): - nt.assert_equal(np.shape(actual) == np.shape(times), True) - nt.assert_equal(np.asarray(actual).dtype == np.dtype(np.float64), True) - nt.assert_allclose( - actual * duration**order, reference[order], rtol=1e-12, atol=1e-12 - ) - - -@pytest.mark.parametrize( - "dtype", - [ - np.float16, - np.float32, - np.float64, - np.int64, - np.complex64, - np.complex128, - np.longdouble, - np.clongdouble, - ], -) -def test_quintic_hermite_func_sample_dtype_compatibility(dtype: type) -> None: - values = ( - [0.3 + 0.2j, 0.7 - 0.1j] - if np.issubdtype(dtype, np.complexfloating) - else [0, 0.3, 1] - ) - samples = np.asarray(values, dtype=dtype) - result_dtype = np.result_type(samples.dtype, np.float64) - polynomial = Polynomial([1, 0, 0, 10, -15, 6]) - for order, actual in enumerate(quintic_hermite_func(1, 2, 1)(samples)): - nt.assert_equal(isinstance(actual, np.ndarray), True) - nt.assert_equal(actual.shape == samples.shape, True) - nt.assert_equal(actual.dtype == result_dtype, True) - nt.assert_allclose( - actual, - polynomial.deriv(order)(samples.astype(result_dtype)), - rtol=1e-12, - atol=1e-12, - ) - - -@pytest.mark.parametrize("dtype", [np.longdouble, np.clongdouble]) -@pytest.mark.parametrize("argument", ["q0", "T", "qd0"]) -def test_quintic_hermite_func_extended_boundary_dtype_compatibility( - dtype: type, argument: str -) -> None: - arguments = {"q0": 1, "qf": 2, "T": 1, "qd0": 0} - arguments[argument] = dtype(arguments[argument]) - result_dtype = np.result_type(dtype, np.float64) - if result_dtype in (np.dtype(np.float64), np.dtype(np.complex128)): - # Some platforms give longdouble the same precision as float64. - nt.assert_allclose( - quintic_hermite_func(**arguments)(0.5), [1.5, 1.875, 0], atol=1e-12 - ) - else: - with pytest.raises( - TypeError, match=f"array type {result_dtype} is unsupported" - ): - quintic_hermite_func(**arguments) - - -def test_quintic_zero_duration_compatibility() -> None: - # Preserve the existing least-squares result for the degenerate duration. - function = quintic_hermite_func(1, 2, 0, qd0=0.2, qdf=-0.1) - position, velocity, acceleration = function(np.array([0, 0.5, 1])) - nt.assert_allclose(position, [1.5, 1.525, 1.55], atol=1e-12) - nt.assert_allclose(velocity, [0.05, 0.05, 0.05], atol=1e-12) - nt.assert_allclose(acceleration, [0, 0, 0], atol=1e-12) - - -@pytest.mark.parametrize("container", [list, np.array]) -def test_quintic_hermite_func_single_element_endpoint_arrays( - container: Callable[[list[int]], ArrayLike], -) -> None: - actual = quintic_hermite_func(container([1]), container([2]), 1)([0, 0.5, 1]) - nt.assert_allclose(actual[0], [1, 1.5, 2], atol=1e-12) - nt.assert_allclose(actual[1], [0, 1.875, 0], atol=1e-12) - nt.assert_allclose(actual[2], [0, 0, 0], atol=1e-12) - +def test_quintic_needs_two_samples() -> None: + with pytest.raises(ValueError, match="at least two"): + quintic(1, 2, 1) + with pytest.raises(TypeError, match="bad argument for time"): + quintic(1, 2, "not time") # type: ignore[arg-type] -def test_quintic_hermite_func_rejects_multi_axis_endpoints() -> None: - with pytest.raises(np.linalg.LinAlgError, match="Incompatible dimensions"): - quintic_hermite_func([1, 2], [3, 4], 1) +def test_bdsim_quintic_block_long_duration() -> None: + from roboticstoolbox.blocks.arm import Traj -def test_quintic_invalid_sampling_argument() -> None: - with pytest.raises(TypeError, match="bad argument for time"): - quintic(1, 2, "not time") + block = Traj(y0=1, yf=2, T=1000, time=True, traj="quintic") + block.start(SimpleNamespace(T=1000)) + nt.assert_allclose(block.output(0.0, [], []), [[1], [0], [0]], atol=1e-9) + nt.assert_allclose(block.output(500.0, [], []), [[1.5], [0.001875], [0]], atol=1e-9) From ec47019d6d6bd803b6607680beea2ac0a0b1d83a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 16 Oct 2026 09:33:48 +1000 Subject: [PATCH 5/5] docs(trajectory): versionchanged notes for the quintic time scaling Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/tools/trajectory.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 0488ddbf2..288c99fb2 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -337,6 +337,11 @@ def quintic( time scaling is based on the first and last element. The returned trajectory retains the supplied time vector. + .. versionchanged:: 1.5.0 + Accurate for long and short durations, see :func:`quintic_func`. A time + vector, or an integer, giving fewer than two samples raises + ``ValueError``; it previously returned the mean of ``q0`` and ``qf``. + :References: - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. @@ -439,6 +444,13 @@ def quintic_func( .. note:: The Hermite form is due to Afloat16, who found the loss of accuracy for long and short durations in PR #736. + .. versionchanged:: 1.5.0 + The system is solved in normalized time. Previously the result was + silently wrong for long and short durations, for example the initial + value of ``quintic_func(1, 2, 1000)`` was close to zero. Added + ``method``. A zero or non-finite ``T`` raises ``ValueError``, where it + previously gave a degenerate result. + :seealso: :func:`quintic` :func:`trapezoidal_func` """ if method not in ("lstsq", "hermite"):