From a03c5adedd4631c0c2eaa8307c477fb991c9ec0d Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sat, 1 Aug 2026 11:47:39 +0000 Subject: [PATCH 1/3] fix dependabot --- .github/dependabot.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index a253c73..c16bcd6 100644 --- a/.github/dependabot.yml +++ b/.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 From a940618e7ea7d3a6015d225007866b7e24775340 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sat, 1 Aug 2026 12:05:39 +0000 Subject: [PATCH 2/3] add controllers roadmap --- .../CableTensionDistribution/explanation.md | 34 +++++++ .../implementation.md | 79 +++++++++++++++++ .../CableTensionDistribution/tests.md | 65 ++++++++++++++ .../ComputedTorqueControl/explanation.md | 30 +++++++ .../ComputedTorqueControl/implementation.md | 76 ++++++++++++++++ .../ComputedTorqueControl/tests.md | 68 ++++++++++++++ .../HybridPositionForceControl/explanation.md | 33 +++++++ .../implementation.md | 84 ++++++++++++++++++ .../HybridPositionForceControl/tests.md | 72 +++++++++++++++ .../ImpedanceControl/explanation.md | 32 +++++++ .../ImpedanceControl/implementation.md | 82 +++++++++++++++++ .../manipulator/ImpedanceControl/tests.md | 65 ++++++++++++++ .../OperationalSpaceControl/explanation.md | 34 +++++++ .../OperationalSpaceControl/implementation.md | 85 ++++++++++++++++++ .../OperationalSpaceControl/tests.md | 65 ++++++++++++++ .../PdGravityCompensation/explanation.md | 31 +++++++ .../PdGravityCompensation/implementation.md | 71 +++++++++++++++ .../PdGravityCompensation/tests.md | 64 ++++++++++++++ .../SlotineLiAdaptiveControl/explanation.md | 33 +++++++ .../implementation.md | 86 ++++++++++++++++++ .../SlotineLiAdaptiveControl/tests.md | 75 ++++++++++++++++ roadmap/math/SE3Transform/explanation.md | 34 +++++++ roadmap/math/SE3Transform/implementation.md | 88 +++++++++++++++++++ roadmap/math/SE3Transform/tests.md | 64 ++++++++++++++ 24 files changed, 1450 insertions(+) create mode 100644 roadmap/controllers/manipulator/CableTensionDistribution/explanation.md create mode 100644 roadmap/controllers/manipulator/CableTensionDistribution/implementation.md create mode 100644 roadmap/controllers/manipulator/CableTensionDistribution/tests.md create mode 100644 roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md create mode 100644 roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md create mode 100644 roadmap/controllers/manipulator/ComputedTorqueControl/tests.md create mode 100644 roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md create mode 100644 roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md create mode 100644 roadmap/controllers/manipulator/HybridPositionForceControl/tests.md create mode 100644 roadmap/controllers/manipulator/ImpedanceControl/explanation.md create mode 100644 roadmap/controllers/manipulator/ImpedanceControl/implementation.md create mode 100644 roadmap/controllers/manipulator/ImpedanceControl/tests.md create mode 100644 roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md create mode 100644 roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md create mode 100644 roadmap/controllers/manipulator/OperationalSpaceControl/tests.md create mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/explanation.md create mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/implementation.md create mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/tests.md create mode 100644 roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md create mode 100644 roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md create mode 100644 roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md create mode 100644 roadmap/math/SE3Transform/explanation.md create mode 100644 roadmap/math/SE3Transform/implementation.md create mode 100644 roadmap/math/SE3Transform/tests.md diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md new file mode 100644 index 0000000..cd1bffe --- /dev/null +++ b/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). diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md new file mode 100644 index 0000000..f784227 --- /dev/null +++ b/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 # 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: `robotics/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: `robotics/controllers/manipulator/CableTensionDistribution.cpp` → + `template class CableTensionDistribution;` +- 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`. diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md new file mode 100644 index 0000000..206ad74 --- /dev/null +++ b/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> 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 new file mode 100644 index 0000000..16cd87f --- /dev/null +++ b/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). diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md new file mode 100644 index 0000000..1316100 --- /dev/null +++ b/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 # 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: `robotics/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: `robotics/controllers/manipulator/ComputedTorqueControl.cpp` → + `template class ComputedTorqueControl;` +- 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`. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md b/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md new file mode 100644 index 0000000..af77784 --- /dev/null +++ b/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> 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 new file mode 100644 index 0000000..9f617c5 --- /dev/null +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md @@ -0,0 +1,33 @@ +# 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 new file mode 100644 index 0000000..34b1c01 --- /dev/null +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md @@ -0,0 +1,84 @@ +# Hybrid Position/Force Control — Implementation Pseudocode + +> Roadmap ref: #M19 (Tier 4) · Target: `robotics/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: `robotics/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: `robotics/controllers/manipulator/HybridPositionForceControl.cpp` → + `template class HybridPositionForceControl;` +- Test: `robotics/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 `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`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md new file mode 100644 index 0000000..e85ee94 --- /dev/null +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md @@ -0,0 +1,72 @@ +# 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 new file mode 100644 index 0000000..4f9c492 --- /dev/null +++ b/roadmap/controllers/manipulator/ImpedanceControl/explanation.md @@ -0,0 +1,32 @@ +# 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 new file mode 100644 index 0000000..3a44e5f --- /dev/null +++ b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md @@ -0,0 +1,82 @@ +# Impedance Control — Implementation Pseudocode + +> Roadmap ref: #M17 (Tier 3) · Target: `robotics/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: `robotics/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: `robotics/controllers/manipulator/ImpedanceControl.cpp` → + `template class ImpedanceControl;` +- Test: `robotics/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 `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`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/tests.md b/roadmap/controllers/manipulator/ImpedanceControl/tests.md new file mode 100644 index 0000000..4341109 --- /dev/null +++ b/roadmap/controllers/manipulator/ImpedanceControl/tests.md @@ -0,0 +1,65 @@ +# 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 new file mode 100644 index 0000000..755a819 --- /dev/null +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md @@ -0,0 +1,34 @@ +# 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 new file mode 100644 index 0000000..184f5c0 --- /dev/null +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md @@ -0,0 +1,85 @@ +# Operational-Space Control — Implementation Pseudocode + +> Roadmap ref: #M18 (Tier 4) · Target: `robotics/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: `robotics/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: `robotics/controllers/manipulator/OperationalSpaceControl.cpp` → + `template class OperationalSpaceControl;` +- Test: `robotics/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 `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`. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md new file mode 100644 index 0000000..d002d8c --- /dev/null +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md @@ -0,0 +1,65 @@ +# 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 new file mode 100644 index 0000000..c05441f --- /dev/null +++ b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 0000000..5e2bd45 --- /dev/null +++ b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md @@ -0,0 +1,71 @@ +# PD + Gravity Compensation — Implementation Pseudocode + +> Roadmap ref: #M5 (Tier 1) · Target: `robotics/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: `robotics/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: `robotics/controllers/manipulator/PdGravityCompensation.cpp` → + `template class PdGravityCompensation;` +- Test: `robotics/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 `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/README.md` → "Deployment shape". diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md new file mode 100644 index 0000000..3c96d4f --- /dev/null +++ b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md @@ -0,0 +1,64 @@ +# 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 new file mode 100644 index 0000000..928e568 --- /dev/null +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md @@ -0,0 +1,33 @@ +# 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 new file mode 100644 index 0000000..9bbf119 --- /dev/null +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md @@ -0,0 +1,86 @@ +# Slotine-Li Adaptive Control — Implementation Pseudocode + +> Roadmap ref: #M20 (Tier 4) · Target: `robotics/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: `robotics/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: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp` → + `template class SlotineLiAdaptiveControl;` +- Test: `robotics/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 `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`. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md new file mode 100644 index 0000000..577f263 --- /dev/null +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md @@ -0,0 +1,75 @@ +# 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/math/SE3Transform/explanation.md b/roadmap/math/SE3Transform/explanation.md new file mode 100644 index 0000000..4edd992 --- /dev/null +++ b/roadmap/math/SE3Transform/explanation.md @@ -0,0 +1,34 @@ +# SE(3) Transform — Overview + +## What it is +`SE(3)` is the group of rigid-body motions in 3D: a rotation plus a translation, stored as a 4×4 +homogeneous matrix `[[R, p],[0, 1]]`. Its "velocity" companions are **twists** (6-vectors pairing +angular and linear velocity) and **wrenches** (moment plus force); the **adjoint** map moves those +6-vectors between coordinate frames. + +## Why it matters (embedded) +This is the algebra every modern manipulator, drone, and SLAM stack is built on. Forward kinematics +chains `SE(3)` products; Jacobians and dynamics are expressed with twists and the adjoint; trajectory +interpolation lives on this group. A compact, correct `SE(3)` type is the foundation the whole +robotics layer reuses. + +## How it works (intuition) +Composing two motions multiplies their homogeneous matrices, which reduces to `R = R₁R₂`, +`p = R₁p₂ + p₁` — no full 4×4 multiply needed. The inverse has the closed form `(Rᵀ, −Rᵀp)`. The +**exponential map** turns a constant twist (a screw axis) applied for a "time" `θ` into the finite +motion it produces — the rigid-body generalization of Rodrigues' rotation formula — and its inverse +**log** recovers the screw from a transform. The **adjoint** is the 6×6 that re-expresses a twist or +wrench in another frame. + +## Key parameters +- **Twist ordering** — `(ω; v)` (angular first) here; the adjoint layout follows this convention. +- **Screw parameter `θ`** — how far along a twist/screw axis the exponential integrates. +- **Orthonormality of `R`** — must be maintained on `SO(3)`; reorthonormalize against drift. + +## Reference +K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3; R. M. Murray, Z. Li, S. S. Sastry, +*A Mathematical Introduction to Robotic Manipulation* (1994). + +## See also +`Quaternion` (item 18, the `SO(3)` rotation part), `Geometry3D` (`RotationAboutAxis`, +`SkewSymmetric`), `MatrixExponential` (item 29, the `se(3)→SE(3)` analogue), `DenavitHartenberg` (M7). diff --git a/roadmap/math/SE3Transform/implementation.md b/roadmap/math/SE3Transform/implementation.md new file mode 100644 index 0000000..4e90173 --- /dev/null +++ b/roadmap/math/SE3Transform/implementation.md @@ -0,0 +1,88 @@ +# SE(3) Transform — Implementation Pseudocode + +> Roadmap ref: #M6 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) + +## Data structures + +``` +template # static_assert(std::is_floating_point_v); instantiated for float +class SE3: + Matrix3 R # rotation, on SO(3) + Vector3 p # translation + # equivalently the 4×4 homogeneous matrix [[R, p],[0 0 0 1]] + +# spatial 6-vectors reuse Vector: +# Twist = (ω; v) angular over linear velocity +# Wrench = (m; f) moment over force +``` + +## Interface + +``` +SE3() # identity +SE3(Matrix3 R, Vector3 p) +SquareMatrix Homogeneous() # 4×4 form +SE3 operator*(SE3 rhs) # compose transforms; hot path +SE3 Inverse() +Vector3 Apply(Vector3 point) # transform a point; hot path +Matrix Adjoint() # maps twists/wrenches between frames +static SE3 Exp(Vector xi, T theta) # exp: se(3) -> SE(3) +Vector Log() # log: SE(3) -> se(3) +``` + +## Algorithm (pseudocode) + +``` +function multiply(a, b): # OPTIMIZE_FOR_SPEED + return SE3(a.R * b.R, a.R * b.p + a.p) + +function Inverse(): # closed-form rigid inverse (no matrix solve) + return SE3(Transpose(R), -Transpose(R) * p) + +function Apply(point): + return R * point + p + +function Adjoint(): # 6×6: [[R, 0],[ [p]× R, R ]] + upper-left = R + lower-left = SkewSymmetric(p) * R # reuse Geometry3D::SkewSymmetric + lower-right = R + upper-right = 0 + +function Exp(xi = (ω; v), theta): # matrix exponential on se(3) + if VectorNorm(ω) == 0: return SE3(I, v*theta) # pure translation + R = RotationAboutAxis(ω, theta) # Rodrigues (reuse Geometry3D) + G = I*theta + (1-cosθ)*SkewSymmetric(ω) + (θ-sinθ)*SkewSymmetric(ω)² # left Jacobian + return SE3(R, G * v) + +function Log(): # inverse of Exp + (ω, theta) = axisAngleFromRotation(R) # log on SO(3) + v = InverseLeftJacobian(theta) * p + return concat(ω*theta, v*theta) +``` + +## Complexity & memory + +- Compose / inverse / apply: `O(1)` — fixed 3×3 and 3-vector arithmetic, no loops. +- Adjoint / Exp / Log: `O(1)` — a handful of 3×3 products; `Log` adds one `acos`. +- Memory: a 3×3 plus a 3-vector per transform (or one 4×4); all stack-allocated, no heap. + +## Numerical / embedded notes + +- Use the **closed-form rigid inverse** `(Rᵀ, −Rᵀp)`; never run a general 4×4 matrix solve. +- Keep `R` on `SO(3)`: reorthonormalize after long product chains (drift accumulates like quaternions). +- `Exp`/`Log` need `sinθ/θ` and `(1−cosθ)/θ²`; use series expansions near `θ→0` to avoid `0/0`. +- Twist ordering here is `(ω; v)` (angular first); the adjoint block layout must match that choice. +- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a + `Q15`/`Q31` specialisation cheap to add later. + +## Deployment + +- Header: `numerical/math/SE3Transform.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `operator*`/`Apply`, and + `extern template class SE3;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/math/SE3Transform.cpp` → `template class SE3;` +- Test: `numerical/math/test/TestSE3Transform.cpp` +- Doc: `doc/math/SE3Transform.md` (new folder; math currently has no doc pages) +- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → + `numerical_add_coverage_sources(numerical.math ...)`; `TestSE3Transform.cpp` → `numerical.math_test`. +- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/math/SE3Transform/tests.md b/roadmap/math/SE3Transform/tests.md new file mode 100644 index 0000000..0138f94 --- /dev/null +++ b/roadmap/math/SE3Transform/tests.md @@ -0,0 +1,64 @@ +# SE(3) Transform — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +``` +class TestSE3 : public ::testing::Test: + SE3 identity + SE3 gAB # rotation 90° about ẑ + translation (1,0,0) +# each case below is a TEST_F(TestSE3, ) +``` + +## Test cases (Arrange / Act / Assert) + +``` +identity_apply_is_noop: + Assert: identity.Apply(p) == p + +compose_with_inverse_is_identity: + Arrange: g = gAB + Assert: g * g.Inverse() ≈ Identity + +apply_rotates_then_translates: + Arrange: g = rot(ẑ, 90°) + p = (1,0,0) + Assert: g.Apply((1,0,0)) ≈ (1,1,0) + +inverse_matches_transpose_form: + Assert: Inverse().R == Rᵀ and Inverse().p == -Rᵀp + +homogeneous_matches_block_form: + Assert: Homogeneous() == [[R, p],[0, 0, 0, 1]] + +adjoint_transforms_twist: + Arrange: known twist in frame B + Assert: Adjoint()·ξ matches the hand-computed twist in frame A + +exp_pure_translation: + Arrange: ω = 0, v = (1,0,0), θ = 2 + Assert: Exp ≈ SE3(I, (2,0,0)) + +exp_pure_rotation: + Arrange: ω = ẑ, v = 0, θ = π/2 + Assert: Exp ≈ SE3(rot(ẑ, 90°), 0) + +exp_log_round_trip: + Arrange: random small twist ξ + Assert: Log(Exp(ξ)) ≈ ξ + +adjoint_of_product_is_product_of_adjoints: + Assert: Adjoint(g1*g2) ≈ Adjoint(g1)·Adjoint(g2) +``` + +## Reference vectors + +- `rot(ẑ, 90°) ⊕ (1,0,0)` applied to `(1,0,0)` ⇒ `(1,1,0)`. +- `Exp(ω = ẑ, v = 0, θ = π/2)` ⇒ rotation-only transform `rot(ẑ, 90°)`. + +## Edge cases + +- `θ → 0` in `Exp`/`Log` ⇒ series fallback, no `sinθ/θ` division blow-up. +- `θ = π` rotation ⇒ axis recovery in `Log` stays well-defined. +- Non-orthonormal `R` fed in ⇒ document the reorthonormalization expectation. +- Twist with zero angular part ⇒ `Adjoint` lower-left `[p]×R` still correct. From 49424ed4c43268cd3716f64b62429098990990a6 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sat, 1 Aug 2026 12:12:07 +0000 Subject: [PATCH 3/3] SE3 transform removed from the list --- roadmap/math/SE3Transform/explanation.md | 34 -------- roadmap/math/SE3Transform/implementation.md | 88 --------------------- roadmap/math/SE3Transform/tests.md | 64 --------------- 3 files changed, 186 deletions(-) delete mode 100644 roadmap/math/SE3Transform/explanation.md delete mode 100644 roadmap/math/SE3Transform/implementation.md delete mode 100644 roadmap/math/SE3Transform/tests.md diff --git a/roadmap/math/SE3Transform/explanation.md b/roadmap/math/SE3Transform/explanation.md deleted file mode 100644 index 4edd992..0000000 --- a/roadmap/math/SE3Transform/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# SE(3) Transform — Overview - -## What it is -`SE(3)` is the group of rigid-body motions in 3D: a rotation plus a translation, stored as a 4×4 -homogeneous matrix `[[R, p],[0, 1]]`. Its "velocity" companions are **twists** (6-vectors pairing -angular and linear velocity) and **wrenches** (moment plus force); the **adjoint** map moves those -6-vectors between coordinate frames. - -## Why it matters (embedded) -This is the algebra every modern manipulator, drone, and SLAM stack is built on. Forward kinematics -chains `SE(3)` products; Jacobians and dynamics are expressed with twists and the adjoint; trajectory -interpolation lives on this group. A compact, correct `SE(3)` type is the foundation the whole -robotics layer reuses. - -## How it works (intuition) -Composing two motions multiplies their homogeneous matrices, which reduces to `R = R₁R₂`, -`p = R₁p₂ + p₁` — no full 4×4 multiply needed. The inverse has the closed form `(Rᵀ, −Rᵀp)`. The -**exponential map** turns a constant twist (a screw axis) applied for a "time" `θ` into the finite -motion it produces — the rigid-body generalization of Rodrigues' rotation formula — and its inverse -**log** recovers the screw from a transform. The **adjoint** is the 6×6 that re-expresses a twist or -wrench in another frame. - -## Key parameters -- **Twist ordering** — `(ω; v)` (angular first) here; the adjoint layout follows this convention. -- **Screw parameter `θ`** — how far along a twist/screw axis the exponential integrates. -- **Orthonormality of `R`** — must be maintained on `SO(3)`; reorthonormalize against drift. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3; R. M. Murray, Z. Li, S. S. Sastry, -*A Mathematical Introduction to Robotic Manipulation* (1994). - -## See also -`Quaternion` (item 18, the `SO(3)` rotation part), `Geometry3D` (`RotationAboutAxis`, -`SkewSymmetric`), `MatrixExponential` (item 29, the `se(3)→SE(3)` analogue), `DenavitHartenberg` (M7). diff --git a/roadmap/math/SE3Transform/implementation.md b/roadmap/math/SE3Transform/implementation.md deleted file mode 100644 index 4e90173..0000000 --- a/roadmap/math/SE3Transform/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# SE(3) Transform — Implementation Pseudocode - -> Roadmap ref: #M6 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SE3: - Matrix3 R # rotation, on SO(3) - Vector3 p # translation - # equivalently the 4×4 homogeneous matrix [[R, p],[0 0 0 1]] - -# spatial 6-vectors reuse Vector: -# Twist = (ω; v) angular over linear velocity -# Wrench = (m; f) moment over force -``` - -## Interface - -``` -SE3() # identity -SE3(Matrix3 R, Vector3 p) -SquareMatrix Homogeneous() # 4×4 form -SE3 operator*(SE3 rhs) # compose transforms; hot path -SE3 Inverse() -Vector3 Apply(Vector3 point) # transform a point; hot path -Matrix Adjoint() # maps twists/wrenches between frames -static SE3 Exp(Vector xi, T theta) # exp: se(3) -> SE(3) -Vector Log() # log: SE(3) -> se(3) -``` - -## Algorithm (pseudocode) - -``` -function multiply(a, b): # OPTIMIZE_FOR_SPEED - return SE3(a.R * b.R, a.R * b.p + a.p) - -function Inverse(): # closed-form rigid inverse (no matrix solve) - return SE3(Transpose(R), -Transpose(R) * p) - -function Apply(point): - return R * point + p - -function Adjoint(): # 6×6: [[R, 0],[ [p]× R, R ]] - upper-left = R - lower-left = SkewSymmetric(p) * R # reuse Geometry3D::SkewSymmetric - lower-right = R - upper-right = 0 - -function Exp(xi = (ω; v), theta): # matrix exponential on se(3) - if VectorNorm(ω) == 0: return SE3(I, v*theta) # pure translation - R = RotationAboutAxis(ω, theta) # Rodrigues (reuse Geometry3D) - G = I*theta + (1-cosθ)*SkewSymmetric(ω) + (θ-sinθ)*SkewSymmetric(ω)² # left Jacobian - return SE3(R, G * v) - -function Log(): # inverse of Exp - (ω, theta) = axisAngleFromRotation(R) # log on SO(3) - v = InverseLeftJacobian(theta) * p - return concat(ω*theta, v*theta) -``` - -## Complexity & memory - -- Compose / inverse / apply: `O(1)` — fixed 3×3 and 3-vector arithmetic, no loops. -- Adjoint / Exp / Log: `O(1)` — a handful of 3×3 products; `Log` adds one `acos`. -- Memory: a 3×3 plus a 3-vector per transform (or one 4×4); all stack-allocated, no heap. - -## Numerical / embedded notes - -- Use the **closed-form rigid inverse** `(Rᵀ, −Rᵀp)`; never run a general 4×4 matrix solve. -- Keep `R` on `SO(3)`: reorthonormalize after long product chains (drift accumulates like quaternions). -- `Exp`/`Log` need `sinθ/θ` and `(1−cosθ)/θ²`; use series expansions near `θ→0` to avoid `0/0`. -- Twist ordering here is `(ω; v)` (angular first); the adjoint block layout must match that choice. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/math/SE3Transform.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `operator*`/`Apply`, and - `extern template class SE3;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/SE3Transform.cpp` → `template class SE3;` -- Test: `numerical/math/test/TestSE3Transform.cpp` -- Doc: `doc/math/SE3Transform.md` (new folder; math currently has no doc pages) -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestSE3Transform.cpp` → `numerical.math_test`. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/math/SE3Transform/tests.md b/roadmap/math/SE3Transform/tests.md deleted file mode 100644 index 0138f94..0000000 --- a/roadmap/math/SE3Transform/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# SE(3) Transform — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSE3 : public ::testing::Test: - SE3 identity - SE3 gAB # rotation 90° about ẑ + translation (1,0,0) -# each case below is a TEST_F(TestSE3, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -identity_apply_is_noop: - Assert: identity.Apply(p) == p - -compose_with_inverse_is_identity: - Arrange: g = gAB - Assert: g * g.Inverse() ≈ Identity - -apply_rotates_then_translates: - Arrange: g = rot(ẑ, 90°) + p = (1,0,0) - Assert: g.Apply((1,0,0)) ≈ (1,1,0) - -inverse_matches_transpose_form: - Assert: Inverse().R == Rᵀ and Inverse().p == -Rᵀp - -homogeneous_matches_block_form: - Assert: Homogeneous() == [[R, p],[0, 0, 0, 1]] - -adjoint_transforms_twist: - Arrange: known twist in frame B - Assert: Adjoint()·ξ matches the hand-computed twist in frame A - -exp_pure_translation: - Arrange: ω = 0, v = (1,0,0), θ = 2 - Assert: Exp ≈ SE3(I, (2,0,0)) - -exp_pure_rotation: - Arrange: ω = ẑ, v = 0, θ = π/2 - Assert: Exp ≈ SE3(rot(ẑ, 90°), 0) - -exp_log_round_trip: - Arrange: random small twist ξ - Assert: Log(Exp(ξ)) ≈ ξ - -adjoint_of_product_is_product_of_adjoints: - Assert: Adjoint(g1*g2) ≈ Adjoint(g1)·Adjoint(g2) -``` - -## Reference vectors - -- `rot(ẑ, 90°) ⊕ (1,0,0)` applied to `(1,0,0)` ⇒ `(1,1,0)`. -- `Exp(ω = ẑ, v = 0, θ = π/2)` ⇒ rotation-only transform `rot(ẑ, 90°)`. - -## Edge cases - -- `θ → 0` in `Exp`/`Log` ⇒ series fallback, no `sinθ/θ` division blow-up. -- `θ = π` rotation ⇒ axis recovery in `Log` stays well-defined. -- Non-orthonormal `R` fed in ⇒ document the reorthonormalization expectation. -- Twist with zero angular part ⇒ `Adjoint` lower-left `[p]×R` still correct.