diff --git a/ROADMAP.md b/ROADMAP.md index 9d25061e..ce589957 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -24,14 +24,162 @@ Difficulty legend: ## Master list (by priority) -| # | Component | Target module | Difficulty | -|----|------------------------------------------------------|---------------------------|------------| +| # | 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` | ★★☆☆☆ | +| 63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | ★★★☆☆ | +| 64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | ★★★☆☆ | +| 65 | Dense convex QP (Goldfarb–Idnani dual active set) | `solvers` | ★★★★☆ | + +Items 53–65 are the generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as +NT53–NT65); each also serves non-robotics consumers (motor control, navigation, calibration). All are +*(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family 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 +- **What:** Rigid transform (rotation + translation) with closed-form inverse, adjoint, twist/wrench + transforms, exponential/logarithm (SO(3) and SE(3)) and a log-map pose error. 6-vectors are ordered + linear part first: twists `(v; ω)`, wrenches `(f; n)`. +- **Embedded value:** The algebra every pose-level consumer shares — robot kinematics, Jacobians and + task-space control in robotics-toolbox-cpp, drone attitude/position control, visual odometry. One + type and one ordering convention prevent silent block-order mismatches between modules. +- **Algorithm / paper:** K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; R. Murray, Z. Li, + S. Sastry (1994), Ch. 2; J. Solà et al., "A micro Lie theory for state estimation in robotics" (2018). +- **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. +- **Embedded value:** Smooth interpolation of waypoint lists and calibration tables in linear time on fixed + storage. +- **Algorithm / paper:** C. de Boor, *A Practical Guide to Splines* (2001), Ch. IV. +- **Reuses:** Item 59. +- **Spec:** [roadmap/math/CubicSpline](roadmap/math/CubicSpline/implementation.md). + +### 62. Damped pseudo-inverse and null-space projector +- **What:** `A⁺_λ·b` and the exact projector `I − A⁺A` for any `Rows × Cols` matrix from one SVD. +- **Embedded value:** Singularity-robust inversion for redundant robots, over-actuated control allocation + and regularized calibration; the projector exposes spare actuation to a secondary objective. +- **Algorithm / paper:** C. Wampler, *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*, + 7(12), 1977. +- **Reuses:** `SingularValueDecomposition` (tall and wide). +- **Spec:** [roadmap/solvers/DampedPseudoInverse](roadmap/solvers/DampedPseudoInverse/implementation.md). + +--- + +## Tier 3 — Moderate ★★★☆☆ + +### 63. Incremental QR least squares +- **What:** Streaming Givens-QR accumulation of an unbounded number of rows into a `P×P` factor, with a + rank-revealing SVD solve (minimum-norm solution, identifiable directions, residual RMS). +- **Embedded value:** Parameter identification from long logs in fixed memory without squaring the + condition number. +- **Algorithm / paper:** G. Golub, C. Van Loan, §5.1.8 and §5.5; Å. Björck (1996), Ch. 2. +- **Reuses:** `GivensRotation`, `SingularValueDecomposition`. +- **Spec:** [roadmap/estimators/offline/IncrementalQrLeastSquares](roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md). + +### 64. Levenberg–Marquardt +- **What:** Damped Gauss–Newton for small nonlinear least squares with a problem-supplied retraction + (vectors, bounds, rotations) and a pluggable damping policy. +- **Embedded value:** One bounded, heap-free solver for IK, parallel-robot forward kinematics, calibration + and scan matching. +- **Algorithm / paper:** K. Levenberg (1944); D. Marquardt (1963); J. Nocedal, S. Wright (2006), Ch. 10. +- **Reuses:** `LuDecomposition`. +- **Spec:** [roadmap/optimization/LevenbergMarquardt](roadmap/optimization/LevenbergMarquardt/implementation.md). + +--- + +## Tier 4 — Advanced ★★★★☆ + +### 65. Dense convex QP +- **What:** Goldfarb–Idnani dual active-set solver for strictly convex QPs with equality and inequality + constraints; finite, bounded iterations. +- **Embedded value:** Exact constrained allocation (cable tensions, thrusters), task-priority control and + constrained MPC without heuristics. +- **Algorithm / paper:** D. Goldfarb, A. Idnani, *Mathematical Programming*, 27(1), 1983; M. J. D. Powell + (1985). +- **Reuses:** `CholeskyDecomposition`, `TriangularSolve`, `GivensRotation`. +- **Spec:** [roadmap/solvers/ActiveSetQp](roadmap/solvers/ActiveSetQp/implementation.md). + +--- + ## Evaluation & metrics primitives Reusable quantities that quantify *how well* an algorithm behaves — the properties the per-family diff --git a/doc/solvers/SingularValueDecomposition.md b/doc/solvers/SingularValueDecomposition.md index aeb20c47..023ac601 100644 --- a/doc/solvers/SingularValueDecomposition.md +++ b/doc/solvers/SingularValueDecomposition.md @@ -2,9 +2,9 @@ ## Overview & Motivation -The singular value decomposition (SVD) factors any $m \times n$ matrix $A$ ($m \geq n$) into +The singular value decomposition (SVD) factors any $m \times n$ matrix $A$ into two orthonormal rotation factors and a non-negative diagonal, revealing the fundamental geometric -action of $A$ as a stretch along $n$ orthogonal directions. It is the most numerically stable +action of $A$ as a stretch along $k = \min(m, n)$ orthogonal directions. It is the most numerically stable matrix factorization available and exposes rank, condition number, and the pseudo-inverse directly from its output. In embedded systems, SVD powers robust least-squares solvers for sensor calibration, redundant-actuator control allocation, and low-rank model reduction — all applications where the @@ -14,16 +14,23 @@ matrix $A$ may be ill-conditioned or rank-deficient and normal-equation approach ### Factorization -For $A \in \mathbb{R}^{m \times n}$ with $m \geq n$, the thin SVD is +For $A \in \mathbb{R}^{m \times n}$ and $k = \min(m, n)$, the thin SVD is $$A = U \Sigma V^\top,$$ -where $U \in \mathbb{R}^{m \times n}$ has orthonormal columns ($U^\top U = I_n$), -$\Sigma = \mathrm{diag}(\sigma_1, \ldots, \sigma_n)$ with $\sigma_1 \geq \cdots \geq \sigma_n \geq 0$, -and $V \in \mathbb{R}^{n \times n}$ is orthogonal ($V^\top V = V V^\top = I_n$). +where $U \in \mathbb{R}^{m \times k}$ and $V \in \mathbb{R}^{n \times k}$ have orthonormal columns +($U^\top U = V^\top V = I_k$) and $\Sigma = \mathrm{diag}(\sigma_1, \ldots, \sigma_k)$ with +$\sigma_1 \geq \cdots \geq \sigma_k \geq 0$. For a tall or square matrix ($m \geq n$) $V$ is square +and orthogonal; for a wide matrix ($m < n$) $U$ is. The scalars $\sigma_i$ are the **singular values** of $A$; the columns of $U$ and $V$ are the left and right **singular vectors**. +### Wide Matrices + +The Golub–Kahan bidiagonalization below runs on a tall matrix. A wide $A$ is handled through its +transpose: factor $A^\top = U' \Sigma V'^\top$ (tall), then $A = V' \Sigma U'^\top$, so $U = V'$ and +$V = U'$. The singular values are shared, and no extra storage is needed beyond the tall factors. + ### Connection to the Symmetric Eigenproblem The singular values satisfy $\sigma_i^2 = \lambda_i(A^\top A)$, where $\lambda_i$ denotes the @@ -43,7 +50,7 @@ or numerical noise. ### Rank and Condition Number -$$\mathrm{rank}(A) = \#\{i : \sigma_i > \tau\}, \quad \kappa(A) = \sigma_1 / \sigma_n.$$ +$$\mathrm{rank}(A) = \#\{i : \sigma_i > \tau\}, \quad \kappa(A) = \sigma_1 / \sigma_k.$$ The condition number $\kappa$ measures sensitivity: a system with $\kappa \gg 1$ amplifies input perturbations by a factor of $\kappa$ in the least-squares solution. @@ -52,10 +59,12 @@ input perturbations by a factor of $\kappa$ in the least-squares solution. | Phase | Time | Space | Notes | |-------------------|-------------------------------------------------|----------|----------------------------------------------| -| Bidiagonalization | $O(mn^2 - n^3/3)$ | $O(mn)$ | Two-sided Householder reflectors | -| QR sweeps | $O(n^2)$ per iteration, $O(n)$ total iterations | $O(n^2)$ | Golub-Reinsch implicit-shift; converges fast | -| Pseudo-inverse | $O(n^2 m)$ | $O(nm)$ | Matrix triple product $V \Sigma^+ U^\top$ | -| Total | $O(mn^2 + n^3)$ | $O(mn)$ | Dominated by bidiagonalization | +| Bidiagonalization | $O(Mk^2 - k^3/3)$ | $O(mn)$ | Two-sided Householder reflectors | +| QR sweeps | $O(k^2)$ per iteration, $O(k)$ total iterations | $O(k^2)$ | Golub-Reinsch implicit-shift; converges fast | +| Pseudo-inverse | $O(k\,mn)$ | $O(nm)$ | Matrix triple product $V \Sigma^+ U^\top$ | +| Total | $O(Mk^2 + k^3)$ | $O(mn)$ | Dominated by bidiagonalization | + +Here $M = \max(m, n)$ and $k = \min(m, n)$: a wide matrix costs the same as its transpose. All storage is stack-allocated; no heap is used. The bidiagonal form is stored implicitly in the working copy of $A$ alongside the Householder reflector scalars. @@ -86,13 +95,14 @@ negated. After descending sort: $\sigma = (3, 2, 1)$. diagonal entries into the sign of the corresponding $U$ column so that all $\sigma_i \geq 0$. - **Repeated singular values:** the corresponding singular-vector subspaces are correct but the individual vectors are not unique; tests should check subspace properties, not individual vectors. -- **Condition number with zero $\sigma_{\min}$:** a rank-deficient matrix has $\sigma_n = 0$; +- **Condition number with zero $\sigma_{\min}$:** a rank-deficient matrix has $\sigma_k = 0$; `ConditionNumber()` returns zero as a sentinel in this case rather than dividing by zero. ## Variants & Generalizations -- **Full SVD:** $U \in \mathbb{R}^{m \times m}$ (all $m$ left singular vectors); the extra - $m - n$ columns span the left null space of $A$. The thin SVD suffices for least squares. +- **Full SVD:** $U \in \mathbb{R}^{m \times m}$ and $V \in \mathbb{R}^{n \times n}$; the extra columns + span the left and right null spaces of $A$. The thin SVD suffices for least squares; a null-space + projector $I - V V^\top$ needs only the thin $V$. - **Truncated SVD:** retain only the $k$ largest singular values for low-rank approximation $A \approx U_k \Sigma_k V_k^\top$; the approximation error in Frobenius norm equals $\sqrt{\sigma_{k+1}^2 + \cdots + \sigma_n^2}$ (Eckart-Young theorem). @@ -105,7 +115,8 @@ negated. After descending sort: $\sigma = (3, 2, 1)$. ## Applications - **Least-squares regression:** $x = A^+ b$ minimises $\|Ax - b\|_2$ and is unique among - minimum-norm solutions when $A$ is rank-deficient. + minimum-norm solutions when $A$ is rank-deficient. For a wide, full-row-rank $A$ (an + underdetermined system) it is the minimum-norm exact solution $A^\top (A A^\top)^{-1} b$. - **Control allocation:** maps actuator space to output space; the pseudo-inverse distributes effort across redundant actuators while respecting the null space. - **System identification:** SVD of a Hankel matrix (Ho-Kalman) recovers system order (rank) diff --git a/numerical/solvers/SingularValueDecomposition.cpp b/numerical/solvers/SingularValueDecomposition.cpp index 6ef40396..98a36d57 100644 --- a/numerical/solvers/SingularValueDecomposition.cpp +++ b/numerical/solvers/SingularValueDecomposition.cpp @@ -4,4 +4,5 @@ namespace solvers { template class SingularValueDecomposition; template class SingularValueDecomposition; + template class SingularValueDecomposition; } diff --git a/numerical/solvers/SingularValueDecomposition.hpp b/numerical/solvers/SingularValueDecomposition.hpp index dd31a8aa..58be826c 100644 --- a/numerical/solvers/SingularValueDecomposition.hpp +++ b/numerical/solvers/SingularValueDecomposition.hpp @@ -18,29 +18,34 @@ namespace solvers class SingularValueDecomposition { static_assert(std::is_floating_point_v, "SingularValueDecomposition supports floating-point types only"); - static_assert(Rows >= Cols, "SingularValueDecomposition requires Rows >= Cols"); + + static constexpr bool wide = Rows < Cols; + static constexpr std::size_t TallRows = wide ? Cols : Rows; + static constexpr std::size_t TallCols = wide ? Rows : Cols; public: + static constexpr std::size_t SingularValueCount = TallCols; + SingularValueDecomposition() = default; OPTIMIZE_FOR_SPEED bool Decompose(const math::Matrix& a); - const math::Vector& SingularValues() const; + const math::Vector& SingularValues() const; math::Matrix PseudoInverse(T tol) const; std::size_t Rank(T tol) const; T ConditionNumber() const; math::Vector SolveLeastSquares(const math::Vector& b) const; - const math::Matrix& U() const; - const math::Matrix& V() const; + const math::Matrix& U() const; + const math::Matrix& V() const; private: - using LeftVectors = std::array, Cols>; - using RightVectors = std::array, Cols>; - using Betas = std::array; + using LeftVectors = std::array, TallCols>; + using RightVectors = std::array, TallCols>; + using Betas = std::array; - void Bidiagonalize(math::Matrix& bidiag, + void Bidiagonalize(math::Matrix& bidiag, LeftVectors& leftVecs, Betas& leftBeta, RightVectors& rightVecs, Betas& rightBeta) const; - void ExtractBidiagonal(const math::Matrix& bidiag); + void ExtractBidiagonal(const math::Matrix& bidiag); void AccumulateU(const LeftVectors& leftVecs, const Betas& leftBeta); void AccumulateV(const RightVectors& rightVecs, const Betas& rightBeta); bool DiagonalizeGolubKahan(); @@ -55,24 +60,24 @@ namespace solvers void SortDescending(); void SwapColumns(std::size_t i, std::size_t j); - math::Matrix uMat{}; - math::Vector sigma{}; - math::Vector superdiag{}; - math::Matrix vMat{}; + math::Matrix uMat{}; + math::Vector sigma{}; + math::Vector superdiag{}; + math::Matrix vMat{}; bool decomposed{ false }; }; template void SingularValueDecomposition::Bidiagonalize( - math::Matrix& bidiag, + math::Matrix& bidiag, LeftVectors& leftVecs, Betas& leftBeta, RightVectors& rightVecs, Betas& rightBeta) const { - math::Vector lvec{}; - math::Vector rvec{}; + math::Vector lvec{}; + math::Vector rvec{}; - for (std::size_t k = 0; k < Cols; ++k) + for (std::size_t k = 0; k < TallCols; ++k) { - for (std::size_t i = 0; i < Rows; ++i) + for (std::size_t i = 0; i < TallRows; ++i) lvec.at(i, 0) = (i >= k) ? bidiag.at(i, k) : T{}; T lbeta{}; @@ -83,9 +88,9 @@ namespace solvers if (lbeta != T{}) math::ApplyReflectorLeft(bidiag, lvec, lbeta, k, k); - if (k + 1 < Cols) + if (k + 1 < TallCols) { - for (std::size_t j = 0; j < Cols; ++j) + for (std::size_t j = 0; j < TallCols; ++j) rvec.at(j, 0) = (j >= k + 1) ? bidiag.at(k, j) : T{}; T rbeta{}; @@ -105,12 +110,12 @@ namespace solvers template void SingularValueDecomposition::ExtractBidiagonal( - const math::Matrix& bidiag) + const math::Matrix& bidiag) { - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) { sigma.at(i, 0) = bidiag.at(i, i); - superdiag.at(i, 0) = (i + 1 < Cols) ? bidiag.at(i, i + 1) : T{}; + superdiag.at(i, 0) = (i + 1 < TallCols) ? bidiag.at(i, i + 1) : T{}; } } @@ -118,11 +123,11 @@ namespace solvers void SingularValueDecomposition::AccumulateU( const LeftVectors& leftVecs, const Betas& leftBeta) { - for (std::size_t i = 0; i < Rows; ++i) - for (std::size_t j = 0; j < Cols; ++j) + for (std::size_t i = 0; i < TallRows; ++i) + for (std::size_t j = 0; j < TallCols; ++j) uMat.at(i, j) = (i == j) ? T{ 1 } : T{}; - for (std::size_t k = Cols; k > 0; --k) + for (std::size_t k = TallCols; k > 0; --k) { std::size_t col = k - 1; T lbeta = leftBeta[col]; @@ -135,11 +140,11 @@ namespace solvers void SingularValueDecomposition::AccumulateV( const RightVectors& rightVecs, const Betas& rightBeta) { - for (std::size_t i = 0; i < Cols; ++i) - for (std::size_t j = 0; j < Cols; ++j) + for (std::size_t i = 0; i < TallCols; ++i) + for (std::size_t j = 0; j < TallCols; ++j) vMat.at(i, j) = (i == j) ? T{ 1 } : T{}; - for (std::size_t row = 0; row + 1 < Cols; ++row) + for (std::size_t row = 0; row + 1 < TallCols; ++row) { T rbeta = rightBeta[row]; if (rbeta != T{}) @@ -151,7 +156,11 @@ namespace solvers OPTIMIZE_FOR_SPEED bool SingularValueDecomposition::Decompose( const math::Matrix& a) { - math::Matrix bidiag = a; + math::Matrix bidiag{}; + if constexpr (wide) + bidiag = a.Transpose(); + else + bidiag = a; LeftVectors leftVecs{}; RightVectors rightVecs{}; @@ -167,10 +176,10 @@ namespace solvers if (!DiagonalizeGolubKahan()) { - uMat = math::Matrix{}; - sigma = math::Vector{}; - superdiag = math::Vector{}; - vMat = math::Matrix{}; + uMat = math::Matrix{}; + sigma = math::Vector{}; + superdiag = math::Vector{}; + vMat = math::Matrix{}; return false; } @@ -184,14 +193,14 @@ namespace solvers template bool SingularValueDecomposition::NextBlock(std::size_t& p, std::size_t& q) { - for (std::size_t i = 0; i < Cols - 1; ++i) + for (std::size_t i = 0; i < TallCols - 1; ++i) { T thresh = T{ 1e-6f } * (math::Abs(sigma.at(i, 0)) + math::Abs(sigma.at(i + 1, 0))); if (math::Abs(superdiag.at(i, 0)) <= thresh) superdiag.at(i, 0) = T{}; } - std::size_t right = Cols - 1; + std::size_t right = TallCols - 1; while (right > 0) { T thresh = T{ 1e-6f } * (math::Abs(sigma.at(right - 1, 0)) + math::Abs(sigma.at(right, 0))); @@ -223,7 +232,7 @@ namespace solvers { T scale{}; - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) { scale = std::max(scale, math::Abs(sigma.at(i, 0))); scale = std::max(scale, math::Abs(superdiag.at(i, 0))); @@ -245,7 +254,7 @@ namespace solvers sigma.at(j, 0) = g.c * sigma.at(j, 0) + g.s * extra; - for (std::size_t i = 0; i < Rows; ++i) + for (std::size_t i = 0; i < TallRows; ++i) math::ApplyGivens(g, uMat.at(i, j), uMat.at(i, k)); if (j > q) @@ -270,7 +279,7 @@ namespace solvers sigma.at(j, 0) = g.c * sigma.at(j, 0) + g.s * extra; - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) math::ApplyGivens(g, vMat.at(i, j), vMat.at(i, q + 1)); if (j == p) @@ -290,7 +299,7 @@ namespace solvers if (scale <= T{}) return false; - const T threshold = scale * std::numeric_limits::epsilon() * static_cast(Cols); + const T threshold = scale * std::numeric_limits::epsilon() * static_cast(TallCols); for (std::size_t k = p; k <= q; ++k) if (math::Abs(sigma.at(k, 0)) <= threshold) @@ -311,7 +320,7 @@ namespace solvers template OPTIMIZE_FOR_SPEED bool SingularValueDecomposition::DiagonalizeGolubKahan() { - constexpr std::size_t maxIter = 30 * Cols * Cols; + constexpr std::size_t maxIter = 30 * TallCols * TallCols; std::size_t p{}; std::size_t q{}; @@ -356,7 +365,7 @@ namespace solvers { math::GivensRotation gv = math::ComputeGivens(f, g); - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) math::ApplyGivens(gv, vMat.at(i, k), vMat.at(i, k + 1)); T dk = sigma.at(k, 0); @@ -373,7 +382,7 @@ namespace solvers math::GivensRotation gu = math::ComputeGivens(f, g); - for (std::size_t i = 0; i < Rows; ++i) + for (std::size_t i = 0; i < TallRows; ++i) math::ApplyGivens(gu, uMat.at(i, k), uMat.at(i, k + 1)); sigma.at(k, 0) = math::Sqrt(f * f + g * g); @@ -381,7 +390,7 @@ namespace solvers f = gu.c * ek + gu.s * dkp1; dkp1 = gu.c * dkp1 - gu.s * ek; - if (k + 1 < Cols - 1) + if (k + 1 < TallCols - 1) { T ekp1 = superdiag.at(k + 1, 0); g = gu.s * ekp1; @@ -397,12 +406,12 @@ namespace solvers template void SingularValueDecomposition::MakeSingularValuesPositive() { - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) { if (sigma.at(i, 0) < T{}) { sigma.at(i, 0) = -sigma.at(i, 0); - for (std::size_t k = 0; k < Rows; ++k) + for (std::size_t k = 0; k < TallRows; ++k) uMat.at(k, i) = -uMat.at(k, i); } } @@ -415,13 +424,13 @@ namespace solvers sigma.at(i, 0) = sigma.at(j, 0); sigma.at(j, 0) = tmp; - for (std::size_t k = 0; k < Rows; ++k) + for (std::size_t k = 0; k < TallRows; ++k) { T tu = uMat.at(k, i); uMat.at(k, i) = uMat.at(k, j); uMat.at(k, j) = tu; } - for (std::size_t k = 0; k < Cols; ++k) + for (std::size_t k = 0; k < TallCols; ++k) { T tv = vMat.at(k, i); vMat.at(k, i) = vMat.at(k, j); @@ -432,10 +441,10 @@ namespace solvers template void SingularValueDecomposition::SortDescending() { - for (std::size_t i = 0; i + 1 < Cols; ++i) + for (std::size_t i = 0; i + 1 < TallCols; ++i) { std::size_t maxIdx = i; - for (std::size_t j = i + 1; j < Cols; ++j) + for (std::size_t j = i + 1; j < TallCols; ++j) if (sigma.at(j, 0) > sigma.at(maxIdx, 0)) maxIdx = j; @@ -445,35 +454,41 @@ namespace solvers } template - const math::Vector& SingularValueDecomposition::SingularValues() const + auto SingularValueDecomposition::SingularValues() const -> const math::Vector& { really_assert(decomposed); return sigma; } template - const math::Matrix& SingularValueDecomposition::U() const + auto SingularValueDecomposition::U() const -> const math::Matrix& { really_assert(decomposed); - return uMat; + if constexpr (wide) + return vMat; + else + return uMat; } template - const math::Matrix& SingularValueDecomposition::V() const + auto SingularValueDecomposition::V() const -> const math::Matrix& { really_assert(decomposed); - return vMat; + if constexpr (wide) + return uMat; + else + return vMat; } template math::Matrix SingularValueDecomposition::PseudoInverse(T tol) const { really_assert(decomposed); - math::Matrix sigmaInv{}; - for (std::size_t i = 0; i < Cols; ++i) + math::Matrix sigmaInv{}; + for (std::size_t i = 0; i < SingularValueCount; ++i) sigmaInv.at(i, i) = (sigma.at(i, 0) > tol) ? T{ 1 } / sigma.at(i, 0) : T{}; - return vMat * sigmaInv * uMat.Transpose(); + return V() * sigmaInv * U().Transpose(); } template @@ -481,7 +496,7 @@ namespace solvers { really_assert(decomposed); std::size_t r{ 0 }; - for (std::size_t i = 0; i < Cols; ++i) + for (std::size_t i = 0; i < TallCols; ++i) if (sigma.at(i, 0) > tol) ++r; return r; @@ -491,7 +506,7 @@ namespace solvers T SingularValueDecomposition::ConditionNumber() const { really_assert(decomposed); - T sMin = sigma.at(Cols - 1, 0); + T sMin = sigma.at(TallCols - 1, 0); if (sMin <= T{}) return T{}; return sigma.at(0, 0) / sMin; @@ -502,12 +517,15 @@ namespace solvers const math::Vector& b) const { really_assert(decomposed); - math::Vector utb{}; - for (std::size_t i = 0; i < Cols; ++i) + const auto& u = U(); + const auto& v = V(); + + math::Vector utb{}; + for (std::size_t i = 0; i < SingularValueCount; ++i) { T dot{ T{} }; for (std::size_t k = 0; k < Rows; ++k) - dot += uMat.at(k, i) * b.at(k, 0); + dot += u.at(k, i) * b.at(k, 0); utb.at(i, 0) = (sigma.at(i, 0) > T{ 1e-10f }) ? dot / sigma.at(i, 0) : T{}; } @@ -515,8 +533,8 @@ namespace solvers for (std::size_t i = 0; i < Cols; ++i) { T dot{ T{} }; - for (std::size_t k = 0; k < Cols; ++k) - dot += vMat.at(i, k) * utb.at(k, 0); + for (std::size_t k = 0; k < SingularValueCount; ++k) + dot += v.at(i, k) * utb.at(k, 0); x.at(i, 0) = dot; } @@ -526,5 +544,6 @@ namespace solvers #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class SingularValueDecomposition; extern template class SingularValueDecomposition; + extern template class SingularValueDecomposition; #endif } diff --git a/numerical/solvers/test/TestSingularValueDecomposition.cpp b/numerical/solvers/test/TestSingularValueDecomposition.cpp index d4cef305..f7c437bc 100644 --- a/numerical/solvers/test/TestSingularValueDecomposition.cpp +++ b/numerical/solvers/test/TestSingularValueDecomposition.cpp @@ -10,6 +10,7 @@ namespace protected: solvers::SingularValueDecomposition svd{}; solvers::SingularValueDecomposition svdSquare{}; + solvers::SingularValueDecomposition svdWide{}; math::Matrix a43{ { 1.0f, 2.0f, 3.0f }, @@ -17,6 +18,8 @@ namespace { 7.0f, 8.0f, 10.0f }, { 1.0f, 0.0f, 2.0f } }; + + math::Matrix a34{ a43.Transpose() }; }; } @@ -270,3 +273,63 @@ TEST_F(TestSingularValueDecomposition, trailing_zero_diagonal_block_is_deflated_ EXPECT_NEAR(svd.SingularValues().at(1, 0), 2.0f, 1e-4f); EXPECT_NEAR(svd.SingularValues().at(2, 0), 0.0f, 1e-4f); } + +TEST_F(TestSingularValueDecomposition, wide_matrix_reconstructs_A) +{ + ASSERT_TRUE(svdWide.Decompose(a34)); + + math::Matrix sigMat{}; + for (std::size_t i = 0; i < 3; ++i) + sigMat.at(i, i) = svdWide.SingularValues().at(i, 0); + + auto reconstructed = svdWide.U() * sigMat * svdWide.V().Transpose(); + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 4; ++j) + EXPECT_NEAR(reconstructed.at(i, j), a34.at(i, j), 1e-4f); +} + +TEST_F(TestSingularValueDecomposition, wide_matrix_U_and_V_orthonormal) +{ + ASSERT_TRUE(svdWide.Decompose(a34)); + + auto utu = svdWide.U().Transpose() * svdWide.U(); + auto vtv = svdWide.V().Transpose() * svdWide.V(); + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + { + float expected = (i == j) ? 1.0f : 0.0f; + EXPECT_NEAR(utu.at(i, j), expected, 1e-4f); + EXPECT_NEAR(vtv.at(i, j), expected, 1e-4f); + } +} + +TEST_F(TestSingularValueDecomposition, wide_pseudo_inverse_is_right_inverse) +{ + ASSERT_TRUE(svdWide.Decompose(a34)); + + auto aaPlus = a34 * svdWide.PseudoInverse(1e-6f); + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + EXPECT_NEAR(aaPlus.at(i, j), (i == j) ? 1.0f : 0.0f, 1e-4f); +} + +TEST_F(TestSingularValueDecomposition, wide_least_squares_returns_minimum_norm_solution) +{ + math::Matrix underdetermined{ + { 1.0f, 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f, 1.0f } + }; + math::Vector b{ { 1.0f }, { 2.0f }, { 3.0f } }; + + ASSERT_TRUE(svdWide.Decompose(underdetermined)); + auto x = svdWide.SolveLeastSquares(b); + + EXPECT_NEAR(x.at(0, 0), 0.5f, 1e-4f); + EXPECT_NEAR(x.at(1, 0), 0.5f, 1e-4f); + EXPECT_NEAR(x.at(2, 0), 1.5f, 1e-4f); + EXPECT_NEAR(x.at(3, 0), 1.5f, 1e-4f); +} diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index 63ab214f..353c1ed0 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -21,7 +21,7 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation 4. **CMake** - Add `.hpp` to `target_sources(...)`, `.cpp` to `numerical_add_coverage_sources(...)`, `Test.cpp` to the `_test` target's `target_sources`. - - New module (`robust_control`, `nonlinear_control`): + - New module (`robust_control`, `nonlinear_control`, `motion_profiles`): create `numerical//CMakeLists.txt` via `numerical_add_header_library(...)`, add a `test/` subdir, register it in the parent `CMakeLists.txt`, and add a `doc//` folder. diff --git a/roadmap/README.md b/roadmap/README.md index 461d7164..59dd9e95 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -20,8 +20,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers `template` interface that is validated and instantiated for **`float`** only (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. -The canonical worked example is -[filters/passive/ExponentialMovingAverage](filters/passive/ExponentialMovingAverage/implementation.md). +The canonical worked example is [math/SE3Transform](math/SE3Transform/implementation.md). ## Deployment shape (float-only) @@ -84,7 +83,10 @@ 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) +`SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) · `FrictionCompensation` (57) + +### `motion_profiles` +`PolynomialProfile` (55) · `TrapezoidalProfile` (56) · `SCurveProfile` (60) ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) @@ -96,16 +98,19 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `SignalDetectors` (5) · `ConvolutionCorrelation` (11) · `GoertzelAlgorithm` (13) · `RealFastFourierTransform` (25) · `HilbertTransform` (37) · `DiscreteWaveletTransform` (38) ### `estimators/offline` -`PolynomialFitting` (12) · `TotalLeastSquares` (44) · `DynamicParameterIdentification` (M22) +`PolynomialFitting` (12) · `TotalLeastSquares` (44) · `IncrementalQrLeastSquares` (63) ### `estimators/online` -`LmsAdaptiveFilter` (21) · `MomentumObserver` (M16) +`LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (M6) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `AngleUtilities` (54) · `TridiagonalSolver` (59) · `CubicSpline` (61) + +### `optimization` +`LevenbergMarquardt` (64) ### `control_analysis` `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) ### `solvers` -`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) +`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `SymplecticIntegrators` (58) · `DampedPseudoInverse` (62) · `ActiveSetQp` (65) diff --git a/roadmap/controllers/FrictionCompensation/explanation.md b/roadmap/controllers/FrictionCompensation/explanation.md new file mode 100644 index 00000000..15b32c98 --- /dev/null +++ b/roadmap/controllers/FrictionCompensation/explanation.md @@ -0,0 +1,35 @@ +# 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 new file mode 100644 index 00000000..5710dedd --- /dev/null +++ b/roadmap/controllers/FrictionCompensation/implementation.md @@ -0,0 +1,86 @@ +# 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 new file mode 100644 index 00000000..b5da24d8 --- /dev/null +++ b/roadmap/controllers/FrictionCompensation/tests.md @@ -0,0 +1,66 @@ +# 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). diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md new file mode 100644 index 00000000..ced17a23 --- /dev/null +++ b/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md @@ -0,0 +1,34 @@ +# Incremental QR Least Squares — Overview + +## What it is +A least-squares solver that takes measurements one row at a time, keeps only a small square summary, +and at the end returns the best-fitting parameters — together with which parameter combinations the +data could actually determine. + +## Why it matters (embedded) +Identification and calibration runs produce long logs: thousands of samples of motor current versus +acceleration, or joint torques versus motion. Storing them all does not fit in a microcontroller, and the +textbook shortcut (normal equations) squares the condition number, which `float` cannot afford. Rotating +each row into a triangular summary keeps memory fixed and accuracy close to a full QR. Real systems are +often rank-deficient — some parameters never show up in the data, or only in fixed combinations — so the +solver must report what is identifiable instead of returning garbage. + +## How it works (intuition) +Keep an upper-triangular matrix and a right-hand-side vector. For each new row, a sequence of plane +rotations zeroes its entries one by one against the triangle; what remains of the right-hand side is +pure misfit and is just accumulated as a squared error. Solving at the end means decomposing the small +triangle into singular directions, inverting the strong ones and dropping the weak ones: the result is +the minimum-norm solution, and the kept directions are the identifiable combinations. + +## Key parameters +- **Parameter count `P`** — compile-time; memory is `P²`. +- **Relative rank tolerance** — which singular values count as identifiable. + +## Reference +G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., §5.1.8 (Givens QR) and §5.5 (rank-deficient +least squares); Å. Björck, *Numerical Methods for Least Squares Problems* (1996), Ch. 2. + +## See also +`RecursiveLeastSquares` (online, forgetting), `LinearRegression` (batch with stored data), +`TotalLeastSquares`, `SingularValueDecomposition`, `GivensRotation`; robotics-toolbox-cpp's dynamic +parameter identification, which feeds the inertial regressor into this solver. diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md new file mode 100644 index 00000000..a22db7c7 --- /dev/null +++ b/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md @@ -0,0 +1,91 @@ +# Incremental QR Least Squares (rank-revealing) — Implementation Pseudocode + +> Roadmap ref: #63 (Tier 3) · Target: `numerical/estimators/offline` · Namespace `estimators` · Type: `float` (templated on `T`, instantiated for `float` only) + +Batch linear least squares `min ‖Y·π − τ‖` over an unbounded number of rows with **fixed memory**: each +row is rotated into an upper-triangular factor with Givens rotations as it arrives, no sample is stored, +and `YᵀY` is never formed. The solve is rank-revealing (SVD of the `P×P` factor), returning the +minimum-norm solution and the identifiable parameter combinations when the regressor is rank-deficient. +Consumers: robot inertial-parameter identification (robotics-toolbox-cpp), motor/drive parameter +identification, sensor calibration from long logs. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct LeastSquaresResult: + math::Vector parameters # minimum-norm solution + std::size_t rank # number of identifiable combinations + math::Matrix rowSpace # first `rank` rows span the identifiable combinations + T residualRms # sqrt(‖Y·π̂ − τ‖² / rows) + +template +class IncrementalQrLeastSquares: + math::SquareMatrix R # upper-triangular factor (STATE) + math::Vector d # Qᵀ·τ (STATE) + T residualSquares # part of ‖τ‖² outside range(R) (STATE) + std::size_t rows +``` + +## Interface + +```cpp +void AddRow(const math::Vector& row, T rhs) # O(P²); hot path of the accumulation +template +void AddRows(const math::Matrix& block, const math::Vector& rhs) # AddRow per row +std::optional> Solve(T relativeTolerance) const # nullopt if no rows +void Reset() +std::size_t Rows() const +``` + +## Algorithm (pseudocode) + +```text +function AddRow(row, rhs): # OPTIMIZE_FOR_SPEED — streaming Givens QR + for j in 0..P−1: # rotate the row into R + if row[j] == 0: continue + g = math::ComputeGivens(R[j][j], row[j]) + rotate (R[j][j..P−1], d[j]) with (row[j..P−1], rhs) using g # zeroes row[j] + residualSquares += rhs² # what is left lies outside range(R) + rows += 1 + +function Solve(tol): + if rows == 0: return nullopt + svd.Decompose(R) # SingularValueDecomposition + rank = #{ σᵢ > tol·σ₀ } + π̂ = Σ_{i)`. + +## Deployment + +- Header: `numerical/estimators/offline/IncrementalQrLeastSquares.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` + on `AddRow`, and `extern template class IncrementalQrLeastSquares;` / `` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/estimators/offline/IncrementalQrLeastSquares.cpp` → the same instantiations. +- Test: `numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp` +- Doc: `doc/estimators/IncrementalQrLeastSquares.md` (per `doc/TEMPLATE.md`) + row in `doc/estimators/README.md` +- CMake: `.hpp` → `target_sources(numerical.estimators.offline PRIVATE ...)`; `.cpp` → + `numerical_add_coverage_sources(numerical.estimators.offline ...)`; test → `numerical.estimators.offline_test`. +- Depends on: `math::GivensRotation`, `solvers::SingularValueDecomposition`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md new file mode 100644 index 00000000..0879ee10 --- /dev/null +++ b/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md @@ -0,0 +1,45 @@ +# Incremental QR Least Squares — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestIncrementalQrLeastSquares : public ::testing::Test: + IncrementalQrLeastSquares line # rows (1, x) + IncrementalQrLeastSquares redundant # rows (1, x, x) — rank 2 + std::array x{ 0, 1, 2, 3 } +# each case below is a TEST_F(TestIncrementalQrLeastSquares, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +exact_data_recovers_parameters: + Arrange: AddRow((1, xᵢ), 2 + 3xᵢ) for every i + Assert: Solve(1e-5) → parameters ≈ (2, 3), rank == 2, residualRms ≈ 0 +noisy_data_matches_batch_least_squares: + Arrange: τ = (2.1, 4.9, 8.1, 10.9) + Assert: parameters ≈ (2.06, 2.96), residualRms ≈ 0.089443 +rank_deficient_regressor_returns_minimum_norm_solution: + Arrange: redundant.AddRow((1, xᵢ, xᵢ), 2 + 3xᵢ) + Assert: rank == 2; parameters ≈ (2, 1.5, 1.5); rowSpace row 2 ∥ (0, 1, −1) +row_order_does_not_change_the_result: + Assert: feeding the noisy rows reversed gives the same parameters and residualRms (tol 1e-5) +block_rows_equal_single_rows: + Assert: AddRows(4×2 block, τ) gives the same result as four AddRow calls +no_rows_returns_nullopt: + Assert: Solve(1e-5) == std::nullopt, also after Reset() +``` + +## Reference vectors + +- `x = (0, 1, 2, 3)`, `τ = (2.1, 4.9, 8.1, 10.9)`: least squares `(2.06, 2.96)`, residual RMS `0.089443`. +- Rows `(1, x, x)`, `τ = 2 + 3x`: `σ = (5.540482, 1.141517, 0)`, minimum-norm `(2, 1.5, 1.5)`, null + direction `(0, 1, −1)/√2` (python3/numpy). + +## Edge cases + +- One row only ⇒ rank 1, finite result. +- Large row count (10⁵ rows of the line model) ⇒ same parameters; no growth in memory. +- Rows that are exact multiples of earlier ones ⇒ rank unchanged. diff --git a/roadmap/math/AngleUtilities/explanation.md b/roadmap/math/AngleUtilities/explanation.md new file mode 100644 index 00000000..d864d52b --- /dev/null +++ b/roadmap/math/AngleUtilities/explanation.md @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 00000000..6a106341 --- /dev/null +++ b/roadmap/math/AngleUtilities/implementation.md @@ -0,0 +1,74 @@ +# 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 new file mode 100644 index 00000000..2f68dbc5 --- /dev/null +++ b/roadmap/math/AngleUtilities/tests.md @@ -0,0 +1,39 @@ +# 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π`. diff --git a/roadmap/math/CubicSpline/explanation.md b/roadmap/math/CubicSpline/explanation.md new file mode 100644 index 00000000..ec463731 --- /dev/null +++ b/roadmap/math/CubicSpline/explanation.md @@ -0,0 +1,36 @@ +# Cubic Spline Interpolation — Overview + +## What it is +A smooth curve through a list of points: between consecutive knots each channel is a cubic +polynomial, and the pieces join so value, slope **and** curvature are continuous (C²). The ends are +either **clamped** (prescribed slopes) or **natural** (zero curvature). Several channels that share the +same knots are fitted together. + +## Why it matters (embedded) +Waypoint lists (robot teach-in programs, CNC paths, planner output), calibration tables and resampled +signals all need a smooth interpolant that does not ring like a high-order polynomial. Fitting is one +linear-time pass on fixed storage; each evaluation is a binary search plus a cubic — cheap and +deterministic. + +## How it works (intuition) +The unknowns are the curvatures at the knots. Once known, each segment is fixed: it must hit both knot +values and match both knot curvatures. Requiring neighbouring segments to have the same slope at every +interior knot gives one equation per knot that involves only the knot and its two neighbours — a +**tridiagonal** system. The end conditions add the first and last equations. The system is diagonally +dominant, so the Thomas algorithm solves it in linear time without pivoting, and its elimination +factors depend only on the knot spacing, so every channel reuses them. + +## Key parameters +- **Knots `x_k`** — strictly increasing abscissae (times, arc length, sensor readings). +- **Values `y_k`** — one vector per knot. +- **End condition** — clamped (end slopes, default zero) or natural (zero end curvature). +- **`MaxKnots`** — compile-time capacity of the bounded storage. + +## Reference +C. de Boor, *A Practical Guide to Splines*, rev. ed. (2001), Ch. IV; L. Biagiotti, C. Melchiorri, +*Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 4. + +## See also +`TridiagonalSolver` (#59, the solve), `PolynomialProfile` (#55, the two-knot special case), +`PolynomialFitting` (least-squares fit rather than interpolation); robotics-toolbox-cpp's joint-space +spline trajectory, a thin wrapper over this interpolant. diff --git a/roadmap/math/CubicSpline/implementation.md b/roadmap/math/CubicSpline/implementation.md new file mode 100644 index 00000000..0ff0055b --- /dev/null +++ b/roadmap/math/CubicSpline/implementation.md @@ -0,0 +1,116 @@ +# Cubic Spline Interpolation (vector-valued, clamped / natural) — Implementation Pseudocode + +> Roadmap ref: #61 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) + +C² piecewise-cubic interpolant through `K` knots `(x_k, y_k)`, with `y_k` a `Dimension`-vector so several +channels that share the abscissa (the joints of a robot, the axes of a CNC path, the channels of a +calibration table) are fitted in one pass. Ends are **clamped** (prescribed slopes) or **natural** (zero +second derivative). Fitting is one tridiagonal solve (#59) whose factorization depends only on the knot +spacing and is shared by all channels. + +## Data structures + +```cpp +enum class SplineEnd : uint8_t { Clamped, Natural } + +template # static_assert(std::is_floating_point_v) +struct SplineBoundary: + SplineEnd condition = SplineEnd::Clamped + math::Vector startSlope{}, endSlope{} # Clamped only; default zero + +template +struct SplinePoint: + math::Vector value, firstDerivative, secondDerivative + +template # static_assert(MaxKnots >= 2); instantiated for float +class CubicSpline: + using Value = math::Vector + std::array knots # x_0 < x_1 < … < x_{K−1} + std::array values # y_k + std::array secondDerivatives # M_k = y''(x_k), solved by Fit + std::array inverseSpan # 1/h_k, h_k = x_{k+1} − x_k + std::size_t count # K, 2 ≤ K ≤ MaxKnots +``` + +## Interface + +```cpp +bool Fit(const std::array& knots, const std::array& values, + std::size_t count, const SplineBoundary& boundary = {}) + # false if count ∉ [2, MaxKnots] or knots not strictly increasing +SplinePoint Evaluate(T x) const # hot path; x clamped to [x_0, x_{K−1}]; precondition: Fit() == true +T First() const; T Last() const # x_0, x_{K−1} +``` + +## Algorithm (pseudocode) + +```text +# Per channel j, unknowns M_k = y''(x_k). On interval k (τ = x − x_k ∈ [0, h_k]): +# y(τ) = y_k + β_k·τ + (M_k/2)·τ² + d_k·τ³ +# β_k = (y_{k+1} − y_k)/h_k − h_k·(2M_k + M_{k+1})/6 # slope at x_k +# d_k = (M_{k+1} − M_k)/(6h_k) +# Value interpolates the knots and the second derivative is shared at knots ⇒ C⁰ and C² by construction. +# Slope continuity at interior knots k = 1..K−2 gives the tridiagonal rows +# h_{k−1}·M_{k−1} + 2(h_{k−1} + h_k)·M_k + h_k·M_{k+1} = 6·[(y_{k+1} − y_k)/h_k − (y_k − y_{k−1})/h_{k−1}] +# End rows: +# Clamped: 2h_0·M_0 + h_0·M_1 = 6·[(y_1 − y_0)/h_0 − s_start] +# h_{K−2}·M_{K−2} + 2h_{K−2}·M_{K−1} = 6·[s_end − (y_{K−1} − y_{K−2})/h_{K−2}] +# Natural: M_0 = 0, M_{K−1} = 0 +# Every row is diagonally dominant (|diag| = 2·Σ|off-diag|, or 1 vs 0) ⇒ Thomas (#59), no pivoting. + +function Fit(x, y, count, boundary): + if count < 2 or count > MaxKnots: return false + for k in 0..count−2: if x[k+1] − x[k] ≤ ε·max(1, |x[k]|): return false + store knots, values, count; inverseSpan[k] = 1/h_k + build sub, diag, sup from h and boundary.condition # stack bands of MaxKnots + if !tridiagonal.Factor(sub, diag, sup, count): return false # math::TridiagonalSolver + for j in 0..Dimension−1: # O(K) per channel + r = right-hand side of channel j (rows above; start/end slope_j when Clamped, 0 when Natural) + M[·][j] = tridiagonal.Solve(r) + return true + +function Evaluate(x): # OPTIMIZE_FOR_SPEED + x = clamp(x, x_0, x_{K−1}) + k = (upper_bound(knots[1 .. K−1], x) index) clamped to [0, K−2] # binary search, O(log K) + τ = x − x_k; h = x_{k+1} − x_k; invH = inverseSpan[k] + for j in 0..Dimension−1: + d = (M_{k+1,j} − M_{k,j})·invH/6 + β = (y_{k+1,j} − y_{k,j})·invH − h·(2M_{k,j} + M_{k+1,j})/6 + value_j = y_{k,j} + τ·(β + τ·(M_{k,j}/2 + τ·d)) + firstDerivative_j = β + τ·(M_{k,j} + 3d·τ) + secondDerivative_j = M_{k,j} + 6d·τ + return { value, firstDerivative, secondDerivative } +``` + +Verified with python3: knots reproduced exactly, one-sided first/second derivatives at interior knots +agree to `2e-15`, clamped end slopes exact, natural end second derivatives zero, collinear knots ⇒ +`M ≡ 0`, two clamped knots ⇒ exactly the cubic Hermite polynomial (`PolynomialProfile`, #55). + +## Complexity & memory + +- `Fit`: `O(K)` shared factorization + `O(K)` per channel ⇒ `O(Dimension·K)`; no pivoting, no iteration. +- `Evaluate`: `O(log K)` interval search + `O(Dimension)` Horner evaluations. +- Memory: `MaxKnots·(2 + 2·Dimension)` scalars in bounded `std::array`s plus the solver's three bands. + No heap. + +## Numerical / embedded notes + +- Fit once, then `Evaluate` is deterministic: one binary search plus fixed arithmetic. +- Reject `h_k ≤ ε`: near-coincident knots make the rows ill-conditioned and `M` huge. +- Clamped ends fix the end slopes but leave the end second derivatives free (generally non-zero); natural + ends zero the second derivatives but leave the slopes free — pick per application. +- A cubic spline interpolates; it does not bound the curve between knots (overshoot is possible). Check + with `Evaluate` where limits matter. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `numerical/math/CubicSpline.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Evaluate`, and + `extern template class CubicSpline;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/math/CubicSpline.cpp` → `template class CubicSpline;` +- Test: `numerical/math/test/TestCubicSpline.cpp` +- Doc: `doc/math/CubicSpline.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 ...)`; `TestCubicSpline.cpp` → `numerical.math_test`. +- Depends on: #59 (`TridiagonalSolver`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/CubicSpline/tests.md b/roadmap/math/CubicSpline/tests.md new file mode 100644 index 00000000..67625cde --- /dev/null +++ b/roadmap/math/CubicSpline/tests.md @@ -0,0 +1,76 @@ +# Cubic Spline Interpolation — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestCubicSpline : public ::testing::Test: + using Spline = CubicSpline + std::array knots{ 0, 1, 2.5, 3, 4.5 } # K = 5 used + std::array, 8> values{ (0, 0.5, 0), (1, 0.2, 0), (−0.5, 0.2, 0), + (0.3, −0.4, 0), (2, 0, 0) } + Spline spline + void SetUp(): ASSERT_TRUE(spline.Fit(knots, values, 5)) # clamped, zero end slopes +# each case below is a TEST_F(TestCubicSpline, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +passes_through_every_knot: + Assert: Evaluate(x_k).value ≈ y_k for k = 0..4, every channel + +first_and_second_derivatives_continuous_at_interior_knots: + Assert: for k = 1..3: Evaluate(x_k − δ) ≈ Evaluate(x_k + δ) in first and second derivative + (δ = 1e-4, tol 1e-2 ≥ |y'''|·2δ), every channel + +knot_second_derivatives_match_reference: + Assert: channel 0: Evaluate(x_k).secondDerivative ≈ (5.665169, −5.330337, 5.991011, −0.737079, −1.898127); + channel 1: ≈ (−1.666292, 1.532584, −2.797753, 3.384270, −2.225468); + channel 2 (all knots 0) ≡ 0 + +clamped_end_slopes_are_honoured: + Arrange: Fit(..., { Clamped, startSlope = (0.4, 0, 0), endSlope = (−0.7, 0, 0) }) + Assert: Evaluate(0).firstDerivative_0 ≈ 0.4, Evaluate(4.5).firstDerivative_0 ≈ −0.7; + Evaluate(0).secondDerivative_0 ≈ 4.296629, Evaluate(4.5).secondDerivative_0 ≈ −3.637453 + +natural_ends_have_zero_second_derivative: + Arrange: Fit(..., { Natural }) + Assert: Evaluate(0).secondDerivative ≈ 0, Evaluate(4.5).secondDerivative ≈ 0; + channel 0 end slopes ≈ 1.680287 and 0.783154 + +collinear_knots_reproduce_straight_line: + Arrange: knots {0, 0.5, 1, 1.5, 2}, y_k = 1 + 0.8·x_k (all channels), Clamped with slope 0.8 + Assert: Evaluate(0.7) ≈ { 1.56, 0.8, 0 }; second derivative ≈ 0 everywhere sampled + +three_knot_reference_values: + Arrange: knots {0, 1, 2}, y {0, 1, 0}, clamped zero + Assert: Evaluate(0.5) ≈ { 0.5, 1.5, 0 }; knot second derivatives (6, −6, 6); Evaluate(1).firstDerivative ≈ 0 + +evaluate_clamps_outside_domain: + Assert: Evaluate(−1) == Evaluate(0) and Evaluate(5.5) == Evaluate(4.5) + +fit_rejects_invalid_knots: + Assert: Fit(count = 1) == false; Fit(count = 9) == false; knots {0, 1, 1, 2} == false +``` + +## Reference vectors + +Computed with python3 (tridiagonal system + Thomas solve, checked against the defining conditions). + +- Fixture channel 0 (`x = 0, 1, 2.5, 3, 4.5`; `y = 0, 1, −0.5, 0.3, 2`): + - clamped zero ⇒ `M = (5.665169, −5.330337, 5.991011, −0.737079, −1.898127)`; + - clamped `(0.4, −0.7)` ⇒ `M = (4.296629, −4.993258, 5.779775, −0.058427, −3.637453)`; + - natural ⇒ `M = (0, −4.081720, 5.605735, −1.400717, 0)`, end slopes `1.680287`, `0.783154`. +- Fixture channel 1 (`y = 0.5, 0.2, 0.2, −0.4, 0`), clamped zero ⇒ + `M = (−1.666292, 1.532584, −2.797753, 3.384270, −2.225468)`. +- Three knots `(0,0), (1,1), (2,0)`, clamped zero ⇒ `M = (6, −6, 6)`, `y(0.5) = 0.5`, `y'(0.5) = 1.5`; + natural ⇒ `M = (0, −3, 0)`, `y(0.5) = 0.6875`. +- Collinear knots with matching clamped slope ⇒ right-hand side ≡ 0 ⇒ `M ≡ 0`. + +## Edge cases + +- `K = 2`, Natural ⇒ `M = 0`: straight line (end slopes free). +- Unequal spacing (fixture) — the formulation never assumes uniform `h`. +- `K = MaxKnots` (8) ⇒ accepted; no out-of-bounds access in the binary search at `x = x_{K−1}`. diff --git a/roadmap/math/SE3Transform/explanation.md b/roadmap/math/SE3Transform/explanation.md index 4edd9927..5976239b 100644 --- a/roadmap/math/SE3Transform/explanation.md +++ b/roadmap/math/SE3Transform/explanation.md @@ -1,34 +1,38 @@ -# SE(3) Transform — Overview +# SE(3) Transform, Twists and Wrenches — Overview ## What it is -`SE(3)` is the group of rigid-body motions in 3D: a rotation plus a translation, stored as a 4×4 -homogeneous matrix `[[R, p],[0, 1]]`. Its "velocity" companions are **twists** (6-vectors pairing -angular and linear velocity) and **wrenches** (moment plus force); the **adjoint** map moves those -6-vectors between coordinate frames. +The algebra of rigid-body motion: a transform is a rotation plus a translation; a *twist* is a rigid +body's instantaneous velocity (linear and angular); a *wrench* is a force together with a moment. The +adjoint map moves twists and wrenches between frames, and the exponential/logarithm maps convert +between a constant twist held for some time and the finite motion it produces. ## Why it matters (embedded) -This is the algebra every modern manipulator, drone, and SLAM stack is built on. Forward kinematics -chains `SE(3)` products; Jacobians and dynamics are expressed with twists and the adjoint; trajectory -interpolation lives on this group. A compact, correct `SE(3)` type is the foundation the whole -robotics layer reuses. +Every pose-level algorithm — manipulator kinematics and Jacobians, pose inverse kinematics, Cartesian +interpolation, task-space control, drone attitude/position control, visual odometry — needs the same +small set of operations. Defining them once, with one ordering convention, prevents the silent sign +and ordering mismatches that otherwise appear when independently written modules exchange 6-vectors. ## How it works (intuition) -Composing two motions multiplies their homogeneous matrices, which reduces to `R = R₁R₂`, -`p = R₁p₂ + p₁` — no full 4×4 multiply needed. The inverse has the closed form `(Rᵀ, −Rᵀp)`. The -**exponential map** turns a constant twist (a screw axis) applied for a "time" `θ` into the finite -motion it produces — the rigid-body generalization of Rodrigues' rotation formula — and its inverse -**log** recovers the screw from a transform. The **adjoint** is the 6×6 that re-expresses a twist or -wrench in another frame. +A transform stores a 3×3 rotation and a 3-vector translation; composing two transforms rotates and +shifts the second by the first, and the inverse has the closed form `(Rᵀ, −Rᵀp)`. A twist written in one +frame is re-expressed in another with the adjoint: rotate both parts, then add the "lever-arm" effect +of the frame offset on the linear part. Wrenches transform with the dual rule so that power (force +times velocity) is the same in every frame. The exponential turns "rotate about this line while +sliding along it" into a finite motion — Rodrigues' formula for the rotation plus a matching integral +for the translation — and the logarithm recovers that motion, which is exactly what pose-error +feedback needs. ## Key parameters -- **Twist ordering** — `(ω; v)` (angular first) here; the adjoint layout follows this convention. -- **Screw parameter `θ`** — how far along a twist/screw axis the exponential integrates. -- **Orthonormality of `R`** — must be maintained on `SO(3)`; reorthonormalize against drift. +- **Ordering convention** — linear part first for twists `(v; ω)` and wrenches `(f; n)`. +- **Singular regions** — small angles and angles near 180° in the logarithm. +- **Orthonormality of `R`** — maintained on SO(3); re-orthonormalize against drift. ## Reference K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3; R. M. Murray, Z. Li, S. S. Sastry, -*A Mathematical Introduction to Robotic Manipulation* (1994). +*A Mathematical Introduction to Robotic Manipulation* (1994), Ch. 2; J. Solà, J. Deray, D. Atchuthan, +"A micro Lie theory for state estimation in robotics," arXiv:1812.01537 (2018). (The first two use +`(ω; v)` ordering; this library uses `(v; ω)`, which only permutes the blocks.) ## See also -`Quaternion` (item 18, the `SO(3)` rotation part), `Geometry3D` (`RotationAboutAxis`, -`SkewSymmetric`), `MatrixExponential` (item 29, the `se(3)→SE(3)` analogue), `DenavitHartenberg` (M7). +`Quaternion` (the SO(3) rotation part), `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), +`MatrixExponential` (the general matrix analogue of `Exp`). diff --git a/roadmap/math/SE3Transform/implementation.md b/roadmap/math/SE3Transform/implementation.md index 942b4983..589362b9 100644 --- a/roadmap/math/SE3Transform/implementation.md +++ b/roadmap/math/SE3Transform/implementation.md @@ -1,88 +1,122 @@ -# SE(3) Transform — Implementation Pseudocode +# SE(3) Transform, Twists and Wrenches — Implementation Pseudocode -> Roadmap ref: #M6 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) +> Roadmap ref: #53 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) + +Rigid-body transform plus the twist/wrench algebra that every pose-level consumer needs (robot +kinematics and control in robotics-toolbox-cpp, SLAM, drone navigation). It fixes the **one ordering +convention** shared by all consumers: 6-vectors are ordered **linear part first** — twists `(v; ω)`, +wrenches `(f; n)` — matching the geometric Jacobian with linear rows first and the tangent ordering of +Sophus and manif. Lynch & Park and Murray et al. use `(ω; v)`; the two only permute the blocks. ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SE3: - Matrix3 R # rotation, on SO(3) - Vector3 p # translation - # equivalently the 4×4 homogeneous matrix [[R, p],[0 0 0 1]] +```cpp +template using Vector6 = math::Vector # twist (v; ω) or wrench (f; n) +template using Matrix6 = math::SquareMatrix -# spatial 6-vectors reuse Vector: -# Twist = (ω; v) angular over linear velocity -# Wrench = (m; f) moment over force +template # static_assert(std::is_floating_point_v); instantiated for float +struct SE3Transform: + Matrix3 R # rotation, orthonormal, det = +1 + Vector3 p # translation + # equivalently the 4×4 homogeneous matrix [[R, p],[0 0 0 1]] ``` ## Interface -``` -SE3() # identity -SE3(Matrix3 R, Vector3 p) -SquareMatrix Homogeneous() # 4×4 form -SE3 operator*(SE3 rhs) # compose transforms; hot path -SE3 Inverse() -Vector3 Apply(Vector3 point) # transform a point; hot path -Matrix Adjoint() # maps twists/wrenches between frames -static SE3 Exp(Vector xi, T theta) # exp: se(3) -> SE(3) -Vector Log() # log: SE(3) -> se(3) +```text +static SE3Transform Identity() +SE3Transform operator*(const SE3Transform& rhs) const # composition; hot path +SE3Transform Inverse() const # closed form, no matrix solve +Vector3 Apply(const Vector3& point) const # R·x + p; hot path +Vector3 Rotate(const Vector3& direction) const # R·x +SquareMatrix Homogeneous() const +Matrix6 Adjoint() const # twist map for (v; ω) +Vector6 TransformTwist(const Vector6& twist) const # Adjoint()·twist +Vector6 TransformWrench(const Vector6& wrench) const # dual map, see below + +static SE3Transform Exp(const Vector6& twist, T theta) # e^{[ξ]θ}; hot path +Vector6 Log() const # inverse of Exp with theta = 1 +static Vector3 LogRotation(const Matrix3& R) # SO(3) log: rotation vector, |·| ≤ π +static Vector6 PoseError(const SE3Transform& target, const SE3Transform& current) ``` ## Algorithm (pseudocode) -``` -function multiply(a, b): # OPTIMIZE_FOR_SPEED - return SE3(a.R * b.R, a.R * b.p + a.p) - -function Inverse(): # closed-form rigid inverse (no matrix solve) - return SE3(Transpose(R), -Transpose(R) * p) - -function Apply(point): - return R * point + p - -function Adjoint(): # 6×6: [[R, 0],[ [p]× R, R ]] - upper-left = R - lower-left = SkewSymmetric(p) * R # reuse Geometry3D::SkewSymmetric - lower-right = R - upper-right = 0 - -function Exp(xi = (ω; v), theta): # matrix exponential on se(3) - if VectorNorm(ω) == 0: return SE3(I, v*theta) # pure translation - R = RotationAboutAxis(ω, theta) # Rodrigues (reuse Geometry3D) - G = I*theta + (1-cosθ)*SkewSymmetric(ω) + (θ-sinθ)*SkewSymmetric(ω)² # left Jacobian - return SE3(R, G * v) - -function Log(): # inverse of Exp - (ω, theta) = axisAngleFromRotation(R) # log on SO(3) - v = InverseLeftJacobian(theta) * p - return concat(ω*theta, v*theta) +```text +function operator*(rhs): return { R·rhs.R, R·rhs.p + p } +function Inverse(): return { Rᵀ, −Rᵀ·p } + +function Adjoint(): # V_A = Ad_T · V_B for V = (v; ω) + return [[ R, Skew(p)·R ], # reuse Geometry3D::SkewSymmetric + [ 0, R ]] + +function TransformWrench(w = (f; n)): # power-consistent dual of Adjoint + fA = R·f + nA = R·n + CrossProduct(p, R·f) + return (fA; nA) + +function Exp(ξ = (v; ω), θ): # OPTIMIZE_FOR_SPEED + if ‖ω‖ < ε: # pure translation + return { I, v·θ } + # general twist: fold the magnitude of ω into θ so the axis is unit + s = ‖ω‖; ω̂ = ω / s; v̂ = v / s; φ = θ·s + R = RotationAboutAxis(ω̂, φ) # reuse Geometry3D (Rodrigues) + G = I·φ + (1 − cos φ)·Skew(ω̂) + (φ − sin φ)·Skew(ω̂)² # left Jacobian · φ + return { R, G·v̂ } + +function LogRotation(R): # rotation vector ω̂·φ, φ ∈ [0, π] + φ = atan2(‖Vee(R − Rᵀ)‖/2, (trace(R) − 1)/2) # accurate near 0 and π, unlike acos + if φ < ε: # small angle: first-order + return Vee(R − Rᵀ) / 2 + if π − φ < ε: # sin φ → 0: use the diagonal form + ω̂ = axis from the largest of sqrt((R_ii + 1) / 2), signs from the off-diagonals + else: + ω̂ = Vee(R − Rᵀ) / (2 sin φ) + return ω̂·φ + +function Log(): # returns (v; ω) with ‖ω‖ = rotation angle + ω = LogRotation(R); φ = ‖ω‖ + if φ < ε: return (p; ω) + ω̂ = ω / φ + G⁻¹ = I/φ − Skew(ω̂)/2 + (1/φ − cot(φ/2)/2)·Skew(ω̂)² + return (G⁻¹·p·φ; ω) + +function PoseError(target, current): # base-frame error, matches a linear-rows-first Jacobian + eP = target.p − current.p + eR = LogRotation(target.R · current.Rᵀ) # |eR| ≤ π, short way round + return (eP; eR) ``` ## Complexity & memory -- Compose / inverse / apply: `O(1)` — fixed 3×3 and 3-vector arithmetic, no loops. -- Adjoint / Exp / Log: `O(1)` — a handful of 3×3 products; `Log` adds one `acos`. -- Memory: a 3×3 plus a 3-vector per transform (or one 4×4); all stack-allocated, no heap. +- Compose / inverse / apply / rotate: `O(1)` — a handful of 3×3 products. +- `Exp` / `Log`: `O(1)` — one `sin`/`cos` pair (plus `atan2` for `Log`). +- Memory: 12 scalars per transform; `Matrix6` only when an adjoint is requested. No heap. ## Numerical / embedded notes -- Use the **closed-form rigid inverse** `(Rᵀ, −Rᵀp)`; never run a general 4×4 matrix solve. -- Keep `R` on `SO(3)`: reorthonormalize after long product chains (drift accumulates like quaternions). -- `Exp`/`Log` need `sinθ/θ` and `(1−cosθ)/θ²`; use series expansions near `θ→0` to avoid `0/0`. -- Twist ordering here is `(ω; v)` (angular first); the adjoint block layout must match that choice. +- **Convention (normative):** twists `(v; ω)`, wrenches `(f; n)`; `PoseError` returns + `(position; rotation-vector)`. robotics-toolbox-cpp's geometric Jacobian, controllers and pose IK + depend on this ordering; changing it is a breaking change for that consumer. +- Use the **closed-form rigid inverse** `(Rᵀ, −Rᵀp)`; never run a general 4×4 solve. +- `Log` must be the true logarithm: `2·vec(quaternion)` equals `2 sin(φ/2)·ω̂`, which is only a + small-angle approximation of `φ·ω̂`. Rotations beyond `π` are re-expressed the short way, so `|eR| ≤ π`. +- Handle both singular regions of `Log` (`φ → 0` and `φ → π`) explicitly; derive `φ` with `atan2`, not + `acos`, which loses precision near ±1 in `float`. +- Composition drifts off SO(3) over long chains of products in `float`; re-orthonormalize (Gram–Schmidt + or via `Quaternion` normalization) when a transform is integrated over time, not after single + compositions. - 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/math/SE3Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `operator*`/`Apply`, and - `extern template class SE3;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/SE3Transform.cpp` → `template class SE3;` + `operator*`/`Apply`/`Exp`, and `extern template struct SE3Transform;` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/math/SE3Transform.cpp` → `template struct SE3Transform;` - Test: `numerical/math/test/TestSE3Transform.cpp` -- Doc: `doc/math/SE3Transform.md` (new folder; math currently has no doc pages) +- Doc: `doc/math/SE3Transform.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 ...)`; `TestSE3Transform.cpp` → `numerical.math_test`. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/SE3Transform/tests.md b/roadmap/math/SE3Transform/tests.md index 0138f942..a114eeaa 100644 --- a/roadmap/math/SE3Transform/tests.md +++ b/roadmap/math/SE3Transform/tests.md @@ -4,61 +4,54 @@ ## Fixture -``` -class TestSE3 : public ::testing::Test: - SE3 identity - SE3 gAB # rotation 90° about ẑ + translation (1,0,0) -# each case below is a TEST_F(TestSE3, ) +```cpp +class TestSE3Transform : public ::testing::Test: + SE3Transform a{ RotationAboutAxis(ẑ, π/2), (1, 0, 0) } + SE3Transform b{ RotationAboutAxis(ŷ, π/3), (0, 2, 0) } +# each case below is a TEST_F(TestSE3Transform, ) ``` ## Test cases (Arrange / Act / Assert) -``` -identity_apply_is_noop: - Assert: identity.Apply(p) == p - -compose_with_inverse_is_identity: - Arrange: g = gAB - Assert: g * g.Inverse() ≈ Identity - +```cpp apply_rotates_then_translates: - Arrange: g = rot(ẑ, 90°) + p = (1,0,0) - Assert: g.Apply((1,0,0)) ≈ (1,1,0) - -inverse_matches_transpose_form: - Assert: Inverse().R == Rᵀ and Inverse().p == -Rᵀp - + Assert: a.Apply((1, 0, 0)) ≈ (1, 1, 0) +composition_applies_right_operand_first: + Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2, 0.5) +inverse_composes_to_identity: + Assert: a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity homogeneous_matches_block_form: - Assert: Homogeneous() == [[R, p],[0, 0, 0, 1]] - -adjoint_transforms_twist: - Arrange: known twist in frame B - Assert: Adjoint()·ξ matches the hand-computed twist in frame A - -exp_pure_translation: - Arrange: ω = 0, v = (1,0,0), θ = 2 - Assert: Exp ≈ SE3(I, (2,0,0)) - -exp_pure_rotation: - Arrange: ω = ẑ, v = 0, θ = π/2 - Assert: Exp ≈ SE3(rot(ẑ, 90°), 0) - -exp_log_round_trip: - Arrange: random small twist ξ - Assert: Log(Exp(ξ)) ≈ ξ - + Assert: a.Homogeneous() == [[R, p],[0, 0, 0, 1]] +adjoint_maps_twists_consistently_with_composition: + Arrange: twist V in the frame of a + Assert: Exp(a.TransformTwist(V), t) ≈ a * Exp(V, t) * a.Inverse() (t = 0.4) adjoint_of_product_is_product_of_adjoints: - Assert: Adjoint(g1*g2) ≈ Adjoint(g1)·Adjoint(g2) + Assert: (a * b).Adjoint() ≈ a.Adjoint() · b.Adjoint() +wrench_and_twist_power_is_frame_invariant: + Arrange: twist V, wrench W in the same frame + Assert: a.TransformWrench(W) · a.TransformTwist(V) ≈ W · V +exp_of_pure_rotation_about_offset_axis: + Arrange: screw about ẑ through (1, 0, 0): ξ = (−ẑ × (1,0,0); ẑ) = ((0,−1,0); ẑ) + Assert: Exp(ξ, π/2).p ≈ (1, −1, 0) and R ≈ Rz(π/2) +exp_of_pure_translation: + Assert: Exp(((1,0,0); 0), 0.3) ≈ { I, (0.3, 0, 0) } +log_inverts_exp: + Assert: Exp(Log(T), 1) ≈ T for T = a, b, and a rotation of 179.9° +log_near_zero_and_near_pi_stays_finite: + Assert: LogRotation of rotations of 1e-6 rad and π − 1e-4 rad are finite with the expected angle +pose_error_takes_the_short_way: + Arrange: target = current rotated by +190° about ẑ + Assert: ‖PoseError(target, current).angular‖ ≈ 170° (2.967 rad), direction −ẑ ``` ## Reference vectors -- `rot(ẑ, 90°) ⊕ (1,0,0)` applied to `(1,0,0)` ⇒ `(1,1,0)`. -- `Exp(ω = ẑ, v = 0, θ = π/2)` ⇒ rotation-only transform `rot(ẑ, 90°)`. +- `Rz(π/2) ⊕ (1,0,0)` applied to `(1,0,0)` ⇒ `(1,1,0)`. +- Screw about `ẑ` through `(1,0,0)` by `π/2` maps the origin to `(1,−1,0)`. +- Rotation error of `+190°` about `ẑ` ⇒ rotation vector `−170°·ẑ`. ## Edge cases -- `θ → 0` in `Exp`/`Log` ⇒ series fallback, no `sinθ/θ` division blow-up. -- `θ = π` rotation ⇒ axis recovery in `Log` stays well-defined. -- Non-orthonormal `R` fed in ⇒ document the reorthonormalization expectation. -- Twist with zero angular part ⇒ `Adjoint` lower-left `[p]×R` still correct. +- `Exp` with `ω = 0` (pure translation) and with non-unit `ω` (magnitude folded into the angle). +- `LogRotation` at `φ = π` exactly: either axis sign is valid; the test accepts both. +- Repeated composition in `float` keeps `RᵀR ≈ I` to `1e-5` after 1000 random products. diff --git a/roadmap/math/TridiagonalSolver/explanation.md b/roadmap/math/TridiagonalSolver/explanation.md new file mode 100644 index 00000000..58638186 --- /dev/null +++ b/roadmap/math/TridiagonalSolver/explanation.md @@ -0,0 +1,29 @@ +# 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 new file mode 100644 index 00000000..e82951f8 --- /dev/null +++ b/roadmap/math/TridiagonalSolver/implementation.md @@ -0,0 +1,77 @@ +# 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 new file mode 100644 index 00000000..d6bb2c23 --- /dev/null +++ b/roadmap/math/TridiagonalSolver/tests.md @@ -0,0 +1,40 @@ +# 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. diff --git a/roadmap/motion_profiles/PolynomialProfile/explanation.md b/roadmap/motion_profiles/PolynomialProfile/explanation.md new file mode 100644 index 00000000..a3a048f7 --- /dev/null +++ b/roadmap/motion_profiles/PolynomialProfile/explanation.md @@ -0,0 +1,32 @@ +# 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 new file mode 100644 index 00000000..b401898b --- /dev/null +++ b/roadmap/motion_profiles/PolynomialProfile/implementation.md @@ -0,0 +1,112 @@ +# 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 new file mode 100644 index 00000000..82180a2c --- /dev/null +++ b/roadmap/motion_profiles/PolynomialProfile/tests.md @@ -0,0 +1,63 @@ +# 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 new file mode 100644 index 00000000..2d177ff6 --- /dev/null +++ b/roadmap/motion_profiles/SCurveProfile/explanation.md @@ -0,0 +1,35 @@ +# 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 new file mode 100644 index 00000000..a889a2ff --- /dev/null +++ b/roadmap/motion_profiles/SCurveProfile/implementation.md @@ -0,0 +1,122 @@ +# 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 new file mode 100644 index 00000000..bd35c638 --- /dev/null +++ b/roadmap/motion_profiles/SCurveProfile/tests.md @@ -0,0 +1,87 @@ +# 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 new file mode 100644 index 00000000..69106280 --- /dev/null +++ b/roadmap/motion_profiles/TrapezoidalProfile/explanation.md @@ -0,0 +1,33 @@ +# 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 new file mode 100644 index 00000000..3d9a677c --- /dev/null +++ b/roadmap/motion_profiles/TrapezoidalProfile/implementation.md @@ -0,0 +1,108 @@ +# 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 new file mode 100644 index 00000000..60c59bd1 --- /dev/null +++ b/roadmap/motion_profiles/TrapezoidalProfile/tests.md @@ -0,0 +1,71 @@ +# 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. diff --git a/roadmap/optimization/LevenbergMarquardt/explanation.md b/roadmap/optimization/LevenbergMarquardt/explanation.md new file mode 100644 index 00000000..418fd882 --- /dev/null +++ b/roadmap/optimization/LevenbergMarquardt/explanation.md @@ -0,0 +1,34 @@ +# Levenberg–Marquardt — Overview + +## What it is +An iterative solver for "find the state that makes these residuals as small as possible" when the +residuals depend nonlinearly on the state. Each iteration linearizes the residuals, solves a small damped +linear least-squares problem for a correction, and applies it through a problem-defined update. + +## Why it matters (embedded) +Inverse kinematics, the forward kinematics of parallel robots, sensor and hand-eye calibration, and scan +matching are all small nonlinear least-squares problems solved every control cycle or at commissioning. +Writing one careful solver — bounded iterations, no heap, singularity-safe damping, a pluggable update for +angles, rotations and joint limits — replaces a hand-rolled Newton loop in every module. + +## How it works (intuition) +Near the current guess the residual behaves like a linear function of a small correction; Gauss–Newton +solves that linear problem exactly. When the linearization is poor or nearly singular, a damping term +penalizes large corrections, trading speed for robustness — the Levenberg–Marquardt idea. The correction +is applied by a "retraction" supplied by the problem: ordinary addition for plain vectors, addition plus +clamping for bounded states, or a rotation update for orientations, so the state never leaves its valid +set. + +## Key parameters +- **Damping policy** — constant `λ`, or a problem-aware schedule (e.g. zero away from singularities). +- **Tolerance** and **iteration budget** — stopping rule and worst-case cost. +- **Step cap** — maximum correction per iteration. + +## Reference +K. Levenberg, *Quarterly of Applied Mathematics*, 2(2), 1944; D. Marquardt, *SIAM J. Appl. Math.*, 11(2), +1963; J. Nocedal, S. Wright, *Numerical Optimization*, 2nd ed. (2006), Ch. 10; P.-A. Absil, R. Mahony, +R. Sepulchre, *Optimization Algorithms on Matrix Manifolds* (2008), Ch. 4 (retractions). + +## See also +`GradientDescent` / `Adam` (first-order optimizers for general costs), `LuDecomposition` (the step solve), +`DampedPseudoInverse` (#62, the same damping for one linear solve), `SE3Transform` (#53, pose retractions). diff --git a/roadmap/optimization/LevenbergMarquardt/implementation.md b/roadmap/optimization/LevenbergMarquardt/implementation.md new file mode 100644 index 00000000..756bc099 --- /dev/null +++ b/roadmap/optimization/LevenbergMarquardt/implementation.md @@ -0,0 +1,118 @@ +# Levenberg–Marquardt (damped Gauss–Newton with retraction) — Implementation Pseudocode + +> Roadmap ref: #64 (Tier 3) · Target: `numerical/optimization` · Namespace `optimization` · Type: `float` (templated on `T`, instantiated for `float` only) + +Bounded-iteration solver for small nonlinear least-squares problems `min ½‖r(x)‖²` whose state may live +on a manifold (an angle, a rotation, a pose) or inside bounds (joint limits). The problem supplies the +residual, its Jacobian with respect to a local increment `δ`, and a **retraction** `x ⊕ δ`; the solver +supplies the damped Gauss–Newton step. Consumers: pose inverse kinematics and parallel-robot forward +kinematics (robotics-toolbox-cpp), hand-eye and sensor calibration, scan matching. + +## Data structures + +```cpp +template +class LeastSquaresProblem: # pure interface; static_assert(std::is_floating_point_v) + virtual ~LeastSquaresProblem() = default + virtual math::Vector Residual(const State& x) const = 0 + virtual math::Matrix Jacobian(const State& x) const = 0 # ∂r(x ⊕ δ)/∂δ at δ = 0 + virtual State Retract(const State& x, const math::Vector& delta) const = 0 + # x ⊕ δ: plain addition, addition + clamping to bounds, or a manifold update (e.g. R ← Exp(δ)·R) + +template +class DampingPolicy: # pure interface + virtual ~DampingPolicy() = default + virtual T DampingSquared(const math::Matrix& jacobian) const = 0 # λ² ≥ 0 + +template +class ConstantDamping : public DampingPolicy: + T lambdaSquared # returns it unconditionally + +template +struct LevenbergMarquardtConfig: + T tolerance # on ‖r‖ + T maxStep # cap on ‖δ‖; +∞ disables + std::size_t maxIterations + +template +struct LevenbergMarquardtResult: + State x + T residualNorm # ‖r(x)‖ at the returned x + std::size_t iterations + bool converged + +template # instantiated for float +class LevenbergMarquardt: + const LeastSquaresProblem& problem # injected + const DampingPolicy& damping # injected + LevenbergMarquardtConfig config +``` + +## Interface + +```cpp +LevenbergMarquardt(const LeastSquaresProblem<...>& problem, const DampingPolicy<...>& damping, + const LevenbergMarquardtConfig& config) +LevenbergMarquardtResult Solve(const State& initial) const # hot path +``` + +## Algorithm (pseudocode) + +```text +function Solve(x0): # OPTIMIZE_FOR_SPEED + x = x0 + for iter in 0..maxIterations−1: + r = problem.Residual(x) + if ‖r‖ < tolerance: return { x, ‖r‖, iter, true } + J = problem.Jacobian(x) + λ² = damping.DampingSquared(J) + if Residuals ≤ Parameters: # wide or square: solve in residual space + lu = LuDecomposition(J·Jᵀ + λ²·I) + if !lu: return { x, ‖r‖, iter, false } # singular undamped system + δ = −Jᵀ·lu.Solve(r) # = −Jᵀ(JJᵀ + λ²I)⁻¹r, minimum-norm step + else: # tall: solve in parameter space + lu = LuDecomposition(Jᵀ·J + λ²·I) + if !lu: return { x, ‖r‖, iter, false } + δ = −lu.Solve(Jᵀ·r) + if ‖δ‖ > maxStep: δ = δ·maxStep/‖δ‖ + x = problem.Retract(x, δ) + rFinal = ‖problem.Residual(x)‖ # recomputed at the returned x + return { x, rFinal, maxIterations, rFinal < tolerance } +``` + +## Complexity & memory + +- Per iteration: one `Residual` + one `Jacobian` (problem cost), `O(Residuals·Parameters·m)` for the + normal matrix and an `O(m³)` LU, `m = min(Residuals, Parameters)`. +- Memory: one Jacobian, one `m×m` system; stack only, no heap. Deterministic upper bound: + `maxIterations + 1` residual evaluations. + +## Numerical / embedded notes + +- **Damping does not bias the answer:** at a fixed point with `J` of full rank the step is zero only when + `r = 0` (wide) or `Jᵀr = 0` (tall); damping slows convergence (linear instead of quadratic) but does not + move the solution. A non-zero final residual means an unreachable target, a singularity or an active + bound in `Retract`. +- **Policies:** `ConstantDamping` is plain Levenberg–Marquardt with fixed `λ` (`λ = 0`: Gauss–Newton). + A problem-aware policy — e.g. robotics-toolbox-cpp's manipulability-based IK damping, zero away from + singularities — plugs in through the same interface. Gain-ratio (Nielsen) adaptation, which needs a + trial evaluation and accept/reject, is a possible later variant. +- Choosing the smaller normal system keeps the solve `min(R, P)`-sized; use `LuDecomposition`, never an + explicit inverse. Upstream LU reports singular below a relative pivot of `1e-6`; with `λ = 0` a + singular `J` therefore ends the solve with `converged = false` instead of dividing by zero. +- `maxStep` bounds the change per iteration (trust-region-like safety for poor initial guesses). +- Warm-start from the previous control cycle for few-iteration convergence. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Headers: `numerical/optimization/LevenbergMarquardt.hpp` (+ `LeastSquaresProblem.hpp`, `DampingPolicy.hpp`, + pure interfaces) — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Solve`, and + `extern template class LevenbergMarquardt, 2, 2>;` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/optimization/LevenbergMarquardt.cpp` → the same instantiation (and `ConstantDamping`). +- Test: `numerical/optimization/test/TestLevenbergMarquardt.cpp` +- Doc: `doc/optimization/LevenbergMarquardt.md` (per `doc/TEMPLATE.md`) + row in `doc/optimization/README.md` +- CMake: `.hpp` → `target_sources(numerical.optimization PRIVATE ...)` (link `numerical.solver` for LU); + `.cpp` → `numerical_add_coverage_sources(numerical.optimization ...)`; test → the optimization `_test` target. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/optimization/LevenbergMarquardt/tests.md b/roadmap/optimization/LevenbergMarquardt/tests.md new file mode 100644 index 00000000..cc405e46 --- /dev/null +++ b/roadmap/optimization/LevenbergMarquardt/tests.md @@ -0,0 +1,51 @@ +# Levenberg–Marquardt — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestLevenbergMarquardt : public ::testing::Test: + RosenbrockResiduals rosenbrock # r = (10(y − x²), 1 − x), J = [[−20x, 10], [−1, 0]], Retract = add + UnitCircleAngle circle{ target } # State = angle θ; r = (cos θ, sin θ) − target; J = (−sin θ, cos θ)ᵀ; + # Retract = θ + δ wrapped to (−π, π] + ConstantDamping gaussNewton{ 0 } + StrictMock> policy + LevenbergMarquardtConfig config{ .tolerance = 1e-5, .maxStep = +∞, .maxIterations = 20 } +# each case below is a TEST_F(TestLevenbergMarquardt, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +gauss_newton_solves_rosenbrock_residuals_in_two_steps: + Act: LevenbergMarquardt{ rosenbrock, gaussNewton, config }.Solve((−1.2, 1)) + Assert: x ≈ (1, 1), converged, iterations == 2 +damping_policy_is_queried_with_the_current_jacobian: + Arrange: config.maxIterations = 1; EXPECT_CALL(policy, DampingSquared(J(−1.2, 1) = [[24, 10], [−1, 0]])) → 0 + Assert: x ≈ (1, −3.84), !converged +step_is_clamped_to_max_step: + Arrange: config.maxIterations = 1, config.maxStep = 1 + Assert: x ≈ (−0.786197, 0.089634) (undamped step (2.2, −4.84) scaled to unit length) +retraction_keeps_a_wrapped_state: + Arrange: circle target (cos 2.5, sin 2.5), θ₀ = −2.5 (1.28 rad away across ±π) + Assert: θ ≈ 2.5, converged, θ ∈ (−π, π] after every step (first step lands at 2.824261 via the wrap) +unreachable_target_reports_non_convergence: + Arrange: circle target (2, 0), ConstantDamping{ 1 }, θ₀ = 0.3 + Assert: !converged, iterations == maxIterations, θ ≈ 0, residualNorm ≈ 1 +singular_undamped_system_stops_without_dividing_by_zero: + Arrange: constant residual (1, 1), J ≡ 0, ConstantDamping{ 0 } + Assert: !converged, iterations == 0, x == x₀ +``` + +## Reference vectors + +- Rosenbrock from `(−1.2, 1)`, Gauss–Newton: `(1, −3.84)` after one step, `(1, 1)` after two (exact). +- First undamped step `(2.2, −4.84)`, norm `5.316540`. +- Circle, `θ₀ = −2.5`, target angle `2.5`: `θ = 2.824261 → 2.505653 → 2.500000` (python3). +- Unreachable target, `λ² = 1`: `θ ← θ − sin θ` ⇒ `θ → 0`, residual `‖(1, 0) − (2, 0)‖ = 1`. + +## Edge cases + +- `maxIterations = 0` ⇒ returns `x₀` with its residual, `converged` iff already within tolerance. +- Tall problem (`Residuals > Parameters`) and wide/square problem exercise the two solve branches. diff --git a/roadmap/solvers/ActiveSetQp/explanation.md b/roadmap/solvers/ActiveSetQp/explanation.md new file mode 100644 index 00000000..bbee8465 --- /dev/null +++ b/roadmap/solvers/ActiveSetQp/explanation.md @@ -0,0 +1,34 @@ +# Dense Convex QP (Goldfarb–Idnani) — Overview + +## What it is +A solver for problems of the form "minimize a convex quadratic cost subject to linear equalities and +inequalities" — the workhorse of constrained control and allocation. It handles small, dense problems +exactly, in a bounded number of steps, without dynamic memory. + +## Why it matters (embedded) +Many embedded decisions are QPs: distribute a wrench over cables that can only pull, allocate thrust to +motors that saturate, track several tasks with priorities and joint limits, keep MPC inputs in bounds. +Heuristics such as clipping or a fixed number of projection sweeps are cheap but can be wrong at the +edges; an exact, finite active-set method gives the true optimum with a predictable worst case. + +## How it works (intuition) +Start from the unconstrained minimum, which is cheap thanks to a Cholesky factor of the cost. If it +violates a constraint, pull the solution towards that constraint along the direction that hurts the cost +least, raising its multiplier. If, on the way, an already active constraint's multiplier would turn +negative (it is pushing the wrong way), release it first. Repeat until nothing is violated. Because every +intermediate point is optimal for the constraints chosen so far, the method never needs a feasible start, +and a running orthogonal factorization keeps each step at quadratic cost. + +## Key parameters +- **Hessian `H`** — must be positive definite (strictly convex cost). +- **Constraint rows** — equalities are always enforced; inequalities only when they bind. +- **Iteration cap** and **feasibility tolerance** — worst-case cost and acceptance threshold. + +## Reference +D. Goldfarb, A. Idnani, "A numerically stable dual method for solving strictly convex quadratic +programs," *Mathematical Programming*, 27(1), 1983; M. J. D. Powell, "On the quadratic programming +algorithm of Goldfarb and Idnani," *Mathematical Programming Study*, 25, 1985. + +## See also +`CholeskyDecomposition`, `GivensRotation`, `Mpc` (box-constrained inputs), `DampedPseudoInverse` (#62, +the unconstrained counterpart); robotics-toolbox-cpp's cable-tension distribution (exact path). diff --git a/roadmap/solvers/ActiveSetQp/implementation.md b/roadmap/solvers/ActiveSetQp/implementation.md new file mode 100644 index 00000000..c658e4b3 --- /dev/null +++ b/roadmap/solvers/ActiveSetQp/implementation.md @@ -0,0 +1,127 @@ +# Dense Convex QP (Goldfarb–Idnani dual active set) — Implementation Pseudocode + +> Roadmap ref: #65 (Tier 4) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) + +Exact solver for small, dense, strictly convex quadratic programs + +```text +minimize ½·xᵀHx + gᵀx subject to E·x = e, C·x ≥ c (H symmetric positive definite) +``` + +in a bounded number of iterations, with no heap. The dual active-set method starts from the +unconstrained minimum and adds the most violated constraint each iteration, so it needs no feasible +starting point and terminates finitely. Consumers: exact cable-tension distribution and task-priority / +whole-body control (robotics-toolbox-cpp), control allocation with actuator limits, `Mpc` with input +constraints (whose current box handling is a fixed-count projected coordinate descent), minimum-snap +trajectories. + +## Data structures + +```cpp +template +struct QuadraticProgram: # static_assert(std::is_floating_point_v) + math::SquareMatrix hessian # H, SPD + math::Vector gradient # g + math::Matrix equalityMatrix # E + math::Vector equalityVector # e + math::Matrix inequalityMatrix # C (rows: C_i·x ≥ c_i) + math::Vector inequalityVector # c + # bounds lo ≤ x ≤ hi are two inequality rows per bounded variable (+e_i ≥ lo_i, −e_i ≥ −hi_i) + +enum class QpStatus : uint8_t { Optimal, Infeasible, NotPositiveDefinite, IterationLimit } + +template # Constraints = Equalities + Inequalities +struct QpResult: + math::Vector x + math::Vector multipliers # equalities first; inequality multipliers ≥ 0, zero if inactive + std::array active + std::size_t iterations + QpStatus status + +template # instantiated for float +class ActiveSetQp: + std::size_t maxIterations # default 2·(Equalities + Inequalities) + Variables + T feasibilityTolerance # relative to the constraint-row scale +``` + +`math::Matrix` has no zero-size form, so `Equalities = 0` is a partial specialization that omits the +equality block (and `Inequalities = 0` likewise); all other code paths are shared. + +## Interface + +```cpp +explicit ActiveSetQp(std::size_t maxIterations = default, T feasibilityTolerance = 1e-6) +QpResult Solve(const QuadraticProgram<...>& qp) const # hot path +``` + +## Algorithm (pseudocode) + +Goldfarb & Idnani (1983) in the numerically stable form of Powell (1985): the active constraint normals +`N` are tracked through `J = L⁻ᵀ·Q` (`Q` orthogonal) and an upper-triangular `R` with `Jᵀ·N = [R; 0]`; +adding or dropping a constraint updates `J` and `R` with Givens rotations. + +```text +function Solve(qp): # OPTIMIZE_FOR_SPEED + L = Cholesky(H); if fails: return { status = NotPositiveDefinite } + J = L⁻ᵀ # triangular inverse, O(n³) once + x = −H⁻¹·g; q = 0 (active count); u = 0; active = none + pending = all equalities # equalities are added first, in order + for iter in 0..maxIterations−1: + # Step 1 — choose the constraint to add + if pending not empty: p = next equality; s = E_p·x − e_p + else: + s, p = min over inactive inequalities i of (C_i·x − c_i) # most violated (raw residual) + if s ≥ −feasibilityTolerance·‖C_p‖: return { x, u, active, iter, Optimal } + n⁺ = constraint normal of p (sign-flipped for an equality with s > 0, so s ≤ 0) + loop: # Step 2 — may drop several constraints before adding p + d = Jᵀ·n⁺ + z = J₂·d₂ # primal direction (columns q..n−1 of J) + r = R⁻¹·d₁ # dual direction for the active set (back substitution) + t₁ = min over active INEQUALITIES j with r_j > 0 of u_j / r_j (∞ if none; k = argmin) + t₂ = ‖z‖ > ε ? −s / (zᵀ·n⁺) : ∞ + t = min(t₁, t₂) + if t == ∞: return { x, u, active, iter, Infeasible } # p cannot be satisfied + u_active = u_active − t·r; u_p = u_p + t + if t₂ < ∞: x = x + t·z; s = s + t·(zᵀ·n⁺) + if t == t₂: # full step: p becomes active + add p: rotate d₂ into its first entry with Givens on J₂'s columns; append column to R; q += 1 + break + drop k: delete column k of R, restore upper-triangular with Givens (applied to J); q −= 1 + if p was the last pending equality: pending = empty + return { x, u, active, maxIterations, IterationLimit } +``` + +## Complexity & memory + +- Setup: one Cholesky and one triangular inverse, `O(n³)` with `n = Variables`. +- Per iteration: `O(n²)` for `d`, `z`, `r` and the Givens updates; at most `maxIterations` iterations + (finite termination is guaranteed in exact arithmetic; the cap guards rounding-induced cycling). +- Memory: `J` (`n×n`), `R` (`n×n`), multipliers and flags; stack only, no heap. + +## Numerical / embedded notes + +- **Strict convexity is required** (`H` SPD): a pure LP or a semidefinite `H` fails in Cholesky + (`NotPositiveDefinite`); add a small `ε·I` regularization when an LP-like problem must be solved this way. + robotics-toolbox-cpp's TOPP-RA keeps its own two-variable LP for that reason. +- Dual feasibility (multipliers of active inequalities ≥ 0) holds at every iteration; primal feasibility is + reached at termination — an early stop returns a super-optimal, slightly infeasible `x`. +- Scale constraint rows to comparable norms: the constraint to add is the largest *raw* violation, and + only the acceptance test is relative to `‖C_p‖`. +- Degenerate (linearly dependent) active constraints show up as `‖z‖ ≈ 0` with a blocking `t₁` — the + drop branch handles them; truly contradictory constraints give `t = ∞` ⇒ `Infeasible`. +- Warm start (re-using the previous active set) is a possible later extension for MPC loops. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `numerical/solvers/ActiveSetQp.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Solve`, and + `extern template class ActiveSetQp;` / `` / `` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/solvers/ActiveSetQp.cpp` → the same instantiations. +- Test: `numerical/solvers/test/TestActiveSetQp.cpp` +- Doc: `doc/solvers/ActiveSetQp.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 ...)`; `TestActiveSetQp.cpp` → `numerical.solvers_test`. +- Follow-up: `Mpc` input constraints can switch from projected coordinate descent to this solver. +- Depends on: `math::CholeskyDecomposition`, `math::TriangularSolve`, `math::GivensRotation`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/ActiveSetQp/tests.md b/roadmap/solvers/ActiveSetQp/tests.md new file mode 100644 index 00000000..18cb4427 --- /dev/null +++ b/roadmap/solvers/ActiveSetQp/tests.md @@ -0,0 +1,57 @@ +# Dense Convex QP (Goldfarb–Idnani) — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestActiveSetQp : public ::testing::Test: + # least-distance problems: H = I, g = −x_ref ⇒ minimize ½‖x − x_ref‖² + ActiveSetQp withEquality + ActiveSetQp boxOnly +# each case below is a TEST_F(TestActiveSetQp, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +inactive_constraints_return_the_unconstrained_minimum: + Arrange: x_ref = (1, 1); box −5 ≤ x ≤ 5 + Assert: x ≈ (1, 1), status Optimal, all multipliers 0, no constraint active +equality_constraint_is_met_with_its_multiplier: + Arrange: x_ref = (0, 0); x₁ + x₂ = 1; inequalities x ≥ −5 (inactive) + Assert: x ≈ (0.5, 0.5), equality multiplier ≈ 0.5 +violated_inequality_becomes_active: + Arrange: boxOnly-style rows: −x₁ − x₂ ≥ −1 (x₁ + x₂ ≤ 1), x_ref = (1, 1) + Assert: x ≈ (0.5, 0.5), that multiplier ≈ 0.5, active +box_bounds_clip_to_the_nearest_corner: + Arrange: x_ref = (2, −1); 0 ≤ x ≤ 1 (four rows) + Assert: x ≈ (1, 0), multipliers of x₁ ≤ 1 and x₂ ≥ 0 ≈ 1, others 0 +blocking_constraint_is_dropped_on_the_way: + Arrange: x_ref = (0, 0); rows x₂ ≥ 1 (raw violation 1) and 0.1·x₁ + 0.5·x₂ ≥ 0.9 (raw violation 0.9) + Assert: x ≈ (0.346154, 1.730769), only the second row active, its multiplier ≈ 3.461538 + (x₂ ≥ 1 is added first, its multiplier turns negative when the second row joins, and it is dropped) +contradictory_constraints_are_infeasible: + Arrange: x₁ ≥ 1 and x₁ ≤ 0 + Assert: status Infeasible +indefinite_hessian_is_rejected: + Arrange: H = diag(1, −1) + Assert: status NotPositiveDefinite +redundant_constraints_do_not_break_the_solve: + Arrange: x₁ + x₂ ≤ 1 stated twice, x_ref = (1, 1) + Assert: x ≈ (0.5, 0.5), Optimal; the two multipliers sum to 0.5 +``` + +## Reference vectors + +- `min ½‖x − (1,1)‖²`, `x₁ + x₂ ≤ 1` ⇒ `x = (0.5, 0.5)`, multiplier `0.5` (KKT: `x − x_ref = −μ·(1, 1)`). +- `min ½‖x‖²`, `x₂ ≥ 1`, `0.1x₁ + 0.5x₂ ≥ 0.9` ⇒ projection onto the second row, + `x = 0.9·(0.1, 0.5)/0.26 = (0.346154, 1.730769)`, multiplier `0.9/0.26 = 3.461538`; `x₂ ≥ 1` holds there + (inactive). Projecting onto `x₂ = 1` first gives `(0, 1)`, which violates the second row (`0.5 < 0.9`). +- `min ½‖x − (2,−1)‖²`, `0 ≤ x ≤ 1` ⇒ `(1, 0)`, multipliers `1` and `1`. + +## Edge cases + +- `Equalities = Variables` (fully determined) ⇒ `x = E⁻¹e` regardless of `g`. +- Iteration cap reached on a pathological cycle ⇒ `IterationLimit`, finite `x`. +- Large constraint-row scale differences ⇒ tolerance is relative, still `Optimal`. diff --git a/roadmap/solvers/DampedPseudoInverse/explanation.md b/roadmap/solvers/DampedPseudoInverse/explanation.md new file mode 100644 index 00000000..e31108d8 --- /dev/null +++ b/roadmap/solvers/DampedPseudoInverse/explanation.md @@ -0,0 +1,34 @@ +# Damped Pseudo-Inverse and Null-Space Projector — Overview + +## What it is +Two tools from one singular value decomposition: a "best effort" inverse of any matrix that stays +well-behaved when the matrix is nearly singular, and a projector that removes from any vector every +component the matrix can see. + +## Why it matters (embedded) +Mapping desired outputs to inputs through a non-square or ill-conditioned matrix is everywhere: joint +rates from a tool velocity, motor commands from a desired thrust and torque, calibration corrections from +residuals. The plain pseudo-inverse explodes near rank loss and commands huge, saturating inputs; damping +caps that at a small, controlled accuracy cost. When there are more inputs than outputs, the projector +exposes the spare freedom so a second goal (stay away from limits, share load, save energy) can use it +without disturbing the first. + +## How it works (intuition) +The SVD splits the matrix into independent directions, each with a gain. Inverting means dividing by each +gain; damping replaces `1/σ` by `σ/(σ² + λ²)`, which matches `1/σ` for large gains and fades to zero for +tiny ones instead of blowing up. The input directions with (numerically) zero gain form the null space; +subtracting a vector's components along the other input directions leaves only what the matrix cannot +feel. That projector must use the undamped, thresholded split — a damped one leaks into the primary task. + +## Key parameters +- **Damping λ** — robustness near singularity versus accuracy (`error ≈ λ²/σ_min²`). +- **Rank tolerance** — relative threshold below which a direction counts as lost. + +## Reference +C. W. Wampler, "Manipulator inverse kinematic solutions based on vector formulations and damped +least-squares methods," *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977; +G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., §6.1. + +## See also +`SingularValueDecomposition` (the factorization), `LevenbergMarquardt` (#64, uses the same damping per +iteration); robotics-toolbox-cpp's redundancy resolution, a thin task-space wrapper over this. diff --git a/roadmap/solvers/DampedPseudoInverse/implementation.md b/roadmap/solvers/DampedPseudoInverse/implementation.md new file mode 100644 index 00000000..5841c7aa --- /dev/null +++ b/roadmap/solvers/DampedPseudoInverse/implementation.md @@ -0,0 +1,86 @@ +# Damped Pseudo-Inverse and Null-Space Projector — Implementation Pseudocode + +> Roadmap ref: #62 (Tier 2) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) + +Singularity-robust solution of `A·x = b` for any `Rows × Cols` matrix, plus the exact projector onto the +null space of `A`, both from one SVD. The damped (Tikhonov) inverse `A⁺_λ` keeps `x` bounded when `A` +loses rank; the projector `P = I − A⁺A` lets a secondary objective move `x` without changing `A·x`. +Consumers: redundant-manipulator resolution and damped IK (robotics-toolbox-cpp), control allocation +for over-actuated drones and vessels, regularized calibration. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class DampedPseudoInverse: + SingularValueDecomposition svd # tall or wide; k = min(Rows, Cols) + T damping # λ ≥ 0 — damps the solve only + T rankTolerance # relative: σᵢ > rankTolerance·σ₀ counts toward rank + std::size_t rank # r, set by Factor +``` + +## Interface + +```cpp +DampedPseudoInverse(T damping, T rankTolerance = 1e-4) +bool Factor(const math::Matrix& a) # one SVD; false if it does not converge +math::Vector Solve(const math::Vector& b) const # A⁺_λ·b; hot path +math::Vector Project(const math::Vector& x) const # (I − A⁺A)·x, undamped; hot path +math::Matrix Inverse() const # A⁺_λ +math::Matrix NullSpaceProjector() const # I − A⁺A +std::size_t Rank() const +``` + +## Algorithm (pseudocode) + +```text +function Factor(a): + if !svd.Decompose(a): return false # A = U·Σ·Vᵀ; uᵢ = column i of U (Rows), vᵢ = column i of V (Cols) + τ = rankTolerance·σ₀; rank = #{ σᵢ > τ } + return true + +function Gain(σ): # damped inverse of one singular value + λ > 0 ? σ / (σ² + λ²) : (σ > τ ? 1/σ : 0) + +function Solve(b): # OPTIMIZE_FOR_SPEED — O(Rows·k + Cols·k) + return Σᵢ Gain(σᵢ)·(uᵢ·b)·vᵢ # lies in the row space of A ⇒ minimum norm + +function Project(x): # OPTIMIZE_FOR_SPEED — without forming P, O(Cols·r) + return x − Σ_{i)`. + +## Deployment + +- Header: `numerical/solvers/DampedPseudoInverse.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Solve`/`Project`, and `extern template class DampedPseudoInverse;` / `` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/solvers/DampedPseudoInverse.cpp` → the same instantiations. +- Test: `numerical/solvers/test/TestDampedPseudoInverse.cpp` +- Doc: `doc/solvers/DampedPseudoInverse.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 ...)`; `TestDampedPseudoInverse.cpp` → `numerical.solvers_test`. +- Depends on: `SingularValueDecomposition` with wide-matrix support. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/DampedPseudoInverse/tests.md b/roadmap/solvers/DampedPseudoInverse/tests.md new file mode 100644 index 00000000..15875030 --- /dev/null +++ b/roadmap/solvers/DampedPseudoInverse/tests.md @@ -0,0 +1,48 @@ +# Damped Pseudo-Inverse and Null-Space Projector — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestDampedPseudoInverse : public ::testing::Test: + math::Matrix a{ { 1, 0, 1 }, { 0, 1, 1 } } # σ = (√3, 1), null space ∥ (1, 1, −1) + math::Vector b{ 1, 2 } + DampedPseudoInverse exact{ 0.0f } + DampedPseudoInverse damped{ 0.1f } + void SetUp(): ASSERT_TRUE(exact.Factor(a)) and ASSERT_TRUE(damped.Factor(a)) +# each case below is a TEST_F(TestDampedPseudoInverse, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +undamped_solve_is_minimum_norm_exact_solution: + Assert: exact.Solve(b) ≈ (0, 1, 1) and a·exact.Solve(b) ≈ b +damped_solve_error_is_bounded: + Assert: damped.Solve(b) ≈ (0.003289, 0.993388, 0.996678); + ‖a·damped.Solve(b) − b‖ ≈ 0.009934 ≤ λ²/σ_min²·‖b‖ = 0.022361 +projection_is_invisible_to_the_matrix_despite_damping: + Assert: damped.Project((1, 0, 0)) ≈ (1/3, 1/3, −1/3) and a·damped.Project((1, 0, 0)) ≈ 0 +projector_is_an_orthogonal_projector: + Arrange: P = damped.NullSpaceProjector() + Assert: P·P ≈ P, Pᵀ ≈ P, trace P ≈ 1 (= Cols − rank) +undamped_inverse_is_a_right_inverse: + Assert: a·exact.Inverse() ≈ I₂ +solve_has_no_null_space_component: + Assert: damped.Project(damped.Solve(b)) ≈ 0 +rank_loss_grows_the_null_space: + Arrange: rank-1 matrix { {1, 1, 0}, {2, 2, 0} } (σ = (√10, 0)) + Assert: Rank() == 1; trace NullSpaceProjector() ≈ 2; exact.Solve((1, 2)) ≈ (0.5, 0.5, 0) +``` + +## Reference vectors + +- `a = [[1,0,1],[0,1,1]]`: `σ = (1.732051, 1)`, `A⁺b = (0, 1, 1)`, `P = (1/3)·[[1,1,−1],[1,1,−1],[−1,−1,1]]`. +- `λ = 0.1`: `A⁺_λb = (0.003289, 0.993388, 0.996678)`, residual `0.009934` (python3/numpy). + +## Edge cases + +- Tall matrix (`Rows > Cols`, full column rank) ⇒ `NullSpaceProjector() ≈ 0`, `Solve` is least squares. +- `λ = 0` with an exactly singular direction ⇒ zero gain on it (no division by zero). +- Zero matrix ⇒ `Rank() == 0`, `Solve` returns zero, `Project` is the identity. diff --git a/roadmap/solvers/SymplecticIntegrators/explanation.md b/roadmap/solvers/SymplecticIntegrators/explanation.md new file mode 100644 index 00000000..1e7054ee --- /dev/null +++ b/roadmap/solvers/SymplecticIntegrators/explanation.md @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 00000000..0db1c3c1 --- /dev/null +++ b/roadmap/solvers/SymplecticIntegrators/implementation.md @@ -0,0 +1,95 @@ +# 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 new file mode 100644 index 00000000..93b852e6 --- /dev/null +++ b/roadmap/solvers/SymplecticIntegrators/tests.md @@ -0,0 +1,45 @@ +# 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.