From d00a2d031f56381bdf7605c4d071be075abce4df Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:10:08 +0000 Subject: [PATCH 1/7] feat(math): add angle utilities (wrap, shortest difference, unwrap) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 54. WrapToPi maps into (−π, π] (both ±π → +π), WrapToTwoPi into [0, 2π) with a guard so rounding never returns 2π, AngleDifference takes the short way round and UnwrapAngle turns a wrapped measurement into a continuous signal. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 10 --- doc/math/AngleUtilities.md | 87 +++++++++++++++++++ doc/math/README.md | 1 + numerical/math/AngleUtilities.cpp | 9 ++ numerical/math/AngleUtilities.hpp | 61 +++++++++++++ numerical/math/CMakeLists.txt | 2 + numerical/math/test/CMakeLists.txt | 1 + numerical/math/test/TestAngleUtilities.cpp | 59 +++++++++++++ roadmap/README.md | 2 +- roadmap/math/AngleUtilities/explanation.md | 31 ------- roadmap/math/AngleUtilities/implementation.md | 74 ---------------- roadmap/math/AngleUtilities/tests.md | 39 --------- 13 files changed, 222 insertions(+), 156 deletions(-) create mode 100644 doc/math/AngleUtilities.md create mode 100644 numerical/math/AngleUtilities.cpp create mode 100644 numerical/math/AngleUtilities.hpp create mode 100644 numerical/math/test/TestAngleUtilities.cpp delete mode 100644 roadmap/math/AngleUtilities/explanation.md delete mode 100644 roadmap/math/AngleUtilities/implementation.md delete mode 100644 roadmap/math/AngleUtilities/tests.md diff --git a/README.md b/README.md index 06bc7a39..b37a16e1 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | diff --git a/ROADMAP.md b/ROADMAP.md index 9ef35bf3..84d160ba 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | ★☆☆☆☆ | | 55 | Polynomial (cubic/quintic) point-to-point profile | `motion_profiles` (new) | ★☆☆☆☆ | | 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | | 57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | ★☆☆☆☆ | @@ -60,15 +59,6 @@ the library does not yet expose. Detailed below under ## Tier 1 — Trivial primitives ★☆☆☆☆ -### 54. Angle utilities -- **What:** `WrapToPi` `(−π, π]`, `WrapToTwoPi` `[0, 2π)`, shortest signed `AngleDifference`, and - `UnwrapAngle` for continuous signals. -- **Embedded value:** One convention for every angle consumer — heading controllers, odometry, encoder - unwrapping, analytical IK; replaces `ComplementaryFilter`'s private wrap. -- **Algorithm / paper:** range reduction with `fmod`; D. Goldberg, *ACM Computing Surveys*, 23(1), 1991. -- **Reuses:** `math::Fmod`. -- **Spec:** [roadmap/math/AngleUtilities](roadmap/math/AngleUtilities/implementation.md). - ### 55. Polynomial point-to-point profile - **What:** Cubic (position + velocity) or quintic (+ acceleration) time law between two states over a fixed duration; defines the shared `TrajectoryState` / `MotionLimits` types of `motion_profiles`. diff --git a/doc/math/AngleUtilities.md b/doc/math/AngleUtilities.md new file mode 100644 index 00000000..1d683f3e --- /dev/null +++ b/doc/math/AngleUtilities.md @@ -0,0 +1,87 @@ +# Angle Utilities + +## Overview & Motivation + +Angles live on a circle, not a line: a heading of $179^\circ$ and one of $-179^\circ$ are $2^\circ$ apart, not $358^\circ$. Controllers that subtract angles naively command a full turn the wrong way, integrators that never wrap lose precision as the value grows, and encoders or IMUs report wrapped values that an estimator must turn back into a continuous signal. These bugs recur in every module that touches angles, so the library provides one stateless, tested set of helpers with one convention: wrap into a canonical interval, take the shortest signed difference, and unwrap a wrapped measurement. + +## Mathematical Theory + +### Wrapping + +Two angles are equivalent when they differ by a multiple of $2\pi$. Wrapping picks the representative inside a fixed half-open interval of length $2\pi$: + +$$\operatorname{wrap}_{(-\pi,\pi]}(a) = r - \pi,\;\; r \in (0, 2\pi],\; r \equiv a + \pi \pmod{2\pi} \qquad \operatorname{wrap}_{[0,2\pi)}(a) = a \bmod 2\pi$$ + +The floating-point remainder (fmod) is *truncated* — it keeps the sign of the dividend. The implementation therefore shifts the angle so the interval starts at zero, takes the remainder $r \in (-2\pi, 2\pi)$, adds $2\pi$ if $r$ is not positive (signed case) or is negative (unsigned case), and shifts back. + +**Convention (normative):** the signed interval is $(-\pi, \pi]$; both $+\pi$ and $-\pi$ map to $+\pi$, so a half turn has exactly one representation. The unsigned interval is $[0, 2\pi)$ and never returns $2\pi$. + +### Shortest signed difference + +For target $\theta_t$ and current $\theta_c$: + +$$\Delta(\theta_t, \theta_c) = \operatorname{wrap}_{(-\pi,\pi]}(\theta_t - \theta_c)$$ + +Its magnitude is the shortest arc and its sign is the direction of travel from $\theta_c$ to $\theta_t$. + +### Unwrapping + +Given the previous continuous value $u_{k-1}$ and a new wrapped sample $w_k$: + +$$u_k = u_{k-1} + \Delta(w_k, u_{k-1})$$ + +This recovers the true continuous angle as long as consecutive true samples differ by less than $\pi$; otherwise the step is aliased by a multiple of $2\pi$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------------|--------|--------|----------------------------------------------| +| Wrap to $(-\pi,\pi]$ | $O(1)$ | $O(1)$ | One fmod, one compare, a few adds | +| Wrap to $[0,2\pi)$ | $O(1)$ | $O(1)$ | One fmod, two compares, one conditional add | +| Shortest difference | $O(1)$ | $O(1)$ | One subtraction plus one signed wrap | +| Unwrap | $O(1)$ | $O(1)$ | One shortest difference plus one add; no state | + +Cost does not depend on the magnitude of the input: the remainder is a single operation, not a loop subtracting $2\pi$. + +## Step-by-Step Walkthrough + +**Shortest difference, target $170^\circ$, current $-170^\circ$:** plain difference $= 340^\circ$. Shift: $340 + 180 = 520$. Remainder mod $360$: $160$ (positive, no correction). Shift back: $160 - 180 = -20^\circ$. The short way is $20^\circ$ in the negative direction, not $340^\circ$ forward. + +**Wrap, $7\pi/3$:** shift: $7\pi/3 + \pi = 10\pi/3$. Remainder mod $2\pi$: $10\pi/3 - 2\pi = 4\pi/3$. Shift back: $4\pi/3 - \pi = \pi/3$. The unsigned wrap gives the same $\pi/3$. + +**Half turn:** $a = -\pi$ shifts to $0$; the remainder is $0$, which is not positive, so $2\pi$ is added and the result is $2\pi - \pi = +\pi$. + +**Unwrap, previous $358^\circ$, new sample $2^\circ$:** $\Delta = \operatorname{wrap}(2 - 358) = \operatorname{wrap}(-356) = +4^\circ$, so the continuous value is $362^\circ$. + +## Pitfalls & Edge Cases + +**Precision degrades with $|a|$** — the remainder itself is exact, but $2\pi$ is not representable, so each full turn removed carries the representation error of $2\pi$ (about $1.7\times10^{-7}$ rad in single precision) and the total error grows like $|a|\,\varepsilon$. Wrap at the source, every integration step, rather than after long accumulation. An unwrapped signal is unbounded by design, so its own resolution shrinks as it grows. + +**Aliasing in unwrap** — if the true angle moves by $\pi$ or more between samples, the direction is ambiguous and the result is off by a multiple of $2\pi$. Sample faster than half a turn per period. + +**Half-turn asymmetry** — because $\pm\pi$ both map to $+\pi$, the shortest difference is not antisymmetric at exactly a half turn: swapping the arguments gives $+\pi$ again, not $-\pi$. + +**Rounding at the unsigned boundary** — a tiny negative remainder plus $2\pi$ can round to exactly $2\pi$; the unsigned wrap detects this and returns $0$, so the result stays strictly inside $[0, 2\pi)$. + +**Non-finite input** — infinities and NaN have no meaningful remainder; inputs must be finite. + +**Float-only** — floating-point types only, enforced at compile time; no fixed-point variants. + +## Variants & Generalizations + +Degrees or turns as the unit only rescale the period ($360$ or $1$ instead of $2\pi$); the unit-turn form avoids the inexact $2\pi$ entirely. Other canonical intervals, such as $[-\pi, \pi)$, differ only in which half-turn representative is kept. For rotations in 3D, quaternions avoid the wrap problem altogether, at the cost of a double cover ($q$ and $-q$). + +## Applications + +- **Heading and attitude control** — error terms computed as the shortest difference never command the long way round. +- **Sensor fusion** — the complementary filter's heading mode blends the measurement along the shortest arc and wraps the estimate back into $(-\pi, \pi]$. +- **Encoder and IMU processing** — unwrap a wrapped reading to track multi-turn position. +- **Odometry and inverse kinematics** — canonical joint angles with a single representation of a half turn. + +## Connections to Other Algorithms + +Used by the complementary filter in its heading mode. Related to the quaternion rotation representation, which has no wrap discontinuity. + +## References & Further Reading + +- Goldberg, D., "What every computer scientist should know about floating-point arithmetic," ACM Computing Surveys 23(1), 1991 (range reduction and the cost of an inexact $2\pi$) diff --git a/doc/math/README.md b/doc/math/README.md index 16f7b0eb..f47fa6ba 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -17,3 +17,4 @@ Core mathematical primitives for numerical computation. | [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | | [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | | [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | +| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | diff --git a/numerical/math/AngleUtilities.cpp b/numerical/math/AngleUtilities.cpp new file mode 100644 index 00000000..5ebd59d4 --- /dev/null +++ b/numerical/math/AngleUtilities.cpp @@ -0,0 +1,9 @@ +#include "numerical/math/AngleUtilities.hpp" + +namespace math +{ + template float WrapToPi(float); + template float WrapToTwoPi(float); + template float AngleDifference(float, float); + template float UnwrapAngle(float, float); +} diff --git a/numerical/math/AngleUtilities.hpp b/numerical/math/AngleUtilities.hpp new file mode 100644 index 00000000..4cb3286c --- /dev/null +++ b/numerical/math/AngleUtilities.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include +#include + +namespace math +{ + template + [[nodiscard]] OPTIMIZE_FOR_SPEED T WrapToPi(T angle) + { + static_assert(std::is_floating_point_v, "WrapToPi supports floating-point types only"); + + constexpr T pi{ std::numbers::pi_v }; + constexpr T twoPi{ T{ 2 } * std::numbers::pi_v }; + + T wrapped{ math::Fmod(angle + pi, twoPi) }; + if (wrapped <= T{}) + wrapped += twoPi; + + return wrapped - pi; + } + + template + [[nodiscard]] T WrapToTwoPi(T angle) + { + static_assert(std::is_floating_point_v, "WrapToTwoPi supports floating-point types only"); + + constexpr T twoPi{ T{ 2 } * std::numbers::pi_v }; + + T wrapped{ math::Fmod(angle, twoPi) }; + if (wrapped < T{}) + wrapped += twoPi; + + return wrapped >= twoPi ? T{} : wrapped; + } + + template + [[nodiscard]] T AngleDifference(T target, T current) + { + static_assert(std::is_floating_point_v, "AngleDifference supports floating-point types only"); + + return WrapToPi(target - current); + } + + template + [[nodiscard]] T UnwrapAngle(T previousUnwrapped, T wrapped) + { + static_assert(std::is_floating_point_v, "UnwrapAngle supports floating-point types only"); + + return previousUnwrapped + AngleDifference(wrapped, previousUnwrapped); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template float WrapToPi(float); + extern template float WrapToTwoPi(float); + extern template float AngleDifference(float, float); + extern template float UnwrapAngle(float, float); +#endif +} diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index 7487ba6c..b314c92f 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -16,6 +16,7 @@ if (NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS) endif() target_sources(numerical.math PRIVATE + AngleUtilities.hpp CholeskyDecomposition.hpp ConstexprMath.hpp ComplexNumber.hpp @@ -42,6 +43,7 @@ target_sources(numerical.math PRIVATE numerical_add_coverage_sources(numerical.math Math.cpp + AngleUtilities.cpp CholeskyDecomposition.cpp ComplexNumber.cpp ConsistencyMetrics.cpp diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index 13573bd3..bfbaa6ad 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(numerical.math_test PUBLIC numerical_link_qemu_runtime(numerical.math_test) target_sources(numerical.math_test PRIVATE + TestAngleUtilities.cpp TestCholeskyDecomposition.cpp TestComplexNumber.cpp TestGradientCheck.cpp diff --git a/numerical/math/test/TestAngleUtilities.cpp b/numerical/math/test/TestAngleUtilities.cpp new file mode 100644 index 00000000..606d1c61 --- /dev/null +++ b/numerical/math/test/TestAngleUtilities.cpp @@ -0,0 +1,59 @@ +#include "numerical/math/AngleUtilities.hpp" +#include +#include + +namespace +{ + class TestAngleUtilities : public ::testing::Test + { + protected: + static constexpr float pi{ std::numbers::pi_v }; + static constexpr float tolerance{ 1e-5f }; + + static constexpr float Degrees(float degrees) + { + return degrees * pi / 180.0f; + } + }; +} + +TEST_F(TestAngleUtilities, wrap_to_pi_maps_into_half_open_interval) +{ + EXPECT_NEAR(math::WrapToPi(1.5f * pi), -0.5f * pi, tolerance); + EXPECT_NEAR(math::WrapToPi(-1.5f * pi), 0.5f * pi, tolerance); + EXPECT_EQ(math::WrapToPi(0.0f), 0.0f); + EXPECT_NEAR(math::WrapToPi(7.0f * pi / 3.0f), pi / 3.0f, tolerance); +} + +TEST_F(TestAngleUtilities, wrap_to_pi_sends_both_half_turns_to_plus_pi) +{ + EXPECT_NEAR(math::WrapToPi(pi), pi, tolerance); + EXPECT_NEAR(math::WrapToPi(-pi), pi, tolerance); +} + +TEST_F(TestAngleUtilities, wrap_to_two_pi_maps_into_zero_to_two_pi) +{ + EXPECT_NEAR(math::WrapToTwoPi(-0.5f * pi), 1.5f * pi, tolerance); + EXPECT_NEAR(math::WrapToTwoPi(2.0f * pi), 0.0f, tolerance); + EXPECT_NEAR(math::WrapToTwoPi(5.0f * pi), pi, 4.0f * tolerance); + EXPECT_LT(math::WrapToTwoPi(-1e-8f), 2.0f * pi); +} + +TEST_F(TestAngleUtilities, angle_difference_takes_the_short_way) +{ + EXPECT_NEAR(math::AngleDifference(Degrees(170.0f), Degrees(-170.0f)), Degrees(-20.0f), tolerance); + EXPECT_NEAR(math::AngleDifference(Degrees(-170.0f), Degrees(170.0f)), Degrees(20.0f), tolerance); +} + +TEST_F(TestAngleUtilities, unwrap_follows_a_continuous_rotation) +{ + float unwrapped{ math::WrapToPi(0.0f) }; + + for (int k = 1; k <= 40; ++k) + { + const float angle{ 0.3f * static_cast(k) }; + unwrapped = math::UnwrapAngle(unwrapped, math::WrapToPi(angle)); + + EXPECT_NEAR(unwrapped, angle, tolerance); + } +} diff --git a/roadmap/README.md b/roadmap/README.md index 59dd9e95..407f26fd 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -104,7 +104,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `AngleUtilities` (54) · `TridiagonalSolver` (59) · `CubicSpline` (61) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `TridiagonalSolver` (59) · `CubicSpline` (61) ### `optimization` `LevenbergMarquardt` (64) diff --git a/roadmap/math/AngleUtilities/explanation.md b/roadmap/math/AngleUtilities/explanation.md deleted file mode 100644 index d864d52b..00000000 --- a/roadmap/math/AngleUtilities/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Angle Utilities — Overview - -## What it is -Small helpers for angles that live on a circle: wrap any angle into a canonical interval, take the -shortest signed difference between two angles, and unwrap a wrapped measurement into a continuous -signal. - -## Why it matters (embedded) -Angles wrap: a heading of 179° and one of −179° are 2° apart, not 358°. Controllers that subtract -angles naively command a full turn the wrong way; integrators that never wrap lose precision; encoders -and IMUs report wrapped values that estimators must unwrap. These bugs recur in every module that -touches angles, so one shared, tested implementation with one convention removes a whole class of -defects. - -## How it works (intuition) -Shift the angle so the target interval starts at zero, take the remainder modulo a full turn, fix the -sign the remainder inherits from a negative input, and shift back. The shortest difference is the -wrapped plain difference. Unwrapping adds that shortest difference to the previous continuous value, -which is correct as long as consecutive samples move by less than half a turn. - -## Key parameters -- **Interval convention** — `(−π, π]` for signed angles, `[0, 2π)` for unsigned ones. -- **Sample rate (unwrap)** — consecutive samples must differ by less than `π`. - -## Reference -D. Goldberg, "What every computer scientist should know about floating-point arithmetic," *ACM -Computing Surveys*, 23(1), 1991 (range reduction and the cost of an inexact `2π`). - -## See also -`ComplementaryFilter` (wraps its estimate), `Quaternion`, `SE3Transform` (#53, rotation-vector angles in -`[0, π]`). diff --git a/roadmap/math/AngleUtilities/implementation.md b/roadmap/math/AngleUtilities/implementation.md deleted file mode 100644 index 6a106341..00000000 --- a/roadmap/math/AngleUtilities/implementation.md +++ /dev/null @@ -1,74 +0,0 @@ -# Angle Utilities (wrap, shortest difference, unwrap) — Implementation Pseudocode - -> Roadmap ref: #54 (Tier 1) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -Every consumer that integrates or compares angles needs the same four operations: wrapping into a -canonical interval, the shortest signed difference between two angles, and unwrapping a wrapped -measurement back into a continuous signal. Today `ComplementaryFilter` carries a private wrap and -robotics-toolbox-cpp's analytical IK, mobile-base odometry and heading controllers would each write -their own. One header, one convention. - -## Data structures - -None — stateless free function templates. - -## Interface - -```cpp -template T WrapToPi(T angle) # (−π, π]; hot path -template T WrapToTwoPi(T angle) # [0, 2π) -template T AngleDifference(T target, T current) # WrapToPi(target − current), short way -template T UnwrapAngle(T previousUnwrapped, T wrapped) # continuous continuation -# each: static_assert(std::is_floating_point_v, "...") -``` - -## Algorithm (pseudocode) - -```text -twoPi = 2·std::numbers::pi_v - -function WrapToPi(a): # OPTIMIZE_FOR_SPEED - r = math::Fmod(a + π, twoPi) # C fmod keeps the sign of the dividend: r ∈ (−2π, 2π) - if r ≤ 0: r = r + twoPi # r ∈ (0, 2π] - return r − π # (−π, π]; a = ±π ⇒ +π - -function WrapToTwoPi(a): - r = math::Fmod(a, twoPi) - if r < 0: r = r + twoPi - return r ≥ twoPi ? 0 : r # rounding of r + 2π can land exactly on 2π - -function AngleDifference(target, current): - return WrapToPi(target − current) - -function UnwrapAngle(previousUnwrapped, wrapped): - return previousUnwrapped + AngleDifference(wrapped, previousUnwrapped) -``` - -## Complexity & memory - -- `O(1)` each: one `fmod`, one compare, one add. No state, no heap. - -## Numerical / embedded notes - -- **Convention (normative):** `WrapToPi` returns `(−π, π]` — `−π` maps to `+π` — so a joint at exactly - half a turn has one representation. `ComplementaryFilter`'s private wrap returns `[−π, π)`; switching it - to `WrapToPi` changes the result only at exactly `±π`. -- Precision degrades with `|angle|`: `fmod` is exact, but `2π` is not representable, so the error grows - like `|angle|·ε`. Wrap at the source (each integration step) rather than after long accumulation. -- `UnwrapAngle` assumes consecutive samples differ by less than `π`; sample faster than half a turn per - period. -- Use `math::Fmod` (platform-dispatched) rather than `std::fmod`, so the ARM implementation is picked up. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/math/AngleUtilities.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `WrapToPi`, and - `extern template float WrapToPi(float);` (and the other three) under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/AngleUtilities.cpp` → `template float WrapToPi(float);` and the others. -- Test: `numerical/math/test/TestAngleUtilities.cpp` -- Doc: `doc/math/AngleUtilities.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestAngleUtilities.cpp` → `numerical.math_test`. -- Follow-up: replace `ComplementaryFilter`'s private wrap with `WrapToPi`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/AngleUtilities/tests.md b/roadmap/math/AngleUtilities/tests.md deleted file mode 100644 index 2f68dbc5..00000000 --- a/roadmap/math/AngleUtilities/tests.md +++ /dev/null @@ -1,39 +0,0 @@ -# Angle Utilities — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestAngleUtilities : public ::testing::Test: - float pi = std::numbers::pi_v -# each case below is a TEST_F(TestAngleUtilities, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -wrap_to_pi_maps_into_half_open_interval: - Assert: WrapToPi(3π/2) ≈ −π/2; WrapToPi(−3π/2) ≈ π/2; WrapToPi(0) == 0; WrapToPi(7π/3) ≈ π/3 -wrap_to_pi_sends_both_half_turns_to_plus_pi: - Assert: WrapToPi(π) ≈ π and WrapToPi(−π) ≈ π -wrap_to_two_pi_maps_into_zero_to_two_pi: - Assert: WrapToTwoPi(−π/2) ≈ 3π/2; WrapToTwoPi(2π) ≈ 0; WrapToTwoPi(5π) ≈ π -angle_difference_takes_the_short_way: - Assert: AngleDifference(170°, −170°) ≈ −20° and AngleDifference(−170°, 170°) ≈ 20° -unwrap_follows_a_continuous_rotation: - Arrange: true angle θₖ = 0.3·k rad, k = 0..40 (≈ 2 turns); wrapped = WrapToPi(θₖ) - Act: u₀ = wrapped₀; uₖ = UnwrapAngle(uₖ₋₁, wrappedₖ) - Assert: uₖ ≈ θₖ for every k -``` - -## Reference vectors - -- `7π/3 = 2π + π/3` ⇒ `π/3`; `5π = 2·2π + π` ⇒ `π` in `[0, 2π)`. -- `170° − (−170°) = 340° ≡ −20°`. - -## Edge cases - -- `±π` exactly ⇒ `+π` (normative convention). -- `|angle|` ≈ `1e4` rad ⇒ result within `|angle|·ε` of the exact wrap; assert with that tolerance. -- `WrapToTwoPi` of a value just below `0` ⇒ never returns `2π`. From e45ddabbbcd838b94e5da88fea029193e54b99fe Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:10:14 +0000 Subject: [PATCH 2/7] refactor(filters): use math::WrapToPi in ComplementaryFilter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the private heading wrap with the shared angle utilities. The result changes only at exactly ±π: the estimate is now reported in (−π, π], so −π maps to +π, matching the library-wide convention. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- doc/filters/active/ComplementaryFilter.md | 2 +- .../filters/active/ComplementaryFilter.hpp | 20 +++---------------- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/doc/filters/active/ComplementaryFilter.md b/doc/filters/active/ComplementaryFilter.md index 119055ee..c7b7a898 100644 --- a/doc/filters/active/ComplementaryFilter.md +++ b/doc/filters/active/ComplementaryFilter.md @@ -50,7 +50,7 @@ the seam. Instead the blend is performed along the shortest arc: $$\delta = \mathrm{WrapToPi}(\theta_{acc} - \hat{\theta})$$ $$\theta_{k+1} = \mathrm{WrapToPi}\!\left(\hat{\theta} + (1-\alpha)\,\delta\right)$$ -where $\mathrm{WrapToPi}(x) = \bigl((x + \pi) \bmod 2\pi\bigr) - \pi$. +where $\mathrm{WrapToPi}$ reduces an angle into $(-\pi, \pi]$ — the library-wide angle convention, so an angle of exactly $-\pi$ is reported as $+\pi$ (see [Angle Utilities](../../math/AngleUtilities.md)). ## Complexity Analysis diff --git a/numerical/filters/active/ComplementaryFilter.hpp b/numerical/filters/active/ComplementaryFilter.hpp index 8d8f0946..584677f8 100644 --- a/numerical/filters/active/ComplementaryFilter.hpp +++ b/numerical/filters/active/ComplementaryFilter.hpp @@ -1,9 +1,8 @@ #pragma once +#include "numerical/math/AngleUtilities.hpp" #include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Math.hpp" #include -#include #include namespace filters @@ -28,8 +27,6 @@ namespace filters T Ts; T angle; bool wrapAngle; - - static T WrapToPi(T x) noexcept; }; //// Implementation //// @@ -59,8 +56,8 @@ namespace filters T predicted{ angle + rate * measuredTs }; if (wrapAngle) { - T delta{ WrapToPi(measuredAngle - predicted) }; - angle = WrapToPi(predicted + (T{ 1 } - alpha) * delta); + T delta{ math::AngleDifference(measuredAngle, predicted) }; + angle = math::WrapToPi(predicted + (T{ 1 } - alpha) * delta); } else { @@ -88,17 +85,6 @@ namespace filters return tau / (tau + Ts); } - template - T ComplementaryFilter::WrapToPi(T x) noexcept - { - constexpr T pi{ std::numbers::pi_v }; - constexpr T twoPi{ T{ 2 } * std::numbers::pi_v }; - T wrapped{ math::Fmod(x + pi, twoPi) }; - if (wrapped < T{}) - wrapped += twoPi; - return wrapped - pi; - } - #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class ComplementaryFilter; #endif From 1ee01a604e29390d9aae5448825297d7ff50c926 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:15:48 +0000 Subject: [PATCH 3/7] feat(math): add tridiagonal (Thomas) solver Deploy roadmap item 59: O(n) factor-once / solve-many solver for tridiagonal systems. Factor stores reciprocal pivots so Solve has no division, and reports a pivot with |p| <= eps * scale (or a size outside [1, MaxSize]) as failure instead of dividing by it. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 10 -- doc/math/README.md | 1 + doc/math/TridiagonalSolver.md | 93 ++++++++++++++++ numerical/math/CMakeLists.txt | 2 + numerical/math/TridiagonalSolver.cpp | 6 ++ numerical/math/TridiagonalSolver.hpp | 101 ++++++++++++++++++ numerical/math/test/CMakeLists.txt | 1 + numerical/math/test/TestTridiagonalSolver.cpp | 62 +++++++++++ roadmap/README.md | 2 +- roadmap/math/TridiagonalSolver/explanation.md | 29 ----- .../math/TridiagonalSolver/implementation.md | 77 ------------- roadmap/math/TridiagonalSolver/tests.md | 40 ------- 13 files changed, 268 insertions(+), 158 deletions(-) create mode 100644 doc/math/TridiagonalSolver.md create mode 100644 numerical/math/TridiagonalSolver.cpp create mode 100644 numerical/math/TridiagonalSolver.hpp create mode 100644 numerical/math/test/TestTridiagonalSolver.cpp delete mode 100644 roadmap/math/TridiagonalSolver/explanation.md delete mode 100644 roadmap/math/TridiagonalSolver/implementation.md delete mode 100644 roadmap/math/TridiagonalSolver/tests.md diff --git a/README.md b/README.md index b37a16e1..a64cdd1a 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | diff --git a/ROADMAP.md b/ROADMAP.md index 84d160ba..e3592219 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -30,7 +30,6 @@ Difficulty legend: | 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | | 57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | ★☆☆☆☆ | | 58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | ★☆☆☆☆ | -| 59 | Tridiagonal (Thomas) solver | `math` | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | | 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | @@ -93,15 +92,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `math::Vector`; complements `RungeKuttaIntegrators`. - **Spec:** [roadmap/solvers/SymplecticIntegrators](roadmap/solvers/SymplecticIntegrators/implementation.md). -### 59. Tridiagonal solver -- **What:** Thomas algorithm, factor once / solve many, pivot-failure reporting. -- **Embedded value:** `O(n)` instead of `O(n³)` for splines and implicit difference schemes. -- **Algorithm / paper:** L. H. Thomas (1949); G. Golub, C. Van Loan, *Matrix Computations*, §4.3. -- **Reuses:** Bounded `std::array` bands. -- **Spec:** [roadmap/math/TridiagonalSolver](roadmap/math/TridiagonalSolver/implementation.md). - ---- - ## Tier 2 — Easy ★★☆☆☆ ### 53. SE(3) transform, twists and wrenches diff --git a/doc/math/README.md b/doc/math/README.md index f47fa6ba..cfc1796c 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -18,3 +18,4 @@ Core mathematical primitives for numerical computation. | [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | | [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | | [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | +| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | diff --git a/doc/math/TridiagonalSolver.md b/doc/math/TridiagonalSolver.md new file mode 100644 index 00000000..2a4a7154 --- /dev/null +++ b/doc/math/TridiagonalSolver.md @@ -0,0 +1,93 @@ +# Tridiagonal Solver (Thomas Algorithm) + +## Overview & Motivation + +A tridiagonal system has nonzeros only on the main diagonal and its two neighbours. Spline interpolation, implicit heat/diffusion steps and several smoothing problems all reduce to such systems. A dense solver wastes cubic time and quadratic memory on zeros; the **Thomas algorithm** is Gaussian elimination that skips every zero, so it runs in linear time using three short bands. + +The elimination sweep depends only on the matrix, so it is performed once and reused: each new right-hand side costs two short sweeps and no division. This fits the common case where the matrix depends on fixed geometry (for example knot spacing) while the right-hand side changes per channel or per tick. + +## Mathematical Theory + +Let $A \in \mathbb{R}^{n \times n}$ be tridiagonal with sub-diagonal $a_k = A_{k,k-1}$ ($a_0$ unused), diagonal $b_k$ and super-diagonal $c_k = A_{k,k+1}$ ($c_{n-1}$ unused). The system $Ax = d$ reads + +$$a_k x_{k-1} + b_k x_k + c_k x_{k+1} = d_k, \quad k = 0, \dots, n-1$$ + +### Forward elimination (matrix only) + +Eliminating $a_k$ with the already-reduced row above leaves an upper bidiagonal system with unit diagonal. Define the pivots and modified super-diagonal + +$$p_0 = b_0, \qquad p_k = b_k - a_k\, c'_{k-1}, \qquad c'_k = \frac{c_k}{p_k}$$ + +The reciprocals $1/p_k$ are stored, so $c'_k = c_k \cdot (1/p_k)$. This is the factorization $A = LU$ with $L$ lower bidiagonal (diagonal $p_k$, sub-diagonal $a_k$) and $U$ unit upper bidiagonal (super-diagonal $c'_k$). + +### Forward substitution (right-hand side) + +$$y_0 = \frac{d_0}{p_0}, \qquad y_k = \frac{d_k - a_k\, y_{k-1}}{p_k}$$ + +### Back substitution + +$$x_{n-1} = y_{n-1}, \qquad x_k = y_k - c'_k\, x_{k+1}, \quad k = n-2, \dots, 0$$ + +Division by $p_k$ is replaced by multiplication with the stored reciprocal. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------------|----------|----------|----------------------------------------------| +| Factorization | $O(n)$ | $O(n)$ | $n$ divisions, one multiply-subtract per row | +| Solve (per RHS) | $O(n)$ | $O(n)$ | About $3n$ multiply-adds, no division | +| Dense LU, for contrast | $O(n^3)$ | $O(n^2)$ | Operates on all the zeros | + +Storage is three bands of fixed capacity; nothing is allocated dynamically. Factoring once and solving $m$ right-hand sides costs $O(n + mn)$ with all divisions paid up front. + +## Step-by-Step Walkthrough + +Take $A = \operatorname{tridiag}(-1, 2, -1)$ with $n = 4$, so $a_k = c_k = -1$, $b_k = 2$. + +| $k$ | $p_k$ | $1/p_k$ | $c'_k$ | +|-----|-------|---------|--------| +| 0 | $2$ | $1/2$ | $-1/2$ | +| 1 | $2 - (-1)(-1/2) = 3/2$ | $2/3$ | $-2/3$ | +| 2 | $2 - (-1)(-2/3) = 4/3$ | $3/4$ | $-3/4$ | +| 3 | $2 - (-1)(-3/4) = 5/4$ | $4/5$ | unused | + +**Right-hand side** $d = (1, 0, 0, 1)$: forward $y = (1/2,\ 1/3,\ 1/4,\ 1)$; back substitution gives $x_3 = 1$, $x_2 = 1/4 + \tfrac34 = 1$, $x_1 = 1/3 + \tfrac23 = 1$, $x_0 = 1/2 + 1/2 = 1$, so $x = (1, 1, 1, 1)$. + +**Right-hand side** $d = (1, 2, 3, 4)$ reuses the same table: forward $y = (1/2,\ 5/3,\ 7/2,\ 6)$; back substitution gives $x_3 = 6$, $x_2 = 7/2 + 9/2 = 8$, $x_1 = 5/3 + 16/3 = 7$, $x_0 = 1/2 + 7/2 = 4$, so $x = (4, 7, 8, 6)$. + +**Verify:** row 1 of $Ax$ is $-4 + 14 - 8 = 2$ ✓ and row 3 is $-8 + 12 = 4$ ✓. + +## Pitfalls & Edge Cases + +- **No pivoting.** The algorithm is stable when $A$ is strictly diagonally dominant ($|b_k| > |a_k| + |c_k|$) or symmetric positive-definite; spline systems are both. For other tridiagonal matrices a zero or tiny pivot can appear even when $A$ is nonsingular, and growth can destroy accuracy. Use LU decomposition with partial pivoting instead. +- **Vanishing pivot.** If $|p_k| \le \varepsilon \cdot s$, where $\varepsilon$ is the machine epsilon and $s$ is the largest absolute entry among the $n$ rows, the factorization reports failure instead of dividing. The relative threshold keeps the test meaningful regardless of the matrix's scale. +- **Size limits.** A size of zero or one exceeding the compile-time capacity is rejected. Entries of the solution beyond $n$ are zero. +- **Failed factorization.** A solve is only meaningful after a successful factorization; refactor whenever the matrix changes. +- **Floating-point only.** Reciprocals and the epsilon test have no meaning for integer types. + +## Variants & Generalizations + +- **Cyclic tridiagonal.** Periodic boundary conditions add corner entries; the Sherman–Morrison formula reduces the solve to two tridiagonal solves. +- **Block tridiagonal.** Scalar pivots become small dense blocks, as in implicit multi-variable PDE steps. +- **Symmetric case.** When $a_k = c_{k-1}$, one band can be dropped and the factorization becomes $LDL^T$. +- **Parallel variants.** Cyclic reduction trades extra work for parallelism; irrelevant on a single embedded core. + +## Applications + +- **Cubic spline interpolation** — the second-derivative system at the knots is tridiagonal and diagonally dominant. +- **Implicit finite differences** — backward-Euler and Crank–Nicolson steps for 1-D heat and diffusion equations. +- **Smoothing** — penalized least-squares smoothers produce banded normal equations. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|---------------------------------------------------|------------------------------------------------------------------------------| +| [LU Decomposition](../solvers/LuDecomposition.md) | General fallback with partial pivoting; $O(n^3)$ and dense | +| [Triangular Solve](TriangularSolve.md) | Same forward/back substitution idea, applied to full triangular factors | +| [Levinson–Durbin](../solvers/LevinsonDurbin.md) | Another structured solver, for Toeplitz systems | +| Cubic spline interpolation | Main consumer; plans the second derivatives by solving one tridiagonal system | + +## References & Further Reading + +- Thomas, L. H., "Elliptic Problems in Linear Difference Equations over a Network", Watson Scientific Computing Laboratory, Columbia University, 1949. +- Golub, G. H. & Van Loan, C. F., *Matrix Computations*, 4th ed., Johns Hopkins University Press, 2013, §4.3. diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index b314c92f..a3afc00b 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -37,6 +37,7 @@ target_sources(numerical.math PRIVATE StepResponseMetrics.hpp Toeplitz.hpp Tolerance.hpp + TridiagonalSolver.hpp Math.hpp TriangularSolve.hpp ) @@ -54,6 +55,7 @@ numerical_add_coverage_sources(numerical.math Quaternion.cpp RecursiveBuffer.cpp Toeplitz.cpp + TridiagonalSolver.cpp ) add_subdirectory(test) diff --git a/numerical/math/TridiagonalSolver.cpp b/numerical/math/TridiagonalSolver.cpp new file mode 100644 index 00000000..4a1bac21 --- /dev/null +++ b/numerical/math/TridiagonalSolver.cpp @@ -0,0 +1,6 @@ +#include "numerical/math/TridiagonalSolver.hpp" + +namespace math +{ + template class TridiagonalSolver; +} diff --git a/numerical/math/TridiagonalSolver.hpp b/numerical/math/TridiagonalSolver.hpp new file mode 100644 index 00000000..aa1d2231 --- /dev/null +++ b/numerical/math/TridiagonalSolver.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include +#include +#include +#include +#include + +namespace math +{ + template + class TridiagonalSolver + { + static_assert(std::is_floating_point_v, "TridiagonalSolver supports floating-point types only"); + static_assert(MaxSize >= 1, "TridiagonalSolver requires MaxSize >= 1"); + + public: + using Band = std::array; + + bool Factor(const Band& subDiagonal, const Band& diagonal, const Band& superDiagonal, std::size_t n); + OPTIMIZE_FOR_SPEED Band Solve(const Band& rhs) const; + std::size_t Size() const; + + private: + static T Scale(const Band& subDiagonal, const Band& diagonal, const Band& superDiagonal, std::size_t n); + + Band sub{}; + Band invPivot{}; + Band supPrime{}; + std::size_t size{ 0 }; + }; + + template + bool TridiagonalSolver::Factor(const Band& subDiagonal, const Band& diagonal, const Band& superDiagonal, std::size_t n) + { + size = 0; + if (n == 0 || n > MaxSize) + return false; + + const T threshold{ std::numeric_limits::epsilon() * Scale(subDiagonal, diagonal, superDiagonal, n) }; + + for (std::size_t k = 0; k < n; ++k) + { + const T pivot{ k == 0 ? diagonal[0] : diagonal[k] - subDiagonal[k] * supPrime[k - 1] }; + if (math::Abs(pivot) <= threshold) + return false; + + invPivot[k] = T{ 1 } / pivot; + supPrime[k] = superDiagonal[k] * invPivot[k]; + } + + sub = subDiagonal; + size = n; + return true; + } + + template + OPTIMIZE_FOR_SPEED auto TridiagonalSolver::Solve(const Band& rhs) const -> Band + { + really_assert(size > 0); + + Band x{}; + x[0] = rhs[0] * invPivot[0]; + for (std::size_t k = 1; k < size; ++k) + x[k] = (rhs[k] - sub[k] * x[k - 1]) * invPivot[k]; + + for (std::size_t k = size - 1; k > 0; --k) + x[k - 1] -= supPrime[k - 1] * x[k]; + + return x; + } + + template + std::size_t TridiagonalSolver::Size() const + { + return size; + } + + template + T TridiagonalSolver::Scale(const Band& subDiagonal, const Band& diagonal, const Band& superDiagonal, std::size_t n) + { + T scale{}; + for (std::size_t k = 0; k < n; ++k) + { + scale = std::max(scale, math::Abs(diagonal[k])); + if (k > 0) + scale = std::max(scale, math::Abs(subDiagonal[k])); + if (k + 1 < n) + scale = std::max(scale, math::Abs(superDiagonal[k])); + } + + return scale; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class TridiagonalSolver; +#endif +} diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index bfbaa6ad..f635a61c 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -33,4 +33,5 @@ target_sources(numerical.math_test PRIVATE TestStepResponseMetrics.cpp TestToeplitz.cpp TestTriangularSolve.cpp + TestTridiagonalSolver.cpp ) diff --git a/numerical/math/test/TestTridiagonalSolver.cpp b/numerical/math/test/TestTridiagonalSolver.cpp new file mode 100644 index 00000000..0d89e2e2 --- /dev/null +++ b/numerical/math/test/TestTridiagonalSolver.cpp @@ -0,0 +1,62 @@ +#include "numerical/math/TridiagonalSolver.hpp" +#include +#include +#include + +namespace +{ + using Solver = math::TridiagonalSolver; + using Band = Solver::Band; + + class TestTridiagonalSolver : public ::testing::Test + { + protected: + Solver solver; + Band sub{ 0.0f, -1.0f, -1.0f, -1.0f }; + Band diag{ 2.0f, 2.0f, 2.0f, 2.0f }; + Band sup{ -1.0f, -1.0f, -1.0f, 0.0f }; + + static void ExpectBandNear(const Band& actual, const Band& expected) + { + for (std::size_t k = 0; k < expected.size(); ++k) + EXPECT_NEAR(actual[k], expected[k], 1e-5f) << "k = " << k; + } + }; +} + +TEST_F(TestTridiagonalSolver, solves_second_difference_system) +{ + ASSERT_TRUE(solver.Factor(sub, diag, sup, 4)); + + ExpectBandNear(solver.Solve(Band{ 1.0f, 0.0f, 0.0f, 1.0f }), Band{ 1.0f, 1.0f, 1.0f, 1.0f }); +} + +TEST_F(TestTridiagonalSolver, one_factorization_serves_several_right_hand_sides) +{ + ASSERT_TRUE(solver.Factor(sub, diag, sup, 4)); + + ExpectBandNear(solver.Solve(Band{ 1.0f, 0.0f, 0.0f, 1.0f }), Band{ 1.0f, 1.0f, 1.0f, 1.0f }); + ExpectBandNear(solver.Solve(Band{ 1.0f, 2.0f, 3.0f, 4.0f }), Band{ 4.0f, 7.0f, 8.0f, 6.0f }); +} + +TEST_F(TestTridiagonalSolver, single_row_system_divides_by_the_diagonal) +{ + ASSERT_TRUE(solver.Factor(Band{}, Band{ 4.0f }, Band{}, 1)); + + ExpectBandNear(solver.Solve(Band{ 2.0f }), Band{ 0.5f }); +} + +TEST_F(TestTridiagonalSolver, vanishing_pivot_is_reported) +{ + const Band ones{ 1.0f, 1.0f, 1.0f, 1.0f }; + const Band minusOnes{ -1.0f, -1.0f, -1.0f, -1.0f }; + + EXPECT_FALSE(solver.Factor(minusOnes, ones, minusOnes, 4)); + EXPECT_EQ(solver.Size(), 0); +} + +TEST_F(TestTridiagonalSolver, size_outside_range_is_rejected) +{ + EXPECT_FALSE(solver.Factor(sub, diag, sup, 0)); + EXPECT_FALSE(solver.Factor(sub, diag, sup, 9)); +} diff --git a/roadmap/README.md b/roadmap/README.md index 407f26fd..4103ee08 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -104,7 +104,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `TridiagonalSolver` (59) · `CubicSpline` (61) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `CubicSpline` (61) ### `optimization` `LevenbergMarquardt` (64) diff --git a/roadmap/math/TridiagonalSolver/explanation.md b/roadmap/math/TridiagonalSolver/explanation.md deleted file mode 100644 index 58638186..00000000 --- a/roadmap/math/TridiagonalSolver/explanation.md +++ /dev/null @@ -1,29 +0,0 @@ -# Tridiagonal Solver (Thomas Algorithm) — Overview - -## What it is -A linear solver specialised for matrices whose only nonzeros are on the main diagonal and its two -neighbours. It is Gaussian elimination that skips every zero, so it runs in linear time. - -## Why it matters (embedded) -Spline interpolation through many points, implicit heat/diffusion steps and several smoothing -problems produce tridiagonal systems. A dense solver wastes cubic time and quadratic memory on zeros; the -Thomas algorithm needs three short arrays and a few multiply-adds per row, so a 50-knot spline is -re-planned in microseconds. - -## How it works (intuition) -Walk down the rows once, eliminating the sub-diagonal entry of each row with the row above; the system -becomes upper bidiagonal. Walk back up, solving one unknown per row. The downward sweep depends only on -the matrix, so it is done once; each new right-hand side then costs only the two short sweeps. - -## Key parameters -- **Size `n`** — up to the compile-time capacity. -- **Diagonal dominance** — the condition under which skipping pivoting is safe. - -## Reference -L. H. Thomas, "Elliptic problems in linear difference equations over a network," Watson Sci. Comput. -Lab. Report, Columbia University, 1949; G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., -§4.3. - -## See also -`TriangularSolve` (the triangular sibling), `LuDecomposition` (general systems, pivoting), `CubicSpline` -(#61, the main consumer), `Toeplitz` and `LevinsonDurbin` (other structured solvers). diff --git a/roadmap/math/TridiagonalSolver/implementation.md b/roadmap/math/TridiagonalSolver/implementation.md deleted file mode 100644 index e82951f8..00000000 --- a/roadmap/math/TridiagonalSolver/implementation.md +++ /dev/null @@ -1,77 +0,0 @@ -# Tridiagonal Solver (Thomas Algorithm) — Implementation Pseudocode - -> Roadmap ref: #59 (Tier 1) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -`O(n)` solve of `A·x = b` for a tridiagonal `A`, factored once and reused for many right-hand sides. -Cubic splines (#61), implicit finite-difference schemes and some smoothing filters all reduce to it; a -dense LU on the same system costs `O(n³)`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); static_assert(MaxSize >= 1) -class TridiagonalSolver: # instantiated for float - using Band = std::array - Band sub # a_k: A[k][k−1], a_0 unused - Band invPivot # 1 / (b_k − a_k·c'_{k−1}) - Band supPrime # c'_k = c_k · invPivot_k - std::size_t size # n, 1 ≤ n ≤ MaxSize; 0 until Factor succeeds -``` - -## Interface - -```cpp -bool Factor(const Band& sub, const Band& diag, const Band& sup, std::size_t n) # false if n ∉ [1, MaxSize] or a pivot vanishes -Band Solve(const Band& rhs) const # hot path; entries ≥ n are zero; precondition: Factor() == true -std::size_t Size() const -``` - -## Algorithm (pseudocode) - -```text -function Factor(a, b, c, n): # forward elimination of the matrix only - if n == 0 or n > MaxSize: return false - scale = max |b_k|, |a_k|, |c_k| over the n rows - pivot = b_0; if |pivot| ≤ ε·scale: return false - invPivot[0] = 1/pivot; supPrime[0] = c_0·invPivot[0] - for k = 1..n−1: - pivot = b_k − a_k·supPrime[k−1] - if |pivot| ≤ ε·scale: return false - invPivot[k] = 1/pivot; supPrime[k] = c_k·invPivot[k] - store a as sub; size = n; return true - -function Solve(r): # OPTIMIZE_FOR_SPEED — O(n) - y[0] = r_0·invPivot[0] - for k = 1..n−1: y[k] = (r_k − sub[k]·y[k−1])·invPivot[k] - x[n−1] = y[n−1] - for k = n−2 down to 0: x[k] = y[k] − supPrime[k]·x[k+1] - return x -``` - -## Complexity & memory - -- `Factor`: `O(n)`, `n` divisions; `Solve`: `O(n)`, `3n` multiply-adds, no division. -- Memory: three `MaxSize` bands; no heap. - -## Numerical / embedded notes - -- **No pivoting:** stable for strictly diagonally dominant or symmetric positive-definite `A` (the spline - systems are both). For other tridiagonal matrices use `LuDecomposition` with partial pivoting. -- The `ε·scale` pivot test reports a singular or non-dominant system as `false` instead of dividing by - (near) zero. -- Factoring once moves every division out of `Solve` — useful when the matrix depends only on knot - spacing and the right-hand side changes per channel or per tick. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/math/TridiagonalSolver.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class TridiagonalSolver;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/TridiagonalSolver.cpp` → `template class TridiagonalSolver;` -- Test: `numerical/math/test/TestTridiagonalSolver.cpp` -- Doc: `doc/math/TridiagonalSolver.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestTridiagonalSolver.cpp` → `numerical.math_test`. - It lives in `math` next to `TriangularSolve` because `CubicSpline` (#61, also `math`) uses it and - `numerical.solver` already depends on `numerical.math`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/TridiagonalSolver/tests.md b/roadmap/math/TridiagonalSolver/tests.md deleted file mode 100644 index d6bb2c23..00000000 --- a/roadmap/math/TridiagonalSolver/tests.md +++ /dev/null @@ -1,40 +0,0 @@ -# Tridiagonal Solver — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestTridiagonalSolver : public ::testing::Test: - TridiagonalSolver solver - # second-difference matrix tridiag(−1, 2, −1), n = 4 - Band sub{ 0, −1, −1, −1 }, diag{ 2, 2, 2, 2 }, sup{ −1, −1, −1, 0 } -# each case below is a TEST_F(TestTridiagonalSolver, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -solves_second_difference_system: - Arrange: Factor(sub, diag, sup, 4); b = (1, 0, 0, 1) - Assert: Solve(b) ≈ (1, 1, 1, 1) -one_factorization_serves_several_right_hand_sides: - Arrange: Factor once; b₁ = (1, 0, 0, 1), b₂ = (1, 2, 3, 4) - Assert: Solve(b₁) ≈ (1, 1, 1, 1) and Solve(b₂) ≈ (4, 7, 8, 6) -single_row_system_divides_by_the_diagonal: - Assert: Factor({0}, {4}, {0}, 1) and Solve({2}) ≈ (0.5) -vanishing_pivot_is_reported: - Arrange: diag = (1, 1, …), sub = sup = −1 ⇒ second pivot 1 − 1·1 = 0 - Assert: Factor(...) == false -size_outside_range_is_rejected: - Assert: Factor(..., 0) == false and Factor(..., 9) == false -``` - -## Reference vectors - -- `tridiag(−1, 2, −1)·(1,1,1,1) = (1,0,0,1)`; `tridiag(−1, 2, −1)·(4,7,8,6) = (1,2,3,4)`. - -## Edge cases - -- Strictly diagonally dominant random systems (`|b_k| > |a_k| + |c_k|`) ⇒ residual `‖A·x − b‖ ≤ 1e-5·‖b‖`. -- Entries of the returned band at index `≥ n` are zero. From 5037e7aa52c29895045c75af7e124589b33cd03f Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:15:58 +0000 Subject: [PATCH 4/7] feat(controllers): add friction compensation feedforward MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 57: per-axis Coulomb + viscous + Stribeck friction model, smoothed through zero velocity with tanh(v/eps) and clamped to maxCompensation, to be added to a torque or current command. The spec's "torque ≈ F_s near zero velocity" check is asserted against the model's exact value at v = 10·eps (0.788237): the Stribeck term has already decayed by e^-0.04 there. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 10 -- doc/controllers/FrictionCompensation.md | 106 +++++++++++++++++ doc/controllers/README.md | 1 + .../implementations/CMakeLists.txt | 2 + .../implementations/FrictionCompensation.cpp | 7 ++ .../implementations/FrictionCompensation.hpp | 84 ++++++++++++++ .../implementations/test/CMakeLists.txt | 1 + .../test/TestFrictionCompensation.cpp | 107 ++++++++++++++++++ roadmap/README.md | 2 +- .../FrictionCompensation/explanation.md | 35 ------ .../FrictionCompensation/implementation.md | 86 -------------- .../controllers/FrictionCompensation/tests.md | 66 ----------- 13 files changed, 310 insertions(+), 199 deletions(-) create mode 100644 doc/controllers/FrictionCompensation.md create mode 100644 numerical/controllers/implementations/FrictionCompensation.cpp create mode 100644 numerical/controllers/implementations/FrictionCompensation.hpp create mode 100644 numerical/controllers/implementations/test/TestFrictionCompensation.cpp delete mode 100644 roadmap/controllers/FrictionCompensation/explanation.md delete mode 100644 roadmap/controllers/FrictionCompensation/implementation.md delete mode 100644 roadmap/controllers/FrictionCompensation/tests.md diff --git a/README.md b/README.md index a64cdd1a..167d08e0 100644 --- a/README.md +++ b/README.md @@ -18,7 +18,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal |--------------------------------------------------------------------|----------------------------------------------------------------------| | [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | | [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | -| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer | +| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | diff --git a/ROADMAP.md b/ROADMAP.md index e3592219..a52a1cb4 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -28,7 +28,6 @@ Difficulty legend: |----|-------------------------------------------------------|-------------------------|------------| | 55 | Polynomial (cubic/quintic) point-to-point profile | `motion_profiles` (new) | ★☆☆☆☆ | | 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | -| 57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | ★☆☆☆☆ | | 58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | @@ -75,15 +74,6 @@ the library does not yet expose. Detailed below under - **Reuses:** Item 55 types. - **Spec:** [roadmap/motion_profiles/TrapezoidalProfile](roadmap/motion_profiles/TrapezoidalProfile/implementation.md). -### 57. Friction compensation -- **What:** Feedforward Coulomb + viscous + Stribeck friction per axis with a `tanh` zero-crossing and an - output clamp. -- **Embedded value:** Removes stick-slip and tracking lag in geared drives and joints for one `exp` and one - `tanh` per axis. -- **Algorithm / paper:** B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, *Automatica*, 30(7), 1994. -- **Reuses:** `math::Vector`. -- **Spec:** [roadmap/controllers/FrictionCompensation](roadmap/controllers/FrictionCompensation/implementation.md). - ### 58. Symplectic integrators - **What:** Semi-implicit Euler and velocity Verlet for second-order systems `q̈ = a(q, q̇, u)`. - **Embedded value:** Bounded energy error for simulated mechanical plants at one or two evaluations per diff --git a/doc/controllers/FrictionCompensation.md b/doc/controllers/FrictionCompensation.md new file mode 100644 index 00000000..b30bd31d --- /dev/null +++ b/doc/controllers/FrictionCompensation.md @@ -0,0 +1,106 @@ +# Friction Compensation (Coulomb + Viscous + Stribeck) + +## Overview & Motivation + +Friction is the dominant unmodelled effect in geared drives, lead screws and robot joints. It causes stick-slip, tracking lag and steady-state error that a feedback loop can only remove by raising its gain, which erodes stability margins. Friction compensation instead predicts the drag from the axis velocity and adds the opposite torque (or force) to the command, so the feedback loop only has to correct the residual. + +The model superposes three effects: **Coulomb** drag (constant magnitude, opposing motion), **viscous** drag (proportional to speed) and **Stribeck** stiction (an extra breakaway level that fades as the axis starts to slide). The ideal curve is discontinuous at zero velocity, so a smooth boundary layer replaces the hard sign function. + +## Mathematical Theory + +### Per-Axis Model + +For one axis with velocity $v$, the compensation torque is: + +$$\tau = \operatorname{clamp}\!\Big(\big[F_c + (F_s - F_c)\,e^{-(|v|/v_s)^{\delta}}\big]\tanh\!\big(v/\varepsilon\big) + F_v\, v,\; -\tau_{max},\; \tau_{max}\Big)$$ + +- $F_c$: Coulomb level, the drag magnitude once sliding. +- $F_s \ge F_c$: stiction (breakaway) level at near-zero speed. +- $F_v$: viscous coefficient. +- $v_s > 0$: Stribeck velocity, the speed scale over which stiction decays into the Coulomb plateau. +- $\delta$: Stribeck shape exponent, typically 1 (exponential decay) or 2 (Gaussian decay). +- $\varepsilon > 0$: boundary-layer width replacing $\operatorname{sign}(v)$. +- $\tau_{max} > 0$: output cap. + +Axes are independent; a multi-axis compensator applies the model to each velocity component with its own parameters. + +### Structure + +The bracketed term is the friction **level** $L(|v|)$. It starts at $F_s$ for $v = 0$ and decays monotonically to $F_c$, which is why $F_s \ge F_c$ is required: otherwise the "Stribeck dip" would turn into a rise. The factor $\tanh(v/\varepsilon)$ supplies the direction, approximating $\operatorname{sign}(v)$ with slope $L/\varepsilon$ at the origin. The viscous term is added outside the boundary layer. + +### Feedforward Use + +The result is a feedforward term, not a feedback law. It is added to the controller output, $\tau_{cmd} = \tau_{control} + \tau$, and composes with any torque or current loop. Because it depends only on velocity, it does not enter the feedback characteristic equation. + +### Desired vs. Measured Velocity + +- **Desired velocity** gives a noise-free, deterministic command, but cannot see actual sticking, so it mis-compensates when the axis lags the trajectory. +- **Measured velocity** tracks the real motion, but passes sensor noise through the boundary-layer slope $\approx F_s/\varepsilon$, and closes an extra velocity-feedback path whose delay can excite limit cycles. + +In both cases $\varepsilon$ sets the sharpness-versus-chatter trade-off. + +### Identification + +For fixed $v_s$, $\delta$ and $\varepsilon$, $\tau$ is linear in the force coefficients: $F_c$, $F_s$ and $F_v$ multiply the known regressors $(1-e^{-(|v|/v_s)^{\delta}})\tanh(v/\varepsilon)$, $e^{-(|v|/v_s)^{\delta}}\tanh(v/\varepsilon)$ and $v$ respectively. They can therefore be estimated by linear least squares from logged velocity and torque data, jointly with other linear parameters. $v_s$ and $\delta$ require an outer search. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|------|------|-------|-------| +| All | $O(N)$ | $O(N)$ | $N$ axes; per axis one exponential, one power, one hyperbolic tangent, two divisions and a clamp; $N$ parameter sets stored, no allocation | + +Cost is dominated by the three transcendental calls; there is no iteration and no dependence on history, so the latency is constant per sample. + +## Step-by-Step Walkthrough + +Parameters: $F_c = 0.5$, $F_s = 0.8$, $F_v = 0.1$, $v_s = 0.05$, $\delta = 2$, $\varepsilon = 10^{-3}$, with a non-binding cap $\tau_{max} = 1$. + +| $v$ | $(\lvert v\rvert/v_s)^2$ | Level $L$ | $\tanh(v/\varepsilon)$ | $F_v v$ | $\tau$ | +|-----|---------------|-----------|------------------------|---------|--------| +| 1 | 400 | $0.5 + 0.3\,e^{-400} \approx 0.5$ | 1 | 0.1 | 0.6 | +| 0.01 | 0.04 | $0.5 + 0.3\,e^{-0.04} \approx 0.788$ | $\tanh 10 \approx 1$ | 0.001 | 0.789 | +| 0.05 ($=v_s$) | 1 | $0.5 + 0.3/e \approx 0.610$ | $\tanh 50 \approx 1$ | 0.005 | 0.615 | + +At high speed the Stribeck term has vanished and only Coulomb plus viscous drag remain. At $v = 0.01$ the axis is still deep in the Stribeck region, so the level is close to $F_s$. At $v = v_s$ the stiction excess has decayed to a fraction $1/e$ of its initial value. At $v = 0$ the output is exactly zero, and $v = -0.01$ gives $-0.789$ by symmetry. + +## Pitfalls & Edge Cases + +- **Boundary layer too narrow.** $\varepsilon \to 0$ recovers $\operatorname{sign}(v)$, so noise and direction reversals toggle the full $\pm L$ each sample, producing chattering and limit cycles around zero velocity. +- **Boundary layer too wide.** For $|v| \lesssim \varepsilon$ the factor $\tanh(v/\varepsilon)$ is well below 1 (0.76 at $|v| = \varepsilon$), so slow moves and the breakaway region are under-compensated. +- **Over-compensation is worse than under-compensation.** An inflated estimate injects energy and can drive the axis on its own, while a low one merely leaves residual error. The clamp at $\tau_{max}$ bounds this; it should still exceed the true drag at the maximum operating speed, or it truncates the viscous term. +- **Parameter validation.** $F_s \ge F_c$, $v_s > 0$, $\varepsilon > 0$ and $\tau_{max} > 0$ are checked at construction and a violation is a precondition failure. The shape exponent is not checked; it must be positive, since $\delta \le 0$ removes the decay or diverges at $v = 0$. +- **Symmetry.** The level depends on $|v|$ only, and $\tanh$, the viscous term and the symmetric clamp are all odd, so $\tau(-v) = -\tau(v)$ and $\tau(0) = 0$ exactly. Real drives often have direction-dependent friction, which this model cannot represent. +- **Float precision.** For large $|v|/v_s$ the exponential underflows to zero harmlessly, and $\tanh$ saturates to $\pm 1$; no cancellation arises. +- **Clamp breaks linearity.** Identification data must be taken where the clamp is inactive. + +## Variants & Generalizations + +| Variant | Key Difference | +|---------|----------------| +| **Coulomb + viscous only** | $F_s = F_c$ removes the Stribeck term; the level is constant | +| **Asymmetric friction** | Separate parameters per direction of motion; handles direction-dependent drag | +| **Karnopp model** | Replaces the smooth layer with a velocity dead-zone and a stick state | +| **Dahl / LuGre** | Dynamic models with internal bristle state; capture presliding and hysteresis at the cost of state and a stiffer ODE | +| **Inverse-dynamics term** | Same static model folded into a robot inverse-dynamics computation | + +## Applications + +- Robot joints and geared servo drives: cancelling joint friction in computed-torque and PD plus gravity controllers. +- Lead-screw and linear stages: reducing stick-slip in precision positioning. +- Gimbals and pan-tilt heads: smooth low-speed tracking. +- Torque or current-controlled actuators where friction dominates the low-speed error. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| [Feedforward / 2-DOF](Feedforward2Dof.md) | Same additive-feedforward structure; friction is a velocity-driven feedforward term | +| [PID Controller](Pid.md) | The feedback loop this term is added to; compensation reduces integrator work and stick-slip hunting | +| [Bang-Bang / Hysteresis](BangBangHysteresis.md) | The $\varepsilon \to 0$ limit is a relay on velocity, with the same chattering mechanism | +| [Saturation / Rate Limiter](SaturationRateLimiter.md) | The output clamp is the same saturation, applied to the compensation term alone | +| [QR Decomposition](../solvers/QrDecomposition.md) | Backbone of the least-squares fit of the linear friction coefficients | + +## References & Further Reading + +- B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, "A survey of models, analysis tools and compensation methods for the control of machines with friction," *Automatica*, 30(7), 1994. +- H. Olsson et al., "Friction models and friction compensation," *European Journal of Control*, 4(3), 1998. diff --git a/doc/controllers/README.md b/doc/controllers/README.md index 38af7689..815a4236 100644 --- a/doc/controllers/README.md +++ b/doc/controllers/README.md @@ -16,3 +16,4 @@ Feedback control algorithms for regulating dynamic systems in real time. | [Linear Time-Invariant Model](LinearTimeInvariant.md) | Discrete-time state-space plant model (A, B, C, D) shared across controllers and filters | | [Feedforward / 2-DOF Controller](Feedforward2Dof.md) | Feedforward + feedback wrapper for decoupled reference tracking and disturbance rejection | | [Luenberger Observer](LuenbergerObserver.md) | Deterministic state estimator using pole placement (Ackermann's formula) to reconstruct unmeasured states | +| [Friction Compensation](FrictionCompensation.md) | Coulomb + viscous + Stribeck feedforward with a tanh boundary layer and output clamp, per axis | diff --git a/numerical/controllers/implementations/CMakeLists.txt b/numerical/controllers/implementations/CMakeLists.txt index b15dffab..ddfbdfa4 100644 --- a/numerical/controllers/implementations/CMakeLists.txt +++ b/numerical/controllers/implementations/CMakeLists.txt @@ -15,6 +15,7 @@ target_sources(numerical.controllers.implementations PRIVATE BangBangHysteresis.hpp DeadbeatControl.hpp Feedforward2Dof.hpp + FrictionCompensation.hpp GainScheduledController.hpp IntegralStateFeedbackLqi.hpp LeadLagCompensator.hpp @@ -30,6 +31,7 @@ numerical_add_coverage_sources(numerical.controllers.implementations BangBangHysteresis.cpp DeadbeatControl.cpp Feedforward2Dof.cpp + FrictionCompensation.cpp GainScheduledController.cpp IntegralStateFeedbackLqi.cpp LeadLagCompensator.cpp diff --git a/numerical/controllers/implementations/FrictionCompensation.cpp b/numerical/controllers/implementations/FrictionCompensation.cpp new file mode 100644 index 00000000..86cd6722 --- /dev/null +++ b/numerical/controllers/implementations/FrictionCompensation.cpp @@ -0,0 +1,7 @@ +#include "numerical/controllers/implementations/FrictionCompensation.hpp" + +namespace controllers +{ + template class FrictionCompensation; + template class FrictionCompensation; +} diff --git a/numerical/controllers/implementations/FrictionCompensation.hpp b/numerical/controllers/implementations/FrictionCompensation.hpp new file mode 100644 index 00000000..ec7895f6 --- /dev/null +++ b/numerical/controllers/implementations/FrictionCompensation.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include +#include + +namespace controllers +{ + template + struct FrictionParameters + { + static_assert(std::is_floating_point_v, "FrictionParameters supports floating-point types only"); + + T coulomb{}; + T stiction{}; + T viscous{}; + T stribeckVelocity{}; + T stribeckShape{}; + T smoothingVelocity{}; + T maxCompensation{}; + }; + + template + class FrictionCompensation + { + static_assert(std::is_floating_point_v, "FrictionCompensation supports floating-point types only"); + static_assert(Axes > 0, "FrictionCompensation requires at least one axis"); + + public: + using Parameters = std::array, Axes>; + using AxisVector = math::Vector; + + explicit FrictionCompensation(const Parameters& params); + + OPTIMIZE_FOR_SPEED AxisVector Compute(const AxisVector& velocity) const; + OPTIMIZE_FOR_SPEED static T AxisTorque(const FrictionParameters& p, T velocity); + + private: + Parameters params; + }; + + template + FrictionCompensation::FrictionCompensation(const Parameters& params) + : params{ params } + { + for (const auto& p : params) + { + really_assert(p.stiction >= p.coulomb); + really_assert(p.stribeckVelocity > T{}); + really_assert(p.smoothingVelocity > T{}); + really_assert(p.maxCompensation > T{}); + } + } + + template + OPTIMIZE_FOR_SPEED auto FrictionCompensation::Compute(const AxisVector& velocity) const -> AxisVector + { + AxisVector torque{}; + for (std::size_t i = 0; i < Axes; ++i) + torque[i] = AxisTorque(params[i], velocity[i]); + + return torque; + } + + template + OPTIMIZE_FOR_SPEED T FrictionCompensation::AxisTorque(const FrictionParameters& p, T velocity) + { + const T fall{ math::Exp(-math::Pow(math::Abs(velocity) / p.stribeckVelocity, p.stribeckShape)) }; + const T level{ p.coulomb + (p.stiction - p.coulomb) * fall }; + const T torque{ level * math::Tanh(velocity / p.smoothingVelocity) + p.viscous * velocity }; + + return std::min(std::max(torque, -p.maxCompensation), p.maxCompensation); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class FrictionCompensation; + extern template class FrictionCompensation; +#endif +} diff --git a/numerical/controllers/implementations/test/CMakeLists.txt b/numerical/controllers/implementations/test/CMakeLists.txt index da40cfa2..9073af24 100644 --- a/numerical/controllers/implementations/test/CMakeLists.txt +++ b/numerical/controllers/implementations/test/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(numerical.controllers_test PRIVATE TestBangBangHysteresis.cpp TestDeadbeatControl.cpp TestFeedforward2Dof.cpp + TestFrictionCompensation.cpp TestGainScheduledController.cpp TestIntegralStateFeedbackLqi.cpp TestLeadLagCompensator.cpp diff --git a/numerical/controllers/implementations/test/TestFrictionCompensation.cpp b/numerical/controllers/implementations/test/TestFrictionCompensation.cpp new file mode 100644 index 00000000..e14d9915 --- /dev/null +++ b/numerical/controllers/implementations/test/TestFrictionCompensation.cpp @@ -0,0 +1,107 @@ +#include "numerical/controllers/implementations/FrictionCompensation.hpp" +#include +#include +#include + +namespace +{ + using Parameters = controllers::FrictionParameters; + using Axis = controllers::FrictionCompensation; + using ThreeAxes = controllers::FrictionCompensation; + + class TestFrictionCompensation : public ::testing::Test + { + protected: + Parameters p{ 0.5f, 0.8f, 0.1f, 0.05f, 2.0f, 1e-3f, 10.0f }; + Axis friction{ { p } }; + + float Torque(float velocity) const + { + return friction.Compute(Axis::AxisVector{ velocity })[0]; + } + }; +} + +TEST_F(TestFrictionCompensation, positive_velocity_positive_torque) +{ + const float tau{ Torque(1.0f) }; + + EXPECT_GT(tau, 0.0f); + EXPECT_NEAR(tau, p.coulomb + p.viscous * 1.0f, 1e-6f); +} + +TEST_F(TestFrictionCompensation, sign_flips_with_velocity) +{ + for (float velocity : { 1e-4f, 0.03f, 0.2f, 2.0f }) + EXPECT_FLOAT_EQ(Torque(velocity), -Torque(-velocity)); +} + +TEST_F(TestFrictionCompensation, viscous_dominates_at_high_speed) +{ + for (float velocity : { 2.0f, 4.0f, 6.0f }) + EXPECT_NEAR(Torque(velocity + 2.0f) - Torque(velocity), p.viscous * 2.0f, 1e-5f); +} + +TEST_F(TestFrictionCompensation, stiction_exceeds_coulomb_near_zero) +{ + const float velocity{ 10.0f * p.smoothingVelocity }; + const float fall{ std::exp(-std::pow(velocity / p.stribeckVelocity, p.stribeckShape)) }; + const float level{ p.coulomb + (p.stiction - p.coulomb) * fall }; + + const float stribeck{ Axis::AxisTorque(p, velocity) - p.viscous * velocity }; + + EXPECT_NEAR(stribeck, 0.788237f, 1e-5f); + EXPECT_NEAR(stribeck, level * std::tanh(10.0f), 1e-6f); + EXPECT_GT(stribeck, p.coulomb); +} + +TEST_F(TestFrictionCompensation, stribeck_decays_to_coulomb) +{ + const float velocity{ 10.0f * p.stribeckVelocity }; + + EXPECT_NEAR(Axis::AxisTorque(p, velocity) - p.viscous * velocity, p.coulomb, 1e-6f); +} + +TEST_F(TestFrictionCompensation, smooth_through_zero) +{ + EXPECT_EQ(Torque(0.0f), 0.0f); + EXPECT_NEAR(Torque(1e-6f), 0.0f, 1e-3f); + EXPECT_NEAR(Torque(-1e-6f), 0.0f, 1e-3f); +} + +TEST_F(TestFrictionCompensation, multi_axis_elementwise) +{ + const ThreeAxes::Parameters params{ p, + Parameters{ 0.2f, 0.2f, 0.05f, 0.1f, 1.0f, 1e-2f, 5.0f }, + Parameters{ 1.0f, 1.5f, 0.3f, 0.02f, 2.0f, 1e-3f, 5.0f } }; + const ThreeAxes friction3{ params }; + const ThreeAxes::AxisVector velocity{ 0.02f, -0.3f, 1.5f }; + + const auto tau{ friction3.Compute(velocity) }; + + for (std::size_t i = 0; i < 3; ++i) + EXPECT_FLOAT_EQ(tau[i], ThreeAxes::AxisTorque(params[i], velocity[i])); +} + +TEST_F(TestFrictionCompensation, feedforward_additive_cancels_model) +{ + constexpr float controlTorque{ 0.25f }; + + for (float velocity : { -0.5f, -0.01f, 0.03f, 0.7f }) + { + const float command{ controlTorque + Torque(velocity) }; + const float plantFriction{ -Axis::AxisTorque(p, velocity) }; + + EXPECT_NEAR(command + plantFriction, controlTorque, 1e-6f); + } +} + +TEST_F(TestFrictionCompensation, output_is_clamped_to_max_compensation) +{ + Parameters limited{ p }; + limited.maxCompensation = 0.6f; + const Axis clamped{ { limited } }; + + EXPECT_EQ(clamped.Compute(Axis::AxisVector{ 5.0f })[0], 0.6f); + EXPECT_EQ(clamped.Compute(Axis::AxisVector{ -5.0f })[0], -0.6f); +} diff --git a/roadmap/README.md b/roadmap/README.md index 4103ee08..0bf1965e 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -83,7 +83,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) ### `controllers` -`SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) · `FrictionCompensation` (57) +`SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) ### `motion_profiles` `PolynomialProfile` (55) · `TrapezoidalProfile` (56) · `SCurveProfile` (60) diff --git a/roadmap/controllers/FrictionCompensation/explanation.md b/roadmap/controllers/FrictionCompensation/explanation.md deleted file mode 100644 index 15b32c98..00000000 --- a/roadmap/controllers/FrictionCompensation/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Friction Compensation (Coulomb + Viscous + Stribeck) — Overview - -## What it is -A feedforward model of actuator friction — the sum of **Coulomb** (constant drag), **viscous** -(speed-proportional drag), and **Stribeck** (extra "stiction" that fades as the axis starts moving) -terms — evaluated from the axis velocity and added to a torque (or force) command to cancel real -friction. - -## Why it matters (embedded) -Friction is the dominant un-modeled effect in geared drives, lead screws and robot joints: it causes -stick-slip, tracking lag, and steady-state error that pure PD/PID struggles to remove. A few -multiplies, one `exp`, and one `tanh` per axis — evaluated in the control loop — recover much of that -lost accuracy without a larger, slower feedback gain that would risk instability. - -## How it works (intuition) -Plot friction torque against velocity and you get a characteristic curve: a tall "breakaway" spike -near zero speed (stiction), dropping into a Coulomb plateau, then rising linearly (viscous) as speed -increases. The model reproduces that curve. Because the ideal curve is discontinuous at zero -velocity (the `sign` flip), a smooth `tanh` boundary layer replaces the hard sign so the feedforward -command does not chatter as the axis reverses direction. - -## Key parameters -- **F_c (Coulomb)** — constant drag magnitude once sliding. -- **F_s (stiction)** — higher breakaway level at near-zero speed (`F_s ≥ F_c`). -- **F_v (viscous)** — drag proportional to velocity. -- **v_s (Stribeck velocity)** — how quickly stiction fades into the Coulomb plateau. -- **ε (smoothing width)** — trades zero-crossing smoothness against compensation sharpness. - -## Reference -B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, "A survey of models, analysis tools and -compensation methods for the control of machines with friction," *Automatica*, 30(7), 1994. - -## See also -`PidIncremental` and `Feedforward2Dof` (loops this term is added to); robotics-toolbox-cpp's PD + -gravity and computed-torque controllers, which add it to their joint torque command. diff --git a/roadmap/controllers/FrictionCompensation/implementation.md b/roadmap/controllers/FrictionCompensation/implementation.md deleted file mode 100644 index 5710dedd..00000000 --- a/roadmap/controllers/FrictionCompensation/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Friction Compensation (Coulomb + Viscous + Stribeck) — Implementation Pseudocode - -> Roadmap ref: #57 (Tier 1) · Target: `numerical/controllers/implementations` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct FrictionParameters: # one per axis (joint, motor, linear stage) - T coulomb # F_c — kinetic friction magnitude - T stiction # F_s — static/breakaway level (>= F_c) - T viscous # F_v — velocity-proportional coefficient - T stribeckVelocity # v_s — width of the Stribeck dip (> 0) - T stribeckShape # delta — exponent, typically 1 or 2 - T smoothingVelocity# eps — zero-crossing smoothing width (> 0) - T maxCompensation # |τ_f| cap — over-compensation can drive the axis (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -class FrictionCompensation: - std::array, Axes> params -``` - -## Interface - -```cpp -FrictionCompensation(const std::array, Axes>& params) - -# Feedforward friction torque (force, for a linear axis) to add to the control law: -Vector Compute(const Vector& velocity) const # hot path - -# Single-axis helper (also reused by the vector form): -static T AxisTorque(const FrictionParameters& p, T velocity) -``` - -## Algorithm (pseudocode) - -```text -function AxisTorque(p, v): # OPTIMIZE_FOR_SPEED - # Stribeck curve: stiction blends down to Coulomb as |v| grows - fall = exp( -(|v| / p.stribeckVelocity) ^ p.stribeckShape ) - level = p.coulomb + (p.stiction - p.coulomb) * fall - # smooth sign (tanh boundary layer) avoids chattering at v = 0 - dir = tanh(v / p.smoothingVelocity) - return clamp(level * dir + p.viscous * v, −p.maxCompensation, p.maxCompensation) - -function Compute(velocity): # OPTIMIZE_FOR_SPEED - for i in 0 .. Axes-1: - tau[i] = AxisTorque(params[i], velocity[i]) - return tau -``` - -## Complexity & memory - -- `Compute`: `O(Axes)` — one `exp` and one `tanh` per axis, no allocation. -- Memory: `O(Axes)` parameter structs, all static/stack-resident. - -## Numerical / embedded notes - -- **Feedforward, not feedback:** add the result to the controller output - (`τ_cmd = τ_control + Compute(v)`); it composes with any torque/current loop — PID, a FOC current - reference, or robotics-toolbox-cpp's PD + gravity and computed-torque controllers. -- Feed **desired** velocity for a noise-free command, or **measured** velocity for accuracy — the - smoothing width `eps` trades tracking sharpness against chattering either way. -- The ideal `sign(v)` is replaced by `tanh(v/eps)`: too small an `eps` reintroduces limit cycles - near zero velocity; too large under-compensates stiction on slow moves. -- Over-compensation is worse than under (it can drive the axis), so the output is clamped to - `maxCompensation`; validate `stiction >= coulomb`, `stribeckVelocity > 0`, `smoothingVelocity > 0` - and `maxCompensation > 0` at construction. -- The same model can be folded into a plant model (robotics-toolbox-cpp adds it to its chain inverse - dynamics), and its parameters are linear in `(F_c, F_v)` for fixed `v_s`, so they can be identified by - least squares together with other linear parameters (`IncrementalQrLeastSquares`, #63). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/controllers/implementations/FrictionCompensation.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`/`AxisTorque`, and - `extern template class FrictionCompensation;` / `` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/implementations/FrictionCompensation.cpp` → - `template class FrictionCompensation;` and ``. -- Test: `numerical/controllers/implementations/test/TestFrictionCompensation.cpp` -- Doc: `doc/controllers/FrictionCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources(numerical.controllers.implementations ...)`; - `TestFrictionCompensation.cpp` → `numerical.controllers_test`. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/controllers/FrictionCompensation/tests.md b/roadmap/controllers/FrictionCompensation/tests.md deleted file mode 100644 index b5da24d8..00000000 --- a/roadmap/controllers/FrictionCompensation/tests.md +++ /dev/null @@ -1,66 +0,0 @@ -# Friction Compensation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestFrictionCompensation : public ::testing::Test: - # single-axis params: Fc=0.5, Fs=0.8, Fv=0.1, vs=0.05, delta=2, eps=1e-3, maxCompensation=10 - FrictionParameters p = MakeParams() - FrictionCompensation friction{ {p} } -# each case below is a TEST_F(TestFrictionCompensation, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -positive_velocity_positive_torque: - Arrange: velocity = +1.0 - Act: tau = Compute(velocity) - Assert: tau[0] > 0 and tau[0] ≈ Fc + Fv*1.0 (dip negligible at high v) - -sign_flips_with_velocity: - Arrange: compare Compute(+v) and Compute(-v) - Assert: tau(+v) ≈ -tau(-v) (odd symmetry) - -viscous_dominates_at_high_speed: - Arrange: sweep large |velocity| - Assert: tau grows ~linearly with slope Fv - -stiction_exceeds_coulomb_near_zero: - Arrange: v = 10·eps (tanh ≈ 1) and v << v_s - Assert: AxisTorque(p, v) − Fv·v ≈ Fs (Stribeck peak) - -stribeck_decays_to_coulomb: - Arrange: v >> v_s - Assert: AxisTorque(p, v) − Fv·v → Fc within tol - -smooth_through_zero: - Arrange: v = 0 - Assert: tau[0] == 0 (tanh(0)=0), continuous, no jump - -multi_axis_elementwise: - Arrange: FrictionCompensation, distinct params, velocity vector - Assert: each output equals its per-axis AxisTorque - -feedforward_additive_cancels_model: - Arrange: plant friction torque = −model(v) (opposes motion); command = τ_control + Compute(v) - Assert: net torque reaching the load ≈ τ_control (compensation cancels) -output_is_clamped_to_max_compensation: - Arrange: maxCompensation = 0.6, v large - Assert: |Compute(v)| == 0.6 -``` - -## Reference vectors - -- High speed `v=1`, dip≈0: `tau ≈ Fc·tanh(1/eps) + Fv·1 ≈ Fc + Fv = 0.6` (fixture `maxCompensation = 10`). -- `v=0`: `tau = 0` exactly (odd, smoothed). -- `v = v_s`: `fall = e^{-1}`, `level = Fc + (Fs−Fc)/e`. - -## Edge cases - -- `velocity = 0` — no torque, no NaN from the `^delta` power. -- Very small `eps` — steep but finite slope; assert no overflow. -- `Fs == Fc` — Stribeck term vanishes, pure Coulomb+viscous. -- Negative-going zero crossing — output stays continuous (no chatter). From 09091e8ca77baf3b9e0fdd18576cadac665dedbc Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:16:08 +0000 Subject: [PATCH 5/7] feat(solvers): add symplectic integrators for second-order systems MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 58: semi-implicit Euler and velocity Verlet (kick-drift-kick) over a new SecondOrderSystem interface. For a conservative plant the energy error stays bounded instead of drifting. The second-order check compares h = 0.1 with h = 0.05 rather than the spec's unspecified small step: at h = 0.01 float32 rounding already dominates the truncation error and the error ratio reads ≈ 5, not 4. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 9 -- doc/solvers/README.md | 1 + doc/solvers/SymplecticIntegrators.md | 112 +++++++++++++++ numerical/solvers/CMakeLists.txt | 3 + numerical/solvers/SecondOrderSystem.hpp | 21 +++ numerical/solvers/SymplecticIntegrators.cpp | 7 + numerical/solvers/SymplecticIntegrators.hpp | 91 ++++++++++++ numerical/solvers/test/CMakeLists.txt | 1 + .../test/TestSymplecticIntegrators.cpp | 135 ++++++++++++++++++ roadmap/README.md | 2 +- .../SymplecticIntegrators/explanation.md | 31 ---- .../SymplecticIntegrators/implementation.md | 95 ------------ .../solvers/SymplecticIntegrators/tests.md | 45 ------ 14 files changed, 373 insertions(+), 182 deletions(-) create mode 100644 doc/solvers/SymplecticIntegrators.md create mode 100644 numerical/solvers/SecondOrderSystem.hpp create mode 100644 numerical/solvers/SymplecticIntegrators.cpp create mode 100644 numerical/solvers/SymplecticIntegrators.hpp create mode 100644 numerical/solvers/test/TestSymplecticIntegrators.cpp delete mode 100644 roadmap/solvers/SymplecticIntegrators/explanation.md delete mode 100644 roadmap/solvers/SymplecticIntegrators/implementation.md delete mode 100644 roadmap/solvers/SymplecticIntegrators/tests.md diff --git a/README.md b/README.md index 167d08e0..62867d40 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan) | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | diff --git a/ROADMAP.md b/ROADMAP.md index a52a1cb4..ead769cb 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -28,7 +28,6 @@ Difficulty legend: |----|-------------------------------------------------------|-------------------------|------------| | 55 | Polynomial (cubic/quintic) point-to-point profile | `motion_profiles` (new) | ★☆☆☆☆ | | 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | -| 58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | | 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | @@ -74,14 +73,6 @@ the library does not yet expose. Detailed below under - **Reuses:** Item 55 types. - **Spec:** [roadmap/motion_profiles/TrapezoidalProfile](roadmap/motion_profiles/TrapezoidalProfile/implementation.md). -### 58. Symplectic integrators -- **What:** Semi-implicit Euler and velocity Verlet for second-order systems `q̈ = a(q, q̇, u)`. -- **Embedded value:** Bounded energy error for simulated mechanical plants at one or two evaluations per - step (RK4 needs four and still drifts). -- **Algorithm / paper:** E. Hairer, C. Lubich, G. Wanner, *Geometric Numerical Integration* (2006). -- **Reuses:** `math::Vector`; complements `RungeKuttaIntegrators`. -- **Spec:** [roadmap/solvers/SymplecticIntegrators](roadmap/solvers/SymplecticIntegrators/implementation.md). - ## Tier 2 — Easy ★★☆☆☆ ### 53. SE(3) transform, twists and wrenches diff --git a/doc/solvers/README.md b/doc/solvers/README.md index d88a4c6d..6d2c5cf0 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -18,3 +18,4 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. | [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | | [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | | [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | +| [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | diff --git a/doc/solvers/SymplecticIntegrators.md b/doc/solvers/SymplecticIntegrators.md new file mode 100644 index 00000000..4522dbfe --- /dev/null +++ b/doc/solvers/SymplecticIntegrators.md @@ -0,0 +1,112 @@ +# Symplectic Integrators (Semi-Implicit Euler + Velocity Verlet) + +## Overview & Motivation + +Mechanical plants (robot arms, pendulums, drones, a motor driving a load) are naturally second-order systems: $\ddot{q} = a(q, \dot{q}, u)$, with acceleration computed from position, velocity and input. They are simulated before hardware exists, so controllers can be tested in model-in-the-loop runs or run on-target as observers. + +A general-purpose integrator treats this as a first-order system and ignores its structure. Over a long run the simulated energy creeps up or down, so a lightly damped plant looks more (or less) stable than it really is. **Symplectic** integrators advance position and velocity in an interleaved order that respects the geometry of mechanics. For a conservative system the energy error stays bounded forever, at one or two acceleration evaluations per step — fewer than the four of RK4. + +Two fixed-step methods are provided: **semi-implicit (symplectic) Euler**, first order and one evaluation, and **velocity Verlet** in kick–drift–kick form, second order and two evaluations. + +## Mathematical Theory + +### Problem Statement + +State $(q, v)$ with $v = \dot{q}$. The input $u$ is held constant over the step (zero-order hold), and the step size $h > 0$ is fixed. + +### Semi-Implicit Euler + +$$ +v^+ = v + h\,a(q, v, u), \qquad q^+ = q + h\,v^+ +$$ + +The only change from explicit Euler is that the position update uses the *new* velocity. Local error is $O(h^2)$, global error $O(h)$. + +### Velocity Verlet (Kick–Drift–Kick) + +$$ +v_{1/2} = v + \tfrac{h}{2}\,a(q, v, u), \qquad q^+ = q + h\,v_{1/2}, \qquad v^+ = v_{1/2} + \tfrac{h}{2}\,a(q^+, v_{1/2}, u) +$$ + +The step is symmetric in time, which cancels the odd error terms: global error $O(h^2)$. For constant acceleration it is exact, since $v^+ = v + h\,a$ and $q^+ = q + h\,v + \tfrac{h^2}{2}a$. Semi-implicit Euler is not: it gives $q^+ = q + h\,v + h^2 a$. + +### Why Energy Stays Bounded + +For a position-only force, $a = -\nabla U(q)$, each method is a composition of shears (a velocity update depending on $q$, a position update depending on $v$), each with unit Jacobian determinant. The step map therefore preserves phase-space area and is *symplectic*. Backward error analysis then shows the numerical trajectory is the exact trajectory of a nearby *modified* energy $\tilde{H} = H + O(h^p)$, so the true energy oscillates around a constant instead of drifting. + +### Unit Oscillator + +For $a = -\omega^2 q$ and $\theta = h\omega$, both methods have a step matrix with determinant $1$ and trace $2 - \theta^2$. The eigenvalues stay on the unit circle, hence the trajectory is bounded, exactly when $|2 - \theta^2| < 2$, i.e. + +$$ +h\,\omega < 2 . +$$ + +The conserved modified energies and the resulting error of the true energy $E = \tfrac12(v^2 + \omega^2 q^2)$ are: + +| Method | Conserved quantity ($\omega = 1$) | Relative energy error | +|--------|-----------------------------------|-----------------------| +| Semi-implicit Euler | $\tfrac12\left(v^2 + q^2 - h\,q\,v\right)$ | oscillates, amplitude $\approx h\omega/2$ | +| Velocity Verlet | $\tfrac12\left(v^2 + (1 - \tfrac{\theta^2}{4})\,q^2\right)$ | oscillates, amplitude $\approx (h\omega)^2/8$ | + +### Velocity-Dependent Forces + +With damping or Coriolis terms the system is not Hamiltonian, so neither method is exactly symplectic. Verlet evaluates the second kick at the half-step velocity $v_{1/2}$, which keeps second-order accuracy. Semi-implicit Euler evaluates its single acceleration at the old velocity. Energy decay is then physical dissipation, not a numerical artifact. + +## Complexity Analysis + +| Method | Acceleration evaluations | Order | Arithmetic | Memory | +|--------|--------------------------|-------|------------|--------| +| Semi-implicit Euler | 1 | 1 | $O(n)$ | two state copies | +| Velocity Verlet | 2 | 2 | $O(n)$ | two state copies | + +Here $n$ is the number of generalized coordinates. The cost of a step is the cost of its evaluations plus a handful of vector operations; no heap is used. Each step is a pure function of its arguments; the second Verlet evaluation is not reused as the next step's first. Every evaluation goes through a virtual call: fine for simulation and model-in-the-loop tests; for interrupt-service use, bind the concrete system type instead. + +## Step-by-Step Walkthrough + +Unit oscillator $a = -q$, $h = 0.1$, start $(q, v) = (1, 0)$, true energy $E = 0.5$. + +**Semi-implicit Euler** + +| Step | Velocity update | Position update | True $E$ | Modified energy | +|------|-----------------|-----------------|----------|-----------------| +| 1 | $0 + 0.1(-1) = -0.1$ | $1 + 0.1(-0.1) = 0.99$ | $0.4950$ | $0.5000$ | +| 2 | $-0.1 + 0.1(-0.99) = -0.199$ | $0.99 + 0.1(-0.199) = 0.9701$ | $0.4903$ | $0.5000$ | + +The true energy wanders (about $1\%$ and $2\%$ low), yet the modified energy is conserved exactly; the offset is $\tfrac12 h\,q\,v$, which cannot grow beyond $h/4$. + +**Velocity Verlet**, one step: half kick $v_{1/2} = 0 + 0.05(-1) = -0.05$; drift $q^+ = 1 + 0.1(-0.05) = 0.995$; half kick with the new position $v^+ = -0.05 + 0.05(-0.995) = -0.09975$. Exact values are $\cos 0.1 = 0.995004$ and $-\sin 0.1 = -0.099833$. The true energy is $0.49999$, and the modified energy $\tfrac12\left(v^2 + 0.9975\,q^2\right) = 0.49875$ equals its initial value. + +## Pitfalls & Edge Cases + +- **Step size.** Both methods require $h\omega < 2$ for the fastest mode; beyond that the iteration diverges. The step size must be positive, and a non-positive value is rejected at construction. +- **Not for stiff or dissipative-dominated plants.** Both methods are explicit. Strong damping or stiffness needs an implicit method or a much smaller step. +- **Conservative forces only for the guarantee.** Velocity-dependent forces and discontinuities (contact, saturation, friction) break symplecticity; the energy bound is lost, though the order of accuracy remains. +- **Single-precision floor.** Rounding bounds the achievable accuracy. At $h = 0.01$ the Verlet error ratio between $h$ and $h/2$ departs from the ideal $4$, while at $h = 0.1$ versus $0.05$ it is $4.00$. Halving the step below the rounding floor buys nothing. +- **Measured energy error.** Over $10^5$ steps at $h = 0.01$ on the unit oscillator, the maximum relative energy error is $0.51\%$ for semi-implicit Euler (matching $h\omega/2$) and $4\times10^{-5}$ for Verlet, of the same order as $(h\omega)^2/8$ with a single-precision rounding contribution. + +## Variants & Generalizations + +| Variant | Key difference | +|---------|----------------| +| **Semi-implicit Euler, velocity Verlet (this)** | Symplectic, orders 1 and 2; explicit Euler differs only in using the old velocity for the drift and its energy grows | +| **Higher-order composition** | Chains of Verlet steps with weights (Yoshida, Suzuki) reach orders 4, 6, ... | +| **Implicit midpoint / Gauss** | Symplectic and implicit; suited to stiff or non-separable systems at the cost of a nonlinear solve | + +## Applications + +- **Plant simulation** — long model-in-the-loop runs of mechanical plants, where numerical energy drift would corrupt stability margins. +- **Lightly damped dynamics** — pendulums, flexible links, motor plus elastic load, whose true damping must not be masked by numerical damping. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| Runge–Kutta 4 | Four evaluations per step, fourth order, general first-order systems; energy drifts slowly on conservative problems | +| Dormand–Prince | Adaptive step and error control for first-order systems; higher accuracy per step, no structure preservation | + +## References & Further Reading + +- Hairer, E., Lubich, C., and Wanner, G., "Geometric numerical integration illustrated by the Störmer–Verlet method," *Acta Numerica*, 12:399–450, 2003. +- Hairer, E., Lubich, C., and Wanner, G., *Geometric Numerical Integration*, 2nd ed., Springer, 2006 — Chapters I and VI. +- Verlet, L., "Computer experiments on classical fluids," *Physical Review*, 159(1):98–103, 1967. diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index d17c569f..1e5bcf74 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -23,9 +23,11 @@ target_sources(numerical.solver PRIVATE OdeSystem.hpp QrDecomposition.hpp RungeKuttaIntegrators.hpp + SecondOrderSystem.hpp SingularValueDecomposition.hpp Solver.hpp SpectralRadius.hpp + SymplecticIntegrators.hpp ) numerical_add_coverage_sources(numerical.solver @@ -40,6 +42,7 @@ numerical_add_coverage_sources(numerical.solver RungeKuttaIntegrators.cpp SingularValueDecomposition.cpp SpectralRadius.cpp + SymplecticIntegrators.cpp ) add_subdirectory(test) diff --git a/numerical/solvers/SecondOrderSystem.hpp b/numerical/solvers/SecondOrderSystem.hpp new file mode 100644 index 00000000..070d85f6 --- /dev/null +++ b/numerical/solvers/SecondOrderSystem.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace solvers +{ + template + class SecondOrderSystem + { + static_assert(std::is_floating_point_v, "SecondOrderSystem supports floating-point types only"); + + public: + virtual ~SecondOrderSystem() = default; + [[nodiscard]] virtual math::Vector Acceleration( + const math::Vector& q, + const math::Vector& qDot, + const math::Vector& u) = 0; + }; +} diff --git a/numerical/solvers/SymplecticIntegrators.cpp b/numerical/solvers/SymplecticIntegrators.cpp new file mode 100644 index 00000000..f67ec2aa --- /dev/null +++ b/numerical/solvers/SymplecticIntegrators.cpp @@ -0,0 +1,7 @@ +#include "numerical/solvers/SymplecticIntegrators.hpp" + +namespace solvers +{ + template class SemiImplicitEuler; + template class VelocityVerlet; +} diff --git a/numerical/solvers/SymplecticIntegrators.hpp b/numerical/solvers/SymplecticIntegrators.hpp new file mode 100644 index 00000000..df0f34f2 --- /dev/null +++ b/numerical/solvers/SymplecticIntegrators.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/SecondOrderSystem.hpp" +#include +#include + +namespace solvers +{ + template + struct SecondOrderState + { + static_assert(std::is_floating_point_v, "SecondOrderState supports floating-point types only"); + + math::Vector q{}; + math::Vector qDot{}; + }; + + template + class SemiImplicitEuler + { + static_assert(std::is_floating_point_v, "SemiImplicitEuler supports floating-point types only"); + + public: + using State = SecondOrderState; + using InputVector = math::Vector; + + SemiImplicitEuler(SecondOrderSystem& system, T h); + OPTIMIZE_FOR_SPEED State Step(const State& x, const InputVector& u); + + private: + SecondOrderSystem& system; + T h; + }; + + template + class VelocityVerlet + { + static_assert(std::is_floating_point_v, "VelocityVerlet supports floating-point types only"); + + public: + using State = SecondOrderState; + using InputVector = math::Vector; + + VelocityVerlet(SecondOrderSystem& system, T h); + OPTIMIZE_FOR_SPEED State Step(const State& x, const InputVector& u); + + private: + SecondOrderSystem& system; + T h; + }; + + template + SemiImplicitEuler::SemiImplicitEuler(SecondOrderSystem& system, T h) + : system{ system } + , h{ h } + { + really_assert(h > T{}); + } + + template + OPTIMIZE_FOR_SPEED auto SemiImplicitEuler::Step(const State& x, const InputVector& u) -> State + { + const math::Vector qDot{ x.qDot + system.Acceleration(x.q, x.qDot, u) * h }; + return { x.q + qDot * h, qDot }; + } + + template + VelocityVerlet::VelocityVerlet(SecondOrderSystem& system, T h) + : system{ system } + , h{ h } + { + really_assert(h > T{}); + } + + template + OPTIMIZE_FOR_SPEED auto VelocityVerlet::Step(const State& x, const InputVector& u) -> State + { + const T halfStep{ h / T{ 2 } }; + const math::Vector qDotHalf{ x.qDot + system.Acceleration(x.q, x.qDot, u) * halfStep }; + const math::Vector q{ x.q + qDotHalf * h }; + return { q, qDotHalf + system.Acceleration(q, qDotHalf, u) * halfStep }; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class SemiImplicitEuler; + extern template class VelocityVerlet; +#endif +} diff --git a/numerical/solvers/test/CMakeLists.txt b/numerical/solvers/test/CMakeLists.txt index 05041974..dfc37bb8 100644 --- a/numerical/solvers/test/CMakeLists.txt +++ b/numerical/solvers/test/CMakeLists.txt @@ -22,4 +22,5 @@ target_sources(numerical.solvers_test PRIVATE TestRungeKuttaIntegrators.cpp TestSingularValueDecomposition.cpp TestSpectralRadius.cpp + TestSymplecticIntegrators.cpp ) diff --git a/numerical/solvers/test/TestSymplecticIntegrators.cpp b/numerical/solvers/test/TestSymplecticIntegrators.cpp new file mode 100644 index 00000000..74ff4799 --- /dev/null +++ b/numerical/solvers/test/TestSymplecticIntegrators.cpp @@ -0,0 +1,135 @@ +#include "numerical/solvers/SymplecticIntegrators.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include +#include + +namespace +{ + using Vector1 = math::Vector; + using State = solvers::SecondOrderState; + + MATCHER_P(ScalarNear, expected, "") + { + return std::abs(arg[0] - expected) <= 1e-6f; + } + + class HarmonicOscillator + : public solvers::SecondOrderSystem + { + public: + explicit HarmonicOscillator(float omega) + : omega{ omega } + {} + + Vector1 Acceleration(const Vector1& q, const Vector1&, const Vector1& u) override + { + return Vector1{ -omega * omega * q[0] + u[0] }; + } + + private: + float omega; + }; + + class MockSecondOrderSystem + : public solvers::SecondOrderSystem + { + public: + MOCK_METHOD(Vector1, Acceleration, (const Vector1& q, const Vector1& qDot, const Vector1& u), (override)); + }; + + class TestSymplecticIntegrators + : public ::testing::Test + { + protected: + HarmonicOscillator oscillator{ 1.0f }; + testing::StrictMock system; + solvers::SemiImplicitEuler euler{ oscillator, 0.01f }; + solvers::VelocityVerlet verlet{ oscillator, 0.01f }; + const Vector1 noInput{ 0.0f }; + + static float Energy(const State& x) + { + return 0.5f * (x.qDot[0] * x.qDot[0] + x.q[0] * x.q[0]); + } + + template + float MaxRelativeEnergyError(Integrator& integrator, int steps) const + { + State x{ Vector1{ 1.0f }, Vector1{ 0.0f } }; + const float initialEnergy{ Energy(x) }; + float maxError{ 0.0f }; + + for (int i = 0; i < steps; ++i) + { + x = integrator.Step(x, noInput); + maxError = std::max(maxError, std::abs(Energy(x) - initialEnergy) / initialEnergy); + } + + return maxError; + } + + float VerletPositionErrorAtOneSecond(float h) + { + solvers::VelocityVerlet integrator{ oscillator, h }; + State x{ Vector1{ 1.0f }, Vector1{ 0.0f } }; + const int steps{ static_cast(std::lround(1.0f / h)) }; + + for (int i = 0; i < steps; ++i) + x = integrator.Step(x, noInput); + + return std::abs(x.q[0] - std::cos(1.0f)); + } + }; +} + +TEST_F(TestSymplecticIntegrators, semi_implicit_euler_updates_position_with_new_velocity) +{ + solvers::SemiImplicitEuler integrator{ system, 0.1f }; + EXPECT_CALL(system, Acceleration(ScalarNear(1.0f), ScalarNear(0.0f), ScalarNear(0.0f))).WillOnce(testing::Return(Vector1{ 2.0f })); + + const State next{ integrator.Step(State{ Vector1{ 1.0f }, Vector1{ 0.0f } }, noInput) }; + + EXPECT_NEAR(next.q[0], 1.02f, 1e-6f); + EXPECT_NEAR(next.qDot[0], 0.2f, 1e-6f); +} + +TEST_F(TestSymplecticIntegrators, velocity_verlet_evaluates_at_half_step_velocity) +{ + solvers::VelocityVerlet integrator{ system, 0.1f }; + testing::InSequence sequence; + EXPECT_CALL(system, Acceleration(ScalarNear(1.0f), ScalarNear(0.0f), ScalarNear(0.0f))).WillOnce(testing::Return(Vector1{ 2.0f })); + EXPECT_CALL(system, Acceleration(ScalarNear(1.01f), ScalarNear(0.1f), ScalarNear(0.0f))).WillOnce(testing::Return(Vector1{ 2.0f })); + + const State next{ integrator.Step(State{ Vector1{ 1.0f }, Vector1{ 0.0f } }, noInput) }; + + EXPECT_NEAR(next.q[0], 1.01f, 1e-6f); + EXPECT_NEAR(next.qDot[0], 0.2f, 1e-6f); +} + +TEST_F(TestSymplecticIntegrators, velocity_verlet_is_exact_for_constant_acceleration) +{ + HarmonicOscillator free{ 0.0f }; + solvers::VelocityVerlet integrator{ free, 0.01f }; + const Vector1 thrust{ 3.0f }; + State x{}; + + for (int i = 0; i < 100; ++i) + x = integrator.Step(x, thrust); + + EXPECT_NEAR(x.q[0], 1.5f, 1e-4f); + EXPECT_NEAR(x.qDot[0], 3.0f, 1e-4f); +} + +TEST_F(TestSymplecticIntegrators, energy_error_stays_bounded_over_long_runs) +{ + EXPECT_LT(MaxRelativeEnergyError(euler, 100000), 0.006f); + EXPECT_LT(MaxRelativeEnergyError(verlet, 100000), 1e-4f); +} + +TEST_F(TestSymplecticIntegrators, verlet_is_second_order) +{ + const float ratio{ VerletPositionErrorAtOneSecond(0.1f) / VerletPositionErrorAtOneSecond(0.05f) }; + + EXPECT_NEAR(ratio, 4.0f, 0.4f); +} diff --git a/roadmap/README.md b/roadmap/README.md index 0bf1965e..c28cde8b 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -113,4 +113,4 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) ### `solvers` -`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `SymplecticIntegrators` (58) · `DampedPseudoInverse` (62) · `ActiveSetQp` (65) +`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `DampedPseudoInverse` (62) · `ActiveSetQp` (65) diff --git a/roadmap/solvers/SymplecticIntegrators/explanation.md b/roadmap/solvers/SymplecticIntegrators/explanation.md deleted file mode 100644 index 1e7054ee..00000000 --- a/roadmap/solvers/SymplecticIntegrators/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Symplectic Integrators — Overview - -## What it is -Two fixed-step methods for mechanical systems written as "acceleration from position, velocity and -input": semi-implicit (symplectic) Euler and velocity Verlet. They advance position and velocity in an -interleaved way that respects the geometry of mechanics. - -## Why it matters (embedded) -Simulated plants are how controllers get tested before hardware: robot arms, pendulums, drones, motor -plus load. With a general-purpose integrator the simulated energy slowly creeps up or down, so a -lightly damped plant appears more (or less) stable than it is after a long run. Symplectic methods keep -the energy error bounded forever at the cost of one or two acceleration evaluations per step — cheaper -than RK4's four. - -## How it works (intuition) -Semi-implicit Euler first updates the velocity from the current acceleration, then moves the position -with that *new* velocity; the tiny change from explicit Euler turns a spiralling phase portrait into a -closed one. Velocity Verlet gives the velocity half a kick, drifts the position a full step, then gives -the second half-kick with the acceleration at the new position — symmetric in time, hence second order. - -## Key parameters -- **Step size `h`** — accuracy and stability (stable for `h·ω < 2` on an oscillator of frequency `ω`). -- **Method** — Euler (one evaluation, first order) or Verlet (two evaluations, second order). - -## Reference -E. Hairer, C. Lubich, G. Wanner, *Geometric Numerical Integration*, 2nd ed. (2006), Ch. I and VI; -L. Verlet, *Physical Review*, 159(1), 1967. - -## See also -`RungeKuttaIntegrators` and `DormandPrince45` (first-order systems, higher accuracy per step); -robotics-toolbox-cpp's forward-dynamics integrator, which binds these to its articulated-body dynamics. diff --git a/roadmap/solvers/SymplecticIntegrators/implementation.md b/roadmap/solvers/SymplecticIntegrators/implementation.md deleted file mode 100644 index 0db1c3c1..00000000 --- a/roadmap/solvers/SymplecticIntegrators/implementation.md +++ /dev/null @@ -1,95 +0,0 @@ -# Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) — Implementation Pseudocode - -> Roadmap ref: #58 (Tier 1) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Fixed-step integrators for second-order systems `q̈ = a(q, q̇, u)` — mechanical plants (robot arms, -pendulums, drones, motor + load) simulated in model-in-the-loop tests or run as on-target observers. -Unlike the existing `RungeKutta4`/`DormandPrince45`, which integrate a first-order `OdeSystem`, these -exploit the position/velocity split: semi-implicit Euler and velocity Verlet keep the energy error of a -conservative system bounded instead of drifting. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v) -class SecondOrderSystem: # pure interface - virtual ~SecondOrderSystem() = default - virtual math::Vector Acceleration(const math::Vector& q, - const math::Vector& qDot, - const math::Vector& u) = 0 - -template -struct SecondOrderState: - math::Vector q, qDot - -template # instantiated for float -class SemiImplicitEuler: # also called symplectic Euler - SecondOrderSystem& system - T h # step size > 0 - -template -class VelocityVerlet: # Störmer–Verlet, kick-drift-kick form - SecondOrderSystem& system - T h -``` - -## Interface - -```cpp -SemiImplicitEuler(SecondOrderSystem& system, T h) -SecondOrderState Step(const SecondOrderState& x, const InputVector& u) # hot path - -VelocityVerlet(SecondOrderSystem& system, T h) -SecondOrderState Step(const SecondOrderState& x, const InputVector& u) # hot path -``` - -`u` is held constant over the step (zero-order hold), matching `RungeKutta4::Step`. - -## Algorithm (pseudocode) - -```text -function SemiImplicitEuler::Step(x, u): # OPTIMIZE_FOR_SPEED — one evaluation - qDot = x.qDot + h·system.Acceleration(x.q, x.qDot, u) - q = x.q + h·qDot # uses the NEW velocity — the symplectic part - return { q, qDot } - -function VelocityVerlet::Step(x, u): # OPTIMIZE_FOR_SPEED — two evaluations - vHalf = x.qDot + (h/2)·system.Acceleration(x.q, x.qDot, u) # kick - q = x.q + h·vHalf # drift - qDot = vHalf + (h/2)·system.Acceleration(q, vHalf, u) # kick - return { q, qDot } -``` - -## Complexity & memory - -- Semi-implicit Euler: one `Acceleration` call and `O(Dimension)` arithmetic per step. -- Velocity Verlet: two `Acceleration` calls per step (the second could be reused as the next step's - first only while `u` is unchanged — not done, to keep `Step` pure). -- Memory: two state copies; no heap. - -## Numerical / embedded notes - -- **Order and energy:** semi-implicit Euler is first order, velocity Verlet second order; for a - conservative, position-only acceleration both are symplectic, so the energy error stays bounded for - all time (no secular drift), unlike explicit Euler (grows) or RK4 (slow drift). -- With velocity-dependent forces (damping, Coriolis) neither method is exactly symplectic; Verlet - evaluates them at the half-step velocity, which keeps second order. Dissipation is then physical, not - numerical. -- Stability: for an oscillator of angular frequency `ω`, both are stable for `h·ω < 2`. -- The interface call is virtual; acceptable off the hard real-time path (simulation, MIL tests). Bind - the concrete system type where the step runs in an ISR. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/solvers/SymplecticIntegrators.hpp` (+ `numerical/solvers/SecondOrderSystem.hpp`, pure - interface) — `#pragma once`, `OPTIMIZE_FOR_SPEED` on both `Step`s, and - `extern template class SemiImplicitEuler;` / `VelocityVerlet` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/solvers/SymplecticIntegrators.cpp` → the same instantiations. -- Test: `numerical/solvers/test/TestSymplecticIntegrators.cpp` -- Doc: `doc/solvers/SymplecticIntegrators.md` (per `doc/TEMPLATE.md`) + row in `doc/solvers/README.md` -- CMake: `.hpp` → `target_sources(numerical.solver PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.solver ...)`; `TestSymplecticIntegrators.cpp` → - `numerical.solvers_test`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/SymplecticIntegrators/tests.md b/roadmap/solvers/SymplecticIntegrators/tests.md deleted file mode 100644 index 93b852e6..00000000 --- a/roadmap/solvers/SymplecticIntegrators/tests.md +++ /dev/null @@ -1,45 +0,0 @@ -# Symplectic Integrators — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSymplecticIntegrators : public ::testing::Test: - HarmonicOscillator oscillator # q̈ = −ω²q + u, ω = 1 (concrete SecondOrderSystem) - StrictMock> system - SemiImplicitEuler euler{ oscillator, 0.01f } - VelocityVerlet verlet{ oscillator, 0.01f } -# each case below is a TEST_F(TestSymplecticIntegrators, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -semi_implicit_euler_updates_position_with_new_velocity: - Arrange: EXPECT_CALL(system, Acceleration(q = 1, q̇ = 0, u = 0)) returns 2; h = 0.1 - Assert: Step({1, 0}, 0) == { 1 + 0.1·0.2, 0.2 } (= { 1.02, 0.2 }) -velocity_verlet_evaluates_at_half_step_velocity: - Arrange: EXPECT_CALL sequence: Acceleration(1, 0, 0) → 2, then Acceleration(1.01, 0.1, 0) → 2; h = 0.1 - Assert: Step({1, 0}, 0) == { 1.01, 0.2 } -velocity_verlet_is_exact_for_constant_acceleration: - Arrange: oscillator with ω = 0 and u = 3, 100 steps from rest - Assert: q ≈ ½·3·t² and q̇ ≈ 3·t at t = 1 -energy_error_stays_bounded_over_long_runs: - Arrange: oscillator from q = 1, q̇ = 0; 10⁵ steps (≈ 160 periods) with each method - Assert: |E − E₀| / E₀ stays < 0.6 % (Euler) and < 1e-4 (Verlet) for every step -verlet_is_second_order: - Arrange: q(1) after 1 s with h and h/2 - Assert: error(h) / error(h/2) ≈ 4 (±10 %) -``` - -## Reference vectors - -- Harmonic oscillator `q(t) = cos t`, energy `E = ½(q̇² + q²) = ½`. -- Semi-implicit Euler conserves a modified energy `½(q̇² + q² − h·q·q̇)`: the true energy oscillates with - relative amplitude ≈ `h·ω/2` (0.5 % at `h = 0.01`) and never drifts. - -## Edge cases - -- `h·ω` near `2` ⇒ still bounded just below, divergent above (documented stability limit). -- Zero acceleration ⇒ uniform motion `q + h·q̇` exactly. From c975a886dbce75df015fb6e66e0f4f591f2fc0f0 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:26:03 +0000 Subject: [PATCH 6/7] docs: align markdown tables Apply markdown-table-formatter's layout to the tables this tier adds or edits, as suggested by MegaLinter. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 26 +++++++-------- doc/controllers/FrictionCompensation.md | 42 ++++++++++++------------- doc/controllers/README.md | 26 +++++++-------- doc/math/AngleUtilities.md | 12 +++---- doc/math/README.md | 30 +++++++++--------- doc/math/TridiagonalSolver.md | 22 ++++++------- doc/solvers/README.md | 28 ++++++++--------- doc/solvers/SymplecticIntegrators.md | 40 +++++++++++------------ 8 files changed, 113 insertions(+), 113 deletions(-) diff --git a/README.md b/README.md index 62867d40..d532083e 100644 --- a/README.md +++ b/README.md @@ -14,20 +14,20 @@ Refer to the documentation to quickly integrate and utilize the library's signal ## Documentation -| Category | Description | -|--------------------------------------------------------------------|----------------------------------------------------------------------| -| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | -| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | -| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | -| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | +| Category | Description | +|--------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | +| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | +| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | +| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | -| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | -| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/doc/controllers/FrictionCompensation.md b/doc/controllers/FrictionCompensation.md index b30bd31d..38fb1a24 100644 --- a/doc/controllers/FrictionCompensation.md +++ b/doc/controllers/FrictionCompensation.md @@ -45,8 +45,8 @@ For fixed $v_s$, $\delta$ and $\varepsilon$, $\tau$ is linear in the force coeff ## Complexity Analysis -| Case | Time | Space | Notes | -|------|------|-------|-------| +| Case | Time | Space | Notes | +|------|--------|--------|--------------------------------------------------------------------------------------------------------------------------------------------| | All | $O(N)$ | $O(N)$ | $N$ axes; per axis one exponential, one power, one hyperbolic tangent, two divisions and a clamp; $N$ parameter sets stored, no allocation | Cost is dominated by the three transcendental calls; there is no iteration and no dependence on history, so the latency is constant per sample. @@ -55,11 +55,11 @@ Cost is dominated by the three transcendental calls; there is no iteration and n Parameters: $F_c = 0.5$, $F_s = 0.8$, $F_v = 0.1$, $v_s = 0.05$, $\delta = 2$, $\varepsilon = 10^{-3}$, with a non-binding cap $\tau_{max} = 1$. -| $v$ | $(\lvert v\rvert/v_s)^2$ | Level $L$ | $\tanh(v/\varepsilon)$ | $F_v v$ | $\tau$ | -|-----|---------------|-----------|------------------------|---------|--------| -| 1 | 400 | $0.5 + 0.3\,e^{-400} \approx 0.5$ | 1 | 0.1 | 0.6 | -| 0.01 | 0.04 | $0.5 + 0.3\,e^{-0.04} \approx 0.788$ | $\tanh 10 \approx 1$ | 0.001 | 0.789 | -| 0.05 ($=v_s$) | 1 | $0.5 + 0.3/e \approx 0.610$ | $\tanh 50 \approx 1$ | 0.005 | 0.615 | +| $v$ | $(\lvert v\rvert/v_s)^2$ | Level $L$ | $\tanh(v/\varepsilon)$ | $F_v v$ | $\tau$ | +|---------------|--------------------------|--------------------------------------|------------------------|---------|--------| +| 1 | 400 | $0.5 + 0.3\,e^{-400} \approx 0.5$ | 1 | 0.1 | 0.6 | +| 0.01 | 0.04 | $0.5 + 0.3\,e^{-0.04} \approx 0.788$ | $\tanh 10 \approx 1$ | 0.001 | 0.789 | +| 0.05 ($=v_s$) | 1 | $0.5 + 0.3/e \approx 0.610$ | $\tanh 50 \approx 1$ | 0.005 | 0.615 | At high speed the Stribeck term has vanished and only Coulomb plus viscous drag remain. At $v = 0.01$ the axis is still deep in the Stribeck region, so the level is close to $F_s$. At $v = v_s$ the stiction excess has decayed to a fraction $1/e$ of its initial value. At $v = 0$ the output is exactly zero, and $v = -0.01$ gives $-0.789$ by symmetry. @@ -75,13 +75,13 @@ At high speed the Stribeck term has vanished and only Coulomb plus viscous drag ## Variants & Generalizations -| Variant | Key Difference | -|---------|----------------| -| **Coulomb + viscous only** | $F_s = F_c$ removes the Stribeck term; the level is constant | -| **Asymmetric friction** | Separate parameters per direction of motion; handles direction-dependent drag | -| **Karnopp model** | Replaces the smooth layer with a velocity dead-zone and a stick state | -| **Dahl / LuGre** | Dynamic models with internal bristle state; capture presliding and hysteresis at the cost of state and a stiffer ODE | -| **Inverse-dynamics term** | Same static model folded into a robot inverse-dynamics computation | +| Variant | Key Difference | +|----------------------------|----------------------------------------------------------------------------------------------------------------------| +| **Coulomb + viscous only** | $F_s = F_c$ removes the Stribeck term; the level is constant | +| **Asymmetric friction** | Separate parameters per direction of motion; handles direction-dependent drag | +| **Karnopp model** | Replaces the smooth layer with a velocity dead-zone and a stick state | +| **Dahl / LuGre** | Dynamic models with internal bristle state; capture presliding and hysteresis at the cost of state and a stiffer ODE | +| **Inverse-dynamics term** | Same static model folded into a robot inverse-dynamics computation | ## Applications @@ -92,13 +92,13 @@ At high speed the Stribeck term has vanished and only Coulomb plus viscous drag ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| [Feedforward / 2-DOF](Feedforward2Dof.md) | Same additive-feedforward structure; friction is a velocity-driven feedforward term | -| [PID Controller](Pid.md) | The feedback loop this term is added to; compensation reduces integrator work and stick-slip hunting | -| [Bang-Bang / Hysteresis](BangBangHysteresis.md) | The $\varepsilon \to 0$ limit is a relay on velocity, with the same chattering mechanism | -| [Saturation / Rate Limiter](SaturationRateLimiter.md) | The output clamp is the same saturation, applied to the compensation term alone | -| [QR Decomposition](../solvers/QrDecomposition.md) | Backbone of the least-squares fit of the linear friction coefficients | +| Algorithm | Relationship | +|-------------------------------------------------------|------------------------------------------------------------------------------------------------------| +| [Feedforward / 2-DOF](Feedforward2Dof.md) | Same additive-feedforward structure; friction is a velocity-driven feedforward term | +| [PID Controller](Pid.md) | The feedback loop this term is added to; compensation reduces integrator work and stick-slip hunting | +| [Bang-Bang / Hysteresis](BangBangHysteresis.md) | The $\varepsilon \to 0$ limit is a relay on velocity, with the same chattering mechanism | +| [Saturation / Rate Limiter](SaturationRateLimiter.md) | The output clamp is the same saturation, applied to the compensation term alone | +| [QR Decomposition](../solvers/QrDecomposition.md) | Backbone of the least-squares fit of the linear friction coefficients | ## References & Further Reading diff --git a/doc/controllers/README.md b/doc/controllers/README.md index 815a4236..6877d070 100644 --- a/doc/controllers/README.md +++ b/doc/controllers/README.md @@ -4,16 +4,16 @@ Feedback control algorithms for regulating dynamic systems in real time. ## Algorithms -| Algorithm | Description | -|--------------------------|-----------------------------------------------------------------------------------------| -| [Bang-Bang / Hysteresis Controller](BangBangHysteresis.md) | Two-state relay controller with Schmitt-trigger dead-band to prevent chatter | -| [Deadbeat Control](DeadbeatControl.md) | Discrete-time N-step deadbeat controller — exact convergence in N samples via reachability pseudo-inverse | -| [PID Controller](Pid.md) | Proportional-Integral-Derivative controller using a discrete recursive formulation | -| [LQR Controller](Lqr.md) | Linear Quadratic Regulator — optimal state-feedback control minimizing a quadratic cost | -| [LQI / Servo Controller](IntegralStateFeedbackLqi.md) | Integral state feedback (LQI) — LQR augmented with integrators on the tracking error for zero steady-state offset | -| [LQG Controller](Lqg.md) | Linear Quadratic Gaussian — output-feedback optimal control via LQR + Kalman Filter | -| [MPC Controller](Mpc.md) | Model Predictive Controller — receding-horizon optimal control with constraint handling | -| [Linear Time-Invariant Model](LinearTimeInvariant.md) | Discrete-time state-space plant model (A, B, C, D) shared across controllers and filters | -| [Feedforward / 2-DOF Controller](Feedforward2Dof.md) | Feedforward + feedback wrapper for decoupled reference tracking and disturbance rejection | -| [Luenberger Observer](LuenbergerObserver.md) | Deterministic state estimator using pole placement (Ackermann's formula) to reconstruct unmeasured states | -| [Friction Compensation](FrictionCompensation.md) | Coulomb + viscous + Stribeck feedforward with a tanh boundary layer and output clamp, per axis | +| Algorithm | Description | +|------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------| +| [Bang-Bang / Hysteresis Controller](BangBangHysteresis.md) | Two-state relay controller with Schmitt-trigger dead-band to prevent chatter | +| [Deadbeat Control](DeadbeatControl.md) | Discrete-time N-step deadbeat controller — exact convergence in N samples via reachability pseudo-inverse | +| [PID Controller](Pid.md) | Proportional-Integral-Derivative controller using a discrete recursive formulation | +| [LQR Controller](Lqr.md) | Linear Quadratic Regulator — optimal state-feedback control minimizing a quadratic cost | +| [LQI / Servo Controller](IntegralStateFeedbackLqi.md) | Integral state feedback (LQI) — LQR augmented with integrators on the tracking error for zero steady-state offset | +| [LQG Controller](Lqg.md) | Linear Quadratic Gaussian — output-feedback optimal control via LQR + Kalman Filter | +| [MPC Controller](Mpc.md) | Model Predictive Controller — receding-horizon optimal control with constraint handling | +| [Linear Time-Invariant Model](LinearTimeInvariant.md) | Discrete-time state-space plant model (A, B, C, D) shared across controllers and filters | +| [Feedforward / 2-DOF Controller](Feedforward2Dof.md) | Feedforward + feedback wrapper for decoupled reference tracking and disturbance rejection | +| [Luenberger Observer](LuenbergerObserver.md) | Deterministic state estimator using pole placement (Ackermann's formula) to reconstruct unmeasured states | +| [Friction Compensation](FrictionCompensation.md) | Coulomb + viscous + Stribeck feedforward with a tanh boundary layer and output clamp, per axis | diff --git a/doc/math/AngleUtilities.md b/doc/math/AngleUtilities.md index 1d683f3e..3ddfdf0c 100644 --- a/doc/math/AngleUtilities.md +++ b/doc/math/AngleUtilities.md @@ -34,12 +34,12 @@ This recovers the true continuous angle as long as consecutive true samples diff ## Complexity Analysis -| Operation | Time | Space | Notes | -|-------------------------|--------|--------|----------------------------------------------| -| Wrap to $(-\pi,\pi]$ | $O(1)$ | $O(1)$ | One fmod, one compare, a few adds | -| Wrap to $[0,2\pi)$ | $O(1)$ | $O(1)$ | One fmod, two compares, one conditional add | -| Shortest difference | $O(1)$ | $O(1)$ | One subtraction plus one signed wrap | -| Unwrap | $O(1)$ | $O(1)$ | One shortest difference plus one add; no state | +| Operation | Time | Space | Notes | +|----------------------|--------|--------|------------------------------------------------| +| Wrap to $(-\pi,\pi]$ | $O(1)$ | $O(1)$ | One fmod, one compare, a few adds | +| Wrap to $[0,2\pi)$ | $O(1)$ | $O(1)$ | One fmod, two compares, one conditional add | +| Shortest difference | $O(1)$ | $O(1)$ | One subtraction plus one signed wrap | +| Unwrap | $O(1)$ | $O(1)$ | One shortest difference plus one add; no state | Cost does not depend on the magnitude of the input: the remainder is a single operation, not a loop subtracting $2\pi$. diff --git a/doc/math/README.md b/doc/math/README.md index cfc1796c..a583ccce 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -4,18 +4,18 @@ Core mathematical primitives for numerical computation. ## Algorithms -| Algorithm | Description | -|-----------------------------|-----------------------------------------------------------------------------------------------| -| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | -| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | -| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | -| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | -| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | -| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | -| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | -| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | -| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | -| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | -| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | -| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | -| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | +| Algorithm | Description | +|----------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------| +| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | +| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | +| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | +| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | +| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | +| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | +| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | +| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | +| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | +| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | +| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | +| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | diff --git a/doc/math/TridiagonalSolver.md b/doc/math/TridiagonalSolver.md index 2a4a7154..3d99a572 100644 --- a/doc/math/TridiagonalSolver.md +++ b/doc/math/TridiagonalSolver.md @@ -44,12 +44,12 @@ Storage is three bands of fixed capacity; nothing is allocated dynamically. Fact Take $A = \operatorname{tridiag}(-1, 2, -1)$ with $n = 4$, so $a_k = c_k = -1$, $b_k = 2$. -| $k$ | $p_k$ | $1/p_k$ | $c'_k$ | -|-----|-------|---------|--------| -| 0 | $2$ | $1/2$ | $-1/2$ | -| 1 | $2 - (-1)(-1/2) = 3/2$ | $2/3$ | $-2/3$ | -| 2 | $2 - (-1)(-2/3) = 4/3$ | $3/4$ | $-3/4$ | -| 3 | $2 - (-1)(-3/4) = 5/4$ | $4/5$ | unused | +| $k$ | $p_k$ | $1/p_k$ | $c'_k$ | +|-----|------------------------|---------|--------| +| 0 | $2$ | $1/2$ | $-1/2$ | +| 1 | $2 - (-1)(-1/2) = 3/2$ | $2/3$ | $-2/3$ | +| 2 | $2 - (-1)(-2/3) = 4/3$ | $3/4$ | $-3/4$ | +| 3 | $2 - (-1)(-3/4) = 5/4$ | $4/5$ | unused | **Right-hand side** $d = (1, 0, 0, 1)$: forward $y = (1/2,\ 1/3,\ 1/4,\ 1)$; back substitution gives $x_3 = 1$, $x_2 = 1/4 + \tfrac34 = 1$, $x_1 = 1/3 + \tfrac23 = 1$, $x_0 = 1/2 + 1/2 = 1$, so $x = (1, 1, 1, 1)$. @@ -80,11 +80,11 @@ Take $A = \operatorname{tridiag}(-1, 2, -1)$ with $n = 4$, so $a_k = c_k = -1$, ## Connections to Other Algorithms -| Algorithm | Relationship | -|---------------------------------------------------|------------------------------------------------------------------------------| -| [LU Decomposition](../solvers/LuDecomposition.md) | General fallback with partial pivoting; $O(n^3)$ and dense | -| [Triangular Solve](TriangularSolve.md) | Same forward/back substitution idea, applied to full triangular factors | -| [Levinson–Durbin](../solvers/LevinsonDurbin.md) | Another structured solver, for Toeplitz systems | +| Algorithm | Relationship | +|---------------------------------------------------|-------------------------------------------------------------------------------| +| [LU Decomposition](../solvers/LuDecomposition.md) | General fallback with partial pivoting; $O(n^3)$ and dense | +| [Triangular Solve](TriangularSolve.md) | Same forward/back substitution idea, applied to full triangular factors | +| [Levinson–Durbin](../solvers/LevinsonDurbin.md) | Another structured solver, for Toeplitz systems | | Cubic spline interpolation | Main consumer; plans the second derivatives by solving one tridiagonal system | ## References & Further Reading diff --git a/doc/solvers/README.md b/doc/solvers/README.md index 6d2c5cf0..7a9cdddc 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -4,18 +4,18 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------------------------------|------------------------------------------------------------------------| -| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | -| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | -| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | -| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | -| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | -| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | -| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | -| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | -| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | -| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | -| [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | -| [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | +| Algorithm | Description | +|----------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------| +| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | +| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | +| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | +| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | +| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | +| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | +| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | +| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | +| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | +| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | +| [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | +| [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | | [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | diff --git a/doc/solvers/SymplecticIntegrators.md b/doc/solvers/SymplecticIntegrators.md index 4522dbfe..79320409 100644 --- a/doc/solvers/SymplecticIntegrators.md +++ b/doc/solvers/SymplecticIntegrators.md @@ -44,10 +44,10 @@ $$ The conserved modified energies and the resulting error of the true energy $E = \tfrac12(v^2 + \omega^2 q^2)$ are: -| Method | Conserved quantity ($\omega = 1$) | Relative energy error | -|--------|-----------------------------------|-----------------------| -| Semi-implicit Euler | $\tfrac12\left(v^2 + q^2 - h\,q\,v\right)$ | oscillates, amplitude $\approx h\omega/2$ | -| Velocity Verlet | $\tfrac12\left(v^2 + (1 - \tfrac{\theta^2}{4})\,q^2\right)$ | oscillates, amplitude $\approx (h\omega)^2/8$ | +| Method | Conserved quantity ($\omega = 1$) | Relative energy error | +|---------------------|-------------------------------------------------------------|-----------------------------------------------| +| Semi-implicit Euler | $\tfrac12\left(v^2 + q^2 - h\,q\,v\right)$ | oscillates, amplitude $\approx h\omega/2$ | +| Velocity Verlet | $\tfrac12\left(v^2 + (1 - \tfrac{\theta^2}{4})\,q^2\right)$ | oscillates, amplitude $\approx (h\omega)^2/8$ | ### Velocity-Dependent Forces @@ -55,10 +55,10 @@ With damping or Coriolis terms the system is not Hamiltonian, so neither method ## Complexity Analysis -| Method | Acceleration evaluations | Order | Arithmetic | Memory | -|--------|--------------------------|-------|------------|--------| -| Semi-implicit Euler | 1 | 1 | $O(n)$ | two state copies | -| Velocity Verlet | 2 | 2 | $O(n)$ | two state copies | +| Method | Acceleration evaluations | Order | Arithmetic | Memory | +|---------------------|--------------------------|-------|------------|------------------| +| Semi-implicit Euler | 1 | 1 | $O(n)$ | two state copies | +| Velocity Verlet | 2 | 2 | $O(n)$ | two state copies | Here $n$ is the number of generalized coordinates. The cost of a step is the cost of its evaluations plus a handful of vector operations; no heap is used. Each step is a pure function of its arguments; the second Verlet evaluation is not reused as the next step's first. Every evaluation goes through a virtual call: fine for simulation and model-in-the-loop tests; for interrupt-service use, bind the concrete system type instead. @@ -68,10 +68,10 @@ Unit oscillator $a = -q$, $h = 0.1$, start $(q, v) = (1, 0)$, true energy $E = 0 **Semi-implicit Euler** -| Step | Velocity update | Position update | True $E$ | Modified energy | -|------|-----------------|-----------------|----------|-----------------| -| 1 | $0 + 0.1(-1) = -0.1$ | $1 + 0.1(-0.1) = 0.99$ | $0.4950$ | $0.5000$ | -| 2 | $-0.1 + 0.1(-0.99) = -0.199$ | $0.99 + 0.1(-0.199) = 0.9701$ | $0.4903$ | $0.5000$ | +| Step | Velocity update | Position update | True $E$ | Modified energy | +|------|------------------------------|-------------------------------|----------|-----------------| +| 1 | $0 + 0.1(-1) = -0.1$ | $1 + 0.1(-0.1) = 0.99$ | $0.4950$ | $0.5000$ | +| 2 | $-0.1 + 0.1(-0.99) = -0.199$ | $0.99 + 0.1(-0.199) = 0.9701$ | $0.4903$ | $0.5000$ | The true energy wanders (about $1\%$ and $2\%$ low), yet the modified energy is conserved exactly; the offset is $\tfrac12 h\,q\,v$, which cannot grow beyond $h/4$. @@ -87,11 +87,11 @@ The true energy wanders (about $1\%$ and $2\%$ low), yet the modified energy is ## Variants & Generalizations -| Variant | Key difference | -|---------|----------------| +| Variant | Key difference | +|-------------------------------------------------|----------------------------------------------------------------------------------------------------------------------| | **Semi-implicit Euler, velocity Verlet (this)** | Symplectic, orders 1 and 2; explicit Euler differs only in using the old velocity for the drift and its energy grows | -| **Higher-order composition** | Chains of Verlet steps with weights (Yoshida, Suzuki) reach orders 4, 6, ... | -| **Implicit midpoint / Gauss** | Symplectic and implicit; suited to stiff or non-separable systems at the cost of a nonlinear solve | +| **Higher-order composition** | Chains of Verlet steps with weights (Yoshida, Suzuki) reach orders 4, 6, ... | +| **Implicit midpoint / Gauss** | Symplectic and implicit; suited to stiff or non-separable systems at the cost of a nonlinear solve | ## Applications @@ -100,10 +100,10 @@ The true energy wanders (about $1\%$ and $2\%$ low), yet the modified energy is ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| Runge–Kutta 4 | Four evaluations per step, fourth order, general first-order systems; energy drifts slowly on conservative problems | -| Dormand–Prince | Adaptive step and error control for first-order systems; higher accuracy per step, no structure preservation | +| Algorithm | Relationship | +|----------------|---------------------------------------------------------------------------------------------------------------------| +| Runge–Kutta 4 | Four evaluations per step, fourth order, general first-order systems; energy drifts slowly on conservative problems | +| Dormand–Prince | Adaptive step and error control for first-order systems; higher accuracy per step, no structure preservation | ## References & Further Reading From 44a914faf2eef5aa16ed8e1e88b8f3ac95c239da Mon Sep 17 00:00:00 2001 From: gfs Date: Fri, 23 Oct 2026 19:05:21 +0200 Subject: [PATCH 7/7] feat(motion_profiles): add polynomial, trapezoidal and S-curve profiles (#356) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(motion_profiles): add module with polynomial point-to-point profile Deploy roadmap item 55 and create the motion_profiles module with the shared TrajectoryState / MotionLimits types. PolynomialProfile solves the closed-form cubic (position + velocity) or quintic (+ acceleration) coefficients once and samples position, velocity, acceleration and jerk by Horner evaluation, with time clamped to [0, tf]. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 * feat(motion_profiles): add trapezoidal (LSPB) velocity profile Deploy roadmap item 56: minimum-time trapezoid / triangle plan from (vMax, aMax), plus PlanWithDuration, which keeps aMax and lowers the cruise speed so several axes can finish together. The cruise speed uses the cancellation-free conjugate form of the smaller root, and infeasible durations return nullopt. At and after tf the profile holds the goal at rest; before t = 0 it holds the start. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 * feat(motion_profiles): add S-curve (jerk-limited) profile Deploy roadmap item 60: seven-phase rest-to-rest profile planned with the closed-form three-step method of Biagiotti & Melchiorri §3.4 (vMax reached; else aMax reached; else neither). The boundary states are integrated once, and each sample evaluates one constant-jerk cubic. Add math::Cbrt (generic wrapper, no ARM override), which the step-3 branch needs. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 * docs: align markdown tables Apply markdown-table-formatter's layout to the tables this tier adds or edits, as suggested by MegaLinter. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 * fix(build): build motion_profiles tests in the QEMU presets The qemu-cortex-m4/m7 build presets list their test targets explicitly, so numerical.motion_profiles_test was registered with CTest but never built, and the QEMU jobs failed with "Could not load kernel". Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --------- Co-authored-by: Claude --- AGENTS.md | 2 +- CMakePresets.json | 2 + README.md | 1 + ROADMAP.md | 29 ---- doc/motion_profiles/PolynomialProfile.md | 107 ++++++++++++ doc/motion_profiles/README.md | 11 ++ doc/motion_profiles/SCurveProfile.md | 120 +++++++++++++ doc/motion_profiles/TrapezoidalProfile.md | 111 +++++++++++++ numerical/CMakeLists.txt | 1 + numerical/math/Math.cpp | 3 + numerical/math/Math.hpp | 12 ++ numerical/math/test/TestMath.cpp | 6 + numerical/motion_profiles/CMakeLists.txt | 26 +++ .../motion_profiles/MotionProfileTypes.hpp | 29 ++++ .../motion_profiles/PolynomialProfile.cpp | 6 + .../motion_profiles/PolynomialProfile.hpp | 129 ++++++++++++++ numerical/motion_profiles/SCurveProfile.cpp | 6 + numerical/motion_profiles/SCurveProfile.hpp | 157 ++++++++++++++++++ .../motion_profiles/TrapezoidalProfile.cpp | 6 + .../motion_profiles/TrapezoidalProfile.hpp | 114 +++++++++++++ numerical/motion_profiles/test/CMakeLists.txt | 16 ++ .../test/TestPolynomialProfile.cpp | 85 ++++++++++ .../test/TestSCurveProfile.cpp | 154 +++++++++++++++++ .../test/TestTrapezoidalProfile.cpp | 118 +++++++++++++ roadmap/README.md | 2 - .../PolynomialProfile/explanation.md | 32 ---- .../PolynomialProfile/implementation.md | 112 ------------- .../PolynomialProfile/tests.md | 63 ------- .../SCurveProfile/explanation.md | 35 ---- .../SCurveProfile/implementation.md | 122 -------------- .../motion_profiles/SCurveProfile/tests.md | 87 ---------- .../TrapezoidalProfile/explanation.md | 33 ---- .../TrapezoidalProfile/implementation.md | 108 ------------ .../TrapezoidalProfile/tests.md | 71 -------- 34 files changed, 1221 insertions(+), 695 deletions(-) create mode 100644 doc/motion_profiles/PolynomialProfile.md create mode 100644 doc/motion_profiles/README.md create mode 100644 doc/motion_profiles/SCurveProfile.md create mode 100644 doc/motion_profiles/TrapezoidalProfile.md create mode 100644 numerical/motion_profiles/CMakeLists.txt create mode 100644 numerical/motion_profiles/MotionProfileTypes.hpp create mode 100644 numerical/motion_profiles/PolynomialProfile.cpp create mode 100644 numerical/motion_profiles/PolynomialProfile.hpp create mode 100644 numerical/motion_profiles/SCurveProfile.cpp create mode 100644 numerical/motion_profiles/SCurveProfile.hpp create mode 100644 numerical/motion_profiles/TrapezoidalProfile.cpp create mode 100644 numerical/motion_profiles/TrapezoidalProfile.hpp create mode 100644 numerical/motion_profiles/test/CMakeLists.txt create mode 100644 numerical/motion_profiles/test/TestPolynomialProfile.cpp create mode 100644 numerical/motion_profiles/test/TestSCurveProfile.cpp create mode 100644 numerical/motion_profiles/test/TestTrapezoidalProfile.cpp delete mode 100644 roadmap/motion_profiles/PolynomialProfile/explanation.md delete mode 100644 roadmap/motion_profiles/PolynomialProfile/implementation.md delete mode 100644 roadmap/motion_profiles/PolynomialProfile/tests.md delete mode 100644 roadmap/motion_profiles/SCurveProfile/explanation.md delete mode 100644 roadmap/motion_profiles/SCurveProfile/implementation.md delete mode 100644 roadmap/motion_profiles/SCurveProfile/tests.md delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/explanation.md delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/implementation.md delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/tests.md diff --git a/AGENTS.md b/AGENTS.md index 0a13925e..08f5c37e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -53,7 +53,7 @@ resource-constrained embedded systems. Real-time, deterministic, no heap. `analysis`, `windowing`, `control_analysis`, `controllers`, `estimators`, `filters` (active Kalman family — **not** `filters::active`), `filters::passive`, `math`, `optimization`, `regularization`, `solvers`, and new: -`robust_control`, `nonlinear_control`. +`robust_control`, `nonlinear_control`, `motion_profiles`. ## Math functions diff --git a/CMakePresets.json b/CMakePresets.json index 953a9772..1e842b47 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -142,6 +142,7 @@ "numerical.filters.active_test", "numerical.filters.passive_test", "numerical.math_test", + "numerical.motion_profiles_test", "numerical.nonlinear_control_test", "numerical.optimization_test", "numerical.regularization_test", @@ -164,6 +165,7 @@ "numerical.filters.active_test", "numerical.filters.passive_test", "numerical.math_test", + "numerical.motion_profiles_test", "numerical.nonlinear_control_test", "numerical.optimization_test", "numerical.regularization_test", diff --git a/README.md b/README.md index d532083e..89f27dcf 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | diff --git a/ROADMAP.md b/ROADMAP.md index ead769cb..f4f382d3 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,10 +26,7 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 55 | Polynomial (cubic/quintic) point-to-point profile | `motion_profiles` (new) | ★☆☆☆☆ | -| 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | -| 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | | 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | | 62 | Damped pseudo-inverse + null-space projector | `solvers` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | @@ -54,25 +51,6 @@ unit-test reference [`TESTING.md`](TESTING.md) depends on but which the library does not yet expose. Detailed below under [Evaluation & metrics primitives](#evaluation--metrics-primitives). -## Tier 1 — Trivial primitives ★☆☆☆☆ - -### 55. Polynomial point-to-point profile -- **What:** Cubic (position + velocity) or quintic (+ acceleration) time law between two states over a - fixed duration; defines the shared `TrajectoryState` / `MotionLimits` types of `motion_profiles`. -- **Embedded value:** Coefficients solved once in closed form; each servo tick is a Horner evaluation. -- **Algorithm / paper:** M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5. -- **Reuses:** Scalar templates. -- **Spec:** [roadmap/motion_profiles/PolynomialProfile](roadmap/motion_profiles/PolynomialProfile/implementation.md). - -### 56. Trapezoidal (LSPB) profile -- **What:** Minimum-time velocity/acceleration-limited rest-to-rest move, triangular for short moves, plus - a fixed-duration plan for multi-axis synchronization. -- **Embedded value:** The standard servo-axis move (robot joints, CNC, conveyors, motor position loops). -- **Algorithm / paper:** L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and - Robots* (2008), Ch. 3. -- **Reuses:** Item 55 types. -- **Spec:** [roadmap/motion_profiles/TrapezoidalProfile](roadmap/motion_profiles/TrapezoidalProfile/implementation.md). - ## Tier 2 — Easy ★★☆☆☆ ### 53. SE(3) transform, twists and wrenches @@ -87,13 +65,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`. - **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md). -### 60. S-curve (jerk-limited) profile -- **What:** Seven-segment jerk-bounded rest-to-rest move with all short-move cases in closed form. -- **Embedded value:** Continuous acceleration — far less vibration than a trapezoid for precision axes. -- **Algorithm / paper:** L. Biagiotti, C. Melchiorri (2008), §3.4. -- **Reuses:** Items 55 (types) and 56 (synchronization scheme). -- **Spec:** [roadmap/motion_profiles/SCurveProfile](roadmap/motion_profiles/SCurveProfile/implementation.md). - ### 61. Cubic spline interpolation - **What:** C² vector-valued spline through `K` knots, clamped or natural ends, shared factorization across channels. diff --git a/doc/motion_profiles/PolynomialProfile.md b/doc/motion_profiles/PolynomialProfile.md new file mode 100644 index 00000000..50b349bb --- /dev/null +++ b/doc/motion_profiles/PolynomialProfile.md @@ -0,0 +1,107 @@ +# Polynomial Point-to-Point Profile + +## Overview & Motivation + +A polynomial profile is a closed-form time law that drives one axis from a start state to a goal state over a fixed duration $T$. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches endpoint acceleration, so the motion has no acceleration step at either end. + +Point-to-point moves are the bread and butter of servo motion: robot joints, CNC and pick-and-place axes, motor position loops. The coefficients come from a handful of algebraic expressions, solved once. Each servo tick is then a fixed number of multiply-adds, with no iteration, no lookup table and a deterministic cycle count. + +## Mathematical Theory + +### Boundary Conditions + +Let $t \in [0, T]$, $\Delta q = q_f - q_0$, and let $\alpha_0, \alpha_f$ be the start and end accelerations. A polynomial of degree $n$ has $n+1$ coefficients, so a cubic satisfies four conditions and a quintic six: + +$$q(0) = q_0,\quad q(T) = q_f,\quad \dot{q}(0) = v_0,\quad \dot{q}(T) = v_f \qquad \text{(quintic also: } \ddot{q}(0) = \alpha_0,\ \ddot{q}(T) = \alpha_f\text{)}$$ + +The conditions are linear in the coefficients $a_k$ of $q(t) = \sum_k a_k t^k$ and the time basis is fixed, so the solution is a plug-in formula rather than a runtime matrix solve. + +### Cubic + +The start conditions give $a_0 = q_0$ and $a_1 = v_0$. The end conditions leave a $2\times 2$ system with the closed-form solution + +$$a_2 = \frac{3\,\Delta q - (2 v_0 + v_f)\,T}{T^2}, \qquad a_3 = \frac{-2\,\Delta q + (v_0 + v_f)\,T}{T^3}$$ + +The acceleration is linear in $t$ and the jerk is the constant $6 a_3$. + +### Quintic + +The start conditions give $a_0 = q_0$, $a_1 = v_0$, $a_2 = \alpha_0 / 2$. The end conditions give + +$$a_3 = \frac{20\,\Delta q - (8 v_f + 12 v_0)\,T - (3\alpha_0 - \alpha_f)\,T^2}{2\,T^3}$$ + +$$a_4 = \frac{-30\,\Delta q + (14 v_f + 16 v_0)\,T + (3\alpha_0 - 2\alpha_f)\,T^2}{2\,T^4}$$ + +$$a_5 = \frac{12\,\Delta q - 6 (v_f + v_0)\,T - (\alpha_0 - \alpha_f)\,T^2}{2\,T^5}$$ + +The acceleration is a cubic matching $\alpha_0$ and $\alpha_f$, hence continuous; the jerk is a quadratic. + +### Sampling + +Position, velocity, acceleration and jerk are four Horner evaluations of the polynomial and its three derivatives: + +$$q = a_0 + t\big(a_1 + t(a_2 + t(a_3 + t(a_4 + t\,a_5)))\big), \qquad \dddot{q} = 6 a_3 + t\,(24 a_4 + 60 a_5\, t)$$ + +The cubic is the same evaluation with $a_4 = a_5 = 0$. The time is first clamped to $[0, T]$. + +### Rest-to-Rest Peaks + +With zero end velocities (and accelerations, for the quintic) the cubic peaks at $1.5\,\Delta q / T$ in velocity and $6\,\Delta q / T^2$ in acceleration; the quintic at $1.875\,\Delta q / T$ and $\tfrac{10}{\sqrt{3}}\,\Delta q / T^2 \approx 5.77\,\Delta q / T^2$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|--------------------|--------|--------------------------------------------------------------------------------------| +| Coefficient solve | $O(1)$ | $O(1)$ | Closed-form expressions, once at construction or reset | +| Sample | $O(\text{degree})$ | $O(1)$ | Four Horner evaluations, at most 14 multiply-adds; no state change, so deterministic | +| Stored state | n/a | $O(1)$ | Six coefficients, the duration and the degree; stack only, no heap | + +## Step-by-Step Walkthrough + +Rest-to-rest cubic with $q_0 = 0$, $q_f = 1$, $T = 2$, $v_0 = v_f = 0$, so $\Delta q = 1$ and $a_0 = a_1 = 0$. + +| Step | Computation | Result | +|--------------|-----------------------------------------------|----------------------------------------------| +| $a_2$ | $(3\cdot 1 - 0)/2^2$ | $0.75$ | +| $a_3$ | $(-2\cdot 1 + 0)/2^3$ | $-0.25$ | +| Position | $0.75\,t^2 - 0.25\,t^3 = 3(t/2)^2 - 2(t/2)^3$ | $q(1) = 0.5$, $q(2) = 1$ | +| Velocity | $1.5\,t - 0.75\,t^2$ | $v(1) = 0.75 = 1.5\,\Delta q / T$ (the peak) | +| Acceleration | $1.5 - 1.5\,t$ | $+1.5$ at $t=0$, $-1.5$ at $t=2$ | +| Jerk | $6 a_3$ | $-1.5$, constant | + +The position is the smoothstep of $t/2$, symmetric about the midpoint, where the velocity peaks. The acceleration jumps from $0$ at rest to $1.5$ at the first instant and from $-1.5$ back to $0$ at the end. + +## Pitfalls & Edge Cases + +- **Duration must be positive.** The coefficients contain $1/T^k$ terms; a zero or negative duration is rejected at construction and reset. +- **Short durations in single precision.** Higher-order coefficients grow like $1/T^5$ for the quintic, and terms of opposite sign cancel near the end of the move, losing digits. +- **Clamped time holds the endpoint state.** Outside $[0, T]$ the sample is the state at $0$ or $T$, so it reports the end velocity and acceleration, not zero, when they are nonzero. +- **Acceleration steps in the cubic.** Acceleration is not matched, so at rest endpoints it jumps from or to zero, which excites flexible loads. The quintic's jerk is finite but generally nonzero at the ends. +- **No limit enforcement.** Peaks follow from $\Delta q$, $T$ and the boundary values; inconsistent boundary velocities can overshoot $q_f$ before returning. + +## Variants & Generalizations + +| Variant | Key difference | +|-----------------------------|---------------------------------------------------------------------------------------------------------------------------| +| **Cubic (this)** | Position and velocity matched; constant jerk; acceleration discontinuous at the ends | +| **Quintic (this)** | Adds acceleration matching; continuous acceleration at about $25\%$ higher peak velocity for rest-to-rest | +| **Path-parameter time law** | A 0 to 1 instance maps time to a normalized path coordinate; with $v_0 = v_f = 1$, $T = 1$ the cubic collapses to $s = t$ | + +## Applications + +- **Joint-space moves** — robot joints and servo axes between taught points; multi-axis moves use one instance per axis with a common $T$ so all axes arrive together. +- **Pick-and-place and CNC positioning** — fixed-time axis moves where smoothness matters more than minimum time. +- **Path timing** — the scalar time law along a geometric path such as an orientation interpolation. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|------------------------------|-----------------------------------------------------------------------------------------------------------------------| +| Trapezoidal velocity profile | Respects velocity and acceleration limits with a free duration; polynomials fix the duration and let the peaks follow | +| S-curve profile | Bounds jerk explicitly under limits; the quintic only smooths the endpoints | +| Cubic spline interpolation | Handles many via points with continuous acceleration; for two knots it reduces to this cubic | + +## References & Further Reading + +- Craig, J. J., "Introduction to Robotics: Mechanics and Control," 3rd ed., Pearson, 2005, Chapter 7 (trajectory generation). +- Biagiotti, L. and Melchiorri, C., "Trajectory Planning for Automatic Machines and Robots," Springer, 2008, §2.1. diff --git a/doc/motion_profiles/README.md b/doc/motion_profiles/README.md new file mode 100644 index 00000000..82135134 --- /dev/null +++ b/doc/motion_profiles/README.md @@ -0,0 +1,11 @@ +# Motion Profiles + +Time laws for point-to-point motion of a single axis: position, velocity, acceleration and jerk as functions of time, planned once and sampled every control tick. + +## Algorithms + +| Algorithm | Description | +|----------------------------------------------|--------------------------------------------------------------------------------------------------------------------| +| [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | +| [Trapezoidal Profile](TrapezoidalProfile.md) | Minimum-time trapezoid / triangle velocity profile (LSPB) and a fixed-duration plan for multi-axis synchronization | +| [S-Curve Profile](SCurveProfile.md) | Seven-phase jerk-limited rest-to-rest profile with a closed-form three-step planner — continuous acceleration | diff --git a/doc/motion_profiles/SCurveProfile.md b/doc/motion_profiles/SCurveProfile.md new file mode 100644 index 00000000..80a6a5f5 --- /dev/null +++ b/doc/motion_profiles/SCurveProfile.md @@ -0,0 +1,120 @@ +# S-Curve (Jerk-Limited) Profile + +## Overview & Motivation + +A trapezoidal velocity profile bounds speed and acceleration, but acceleration jumps instantaneously at every corner. Those jumps are impulses in jerk: they excite structural resonances, wear gearing, and make payloads wobble or liquids spill. The S-curve (double-S) profile also bounds **jerk**, so acceleration ramps linearly in and out and is continuous, giving far less vibration than a trapezoid. The price is a slightly longer move. + +For a rest-to-rest move the phase durations have a closed form: planning is a handful of arithmetic operations, with no iteration. Three limits drive it: a velocity ceiling $v_{\max}$, an acceleration ceiling $a_{\max}$ and a jerk ceiling $j_{\max}$. Short moves never reach the ceilings; the corresponding phases shrink to zero length. + +## Mathematical Theory + +### Phase Structure + +A rest-to-rest move of length $h > 0$ is built from seven constant-jerk phases: + +| Phase | Jerk | Acceleration | Duration | +|-------|-------------|---------------------------|--------------| +| 1 | $+j_{\max}$ | ramps $0 \to a_{lim}$ | $T_j$ | +| 2 | $0$ | constant $a_{lim}$ | $T_a - 2T_j$ | +| 3 | $-j_{\max}$ | ramps $a_{lim} \to 0$ | $T_j$ | +| 4 | $0$ | zero, cruise at $v_{lim}$ | $T_v$ | +| 5 | $-j_{\max}$ | ramps $0 \to -a_{lim}$ | $T_j$ | +| 6 | $0$ | constant $-a_{lim}$ | $T_a - 2T_j$ | +| 7 | $+j_{\max}$ | ramps $-a_{lim} \to 0$ | $T_j$ | + +Phases 1–3 form the acceleration stage of length $T_a$; phases 5–7 mirror it, so the profile is symmetric about its midpoint and the total time is $T = 2T_a + T_v$. Integrating the stage gives the peaks and the distance: + +$$a_{lim} = j_{\max} T_j, \qquad v_{lim} = a_{lim}\,(T_a - T_j), \qquad h = v_{lim}\,(T_a + T_v)$$ + +### Closed-Form Three-Step Planner + +This is the case $v_0 = v_1 = 0$ of Biagiotti and Melchiorri, §3.4. Each step assumes one more ceiling is unreachable. + +**Step 1 — assume $v_{\max}$ is reached** ($v_{lim} = v_{\max}$): + +$$ +\text{if } v_{\max} j_{\max} \ge a_{\max}^2:\; T_j = \frac{a_{\max}}{j_{\max}},\; T_a = T_j + \frac{v_{\max}}{a_{\max}} +\qquad\text{else}\qquad +T_j = \sqrt{\frac{v_{\max}}{j_{\max}}},\; T_a = 2T_j +$$ + +$$T_v = \frac{h}{v_{\max}} - T_a$$ + +The first case has an acceleration plateau; the second has a pure triangular ramp with peak $j_{\max}T_j < a_{\max}$. Here $T = h/v_{\max} + T_a$, so the jerk limit costs exactly $a_{\max}/j_{\max}$ over a trapezoid with the same $v_{\max}, a_{\max}$. + +**Step 2 — if $T_v < 0$**, set $T_v = 0$ and assume $a_{\max}$ is still reached, $T_j = a_{\max}/j_{\max}$. Then $h = a_{\max} T_a (T_a - T_j)$ is a quadratic in $T_a$: + +$$\Delta = \frac{a_{\max}^4}{j_{\max}^2} + 4\,a_{\max} h, \qquad T_a = \frac{a_{\max}^2/j_{\max} + \sqrt{\Delta}}{2\,a_{\max}}$$ + +**Step 3 — if $T_a < 2T_j$**, the plateau would have negative length, so $a_{\max}$ is unreachable too. Both ramps are triangles, $T_a = 2T_j$ and $h = 2 j_{\max} T_j^3$: + +$$T_j = \sqrt[3]{\frac{h}{2\,j_{\max}}}, \qquad T_a = 2T_j$$ + +Step 2 applies when $h < v_{\max} T_a$, and step 3 when $h < 2 a_{\max}^3 / j_{\max}^2$ (from $T_a \ge 2T_j \iff \Delta \ge 9 a_{\max}^4/j_{\max}^2$). Neighbouring branches agree on $T$ at each threshold, so $T$ is continuous in $h$. No duration is ever negative: in step 1, $T_a - 2T_j = v_{\max}/a_{\max} - a_{\max}/j_{\max} \ge 0$ is the branch condition itself; step 2 is guarded by the step-3 test; step 3 has $T_a - 2T_j = 0$. Zero-length phases collapse out of the profile. + +### Exact Evaluation + +Within a phase of jerk $j_i$ starting from state $(s_i, v_i, a_i)$, with local time $\tau$: + +$$a = a_i + j_i\tau, \qquad v = v_i + a_i\tau + \tfrac12 j_i\tau^2, \qquad s = s_i + v_i\tau + \tfrac12 a_i\tau^2 + \tfrac16 j_i\tau^3$$ + +The seven boundary states are integrated exactly once, at planning time, with these polynomials. Each sample then locates its phase and evaluates a cubic in time. At and beyond $T$ the profile holds the goal at rest; before $t = 0$ it holds the start. A negative move direction negates the offset, velocity, acceleration and jerk. + +### Time Scaling for Synchronization + +Replanning with $(v_{\max}/\lambda,\; a_{\max}/\lambda^2,\; j_{\max}/\lambda^3)$ stretches an axis by $\lambda \ge 1$. Every planner term scales consistently ($T_j \to \lambda T_j$, $T_a \to \lambda T_a$, $\Delta \to \lambda^{-2}\Delta$) and the branch tests are invariant, so the duration scales by exactly $\lambda$ in every branch. Axes stretched to a common $T$ finish together. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|--------|--------|---------------------------------------------------------| +| Plan | $O(1)$ | $O(1)$ | At most one square root and one cube root, no iteration | +| Sample | $O(1)$ | $O(1)$ | Locate one of seven phases, evaluate one cubic | + +Stored state is a few fixed-size tables of phase boundaries on the stack; no heap is used. Cost is independent of $h$ and of the branch taken. + +## Step-by-Step Walkthrough + +Limits $(v_{\max}, a_{\max}, j_{\max}) = (1, 2, 10)$, $h = 0.3$. + +1. Step 1: $v_{\max} j_{\max} = 10 \ge 4$, so $T_j = 0.2$, $T_a = 0.7$, $T_v = 0.3 - 0.7 = -0.4 < 0$. +2. Step 2: $T_v = 0$, $\Delta = 0.16 + 2.4 = 2.56$, $T_a = (0.4 + 1.6)/4 = 0.5 \ge 2T_j = 0.4$, so step 3 is not needed. Then $a_{lim} = 2$, $v_{lim} = 2(0.5 - 0.2) = 0.6$, $T = 1.0$. + +Boundary states $(a, v, s)$ of the acceleration stage: at $t = 0.2$, $(2,\, 0.2,\, 0.0133)$; at $t = 0.3$, $(2,\, 0.4,\, 0.0433)$; at $t = 0.5$, $(0,\, 0.6,\, 0.15)$. The mirrored stage ends at $s = 0.3$, at rest. A sample at $t = 0.4$ lies in phase 3 with $\tau = 0.1$: $a = 1$, $v = 0.55$, $s = 0.0917$. + +| $(v_{\max}, a_{\max}, j_{\max})$ | $h$ | Branch | $T_j$ | $T_a$ | $T_v$ | $T$ | +|----------------------------------|------|-------------|-------|-------|-------|-----| +| (1, 2, 10) | 5 | 1, plateau | 0.2 | 0.7 | 4.3 | 5.7 | +| (1, 2, 1) | 5 | 1, triangle | 1 | 2 | 3 | 7 | +| (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | +| (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | + +For $(1, 2, 10)$ the thresholds are $h \ge 0.7$ for step 1 and $h \ge 2a_{\max}^3/j_{\max}^2 = 0.16$ for step 2. The last row has $a_{lim} = 1$ and $v_{lim} = 0.1$. + +## Pitfalls & Edge Cases + +- **Positive limits.** The velocity, acceleration and jerk ceilings must all be strictly positive. +- **Zero distance.** With $h = 0$ every phase has zero length; the profile is the start state, at rest. +- **Rest-to-rest only.** Non-zero boundary velocities need the iterative acceleration reduction of the general case, which is not covered. +- **Float-only.** Only floating-point types are supported; there is no fixed-point variant. +- **Validation.** On 20 000 random $(h, v_{\max}, a_{\max}, j_{\max})$ plans, every plan ended at $h$ at rest and never exceeded the limits. + +## Variants & Generalizations + +- **Trapezoid** — the limit $j_{\max} \to \infty$: jerk phases vanish and acceleration steps. +- **Non-zero boundary velocities** — same seven phases, with the acceleration ceiling reduced iteratively until the move is feasible. + +## Applications + +- **Pick-and-place, CNC, semiconductor handling** — fast moves without exciting resonances or payload sway; time-scaled axes finish together. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|----------------------------|-----------------------------------------------------------------| +| Trapezoidal profile | Jerk-unbounded predecessor; shares the synchronization scheme | +| Quintic polynomial profile | Fixed duration and smooth, but limits are not enforced directly | + +## References & Further Reading + +- Biagiotti, L. and Melchiorri, C., *Trajectory Planning for Automatic Machines and Robots*, Springer, 2008, §3.4. diff --git a/doc/motion_profiles/TrapezoidalProfile.md b/doc/motion_profiles/TrapezoidalProfile.md new file mode 100644 index 00000000..3df87851 --- /dev/null +++ b/doc/motion_profiles/TrapezoidalProfile.md @@ -0,0 +1,111 @@ +# Trapezoidal Profile (Linear Segment with Parabolic Blends) + +## Overview & Motivation + +A point-to-point move has to reach its goal as fast as the actuator allows without saturating the drive. The trapezoidal profile — *linear segment with parabolic blends* (LSPB) — does this with the simplest rule that respects both a velocity ceiling and an acceleration ceiling: accelerate at the maximum rate, coast at the maximum speed, decelerate at the maximum rate. Velocity plotted against time is a trapezoid; a move too short to reach the ceiling becomes a triangle. + +From the limits and the distance alone, the plan is the **minimum-time** one. A second mode fixes the duration instead, which multi-axis synchronization needs: all axes must finish together, so the faster ones are slowed rather than the slowest sped up. + +## Mathematical Theory + +### Problem Statement + +Rest-to-rest move of length $d = |q_f - q_0|$ in direction $\sigma = \pm 1$ (positive when $q_f \ge q_0$), with limits $v_{max} > 0$ and $a_{max} > 0$. The profile is planned for the magnitude and mirrored by $\sigma$. + +### Minimum-Time Plan + +A ramp between rest and speed $v$ takes $v/a_{max}$ and covers $v^2/(2a_{max})$, so the two ramps together consume the **blend distance** $v^2/a_{max}$. Comparing $d$ with the blend distance at $v_{max}$ selects the shape: + +$$ +\begin{cases} +d \ge v_{max}^2/a_{max}: & v = v_{max}, \quad t_a = v/a_{max}, \quad t_c = (d - v^2/a_{max})/v \quad\text{(trapezoid)} \\ +d < v_{max}^2/a_{max}: & v = \sqrt{d\,a_{max}}, \quad t_a = v/a_{max}, \quad t_c = 0 \quad\text{(triangle)} +\end{cases} +$$ + +The total time is $T = 2t_a + t_c$. The cases agree at $d = v_{max}^2/a_{max}$, so $T$ is continuous in $d$. + +### Sampling + +With $v$, $t_a$, $t_c$, $T$ precomputed, the unsigned state is: + +| Phase | Interval | Acceleration | Velocity | Position | +|---------------------|-------------------------|--------------|------------------|---------------------------------| +| Parabolic ramp-up | $0 \le t < t_a$ | $a_{max}$ | $a_{max}\,t$ | $\tfrac12 a_{max}\,t^2$ | +| Linear cruise | $t_a \le t < t_a + t_c$ | $0$ | $v$ | $v\,(t - t_a/2)$ | +| Parabolic ramp-down | $t_a + t_c \le t < T$ | $-a_{max}$ | $a_{max}\,(T-t)$ | $d - \tfrac12 a_{max}\,(T-t)^2$ | + +Velocity, acceleration and the position offset are multiplied by $\sigma$. At and beyond $t = T$ the profile holds the goal at rest (velocity and acceleration zero); times before $0$ are treated as $t = 0$, holding the start with zero velocity. The reported jerk is zero in every phase. + +### Fixed-Duration Plan + +The acceleration $a_{max}$ is kept and the cruise speed is lowered until the move takes exactly $T$. The ramps cover $v\,t_a$ in total and the cruise covers $v\,(T - 2t_a)$, so with $t_a = v/a_{max}$ + +$$ +d = v\left(T - \frac{v}{a_{max}}\right) \;\Longleftrightarrow\; v^2 - a_{max}T\,v + a_{max}\,d = 0 . +$$ + +The smaller root keeps $t_c \ge 0$. Multiplying it by its conjugate gives a form with no subtraction of nearly equal numbers: + +$$ +v = \frac{a_{max}T - \sqrt{a_{max}^2T^2 - 4a_{max}d}}{2} = \frac{2\,a_{max}\,d}{a_{max}T + \sqrt{a_{max}^2T^2 - 4a_{max}d}} . +$$ + +A real root exists iff $a_{max}\,T^2 \ge 4d$. Equality gives a double root $v = a_{max}T/2$ and a triangle: $T$ is then the shortest duration achievable at this acceleration. Otherwise $t_a = v/a_{max}$ and $t_c = T - 2t_a$. + +### Multi-Axis Synchronization + +Let $T_{sync}$ be the longest axis time and $\lambda = T_{sync}/T_{axis} \ge 1$. Planning an axis again with $(v_{max}/\lambda,\; a_{max}/\lambda^2)$ leaves the blend distance $v_{max}^2/a_{max}$ unchanged, so the same shape is selected, while $t_a$ and $t_c$ both scale by $\lambda$: the duration is exactly $\lambda\,T_{axis}$. Velocity is divided by $\lambda$ and acceleration by $\lambda^2$. + +The fixed-duration plan is the alternative. It keeps $a_{max}$ and only lowers the cruise speed: the smaller root decreases as $T$ grows, so for $T \ge T_{min}$ the cruise speed never exceeds $v_{max}$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------------|--------|--------|--------------------------------------------------------| +| Minimum-time plan | $O(1)$ | $O(1)$ | One comparison, one square root (triangle only) | +| Fixed-duration plan | $O(1)$ | $O(1)$ | One square root, one division | +| Sample | $O(1)$ | $O(1)$ | One phase branch, a few multiply-adds; no state change | + +Memory is a fixed handful of scalars: start, direction, distance, acceleration, peak speed and three phase times. + +## Step-by-Step Walkthrough + +**Trapezoid.** $d = 5$, $v_{max} = 1$, $a_{max} = 2$. The blend distance is $1/2 = 0.5 \le 5$, so a cruise exists: $t_a = 0.5$, $t_c = (5 - 0.5)/1 = 4.5$, $T = 2(0.5) + 4.5 = 5.5$. + +| $t$ | Phase | Position | Velocity | Acceleration | +|-----------|-----------|--------------------------------------------|----------|--------------| +| $0.25$ | ramp-up | $\tfrac12\cdot 2\cdot 0.25^2 = 0.0625$ | $0.5$ | $2$ | +| $3$ | cruise | $1\cdot(3 - 0.25) = 2.75$ | $1$ | $0$ | +| $5.25$ | ramp-down | $5 - \tfrac12\cdot 2\cdot 0.25^2 = 4.9375$ | $0.5$ | $-2$ | +| $\ge 5.5$ | held | $5$ | $0$ | $0$ | + +The ramp-up ends at position $0.25$, and the cruise formula gives the same value at $t = 0.5$. The triangle threshold for these limits is $d = 0.5$; for $d = 0.25$, $v = \sqrt{0.5} \approx 0.7071$ and $T \approx 0.7071$, with no cruise. + +**Fixed duration.** $d = 4$, $a_{max} = 1$, $T = 5$: discriminant $25 - 16 = 9$, $v = 2\cdot 4/(5 + 3) = 1$, $t_a = 1$, $t_c = 3$; check $1\cdot(5 - 1) = 4$. For $d = 5$, $a_{max} = 2$, $T = 6$: discriminant $144 - 40 = 104$, $v = 20/(12 + \sqrt{104}) \approx 0.900980$. + +## Pitfalls & Edge Cases + +- **Non-positive limits.** The minimum-time plan requires $v_{max} > 0$ and $a_{max} > 0$; a violation is an assertion failure, not a recoverable status. The fixed-duration plan returns no result for $a_{max} \le 0$ or $T \le 0$. +- **Infeasible duration.** If $a_{max}T^2 < 4d$ the plan is rejected, never clamped; the shortest feasible duration is $2\sqrt{d/a_{max}}$. At the boundary, single-precision rounding can push the discriminant just below zero, so plan the slowest axis with the minimum-time plan. +- **Cancellation.** The textbook root $(a_{max}T - \sqrt{\cdot})/2$ loses its significant digits in single precision when $4a_{max}d \ll a_{max}^2T^2$; the conjugate form does not. The computed cruise time is clamped at zero. +- **Zero distance.** The minimum-time plan has $v = 0$ and $T = 0$: a zero-length profile that is at rest at the goal for every $t \ge 0$. +- **Discontinuous acceleration.** Position and velocity are continuous, but acceleration jumps between $0$ and $\pm a_{max}$ at the joins. The jerk impulses are not represented in the reported jerk and can excite structural modes; use the S-curve profile in that case. + +## Variants & Generalizations + +The **S-curve** profile adds a jerk limit (seven segments) for continuous acceleration at the cost of a longer move. **Polynomial** profiles (cubic, quintic) are fixed-duration and smooth by construction but enforce no velocity or acceleration ceiling. Unequal acceleration and deceleration limits use the same construction with unequal ramp times. + +## Applications + +- **Point-to-point axis moves** — servo and stepper drives, gantries, pick-and-place, gimbal slews. +- **Multi-axis coordination** — time-scaled or fixed-duration plans make every axis arrive together. + +## Connections to Other Algorithms + +The S-curve profile is the jerk-limited upgrade under the same velocity and acceleration limits. The polynomial profile is fixed-time and smooth but offers no limit guarantees. The saturation rate limiter slews a changing setpoint online, whereas the trapezoid plans a whole move offline. + +## References & Further Reading + +- Biagiotti, L. and Melchiorri, C., "Trajectory Planning for Automatic Machines and Robots," Springer, 2008, §3.2. +- Siciliano, B., Sciavicco, L., Villani, L., and Oriolo, G., "Robotics: Modelling, Planning and Control," Springer, 2009, §4.2. diff --git a/numerical/CMakeLists.txt b/numerical/CMakeLists.txt index 0166abbd..c61f65fb 100644 --- a/numerical/CMakeLists.txt +++ b/numerical/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(controllers) add_subdirectory(estimators) add_subdirectory(filters) add_subdirectory(math) +add_subdirectory(motion_profiles) add_subdirectory(optimization) add_subdirectory(regularization) add_subdirectory(nonlinear_control) diff --git a/numerical/math/Math.cpp b/numerical/math/Math.cpp index 6146cec2..1d258b3b 100644 --- a/numerical/math/Math.cpp +++ b/numerical/math/Math.cpp @@ -8,6 +8,9 @@ template float math::Abs(float); #ifndef MATH_SQRT_OVERRIDE template float math::Sqrt(float); #endif +#ifndef MATH_CBRT_OVERRIDE +template float math::Cbrt(float); +#endif #ifndef MATH_SIN_OVERRIDE template float math::Sin(float); #endif diff --git a/numerical/math/Math.hpp b/numerical/math/Math.hpp index eed45f71..0d279a58 100644 --- a/numerical/math/Math.hpp +++ b/numerical/math/Math.hpp @@ -25,6 +25,15 @@ namespace math } #endif +#ifndef MATH_CBRT_OVERRIDE + template + constexpr T Cbrt(T x) + { + static_assert(std::is_floating_point_v, "T must be a floating-point type"); + return std::cbrt(x); + } +#endif + #ifndef MATH_SIN_OVERRIDE template constexpr T Sin(T x) @@ -252,6 +261,9 @@ namespace math #ifndef MATH_SQRT_OVERRIDE extern template float Sqrt(float); #endif +#ifndef MATH_CBRT_OVERRIDE + extern template float Cbrt(float); +#endif #ifndef MATH_SIN_OVERRIDE extern template float Sin(float); #endif diff --git a/numerical/math/test/TestMath.cpp b/numerical/math/test/TestMath.cpp index 8f8d34f8..595d7240 100644 --- a/numerical/math/test/TestMath.cpp +++ b/numerical/math/test/TestMath.cpp @@ -127,6 +127,12 @@ TEST_F(TestMath, Copysign_transfers_sign) EXPECT_FLOAT_EQ(math::Copysign(-3.0f, 1.0f), 3.0f); } +TEST_F(TestMath, Cbrt_known_value) +{ + EXPECT_NEAR(math::Cbrt(27.0f), 3.0f, kEps); + EXPECT_NEAR(math::Cbrt(-0.001f), -0.1f, kEps); +} + TEST_F(TestMath, Fmod_known_value) { EXPECT_NEAR(math::Fmod(5.5f, 2.0f), 1.5f, kEps); diff --git a/numerical/motion_profiles/CMakeLists.txt b/numerical/motion_profiles/CMakeLists.txt new file mode 100644 index 00000000..e5022541 --- /dev/null +++ b/numerical/motion_profiles/CMakeLists.txt @@ -0,0 +1,26 @@ +numerical_add_header_library(numerical.motion_profiles) + +target_include_directories(numerical.motion_profiles ${NUMERICAL_VISIBILITY} + "$" + "$" +) + +target_link_libraries(numerical.motion_profiles ${NUMERICAL_VISIBILITY} + infra.util + numerical.math +) + +target_sources(numerical.motion_profiles PRIVATE + MotionProfileTypes.hpp + PolynomialProfile.hpp + SCurveProfile.hpp + TrapezoidalProfile.hpp +) + +numerical_add_coverage_sources(numerical.motion_profiles + PolynomialProfile.cpp + SCurveProfile.cpp + TrapezoidalProfile.cpp +) + +add_subdirectory(test) diff --git a/numerical/motion_profiles/MotionProfileTypes.hpp b/numerical/motion_profiles/MotionProfileTypes.hpp new file mode 100644 index 00000000..9ebf3030 --- /dev/null +++ b/numerical/motion_profiles/MotionProfileTypes.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include + +namespace motion_profiles +{ + template + struct TrajectoryState + { + static_assert(std::is_floating_point_v, "TrajectoryState supports floating-point types only"); + + T position{}; + T velocity{}; + T acceleration{}; + T jerk{}; + + bool operator==(const TrajectoryState& other) const = default; + }; + + template + struct MotionLimits + { + static_assert(std::is_floating_point_v, "MotionLimits supports floating-point types only"); + + T vMax{}; + T aMax{}; + T jMax{}; + }; +} diff --git a/numerical/motion_profiles/PolynomialProfile.cpp b/numerical/motion_profiles/PolynomialProfile.cpp new file mode 100644 index 00000000..6a756330 --- /dev/null +++ b/numerical/motion_profiles/PolynomialProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/PolynomialProfile.hpp" + +namespace motion_profiles +{ + template class PolynomialProfile; +} diff --git a/numerical/motion_profiles/PolynomialProfile.hpp b/numerical/motion_profiles/PolynomialProfile.hpp new file mode 100644 index 00000000..5e4d6934 --- /dev/null +++ b/numerical/motion_profiles/PolynomialProfile.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include +#include + +namespace motion_profiles +{ + enum class Degree : std::uint8_t + { + Cubic = 3, + Quintic = 5 + }; + + template + struct BoundaryConditions + { + static_assert(std::is_floating_point_v, "BoundaryConditions supports floating-point types only"); + + T q0{}; + T qf{}; + T v0{}; + T vf{}; + T a0{}; + T af{}; + }; + + template + class PolynomialProfile + { + static_assert(std::is_floating_point_v, "PolynomialProfile supports floating-point types only"); + + public: + PolynomialProfile(const BoundaryConditions& bc, T tf, Degree degree); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + void Reset(const BoundaryConditions& bc, T tf); + + private: + void SolveCubic(const BoundaryConditions& bc); + void SolveQuintic(const BoundaryConditions& bc); + + std::array coeff{}; + T duration{}; + Degree degree; + }; + + template + PolynomialProfile::PolynomialProfile(const BoundaryConditions& bc, T tf, Degree degree) + : degree{ degree } + { + Reset(bc, tf); + } + + template + void PolynomialProfile::Reset(const BoundaryConditions& bc, T tf) + { + really_assert(tf > T{}); + + duration = tf; + if (degree == Degree::Cubic) + SolveCubic(bc); + else + SolveQuintic(bc); + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState PolynomialProfile::Sample(T t) const + { + t = std::min(std::max(t, T{}), duration); + + const auto& a{ coeff }; + return { + a[0] + t * (a[1] + t * (a[2] + t * (a[3] + t * (a[4] + t * a[5])))), + a[1] + t * (T{ 2 } * a[2] + t * (T{ 3 } * a[3] + t * (T{ 4 } * a[4] + t * T{ 5 } * a[5]))), + T{ 2 } * a[2] + t * (T{ 6 } * a[3] + t * (T{ 12 } * a[4] + t * T{ 20 } * a[5])), + T{ 6 } * a[3] + t * (T{ 24 } * a[4] + t * T{ 60 } * a[5]) + }; + } + + template + T PolynomialProfile::Duration() const + { + return duration; + } + + template + void PolynomialProfile::SolveCubic(const BoundaryConditions& bc) + { + const T tf{ duration }; + const T distance{ bc.qf - bc.q0 }; + + coeff = { + bc.q0, + bc.v0, + (T{ 3 } * distance - (T{ 2 } * bc.v0 + bc.vf) * tf) / (tf * tf), + (T{ -2 } * distance + (bc.v0 + bc.vf) * tf) / (tf * tf * tf), + T{}, + T{} + }; + } + + template + void PolynomialProfile::SolveQuintic(const BoundaryConditions& bc) + { + const T tf{ duration }; + const T tf2{ tf * tf }; + const T tf3{ tf2 * tf }; + const T distance{ bc.qf - bc.q0 }; + + coeff = { + bc.q0, + bc.v0, + bc.a0 / T{ 2 }, + (T{ 20 } * distance - (T{ 8 } * bc.vf + T{ 12 } * bc.v0) * tf - (T{ 3 } * bc.a0 - bc.af) * tf2) / (T{ 2 } * tf3), + (T{ -30 } * distance + (T{ 14 } * bc.vf + T{ 16 } * bc.v0) * tf + (T{ 3 } * bc.a0 - T{ 2 } * bc.af) * tf2) / (T{ 2 } * tf3 * tf), + (T{ 12 } * distance - T{ 6 } * (bc.vf + bc.v0) * tf - (bc.a0 - bc.af) * tf2) / (T{ 2 } * tf3 * tf2) + }; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class PolynomialProfile; +#endif +} diff --git a/numerical/motion_profiles/SCurveProfile.cpp b/numerical/motion_profiles/SCurveProfile.cpp new file mode 100644 index 00000000..2550fec2 --- /dev/null +++ b/numerical/motion_profiles/SCurveProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/SCurveProfile.hpp" + +namespace motion_profiles +{ + template class SCurveProfile; +} diff --git a/numerical/motion_profiles/SCurveProfile.hpp b/numerical/motion_profiles/SCurveProfile.hpp new file mode 100644 index 00000000..88e472d5 --- /dev/null +++ b/numerical/motion_profiles/SCurveProfile.hpp @@ -0,0 +1,157 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include +#include + +namespace motion_profiles +{ + template + class SCurveProfile + { + static_assert(std::is_floating_point_v, "SCurveProfile supports floating-point types only"); + + public: + SCurveProfile(T q0, T qf, const MotionLimits& limits); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + bool ReachesMaxAccel() const; + bool ReachesMaxVel() const; + + private: + static constexpr std::size_t phases{ 7 }; + static constexpr std::array jerkSign{ T{ 1 }, T{}, T{ -1 }, T{}, T{ -1 }, T{}, T{ 1 } }; + + void Plan(const MotionLimits& limits); + void Integrate(T jerkPhase, T accelPhase, T cruisePhase); + + T q0; + T direction; + T distance; + T jMax; + std::array segStart{}; + std::array segPos{}; + std::array segVel{}; + std::array segAcc{}; + T tf{}; + bool reachesMaxAccel{ false }; + bool reachesMaxVel{ false }; + }; + + template + SCurveProfile::SCurveProfile(T q0, T qf, const MotionLimits& limits) + : q0{ q0 } + , direction{ qf >= q0 ? T{ 1 } : T{ -1 } } + , distance{ math::Abs(qf - q0) } + , jMax{ limits.jMax } + { + really_assert(limits.vMax > T{}); + really_assert(limits.aMax > T{}); + really_assert(limits.jMax > T{}); + + if (distance > T{}) + Plan(limits); + } + + template + void SCurveProfile::Plan(const MotionLimits& limits) + { + const T vMax{ limits.vMax }; + const T aMax{ limits.aMax }; + + T tj{ aMax / jMax }; + T ta{ tj + vMax / aMax }; + if (vMax * jMax < aMax * aMax) + { + tj = math::Sqrt(vMax / jMax); + ta = T{ 2 } * tj; + } + + T tv{ distance / vMax - ta }; + if (tv < T{}) + { + tv = T{}; + tj = aMax / jMax; + const T delta{ aMax * aMax * aMax * aMax / (jMax * jMax) + T{ 4 } * aMax * distance }; + ta = (aMax * aMax / jMax + math::Sqrt(delta)) / (T{ 2 } * aMax); + + if (ta < T{ 2 } * tj) + { + tj = math::Cbrt(distance / (T{ 2 } * jMax)); + ta = T{ 2 } * tj; + } + } + + const T accelPhase{ std::max(ta - T{ 2 } * tj, T{}) }; + reachesMaxAccel = accelPhase > T{}; + reachesMaxVel = tv > T{}; + Integrate(tj, accelPhase, tv); + } + + template + void SCurveProfile::Integrate(T jerkPhase, T accelPhase, T cruisePhase) + { + const std::array duration{ jerkPhase, accelPhase, jerkPhase, cruisePhase, jerkPhase, accelPhase, jerkPhase }; + + for (std::size_t i = 0; i < phases; ++i) + { + const T tau{ duration[i] }; + const T jerk{ jerkSign[i] * jMax }; + segPos[i + 1] = segPos[i] + tau * (segVel[i] + tau * (segAcc[i] / T{ 2 } + tau * jerk / T{ 6 })); + segVel[i + 1] = segVel[i] + tau * (segAcc[i] + tau * jerk / T{ 2 }); + segAcc[i + 1] = segAcc[i] + tau * jerk; + segStart[i + 1] = segStart[i] + tau; + } + + tf = segStart[phases]; + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState SCurveProfile::Sample(T t) const + { + t = std::max(t, T{}); + if (t >= tf) + return { q0 + direction * distance, T{}, T{}, T{} }; + + std::size_t i{ 0 }; + while (i + 1 < phases && segStart[i + 1] <= t) + ++i; + + const T tau{ t - segStart[i] }; + const T jerk{ jerkSign[i] * jMax }; + return { + q0 + direction * (segPos[i] + tau * (segVel[i] + tau * (segAcc[i] / T{ 2 } + tau * jerk / T{ 6 }))), + direction * (segVel[i] + tau * (segAcc[i] + tau * jerk / T{ 2 })), + direction * (segAcc[i] + tau * jerk), + direction * jerk + }; + } + + template + T SCurveProfile::Duration() const + { + return tf; + } + + template + bool SCurveProfile::ReachesMaxAccel() const + { + return reachesMaxAccel; + } + + template + bool SCurveProfile::ReachesMaxVel() const + { + return reachesMaxVel; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class SCurveProfile; +#endif +} diff --git a/numerical/motion_profiles/TrapezoidalProfile.cpp b/numerical/motion_profiles/TrapezoidalProfile.cpp new file mode 100644 index 00000000..d9e5ed76 --- /dev/null +++ b/numerical/motion_profiles/TrapezoidalProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/TrapezoidalProfile.hpp" + +namespace motion_profiles +{ + template class TrapezoidalProfile; +} diff --git a/numerical/motion_profiles/TrapezoidalProfile.hpp b/numerical/motion_profiles/TrapezoidalProfile.hpp new file mode 100644 index 00000000..494138b8 --- /dev/null +++ b/numerical/motion_profiles/TrapezoidalProfile.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include + +namespace motion_profiles +{ + template + class TrapezoidalProfile + { + static_assert(std::is_floating_point_v, "TrapezoidalProfile supports floating-point types only"); + + public: + TrapezoidalProfile(T q0, T qf, const MotionLimits& limits); + static std::optional PlanWithDuration(T q0, T qf, T aMax, T duration); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + bool IsTriangular() const; + + private: + TrapezoidalProfile(T q0, T qf, T aMax); + + T q0; + T direction; + T distance; + T aMax; + T vPeak{}; + T tAccel{}; + T tCruise{}; + T tf{}; + }; + + template + TrapezoidalProfile::TrapezoidalProfile(T q0, T qf, T aMax) + : q0{ q0 } + , direction{ qf >= q0 ? T{ 1 } : T{ -1 } } + , distance{ math::Abs(qf - q0) } + , aMax{ aMax } + {} + + template + TrapezoidalProfile::TrapezoidalProfile(T q0, T qf, const MotionLimits& limits) + : TrapezoidalProfile{ q0, qf, limits.aMax } + { + really_assert(limits.vMax > T{}); + really_assert(limits.aMax > T{}); + + const T blendDistance{ limits.vMax * limits.vMax / limits.aMax }; + if (distance >= blendDistance) + { + vPeak = limits.vMax; + tCruise = (distance - blendDistance) / vPeak; + } + else + vPeak = math::Sqrt(distance * aMax); + + tAccel = vPeak / aMax; + tf = T{ 2 } * tAccel + tCruise; + } + + template + std::optional> TrapezoidalProfile::PlanWithDuration(T q0, T qf, T aMax, T duration) + { + TrapezoidalProfile profile{ q0, qf, aMax }; + const T discriminant{ aMax * aMax * duration * duration - T{ 4 } * aMax * profile.distance }; + if (aMax <= T{} || duration <= T{} || discriminant < T{}) + return std::nullopt; + + profile.vPeak = T{ 2 } * aMax * profile.distance / (aMax * duration + math::Sqrt(discriminant)); + profile.tAccel = profile.vPeak / aMax; + profile.tCruise = std::max(duration - T{ 2 } * profile.tAccel, T{}); + profile.tf = duration; + return profile; + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState TrapezoidalProfile::Sample(T t) const + { + t = std::max(t, T{}); + if (t >= tf) + return { q0 + direction * distance, T{}, T{}, T{} }; + + if (t < tAccel) + return { q0 + direction * T{ 0.5 } * aMax * t * t, direction * aMax * t, direction * aMax, T{} }; + + if (t < tAccel + tCruise) + return { q0 + direction * vPeak * (t - T{ 0.5 } * tAccel), direction * vPeak, T{}, T{} }; + + const T remaining{ tf - t }; + return { q0 + direction * (distance - T{ 0.5 } * aMax * remaining * remaining), direction * aMax * remaining, -direction * aMax, T{} }; + } + + template + T TrapezoidalProfile::Duration() const + { + return tf; + } + + template + bool TrapezoidalProfile::IsTriangular() const + { + return tCruise <= T{}; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class TrapezoidalProfile; +#endif +} diff --git a/numerical/motion_profiles/test/CMakeLists.txt b/numerical/motion_profiles/test/CMakeLists.txt new file mode 100644 index 00000000..24459986 --- /dev/null +++ b/numerical/motion_profiles/test/CMakeLists.txt @@ -0,0 +1,16 @@ +add_executable(numerical.motion_profiles_test) +emil_build_for(numerical.motion_profiles_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) +emil_add_test(numerical.motion_profiles_test) + +target_link_libraries(numerical.motion_profiles_test PUBLIC + gmock_main + numerical.motion_profiles +) + +numerical_link_qemu_runtime(numerical.motion_profiles_test) + +target_sources(numerical.motion_profiles_test PRIVATE + TestPolynomialProfile.cpp + TestSCurveProfile.cpp + TestTrapezoidalProfile.cpp +) diff --git a/numerical/motion_profiles/test/TestPolynomialProfile.cpp b/numerical/motion_profiles/test/TestPolynomialProfile.cpp new file mode 100644 index 00000000..0371cfde --- /dev/null +++ b/numerical/motion_profiles/test/TestPolynomialProfile.cpp @@ -0,0 +1,85 @@ +#include "numerical/motion_profiles/PolynomialProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::BoundaryConditions; + using motion_profiles::Degree; + using motion_profiles::PolynomialProfile; + + class TestPolynomialProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + const BoundaryConditions restToRest{ 0.0f, 1.0f }; + PolynomialProfile cubic{ restToRest, 2.0f, Degree::Cubic }; + }; +} + +TEST_F(TestPolynomialProfile, endpoints_match_boundary_positions) +{ + EXPECT_NEAR(cubic.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(cubic.Sample(cubic.Duration()).position, 1.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, endpoint_velocities_are_matched) +{ + EXPECT_NEAR(cubic.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(cubic.Sample(cubic.Duration()).velocity, 0.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_midpoint_is_symmetric) +{ + EXPECT_NEAR(cubic.Sample(1.0f).position, 0.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_peak_velocity_matches_formula) +{ + float peak{ 0.0f }; + for (int i = 0; i <= 200; ++i) + peak = std::max(peak, std::abs(cubic.Sample(0.01f * static_cast(i)).velocity)); + + EXPECT_NEAR(peak, 1.5f * 1.0f / 2.0f, tolerance); + EXPECT_NEAR(cubic.Sample(1.0f).velocity, 0.75f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_jerk_is_constant_third_derivative) +{ + for (float t : { 0.0f, 1.0f, 2.0f }) + EXPECT_NEAR(cubic.Sample(t).jerk, -1.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, quintic_endpoint_accelerations_are_zero) +{ + const PolynomialProfile quintic{ restToRest, 2.0f, Degree::Quintic }; + + EXPECT_NEAR(quintic.Sample(0.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).position, 1.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).velocity, 0.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, nonzero_boundary_velocity_is_honoured) +{ + const PolynomialProfile moving{ BoundaryConditions{ 0.0f, 1.0f, 0.5f, -0.5f }, 2.0f, Degree::Cubic }; + + EXPECT_NEAR(moving.Sample(0.0f).velocity, 0.5f, tolerance); + EXPECT_NEAR(moving.Sample(2.0f).velocity, -0.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(cubic.Sample(-1.0f), cubic.Sample(0.0f)); + EXPECT_EQ(cubic.Sample(cubic.Duration() + 1.0f), cubic.Sample(cubic.Duration())); +} + +TEST_F(TestPolynomialProfile, reset_recomputes_coefficients) +{ + cubic.Reset(BoundaryConditions{ 1.0f, 3.0f }, 1.0f); + + EXPECT_EQ(cubic.Duration(), 1.0f); + EXPECT_NEAR(cubic.Sample(0.0f).position, 1.0f, tolerance); + EXPECT_NEAR(cubic.Sample(1.0f).position, 3.0f, tolerance); +} diff --git a/numerical/motion_profiles/test/TestSCurveProfile.cpp b/numerical/motion_profiles/test/TestSCurveProfile.cpp new file mode 100644 index 00000000..ec14baec --- /dev/null +++ b/numerical/motion_profiles/test/TestSCurveProfile.cpp @@ -0,0 +1,154 @@ +#include "numerical/motion_profiles/SCurveProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::MotionLimits; + using motion_profiles::SCurveProfile; + using motion_profiles::TrajectoryState; + + class TestSCurveProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + static constexpr int samples{ 5000 }; + const MotionLimits limits{ 1.0f, 2.0f, 10.0f }; + const SCurveProfile profile{ 0.0f, 5.0f, limits }; + + static TrajectoryState SampleAt(const SCurveProfile& p, int i) + { + return p.Sample(p.Duration() * static_cast(i) / static_cast(samples)); + } + + template + static float MaxOver(const SCurveProfile& p, Metric metric) + { + float maximum{ 0.0f }; + for (int i = 0; i <= samples; ++i) + maximum = std::max(maximum, metric(SampleAt(p, i))); + + return maximum; + } + }; +} + +TEST_F(TestSCurveProfile, endpoints_reached_exactly) +{ + EXPECT_NEAR(profile.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).position, 5.0f, tolerance); +} + +TEST_F(TestSCurveProfile, endpoints_at_rest_and_zero_accel) +{ + EXPECT_NEAR(profile.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(0.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).acceleration, 0.0f, tolerance); +} + +TEST_F(TestSCurveProfile, velocity_never_exceeds_vmax) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.velocity); + }), + limits.vMax + tolerance); +} + +TEST_F(TestSCurveProfile, acceleration_never_exceeds_amax) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.acceleration); + }), + limits.aMax + tolerance); +} + +TEST_F(TestSCurveProfile, jerk_never_exceeds_jmax) +{ + for (int i = 0; i <= samples; ++i) + { + const float jerk{ std::abs(SampleAt(profile, i).jerk) }; + EXPECT_TRUE(jerk == 0.0f || jerk == limits.jMax) << "jerk = " << jerk; + } +} + +TEST_F(TestSCurveProfile, acceleration_is_continuous) +{ + const float dt{ profile.Duration() / static_cast(samples) }; + + for (int i = 1; i <= samples; ++i) + EXPECT_LE(std::abs(SampleAt(profile, i).acceleration - SampleAt(profile, i - 1).acceleration), limits.jMax * dt + tolerance); +} + +TEST_F(TestSCurveProfile, step1_full_profile_durations) +{ + EXPECT_NEAR(profile.Duration(), 5.7f, tolerance); + EXPECT_NEAR(profile.Sample(0.2f).acceleration, 2.0f, tolerance); + EXPECT_NEAR(profile.Sample(0.7f).velocity, 1.0f, tolerance); + EXPECT_TRUE(profile.ReachesMaxAccel()); + EXPECT_TRUE(profile.ReachesMaxVel()); +} + +TEST_F(TestSCurveProfile, step1_triangular_acceleration_when_jerk_low) +{ + const SCurveProfile lowJerk{ 0.0f, 5.0f, MotionLimits{ 1.0f, 2.0f, 1.0f } }; + + EXPECT_NEAR(lowJerk.Duration(), 7.0f, tolerance); + EXPECT_NEAR(lowJerk.Sample(1.0f).acceleration, 1.0f, tolerance); + EXPECT_NEAR(lowJerk.Sample(3.5f).velocity, 1.0f, tolerance); + EXPECT_FALSE(lowJerk.ReachesMaxAccel()); + EXPECT_TRUE(lowJerk.ReachesMaxVel()); +} + +TEST_F(TestSCurveProfile, step2_short_move_reaches_amax_not_vmax) +{ + const SCurveProfile shortMove{ 0.0f, 0.3f, limits }; + + EXPECT_NEAR(shortMove.Duration(), 1.0f, tolerance); + EXPECT_NEAR(shortMove.Sample(0.5f).velocity, 0.6f, tolerance); + EXPECT_NEAR(MaxOver(shortMove, [](const auto& s) + { + return std::abs(s.acceleration); + }), + 2.0f, tolerance); + EXPECT_TRUE(shortMove.ReachesMaxAccel()); + EXPECT_FALSE(shortMove.ReachesMaxVel()); + EXPECT_NEAR(shortMove.Sample(shortMove.Duration()).position, 0.3f, tolerance); +} + +TEST_F(TestSCurveProfile, step3_very_short_move_reaches_neither) +{ + const SCurveProfile veryShortMove{ 0.0f, 0.02f, limits }; + + EXPECT_NEAR(veryShortMove.Duration(), 0.4f, tolerance); + EXPECT_NEAR(veryShortMove.Sample(0.1f).acceleration, 1.0f, tolerance); + EXPECT_NEAR(veryShortMove.Sample(0.2f).velocity, 0.1f, tolerance); + EXPECT_FALSE(veryShortMove.ReachesMaxAccel()); + EXPECT_FALSE(veryShortMove.ReachesMaxVel()); + EXPECT_NEAR(veryShortMove.Sample(veryShortMove.Duration()).position, 0.02f, tolerance); +} + +TEST_F(TestSCurveProfile, negative_direction_mirrors_profile) +{ + const SCurveProfile backwards{ 5.0f, 0.0f, limits }; + + for (int i = 0; i <= samples; i += 50) + { + const auto forward{ SampleAt(profile, i) }; + const auto mirrored{ SampleAt(backwards, i) }; + + EXPECT_NEAR(mirrored.position, 5.0f - forward.position, tolerance); + EXPECT_NEAR(mirrored.velocity, -forward.velocity, tolerance); + EXPECT_NEAR(mirrored.acceleration, -forward.acceleration, tolerance); + EXPECT_NEAR(mirrored.jerk, -forward.jerk, tolerance); + } +} + +TEST_F(TestSCurveProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(profile.Sample(-1.0f), profile.Sample(0.0f)); + EXPECT_EQ(profile.Sample(profile.Duration() + 1.0f), profile.Sample(profile.Duration())); +} diff --git a/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp b/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp new file mode 100644 index 00000000..75e901b1 --- /dev/null +++ b/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp @@ -0,0 +1,118 @@ +#include "numerical/motion_profiles/TrapezoidalProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::MotionLimits; + using motion_profiles::TrapezoidalProfile; + + class TestTrapezoidalProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + const MotionLimits limits{ 1.0f, 2.0f }; + const TrapezoidalProfile profile{ 0.0f, 5.0f, limits }; + + template + static float MaxOver(const TrapezoidalProfile& p, Metric metric) + { + float maximum{ -1e30f }; + for (int i = 0; i <= 1000; ++i) + maximum = std::max(maximum, metric(p.Sample(p.Duration() * static_cast(i) / 1000.0f))); + + return maximum; + } + }; +} + +TEST_F(TestTrapezoidalProfile, endpoints_reached_exactly) +{ + EXPECT_NEAR(profile.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).position, 5.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, endpoints_are_at_rest) +{ + EXPECT_NEAR(profile.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).velocity, 0.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, cruise_velocity_is_clamped_to_vmax) +{ + const float peak{ MaxOver(profile, [](const auto& s) + { + return s.velocity; + }) }; + + EXPECT_NEAR(peak, limits.vMax, tolerance); + EXPECT_LE(peak, limits.vMax); +} + +TEST_F(TestTrapezoidalProfile, acceleration_is_within_limit) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.acceleration); + }), + limits.aMax); +} + +TEST_F(TestTrapezoidalProfile, short_move_is_triangular) +{ + const TrapezoidalProfile shortMove{ 0.0f, 0.1f, limits }; + + EXPECT_TRUE(shortMove.IsTriangular()); + EXPECT_FALSE(profile.IsTriangular()); + EXPECT_LT(MaxOver(shortMove, [](const auto& s) + { + return s.velocity; + }), + limits.vMax); + EXPECT_NEAR(shortMove.Sample(shortMove.Duration()).position, 0.1f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, phase_durations_match_trapezoid_formula) +{ + EXPECT_NEAR(profile.Duration(), 5.5f, tolerance); + EXPECT_NEAR(profile.Sample(0.25f).velocity, 0.5f, tolerance); + EXPECT_NEAR(profile.Sample(0.5f).velocity, 1.0f, tolerance); + EXPECT_NEAR(profile.Sample(5.0f).velocity, 1.0f, tolerance); + EXPECT_NEAR(profile.Sample(5.25f).velocity, 0.5f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, negative_direction_move) +{ + const TrapezoidalProfile backwards{ 2.0f, 0.0f, limits }; + + EXPECT_LE(MaxOver(backwards, [](const auto& s) + { + return s.velocity; + }), + 0.0f); + EXPECT_NEAR(backwards.Sample(0.0f).position, 2.0f, tolerance); + EXPECT_NEAR(backwards.Sample(backwards.Duration()).position, 0.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, plan_with_duration_lowers_cruise_speed) +{ + const auto planned{ TrapezoidalProfile::PlanWithDuration(0.0f, 4.0f, 1.0f, 5.0f) }; + + ASSERT_TRUE(planned.has_value()); + EXPECT_NEAR(planned->Duration(), 5.0f, tolerance); + EXPECT_NEAR(planned->Sample(0.5f).velocity, 0.5f, tolerance); + EXPECT_NEAR(planned->Sample(2.5f).velocity, 1.0f, tolerance); + EXPECT_NEAR(planned->Sample(5.0f).position, 4.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, plan_with_duration_rejects_infeasible_time) +{ + EXPECT_FALSE(TrapezoidalProfile::PlanWithDuration(0.0f, 4.0f, 1.0f, 3.0f).has_value()); +} + +TEST_F(TestTrapezoidalProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(profile.Sample(-1.0f), profile.Sample(0.0f)); + EXPECT_EQ(profile.Sample(profile.Duration() + 1.0f), profile.Sample(profile.Duration())); +} diff --git a/roadmap/README.md b/roadmap/README.md index c28cde8b..ab332c7c 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -85,8 +85,6 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `controllers` `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) -### `motion_profiles` -`PolynomialProfile` (55) · `TrapezoidalProfile` (56) · `SCurveProfile` (60) ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) diff --git a/roadmap/motion_profiles/PolynomialProfile/explanation.md b/roadmap/motion_profiles/PolynomialProfile/explanation.md deleted file mode 100644 index a3a048f7..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Polynomial Point-to-Point Profile — Overview - -## What it is -A closed-form time law that drives an axis from a start state to a goal state over a fixed -duration. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches -endpoint acceleration, giving a smoother (bounded-jerk) motion. - -## Why it matters (embedded) -Point-to-point moves are the bread and butter of servo motion — robot joints, CNC and pick-and-place -axes, motor position loops. The coefficients are solved **once** from a handful of algebraic -expressions, after which each servo tick is just a Horner polynomial evaluation — no iteration, no -lookup table, fully deterministic cycle count. - -## How it works (intuition) -A degree-`n` polynomial has `n+1` free coefficients. A cubic (4 coefficients) exactly satisfies -four boundary conditions — start/end position and start/end velocity. A quintic (6 coefficients) -adds start/end acceleration. Because the boundary equations are linear in the coefficients and the -time basis is fixed, the solution is a plug-in formula rather than a matrix solve. - -## Key parameters -- **Boundary conditions** — start/end position, velocity, and (quintic) acceleration. -- **Duration `tf`** — the move time; sets the peak velocity/acceleration for a given distance. -- **Degree** — cubic (velocity-matched) or quintic (acceleration-matched, jerk-limited endpoints). - -## Reference -M. W. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5 (polynomial -trajectories). - -## See also -`TrapezoidalProfile` (limit-respecting, not fixed-time), `SCurveProfile` (jerk-bounded), -`CubicSpline` (#61, many via points, C² — reduces to this cubic for two knots); robotics-toolbox-cpp's -`CartesianSlerpInterpolation` (task-space paths; a 0 → 1 polynomial can drive its time law). diff --git a/roadmap/motion_profiles/PolynomialProfile/implementation.md b/roadmap/motion_profiles/PolynomialProfile/implementation.md deleted file mode 100644 index b401898b..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/implementation.md +++ /dev/null @@ -1,112 +0,0 @@ -# Polynomial Point-to-Point Profile — Implementation Pseudocode - -> Roadmap ref: #55 (Tier 1) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -Shared motion-profile types — **canonical definition**, created once in -`numerical/motion_profiles/MotionProfileTypes.hpp` by the first `motion_profiles` item deployed; every other -profile spec includes it and never redefines these names (robotics-toolbox-cpp's time laws consume them too): - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: # one scalar axis - T position{}, velocity{}, acceleration{}, jerk{} # jerk = 0 where a profile leaves it undefined - -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax{}, aMax{} # > 0 for every limit-driven profile - T jMax{} # > 0 required only by SCurveProfile -``` - -This item: - -```cpp -enum class Degree : uint8_t { Cubic = 3, Quintic = 5 } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct BoundaryConditions: # per axis - T q0, qf # start / end position - T v0 = 0, vf = 0 # start / end velocity - T a0 = 0, af = 0 # start / end acceleration (quintic only) - -template # static_assert(std::is_floating_point_v); instantiated for float -class PolynomialProfile: - array coeff # a0..a5 (cubic uses a0..a3, a4 = a5 = 0) - T duration # tf > 0 - Degree degree -``` - -## Interface - -```text -PolynomialProfile(BoundaryConditions bc, T tf, Degree degree) -TrajectoryState Sample(T t) # hot path -T Duration() -void Reset(BoundaryConditions bc, T tf) # keeps the degree -``` - -## Algorithm (pseudocode) - -```text -function solveCubic(bc, tf): # closed form, no linear solve - a0 = bc.q0 - a1 = bc.v0 - a2 = ( 3*(bc.qf-bc.q0) - (2*bc.v0 + bc.vf)*tf) / tf^2 - a3 = (-2*(bc.qf-bc.q0) + ( bc.v0 + bc.vf)*tf) / tf^3 - a4 = a5 = 0 - -function solveQuintic(bc, tf): # six matched boundary conditions - a0 = bc.q0; a1 = bc.v0; a2 = bc.a0 / 2 - a3 = ( 20*Δq - (8*bc.vf + 12*bc.v0)*tf - (3*bc.a0 - bc.af)*tf^2) / (2*tf^3) - a4 = (-30*Δq + (14*bc.vf + 16*bc.v0)*tf + (3*bc.a0 - 2*bc.af)*tf^2) / (2*tf^4) - a5 = ( 12*Δq - ( 6*bc.vf + 6*bc.v0)*tf - ( bc.a0 - bc.af)*tf^2) / (2*tf^5) - # Δq = qf - q0 - -constructor / Reset: - degree == Degree::Cubic ? solveCubic(bc, tf) : solveQuintic(bc, tf) - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, duration) - pos = Horner(coeff, t) # a0 + a1 t + ... + a5 t^5 - vel = Horner(derivative(coeff), t) - acc = Horner(secondDerivative(coeff), t) - jerk = Horner(thirdDerivative(coeff), t) # 6 a3 + 24 a4 t + 60 a5 t^2 - return { pos, vel, acc, jerk } -``` - -## Complexity & memory - -- Coefficient solve: `O(1)` once at construction (closed-form expressions). -- `Sample`: `O(degree)` — four Horner evaluations, ≤ 14 multiply-adds. -- Memory: `O(1)` — six coefficients, a duration and the degree; all stack-resident, no heap. - -## Numerical / embedded notes - -- Solve coefficients **once** at construction; `Sample` is then pure Horner — deterministic cycles. -- Multi-axis moves: hold one instance per axis; for many via points use `math::CubicSpline` (#61). -- Guard `tf > 0`; the `1/tf^k` terms blow up for a zero-duration segment. -- Quintic gives continuous acceleration (zero jerk endpoints); prefer it when actuators are jerk-sensitive. -- A 0 → 1 instance (e.g. `v0 = vf = 1`, `tf = 1` cubic ⇒ `s = t`) is a valid path-parameter time law - (robotics-toolbox-cpp's `CartesianSlerpInterpolation` uses it that way). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/PolynomialProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class PolynomialProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it from the block above if this is the first - `motion_profiles` item; header-only aggregates, no coverage `.cpp`). -- Coverage: `numerical/motion_profiles/PolynomialProfile.cpp` → `template class PolynomialProfile;` -- Test: `numerical/motion_profiles/test/TestPolynomialProfile.cpp` -- Doc: `doc/motion_profiles/PolynomialProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles PolynomialProfile.cpp)`; - `TestPolynomialProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)` (links `numerical.math`), `MotionProfileTypes.hpp` in - `target_sources`, a `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, - and a `doc/motion_profiles/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/PolynomialProfile/tests.md b/roadmap/motion_profiles/PolynomialProfile/tests.md deleted file mode 100644 index 82180a2c..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/tests.md +++ /dev/null @@ -1,63 +0,0 @@ -# Polynomial Point-to-Point Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPolynomialProfile : public ::testing::Test: - BoundaryConditions restToRest{ .q0 = 0, .qf = 1 } # v0=vf=0 - PolynomialProfile cubic{ restToRest, 2.0f, Degree::Cubic } -# each case below is a TEST_F(TestPolynomialProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -endpoints_match_boundary_positions: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 1 - -endpoint_velocities_are_matched: - Arrange: rest-to-rest cubic - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cubic_midpoint_is_symmetric: - Arrange: q0=0, qf=1, tf=2 rest-to-rest - Assert: Sample(1.0).position ≈ 0.5 (half distance at half time) - -cubic_peak_velocity_matches_formula: - Arrange: rest-to-rest, distance d, duration tf - Assert: max |velocity| ≈ 1.5 * d / tf (at midpoint) - -cubic_jerk_is_constant_third_derivative: - Arrange: q0=0, qf=1, tf=2 rest-to-rest cubic - Assert: Sample(t).jerk ≈ 6·a3 = -1.5 for t ∈ {0, 1, 2} - -quintic_endpoint_accelerations_are_zero: - Arrange: quintic, a0 = af = 0 - Assert: Sample(0).acceleration ≈ 0 and Sample(tf).acceleration ≈ 0 - -nonzero_boundary_velocity_is_honoured: - Arrange: v0 = 0.5, vf = -0.5 - Assert: Sample(0).velocity ≈ 0.5 and Sample(tf).velocity ≈ -0.5 - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) - -reset_recomputes_coefficients: - Arrange: Reset to q0=1, qf=3, tf=1 - Assert: new endpoints reproduced exactly -``` - -## Reference vectors - -- Rest-to-rest cubic, `q0=0, qf=1, tf=2`: `q(t) = 3(t/2)² − 2(t/2)³`; `q(1) = 0.5`, `v(1) = 0.75`. -- Peak velocity of a rest-to-rest cubic `= 1.5·(qf−q0)/tf`. -- Same cubic: `a3 = −2·(qf−q0)/tf³ = −0.25` ⇒ constant jerk `6·a3 = −1.5`. -- `q0=0, qf=1, v0=vf=1, tf=1` cubic ⇒ `a2 = a3 = 0`, i.e. `s(t) = t` (linear time law). - -## Edge cases - -- Very small `tf` ⇒ large coefficients; assert no overflow / `inf`. -- `q0 == qf` with zero boundary velocities ⇒ constant, zero velocity everywhere. -- Cubic vs quintic on the same move ⇒ quintic has smoother (bounded-jerk) acceleration. diff --git a/roadmap/motion_profiles/SCurveProfile/explanation.md b/roadmap/motion_profiles/SCurveProfile/explanation.md deleted file mode 100644 index 2d177ff6..00000000 --- a/roadmap/motion_profiles/SCurveProfile/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Overview - -## What it is -A **seven-segment, jerk-bounded** motion profile (a "double-S" velocity curve). It extends the -trapezoid by ramping acceleration in and out with a bounded rate of change (jerk), so acceleration -is continuous rather than stepping instantaneously. - -## Why it matters (embedded) -Sudden acceleration jumps excite structural resonances, wear gearing, and spill liquids or wobble -payloads. Limiting **jerk** smooths those transitions, cutting vibration and mechanical stress at -the cost of a slightly longer move. It is the profile of choice for high-precision or -vibration-sensitive machines (pick-and-place, CNC, semiconductor handling). - -## How it works (intuition) -The velocity curve is built from seven phases: jerk up, hold acceleration, jerk down (to reach -cruise velocity), cruise, then the mirror-image deceleration triple. Depending on the distance and -limits, some phases shrink to zero — a short move may never reach `aMax` or `vMax`. For rest-to-rest -moves the phase durations follow from a three-step closed form: assume `vMax` is reached; if the move is -too short, drop the cruise and assume only `aMax` is reached; if still too short, neither is reached and -the acceleration ramps are pure triangles. Several axes finish together by stretching the faster ones in -time by a common factor `λ` (velocity `/λ`, acceleration `/λ²`, jerk `/λ³`). - -## Key parameters -- **`vMax`** — cruise-velocity ceiling. -- **`aMax`** — acceleration ceiling (flat top of the acceleration curve). -- **`jMax`** — jerk ceiling; the slope of the acceleration ramps and the vibration knob. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -§3.4 (double-S / jerk-limited profiles; the non-zero boundary-velocity case there needs an iterative -`aMax` reduction and is future work). - -## See also -`TrapezoidalProfile` (jerk-unbounded predecessor), `PolynomialProfile` (fixed-time quintic), -robotics-toolbox-cpp's `TimeOptimalPathParameterization` (limits along a geometric path). diff --git a/roadmap/motion_profiles/SCurveProfile/implementation.md b/roadmap/motion_profiles/SCurveProfile/implementation.md deleted file mode 100644 index a889a2ff..00000000 --- a/roadmap/motion_profiles/SCurveProfile/implementation.md +++ /dev/null @@ -1,122 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Implementation Pseudocode - -> Roadmap ref: #60 (Tier 2) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `numerical/motion_profiles/MotionProfileTypes.hpp` -(canonical definition: PolynomialProfile spec → Data structures); not redefined here. This profile -requires `vMax`, `aMax`, `jMax > 0`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class SCurveProfile: - T q0, direction, jMax - T Tj, Ta, Tv # jerk sub-phase, whole accel phase, cruise - array segT # duration of each of the 7 phases - array segStart # start time of each phase (+ tf) - array segPos, segVel, segAcc # state at each phase boundary - T tf - bool reachesMaxAccel, reachesMaxVel -``` - -## Interface - -```text -SCurveProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path -T Duration() -bool ReachesMaxAccel() / ReachesMaxVel() -``` - -## Algorithm (pseudocode) - -Rest-to-rest closed form (Biagiotti & Melchiorri §3.4, `v0 = v1 = 0`), `h = |qf − q0|`: - -```cpp -# Seven phases: [+j][a=const][-j][v=const][-j][a=const][+j] -function plan(q0, qf, lim): - h = |qf - q0|; direction = sign(qf - q0); jMax = lim.jMax - if h == 0: all durations 0; return - # Step 1 — assume vMax is reached - if lim.vMax * lim.jMax >= lim.aMax^2: # aMax reached on the way to vMax - Tj = lim.aMax / lim.jMax - Ta = Tj + lim.vMax / lim.aMax - else: # triangular accel, peak jMax·Tj < aMax - Tj = sqrt(lim.vMax / lim.jMax) - Ta = 2*Tj - Tv = h / lim.vMax - Ta - if Tv < 0: - # Step 2 — vMax not reached (Tv = 0), assume aMax still reached - Tv = 0 - Tj = lim.aMax / lim.jMax - Δ = lim.aMax^4 / lim.jMax^2 + 4*lim.aMax*h - Ta = (lim.aMax^2 / lim.jMax + sqrt(Δ)) / (2*lim.aMax) - if Ta < 2*Tj: - # Step 3 — neither aMax nor vMax reached - Tj = cbrt(h / (2*lim.jMax)) - Ta = 2*Tj - aLim = jMax*Tj; vLim = aLim*(Ta - Tj) # peak |acc|, peak |vel| - reachesMaxAccel = (Ta > 2*Tj); reachesMaxVel = (Tv > 0) - segT = [Tj, Ta-2*Tj, Tj, Tv, Tj, Ta-2*Tj, Tj] # tf = 2·Ta + Tv - jerkOfPhase = [+jMax, 0, -jMax, 0, -jMax, 0, +jMax] - segPos[0] = segVel[0] = segAcc[0] = 0 - for i in 0..6: # exact constant-jerk integration - τ = segT[i]; j = jerkOfPhase[i] - segPos[i+1] = segPos[i] + segVel[i]*τ + segAcc[i]*τ^2/2 + j*τ^3/6 - segVel[i+1] = segVel[i] + segAcc[i]*τ + j*τ^2/2 - segAcc[i+1] = segAcc[i] + j*τ - segStart[i+1] = segStart[i] + τ - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - i = last phase with segStart[i] ≤ t (i ≤ 6); tau = t - segStart[i] - j = jerkOfPhase[i] - acc = segAcc[i] + j*tau - vel = segVel[i] + segAcc[i]*tau + 0.5*j*tau^2 - s = segPos[i] + segVel[i]*tau + 0.5*segAcc[i]*tau^2 + (1/6)*j*tau^3 - return { q0 + direction*s, direction*vel, direction*acc, direction*j } -``` - -Branch consistency (checked numerically on 20 000 random `h, vMax, aMax, jMax`): every branch ends at `h` -with zero velocity and acceleration, `|v| ≤ vMax`, `|a| ≤ aMax`, `|j| = jMax`; no phase duration is -negative (`Ta − 2Tj = vMax/aMax − aMax/jMax ≥ 0` in step 1a; the step-2 test guards it otherwise). - -## Complexity & memory - -- Planning: `O(1)` — at most three closed-form candidates (one `sqrt`, one `cbrt`), no iteration. -- `Sample`: `O(1)` — locate one of seven phases, evaluate a cubic-in-time (Horner). -- Memory: `O(1)` — four 8-entry tables plus seven durations; stack only, no heap. - -## Numerical / embedded notes - -- Precompute the seven durations and boundary states **once**; each tick is one cubic evaluation. -- Short moves collapse segments to zero length (`Tv = 0`, `Ta = 2·Tj`) — never a negative duration. -- Bounded jerk means acceleration is **continuous** ⇒ far less vibration than a trapezoid. -- Profile is symmetric about its midpoint for rest-to-rest moves (`Td = Ta`). -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1`: velocity, acceleration and jerk become `v/λ, a/λ², j/λ³`, sampled at - `t/λ`. Replanning with limits `(vMax/λ, aMax/λ², jMax/λ³)` gives exactly `Duration() = λ·T_axis` in every - branch (branch tests and the step formulas are invariant under this scaling) — planning stays `O(1)`. -- **Non-rest boundary velocities** (`v0, v1 ≠ 0`, B&M §3.4 general case) need the iterative `aMax` - reduction (`aMax ← γ·aMax`, `0 < γ < 1`) plus a feasibility pre-check; not covered — future work. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/SCurveProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class SCurveProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it if this is the first `motion_profiles` item). -- Coverage: `numerical/motion_profiles/SCurveProfile.cpp` → `template class SCurveProfile;` -- Test: `numerical/motion_profiles/test/TestSCurveProfile.cpp` -- Doc: `doc/motion_profiles/SCurveProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles SCurveProfile.cpp)`; - `TestSCurveProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)`, `MotionProfileTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, and a `doc/motion_profiles/` folder. -- Depends on: #56 (`TrapezoidalProfile`, shares the synchronization scheme). -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/SCurveProfile/tests.md b/roadmap/motion_profiles/SCurveProfile/tests.md deleted file mode 100644 index bd35c638..00000000 --- a/roadmap/motion_profiles/SCurveProfile/tests.md +++ /dev/null @@ -1,87 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSCurveProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f, .jMax = 10.0f } - SCurveProfile profile{ 0.0f, 5.0f, limits } # full 7-segment move (step 1, aMax reached) -# each case below is a TEST_F(TestSCurveProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_at_rest_and_zero_accel: - Assert: Sample(0){vel,acc} ≈ 0 and Sample(tf){vel,acc} ≈ 0 - -velocity_never_exceeds_vmax: - Assert: max |velocity| ≤ vMax (1.0) across the profile - -acceleration_never_exceeds_amax: - Assert: max |acceleration| ≤ aMax (2.0) across the profile - -jerk_never_exceeds_jmax: - Assert: every sampled |jerk| ∈ {0, jMax}, ≤ jMax (10.0) - -acceleration_is_continuous: - Arrange: dense sampling across phase joins - Assert: no acceleration step between adjacent samples (bounded by jMax·dt) - -step1_full_profile_durations: # vMax·jMax ≥ aMax², vMax and aMax reached - Assert: Duration() ≈ 5.7; Sample(0.2).acceleration ≈ 2 (end of jerk phase, Tj = 0.2); - Sample(0.7).velocity ≈ 1 (end of accel phase, Ta = 0.7); - ReachesMaxAccel() and ReachesMaxVel() - -step1_triangular_acceleration_when_jerk_low: # vMax·jMax < aMax² - Arrange: limits { vMax=1, aMax=2, jMax=1 }, q0=0, qf=5 - Assert: Duration() ≈ 7; Sample(1).acceleration ≈ 1 (peak jMax·Tj < aMax); - Sample(3.5).velocity ≈ 1; !ReachesMaxAccel() and ReachesMaxVel() - -step2_short_move_reaches_amax_not_vmax: - Arrange: q0=0, qf=0.3 (fixture limits) - Assert: Duration() ≈ 1.0; Sample(0.5).velocity ≈ 0.6 (vLim, at tf/2); - max |acceleration| ≈ 2; ReachesMaxAccel() and !ReachesMaxVel(); Sample(tf).position ≈ 0.3 - -step3_very_short_move_reaches_neither: - Arrange: q0=0, qf=0.02 (fixture limits) - Assert: Duration() ≈ 0.4; Sample(0.1).acceleration ≈ 1 (aLim = jMax·Tj); - Sample(0.2).velocity ≈ 0.1 (vLim); !ReachesMaxAccel() and !ReachesMaxVel(); - Sample(tf).position ≈ 0.02 - -negative_direction_mirrors_profile: - Arrange: q0=5, qf=0 - Assert: Sample(t) = { 5 − p(t), −v(t), −a(t), −j(t) } of the fixture profile - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -Computed by integrating the 7-segment constant-jerk profile (python3, exact): each ends at `h` with -zero velocity/acceleration and respects `vMax`, `aMax`, `jMax`. - -| Case | Limits `(vMax, aMax, jMax)` | `h` | Branch | `Tj` | `Ta` | `Tv` | `tf` | `aLim` | `vLim` | -|------------|-----------------------------|------|---------------|------|------|------|------|--------|--------| -| full | (1, 2, 10) | 5 | 1 (`vj ≥ a²`) | 0.2 | 0.7 | 4.3 | 5.7 | 2 | 1 | -| low jerk | (1, 2, 1) | 5 | 1 (`vj < a²`) | 1.0 | 2.0 | 3.0 | 7.0 | 1 | 1 | -| short | (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | 2 | 0.6 | -| very short | (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | 1 | 0.1 | - -- Step 2 for `h = 0.3`: `Δ = 2⁴/10² + 4·2·0.3 = 2.56`, `Ta = (0.4 + 1.6)/4 = 0.5 ≥ 2·Tj = 0.4`. -- Step 3 for `h = 0.02`: `Tj = (0.02/20)^{1/3} = 0.1`. -- Branch thresholds for `(1, 2, 10)`: step 1 iff `h ≥ vMax·Ta = 0.7`; step 2 iff `h ≥ 2·aMax³/jMax² = 0.16`. -- Synchronization: replanning with `(vMax/λ, aMax/λ², jMax/λ³)` scales `tf` by exactly `λ` in every branch - (e.g. full case, `λ = 2` ⇒ `11.4`). - -## Edge cases - -- `jMax → ∞` ⇒ jerk phases vanish, profile degenerates to a trapezoid (cross-check limit). -- `h` exactly at a branch threshold (0.7 or 0.16 above) ⇒ neighbouring branches give the same `tf`. -- `d == 0` ⇒ zero-length profile; `Sample(0)` is the start, fully at rest. diff --git a/roadmap/motion_profiles/TrapezoidalProfile/explanation.md b/roadmap/motion_profiles/TrapezoidalProfile/explanation.md deleted file mode 100644 index 69106280..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Overview - -## What it is -A **linear-segment-with-parabolic-blends** (LSPB) motion profile: accelerate at a constant rate, -cruise at a constant velocity, then decelerate at a constant rate. The velocity-vs-time graph is a -trapezoid (or a triangle for short moves that never reach cruise speed). - -## Why it matters (embedded) -It is the standard "get there as fast as the actuator allows" profile. Instead of fixing the -duration, you fix the **velocity and acceleration limits** and the profile computes the minimum -time that respects them — exactly what a motion controller needs to drive real hardware without -saturating drives. - -## How it works (intuition) -Time splits into three phases with a simple symmetry: the accel and decel ramps are mirror images. -If the move is long enough, velocity saturates at `vMax` and coasts; if it is short, the profile -peaks below `vMax` and immediately decelerates — a triangle. A single distance test -(`d ≥ vMax²/aMax`) decides which case applies. - -## Key parameters -- **`vMax`** — cruise velocity ceiling; caps the flat top of the trapezoid. -- **`aMax`** — ramp acceleration; sets the blend duration `vMax/aMax`. -- **Distance `qf − q0`** — decides trapezoid vs triangle and the total time. -- **Duration `T` (fixed-time variant)** — for synchronizing several axes: keep `aMax`, solve for the - lower cruise speed that finishes exactly at `T` (feasible iff `aMax·T² ≥ 4·distance`). - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (trapezoidal / LSPB profiles). - -## See also -`PolynomialProfile` (fixed-time, smooth), `SCurveProfile` (jerk-limited upgrade), -`SaturationRateLimiter` (`controllers`, online slew limiting). diff --git a/roadmap/motion_profiles/TrapezoidalProfile/implementation.md b/roadmap/motion_profiles/TrapezoidalProfile/implementation.md deleted file mode 100644 index 3d9a677c..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/implementation.md +++ /dev/null @@ -1,108 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Implementation Pseudocode - -> Roadmap ref: #56 (Tier 1) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `numerical/motion_profiles/MotionProfileTypes.hpp` -(canonical definition: PolynomialProfile spec → Data structures); not redefined here. This profile -reads `vMax`, `aMax` and ignores `jMax`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class TrapezoidalProfile: - T q0, direction, distance # start, sign(qf - q0), |qf - q0| - T vPeak, aMax # cruise velocity actually reached, ramp acceleration - T tAccel, tCruise, tf # phase boundaries (blend, flat, total) -``` - -## Interface - -```cpp -TrapezoidalProfile(T q0, T qf, MotionLimits limits) # minimum-time plan -static std::optional PlanWithDuration(T q0, T qf, T aMax, T duration) - # fixed-time plan (synchronization) -TrajectoryState Sample(T t) # hot path; jerk = 0 -T Duration() -bool IsTriangular() # true when no cruise phase exists -``` - -## Algorithm (pseudocode) - -```cpp -function plan(q0, qf, limits): - distance = |qf - q0|; direction = sign(qf - q0); aMax = limits.aMax - dBlend = limits.vMax^2 / limits.aMax # distance used by accel + decel - if distance >= dBlend: # trapezoid: cruise phase exists - vPeak = limits.vMax - tAccel = vPeak / aMax - tCruise = (distance - dBlend) / vPeak - else: # triangle: peak below vMax - vPeak = sqrt(distance * aMax) - tAccel = vPeak / aMax - tCruise = 0 - tf = 2*tAccel + tCruise - -function PlanWithDuration(q0, qf, aMax, duration): - # d = v·(duration - v/aMax) ⇒ v² - aMax·duration·v + aMax·d = 0, take the smaller root - d = |qf - q0| - disc = aMax^2 * duration^2 - 4*aMax*d - if aMax <= 0 or duration <= 0 or disc < 0: # feasible iff aMax·duration² ≥ 4d - return nullopt - vPeak = 2*aMax*d / (aMax*duration + sqrt(disc)) # = (aMax·duration − sqrt(disc))/2, cancellation-free - tAccel = vPeak / aMax - tCruise = duration - 2*tAccel - tf = duration - return profile{ q0, sign(qf - q0), d, vPeak, aMax, tAccel, tCruise, tf } - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - if t < tAccel: # parabolic ramp-up - acc = aMax; vel = aMax*t; s = 0.5*aMax*t^2 - else if t < tAccel + tCruise: # linear cruise - acc = 0; vel = vPeak; s = vPeak*(t - 0.5*tAccel) - else: # parabolic ramp-down - td = tf - t - acc = -aMax; vel = aMax*td; s = distance - 0.5*aMax*td^2 - return { q0 + direction*s, direction*vel, direction*acc, 0 } -``` - -## Complexity & memory - -- Planning (both variants): `O(1)` — one `sqrt` and a branch. -- `Sample`: `O(1)` — one phase branch, a couple of multiply-adds. -- Memory: `O(1)` — eight scalars; entirely stack-resident, no heap. - -## Numerical / embedded notes - -- Precompute the three phase boundaries **once**; `Sample` then reduces to one branch per tick. -- Velocity is continuous but acceleration is **discontinuous** at blend joins (bounded jerk spikes) — - upgrade to `SCurveProfile` when that excites structural modes. -- Guard degenerate inputs: `distance == 0` ⇒ zero-length profile; `aMax <= 0` or `vMax <= 0` ⇒ reject. -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1` (`T_sync` = largest `Duration()`): its velocity, acceleration and - jerk become `v/λ, a/λ², j/λ³` and it is sampled at `t/λ`. Replanning that axis with limits - `(vMax/λ, aMax/λ²)` yields exactly `Duration() = λ·T_axis` (same branch, same shape). - `PlanWithDuration(q0, qf, aMax, T_sync)` is the alternative that keeps `aMax` and only lowers the cruise - speed; for `T_sync ≥` the minimum-time duration its `vPeak ≤ vMax` (the root decreases with duration). -- `PlanWithDuration` uses the conjugate form of the smaller root — the textbook - `(aMax·T − sqrt(disc))/2` cancels catastrophically in `float` when `4·aMax·d ≪ aMax²·T²`. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/TrapezoidalProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TrapezoidalProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it if this is the first `motion_profiles` item). -- Coverage: `numerical/motion_profiles/TrapezoidalProfile.cpp` → `template class TrapezoidalProfile;` -- Test: `numerical/motion_profiles/test/TestTrapezoidalProfile.cpp` -- Doc: `doc/motion_profiles/TrapezoidalProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles TrapezoidalProfile.cpp)`; - `TestTrapezoidalProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)`, `MotionProfileTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, and a `doc/motion_profiles/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/TrapezoidalProfile/tests.md b/roadmap/motion_profiles/TrapezoidalProfile/tests.md deleted file mode 100644 index 60c59bd1..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/tests.md +++ /dev/null @@ -1,71 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestTrapezoidalProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f } - TrapezoidalProfile profile{ 0.0f, 5.0f, limits } # long move ⇒ trapezoid -# each case below is a TEST_F(TestTrapezoidalProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_are_at_rest: - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cruise_velocity_is_clamped_to_vmax: - Arrange: long move - Assert: max velocity over the profile ≈ vMax (1.0), never exceeds it - -acceleration_is_within_limit: - Assert: |acceleration| ≤ aMax at all sampled instants - -short_move_is_triangular: - Arrange: q0=0, qf=0.1 (d < vMax²/aMax) - Assert: IsTriangular() == true and peak velocity < vMax - -phase_durations_match_trapezoid_formula: - Arrange: d=5, vMax=1, aMax=2 - Assert: tAccel ≈ 0.5, tCruise ≈ 4.5, Duration ≈ 5.5 - -negative_direction_move: - Arrange: q0=2, qf=0 - Assert: velocity ≤ 0 throughout, endpoints (2 → 0) reached - -plan_with_duration_lowers_cruise_speed: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=5) - Assert: has_value; Duration() ≈ 5; Sample(0.5).velocity ≈ 0.5 (ramp at aMax); - Sample(2.5).velocity ≈ 1 (cruise vPeak = 1 < aMax·T/2); Sample(5).position ≈ 4 - -plan_with_duration_rejects_infeasible_time: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=3) # aMax·T² = 9 < 4d = 16 - Assert: == nullopt - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -- `d=5, vMax=1, aMax=2`: `tAccel = vMax/aMax = 0.5`, `tCruise = (d − vMax²/aMax)/vMax = 4.5`, - `tf = 5.5`. -- Triangular threshold distance `= vMax²/aMax = 0.5`. -- `PlanWithDuration`, `d=4, aMax=1, T=5`: `v = (aMax·T − sqrt(aMax²T² − 4·aMax·d))/2 = (5 − 3)/2 = 1`, - `tAccel = 1`, `tCruise = 3`. `d=5, aMax=2, T=6`: `v = 0.900980`, `tAccel = 0.450490`, - `tCruise = 5.099020`. Feasible iff `aMax·T² ≥ 4d` (equality ⇒ triangle, `v = aMax·T/2`). -- Synchronization: replanning with `(vMax/λ, aMax/λ²)` scales `Duration()` by exactly `λ` - (e.g. fixture, `λ = 2` ⇒ `11.0`). - -## Edge cases - -- `d` exactly equal to `vMax²/aMax` ⇒ zero cruise time (trapezoid degenerates to triangle). -- `d == 0` ⇒ `tf == 0`, `Sample(0)` is the start at rest. -- `aMax` very large ⇒ ramps vanish, profile approaches a pure velocity step. -- `PlanWithDuration` with `d == 0` ⇒ `vPeak = 0`, holds `q0` at rest for the whole duration.