diff --git a/src/roboticstoolbox/tools/trajectory.py b/src/roboticstoolbox/tools/trajectory.py index cff845ec6..288c99fb2 100644 --- a/src/roboticstoolbox/tools/trajectory.py +++ b/src/roboticstoolbox/tools/trajectory.py @@ -1,7 +1,8 @@ import math 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,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. @@ -351,6 +357,8 @@ 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_func(q0, qf, tf, qd0, qdf) @@ -364,63 +372,168 @@ 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, + method: Literal["lstsq", "hermite"] = "lstsq", +) -> 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, a finite non-zero value :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 + :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 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. + 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) + + .. 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` """ - - # solve for the polynomial coefficients using least squares + 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 - 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, qdf: float +) -> Callable[[ArrayLike], tuple[ArrayLike, ArrayLike, ArrayLike]]: + """ + Quintic scalar polynomial using a Hermite basis + + 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. + """ + # velocities with respect to normalized time + v0 = qd0 * T + vf = qdf * T + delta = qf - q0 + + def polynomial(x: ArrayLike) -> tuple[ArrayLike, ArrayLike, ArrayLike]: + tau = np.asarray(x, dtype=float) / T + 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) + ) + / T, + 12 + * tau + * remaining + * (5 * delta * (1 - 2 * tau) - v0 * (3 - 5 * tau) - vf * (2 - 5 * tau)) + / T + / T, + ) + + return polynomial # -------------------------------------------------------------------------- # diff --git a/tests/test_quintic_conditioning.py b/tests/test_quintic_conditioning.py new file mode 100644 index 000000000..797f10c96 --- /dev/null +++ b/tests/test_quintic_conditioning.py @@ -0,0 +1,172 @@ +"""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 types import SimpleNamespace + +import numpy as np +import numpy.testing as nt +import pytest +from numpy.polynomial import Polynomial + +from roboticstoolbox import jtraj, mtraj, quintic, quintic_func + +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("method", ["lstsq", "hermite"]) +@pytest.mark.parametrize("duration", DURATIONS) +@pytest.mark.parametrize("moving_endpoints", [False, True]) +def test_quintic_func_matches_polynomial( + method: str, duration: float, moving_endpoints: bool +) -> None: + # 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] + ) + # 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 + ) + for order, values in enumerate(actual): + nt.assert_allclose( + values * duration**order, + polynomial.deriv(order)(SAMPLES), + rtol=1e-9, + atol=1e-9, + ) + + +@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 + ) + + +@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) + + +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("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]) +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)) + nt.assert_equal(not trajectory.istime, True) + 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 + ) + + +@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 * SAMPLES + elapsed = times[-1] - times[0] + polynomial = Polynomial([1, 1, 0, -4, 7, -3]) + trajectory = quintic(1, 2, times, qd0=1 / elapsed, qdf=2 / elapsed) + + nt.assert_array_equal(trajectory.t, times) + nt.assert_equal(trajectory.istime, True) + tau = (times - times[0]) / elapsed + for values, reference, scale in ( + (trajectory.q, polynomial, 1), + (trajectory.qd, polynomial.deriv(), elapsed), + (trajectory.qdd, polynomial.deriv(2), elapsed**2), + ): + 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_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, (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) + + +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_bdsim_quintic_block_long_duration() -> None: + from roboticstoolbox.blocks.arm import Traj + + 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)