feat: add angle utilities, tridiagonal solver, friction compensation, symplectic integrators and motion profiles (#355)

* feat(math): add angle utilities (wrap, shortest difference, unwrap)

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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

* refactor(filters): use math::WrapToPi in ComplementaryFilter

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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

* 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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

* feat(controllers): add friction compensation feedforward

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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

* feat(solvers): add symplectic integrators for second-order systems

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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

* feat(motion_profiles): add polynomial, trapezoidal and S-curve profiles (#356)

* 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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5

---------

Co-authored-by: Claude <noreply@anthropic.com>

---------

Co-authored-by: Claude <noreply@anthropic.com>

Author: gabrielfrasantos

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
 

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",

README.md

@@ -14,20 +14,21 @@ 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 |
-| [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                             |
-| [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 |
-| [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                                                                                                                                                                                                                                                               |
+| [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                                                                                                                                                                                                                                                  |
+| [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.
 

ROADMAP.md

@@ -26,14 +26,7 @@ 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`           | ★☆☆☆☆      |
-| 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`                  | ★★☆☆☆      |
 | 62 | Damped pseudo-inverse + null-space projector          | `solvers`               | ★★☆☆☆      |
 | 66 | SE(2) transform, twists, exp/log, pose error          | `math`                  | ★★☆☆☆      |
@@ -58,60 +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 ★☆☆☆☆
-
-### 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`.
-- **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).
-
-### 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
-  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).
-
-### 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
@@ -126,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.