diff --git a/ROADMAP.md b/ROADMAP.md index 1f34e7ca..7e3e1aa5 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -228,16 +228,16 @@ the library does not yet expose. Detailed below under - **Reuses:** [KalmanFilterBase.hpp](numerical/filters/active/KalmanFilterBase.hpp), [Cholesky](numerical/solvers/CholeskyDecomposition.hpp), item 27. ### 40. Feedback linearization *(float-first)* -- **What:** Cancel known nonlinear dynamics via a coordinate transform + inner control law so an outer linear controller can be applied. -- **Embedded value:** Exact control of robot manipulators and other structurally-known nonlinear plants. +- **What:** Cancel a control-affine system's known nonlinear dynamics via a coordinate transform + inner control law, leaving an equivalent linear system that an outer loop (PD/LQR) can drive. +- **Embedded value:** One linear gain set works across the whole operating envelope of any structurally-known nonlinear plant (robot arms, quadrotors, electromechanical drives) — no gain scheduling, no lookup tables. - **Algorithm / paper:** A. Isidori, *Nonlinear Control Systems* (1995); Slotine & Li, *Applied Nonlinear Control*. -- **Reuses:** `dynamics/` models, `math::Matrix`, new `nonlinear_control/` module. +- **Reuses:** an injected control-affine plant model, `math::Matrix`, new `nonlinear_control/` module. (The manipulator computed-torque instance lives in robotics-toolbox-cpp.) ### 41. Backstepping controller *(float-first)* - **What:** Recursive Lyapunov-based design for strict-feedback systems, stabilizing one integrator stage at a time. - **Embedded value:** Systematic, provably-stable control for cascaded nonlinear plants (electromechanical, flight). - **Algorithm / paper:** M. Krstić, I. Kanellakopoulos, P. Kokotović, *Nonlinear and Adaptive Control Design* (1995). -- **Reuses:** `dynamics/`, `math::Matrix`, new `nonlinear_control/` module. +- **Reuses:** `math::Matrix`, new `nonlinear_control/` module. --- diff --git a/TESTING.md b/TESTING.md index e39d152b..450fa287 100644 --- a/TESTING.md +++ b/TESTING.md @@ -61,7 +61,6 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. | Optimization | ● | | | | ● | ● | | | | | Regularization | ● | | | | | ● | ● | | | | Solvers (linear / ODE / roots) | ● | | ○ | ● | ● | ● | ● | | ● | -| Dynamics & kinematics | ● | | ● | | ● | ● | ● | | | | Neural network | ● | | | | ○ | ● | ● | | | | Math foundation | ● | | | | | ● | ● | | ● | @@ -194,18 +193,7 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. within the requested tolerance. - **M7 conservation** — energy drift bounded for a conservative system over many steps. -### 10. Dynamics & kinematics — `dynamics/`, `kinematics/` -`ForwardKinematics`, `InverseKinematics`, `NewtonEulerSolver`, `RecursiveNewtonEuler`, -`EulerLagrangeSolver`, `ArticulatedBodyAlgorithm`. - -- **M1 forward kinematics** — end-effector pose matches known geometry for canonical joint angles. -- **M5 inverse kinematics round-trip** — `FK(IK(pose)) ≈ pose`; converges within iteration budget; - handles reachable vs unreachable targets (M6). -- **M1 cross-method consistency** — `RecursiveNewtonEuler` and `EulerLagrange` produce the same joint - torques for the same state; both match the analytic torque of a simple pendulum / 2-link arm. -- **M7 energy** — conservation in free (unforced) motion; passivity of the mass matrix (SPD). - -### 11. Neural network — `neural_network/` +### 10. Neural network — `neural_network/` `activation/*`, `layer/Dense`, `losses/*`, `model/Model`. - **M1 activation values** — reference points: `sigmoid(0)=0.5`, `tanh(0)=0`, `relu(−x)=0`, @@ -216,7 +204,7 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. - **M1 dense layer** — `output = W·x + b`; back-prop gradient check. - **Model (M6)** — forward pass is deterministic and equals the manual layer composition. -### 12. Math foundation — `math/` +### 11. Math foundation — `math/` `Matrix`, `ComplexNumber`, `Quaternion`, `Cordic`, `TrigonometricFunctions`, `HyperbolicFunctions`, `AdvancedFunctions`, `Statistics`, `LinearTimeInvariant`, `Toeplitz`, `QNumber`. diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index a9167ceb..01bfb5ef 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -22,7 +22,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 (`trajectory`, `robust_control`, `nonlinear_control`, `controllers/manipulator`): + - New module (`robust_control`, `nonlinear_control`): 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 dc847c42..77dad4a5 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -112,15 +112,3 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `solvers` `RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) - -### `trajectory` -`PolynomialTrajectory` (M2) · `TrapezoidalProfile` (M3) · `SCurveProfile` (M9) · `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) - -### `controllers/manipulator` -`PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) - -### `kinematics` -`DenavitHartenberg` (M7) · `SpatialJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkPieper` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) - -### `dynamics` -`GenericJointLink` (M1) · `FrictionCompensation` (M4) diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md deleted file mode 100644 index cd1bffe5..00000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# 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). diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md deleted file mode 100644 index a36c07cf..00000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ /dev/null @@ -1,79 +0,0 @@ -# Cable Tension Distribution — Implementation Pseudocode - -> Roadmap ref: #M25 (Tier 4) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class CableTensionDistribution: # WrenchDim = 6 (or 3 planar) - const kinematics::SpatialJacobian& structure # J → A = −Jᵀ (WrenchDim×NumCables), injected (#M8) - controllers::Mpc 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> - 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)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/controllers/manipulator/CableTensionDistribution.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Distribute`, and - `extern template class CableTensionDistribution;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/CableTensionDistribution.cpp` → - `template class CableTensionDistribution;` -- Test: `numerical/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 `numerical/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`. diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md deleted file mode 100644 index 206ad744..00000000 --- a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md +++ /dev/null @@ -1,65 +0,0 @@ -# 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> structure - float tMin = 10 - float tMax = 200 - CableTensionDistribution distributor{ structure, tMin, tMax } - # concrete Mpc QP solver used internally (not mocked) -# each case below is a TEST_F(TestCableTensionDistribution, ) -``` - -## 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. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md deleted file mode 100644 index 16cd87fe..00000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# 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). diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md deleted file mode 100644 index b522380a..00000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md +++ /dev/null @@ -1,76 +0,0 @@ -# Computed-Torque Control — Implementation Pseudocode - -> Roadmap ref: #M12 (Tier 3) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class ComputedTorqueControl: - const dynamics::EulerLagrangeDynamics& model # M(q), C(q,q̇)q̇, g(q) - math::SquareMatrix Kp # position-error gain - math::SquareMatrix Kd # velocity-error gain -``` - -## Interface - -``` -# Full dynamics model injected (DIP); gains chosen for the resulting double integrator: -ComputedTorqueControl(const EulerLagrangeDynamics& model, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector 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)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class ComputedTorqueControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/ComputedTorqueControl.cpp` → - `template class ComputedTorqueControl;` -- Test: `numerical/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 `numerical/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`. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md b/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md deleted file mode 100644 index af77784c..00000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md +++ /dev/null @@ -1,68 +0,0 @@ -# Computed-Torque Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestComputedTorqueControl : public ::testing::Test: - StrictMock> model - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - ComputedTorqueControl controller{ model, Kp, Kd } -# each case below is a TEST_F(TestComputedTorqueControl, ) -``` - -## 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. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md deleted file mode 100644 index 9f617c58..00000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Hybrid Position/Force Control — Overview - -## What it is -A task-space controller that splits the end-effector's directions into two disjoint groups: some -axes are **position-controlled**, the rest are **force-controlled**. A diagonal selection matrix -`S` decides which is which, and its complement `I−S` handles the others. - -## Why it matters (embedded) -Many contact tasks are naturally hybrid: sliding a tool on a surface, you want to *track a path* -tangentially while *regulating the normal force* — you cannot command both position and force on the -same axis, because the environment already fixes one of them. Real-time force regulation with clean -axis partitioning is essential for deburring, polishing, assembly, and grinding on resource-limited -controllers. - -## How it works (intuition) -Along motion axes a PD law pulls the tool toward the reference path. Along force axes a PI law drives -the measured contact force to the desired force. The two commands live in orthogonal subspaces, are -summed into a single Cartesian wrench, then mapped to joint torques by `Jᵀ`. Because `S` and `I−S` -never overlap, the loops do not fight each other. - -## Key parameters -- **S (selection matrix)** — which task axes are motion (1) vs force (0), set in the constraint frame. -- **Kp, Kd** — motion-subspace position/velocity gains. -- **Kf, Ki** — force-subspace proportional/integral gains (integral removes steady force error). -- **dt** — sample period for the force integral (needs anti-windup). - -## Reference -M. Raibert, J. Craig, "Hybrid Position/Force Control of Manipulators," *ASME J. Dyn. Sys. Meas. -Control*, 1981. - -## See also -`ImpedanceControl` (#M17, compliant unified motion/force), `OperationalSpaceControl` (#M18, -task-space dynamics), `SpatialJacobian` (#M8, the `Jᵀ` wrench map). diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md deleted file mode 100644 index 911853de..00000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ /dev/null @@ -1,84 +0,0 @@ -# Hybrid Position/Force Control — Implementation Pseudocode - -> Roadmap ref: #M19 (Tier 4) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class HybridPositionForceControl: - const dynamics::EulerLagrangeDynamics& model # g(q), C(q,q̇)q̇ - const kinematics::SpatialJacobian& jacobian # 6×Dof, injected (#M8) - math::SquareMatrix S # selection: 1 = motion axis, 0 = force axis - math::SquareMatrix Kp, Kd # motion-subspace PD gains - math::SquareMatrix Kf, Ki # force-subspace P / I gains - math::Vector forceIntegral # accumulated force error (STATE) - T dt -``` - -## Interface - -``` -HybridPositionForceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& S, const SquareMatrix& Kp, const SquareMatrix& Kd, - const SquareMatrix& Kf, const SquareMatrix& Ki, T dt) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& fMeasured, - const TaskVector& fd) # hot path -void Reset() # clears force integral -``` - -## Algorithm (pseudocode) - -``` -function ComputeTorque(q, qDot, x, xd, xdDot, fMeasured, fd): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) - xDot = J * qDot - # --- motion subspace (S selects position-controlled axes) --- - Fmotion = Kp*(xd - x) + Kd*(xdDot - xDot) - # --- force subspace (I−S selects force-controlled axes), PI on force error --- - eF = fd - fMeasured - forceIntegral = forceIntegral + eF * dt - Fforce = fd + Kf*eF + Ki*forceIntegral - # --- complementary partition: an axis is motion- XOR force-controlled --- - F = S*Fmotion + (Identity(TaskDim) - S)*Fforce - # map task wrench to joint torque; cancel arm gravity + Coriolis: - return transpose(J)*F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) -``` - -## Complexity & memory - -- Time: `O(TaskDim·Dof)` for `J·qDot` and `Jᵀ·F`; model terms `O(Dof)`–`O(Dof²)`. -- Memory: `O(TaskDim²)` gains + `O(TaskDim)` integral state; no heap. - -## Numerical / embedded notes - -- `S` is a **complementary orthogonal projector**: `S² = S` and `S·(I−S) = 0`, so the motion and - force loops act on disjoint task axes and never fight (Raibert–Craig). -- Axes are expressed in a **constraint frame** aligned with the contact surface — e.g. peg-in-hole: - normal = force-controlled, insertion/tangential = motion-controlled. -- The force loop is **PI** (not PD): integral action drives steady-state force error to zero on a - stiff environment. Add anti-windup / `Reset()` on contact loss to stop integral run-off. -- `Jᵀ` mapping only (no inverse) ⇒ passes through kinematic singularities safely, like impedance. -- Reuses the `Jᵀ` wrench map of `ImpedanceControl` (#M17) and the spatial Jacobian (#M8); the added - force loop is what makes it *hybrid*. -- 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/manipulator/HybridPositionForceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class HybridPositionForceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/HybridPositionForceControl.cpp` → - `template class HybridPositionForceControl;` -- Test: `numerical/controllers/manipulator/test/TestHybridPositionForceControl.cpp` -- Doc: `doc/controllers/manipulator/HybridPositionForceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestHybridPositionForceControl.cpp` → the `_test` target. -- New module: create `numerical/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`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md deleted file mode 100644 index e85ee945..00000000 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md +++ /dev/null @@ -1,72 +0,0 @@ -# Hybrid Position/Force Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestHybridPositionForceControl : public ::testing::Test: - # both dependencies mocked; planar TaskDim = 2, Dof = 2: - StrictMock> model - StrictMock> jacobian - SquareMatrix S = diag(1, 0) # axis 0 = motion, axis 1 = force - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - SquareMatrix Kf = diag( 2, 2) - SquareMatrix Ki = diag( 5, 5) - float dt = 0.001 - HybridPositionForceControl controller{ model, jacobian, S, Kp, Kd, Kf, Ki, dt } -# each case below is a TEST_F(TestHybridPositionForceControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -motion_axis_does_position_pd: - Arrange: J=I, model 0; only axis-0 position error, force error 0 - Assert: F[0] == Kp[0]·eX[0] + Kd[0]·eXDot[0]; axis-1 unaffected - -force_axis_does_force_pi: - Arrange: J=I, model 0; only axis-1 force error, position error 0, one step - Assert: F[1] == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt - -selection_partitions_axes: - Arrange: S = diag(1,0), both motion and force commands nonzero - Assert: F[0] from motion loop, F[1] from force loop (no cross-talk) - -complementary_projectors_orthogonal: - Arrange: verify S·(I−S) == 0 and S·S == S - Assert: each axis contributes exactly one loop - -force_integral_accumulates: - Arrange: constant force error, call ComputeTorque N times - Assert: integral term grows as Ki·eF·(N·dt) - -reset_clears_force_integral: - Arrange: accumulate integral, Reset() - Assert: next force term has no integral history - -jacobian_transpose_maps_wrench: - Arrange: J = [[1,0],[0,2]], known F - Assert: τ == transpose(J)·F (+ model terms) - -gravity_and_coriolis_added: - Arrange: model g, Cq̇ nonzero; F = 0 - Assert: τ == Cq̇ + g - -both_dependencies_queried: - Arrange: any state - Assert: jacobian.Compute once; model gravity + Coriolis each once; - ComputeMassMatrix never called (StrictMock) -``` - -## Reference vectors - -- `S=diag(1,0)`, `J=I`, model 0: `F = [Kp·eX[0], fd[1]+Kf·eF[1]+Ki·∫eF[1]]`, `τ = JᵀF` — hand-checkable. -- Contact equilibrium on the force axis: PI drives `fMeasured → fd` ⇒ steady `eF = 0`. - -## Edge cases - -- Contact loss (`fMeasured → 0`): force integral winds up ⇒ `Reset()` / anti-windup required. -- `S = I` (all motion) reduces to pure Cartesian PD; `S = 0` (all force) to pure force control. -- Near-singular `J`: `Jᵀ` stays finite, torque bounded (no inversion). diff --git a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md b/roadmap/controllers/manipulator/ImpedanceControl/explanation.md deleted file mode 100644 index 4f9c4925..00000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Impedance Control — Overview - -## What it is -Instead of commanding a position, impedance control makes the end-effector *behave* like a chosen -mass–spring–damper. You program the stiffness, damping, and (optionally) inertia the robot presents -to the world — a tunable "softness" rather than a rigid trajectory. - -## Why it matters (embedded) -Rigid position control shatters on contact: the tiniest position error against a hard surface -generates enormous force. Impedance control lets a robot push, insert, wipe, and physically interact -with people *safely*, with predictable and adjustable compliance. It is the foundation of -collaborative robots, assembly, and teleoperation. - -## How it works (intuition) -Measure the Cartesian error between where the tip is and where it should be. Convert that error into -the force a virtual spring–damper would exert, add any commanded inertia and external-force term, -then use the Jacobian *transpose* to turn that tip force into joint torques. Compensating the arm's -own gravity and Coriolis terms ensures the felt impedance is the one you programmed — not the robot's -native dynamics. Because the mapping uses `Jᵀ` (never an inverse), it stays safe near singularities. - -## Key parameters -- **Md (rendered inertia), Dd (damping), Kstiff (stiffness)** — the target impedance per Cartesian axis. -- **model, jacobian** — injected dynamics (`g`, `Cq̇`) and 6×N Jacobian. -- **fExternal** — optional measured contact wrench from a wrist force/torque sensor. - -## Reference -N. Hogan, "Impedance Control: An Approach to Manipulation, Parts I–III," -*ASME J. Dynamic Systems, Measurement, and Control*, 1985. - -## See also -`OperationalSpaceControl` (needed to truly reshape inertia via `Λ`); `HybridPositionForceControl` -(partition force/motion axes); `ComputedTorqueControl` (rigid tracking counterpart). diff --git a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md deleted file mode 100644 index 9e4708f8..00000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md +++ /dev/null @@ -1,82 +0,0 @@ -# Impedance Control — Implementation Pseudocode - -> Roadmap ref: #M17 (Tier 3) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class ImpedanceControl: # TaskDim = 6 (pose) or 3 (position-only) - const dynamics::EulerLagrangeDynamics& model # g(q), C(q,q̇)q̇ - const kinematics::SpatialJacobian& jacobian # 6×Dof, injected (#M8) - math::SquareMatrix Md # desired end-effector inertia - math::SquareMatrix Dd # desired damping - math::SquareMatrix Kstiff # desired stiffness -``` - -## Interface - -``` -# Dynamics model and Jacobian injected (DIP): -ImpedanceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& Md, const SquareMatrix& Dd, const SquareMatrix& Kstiff) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& xdDdot, - const TaskVector& fExternal) # hot path -``` - -## Algorithm (pseudocode) - -``` -function ComputeTorque(q, qDot, x, xd, xdDot, xdDdot, fExt): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) # TaskDim×Dof spatial Jacobian - xDot = J * qDot # measured end-effector twist - eX = xd - x # Cartesian pose error - eXDot = xdDot - xDot # Cartesian velocity error - # desired end-effector wrench rendering Md·ẍ + Dd·ẋ + Kstiff·x = f_ext : - F = Md * xdDdot + Dd * eXDot + Kstiff * eX + fExt - # map task wrench to joint torque (Jᵀ); cancel the arm's own gravity + Coriolis: - return transpose(J) * F - + model.ComputeCoriolisTerms(q, qDot) - + model.ComputeGravityTerms(q) -``` - -## Complexity & memory - -- Time: `O(TaskDim·Dof)` for `J·qDot` and `Jᵀ·F`; model terms `O(Dof)`–`O(Dof²)`. -- Memory: `O(TaskDim²)` for the three impedance matrices; no dynamic state, no heap. - -## Numerical / embedded notes - -- The `Jᵀ` map is always well-defined (no inverse) ⇒ the controller passes through kinematic - singularities safely; only *inertia shaping* (`Md` ≠ natural inertia) needs the task inertia - `Λ = (J M⁻¹ Jᵀ)⁻¹` — see `OperationalSpaceControl`. Leaving `Md` at the natural inertia gives - the cheap, robust "stiffness control" variant. -- **Admittance** is the dual: measure `fExt`, integrate to a motion command, feed a position loop — - better on stiff/non-backdrivable robots; impedance is better on backdrivable ones. -- Passivity: with SPD `Dd` and `Kstiff` the rendered port is passive ⇒ stable contact with any - passive environment. -- Diagonal `Kstiff`/`Dd` are chosen per Cartesian axis (e.g. stiff normal, compliant tangential for - insertion tasks). -- `fExt` comes from a wrist force/torque sensor; low-pass it and match `Dd` to its bandwidth. Set it - to zero for pure motion impedance without contact sensing. -- 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/manipulator/ImpedanceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class ImpedanceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/ImpedanceControl.cpp` → - `template class ImpedanceControl;` -- Test: `numerical/controllers/manipulator/test/TestImpedanceControl.cpp` -- Doc: `doc/controllers/manipulator/ImpedanceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestImpedanceControl.cpp` → the `_test` target. -- New module: create `numerical/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`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/tests.md b/roadmap/controllers/manipulator/ImpedanceControl/tests.md deleted file mode 100644 index 4341109d..00000000 --- a/roadmap/controllers/manipulator/ImpedanceControl/tests.md +++ /dev/null @@ -1,65 +0,0 @@ -# Impedance Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestImpedanceControl : public ::testing::Test: - # both injected dependencies mocked; TaskDim = 2 (planar xy point), Dof = 2: - StrictMock> model - StrictMock> jacobian - SquareMatrix Md = diag(1, 1) - SquareMatrix Dd = diag(10, 10) - SquareMatrix Kstiff = diag(500, 500) - ImpedanceControl controller{ model, jacobian, Md, Dd, Kstiff } -# each case below is a TEST_F(TestImpedanceControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -stiffness_pulls_toward_target: - Arrange: jacobian J=I, model terms 0; eX != 0, velocities 0, fExt 0 - Assert: τ == Kstiff · eX - -damping_opposes_task_velocity: - Arrange: J=I, qDot != 0 (so xDot != 0), eX 0, xdDot 0 - Assert: τ contribution == -Dd · xDot - -inertia_feedforward_applied: - Arrange: xdDdot != 0, other terms 0 - Assert: F includes Md · xdDdot - -external_force_passthrough: - Arrange: fExt != 0, all errors 0, J=I - Assert: τ == fExt (Jᵀ = I) - -jacobian_transpose_maps_wrench: - Arrange: J = [[1,0],[0,2]], known F, model 0 - Assert: τ == transpose(J) · F - -gravity_and_coriolis_added: - Arrange: model g, Cq̇ nonzero; F = 0 - Assert: τ == Cq̇ + g - -zero_error_no_contact_holds_gravity: - Arrange: all errors 0, fExt 0 - Assert: τ == Cq̇ + g - -both_dependencies_queried: - Arrange: any state - Assert: jacobian.Compute called once; model gravity + Coriolis each once; - ComputeMassMatrix never called (StrictMock) -``` - -## Reference vectors - -- Pure spring (`J=I`, model 0): `F = Kstiff·eX`, `τ = JᵀF` — hand-checkable. -- Contact equilibrium: at steady state `Kstiff·eX = −fExt` ⇒ `eX_ss = −Kstiff⁻¹·fExt` (rendered compliance). - -## Edge cases - -- Near-singular `J`: `Jᵀ` stays finite, torque bounded (no inversion) — the key robustness property. -- Very high `Kstiff`: approaches rigid position control; watch contact instability vs sample rate. -- Noisy `fExt`: `Dd` filters the response; document the sensor bandwidth. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md deleted file mode 100644 index 755a8198..00000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Operational-Space Control — Overview - -## What it is -A control framework that acts directly in Cartesian (task) space while correctly accounting for how -the arm's joint-space inertia *appears* at the end-effector — the task-space inertia `Λ`. Any leftover -joint freedom in a redundant arm is used by a secondary objective in the dynamically-consistent null -space. - -## Why it matters (embedded) -It is the natural formulation for interaction and force control and for redundant (7-DOF) arms. By -reflecting the inertia through the Jacobian, it **dynamically decouples** task directions — a push in -`x` no longer disturbs `y` — and it lets a redundant arm avoid joint limits or obstacles while -rigidly holding a Cartesian pose. - -## How it works (intuition) -Reflect the joint-space mass matrix through the Jacobian to obtain the effective end-effector inertia -`Λ = (J M⁻¹ Jᵀ)⁻¹`. Command a Cartesian acceleration with a task-space PD law, convert it to a wrench -by multiplying with `Λ`, and map that wrench to joint torque with `Jᵀ`. Finally, inject any secondary -joint torque through a null-space projector chosen so it is *invisible* to the task. The one delicate -step is the `M⁻¹Jᵀ` solve, handled by Gaussian elimination rather than an explicit inverse. - -## Key parameters -- **Kp, Kd** — task-space position and damping gains. -- **model, jacobian** — injected dynamics and 6×N Jacobian. -- **tauSecondary** — secondary-objective joint torque (joint-limit / obstacle avoidance). -- **damping factor** — regularises `Λ` near singularities. - -## Reference -O. Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational -Space Formulation," *IEEE J. Robotics and Automation*, 3(1), 1987. - -## See also -`ImpedanceControl` (`Λ` enables true inertia shaping); `HybridPositionForceControl` (adds force axes); -`RedundancyResolution` (kinematic null-space); `solvers/GaussianElimination` (the `M⁻¹Jᵀ` solve). diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md deleted file mode 100644 index f8f8526c..00000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# Operational-Space Control — Implementation Pseudocode - -> Roadmap ref: #M18 (Tier 4) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class OperationalSpaceControl: - const dynamics::EulerLagrangeDynamics& model # M(q), C(q,q̇)q̇, g(q) - const kinematics::SpatialJacobian& jacobian # J (TaskDim×Dof), injected (#M8) - math::SquareMatrix Kp # task-space position gain - math::SquareMatrix Kd # task-space damping gain - solvers::GaussianElimination linearSolver # for the M⁻¹Jᵀ solve -``` - -## Interface - -``` -OperationalSpaceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& xdDdot, - const StateVector& tauSecondary) # hot path -``` - -## Algorithm (pseudocode) - -``` -function ComputeTorque(q, qDot, x, xd, xdDot, xdDdot, tauSecondary): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) # TaskDim×Dof - M = model.ComputeMassMatrix(q) # Dof×Dof (SPD) - # --- task-space (operational-space) inertia Λ = (J M⁻¹ Jᵀ)⁻¹ --- - MinvJt = linearSolver.SolveColumns(M, transpose(J)) # solve M·X = Jᵀ (no explicit inverse) - Lambda = inverse(J * MinvJt) # TaskDim×TaskDim (small: TaskDim ≤ 6) - # --- dynamically-consistent inverse and null-space projector --- - Jbar = MinvJt * Lambda # M⁻¹JᵀΛ (Dof×TaskDim) - N = Identity(Dof) - transpose(J) * transpose(Jbar) - # --- task command wrench --- - xDot = J * qDot - aX = xdDdot + Kd*(xdDot - xDot) + Kp*(xd - x) - mu = transpose(Jbar) * model.ComputeCoriolisTerms(q, qDot) # task-space Coriolis/centrifugal - p = transpose(Jbar) * model.ComputeGravityTerms(q) # task-space gravity - F = Lambda * aX + mu + p - # --- joint torque: task term + dynamically-consistent secondary term --- - return transpose(J) * F + N * tauSecondary -``` - -## Complexity & memory - -- Time: `O(Dof³)` for the `M`-solve (LU/Gaussian); `O(TaskDim³)` for the `Λ` inverse (small); - `O(Dof²·TaskDim)` for the projector. -- Memory: `O(Dof²)` working matrices; all stack/static, no heap. - -## Numerical / embedded notes - -- Solve `M·X = Jᵀ` with `solvers::GaussianElimination` (or LU) rather than forming `M⁻¹` — - cheaper and better-conditioned. -- `Λ = (J M⁻¹ Jᵀ)⁻¹` blows up at kinematic singularities (`J` loses rank); near `det(J M⁻¹ Jᵀ)→0` - switch to a **damped** inverse (add `σ²I`). -- The dynamically-consistent null-space `N = I − Jᵀ J̄ᵀ` guarantees `tauSecondary` produces **no** - task-space acceleration — clean priority stacking for redundant arms. -- The `−Λ J̇ q̇` centrifugal correction is added when the Jacobian derivative is available; omitting - it costs a small high-speed tracking error. -- `Λ` and `Jbar` are small (`TaskDim`-wide) — cache them when the task dimension ≪ `Dof`. -- 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/manipulator/OperationalSpaceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class OperationalSpaceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/OperationalSpaceControl.cpp` → - `template class OperationalSpaceControl;` -- Test: `numerical/controllers/manipulator/test/TestOperationalSpaceControl.cpp` -- Doc: `doc/controllers/manipulator/OperationalSpaceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestOperationalSpaceControl.cpp` → the `_test` target. -- New module: create `numerical/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`. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md deleted file mode 100644 index d002d8c8..00000000 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md +++ /dev/null @@ -1,65 +0,0 @@ -# Operational-Space Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestOperationalSpaceControl : public ::testing::Test: - StrictMock> model - StrictMock> jacobian - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - OperationalSpaceControl controller{ model, jacobian, Kp, Kd } -# each case below is a TEST_F(TestOperationalSpaceControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -unit_inertia_gives_task_pd: - Arrange: model M=I, J=I, g=0, C=0; eX, eXDot given - Act: τ = ComputeTorque(...) - Assert: Λ=I ⇒ τ == xdDdot + Kd·eXDot + Kp·eX - -task_inertia_computed_from_mass: - Arrange: M = diag(2,2), J=I - Assert: Λ == diag(2,2); F scaled accordingly - -gravity_mapped_to_task_and_back: - Arrange: model g != 0, M=I, J=I, errors 0 - Assert: τ == g (Jᵀ J̄ᵀ g collapses to g) - -jacobian_transpose_realises_wrench: - Arrange: nontrivial J, known F components - Assert: τ == transpose(J)·F (+ null-space term) - -secondary_torque_projected_to_nullspace: - Arrange: redundant case (Dof>TaskDim), tauSecondary != 0 - Assert: J·M⁻¹·(N·tauSecondary) ≈ 0 (no task-space acceleration) - -nullspace_orthogonality: - Arrange: any tauSecondary - Assert: J · Jbar == Identity(TaskDim) and J·M⁻¹·N ≈ 0 - -singular_jacobian_uses_damping: - Arrange: J rank-deficient - Assert: damped Λ stays finite (no NaN / inf) - -msolve_and_model_terms_queried: - Arrange: any state - Assert: ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms each once; - jacobian.Compute once (StrictMock) -``` - -## Reference vectors - -- `M=I`, `J=I`: `Λ=I`, controller collapses to task-space PD `τ = Kp·eX + Kd·eXDot + xdDdot + g`. -- Consistency: `J·M⁻¹·N ≈ 0` — the null-space produces no task motion (verify numerically ≈ 0). - -## Edge cases - -- Redundant arm (`Dof > TaskDim`): non-trivial null space; secondary objective realised without - disturbing the task. -- Singularity: damped inverse keeps torque bounded. -- Model mismatch: task decoupling degrades gracefully, stays stable. diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md deleted file mode 100644 index c05441fb..00000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# PD + Gravity Compensation — Overview - -## What it is -The simplest globally-stable manipulator set-point regulator: a PD loop on the joint error plus an -exact gravity-cancelling feedforward term — `τ = Kp·e − Kd·q̇ + g(q)`. - -## Why it matters (embedded) -It is the cheapest *model-based* joint controller in robotics. It needs only the gravity vector -`g(q)` — not the full mass matrix or Coriolis terms — so it runs in a handful of flops on a modest -MCU, yet it drives the arm to any pose with **zero steady-state droop** and a proof of global -stability. Ideal for hold/positioning tasks and as a safe fallback controller. - -## How it works (intuition) -The PD term behaves like a virtual spring-damper pulling each joint toward its target and bleeding -off velocity. Left alone, that spring would sag under gravity and settle short of the goal. The -`g(q)` feedforward supplies exactly the torque needed to counter gravity at every configuration, so -the spring no longer fights it and the joint lands precisely on the set-point. A Lyapunov -energy argument (kinetic + spring potential) guarantees the arm always converges. - -## Key parameters -- **model** — injected dynamics object supplying the gravity term `g(q)`. -- **Kp (stiffness)** — how hard the controller pulls toward the target. -- **Kd (damping)** — how strongly it resists velocity; pick `Kd ≈ 2√(Kp·inertia)` for critical damping. - -## Reference -M. Takegaki, S. Arimoto, "A New Feedback Method for Dynamic Control of Manipulators," -*ASME J. Dynamic Systems, Measurement, and Control*, 1981. - -## See also -`ComputedTorqueControl` (full inverse-dynamics tracking); `ImpedanceControl` (compliant contact); -`FrictionCompensation` (adds a friction feedforward); `dynamics/EulerLagrangeDynamics` (the injected model). diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md deleted file mode 100644 index 85fc4540..00000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md +++ /dev/null @@ -1,71 +0,0 @@ -# PD + Gravity Compensation — Implementation Pseudocode - -> Roadmap ref: #M5 (Tier 1) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class PdGravityCompensation: - const dynamics::EulerLagrangeDynamics& model # only g(q) is queried - math::SquareMatrix Kp # proportional gain (SPD) - math::SquareMatrix Kd # derivative gain (SPD) -``` - -## Interface - -``` -# Dynamics model injected (DIP); the set-point regulator needs only its gravity term: -PdGravityCompensation(const EulerLagrangeDynamics& model, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector ComputeTorque(const StateVector& q, - const StateVector& qDot, - const StateVector& qDesired) # hot path -``` - -## Algorithm (pseudocode) - -``` -function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED - # set-point regulation: qd is constant, so the desired velocity is zero - e = qd - q - # τ = Kp·e − Kd·q̇ + g(q) - g = model.ComputeGravityTerms(q) - return Kp * e - Kd * qDot + g -``` - -## Complexity & memory - -- Time: `O(Dof²)` for the two gain products (`O(Dof)` when the gains are diagonal); gravity - evaluation is `O(Dof)`–`O(Dof²)` via the injected RNEA / Euler-Lagrange model. -- Memory: `O(Dof²)` for the two gains; no dynamic state, no heap. - -## Numerical / embedded notes - -- Globally asymptotically stable for *any* SPD `Kp`, `Kd` (Takegaki–Arimoto): the Lyapunov function - `V = ½q̇ᵀM(q)q̇ + ½eᵀKp e` decreases as the `Kd·q̇` term drains kinetic energy. -- The `g(q)` term removes the static "droop"; without it a gravity-loaded joint settles with a - steady offset `e_ss = Kp⁻¹·g`. -- **Set-point regulator only** — `qd` is constant. For trajectory tracking use `ComputedTorqueControl`. -- Diagonal `Kp`, `Kd` decouple the joints and keep the hot path to `Dof` multiplies. -- Only `g(q)` is drawn from the injected model — the same interface used by the full computed-torque - law, so a single dynamics object serves both controllers (ISP / DIP). -- 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/manipulator/PdGravityCompensation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class PdGravityCompensation;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/PdGravityCompensation.cpp` → - `template class PdGravityCompensation;` -- Test: `numerical/controllers/manipulator/test/TestPdGravityCompensation.cpp` -- Doc: `doc/controllers/manipulator/PdGravityCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestPdGravityCompensation.cpp` → the `_test` target. -- New module: create `numerical/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/README.md` → "Deployment shape". diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md deleted file mode 100644 index 3c96d4f2..00000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# PD + Gravity Compensation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPdGravityCompensation : public ::testing::Test: - # Injected model mocked so torque math is verified in isolation: - StrictMock> model - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - PdGravityCompensation controller{ model, Kp, Kd } -# each case below is a TEST_F(TestPdGravityCompensation, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -gravity_only_at_matched_setpoint: - Arrange: q = qd, qDot = 0, model g(q) = [0, mgL] - Act: τ = ComputeTorque(q, 0, qd) - Assert: τ == g (pure holding torque, no PD contribution) - -proportional_term_on_position_error: - Arrange: e = qd - q != 0, qDot = 0, g = 0 - Assert: τ == Kp · e - -derivative_term_damps_velocity: - Arrange: qDot != 0, e = 0, g = 0 - Assert: τ == -Kd · qDot - -superposition_of_all_terms: - Arrange: e != 0, qDot != 0, g != 0 - Assert: τ == Kp·e - Kd·qDot + g - -diagonal_gains_decouple_joints: - Arrange: error only in joint 0, g = 0 - Assert: torque appears only in joint 0 - -zero_error_zero_velocity_holds_gravity: - Arrange: e = 0, qDot = 0 - Assert: τ == g (equilibrium holding torque) - -only_gravity_method_queried: - Arrange: any state - Assert: ComputeGravityTerms called once; ComputeMassMatrix / ComputeCoriolisTerms - never called (StrictMock enforces) - -regulates_to_setpoint_closed_loop: - Arrange: wrap plant q̈ = M⁻¹(τ − Cq̇ − g); run K steps from q0 != qd - Assert: q -> qd, ||e|| -> 0 with zero steady-state error -``` - -## Reference vectors - -- Single-pendulum hold: `g(q) = m g L sin(q)`; at `qd` with `e = 0`, `q̇ = 0` ⇒ `τ = m g L sin(qd)`. -- Unique closed-loop equilibrium at `e = 0` (SPD `Kp` invertible), independent of `Kd`. - -## Edge cases - -- Large initial error: `τ` bounded by `Kp·e_max + g_max` (external actuator clamp documented). -- `Kp → 0`: convergence slows but stays stable while `Kp` remains SPD. -- Gravity model mismatch (mock `g` scaled 1.1): bounded steady droop `e_ss = Kp⁻¹·Δg`. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md deleted file mode 100644 index 928e5688..00000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Slotine-Li Adaptive Control — Overview - -## What it is -A trajectory-tracking manipulator controller that **learns the robot's inertial parameters online** -while it moves. It exploits the fact that rigid-body dynamics are *linear in the inertial -parameters*: `M(q)q̈ + C(q,q̇)q̇ + g(q) = Y(q,q̇,q̈)·a`, so the unknown masses/inertias `a` can be -estimated by a simple adaptation law. - -## Why it matters (embedded) -Real robots carry unknown or changing payloads. Rather than re-identifying the model offline, this -controller adapts its feedforward in real time, giving accurate tracking without precise a-priori -mass data — valuable when the same low-cost arm must handle varied loads. - -## How it works (intuition) -It combines a *sliding variable* `s = ė + Λe` (a filtered tracking error) with a regressor `Y` -evaluated at a **reference** acceleration, so no noisy joint-acceleration measurement is needed. The -torque is a model feedforward `Y·â` plus damping `−Kd·s`. Whenever `s` is nonzero the parameter -estimate `â` slides *downhill* on a Lyapunov function (`â̇ = −Γ Yᵀ s`), simultaneously shrinking the -tracking error and improving the model. Passivity guarantees stability. - -## Key parameters -- **Λ (surface slope)** — bandwidth of the `s = ė + Λe` error manifold. -- **Kd (sliding gain)** — damping on `s`; sets transient stiffness. -- **Γ (adaptation gain)** — how fast parameters are learned; too large ⇒ oscillation. -- **â₀ (initial estimate)** — parameter seed; a rough guess speeds convergence. - -## Reference -J.-J. Slotine, W. Li, "On the Adaptive Control of Robot Manipulators," *Int. J. Robotics Research*, -6(3), 1987. - -## See also -`ComputedTorqueControl` (#M12, exact-model sibling), `ModelReferenceAdaptiveControl` (#47), -`DynamicParameterIdentification` (#M22, offline regressor identification). diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md deleted file mode 100644 index 05ad5d05..00000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Slotine-Li Adaptive Control — Implementation Pseudocode - -> Roadmap ref: #M20 (Tier 4) · Target: `numerical/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SlotineLiAdaptiveControl: - const dynamics::InertialRegressor& regressor # Y(q,q̇,q̇r,q̈r) - math::SquareMatrix Lambda # sliding-surface slope (SPD) - math::SquareMatrix Kd # sliding-variable gain (SPD) - math::SquareMatrix Gamma # adaptation gain (SPD) - math::Vector aHat # inertial-parameter estimate (STATE) - T dt -``` - -## Interface - -``` -# Regressor injected (DIP): supplies Y such that M(q)q̈r + C(q,q̇)q̇r + g(q) = Y·a -SlotineLiAdaptiveControl(const InertialRegressor& regressor, const SquareMatrix& Lambda, - const SquareMatrix& Kd, const SquareMatrix& Gamma, - const Vector& aHat0, T dt) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const StateVector& qd, const StateVector& qdDot, - const StateVector& qdDdot) # hot path, updates aHat -const Vector& ParameterEstimate() const -void Reset(const Vector& aHat0) -``` - -## Algorithm (pseudocode) - -``` -function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - qTilde = q - qd - qTildeDot = qDot - qdDot - # sliding variable and reference (not measured) motion — no q̈ measurement needed: - s = qTildeDot + Lambda * qTilde - qrDot = qdDot - Lambda * qTilde - qrDdot = qdDdot - Lambda * qTildeDot - # linearity-in-parameters: M q̈r + C q̇r + g = Y·a - Y = regressor.Compute(q, qDot, qrDot, qrDdot) # Dof×NumParams - # control: model feedforward on the current estimate + sliding-variable damping - tau = Y * aHat - Kd * s - # passivity-based parameter update (integrate â̇ = −Γ Yᵀ s): - aHat = aHat - (Gamma * transpose(Y) * s) * dt - return tau -``` - -## Complexity & memory - -- Time: `O(Dof·NumParams)` for `Y·aHat` and `Yᵀ·s`; regressor build is `O(Dof·NumParams)` (RNEA form). -- Memory: `O(NumParams)` parameter state + `O(Dof²)` gains; no heap. - -## Numerical / embedded notes - -- **No acceleration measurement:** `Y` is evaluated at the *reference* `q̈r`, not measured `q̈` — the - key robustness advantage over direct inverse-dynamics identification. -- Global tracking convergence via Lyapunov `V = ½sᵀM(q)s + ½ãᵀΓ⁻¹ã` (SPD `M`, `ã = âHat − a`): - `s → 0` hence `q̃ → 0`; parameters stay **bounded** but converge only under persistent excitation. -- Add a **projection** (or dead-zone) so `aHat` stays in a physically valid set (positive masses) - and does not drift under sensor noise or unmodelled dynamics. -- `Λ` sets the error-manifold bandwidth, `Kd` damps `s`, `Γ` sets adaptation speed — too large `Γ` - causes estimate oscillation and can excite unmodelled modes. -- Reuses the RNEA regressor form; sibling of `ComputedTorqueControl` (#M12, non-adaptive) and - `ModelReferenceAdaptiveControl` (#47). -- 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/manipulator/SlotineLiAdaptiveControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class SlotineLiAdaptiveControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/controllers/manipulator/SlotineLiAdaptiveControl.cpp` → - `template class SlotineLiAdaptiveControl;` -- Test: `numerical/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp` -- Doc: `doc/controllers/manipulator/SlotineLiAdaptiveControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestSlotineLiAdaptiveControl.cpp` → the `_test` target. -- New module: create `numerical/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`. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md deleted file mode 100644 index 577f2637..00000000 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md +++ /dev/null @@ -1,75 +0,0 @@ -# Slotine-Li Adaptive Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSlotineLiAdaptiveControl : public ::testing::Test: - # regressor injected & mocked; Dof = 2, NumParams = 3: - StrictMock> regressor - SquareMatrix Lambda = diag(5, 5) - SquareMatrix Kd = diag(20, 20) - SquareMatrix Gamma = diag(1, 1, 1) - Vector aHat0 = {0, 0, 0} - float dt = 0.001 - SlotineLiAdaptiveControl controller{ regressor, Lambda, Kd, Gamma, aHat0, dt } -# each case below is a TEST_F(TestSlotineLiAdaptiveControl, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -zero_error_uses_feedforward_only: - Arrange: q=qd, qDot=qdDot ⇒ s=0; regressor Y given - Act: τ = ComputeTorque(...) - Assert: τ == Y · aHat (no −Kd·s term) - -sliding_variable_damping: - Arrange: s != 0 via qTildeDot, regressor Y = 0 - Assert: τ == -Kd · s - -sliding_variable_definition: - Arrange: known qTilde, qTildeDot - Assert: s == qTildeDot + Lambda · qTilde - -reference_motion_uses_lambda: - Arrange: known errors - Assert: regressor called with qrDot == qdDot − Λ·qTilde and qrDdot == qdDdot − Λ·qTildeDot - -parameter_estimate_adapts: - Arrange: s != 0, Y != 0, one step - Assert: aHat == aHat0 − Γ·Yᵀ·s·dt - -no_adaptation_on_zero_sliding: - Arrange: s = 0 - Assert: aHat unchanged after the step - -estimate_persists_across_calls: - Arrange: two steps with the same s, Y - Assert: aHat integrates twice; ParameterEstimate() reflects both - -convergence_tracks_trajectory: - Arrange: wrap a plant M q̈ + Cq̇ + g = τ with unknown a; run K steps - Assert: ||q − qd|| -> 0 (tracking) even though aHat != a - -reset_restores_initial_estimate: - Arrange: adapt, then Reset(aHat0) - Assert: ParameterEstimate() == aHat0 - -regressor_queried_once_per_call: - Arrange: any state - Assert: regressor.Compute called exactly once (StrictMock) -``` - -## Reference vectors - -- By construction `s = q̃̇ + Λq̃`; single joint, `Λ=λ`, constant `q̃=e`, `q̃̇=0` ⇒ `s = λe` (hand-checkable). -- With `aHat = a` (true params) the law reduces to computed-torque-like `τ = Y·a − Kd·s`. -- One adaptation step: `Δa = −Γ·Yᵀ·s·dt` — exact, hand-verifiable. - -## Edge cases - -- Non-persistent excitation: `aHat` settles on a manifold; tracking still converges. -- Large `Γ`: estimate oscillation — assert bounded, no divergence. -- Parameter drift under noisy `s`: projection / dead-zone keeps `aHat` physical. diff --git a/roadmap/nonlinear_control/FeedbackLinearization/explanation.md b/roadmap/nonlinear_control/FeedbackLinearization/explanation.md index e4b9bbda..bb501eaa 100644 --- a/roadmap/nonlinear_control/FeedbackLinearization/explanation.md +++ b/roadmap/nonlinear_control/FeedbackLinearization/explanation.md @@ -1,27 +1,31 @@ # Feedback Linearization — Overview ## What it is -A control technique that **cancels** a plant's known nonlinear dynamics with an inner control law, -leaving an equivalent linear system that a simple outer loop (PD, LQR) can drive. For mechanical -systems this is the *computed-torque* method: `τ = M(q)·v + C(q,q̇)q̇ + g(q)` turns the robot into -a set of decoupled double integrators `q̈ = v`. +A control technique that **cancels** a control-affine plant's known nonlinear dynamics with an inner +control law, leaving an equivalent linear system that a simple outer loop (PD, LQR) can drive. For a +system whose input enters through a state-dependent decoupling matrix, the cancelling law is +`u = B(x)·v + a(x)`, which turns the plant into decoupled integrator chains `ÿ = v`. The mechanical +*computed-torque* method `τ = M(q)·v + C(q,q̇)q̇ + g(q)` is the canonical instance (`B = M`, +`a = Cq̇ + g`). ## Why it matters (embedded) Robot arms, quadrotors, and other structurally-known machines are strongly nonlinear — a fixed PID tuned at one operating point misbehaves at another. Feedback linearization uses the *model you -already have* (`M`, `C`, `g` from `dynamics/`) to erase that nonlinearity, so one linear gain set -works across the whole workspace. No gain scheduling, no lookup tables. +already have* to erase that nonlinearity, so one linear gain set works across the whole operating +envelope. No gain scheduling, no lookup tables. ## How it works (intuition) -Split the controller in two. The **inner** law evaluates the model at the current state and injects -exactly the torque needed to cancel gravity, Coriolis, and inertia coupling. What remains behaves -like unit masses. The **outer** law then commands a virtual acceleration `v = q̈_d + Kd·ė + Kp·e` -as if controlling those trivial masses. Because the cancellation multiplies by `M(q)` (never -inverts it), the hot path stays well-conditioned. +Split the controller in two. The **inner** law evaluates the injected model at the current state and +injects exactly the input needed to cancel the drift `a(x)`. What remains behaves like plain +integrator chains. The **outer** law then commands a virtual input `v = y_d^{(r)} + Kd·ė + Kp·e` +as if controlling those trivial integrators. In the mechanical (input-state) case the cancellation +multiplies by `B(x) = M(q)` — never inverts it — so the hot path stays well-conditioned. ## Key parameters -- **nonlinear model** — injected `EulerLagrangeDynamics` supplying `M(q)`, `C(q,q̇)q̇`, `g(q)`. -- **Kp, Kd** — outer-loop gains for the linearized double integrator; pick `Kd = 2√Kp` for +- **nonlinear model** — an injected control-affine model supplying the decoupling matrix `B(x)` and + the drift term `a(x)` to be cancelled. (The manipulator instance — `M(q)`, `C(q,q̇)q̇`, `g(q)` — + lives in robotics-toolbox-cpp.) +- **Kp, Kd** — outer-loop gains for the linearized integrator chain; pick `Kd = 2√Kp` for critical damping. - **relative degree** (general form) — how many times to differentiate the output before the input appears; sets the structure of the cancellation. @@ -33,4 +37,5 @@ A. Isidori, *Nonlinear Control Systems*, 3rd ed. (1995); J.-J. Slotine, W. Li, ## See also `BacksteppingControl` (recursive alternative that tolerates non-cancellable terms); `ModelReferenceAdaptiveControl` (adapts the model online when parameters are unknown); -`dynamics/EulerLagrangeDynamics` (the injected model); `Lqr` (a natural outer loop). +`Lqr` (a natural outer loop). The manipulator computed-torque instantiation is +`controllers/manipulator/ComputedTorqueControl` in robotics-toolbox-cpp. diff --git a/roadmap/nonlinear_control/FeedbackLinearization/implementation.md b/roadmap/nonlinear_control/FeedbackLinearization/implementation.md index beb258f8..b5f776f2 100644 --- a/roadmap/nonlinear_control/FeedbackLinearization/implementation.md +++ b/roadmap/nonlinear_control/FeedbackLinearization/implementation.md @@ -1,44 +1,53 @@ -# Feedback Linearization (Computed Torque) — Implementation Pseudocode +# Feedback Linearization — Implementation Pseudocode > Roadmap ref: #40 (Tier 4) · Target: `numerical/nonlinear_control` · Namespace `nonlinear_control` · Type: `float` (templated on `T`, instantiated for `float` only) ## Data structures ``` -template # static_assert(std::is_floating_point_v); instantiated for float +# Injected abstract model of a control-affine plant y^(r) = a(x) + B(x)·u : +template +class ControlAffineModel: # pure-virtual interface (DIP) + virtual ~ControlAffineModel() = default + virtual SquareMatrix DecouplingMatrix(const StateVector& x) const # B(x) + virtual Vector DriftTerm(const StateVector& x) const # a(x) to cancel + +template # static_assert(std::is_floating_point_v); instantiated for float class FeedbackLinearization: - const dynamics::EulerLagrangeDynamics& model # injected M(q), C(q,q̇)q̇, g(q) - math::SquareMatrix Kp # outer proportional gain - math::SquareMatrix Kd # outer derivative gain + const ControlAffineModel& model # supplies B(x), a(x) + math::SquareMatrix Kp # outer proportional gain + math::SquareMatrix Kd # outer derivative gain ``` +The mechanical *computed-torque* instance sets `B(x) = M(q)` and `a(x) = C(q,q̇)q̇ + g(q)`; that +manipulator model lives in robotics-toolbox-cpp and is injected here through `ControlAffineModel`. + ## Interface ``` -# Nonlinear model injected (DIP); outer linear gains chosen for the double-integrator: -FeedbackLinearization(const EulerLagrangeDynamics& model, +# Nonlinear model injected (DIP); outer linear gains chosen for the integrator chain: +FeedbackLinearization(const ControlAffineModel& model, const SquareMatrix& Kp, const SquareMatrix& Kd) -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const StateVector& qd, const StateVector& qdDot, - const StateVector& qdDdot) # hot path +Vector ComputeInput(const StateVector& x, const StateVector& xDot, + const StateVector& yd, const StateVector& ydDot, + const StateVector& ydDdot) # hot path ``` ## Algorithm (pseudocode) ``` -function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - # --- outer loop: linear control on the linearized plant ÿ = q̈ = v --- - e = qd - q - eDot = qdDot - qDot - v = qdDdot + Kd * eDot + Kp * e # virtual acceleration command - - # --- inner loop: cancel the known nonlinearities (input-state form) --- - # τ = M(q)·v + C(q,q̇)q̇ + g(q) ⇒ q̈ = v exactly - M = model.ComputeMassMatrix(q) - Cqd = model.ComputeCoriolisTerms(q, qDot) - g = model.ComputeGravityTerms(q) - return M * v + Cqd + g +function ComputeInput(x, xDot, yd, ydDot, ydDdot): # OPTIMIZE_FOR_SPEED + # --- outer loop: linear control on the linearized plant ÿ = v --- + e = yd - x + eDot = ydDot - xDot + v = ydDdot + Kd * eDot + Kp * e # virtual input command + + # --- inner loop: cancel the known drift (input-state form) --- + # u = B(x)·v + a(x) ⇒ ÿ = v exactly + B = model.DecouplingMatrix(x) + a = model.DriftTerm(x) + return B * v + a # General SISO input-output form (relative degree r), for non-mechanical plants: # y^(r) = L_f^r h(x) + L_g L_f^(r-1) h(x) · u @@ -47,29 +56,29 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED ## Complexity & memory -- `ComputeTorque`: `O(Dof²)` for `M·v`; model evaluation is `O(Dof)`–`O(Dof²)` (RNEA-style). -- Memory: `O(Dof²)` for the two gains; no dynamic state — all static, no heap. +- `ComputeInput`: `O(Dim²)` for `B·v`; model evaluation is `O(Dim)`–`O(Dim²)`. +- Memory: `O(Dim²)` for the two gains; no dynamic state — all static, no heap. ## Numerical / embedded notes -- Computed-torque **multiplies** by `M(q)` — it never inverts it, so no ill-conditioned solve on - the hot path (unlike forward dynamics). `M(q)` is SPD for mechanical systems. +- The input-state form **multiplies** by `B(x)` — it never inverts it, so no ill-conditioned solve + on the hot path (unlike forward dynamics). For mechanical plants `B(x) = M(q)` is SPD. - Cancellation is only as good as the model: parameter mismatch leaves a residual nonlinearity — pair with a robust (sliding-mode) or adaptive (MRAC) outer term to mop up the error. - The general input-output form loses well-posedness where `L_g L_f^(r-1) h → 0` (a singularity); keep the operating region away from it, and watch for unstable internal dynamics (zero dynamics). -- Choose `Kp`, `Kd` for a critically-damped double integrator (`Kd = 2√Kp`) per channel. +- Choose `Kp`, `Kd` for a critically-damped integrator chain (`Kd = 2√Kp`) per channel. - 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/nonlinear_control/FeedbackLinearization.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class FeedbackLinearization;` + `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeInput`, and + `extern template class FeedbackLinearization;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. - Coverage: `numerical/nonlinear_control/FeedbackLinearization.cpp` → - `template class FeedbackLinearization;` + `template class FeedbackLinearization;` - Test: `numerical/nonlinear_control/test/TestFeedbackLinearization.cpp` - Doc: `doc/nonlinear_control/FeedbackLinearization.md` (expand to follow `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; diff --git a/roadmap/nonlinear_control/FeedbackLinearization/tests.md b/roadmap/nonlinear_control/FeedbackLinearization/tests.md index b0fa3d9f..7b434948 100644 --- a/roadmap/nonlinear_control/FeedbackLinearization/tests.md +++ b/roadmap/nonlinear_control/FeedbackLinearization/tests.md @@ -6,8 +6,8 @@ ``` class TestFeedbackLinearization : public ::testing::Test: - # Injected nonlinear model is mocked so torque math is verified in isolation: - StrictMock> model + # Injected control-affine model is mocked so the law is verified in isolation: + StrictMock> model SquareMatrix Kp = diag(100, 100) SquareMatrix Kd = diag( 20, 20) FeedbackLinearization controller{ model, Kp, Kd } @@ -17,48 +17,45 @@ class TestFeedbackLinearization : public ::testing::Test: ## Test cases (Arrange / Act / Assert) ``` -cancels_to_double_integrator: - Arrange: model returns M=I, C=0, g=0; error and error-rate = 0, qdDdot = a - Act: τ = ComputeTorque(...) - Assert: τ == a (τ = M·v reduces to the commanded acceleration) +cancels_to_integrator_chain: + Arrange: model returns B=I, a=0; error and error-rate = 0, ydDdot = c + Act: u = ComputeInput(...) + Assert: u == c (u = B·v reduces to the commanded virtual input) -adds_gravity_compensation: - Arrange: model g(q) = [0, mgL]; all setpoints match state, v = 0 - Assert: τ == g(q) (pure gravity hold torque) - -adds_coriolis_terms: - Arrange: model C(q,q̇)q̇ = c; v = 0 - Assert: τ == c +adds_drift_compensation: + Arrange: model a(x) = [0, d]; all setpoints match state, v = 0 + Assert: u == a(x) (pure drift-cancelling input) pd_law_drives_position_error: - Arrange: e = qd - q != 0, eDot = 0, feedforward = 0, M=I - Assert: τ == Kp · e (proportional term appears through M·v) + Arrange: e = yd - x != 0, eDot = 0, feedforward = 0, B=I + Assert: u == Kp · e (proportional term appears through B·v) pd_law_drives_velocity_error: - Arrange: eDot != 0, e = 0, M=I - Assert: τ == Kd · eDot + Arrange: eDot != 0, e = 0, B=I + Assert: u == Kd · eDot -mass_matrix_scales_virtual_input: - Arrange: M = diag(2,3), v = [1,1] - Assert: τ == [2,3] (M·v applied, not v alone) +decoupling_matrix_scales_virtual_input: + Arrange: B = diag(2,3), v = [1,1] + Assert: u == [2,3] (B·v applied, not v alone) closed_loop_error_decays: - Arrange: wrap a plant that integrates q̈ = M⁻¹(τ − Cq̇ − g); run K steps - Assert: ||qd − q|| -> 0 monotonically (exact linearization) + Arrange: wrap a plant that integrates ÿ = B⁻¹(u − a); run K steps + Assert: ||yd − x|| -> 0 monotonically (exact linearization) reference_feedforward_used: - Arrange: qdDdot = a, all errors 0, M=I - Assert: τ == a (acceleration feedforward passes through) + Arrange: ydDdot = c, all errors 0, B=I + Assert: u == c (feedforward passes through) ``` ## Reference vectors - Ideal cancellation: with the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — a linear ODE whose decay rate is hand-computable from `Kp`, `Kd`. -- Single pendulum gravity hold: `τ = m g L sin(q)` is the golden torque at zero acceleration. +- Computed-torque instance (single pendulum): with `B = M`, `a = g(q)`, the gravity-hold input at + zero acceleration is the golden `τ = m g L sin(q)`. ## Edge cases -- Model mismatch (mock M scaled by 1.2): closed loop stays stable but shows bounded steady error. -- `qdDdot` large: torque must not saturate the (documented) actuator model in tests. -- Near-singular `M` in the general input-output form: division guard exercised. +- Model mismatch (mock `B` scaled by 1.2): closed loop stays stable but shows bounded steady error. +- `ydDdot` large: input must not saturate the (documented) actuator model in tests. +- Near-singular `B` / decoupling matrix in the general input-output form: division guard exercised.