chore: add controllers roadmap

.github/dependabot.yml

@@ -13,6 +13,8 @@ updates:
     directory: /
     schedule:
       interval: weekly
+    commit-message:
+      prefix: build
     groups:
       patch-minor-action-updates:
         update-types:
@@ -22,9 +24,13 @@ updates:
     directory: .devcontainer
     schedule:
       interval: daily
+    commit-message:
+      prefix: build
   - package-ecosystem: gitsubmodule
     directory: /
     schedule:
       interval: weekly
+    commit-message:
+      prefix: build
     registries:
       - github-private

roadmap/controllers/manipulator/CableTensionDistribution/explanation.md

@@ -0,0 +1,34 @@
+# Cable Tension Distribution — Overview
+
+## What it is
+The force-allocation step for a cable-driven robot: given a desired wrench (force + torque) on the
+moving platform, compute a set of **non-negative** cable tensions that produce exactly that wrench.
+Because cables can only *pull*, and there are usually more cables than task dimensions, this is a
+constrained optimisation, not a plain linear solve.
+
+## Why it matters (embedded)
+Cable robots — warehouse cranes, camera rigs (SkyCam), tendon-driven hands, large 3D printers —
+actuate through tension only. Every control cycle must hand the winches a feasible, bounded tension
+vector; a negative or over-limit request is physically impossible and can slacken a cable or snap
+it. The allocator runs in the real-time loop on the same controller that computes the wrench.
+
+## How it works (intuition)
+The **structure matrix** `A` maps cable tensions to platform wrench (`A·t = w`). With more cables
+than task DOF, infinitely many tension sets produce the same wrench — the extra freedom is *internal
+pretension*. A small quadratic program picks the tension vector closest to the mid-range value while
+satisfying `A·t = w` and staying within `[tMin, tMax]`. Centring the tensions keeps every cable
+comfortably taut, maximising the margin against both going slack and overloading.
+
+## Key parameters
+- **tMin** — minimum tension (> 0) so cables never go slack.
+- **tMax** — maximum tension set by winch/cable strength.
+- **structure matrix A = −Jᵀ** — geometry of cable directions and attachment points.
+- **objective centre (tMid)** — value the QP biases toward for maximum disturbance margin.
+
+## Reference
+T. Bruckmann, A. Pott (eds.), *Cable-Driven Parallel Robots*, Springer, 2013 (Pott
+tension-distribution methods).
+
+## See also
+`SpatialJacobian` (#M8, supplies `A = −Jᵀ`), `Mpc` (the reused bounded-QP solver),
+`OperationalSpaceControl` (#M18, wrench-to-torque mapping for rigid arms).

roadmap/controllers/manipulator/CableTensionDistribution/implementation.md

@@ -0,0 +1,79 @@
+# Cable Tension Distribution — Implementation Pseudocode
+
+> Roadmap ref: #M25 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only)
+
+## Data structures
+
+```
+template<typename T, std::size_t NumCables, std::size_t WrenchDim>   # static_assert(std::is_floating_point_v<T>); instantiated for float
+class CableTensionDistribution:                                 # WrenchDim = 6 (or 3 planar)
+    const kinematics::SpatialJacobian<T, NumCables>& structure  # J → A = −Jᵀ (WrenchDim×NumCables), injected (#M8)
+    controllers::Mpc<T, NumCables, WrenchDim> qpSolver          # reused bounded-QP machinery
+    T tMin        # minimum cable tension (> 0: cables never go slack)
+    T tMax        # maximum cable tension (actuator / cable limit)
+```
+
+## Interface
+
+```
+CableTensionDistribution(const SpatialJacobian& structure, T tMin, T tMax)
+
+# returns nullopt when the pose is outside the wrench-feasible workspace:
+std::optional<Vector<T,NumCables>>
+    Distribute(const WrenchVector& wDesired, const StateVector& pose)   # hot path
+```
+
+## Algorithm (pseudocode)
+
+```
+function Distribute(wDesired, pose):        # OPTIMIZE_FOR_SPEED
+    A = -transpose(structure.Compute(pose))       # WrenchDim×NumCables structure matrix
+    # pull-only, bounded tensions that realise the wrench, kept away from the limits:
+    #   minimise ½‖t − tMid‖²                       (closest-to-centre ⇒ max disturbance margin)
+    #   s.t.     A·t = wDesired                      (exact wrench, equality)
+    #            tMin ≤ t ≤ tMax                      (cables pull only, t > 0)
+    tMid   = 0.5*(tMin + tMax) * Ones(NumCables)
+    result = qpSolver.Solve(H = Identity(NumCables), gradient = -tMid,
+                            equality = { A, wDesired },
+                            lower = tMin, upper = tMax)
+    if not result.feasible:
+        return nullopt                            # pose outside wrench-feasible workspace
+    return result.tension
+```
+
+## Complexity & memory
+
+- Time: bounded active-set / interior-point QP with `NumCables` variables and `WrenchDim` equalities:
+  `O(NumCables³)` worst case; small (`NumCables ≤ 8`).
+- Memory: `O(NumCables²)` working matrices; all bounded/static, no heap.
+
+## Numerical / embedded notes
+
+- **Cables pull only** (`t ≥ tMin > 0`) — the defining constraint. A rigid-robot statics solve can
+  return compression, which is physically impossible here, so the bounded QP is mandatory.
+- Redundancy (`NumCables > WrenchDim`) leaves a null space; **centring** tensions at `tMid` keeps
+  them away from slack (`tMin`) and snap (`tMax`), maximising the wrench-disturbance margin (Pott).
+- **Infeasible ⇒ `nullopt`, never an exception** — the caller treats it as a workspace-boundary
+  event; matches the no-exception embedded convention.
+- Keep `tMin > 0` so cables stay taut (avoids backlash / control loss); size `tMax` to the winch.
+- A 2-norm (or Δt-regularised) objective gives **continuous** tensions between cycles — no chatter
+  when the desired wrench moves smoothly.
+- Structure matrix `A = −Jᵀ` reuses the spatial Jacobian (#M8); the QP reuses the `Mpc` solver.
+- Float-only: `static_assert(std::is_floating_point_v<T>)`; the generic `T` signature keeps a
+  `Q15`/`Q31` specialisation cheap to add later.
+
+## Deployment
+
+- Header: `robotics/controllers/manipulator/CableTensionDistribution.hpp` — `#pragma once` →
+  `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Distribute`, and
+  `extern template class CableTensionDistribution<float, NumCables, WrenchDim>;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`.
+- Coverage: `robotics/controllers/manipulator/CableTensionDistribution.cpp` →
+  `template class CableTensionDistribution<float, NumCables, WrenchDim>;`
+- Test: `robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp`
+- Doc: `doc/controllers/manipulator/CableTensionDistribution.md` (expand to follow `doc/TEMPLATE.md`)
+- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`;
+  `TestCableTensionDistribution.cpp` → the `_test` target.
+- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`,
+  add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a
+  `doc/controllers/manipulator/` folder.
+- Generic pattern: see `roadmap/DEPLOYMENT.md`.

roadmap/controllers/manipulator/CableTensionDistribution/tests.md

@@ -0,0 +1,65 @@
+# Cable Tension Distribution — Unit Test Plan (Pseudocode)
+
+> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap.
+
+## Fixture
+
+```
+class TestCableTensionDistribution : public ::testing::Test:
+    # structure Jacobian injected & mocked; planar WrenchDim = 2, NumCables = 3:
+    StrictMock<kinematics::MockSpatialJacobian<float,3>> structure
+    float tMin = 10
+    float tMax = 200
+    CableTensionDistribution<float,3,2> distributor{ structure, tMin, tMax }
+    # concrete Mpc QP solver used internally (not mocked)
+# each case below is a TEST_F(TestCableTensionDistribution, <name>)
+```
+
+## Test cases (Arrange / Act / Assert)
+
+```
+square_case_unique_solution:
+    Arrange: NumCables == WrenchDim, invertible A (mock structure)
+    Act:     t = Distribute(wDesired, pose)
+    Assert:  t == A⁻¹ · wDesired
+
+redundant_case_centres_tensions:
+    Arrange: A = 2×3, one-DOF redundancy
+    Assert:  t is the min-norm-to-tMid solution satisfying A·t = wDesired
+
+all_tensions_nonnegative:
+    Arrange: any feasible wrench
+    Assert:  t[i] >= tMin > 0 for all i
+
+respects_upper_bound:
+    Arrange: large wrench pushing one cable toward the limit
+    Assert:  t[i] <= tMax for all i
+
+wrench_is_reproduced:
+    Arrange: feasible wrench
+    Assert:  A · t ≈ wDesired  (equality constraint satisfied)
+
+infeasible_pose_returns_nullopt:
+    Arrange: wrench outside the feasible cone (no pull-only solution)
+    Assert:  Distribute(...) == nullopt
+
+zero_wrench_uses_internal_pretension:
+    Arrange: wDesired = 0
+    Assert:  t in null(A), all >= tMin (taut, no external load)
+
+structure_matrix_queried_once:
+    Arrange: any pose
+    Assert:  structure.Compute called exactly once (StrictMock)
+```
+
+## Reference vectors
+
+- Planar 3-cable point mass, symmetric angles: hand-computed `A` (2×3); a vertical wrench ⇒ symmetric
+  tensions, exactly reproducible.
+- Square `A` (NumCables = WrenchDim): unique `t = A⁻¹w` — golden check.
+
+## Edge cases
+
+- Wrench on the workspace boundary: one tension saturates at `tMax`, still feasible.
+- Beyond the boundary: `nullopt`.
+- Near-parallel cables (`A` ill-conditioned): QP regularisation / `tMid` centring keeps `t` finite.

roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md

@@ -0,0 +1,30 @@
+# Computed-Torque Control — Overview
+
+## What it is
+The workhorse model-based *tracking* law for manipulators. An inverse-dynamics feedforward plus a
+PD correction linearises and decouples the arm into independent unit double integrators:
+`τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q,q̇)q̇ + g(q)`.
+
+## Why it matters (embedded)
+A fixed PID tuned at one posture misbehaves at another because a robot's inertia and gravity change
+with configuration. Computed-torque uses the `M`, `C`, `g` you already evaluate with RNEA to erase
+that variation, so a *single* gain set tracks fast trajectories across the entire workspace — no
+gain scheduling, no lookup tables — at a deterministic `O(n)` cost.
+
+## How it works (intuition)
+Work out the acceleration you actually want: the desired trajectory acceleration plus a PD term that
+corrects position and velocity error. Then ask the dynamics model, "what joint torque produces
+exactly that acceleration *right now*?" Because the answer multiplies by the mass matrix `M(q)`, the
+arm's inertial coupling, Coriolis, and gravity are all cancelled, leaving clean, identical
+second-order error dynamics on every joint.
+
+## Key parameters
+- **model** — injected dynamics supplying `M(q)`, `C(q,q̇)q̇`, `g(q)`.
+- **Kp, Kd** — error gains for the linearised double integrator; pick `Kd = 2√Kp` for critical damping.
+
+## Reference
+M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980).
+
+## See also
+`PdGravityCompensation` (set-point-only special case); `FeedbackLinearization` (same idea for general
+plants); `SlotineLiAdaptiveControl` (adapts unknown parameters); `dynamics/RecursiveNewtonEuler` (term source).

roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md

@@ -0,0 +1,76 @@
+# Computed-Torque Control — Implementation Pseudocode
+
+> Roadmap ref: #M12 (Tier 3) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only)
+
+## Data structures
+
+```
+template<typename T, std::size_t Dof>    # static_assert(std::is_floating_point_v<T>); instantiated for float
+class ComputedTorqueControl:
+    const dynamics::EulerLagrangeDynamics<T, Dof>& model   # M(q), C(q,q̇)q̇, g(q)
+    math::SquareMatrix<T, Dof> Kp        # position-error gain
+    math::SquareMatrix<T, Dof> Kd        # velocity-error gain
+```
+
+## Interface
+
+```
+# Full dynamics model injected (DIP); gains chosen for the resulting double integrator:
+ComputedTorqueControl(const EulerLagrangeDynamics<T,Dof>& model,
+                      const SquareMatrix& Kp, const SquareMatrix& Kd)
+
+Vector<T,Dof> ComputeTorque(const StateVector& q,      const StateVector& qDot,
+                            const StateVector& qd,     const StateVector& qdDot,
+                            const StateVector& qdDdot)          # hot path
+```
+
+## Algorithm (pseudocode)
+
+```
+function ComputeTorque(q, qDot, qd, qdDot, qdDdot):     # OPTIMIZE_FOR_SPEED
+    e    = qd    - q
+    eDot = qdDot - qDot
+    # inner-loop joint-space acceleration command (feedforward + PD):
+    aq   = qdDdot + Kd * eDot + Kp * e
+    # inverse-dynamics torque that realises aq exactly:
+    #   τ = M(q)·aq + C(q,q̇)q̇ + g(q)
+    M    = model.ComputeMassMatrix(q)
+    Cqd  = model.ComputeCoriolisTerms(q, qDot)
+    g    = model.ComputeGravityTerms(q)
+    return M * aq + Cqd + g
+```
+
+## Complexity & memory
+
+- Time: `O(Dof²)` for `M·aq`; model evaluation `O(Dof)`–`O(Dof²)` (RNEA inverse dynamics is `O(Dof)`).
+- Memory: `O(Dof²)` for the two gains; no dynamic state, no heap.
+
+## Numerical / embedded notes
+
+- Substituting `τ` into `M q̈ + Cq̇ + g = τ` gives the **decoupled** linear error dynamics
+  `ë + Kd·ė + Kp·e = 0` — every joint becomes an independent, tunable second-order system.
+- The law **multiplies** by `M(q)` (SPD) — it never inverts it, so the hot path stays
+  well-conditioned (unlike forward dynamics).
+- RNEA supplies `M`, `C·q̇`, `g` without forming `C` explicitly; the injected model wraps that
+  detail (DIP), so the controller is agnostic to how the terms are produced.
+- Model error leaves a residual (`ë + Kd·ė + Kp·e = M⁻¹Δ`); pair with an integral, robust
+  (sliding-mode), or adaptive (`SlotineLiAdaptiveControl`) term to reject it.
+- Choose `Kd = 2√Kp` per channel for a critically-damped response.
+- Float-only: `static_assert(std::is_floating_point_v<T>)`; the generic `T` signature keeps a
+  `Q15`/`Q31` specialisation cheap to add later.
+
+## Deployment
+
+- Header: `robotics/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once` →
+  `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and
+  `extern template class ComputedTorqueControl<float, Dof>;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`.
+- Coverage: `robotics/controllers/manipulator/ComputedTorqueControl.cpp` →
+  `template class ComputedTorqueControl<float, Dof>;`
+- Test: `robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp`
+- Doc: `doc/controllers/manipulator/ComputedTorqueControl.md` (expand to follow `doc/TEMPLATE.md`)
+- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`;
+  `TestComputedTorqueControl.cpp` → the `_test` target.
+- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`,
+  add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a
+  `doc/controllers/manipulator/` folder.
+- Generic pattern: see `roadmap/DEPLOYMENT.md`.

roadmap/controllers/manipulator/ComputedTorqueControl/tests.md

@@ -0,0 +1,68 @@
+# Computed-Torque Control — Unit Test Plan (Pseudocode)
+
+> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap.
+
+## Fixture
+
+```
+class TestComputedTorqueControl : public ::testing::Test:
+    StrictMock<dynamics::MockEulerLagrangeDynamics<float,2>> model
+    SquareMatrix<float,2> Kp = diag(100, 100)
+    SquareMatrix<float,2> Kd = diag( 20,  20)
+    ComputedTorqueControl<float,2> controller{ model, Kp, Kd }
+# each case below is a TEST_F(TestComputedTorqueControl, <name>)
+```
+
+## Test cases (Arrange / Act / Assert)
+
+```
+pure_feedforward_tracks_acceleration:
+    Arrange: model M=I, C=0, g=0; all errors 0, qdDdot = a
+    Act:     τ = ComputeTorque(...)
+    Assert:  τ == a           (M·aq reduces to the commanded acceleration)
+
+gravity_compensation_at_rest:
+    Arrange: model g(q) = [0, mgL]; all setpoints match state, aq = 0
+    Assert:  τ == g
+
+coriolis_terms_added:
+    Arrange: model C(q,q̇)q̇ = c; aq = 0
+    Assert:  τ == c
+
+mass_matrix_shapes_command:
+    Arrange: M = diag(2,3), aq = [1,1] (via qdDdot, errors 0)
+    Assert:  τ == [2,3]
+
+position_error_maps_through_mass:
+    Arrange: e = qd - q != 0, others 0, M = I
+    Assert:  τ == Kp · e
+
+velocity_error_maps_through_mass:
+    Arrange: eDot != 0, e = 0, M = I
+    Assert:  τ == Kd · eDot
+
+full_law_superposition:
+    Arrange: nonzero M, C, g, and errors
+    Assert:  τ == M·(qdDdot + Kd·ė + Kp·e) + Cq̇ + g
+
+decoupled_error_dynamics:
+    Arrange: wrap plant q̈ = M⁻¹(τ − Cq̇ − g); run K steps
+    Assert:  ||qd − q|| -> 0 matching ë + Kd·ė + Kp·e = 0
+
+all_three_model_terms_queried:
+    Arrange: any state
+    Assert:  ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms each called once
+             (StrictMock)
+```
+
+## Reference vectors
+
+- With the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — a linear ODE whose decay
+  rate is hand-computable from `Kp`, `Kd`.
+- 2-link at rest with all setpoints matched: golden `τ = g(q)`.
+
+## Edge cases
+
+- Near-singular `M` (mock): still multiplied, never inverted ⇒ no torque blow-up.
+- Model mismatch (mock `M` scaled 1.2): bounded tracking error, closed loop stays stable.
+- Large `qdDdot`: torque stays within the documented actuator model.