diff --git a/README.md b/README.md index 4503844..5e94a65 100644 --- a/README.md +++ b/README.md @@ -52,6 +52,13 @@ is a comparison quantity, not an exact figure. The minimum-jerk cost has no such jerk is continuous: `720 d^2 / T^5 = 720 * 2.25 / 2.34375^5 = 22.9065`, which the test suite checks against the closed form directly. +The caveat applies to the table rather than to the library. `certify_jerk_cost` integrates the +coefficient table segment by segment rather than running a quadrature over a trace, which is exact +for a piecewise polynomial however discontinuous the integrand is, and for this S-curve it returns +239.99999999999994, the analytic 240.0 to fifteen digits, at any sample count, because it reads no +samples at all. The table above keeps the quadrature so that every column of it comes from the same +instrument. + How much of each budget the three profiles actually consume: | Profile | Velocity used | Acceleration used | Jerk used | @@ -96,6 +103,7 @@ The package ships a `py.typed` marker, so it delivers its annotations to anythin from trajectory_optimizer import ( LimitSet, PointToPointMove, + certify_jerk_cost, certify_limits, minimum_jerk_time_optimal, sample_trajectory, @@ -109,12 +117,14 @@ report = certify_limits(trajectory, limits) print(f"duration {trajectory.duration:.5f} s") print(f"peak velocity {trajectory.peak_derivatives().row('velocity')[0]:.5f} rad/s") +print(f"jerk cost {certify_jerk_cost(trajectory):.5f} rad^2/s^5") print(f"within limits everywhere: {report.ok}") ``` ```text duration 2.34375 s peak velocity 1.20000 rad/s +jerk cost 22.90649 rad^2/s^5 within limits everywhere: True ``` @@ -288,6 +298,12 @@ instant, and the time scaling search uses it for its feasibility predicate. That peaks that matter often sit at irrational fractions of the duration: the acceleration peak of a rest-to-rest minimum-jerk move is at `t / T = (1 - 1 / sqrt(3)) / 2`, which no uniform grid contains. +The objective is integrated rather than sampled by the same argument. The square of a segment's jerk +polynomial is a polynomial too, so the integral over that segment is a quadratic form in the jerk +coefficients, and `certify_jerk_cost` sums that form over every segment and every joint. Scaling +divides the result by the fifth power of its factor and synchronisation adds its components, so both +wrappers forward the exact value rather than resampling what they wrap. + Time-optimal time scaling holds the geometric path fixed and searches for the smallest stretch factor whose scaled derivatives all fit inside their bounds. Because a stretch by `s` divides velocity by `s`, acceleration by `s` squared, and jerk by `s` cubed, feasibility is monotone in `s`, @@ -302,8 +318,8 @@ distinction, the alternatives that were rejected, and the limitations that remai | Module | Responsibility | | --- | --- | | `src/trajectory_optimizer/model.py` | Pure dataclasses for joint limits, via points, boundary conditions, and moves, with validation and no input or output | -| `src/trajectory_optimizer/algorithm/protocol.py` | The `Trajectory` protocol every generator satisfies, the `CertifiedTrajectory` protocol for those that solve for their own peaks, and the single-instant state record | -| `src/trajectory_optimizer/algorithm/piecewise.py` | Piecewise polynomial container giving exact derivatives and exact peaks, plus the mapping of a scalar path profile onto a joint-space line | +| `src/trajectory_optimizer/algorithm/protocol.py` | The `Trajectory` protocol every generator satisfies, the `CertifiedTrajectory` and `IntegrableTrajectory` protocols for those that solve for their own peaks and integrate their own jerk cost, and the single-instant state record | +| `src/trajectory_optimizer/algorithm/piecewise.py` | Piecewise polynomial container giving exact derivatives, exact peaks, and the exact jerk cost integral, plus the mapping of a scalar path profile onto a joint-space line | | `src/trajectory_optimizer/algorithm/minimum_jerk.py` | Closed-form quintic minimum-jerk solution and its analytic time-optimal duration | | `src/trajectory_optimizer/algorithm/trapezoidal.py` | Trapezoidal velocity profile including the triangular degenerate case | | `src/trajectory_optimizer/algorithm/scurve.py` | Seven segment jerk limited profile including both degenerate cases | @@ -312,7 +328,7 @@ distinction, the alternatives that were rejected, and the limitations that remai | `src/trajectory_optimizer/algorithm/synchronisation.py` | Stretching independent joint trajectories onto one common duration | | `src/trajectory_optimizer/pipeline.py` | Sampling any trajectory into a structured trace of times and derivatives | | `src/trajectory_optimizer/analysis/limits.py` | The sampled limit check, the certified one, and peak usage per joint | -| `src/trajectory_optimizer/analysis/metrics.py` | Duration, peak derivatives, jerk cost, and joint-space path length | +| `src/trajectory_optimizer/analysis/metrics.py` | Duration, peak derivatives, joint-space path length, and the jerk cost both as a quadrature of a trace and as an exact integral of the coefficient table | | `src/trajectory_optimizer/analysis/report.py` | Markdown rendering of comparison tables, trade-offs, limit reports, and scaling results | | `src/trajectory_optimizer/analysis/figures.py` | Stacked derivative figures and profile overlays, drawn without a pyplot backend | | `examples/` | Wiring scripts only, with no logic of their own | @@ -331,8 +347,8 @@ uv run ruff format --check . uv run mypy ``` -The suite is 203 tests over three tiers and completes in about 10 seconds. Coverage of -`src/trajectory_optimizer` is 97 percent of 1204 statements; CI runs the same command with +The suite is 218 tests over three tiers and completes in about 10 seconds. Coverage of +`src/trajectory_optimizer` is 97 percent of 1230 statements; CI runs the same command with `--cov-fail-under=95` on Ubuntu and on Windows, along with the linter and mypy in strict mode over the package, the examples, and the figure script. @@ -346,6 +362,9 @@ point, and synchronisation gives every joint identical start and end times. The checked against `scipy.interpolate.CubicSpline` and the bisection scale factor against the analytic one. The closed-form peaks are checked against their analytic expressions, against a two million point grid, and against a constructed move whose violation a three point check calls compliant. The +integrated jerk cost is checked against a hand-computed integral, against `720 d^2 / T^5` for the +minimum-jerk quintic and `400 * 4 * 0.15` for the S-curve, and against the quadrature it replaces at +4001, 40001, and 400001 samples, where the error falls by a factor of ten at each step. The second tier pins recorded profiles at eleven sampled fractions of each duration to an absolute tolerance of 1e-9. The third tier runs every script in `examples/` as a subprocess with `--samples 51`. diff --git a/docs/design-notes.md b/docs/design-notes.md index 74c291b..485d596 100644 --- a/docs/design-notes.md +++ b/docs/design-notes.md @@ -149,6 +149,30 @@ synchronisation concatenates the blocks of its components, which is exact becaus owns a disjoint set of joints. Both raise `TypeError` when what they wrap cannot answer, so a certificate over an estimate is never produced. +### The jerk cost integrated rather than sampled + +The same argument settles the objective itself. On one segment the jerk of one joint is a polynomial +in the local time, so its square is a polynomial of twice the degree and the integral over that +segment is a quadratic form in the jerk coefficients `c` of that segment and joint, + +```text +c^T M c, M_ik = L^(i + k + 1) / (i + k + 1), +``` + +with `L` the length of the segment. `PiecewisePolynomialTrajectory.jerk_cost` sums that form over +every segment and every joint and reads no samples at all, so the value carries no quadrature error +however discontinuous the integrand is. `certify_jerk_cost` is the analysis-layer entry point. + +The two wrappers forward rather than repeat, as they do for the peaks. `TimeScaledTrajectory` +divides by the fifth power of its scale, because stretching time divides the jerk by the cube of the +scale and multiplies the interval of integration by it, and `SynchronisedTrajectory` adds its +components, because the cost is a sum over joints and each component owns a disjoint set of them. +Both raise `TypeError` when what they wrap cannot answer. + +An acceleration that steps at a breakpoint carries an impulse of jerk that the segment polynomials +do not contain, so the cost reported for a trapezoidal profile is zero. That is the same one-sided +reading of a piecewise definition as its reported peak jerk of zero, and it is not a bound either. + ### Time-optimal time scaling by bisection Given a trajectory `q` on `[0, T]`, the scaled trajectory `q_s(t) = q(t / s)` traverses the same @@ -260,6 +284,36 @@ that is not a piecewise polynomial cannot be certified by this method; its repor Third, the certificate covers the kinematic limits only, which is the boundary set out under "No dynamics, no actuator model" below. +### The jerk cost was a quadrature over a sampled trace + +This entry recorded that the jerk cost was a Simpson quadrature of the sampled squared jerk, which +converges to machine precision only where the jerk is continuous. For the minimum-jerk profile the +analytic cost of a 1.5 rad move over 2.0 s is 50.625 and the quadrature returned 50.625000000000014 +at 20001 samples. For the S-curve, whose jerk is piecewise constant and discontinuous, the error +fell only as the reciprocal of the sample count: against an analytic 240.0 the reported values were +240.360000, 240.036000, and 240.003600 at 4001, 40001, and 400001 samples, so the figure was a +comparison quantity rather than an exact one. The entry named its own remedy: integrate the +coefficient table segment by segment. + +That is now implemented and the entry is closed. `PiecewisePolynomialTrajectory.jerk_cost` +integrates each segment exactly, `certify_jerk_cost` is the entry point in the analysis layer, and +both wrappers forward. The method is described under "The jerk cost integrated rather than sampled" +above. + +What it cost. Twenty six executable statements across five modules and one new protocol, +`IntegrableTrajectory`. No new dependency and no new record, because the result is a float. The +S-curve above now reports 239.99999999999994 at any sample count, which is the analytic 240.0 to +fifteen digits, and the time-optimal minimum-jerk move reports 22.906492245333652 against a closed +form of 22.906492245333332. Runtime is one quadratic form of order at most three per segment and +joint, so it is cheaper than sampling the trace it replaces. + +What remains. Two things. First, `compute_metrics` still reads a trace and its jerk cost is still a +quadrature, because a trajectory that is not a piecewise polynomial cannot be integrated this way; +its docstring now says so. Second, the path length in the same record is still the polyline through +the samples, which under-estimates a curved joint-space path by an amount falling as the square of +the sample spacing. Point to point moves are straight lines in joint space, where the polyline is +exact, so that error is confined to the spline paths. + ## Known limitations ### Time scaling along a fixed path is not global time optimality @@ -308,17 +362,6 @@ segment case analysis does extend to nonzero end velocities, at the cost of seve including the case where the profile must first decelerate, and that extension is not implemented. Moves with nonzero end derivatives should use the minimum-jerk generator or a clamped spline. -### Metrics are quadratures over sampled traces - -The jerk cost is a Simpson quadrature of the sampled squared jerk. For the minimum-jerk profile, -where jerk is continuous, this converges to machine precision: the analytic cost of a 1.5 rad move -over 2.0 s is 50.625 and the quadrature returns 50.625000000000014 at 20001 samples. For the -S-curve, where jerk is piecewise constant and discontinuous, the quadrature error falls only as the -reciprocal of the sample count: the analytic cost is 240.0 and the reported values are 240.360000, -240.036000, and 240.003600 at 4001, 40001, and 400001 samples. Reading the S-curve jerk cost as an -exact figure would be a mistake; it is a comparison quantity. Integrating the coefficient table -segment by segment would make it exact and is the obvious improvement. - ### No dynamics, no actuator model Everything here is kinematic. Torque limits, payload-dependent inertia, friction, and joint diff --git a/src/trajectory_optimizer/__init__.py b/src/trajectory_optimizer/__init__.py index bb9c495..0e965d6 100644 --- a/src/trajectory_optimizer/__init__.py +++ b/src/trajectory_optimizer/__init__.py @@ -6,6 +6,7 @@ from trajectory_optimizer.algorithm import ( CertifiedTrajectory, + IntegrableTrajectory, PeakDerivatives, PiecewisePolynomialTrajectory, SynchronisedTrajectory, @@ -21,7 +22,12 @@ time_optimal_scaling, trapezoidal_move, ) -from trajectory_optimizer.analysis import certify_limits, check_limits, compute_metrics +from trajectory_optimizer.analysis import ( + certify_jerk_cost, + certify_limits, + check_limits, + compute_metrics, +) from trajectory_optimizer.model import ( BoundaryConditions, BoundaryKind, @@ -37,6 +43,7 @@ "BoundaryConditions", "BoundaryKind", "CertifiedTrajectory", + "IntegrableTrajectory", "JointLimits", "LimitSet", "PeakDerivatives", @@ -49,6 +56,7 @@ "ViaPoints", "Waypoint", "__version__", + "certify_jerk_cost", "certify_limits", "check_limits", "compute_metrics", diff --git a/src/trajectory_optimizer/algorithm/__init__.py b/src/trajectory_optimizer/algorithm/__init__.py index 3b437e5..f6a7d3c 100644 --- a/src/trajectory_optimizer/algorithm/__init__.py +++ b/src/trajectory_optimizer/algorithm/__init__.py @@ -20,6 +20,7 @@ from trajectory_optimizer.algorithm.protocol import ( DERIVATIVE_NAMES, CertifiedTrajectory, + IntegrableTrajectory, PeakDerivatives, Trajectory, TrajectoryState, @@ -50,6 +51,7 @@ __all__ = [ "DERIVATIVE_NAMES", "CertifiedTrajectory", + "IntegrableTrajectory", "LimitActivation", "PeakDerivatives", "PiecewisePolynomialTrajectory", diff --git a/src/trajectory_optimizer/algorithm/piecewise.py b/src/trajectory_optimizer/algorithm/piecewise.py index e8f7628..934b3ca 100644 --- a/src/trajectory_optimizer/algorithm/piecewise.py +++ b/src/trajectory_optimizer/algorithm/piecewise.py @@ -230,6 +230,35 @@ def peak_derivatives(self) -> PeakDerivatives: times[row, joint] = origin + float(candidates[best]) return PeakDerivatives(values=values, times=times) + def jerk_cost(self) -> float: + """Integrated squared jerk summed over joints, solved in closed form. + + On one segment the jerk of one joint is a polynomial in the local time, + so its square is a polynomial too and the integral over that segment is + the quadratic form ``c @ M @ c`` in the jerk coefficients ``c``, with + + ``M[i, k] = span ** (i + k + 1) / (i + k + 1)``. + + Summing that over every segment and every joint gives the objective the + minimum-jerk generator minimises, with no quadrature error: the value is + exact rather than converging as a sample count grows. That matters + because the integrand of a jerk limited profile is piecewise constant + and discontinuous, and Simpson's rule then converges only as the + reciprocal of the sample count. + + A profile whose acceleration steps at a breakpoint, the trapezoidal one + for instance, carries an impulse of jerk there that no integral of the + segment polynomials can see, so the cost reported for it is zero. That + is the same one-sided reading of a piecewise definition that + :meth:`peak_derivatives` gives, and it is not a bound either. + """ + table = self._derivatives[3] + orders = np.arange(table.shape[2], dtype=np.float64) + exponents = orders[:, None] + orders[None, :] + 1.0 + spans = np.diff(self._breakpoints)[:, None, None] + weights = spans**exponents / exponents + return float(np.einsum("sji,sik,sjk->", table, weights, table)) + def time_scaled(self, factor: float) -> PiecewisePolynomialTrajectory: """Return the same geometric path traversed ``factor`` times more slowly. diff --git a/src/trajectory_optimizer/algorithm/protocol.py b/src/trajectory_optimizer/algorithm/protocol.py index 42245cd..6a6c236 100644 --- a/src/trajectory_optimizer/algorithm/protocol.py +++ b/src/trajectory_optimizer/algorithm/protocol.py @@ -16,6 +16,7 @@ __all__ = [ "DERIVATIVE_NAMES", "CertifiedTrajectory", + "IntegrableTrajectory", "PeakDerivatives", "Trajectory", "TrajectoryState", @@ -111,6 +112,23 @@ def peak_derivatives(self) -> PeakDerivatives: ... +@runtime_checkable +class IntegrableTrajectory(Protocol): + """A trajectory that integrates its own squared jerk in closed form. + + A quadrature over a sampled trace converges to the jerk cost only as fast as + the integrand is smooth, and the integrand of a jerk limited profile is + discontinuous. A trajectory that implements this protocol integrates its own + coefficient table instead, which carries no quadrature error at all. + Wrappers raise :class:`TypeError` when the trajectory they wrap cannot do + the same. + """ + + def jerk_cost(self) -> float: + """Exact integral of the squared jerk, summed over joints.""" + ... + + @dataclass(frozen=True, slots=True, eq=False) class TrajectoryState: """Position and its first three derivatives at a single instant.""" diff --git a/src/trajectory_optimizer/algorithm/scaling.py b/src/trajectory_optimizer/algorithm/scaling.py index 1574ace..4ec4742 100644 --- a/src/trajectory_optimizer/algorithm/scaling.py +++ b/src/trajectory_optimizer/algorithm/scaling.py @@ -20,6 +20,7 @@ from trajectory_optimizer.algorithm.protocol import ( DERIVATIVE_NAMES, CertifiedTrajectory, + IntegrableTrajectory, PeakDerivatives, Trajectory, ) @@ -111,6 +112,20 @@ def peak_derivatives(self) -> PeakDerivatives: times=peaks.times * self._scale, ) + def jerk_cost(self) -> float: + """Exact jerk cost of the base, divided by the fifth power of the scale. + + Stretching time divides the jerk by ``scale ** 3`` and multiplies the + interval it is integrated over by ``scale``, so the integral of the + squared jerk is divided by ``scale ** 5``. Raises :class:`TypeError` + when the base cannot integrate its own squared jerk in closed form. + """ + if not isinstance(self._base, IntegrableTrajectory): + raise TypeError( + f"{type(self._base).__name__} cannot integrate its squared jerk in closed form" + ) + return self._base.jerk_cost() / self._scale**5 + @dataclass(frozen=True, slots=True) class LimitActivation: diff --git a/src/trajectory_optimizer/algorithm/synchronisation.py b/src/trajectory_optimizer/algorithm/synchronisation.py index 24cbe65..921c915 100644 --- a/src/trajectory_optimizer/algorithm/synchronisation.py +++ b/src/trajectory_optimizer/algorithm/synchronisation.py @@ -17,6 +17,7 @@ from trajectory_optimizer.algorithm.piecewise import constant_trajectory from trajectory_optimizer.algorithm.protocol import ( CertifiedTrajectory, + IntegrableTrajectory, PeakDerivatives, Trajectory, ) @@ -98,6 +99,24 @@ def peak_derivatives(self) -> PeakDerivatives: times=np.concatenate([block.times for block in blocks], axis=1), ) + def jerk_cost(self) -> float: + """Exact jerk cost of every component, added together. + + The cost is a sum over joints and each component owns a disjoint block + of them, so adding the component costs is exact. Raises + :class:`TypeError` when any component cannot integrate its own squared + jerk in closed form. + """ + total = 0.0 + for index, component in enumerate(self._components): + if not isinstance(component, IntegrableTrajectory): + raise TypeError( + f"component {index} of type {type(component).__name__} cannot integrate " + "its squared jerk in closed form" + ) + total += component.jerk_cost() + return total + def synchronise(components: Sequence[Trajectory]) -> SynchronisedTrajectory: """Stretch every component onto the duration of the slowest one. diff --git a/src/trajectory_optimizer/analysis/__init__.py b/src/trajectory_optimizer/analysis/__init__.py index d635a2b..1f258d0 100644 --- a/src/trajectory_optimizer/analysis/__init__.py +++ b/src/trajectory_optimizer/analysis/__init__.py @@ -12,7 +12,11 @@ certify_limits, check_limits, ) -from trajectory_optimizer.analysis.metrics import TrajectoryMetrics, compute_metrics +from trajectory_optimizer.analysis.metrics import ( + TrajectoryMetrics, + certify_jerk_cost, + compute_metrics, +) from trajectory_optimizer.analysis.report import ( ProfileSummary, format_comparison_table, @@ -29,6 +33,7 @@ "LimitViolation", "ProfileSummary", "TrajectoryMetrics", + "certify_jerk_cost", "certify_limits", "check_limits", "comparison_figure", diff --git a/src/trajectory_optimizer/analysis/metrics.py b/src/trajectory_optimizer/analysis/metrics.py index 8650425..a5a19ec 100644 --- a/src/trajectory_optimizer/analysis/metrics.py +++ b/src/trajectory_optimizer/analysis/metrics.py @@ -1,4 +1,13 @@ -"""Cost metrics computed from a sampled trace.""" +"""Cost metrics, either over a sampled trace or from the coefficient table. + +Two routes to the jerk cost live here and they answer different questions. +:func:`compute_metrics` reads a +:class:`~trajectory_optimizer.pipeline.TrajectoryTrace`, so it works for +anything that can be sampled, and its jerk cost is a quadrature of what the grid +saw. :func:`certify_jerk_cost` reads the trajectory itself and asks it to +integrate its own squared jerk segment by segment, which is exact for a +piecewise polynomial however discontinuous the integrand is. +""" from __future__ import annotations @@ -7,9 +16,10 @@ import numpy as np from scipy.integrate import simpson +from trajectory_optimizer.algorithm.protocol import IntegrableTrajectory, Trajectory from trajectory_optimizer.pipeline import TrajectoryTrace -__all__ = ["TrajectoryMetrics", "compute_metrics"] +__all__ = ["TrajectoryMetrics", "certify_jerk_cost", "compute_metrics"] @dataclass(frozen=True, slots=True) @@ -46,6 +56,10 @@ def compute_metrics(trace: TrajectoryTrace) -> TrajectoryMetrics: is the objective the minimum-jerk generator minimises. It is evaluated with Simpson's rule when the sample count allows it. The path length is the accumulated Euclidean distance between successive joint-space samples. + + The result describes the grid, not the trajectory: the quadrature converges + to the true cost only as fast as the integrand is smooth. Use + :func:`certify_jerk_cost` when the trajectory can integrate its own jerk. """ if trace.sample_count == 1: return TrajectoryMetrics( @@ -67,3 +81,24 @@ def compute_metrics(trace: TrajectoryTrace) -> TrajectoryMetrics: integrated_squared_jerk=cost, path_length=float(np.sum(steps)), ) + + +def certify_jerk_cost(trajectory: Trajectory) -> float: + """Exact integrated squared jerk of ``trajectory``, summed over joints. + + The trajectory must be able to integrate its own coefficient table, which + every piecewise polynomial can and which the scaling and synchronisation + wrappers forward. The returned value carries no quadrature error, so it can + be compared against an analytic cost digit for digit rather than to the + accuracy of a grid. + + An acceleration that steps at a breakpoint is an impulse of jerk that no + integral of the segment polynomials sees, so the cost of a trapezoidal + profile is zero here for the same reason its reported peak jerk is. + """ + if not isinstance(trajectory, IntegrableTrajectory): + raise TypeError( + f"{type(trajectory).__name__} cannot integrate its squared jerk in closed form; " + "sample it and call compute_metrics instead" + ) + return trajectory.jerk_cost() diff --git a/tests/conftest.py b/tests/conftest.py index 7e8c3f1..76d0a38 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -6,7 +6,7 @@ import numpy as np import pytest -from numpy.typing import NDArray +from numpy.typing import ArrayLike, NDArray from trajectory_optimizer.algorithm import Trajectory from trajectory_optimizer.model import LimitSet, PointToPointMove, ViaPoints @@ -37,6 +37,39 @@ def via() -> ViaPoints: ) +class Opaque: + """A trajectory that forwards everything but solves for nothing itself. + + Nothing in the package produces one of these. It exists so that the fallback + paths, the ones every trajectory took before the peaks were solved and the + jerk cost integrated in closed form, stay exercised and stay honest about + what they measured. + """ + + def __init__(self, base: Trajectory) -> None: + self._base = base + + @property + def duration(self) -> float: + return self._base.duration + + @property + def dof(self) -> int: + return self._base.dof + + def position(self, times: ArrayLike) -> NDArray[np.float64]: + return self._base.position(times) + + def velocity(self, times: ArrayLike) -> NDArray[np.float64]: + return self._base.velocity(times) + + def acceleration(self, times: ArrayLike) -> NDArray[np.float64]: + return self._base.acceleration(times) + + def jerk(self, times: ArrayLike) -> NDArray[np.float64]: + return self._base.jerk(times) + + def central_difference( function: Callable[[NDArray[np.float64]], NDArray[np.float64]], times: NDArray[np.float64], diff --git a/tests/test_certification.py b/tests/test_certification.py index 6177481..1954df8 100644 --- a/tests/test_certification.py +++ b/tests/test_certification.py @@ -11,8 +11,9 @@ import numpy as np import pytest -from numpy.typing import ArrayLike, NDArray +from numpy.typing import NDArray +from tests.conftest import Opaque from trajectory_optimizer.algorithm import ( CertifiedTrajectory, PeakDerivatives, @@ -36,38 +37,6 @@ DENSE = 2_000_001 -class Opaque: - """A trajectory that forwards everything but refuses to solve for its peaks. - - Nothing in the package produces one of these. It exists so that the fallback - path, the one every trajectory took before the peaks were solved in closed - form, stays exercised and stays honest about what it measured. - """ - - def __init__(self, base: Trajectory) -> None: - self._base = base - - @property - def duration(self) -> float: - return self._base.duration - - @property - def dof(self) -> int: - return self._base.dof - - def position(self, times: ArrayLike) -> NDArray[np.float64]: - return self._base.position(times) - - def velocity(self, times: ArrayLike) -> NDArray[np.float64]: - return self._base.velocity(times) - - def acceleration(self, times: ArrayLike) -> NDArray[np.float64]: - return self._base.acceleration(times) - - def jerk(self, times: ArrayLike) -> NDArray[np.float64]: - return self._base.jerk(times) - - def dense_peaks(trajectory: Trajectory, samples: int = DENSE) -> NDArray[np.float64]: """Peak absolute derivatives read off a very dense uniform grid.""" trace = sample_trajectory(trajectory, samples=samples) diff --git a/tests/test_jerk_cost.py b/tests/test_jerk_cost.py new file mode 100644 index 0000000..28a8443 --- /dev/null +++ b/tests/test_jerk_cost.py @@ -0,0 +1,176 @@ +"""Tier one: the jerk cost integrated in closed form rather than quadratured. + +The objective the minimum-jerk generator minimises was previously available only +as a Simpson quadrature of a sampled trace, whose error falls as the reciprocal +of the sample count when the jerk is discontinuous. These tests pin the +integrated value against a hand-computed integral, against the analytic cost of +the minimum-jerk quintic and of the seven segment S-curve, against the +quadrature it replaces at three sample counts, and against the variational claim +that the natural quintic spline is the minimum-jerk interpolant of its via +points. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from tests.conftest import Opaque +from trajectory_optimizer.algorithm import ( + IntegrableTrajectory, + PiecewisePolynomialTrajectory, + Trajectory, + constant_trajectory, + cubic_spline, + minimum_jerk_move, + quintic_spline, + s_curve_move, + scale_to_duration, + synchronise, + trapezoidal_move, +) +from trajectory_optimizer.algorithm.scaling import TimeScaledTrajectory +from trajectory_optimizer.algorithm.synchronisation import SynchronisedTrajectory +from trajectory_optimizer.analysis import certify_jerk_cost, compute_metrics +from trajectory_optimizer.model import BoundaryConditions, JointLimits, LimitSet, ViaPoints +from trajectory_optimizer.model import PointToPointMove as Move +from trajectory_optimizer.pipeline import sample_trajectory + + +def quadrature(trajectory: Trajectory, samples: int) -> float: + """The sampled jerk cost, for comparison against the integrated one.""" + return compute_metrics(sample_trajectory(trajectory, samples=samples)).integrated_squared_jerk + + +def test_the_integral_of_a_hand_written_polynomial() -> None: + """One joint with position ``t^3 + t^4 + t^5`` has jerk ``6 + 24 t + 60 t^2``. + + The square integrates to ``36 + 144 + 432 + 720 + 720`` over a unit segment, + so two of them cost twice that, and every cross term of the quadratic form + is exercised on the way. + """ + coefficients = np.tile(np.array([0.0, 0.0, 0.0, 1.0, 1.0, 1.0]), (2, 1, 1)) + trajectory = PiecewisePolynomialTrajectory.from_segments((1.0, 1.0), coefficients) + assert trajectory.jerk_cost() == pytest.approx(2.0 * 2052.0, rel=1e-12) + + +def test_the_cost_matches_the_minimum_jerk_closed_form(move: Move) -> None: + """The rest-to-rest quintic costs ``720 d^2 / T^5`` exactly.""" + duration = 2.0 + distance = abs(move.displacement[0]) + trajectory = minimum_jerk_move(move, duration) + expected = 720.0 * distance**2 / duration**5 + assert certify_jerk_cost(trajectory) == pytest.approx(expected, rel=1e-12) + + +def test_the_cost_of_an_s_curve_is_its_analytic_value(move: Move, limits: LimitSet) -> None: + """Four jerk phases of 0.15 s at 20 rad/s cubed cost ``400 * 4 * 0.15``.""" + assert certify_jerk_cost(s_curve_move(move, limits)) == pytest.approx(240.0, rel=1e-12) + + +def test_the_quadrature_only_approaches_what_the_integral_gives( + move: Move, + limits: LimitSet, +) -> None: + """The S-curve integrand is discontinuous, so Simpson converges as 1 / N.""" + trajectory = s_curve_move(move, limits) + exact = certify_jerk_cost(trajectory) + errors = [abs(quadrature(trajectory, samples) - exact) for samples in (4001, 40001, 400001)] + assert errors[0] == pytest.approx(0.36, rel=1e-3) + assert errors[1] == pytest.approx(0.1 * errors[0], rel=1e-3) + assert errors[2] == pytest.approx(0.1 * errors[1], rel=1e-3) + + +def test_a_continuous_integrand_agrees_with_its_quadrature(move: Move) -> None: + """Where the jerk is continuous the quadrature was already adequate.""" + trajectory = minimum_jerk_move(move, 2.0) + assert quadrature(trajectory, 20001) == pytest.approx(certify_jerk_cost(trajectory), rel=1e-12) + + +def test_the_trapezoidal_cost_is_the_value_between_its_impulses( + move: Move, + limits: LimitSet, +) -> None: + """Its jerk is zero on every segment and an impulse at the two phase changes.""" + assert certify_jerk_cost(trapezoidal_move(move, limits)) == 0.0 + + +def test_a_zero_duration_trajectory_costs_nothing() -> None: + assert certify_jerk_cost(constant_trajectory((0.4, -0.2))) == 0.0 + + +def test_the_cost_sums_over_joints(limits: LimitSet) -> None: + pair = s_curve_move( + Move.rest_to_rest((0.0, 0.0), (1.5, -1.5)), + LimitSet.uniform(2, 1.2, 3.0, 20.0), + ) + single = s_curve_move(Move.rest_to_rest((0.0,), (1.5,)), limits) + assert certify_jerk_cost(pair) == pytest.approx(2.0 * certify_jerk_cost(single), rel=1e-12) + + +def test_the_natural_quintic_spline_has_the_lowest_cost_of_the_four(via: ViaPoints) -> None: + """Its end conditions are the variational ones of the minimum-jerk interpolant.""" + natural = quintic_spline(via, BoundaryConditions.natural()) + costs = { + "cubic natural": certify_jerk_cost(cubic_spline(via, BoundaryConditions.natural())), + "cubic clamped": certify_jerk_cost(cubic_spline(via, BoundaryConditions.clamped())), + "quintic natural": certify_jerk_cost(natural), + "quintic clamped": certify_jerk_cost(quintic_spline(via, BoundaryConditions.clamped())), + } + assert min(costs, key=lambda name: costs[name]) == "quintic natural" + assert costs["quintic natural"] == pytest.approx(quadrature(natural, 200001), rel=1e-6) + + +def test_stretching_time_divides_the_cost_by_the_fifth_power( + move: Move, + limits: LimitSet, +) -> None: + """Jerk is divided by the cube of the scale and the interval multiplied by it.""" + base = s_curve_move(move, limits) + factor = 2.5 + scaled = TimeScaledTrajectory(base, factor) + expected = certify_jerk_cost(base) / factor**5 + assert certify_jerk_cost(scaled) == pytest.approx(expected, rel=1e-12) + assert certify_jerk_cost(scaled) == pytest.approx(quadrature(scaled, 400001), rel=1e-3) + + +def test_the_minimum_jerk_profile_costs_less_than_a_retimed_s_curve( + move: Move, + limits: LimitSet, +) -> None: + duration = 2.5 + retimed = scale_to_duration(s_curve_move(move, limits), duration) + assert certify_jerk_cost(minimum_jerk_move(move, duration)) < certify_jerk_cost(retimed) + + +def test_synchronisation_adds_the_cost_of_its_components() -> None: + joint_limits = (JointLimits(1.2, 3.0, 20.0), JointLimits(1.0, 2.5, 18.0)) + components = [ + s_curve_move(Move.rest_to_rest((0.0,), (displacement,)), LimitSet.of(limit)) + for displacement, limit in zip((1.5, -0.8), joint_limits, strict=True) + ] + combined = synchronise(components) + blocks = sum(certify_jerk_cost(component) for component in combined.components) + assert certify_jerk_cost(combined) == pytest.approx(blocks, rel=1e-12) + assert certify_jerk_cost(combined) < sum(certify_jerk_cost(item) for item in components) + + +def test_certify_jerk_cost_refuses_a_trajectory_that_cannot_integrate_its_own_jerk( + move: Move, + limits: LimitSet, +) -> None: + with pytest.raises(TypeError, match="closed form"): + certify_jerk_cost(Opaque(s_curve_move(move, limits))) + + +def test_a_scaled_wrapper_refuses_when_its_base_refuses(move: Move, limits: LimitSet) -> None: + scaled = TimeScaledTrajectory(Opaque(s_curve_move(move, limits)), 2.0) + assert isinstance(scaled, IntegrableTrajectory) + with pytest.raises(TypeError, match="closed form"): + scaled.jerk_cost() + + +def test_synchronisation_refuses_when_a_component_refuses(move: Move, limits: LimitSet) -> None: + direct = SynchronisedTrajectory([Opaque(s_curve_move(move, limits))]) + with pytest.raises(TypeError, match="component 0"): + direct.jerk_cost()