gabrielfrasantos · Oct 23, 2026 · Oct 23, 2026 · Oct 23, 2026 · Oct 23, 2026 · Oct 23, 2026
diff --git 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
@@ -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
@@ -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.
 

diff --git a/ROADMAP.md b/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.