From e1636c528ea96d56de971a298242b8eb0f0fa671 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/3] fix(trajectory): normalize quintic profile time --- src/roboticstoolbox/tools/trajectory.py | 87 +++++--- tests/test_quintic_conditioning.py | 268 ++++++++++++++++++++++++ 2 files changed, 322 insertions(+), 33 deletions(-) create mode 100644 tests/test_quintic_conditioning.py diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index 06670cfc1..9a6c6ad48 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -2,6 +2,7 @@ import math import warnings from collections import namedtuple +from collections.abc import Callable import matplotlib.pyplot as plt from numpy.typing import ArrayLike from spatialmath import SE3 @@ -364,25 +365,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 +397,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 e3ff4576029ec246e46abe287adda68e4d424eae 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/3] 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 187ee0a3748027ef9c1bc90cebc611cb4f4c0167 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/3] 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 3fe8f44c7..63f1e3e6c 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 9a6c6ad48..40dd1ad1a 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -337,6 +337,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. @@ -354,7 +358,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]) @@ -369,7 +373,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 @@ -377,35 +381,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 @@ -438,7 +532,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: