From bab86c56afa25d73fc74498b9818235071452a13 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 13:33:23 +0000 Subject: [PATCH 1/4] perf!: stop setting optimization options per header Mirrors embedded-pro/numerical-toolbox-cpp#348 and bumps the numerical-toolbox pin to 9b37e08, which carries it. Every robotics header applied an unscoped #pragma GCC optimize("O3", "fast-math"). GCC does not inline across differing optimization options, so toolbox helpers could not be inlined into consumer code, and the unscoped pragma leaked fast-math into every translation unit that included a robotics header. - Drop the pragma from all robotics headers. - AGENTS.md, CLAUDE.md, the Copilot instructions, the agents, the roadmap recipe and every spec's deployment note forbid optimize pragmas/attributes and leave the optimization level and floating-point model to the consumer, per translation unit. BREAKING CHANGE: robotics headers no longer force O3 and fast-math. Consumers that want them pass them for whole translation units, e.g. -O2/-O3 -ffast-math -fno-finite-math-only. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- .github/agents/algo-implementer.agent.md | 2 +- .github/agents/planner.agent.md | 2 +- .github/agents/reviewer.agent.md | 2 +- .github/copilot-instructions.md | 2 +- .../instructions/robotics-cpp.instructions.md | 12 +++------- AGENTS.md | 24 ++++++++++--------- CLAUDE.md | 2 +- CMakeLists.txt | 2 +- roadmap/DEPLOYMENT.md | 3 +-- roadmap/README.md | 3 --- .../implementation.md | 4 ++-- .../ComputedTorqueControl/implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../ImpedanceControl/implementation.md | 2 +- .../OperationalSpaceControl/implementation.md | 4 ++-- .../PdGravityCompensation/implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../ChainDynamicsModel/implementation.md | 2 +- .../implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../FrictionCompensation/implementation.md | 4 ++-- .../GenericJointLink/implementation.md | 2 +- .../MomentumObserver/implementation.md | 2 +- .../AnalyticalIkOpw/implementation.md | 4 ++-- .../ChainPoseKinematics/implementation.md | 4 ++-- .../ContinuumKinematics/implementation.md | 4 ++-- .../DenavitHartenberg/implementation.md | 4 ++-- .../GeometricJacobian/implementation.md | 2 +- .../ManipulabilityIndex/implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../implementation.md | 2 +- .../PoseInverseKinematics/implementation.md | 4 ++-- .../ProductOfExponentials/implementation.md | 4 ++-- .../RedundancyResolution/implementation.md | 4 ++-- .../kinematics/SE3Transform/implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../CubicSplineTrajectory/implementation.md | 4 ++-- .../PolynomialTrajectory/implementation.md | 4 ++-- .../SCurveProfile/implementation.md | 4 ++-- .../implementation.md | 4 ++-- .../TrapezoidalProfile/implementation.md | 4 ++-- .../dynamics/ArticulatedBodyAlgorithm.hpp | 4 ---- robotics/dynamics/EulerLagrangeDynamics.hpp | 4 ---- robotics/dynamics/EulerLagrangeSolver.hpp | 4 ---- robotics/dynamics/NewtonEulerBody.hpp | 4 ---- robotics/dynamics/NewtonEulerSolver.hpp | 4 ---- robotics/dynamics/RecursiveNewtonEuler.hpp | 4 ---- robotics/dynamics/RevoluteJointLink.hpp | 4 ---- robotics/kinematics/ForwardKinematics.hpp | 4 ---- robotics/kinematics/InverseKinematics.hpp | 4 ---- 52 files changed, 83 insertions(+), 127 deletions(-) diff --git a/.github/agents/algo-implementer.agent.md b/.github/agents/algo-implementer.agent.md index 97b5705..35231cf 100644 --- a/.github/agents/algo-implementer.agent.md +++ b/.github/agents/algo-implementer.agent.md @@ -33,5 +33,5 @@ Authoritative rules: `AGENTS.md`. Recipe: `roadmap/DEPLOYMENT.md`. Follow both e - **No comments** (except license/`NOLINT`). Allman braces, brace-init. - **Tests**: `TEST_F` on `float`, `StrictMock` only, anonymous-namespace fixture; implement EXACTLY the spec's cases — no redundant or extra tests. -- **Embedded**: `#pragma GCC optimize` + `OPTIMIZE_FOR_SPEED` on hot paths. +- **Embedded**: no `#pragma GCC optimize`/`optimize` attribute; `OPTIMIZE_FOR_SPEED` (forced inlining) on hot paths. - **Terse**: no preamble/postamble, no plan restatement, no narration; don't re-read files; batch reads. diff --git a/.github/agents/planner.agent.md b/.github/agents/planner.agent.md index b60576c..5c7485b 100644 --- a/.github/agents/planner.agent.md +++ b/.github/agents/planner.agent.md @@ -30,7 +30,7 @@ Canonical rules: `AGENTS.md`. Produce plans only — no code edits. - [ ] No heap; no recursion; tests too - [ ] `template` + `static_assert(std::is_floating_point_v)`; `float` only - [ ] `TEST_F` on `float` — no `TYPED_TEST`; `StrictMock` only; never plain `TEST()` - - [ ] `#pragma GCC optimize` + `OPTIMIZE_FOR_SPEED` on hot paths + - [ ] no `#pragma GCC optimize`/`optimize` attribute; `OPTIMIZE_FOR_SPEED` on hot paths - [ ] `doc/` update planned **Terse**: no preamble/postamble, no plan restatement; don't re-read files; batch reads. diff --git a/.github/agents/reviewer.agent.md b/.github/agents/reviewer.agent.md index ead8416..4f9aedc 100644 --- a/.github/agents/reviewer.agent.md +++ b/.github/agents/reviewer.agent.md @@ -43,7 +43,7 @@ End with totals + verdict: APPROVE / REQUEST CHANGES. - [ ] `extern template` guarded by `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. **Embedded optimizations (WARNING)** -- [ ] `#pragma GCC optimize("O3","fast-math")` after `#pragma once` in algorithm headers. +- [ ] No `#pragma GCC optimize` and no `optimize` attribute anywhere (they stop GCC inlining across the boundary). - [ ] `OPTIMIZE_FOR_SPEED` on `Filter/Compute/Update/Solve/Step`. **Namespaces (WARNING)** diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md index 7306eab..56f8ed7 100644 --- a/.github/copilot-instructions.md +++ b/.github/copilot-instructions.md @@ -6,7 +6,7 @@ Code-file specifics: `.github/instructions/` (applyTo-scoped). Deployment recipe Essentials (full detail in AGENTS.md): - **No heap** — bounded containers / `std::array` / `std::optional`; no recursion; tests too. - **Float-only** — generic `template` + `static_assert(std::is_floating_point_v)`; instantiate/test `float`; no Q15/Q31. -- **Embedded** — `#pragma GCC optimize("O3","fast-math")` + `OPTIMIZE_FOR_SPEED` on hot paths. +- **Embedded** — no `#pragma GCC optimize` / `optimize` attribute (they block inlining); the consumer sets optimization flags per TU; `OPTIMIZE_FOR_SPEED` (forced inlining) on hot paths. - **No comments** (except license/NOLINT). Allman braces, brace-init, PascalCase/camelCase. - **Tests** — `TEST_F` on `float`, `StrictMock` only, never plain `TEST()`, no redundant cases. - **No exceptions** — `std::optional`/status enums; interfaces `virtual ~I() = default`. diff --git a/.github/instructions/robotics-cpp.instructions.md b/.github/instructions/robotics-cpp.instructions.md index 809c3b4..c0cea53 100644 --- a/.github/instructions/robotics-cpp.instructions.md +++ b/.github/instructions/robotics-cpp.instructions.md @@ -29,17 +29,11 @@ Use `std::numbers::pi_v` — never hardcode `3.14159265f`. ## Embedded Optimizations -Every algorithm header MUST include: +Never use `#pragma GCC optimize` or the `optimize` attribute. GCC does not inline a callee whose optimization options differ from its caller's, so options set per header or per function turn every small helper on a hot path into an out-of-line call. Optimization level and floating-point flags are the consumer's, applied to whole translation units (embedded: `-O2`/`-O3` with `-ffast-math -fno-finite-math-only`). -```cpp -#pragma once +Apply `OPTIMIZE_FOR_SPEED` (from `numerical/math/CompilerOptimizations.hpp`) on hot-path methods: `Compute()`, `Filter()`, `Calculate()`, `Solve()`, `Update()`, `Step()`. It forces inlining when `NumericalToolbox_ENABLE_OPTIMIZATIONS` is defined and expands to nothing otherwise. -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif -``` - -Apply `OPTIMIZE_FOR_SPEED` (from `numerical/math/CompilerOptimizations.hpp`) on hot-path methods: `Compute()`, `Filter()`, `Calculate()`, `Solve()`, `Update()`, `Step()`. +Use `math::IsFinite` for finiteness checks: a consumer may still build with `-ffinite-math-only`. ## Naming diff --git a/AGENTS.md b/AGENTS.md index e691d7c..47822fe 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -25,17 +25,19 @@ and included as `numerical//…`. `infra::BoundedDeque::WithMaxSize`, `infra::BoundedList::WithMaxSize`, `std::array`, `std::optional`. Stack/static only. No recursion. **Tests too.** -## Embedded optimizations (algorithm headers) - -```cpp -#pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif -#include "numerical/math/CompilerOptimizations.hpp" -``` - -`OPTIMIZE_FOR_SPEED` on hot paths (`Filter/Compute/Update/Solve/Step`). Pure interfaces exempt. +## Embedded optimizations + +- **No `#pragma GCC optimize` and no `optimize` attribute**, in headers or sources. GCC does not + inline a callee whose optimization options differ from its caller's, so options set per header or + per function turn every small helper on a hot path into an out-of-line call; an unscoped pragma in + a header also leaks fast-math into every including translation unit. +- The optimization level and floating-point model belong to the consumer and apply to whole + translation units. For embedded targets: `-O2` or `-O3` with `-ffast-math -fno-finite-math-only`. +- `OPTIMIZE_FOR_SPEED` (from `numerical/math/CompilerOptimizations.hpp`) on hot paths + (`Filter/Compute/Update/Solve/Step`). Pure interfaces exempt. With + `NumericalToolbox_ENABLE_OPTIMIZATIONS` it forces inlining (`always_inline`, `hot`, `inline`); + without it, it expands to nothing. +- Use `math::IsFinite` for finiteness checks: a consumer may still build with `-ffinite-math-only`. ## Style diff --git a/CLAUDE.md b/CLAUDE.md index 990e0f3..15a247e 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -7,7 +7,7 @@ Shared numerical primitives come from numerical-toolbox-cpp via FetchContent (`n Essentials (full detail in AGENTS.md): - **No heap** — bounded containers / `std::array` / `std::optional`; no recursion; tests too. - **Float-only** — generic `template` + `static_assert(std::is_floating_point_v)`; instantiate/test `float`; no Q15/Q31. -- **Embedded** — `#pragma GCC optimize("O3","fast-math")` + `OPTIMIZE_FOR_SPEED` on hot paths. +- **Embedded** — no `#pragma GCC optimize` / `optimize` attribute (they block inlining); the consumer sets optimization flags per TU; `OPTIMIZE_FOR_SPEED` (forced inlining) on hot paths. - **No comments** (except license/NOLINT). Allman braces, brace-init, PascalCase/camelCase. - **Tests** — `TEST_F` on `float`, `StrictMock` only, never plain `TEST()`, no redundant cases. - **No exceptions** — `std::optional`/status enums; interfaces `virtual ~I() = default`. diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d04436..3929c7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG 69ff13e719895b74b167c4742130442c6b96a857 # main + GIT_TAG 9b37e0885d31a9e444739a5cfaa3bc004aad9d37 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index 40c2385..940ce4f 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -4,8 +4,7 @@ Turn one `roadmap///` spec into shipped code. Rules: `../AGENTS.md Read the spec's three files first (`implementation.md`, `tests.md`, `explanation.md`), then: 1. **Header** `robotics//.hpp` - - `#pragma once` → `#pragma GCC optimize("O3","fast-math")` (GCC/Clang guard) → - `#include "numerical/math/CompilerOptimizations.hpp"`. + - `#pragma once` → `#include "numerical/math/CompilerOptimizations.hpp"`; no `#pragma GCC optimize`. - `template` with `static_assert(std::is_floating_point_v, " supports floating-point types");`. - Implement per `implementation.md`; `OPTIMIZE_FOR_SPEED` on the hot path(s). diff --git a/roadmap/README.md b/roadmap/README.md index e45a69a..1764222 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -43,9 +43,6 @@ Header shape (mirroring existing components such as `ForwardKinematics.hpp`): ```cpp #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif #include "numerical/math/CompilerOptimizations.hpp" namespace diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md index fc71626..7ca1fed 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md @@ -94,8 +94,8 @@ function Distribute(w, pose): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/controllers/manipulator/CableTensionDistribution.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Distribute`, and +- Header: `robotics/controllers/manipulator/CableTensionDistribution.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Distribute`, and `extern template class CableTensionDistribution;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/CableTensionDistribution.cpp` → diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md index 04f5423..534c6d1 100644 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md +++ b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md @@ -61,8 +61,8 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and +- Header: `robotics/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and `extern template class ComputedTorqueControl;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/ComputedTorqueControl.cpp` → diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md index e797942..aae8b94 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md @@ -84,8 +84,8 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, fMeasured, fd): # OPTIMIZE ## Deployment -- Header: `robotics/controllers/manipulator/HybridPositionForceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and +- Header: `robotics/controllers/manipulator/HybridPositionForceControl.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and `extern template class HybridPositionForceControl;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/HybridPositionForceControl.cpp` → diff --git a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md index afaf853..28fe2ee 100644 --- a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md +++ b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md @@ -106,7 +106,7 @@ function ComputeTorqueWithInertiaShaping(q, qDot, desiredPose, xdDot, xdDdot, fE ## Deployment - Header: `robotics/controllers/manipulator/ImpedanceControl.hpp` (+ `TaskError.hpp`, shared with M18/M19; - deploy it with whichever lands first) — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, + deploy it with whichever lands first) — `#pragma once`, `OPTIMIZE_FOR_SPEED` on both torque methods, and `extern template class ImpedanceControl;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/ImpedanceControl.cpp` → diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md index 490ff8f..fbff83f 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md @@ -80,8 +80,8 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, tauSecondary): # O ## Deployment -- Header: `robotics/controllers/manipulator/OperationalSpaceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and +- Header: `robotics/controllers/manipulator/OperationalSpaceControl.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and `extern template class OperationalSpaceControl;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/OperationalSpaceControl.cpp` → diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md index 3d84042..59a6f26 100644 --- a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md +++ b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md @@ -59,8 +59,8 @@ function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/controllers/manipulator/PdGravityCompensation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and +- Header: `robotics/controllers/manipulator/PdGravityCompensation.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and `extern template class PdGravityCompensation;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/PdGravityCompensation.cpp` → diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md index 8db7605..f67bc96 100644 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md @@ -78,8 +78,8 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and +- Header: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and `extern template class SlotineLiAdaptiveControl;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD` (`20 = 10·N` matches `ChainInertialRegressor`). - Coverage: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp` → diff --git a/roadmap/dynamics/ChainDynamicsModel/implementation.md b/roadmap/dynamics/ChainDynamicsModel/implementation.md index 9677f0b..9e68fce 100644 --- a/roadmap/dynamics/ChainDynamicsModel/implementation.md +++ b/roadmap/dynamics/ChainDynamicsModel/implementation.md @@ -68,7 +68,7 @@ ComputeInverseDynamics(q,q̇,q̈) = rnea.InverseDynamics(links, q, q̇, q̈, gra ## Deployment - Headers: `robotics/dynamics/InverseDynamicsModel.hpp` (pure interface) and - `robotics/dynamics/ChainDynamicsModel.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, + `robotics/dynamics/ChainDynamicsModel.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `ComputeInverseDynamics`, and `extern template class ChainDynamicsModel;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/ChainDynamicsModel.cpp` → the same instantiations. diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md index d92f754..1e154ab 100644 --- a/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md +++ b/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md @@ -62,8 +62,8 @@ function Compute(links, q): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/dynamics/CompositeRigidBodyAlgorithm.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and +- Header: `robotics/dynamics/CompositeRigidBodyAlgorithm.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class CompositeRigidBodyAlgorithm;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/CompositeRigidBodyAlgorithm.cpp` → the same instantiations. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md index da7beef..e6f155f 100644 --- a/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md +++ b/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md @@ -90,8 +90,8 @@ function ChainInertialRegressor.Compute(q, q̇, q̇r, q̈r): # O(N²) ## Deployment - Headers: `robotics/dynamics/ModifiedRecursiveNewtonEuler.hpp`, `CoriolisMatrix.hpp`, - `InertialRegressor.hpp` (interface), `ChainInertialRegressor.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on the passes, and `extern template` + `InertialRegressor.hpp` (interface), `ChainInertialRegressor.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on the passes, and `extern template` declarations for `` and `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: matching `.cpp` files with the same instantiations. - Tests: `robotics/dynamics/test/TestCoriolisMatrix.cpp`, `TestChainInertialRegressor.cpp` diff --git a/roadmap/dynamics/DynamicParameterIdentification/implementation.md b/roadmap/dynamics/DynamicParameterIdentification/implementation.md index 0e15732..b6360e5 100644 --- a/roadmap/dynamics/DynamicParameterIdentification/implementation.md +++ b/roadmap/dynamics/DynamicParameterIdentification/implementation.md @@ -79,8 +79,8 @@ function Solve(tol): ## Deployment -- Header: `robotics/dynamics/DynamicParameterIdentification.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `extern template class DynamicParameterIdentification;` +- Header: `robotics/dynamics/DynamicParameterIdentification.hpp` — `#pragma once`, + `extern template class DynamicParameterIdentification;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/DynamicParameterIdentification.cpp` → the same instantiation. - Test: `robotics/dynamics/test/TestDynamicParameterIdentification.cpp` diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md index 56c8de8..e2bdd78 100644 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md @@ -61,8 +61,8 @@ function Step(x, τ, dt): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/dynamics/ForwardDynamicsIntegrator.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Step`, `extern template` for +- Header: `robotics/dynamics/ForwardDynamicsIntegrator.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Step`, `extern template` for `` and `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/ForwardDynamicsIntegrator.cpp` → the same instantiations. diff --git a/roadmap/dynamics/FrictionCompensation/implementation.md b/roadmap/dynamics/FrictionCompensation/implementation.md index cb43e39..bfeeeaa 100644 --- a/roadmap/dynamics/FrictionCompensation/implementation.md +++ b/roadmap/dynamics/FrictionCompensation/implementation.md @@ -72,8 +72,8 @@ function Compute(qDot): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/dynamics/FrictionCompensation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorque`, and +- Header: `robotics/dynamics/FrictionCompensation.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorque`, and `extern template class FrictionCompensation;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/FrictionCompensation.cpp` → `template class FrictionCompensation;` and ``. diff --git a/roadmap/dynamics/GenericJointLink/implementation.md b/roadmap/dynamics/GenericJointLink/implementation.md index cc23f53..db03819 100644 --- a/roadmap/dynamics/GenericJointLink/implementation.md +++ b/roadmap/dynamics/GenericJointLink/implementation.md @@ -78,7 +78,7 @@ Limits: consumed by IK clamping (M13), trajectory plan ## Deployment - Header: `robotics/dynamics/JointLink.hpp` (and `RevoluteJointLink.hpp` becomes the alias) — - `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `extern template struct JointLink;` + `#pragma once`, `extern template struct JointLink;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/JointLink.cpp` → `template struct JointLink;` - Tests: `robotics/dynamics/test/TestJointLink.cpp` plus prismatic cases in the RNEA/ABA/FK tests. diff --git a/roadmap/dynamics/MomentumObserver/implementation.md b/roadmap/dynamics/MomentumObserver/implementation.md index 61c71da..809cdb8 100644 --- a/roadmap/dynamics/MomentumObserver/implementation.md +++ b/roadmap/dynamics/MomentumObserver/implementation.md @@ -57,7 +57,7 @@ function Update(q, q̇, τ, dt): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/dynamics/MomentumObserver.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, +- Header: `robotics/dynamics/MomentumObserver.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Update`, `extern template class MomentumObserver;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/MomentumObserver.cpp` → the same instantiations. diff --git a/roadmap/kinematics/AnalyticalIkOpw/implementation.md b/roadmap/kinematics/AnalyticalIkOpw/implementation.md index dc4d931..768fe58 100644 --- a/roadmap/kinematics/AnalyticalIkOpw/implementation.md +++ b/roadmap/kinematics/AnalyticalIkOpw/implementation.md @@ -113,8 +113,8 @@ function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each wrapp ## Deployment -- Header: `robotics/kinematics/AnalyticalIkOpw.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and +- Header: `robotics/kinematics/AnalyticalIkOpw.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Solve`, and `extern template class AnalyticalIkOpw;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/AnalyticalIkOpw.cpp` → `template class AnalyticalIkOpw;` - Test: `robotics/kinematics/test/TestAnalyticalIkOpw.cpp` diff --git a/roadmap/kinematics/ChainPoseKinematics/implementation.md b/roadmap/kinematics/ChainPoseKinematics/implementation.md index f148dd1..6a42403 100644 --- a/roadmap/kinematics/ChainPoseKinematics/implementation.md +++ b/roadmap/kinematics/ChainPoseKinematics/implementation.md @@ -64,8 +64,8 @@ function Compute(links, q): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/kinematics/ChainPoseKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and +- Header: `robotics/kinematics/ChainPoseKinematics.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class ChainPoseKinematics;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/ChainPoseKinematics.cpp` → the same three instantiations. diff --git a/roadmap/kinematics/ContinuumKinematics/implementation.md b/roadmap/kinematics/ContinuumKinematics/implementation.md index 0c72917..88c45c8 100644 --- a/roadmap/kinematics/ContinuumKinematics/implementation.md +++ b/roadmap/kinematics/ContinuumKinematics/implementation.md @@ -73,8 +73,8 @@ function InverseSection(tip): # tip position in the section ba ## Deployment -- Header: `robotics/kinematics/ContinuumKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `SectionTransform`/`Forward`, and +- Header: `robotics/kinematics/ContinuumKinematics.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `SectionTransform`/`Forward`, and `extern template class ContinuumKinematics;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/ContinuumKinematics.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestContinuumKinematics.cpp` diff --git a/roadmap/kinematics/DenavitHartenberg/implementation.md b/roadmap/kinematics/DenavitHartenberg/implementation.md index edc02a0..e564f03 100644 --- a/roadmap/kinematics/DenavitHartenberg/implementation.md +++ b/roadmap/kinematics/DenavitHartenberg/implementation.md @@ -105,8 +105,8 @@ function Frames(q): ## Deployment -- Header: `robotics/kinematics/DenavitHartenberg.hpp` (+ `DhTaskJacobian.hpp`) — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `LinkTransform`/`Forward`, and +- Header: `robotics/kinematics/DenavitHartenberg.hpp` (+ `DhTaskJacobian.hpp`) — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `LinkTransform`/`Forward`, and `extern template class DenavitHartenberg;` / `` / `` plus `extern template class DhTaskJacobian;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/DenavitHartenberg.cpp` → the same instantiations. diff --git a/roadmap/kinematics/GeometricJacobian/implementation.md b/roadmap/kinematics/GeometricJacobian/implementation.md index 1f840fa..55ff2df 100644 --- a/roadmap/kinematics/GeometricJacobian/implementation.md +++ b/roadmap/kinematics/GeometricJacobian/implementation.md @@ -95,7 +95,7 @@ function JointTorques(J, w): # statics: τ = Jᵀ·w, w = (f; ## Deployment - Header: `robotics/kinematics/GeometricJacobian.hpp` (+ `JacobianProvider.hpp`, `ChainTaskJacobian.hpp`) — - `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on + `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Compute`/`BiasAcceleration`, and `extern template class GeometricJacobian;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/GeometricJacobian.cpp` → the same instantiations. diff --git a/roadmap/kinematics/ManipulabilityIndex/implementation.md b/roadmap/kinematics/ManipulabilityIndex/implementation.md index 91c39fa..6ce4add 100644 --- a/roadmap/kinematics/ManipulabilityIndex/implementation.md +++ b/roadmap/kinematics/ManipulabilityIndex/implementation.md @@ -82,8 +82,8 @@ function NearSingular(q, eps): ## Deployment -- Header: `robotics/kinematics/ManipulabilityIndex.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and +- Header: `robotics/kinematics/ManipulabilityIndex.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class ManipulabilityIndex;` / `` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/ManipulabilityIndex.cpp` → the same instantiations. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md index 265f357..7738fa9 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md @@ -71,8 +71,8 @@ function WheelSpeeds(v, ω): ## Deployment -- Header: `robotics/kinematics/MobileManipulatorKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and +- Header: `robotics/kinematics/MobileManipulatorKinematics.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class MobileManipulatorKinematics;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/MobileManipulatorKinematics.cpp` → the same instantiations. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md index 9753db9..a30ccdc 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md @@ -120,7 +120,7 @@ function Forward(θ): # three-sphere intersection, c ## Deployment - Headers: `robotics/kinematics/StewartGoughKinematics.hpp`, `robotics/kinematics/DeltaKinematics.hpp` — - `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Inverse`, and + `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Inverse`, and `extern template class StewartGoughKinematics;` / `extern template class DeltaKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/StewartGoughKinematics.cpp`, `robotics/kinematics/DeltaKinematics.cpp` diff --git a/roadmap/kinematics/PoseInverseKinematics/implementation.md b/roadmap/kinematics/PoseInverseKinematics/implementation.md index 9ac657c..caf56b1 100644 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ b/roadmap/kinematics/PoseInverseKinematics/implementation.md @@ -96,8 +96,8 @@ function Damping(A0): # Nakamura–Hanafusa / Chiaveri ## Deployment -- Header: `robotics/kinematics/PoseInverseKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and +- Header: `robotics/kinematics/PoseInverseKinematics.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Solve`, and `extern template class PoseInverseKinematics;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/PoseInverseKinematics.cpp` → the same instantiations. diff --git a/roadmap/kinematics/ProductOfExponentials/implementation.md b/roadmap/kinematics/ProductOfExponentials/implementation.md index 81e01bb..8e23172 100644 --- a/roadmap/kinematics/ProductOfExponentials/implementation.md +++ b/roadmap/kinematics/ProductOfExponentials/implementation.md @@ -80,8 +80,8 @@ function SpaceToGeometric(Js, pTool): # J_geo = [[I, −Skew(pTool)], ## Deployment -- Header: `robotics/kinematics/ProductOfExponentials.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and +- Header: `robotics/kinematics/ProductOfExponentials.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class ProductOfExponentials;` / `` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/ProductOfExponentials.cpp` → the same instantiations. diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md index 0f07afb..b83be78 100644 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ b/roadmap/kinematics/RedundancyResolution/implementation.md @@ -71,8 +71,8 @@ function NullSpaceProjector(q): Factor(q); return I_Dof − Σ_{i;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/RedundancyResolution.cpp` → the same instantiations. diff --git a/roadmap/kinematics/SE3Transform/implementation.md b/roadmap/kinematics/SE3Transform/implementation.md index 2b92c00..3da3ea1 100644 --- a/roadmap/kinematics/SE3Transform/implementation.md +++ b/roadmap/kinematics/SE3Transform/implementation.md @@ -101,8 +101,8 @@ function PoseError(target, current): # base-frame error, matches the geom ## Deployment -- Header: `robotics/kinematics/SE3Transform.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Exp`/`operator*`, and +- Header: `robotics/kinematics/SE3Transform.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Exp`/`operator*`, and `extern template struct SE3Transform;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/kinematics/SE3Transform.cpp` → `template struct SE3Transform;` - Test: `robotics/kinematics/test/TestSE3Transform.cpp` diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md index 794f3d3..4dcd070 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md @@ -87,8 +87,8 @@ including the sign-flipped (obtuse) case. ## Deployment -- Header: `robotics/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and +- Header: `robotics/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class CartesianSlerpInterpolation>;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp`, `robotics/kinematics/SE3Transform.hpp` (M6) and diff --git a/roadmap/trajectory/CubicSplineTrajectory/implementation.md b/roadmap/trajectory/CubicSplineTrajectory/implementation.md index 9925ad1..77c596e 100644 --- a/roadmap/trajectory/CubicSplineTrajectory/implementation.md +++ b/roadmap/trajectory/CubicSplineTrajectory/implementation.md @@ -108,8 +108,8 @@ knots ⇒ exactly the `PolynomialTrajectory` cubic. ## Deployment -- Header: `robotics/trajectory/CubicSplineTrajectory.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and +- Header: `robotics/trajectory/CubicSplineTrajectory.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class CubicSplineTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). - Coverage: `robotics/trajectory/CubicSplineTrajectory.cpp` → `template class CubicSplineTrajectory;` diff --git a/roadmap/trajectory/PolynomialTrajectory/implementation.md b/roadmap/trajectory/PolynomialTrajectory/implementation.md index b91b033..9185d70 100644 --- a/roadmap/trajectory/PolynomialTrajectory/implementation.md +++ b/roadmap/trajectory/PolynomialTrajectory/implementation.md @@ -98,8 +98,8 @@ function Sample(t): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/trajectory/PolynomialTrajectory.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and +- Header: `robotics/trajectory/PolynomialTrajectory.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class PolynomialTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it from the block above if this is the first trajectory item; header-only aggregates, no coverage `.cpp`). diff --git a/roadmap/trajectory/SCurveProfile/implementation.md b/roadmap/trajectory/SCurveProfile/implementation.md index f780a00..2e4b715 100644 --- a/roadmap/trajectory/SCurveProfile/implementation.md +++ b/roadmap/trajectory/SCurveProfile/implementation.md @@ -105,8 +105,8 @@ negative (`Ta − 2Tj = vMax/aMax − aMax/jMax ≥ 0` in step 1a; the step-2 te ## Deployment -- Header: `robotics/trajectory/SCurveProfile.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and +- Header: `robotics/trajectory/SCurveProfile.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class SCurveProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). - Coverage: `robotics/trajectory/SCurveProfile.cpp` → `template class SCurveProfile;` diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md index 454d5a9..033a49d 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md @@ -173,8 +173,8 @@ optima `min x` / `max x` without an iterative solver. ## Deployment - Header: `robotics/trajectory/TimeOptimalPathParameterization.hpp` (interfaces `PathGeometry`, - `JointLimits`, the `InverseDynamicsJointLimits` adapter and the planner) — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and + `JointLimits`, the `InverseDynamicsJointLimits` adapter and the planner) — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class TimeOptimalPathParameterization;` / `extern template class InverseDynamicsJointLimits;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp` and `robotics/dynamics/InverseDynamicsModel.hpp` (M29); diff --git a/roadmap/trajectory/TrapezoidalProfile/implementation.md b/roadmap/trajectory/TrapezoidalProfile/implementation.md index 4e63412..c601ecd 100644 --- a/roadmap/trajectory/TrapezoidalProfile/implementation.md +++ b/roadmap/trajectory/TrapezoidalProfile/implementation.md @@ -92,8 +92,8 @@ function Sample(t): # OPTIMIZE_FOR_SPEED ## Deployment -- Header: `robotics/trajectory/TrapezoidalProfile.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and +- Header: `robotics/trajectory/TrapezoidalProfile.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class TrapezoidalProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). - Coverage: `robotics/trajectory/TrapezoidalProfile.cpp` → `template class TrapezoidalProfile;` diff --git a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp index 3d99188..01ebda4 100644 --- a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp +++ b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "infra/util/ReallyAssert.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" diff --git a/robotics/dynamics/EulerLagrangeDynamics.hpp b/robotics/dynamics/EulerLagrangeDynamics.hpp index 97cbc1d..5819d22 100644 --- a/robotics/dynamics/EulerLagrangeDynamics.hpp +++ b/robotics/dynamics/EulerLagrangeDynamics.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "numerical/math/Matrix.hpp" namespace dynamics diff --git a/robotics/dynamics/EulerLagrangeSolver.hpp b/robotics/dynamics/EulerLagrangeSolver.hpp index 7a80a54..3794466 100644 --- a/robotics/dynamics/EulerLagrangeSolver.hpp +++ b/robotics/dynamics/EulerLagrangeSolver.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "infra/util/ReallyAssert.hpp" #include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" diff --git a/robotics/dynamics/NewtonEulerBody.hpp b/robotics/dynamics/NewtonEulerBody.hpp index 700b9af..295ec7f 100644 --- a/robotics/dynamics/NewtonEulerBody.hpp +++ b/robotics/dynamics/NewtonEulerBody.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "numerical/math/Matrix.hpp" namespace dynamics diff --git a/robotics/dynamics/NewtonEulerSolver.hpp b/robotics/dynamics/NewtonEulerSolver.hpp index 89ad7ec..6f87a02 100644 --- a/robotics/dynamics/NewtonEulerSolver.hpp +++ b/robotics/dynamics/NewtonEulerSolver.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "infra/util/ReallyAssert.hpp" #include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" diff --git a/robotics/dynamics/RecursiveNewtonEuler.hpp b/robotics/dynamics/RecursiveNewtonEuler.hpp index e02cd7c..f5639f4 100644 --- a/robotics/dynamics/RecursiveNewtonEuler.hpp +++ b/robotics/dynamics/RecursiveNewtonEuler.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" diff --git a/robotics/dynamics/RevoluteJointLink.hpp b/robotics/dynamics/RevoluteJointLink.hpp index 9226d62..3719d53 100644 --- a/robotics/dynamics/RevoluteJointLink.hpp +++ b/robotics/dynamics/RevoluteJointLink.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" #include diff --git a/robotics/kinematics/ForwardKinematics.hpp b/robotics/kinematics/ForwardKinematics.hpp index a8a8bbb..743391f 100644 --- a/robotics/kinematics/ForwardKinematics.hpp +++ b/robotics/kinematics/ForwardKinematics.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" diff --git a/robotics/kinematics/InverseKinematics.hpp b/robotics/kinematics/InverseKinematics.hpp index bfc7af8..a2a0719 100644 --- a/robotics/kinematics/InverseKinematics.hpp +++ b/robotics/kinematics/InverseKinematics.hpp @@ -1,9 +1,5 @@ #pragma once -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - #include "infra/util/ReallyAssert.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" From e585d8ee4c911079260bfc7130e7a24a992e25e1 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 13:38:35 +0000 Subject: [PATCH 2/4] docs(roadmap): factor wide Jacobians directly in M11 and M14 numerical-toolbox's SingularValueDecomposition now accepts Rows < Cols, so ManipulabilityIndex (M11) and RedundancyResolution (M14) decompose the task Jacobian itself instead of its transpose and no longer swap the singular-vector factors by hand. Both need a numerical-toolbox pin that includes the wide-matrix change. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- .../ManipulabilityIndex/implementation.md | 10 ++++--- .../RedundancyResolution/implementation.md | 28 ++++++++++--------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/roadmap/kinematics/ManipulabilityIndex/implementation.md b/roadmap/kinematics/ManipulabilityIndex/implementation.md index 6ce4add..f8a86aa 100644 --- a/roadmap/kinematics/ManipulabilityIndex/implementation.md +++ b/roadmap/kinematics/ManipulabilityIndex/implementation.md @@ -43,8 +43,8 @@ function ComputeRotational(q): # TaskDim = 6 function EllipsoidAxes(q): B = jacobian.Jacobian(q).rows(0 .. Rows−1) - svd = solvers::SingularValueDecomposition - svd.Decompose(Rows ≥ Dof ? B : Bᵀ) # upstream requires Rows ≥ Cols; σ(Bᵀ) = σ(B) + svd = solvers::SingularValueDecomposition # tall or wide; Axes = SingularValueCount + svd.Decompose(B) return svd.SingularValues() # descending; ∏ σᵢ = GramRoot(B) function ConditionNumber(q): @@ -58,7 +58,8 @@ function NearSingular(q, eps): ## Complexity & memory - `Compute`: `O(Rows·Dof·min(Rows, Dof))` for the Gram product + `O(min(Rows, Dof)³)` LU. -- `EllipsoidAxes` / `ConditionNumber`: one Golub–Kahan SVD of a `max × min` matrix. +- `EllipsoidAxes` / `ConditionNumber`: one Golub–Kahan SVD of the `Rows × Dof` block (a wide block is + factored through its transpose upstream, at the same cost). - Memory: one `TaskDim×Dof` Jacobian, one Gram matrix or SVD factors; stack only. ## Numerical / embedded notes @@ -91,5 +92,6 @@ function NearSingular(q, eps): - Doc: `doc/kinematics/ManipulabilityIndex.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestManipulabilityIndex.cpp` → the `_test` target. -- Depends on: M8 (`JacobianProvider`); upstream `solvers::SingularValueDecomposition`, `solvers::LuDecomposition`. +- Depends on: M8 (`JacobianProvider`); upstream `solvers::SingularValueDecomposition` (wide-matrix + support; needs a numerical-toolbox pin that includes it), `solvers::LuDecomposition`. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md index b83be78..492b551 100644 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ b/roadmap/kinematics/RedundancyResolution/implementation.md @@ -10,7 +10,7 @@ class RedundancyResolution: # instantiated fo const JacobianProvider& jacobian # M8 seam; TaskDim 3 (position) or 6 (pose) T damping # λ — primary term only T rankTolerance # relative: σᵢ > rankTolerance·σ₀ counts toward rank - solvers::SingularValueDecomposition svd # of Jᵀ (upstream needs Rows ≥ Cols) + solvers::SingularValueDecomposition svd # of J (wide: U TaskDim×TaskDim, V Dof×TaskDim) ``` ## Interface @@ -28,8 +28,8 @@ Matrix NullSpaceProjector(const JointVector& q) # P = ```cpp function Factor(q): J = jacobian.Jacobian(q) # TaskDim × Dof - svd.Decompose(Jᵀ) # Jᵀ = U·Σ·Vᵀ ⇒ J = V·Σ·Uᵀ - # uᵢ = column i of U (Dof): right singular vectors of J; vᵢ = column i of V (TaskDim) + svd.Decompose(J) # J = U·Σ·Vᵀ + # uᵢ = column i of U (TaskDim): left singular vectors; vᵢ = column i of V (Dof): right singular vectors τ = rankTolerance · σ₀; r = #{ σᵢ > τ } function Gain(σ): # damped inverse of one singular value @@ -37,28 +37,29 @@ function Gain(σ): # damped inverse of one singula function Resolve(q, ẋ, q̇₀): # OPTIMIZE_FOR_SPEED Factor(q) - q̇ = Σᵢ Gain(σᵢ)·(vᵢ·ẋ)·uᵢ # primary: J⁺_λ·ẋ, lies in the row space of J - q̇ = q̇ + q̇₀ − Σ_{i 0` the primary task is met only + `P² = P`, `Pᵀ = P`, `J·P = Σ_{σᵢ ≤ τ} σᵢuᵢvᵢᵀ` (zero for exact rank loss, rounding-level at full rank). +- **What the tool sees:** `J·q̇ = Σᵢ σᵢ·Gain(σᵢ)·uᵢuᵢᵀ·ẋ`. With `λ > 0` the primary task is met only approximately: `‖J·q̇ − ẋ‖ ≤ λ²/(σ_min² + λ²)·‖ẋ‖` for `ẋ` in the range of `J`. The secondary term adds nothing to the task velocity regardless of `λ` or `‖q̇₀‖`. - The primary term lies in the row space (`P·J⁺_λ·ẋ = 0`), so it is the minimum-norm solution. @@ -80,5 +81,6 @@ function NullSpaceProjector(q): Factor(q); return I_Dof − Σ_{i Date: Fri, 23 Oct 2026 13:42:13 +0000 Subject: [PATCH 3/4] docs(roadmap): take SE(3) from numerical-toolbox (NT53) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The SE(3) spec existed twice: roadmap/kinematics/SE3Transform here and roadmap/math/SE3Transform in numerical-toolbox-cpp, with opposite 6-vector orderings and the same id (M6). SE(3) is generic, so the upstream spec is now the only one; it adopts this repo's linear-first convention, twists (v; ω) and wrenches (f; n), and its PoseError. - Delete roadmap/kinematics/SE3Transform and the M6 row. - Specs use math::SE3Transform / math::Vector6 from numerical/math/SE3Transform.hpp and depend on NT53. - ROADMAP.md gains an "Upstream prerequisites" table: generic items are referenced as NT, numerical-toolbox's master-list number. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- AGENTS.md | 2 +- ROADMAP.md | 51 ++++---- roadmap/README.md | 8 +- .../CableTensionDistribution/explanation.md | 2 +- .../implementation.md | 8 +- .../implementation.md | 4 +- .../ImpedanceControl/implementation.md | 14 +-- .../OperationalSpaceControl/implementation.md | 4 +- .../RneaExternalWrench/implementation.md | 2 +- .../kinematics/AnalyticalIkOpw/explanation.md | 2 +- .../AnalyticalIkOpw/implementation.md | 2 +- .../ChainPoseKinematics/explanation.md | 2 +- .../ChainPoseKinematics/implementation.md | 4 +- .../ContinuumKinematics/explanation.md | 2 +- .../ContinuumKinematics/implementation.md | 4 +- .../DenavitHartenberg/explanation.md | 2 +- .../DenavitHartenberg/implementation.md | 4 +- .../GeometricJacobian/implementation.md | 4 +- .../explanation.md | 2 +- .../implementation.md | 6 +- .../explanation.md | 2 +- .../implementation.md | 4 +- .../PoseInverseKinematics/explanation.md | 2 +- .../PoseInverseKinematics/implementation.md | 6 +- .../kinematics/PoseInverseKinematics/tests.md | 4 +- .../ProductOfExponentials/explanation.md | 2 +- .../ProductOfExponentials/implementation.md | 8 +- .../kinematics/SE3Transform/explanation.md | 36 ------ .../kinematics/SE3Transform/implementation.md | 112 ------------------ roadmap/kinematics/SE3Transform/tests.md | 50 -------- .../explanation.md | 2 +- .../implementation.md | 14 +-- .../CartesianSlerpInterpolation/tests.md | 4 +- 33 files changed, 94 insertions(+), 281 deletions(-) delete mode 100644 roadmap/kinematics/SE3Transform/explanation.md delete mode 100644 roadmap/kinematics/SE3Transform/implementation.md delete mode 100644 roadmap/kinematics/SE3Transform/tests.md diff --git a/AGENTS.md b/AGENTS.md index 47822fe..0fe6b50 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -60,7 +60,7 @@ primitives keep their upstream namespaces (`math`, `solvers`, …) from numerica ## Kinematics & dynamics conventions - 6-vectors are ordered **linear part first**: twists `(v; ω)`, wrenches `(f; n)`; the geometric - Jacobian has linear rows first (roadmap M6/M8). Featherstone `(ω; v)` ordering stays internal to the + Jacobian has linear rows first (upstream `math::SE3Transform`, roadmap NT53/M8). Featherstone `(ω; v)` ordering stays internal to the recursive dynamics algorithms. - Orientation errors use the SE(3) logarithm (`PoseError`), never angle subtraction. - The tool frame and the base mounting offset are explicit kinematic parameters — never derived from a diff --git a/ROADMAP.md b/ROADMAP.md index 43bb337..bc03fab 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -3,8 +3,10 @@ Prioritized backlog for the robot-manipulator stack (kinematics, dynamics, trajectories, and manipulator control). Shared numerical primitives (`math`, `solvers`, …) are consumed from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp); upstream already -provides `Matrix`, `Quaternion`, `MatrixExponential`, Gaussian elimination, Cholesky, LU, QR, SVD, -Jacobi eigen-solver and Runge-Kutta integrators, but no SE(3) type and no general QP solver. +provides `Matrix`, `Quaternion`, `MatrixExponential`, Gaussian elimination, Cholesky, LU, QR, SVD +(tall and wide), Jacobi eigen-solver and Runge-Kutta integrators, but no general QP solver. Generic +pieces this roadmap needs are specified and built upstream; see +[Upstream prerequisites](#upstream-prerequisites-numerical-toolbox). ## Robot manipulators & other manipulator types @@ -21,15 +23,25 @@ usable manipulator stack. Two current limitations gate many items below: > limits, armature), **M30** (full-pose kinematics) and **M8** (6×N Jacobian) lift these limits. **Conventions (normative for every item):** 6-vectors are ordered linear part first — twists `(v; ω)`, -wrenches `(f; n)` — as defined by **M6**; the geometric Jacobian has linear rows first; orientation -errors use the SE(3) logarithm (`PoseError`), never angle subtraction. Controllers receive dynamics and -Jacobians through small interfaces (`EulerLagrangeDynamics`, `InverseDynamicsModel`, `JacobianProvider`) -implemented by M29 / M8, so they can be tested with StrictMock and bound to concrete types on -hard real-time paths. +wrenches `(f; n)` — as defined by upstream `math::SE3Transform` (**NT53**); the geometric Jacobian +has linear rows first; orientation errors use the SE(3) logarithm (`PoseError`), never angle +subtraction. Controllers receive dynamics and Jacobians through small interfaces +(`EulerLagrangeDynamics`, `InverseDynamicsModel`, `JacobianProvider`) implemented by M29 / M8, so +they can be tested with StrictMock and bound to concrete types on hard real-time paths. All items are *float-only* — torques, lengths, and inertias exceed the `Q15`/`Q31` range, matching the existing `dynamics/` convention. +### Upstream prerequisites (numerical-toolbox) + +Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its +master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a +numerical-toolbox pin that ships it. + +| # | Component | Upstream module | Spec | Needed by | +|------|-------------------------------------------------------|-----------------|----------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------| +| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md) | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30 | + ### Manipulator list (by priority) | # | Component | Target module | Depends on | Difficulty | @@ -40,33 +52,32 @@ existing `dynamics/` convention. | M4 | Friction compensation (Coulomb + viscous + Stribeck) | `dynamics` | — | ★☆☆☆☆ | | M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M6 | SE(3) transform, twists, wrenches, adjoint, exp/log | `kinematics` | — | ★★☆☆☆ | | M28 | Composite Rigid Body Algorithm (mass matrix) | `dynamics` | — | ★★☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | -| M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | M6 | ★★☆☆☆ | -| M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | M6, M30 | ★★☆☆☆ | -| M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | M6, M30 | ★★☆☆☆ | +| M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | +| M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | +| M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | NT53, M30 | ★★☆☆☆ | | M9 | S-curve (jerk-limited) trajectory | `trajectory` (new) | M3 | ★★☆☆☆ | -| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | M6 | ★★☆☆☆ | +| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53 | ★★☆☆☆ | | M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | M8 | ★★☆☆☆ | | M33 | Cubic spline through via points | `trajectory` (new) | — | ★★☆☆☆ | | M34 | Forward-dynamics integrator (simulation step) | `dynamics` | — | ★★☆☆☆ | | M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` | M29 | ★★★☆☆ | -| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | M6, M8 | ★★★☆☆ | +| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, M8 | ★★★☆☆ | | M14 | Redundancy resolution / null-space projection | `kinematics` | M8 | ★★★☆☆ | -| M15 | Product-of-Exponentials forward kinematics | `kinematics` | M6 | ★★★☆☆ | +| M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | | M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | | M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | | M31 | Coriolis matrix and inertial-parameter regressor | `dynamics` | M28 | ★★★☆☆ | | M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | | M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | -| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | M6 | ★★★★☆ | +| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | NT53 | ★★★★☆ | | M22 | Dynamic (base-parameter) identification | `dynamics` | M31 | ★★★★☆ | -| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | M6 | ★★★★☆ | +| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | NT53 | ★★★★☆ | | M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14 | ★★★★☆ | -| M25 | Cable-driven tension distribution | `controllers/manipulator` | M6 | ★★★★☆ | -| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | M6 | ★★★★☆ | +| M25 | Cable-driven tension distribution | `controllers/manipulator` | NT53 | ★★★★☆ | +| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | NT53 | ★★★★☆ | | M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | ### Tier 1 — Trivial ★☆☆☆☆ @@ -92,10 +103,6 @@ existing `dynamics/` convention. ### Tier 2 — Easy ★★☆☆☆ -**M6. SE(3) transform, twists and wrenches.** Rigid transforms, adjoint, twist/wrench transforms, exponential/logarithm and `PoseError`; fixes the library's `(v; ω)` convention. -- *Algorithm / paper:* K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; Murray, Li, Sastry (1994). -- *Reuses / builds on:* upstream `Geometry3D`, `Quaternion`. - **M28. Composite Rigid Body Algorithm.** `O(n²)` joint-space mass matrix, sharing the spatial algebra of ABA. - *Algorithm / paper:* Walker & Orin (1982); Featherstone (2008), Ch. 6. diff --git a/roadmap/README.md b/roadmap/README.md index 1764222..b157f98 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -24,7 +24,7 @@ The canonical worked example is [trajectory/PolynomialTrajectory](trajectory/PolynomialTrajectory/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches -`(f; n)` — as fixed by [kinematics/SE3Transform](kinematics/SE3Transform/implementation.md); dynamics +`(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md) (NT53); dynamics and Jacobians reach controllers through interfaces implemented by [dynamics/ChainDynamicsModel](dynamics/ChainDynamicsModel/implementation.md) and [kinematics/GeometricJacobian](kinematics/GeometricJacobian/implementation.md). @@ -79,10 +79,14 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). > **Float-only.** The `template` signature is retained so `Q15`/`Q31` could be enabled > per algorithm later by relaxing the `static_assert` and adding instantiations; none is planned. +Generic algorithms are specified in numerical-toolbox-cpp's own `roadmap/` and referenced here as +**NT** plus its master-list item number (NT53 is upstream item 53); see `../ROADMAP.md` → Upstream +prerequisites. + ## Index ### `kinematics` -`SE3Transform` (M6) · `DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) +`DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) ### `dynamics` `GenericJointLink` (M1) · `FrictionCompensation` (M4) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CompositeRigidBodyAlgorithm` (M28) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md index 602c3fe..663ea02 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md @@ -35,6 +35,6 @@ A. Pott, T. Bruckmann, L. Mikelsons, "Closed-form Force Distribution for Paralle Over-Constrained Cable-Driven Parallel Robots," *Computational Kinematics*, 2014. ## See also -`SE3Transform` (M6, platform pose and wrench convention); `ParallelManipulatorKinematics` (M23, +`math::SE3Transform` (NT53, platform pose and wrench convention); `ParallelManipulatorKinematics` (M23, platform pose from actuator lengths); `solvers::TrySolveSystem` (the small wrench-space solve, from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)). diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md index 7ca1fed..b75e7ca 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md @@ -11,7 +11,7 @@ serial-arm Jacobian — and no QP solver is needed. ```cpp template # static_assert(std::is_floating_point_v); instantiated for float class CableTensionDistribution: # static_assert(WrenchDim == 6 || WrenchDim <= 3); NumCables ≥ WrenchDim - # WrenchDim = 6: spatial platform, wrench (f; n) about the platform origin, world frame (M6 order) + # WrenchDim = 6: spatial platform, wrench (f; n) about the platform origin, world frame (NT53 order) # WrenchDim ≤ 3: point-mass platform, force rows only (2 = planar xy); platform anchors ignored std::array, NumCables> baseAnchors # aᵢ, world frame std::array, NumCables> platformAnchors # bᵢ, platform frame @@ -28,11 +28,11 @@ using WrenchVector = math::Vector # wrench the cab ```cpp CableTensionDistribution(const AnchorArray& baseAnchors, const AnchorArray& platformAnchors, T tMin, T tMax) -std::optional ComputeStructureMatrix(const kinematics::SE3Transform& platformPose) const +std::optional ComputeStructureMatrix(const math::SE3Transform& platformPose) const # nullopt when the wrench is not reachable with tMin ≤ t ≤ tMax (or the geometry is degenerate): std::optional Distribute(const WrenchVector& wrench, - const kinematics::SE3Transform& platformPose) const # hot path + const math::SE3Transform& platformPose) const # hot path ``` ## Algorithm (pseudocode) @@ -107,6 +107,6 @@ function Distribute(w, pose): # OPTIMIZE_FOR_SPEED - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: M6 (`SE3Transform`); `solvers::TrySolveSystem` from +- Depends on: NT53 (`math::SE3Transform`); `solvers::TrySolveSystem` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md index aae8b94..75fb547 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md @@ -29,7 +29,7 @@ HybridPositionForceControl(const dynamics::EulerLagrangeDynamics& model, const SquareMatrix& Rc, const SquareMatrix& S, const Gains& gains, T dt) JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const kinematics::SE3Transform& desiredPose, const TaskVector& xdDot, + const math::SE3Transform& desiredPose, const TaskVector& xdDot, const TaskVector& fMeasured, const TaskVector& fd) # hot path void Reset() # clears force integral ``` @@ -97,6 +97,6 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, fMeasured, fd): # OPTIMIZE - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: M6 (`SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` (specified in M17, deployed with whichever of M17/M18/M19 lands first). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md index 28fe2ee..cf74d35 100644 --- a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md +++ b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md @@ -3,7 +3,7 @@ > Roadmap ref: #M17 (Tier 3) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) Sign convention: `fExternal` is the wrench exerted **by the environment on the tool**, base frame, -`(f; n)` about the tool point (M6 ordering), so the arm obeys `M q̈ + C q̇ + g = τ + Jᵀ·fExternal`. +`(f; n)` about the tool point (NT53 ordering), so the arm obeys `M q̈ + C q̇ + g = τ + Jᵀ·fExternal`. Task error `e = xd ⊖ x` (`TaskError`, below); `x̃ = x − xd = −e`. ## Data structures @@ -19,8 +19,8 @@ class ImpedanceControl: # TaskDim = 6 (pos # shared by M17/M18/M19 — robotics/controllers/manipulator/TaskError.hpp template -math::Vector TaskError(const kinematics::SE3Transform& desired, - const kinematics::SE3Transform& current) +math::Vector TaskError(const math::SE3Transform& desired, + const math::SE3Transform& current) ``` ## Interface @@ -32,12 +32,12 @@ ImpedanceControl(const dynamics::EulerLagrangeDynamics& model, # (a) stiffness/damping impedance — no force sensor, no mass matrix: JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const kinematics::SE3Transform& desiredPose, + const math::SE3Transform& desiredPose, const TaskVector& xdDot) # hot path # (b) full impedance with inertia shaping — needs M, J̇q̇ and the measured fExternal: JointVector ComputeTorqueWithInertiaShaping(const JointVector& q, const JointVector& qDot, - const kinematics::SE3Transform& desiredPose, + const math::SE3Transform& desiredPose, const TaskVector& xdDot, const TaskVector& xdDdot, const TaskVector& fExternal) # hot path ``` @@ -46,7 +46,7 @@ JointVector ComputeTorqueWithInertiaShaping(const JointVector& q, const JointVec ```text function TaskError(desired, current): # never subtracts orientations - if TaskDim == 6: return kinematics::SE3Transform::PoseError(desired, current) # (Δp; rotation vector) + if TaskDim == 6: return math::SE3Transform::PoseError(desired, current) # (Δp; rotation vector) return first TaskDim entries of (desired.p − current.p) function ComputeTorque(q, qDot, desiredPose, xdDot): # (a) OPTIMIZE_FOR_SPEED @@ -118,5 +118,5 @@ function ComputeTorqueWithInertiaShaping(q, qDot, desiredPose, xdDot, xdDdot, fE - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: M6 (`SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`). +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md index fbff83f..d4f5848 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md @@ -22,7 +22,7 @@ OperationalSpaceControl(const dynamics::EulerLagrangeDynamics& model, const SquareMatrix& Kp, const SquareMatrix& Kd, T sigma) JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, - const kinematics::SE3Transform& desiredPose, + const math::SE3Transform& desiredPose, const TaskVector& xdDot, const TaskVector& xdDdot, const JointVector& tauSecondary) # hot path ``` @@ -93,7 +93,7 @@ function ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, tauSecondary): # O - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: M6 (`SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`), M29 (`ChainDynamicsModel`); `TaskError.hpp` (specified in M17, deployed with whichever of M17/M18/M19 lands first); `solvers::SolveSystem` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/RneaExternalWrench/implementation.md b/roadmap/dynamics/RneaExternalWrench/implementation.md index 3ed6ba4..fb1881d 100644 --- a/roadmap/dynamics/RneaExternalWrench/implementation.md +++ b/roadmap/dynamics/RneaExternalWrench/implementation.md @@ -51,7 +51,7 @@ function InverseDynamics(links, q, q̇, q̈, g, w): # OPTIMIZE_FOR_SPEED 0.5 kg payload with gravity `(0, 0, −9.81)` is `w.force = (0, 0, −4.905)`. - Equivalence to the Jacobian-transpose form: `τ(w) − τ(0) = −Jᵀ·(f; n)` with the geometric Jacobian (M8) at the same tool point — the key test. -- Orders `(f; n)` linear first, as everywhere (M6). +- Orders `(f; n)` linear first, as everywhere (NT53). - Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment diff --git a/roadmap/kinematics/AnalyticalIkOpw/explanation.md b/roadmap/kinematics/AnalyticalIkOpw/explanation.md index 88d3cc4..aacc27b 100644 --- a/roadmap/kinematics/AnalyticalIkOpw/explanation.md +++ b/roadmap/kinematics/AnalyticalIkOpw/explanation.md @@ -36,6 +36,6 @@ Robotics Workshop*, 2014; D. L. Pieper, "The Kinematics of Manipulators Under Co thesis, Stanford University, 1968. ## See also -`SE3Transform` (M6, the flange pose), `PoseInverseKinematics` (M13, the iterative fallback for other +`math::SE3Transform` (NT53, the flange pose), `PoseInverseKinematics` (M13, the iterative fallback for other layouts), `DenavitHartenberg` (M7), `Cordic` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), fixed-cost trig). diff --git a/roadmap/kinematics/AnalyticalIkOpw/implementation.md b/roadmap/kinematics/AnalyticalIkOpw/implementation.md index 768fe58..e8eb4bd 100644 --- a/roadmap/kinematics/AnalyticalIkOpw/implementation.md +++ b/roadmap/kinematics/AnalyticalIkOpw/implementation.md @@ -121,5 +121,5 @@ function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each wrapp - Doc: `doc/kinematics/AnalyticalIkOpw.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestAnalyticalIkOpw.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ChainPoseKinematics/explanation.md b/roadmap/kinematics/ChainPoseKinematics/explanation.md index d9649a5..3386321 100644 --- a/roadmap/kinematics/ChainPoseKinematics/explanation.md +++ b/roadmap/kinematics/ChainPoseKinematics/explanation.md @@ -26,4 +26,4 @@ B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, *Robotics: Modelling, Plannin Ch. 2 (direct kinematics). ## See also -`ForwardKinematics` (shipped, position-only), `SE3Transform` (M6), `GeometricJacobian` (M8). +`ForwardKinematics` (shipped, position-only), `math::SE3Transform` (NT53), `GeometricJacobian` (M8). diff --git a/roadmap/kinematics/ChainPoseKinematics/implementation.md b/roadmap/kinematics/ChainPoseKinematics/implementation.md index 6a42403..4248510 100644 --- a/roadmap/kinematics/ChainPoseKinematics/implementation.md +++ b/roadmap/kinematics/ChainPoseKinematics/implementation.md @@ -19,7 +19,7 @@ struct FrameChain: std::array, N> jointOrigins # base frame std::array, N> jointAxes # unit, base frame std::array jointTypes - SE3Transform tool # tool pose in the base frame (M6) + SE3Transform tool # tool pose in the base frame (NT53) template class ChainPoseKinematics: @@ -71,5 +71,5 @@ function Compute(links, q): # OPTIMIZE_FOR_SPEED - Coverage: `robotics/kinematics/ChainPoseKinematics.cpp` → the same three instantiations. - Test: `robotics/kinematics/test/TestChainPoseKinematics.cpp` - Doc: `doc/kinematics/ChainPoseKinematics.md` (or extend `ForwardKinematics.md`) -- Depends on: M6 (`SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ContinuumKinematics/explanation.md b/roadmap/kinematics/ContinuumKinematics/explanation.md index e4c35a4..ecf5456 100644 --- a/roadmap/kinematics/ContinuumKinematics/explanation.md +++ b/roadmap/kinematics/ContinuumKinematics/explanation.md @@ -34,6 +34,6 @@ R. J. Webster III, B. A. Jones, "Design and Kinematic Modeling of Constant Curva A Review," *Int. J. Robotics Research*, 29(13), 2010. ## See also -`SE3Transform` (M6, per-section transform and composition), `PoseInverseKinematics` (M13, the +`math::SE3Transform` (NT53, per-section transform and composition), `PoseInverseKinematics` (M13, the iterative multi-section inverse), `Quaternion` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), orientation blending). diff --git a/roadmap/kinematics/ContinuumKinematics/implementation.md b/roadmap/kinematics/ContinuumKinematics/implementation.md index 88c45c8..547cf61 100644 --- a/roadmap/kinematics/ContinuumKinematics/implementation.md +++ b/roadmap/kinematics/ContinuumKinematics/implementation.md @@ -40,7 +40,7 @@ function SectionTransform(arc): # OPTIMIZE_FOR_SPEED, robot-inde function Forward(): # OPTIMIZE_FOR_SPEED A = Identity - for i in 0..NumSections-1: A = A * SectionTransform(sections[i]) # M6 compose + for i in 0..NumSections-1: A = A * SectionTransform(sections[i]) # NT53 compose return A function InverseSection(tip): # tip position in the section base frame, κs < 2π @@ -81,5 +81,5 @@ function InverseSection(tip): # tip position in the section ba - Doc: `doc/kinematics/ContinuumKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestContinuumKinematics.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/DenavitHartenberg/explanation.md b/roadmap/kinematics/DenavitHartenberg/explanation.md index deb243e..3ccd19c 100644 --- a/roadmap/kinematics/DenavitHartenberg/explanation.md +++ b/roadmap/kinematics/DenavitHartenberg/explanation.md @@ -35,6 +35,6 @@ J. Denavit, R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Ba 4th ed., Ch. 3 (modified convention). ## See also -`SE3Transform` (M6, the per-link transform), `ChainPoseKinematics` (M30, the `FrameChain` it emits), +`math::SE3Transform` (NT53, the per-link transform), `ChainPoseKinematics` (M30, the `FrameChain` it emits), `GeometricJacobian` (M8, consumes the frame chain), `ProductOfExponentials` (M15, the screw-theory alternative). diff --git a/roadmap/kinematics/DenavitHartenberg/implementation.md b/roadmap/kinematics/DenavitHartenberg/implementation.md index e564f03..6984e3c 100644 --- a/roadmap/kinematics/DenavitHartenberg/implementation.md +++ b/roadmap/kinematics/DenavitHartenberg/implementation.md @@ -23,7 +23,7 @@ template class DenavitHartenberg: std::array, N> links DhConvention convention - SE3Transform tool # constant tool pose in the last DH frame (M6) + SE3Transform tool # constant tool pose in the last DH frame (NT53) std::array cosAlpha, sinAlpha # precomputed at construction template # TaskDim ∈ {3, 6} @@ -114,5 +114,5 @@ function Frames(q): - Doc: `doc/kinematics/DenavitHartenberg.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestDenavitHartenberg.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform`), M30 (`FrameChain`), M8 (`GeometricJacobian`, `JacobianProvider`). +- Depends on: NT53 (`math::SE3Transform`), M30 (`FrameChain`), M8 (`GeometricJacobian`, `JacobianProvider`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/GeometricJacobian/implementation.md b/roadmap/kinematics/GeometricJacobian/implementation.md index 55ff2df..27d095c 100644 --- a/roadmap/kinematics/GeometricJacobian/implementation.md +++ b/roadmap/kinematics/GeometricJacobian/implementation.md @@ -3,7 +3,7 @@ > Roadmap ref: #M8 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) The **geometric** Jacobian maps joint rates to the tool's linear velocity (of the tool point) and -angular velocity, both in the base frame, ordered `(v; ω)` (M6 convention). It is *not* the +angular velocity, both in the base frame, ordered `(v; ω)` (NT53 convention). It is *not* the Lynch–Park "space Jacobian" (whose linear part is the velocity of the point at the base origin); M15 provides that one. The routine is model-agnostic: it consumes a `FrameChain` (M30), which the link model (M30), DH (M7) and PoE (M15) all produce, so FK, IK, dynamics and controllers share a @@ -101,6 +101,6 @@ function JointTorques(J, w): # statics: τ = Jᵀ·w, w = (f; - Coverage: `robotics/kinematics/GeometricJacobian.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestGeometricJacobian.cpp` - Doc: `doc/kinematics/GeometricJacobian.md` (per `doc/TEMPLATE.md`) -- Depends on: M6 (`SE3Transform`), M30 (`FrameChain`, `ChainPoseKinematics`). +- Depends on: NT53 (`math::SE3Transform`), M30 (`FrameChain`, `ChainPoseKinematics`). - After deployment, the shipped `InverseKinematics` should build its 3×N Jacobian from this routine. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md index 493ef3d..2687d08 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md @@ -33,5 +33,5 @@ Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manip Automatic Control*, 39(6), 1994. ## See also -`JacobianProvider` (M8, the arm block), `SE3Transform` (M6, pose composition), `RedundancyResolution` +`JacobianProvider` (M8, the arm block), `math::SE3Transform` (NT53, pose composition), `RedundancyResolution` (M14, splitting base vs arm). diff --git a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md index 7738fa9..ad0bd6f 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md @@ -12,7 +12,7 @@ struct BaseState: # planar pose of the axle midpoi template class MobileManipulatorKinematics: const JacobianProvider& arm # M8: J and ToolPose in the arm-base frame - SE3Transform mount # arm base in the mobile-base frame (M6) + SE3Transform mount # arm base in the mobile-base frame (NT53) T wheelBase # L, axle length ``` @@ -27,7 +27,7 @@ std::array WheelSpeeds(T v, T omega) const # (left, right) ``` Columns of `Compute` act on `u = (v, ω, q̇)`: base forward speed, base yaw rate, arm joint rates. -Rows are the tool-point twist `(v; ω)` in the world frame (M6/M8 ordering). +Rows are the tool-point twist `(v; ω)` in the world frame (NT53/M8 ordering). ## Algorithm (pseudocode) @@ -80,5 +80,5 @@ function WheelSpeeds(v, ω): - Doc: `doc/kinematics/MobileManipulatorKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestMobileManipulatorKinematics.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform`), M8 (`JacobianProvider`). +- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md index ebc92be..579ef85 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md @@ -34,6 +34,6 @@ J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, "Delta, a Fast Robot Geometry," *Int. Symp. on Industrial Robots*, 1988. ## See also -`SE3Transform` (M6, platform pose and retraction), `LuDecomposition` +`math::SE3Transform` (NT53, platform pose and retraction), `LuDecomposition` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), the Newton step), `ManipulabilityIndex` (M11, singularity monitoring). diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md index a30ccdc..49b70cc 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md @@ -64,7 +64,7 @@ function Inverse(pose): # OPTIMIZE_FOR_SPEED, closed fo function LegJacobian(pose): # row i = (nᵢᵀ, (R·pᵢ × nᵢ)ᵀ), nᵢ = unit leg vector for i: a = pose.R·pᵢ; n = Normalize(a + pose.p − bᵢ); row i = (n ; CrossProduct(a, n)) - # (v; ω): platform-origin velocity and angular velocity, base frame (M6 order); statics: wrench (f; n) = Jᵀ·legForces + # (v; ω): platform-origin velocity and angular velocity, base frame (NT53 order); statics: wrench (f; n) = Jᵀ·legForces function Forward(L, guess): # Newton on SE(3) x = guess @@ -128,5 +128,5 @@ function Forward(θ): # three-sphere intersection, c - Tests: `robotics/kinematics/test/TestStewartGoughKinematics.cpp`, `robotics/kinematics/test/TestDeltaKinematics.cpp` - Doc: `doc/kinematics/ParallelManipulatorKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; both tests → the `_test` target. -- Depends on: M6 (`SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/explanation.md b/roadmap/kinematics/PoseInverseKinematics/explanation.md index 69c6700..2ca0ccb 100644 --- a/roadmap/kinematics/PoseInverseKinematics/explanation.md +++ b/roadmap/kinematics/PoseInverseKinematics/explanation.md @@ -36,7 +36,7 @@ S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoi Least Squares methods," 2004. ## See also -`JacobianProvider` / `GeometricJacobian` (M8, the linearization), `SE3Transform::PoseError` (M6, the +`JacobianProvider` / `GeometricJacobian` (M8, the linearization), `math::SE3Transform::PoseError` (NT53, the orientation error), `ManipulabilityIndex` (M11), `AnalyticalIkOpw` (M21, the closed-form alternative for ortho-parallel 6R arms), `LuDecomposition` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), the 6×6 solve). diff --git a/roadmap/kinematics/PoseInverseKinematics/implementation.md b/roadmap/kinematics/PoseInverseKinematics/implementation.md index caf56b1..025a1c4 100644 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ b/roadmap/kinematics/PoseInverseKinematics/implementation.md @@ -42,7 +42,7 @@ PoseIkResult Solve(const SE3Transform& target, const JointVector& q0 ```text function WeightedError(target, q): - e = SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), M6 + e = SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), NT53 return (e.linear; ρ·e.angular) function Solve(target, q0): # OPTIMIZE_FOR_SPEED @@ -83,7 +83,7 @@ function Damping(A0): # Nakamura–Hanafusa / Chiaveri - **Units:** position error is in m, rotation error in rad. `ρ` (≈ the arm's reach or tool length) converts radians to metres in both the stopping test and the step. With `λ = 0` and invertible `J`, `W` cancels (`Δq = J⁻¹e`); it matters for damping, redundancy and unreachable targets. -- **Orientation error** is `SE3Transform::PoseError` — the true log map, `|eR| ≤ π`, short way round. +- **Orientation error** is `math::SE3Transform::PoseError` — the true log map, `|eR| ≤ π`, short way round. `2·vec(error quaternion)` equals `2 sin(φ/2)·n̂`, which is only a small-angle approximation of `φ·n̂` (0.959 vs 1 rad at φ = 1 rad) and must not be used. - **Adaptive damping:** `λ = 0` away from singularities (fast Newton convergence), rising smoothly to @@ -105,5 +105,5 @@ function Damping(A0): # Nakamura–Hanafusa / Chiaveri - Doc: `doc/kinematics/PoseInverseKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestPoseInverseKinematics.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform::PoseError`), M8 (`JacobianProvider`); upstream `solvers::LuDecomposition`. +- Depends on: NT53 (`math::SE3Transform::PoseError`), M8 (`JacobianProvider`); upstream `solvers::LuDecomposition`. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/tests.md b/roadmap/kinematics/PoseInverseKinematics/tests.md index 23e6272..687268b 100644 --- a/roadmap/kinematics/PoseInverseKinematics/tests.md +++ b/roadmap/kinematics/PoseInverseKinematics/tests.md @@ -64,10 +64,10 @@ unreachable_target_fails_gracefully: on that scale. Wrist-singular case above converges in 8 iterations (double). - Stale-error scenario on the real arm: initial `‖W e‖ = 0.136739`, after one step `0.011815` — the result must report the latter. -- `+190°` about `ẑ` ⇒ rotation vector `−2.967060·ẑ` (short way, via M6). +- `+190°` about `ẑ` ⇒ rotation vector `−2.967060·ẑ` (short way, via NT53). ## Edge cases -- Antipodal orientation (180°) ⇒ `PoseError` axis ambiguous (either sign valid, M6). +- Antipodal orientation (180°) ⇒ `PoseError` axis ambiguous (either sign valid, NT53). - Target at the reach boundary ⇒ slow but bounded convergence (adaptive damping engages). - `Dof = 7` ⇒ same code; the minimum-norm step drifts in the null space — use M14 to steer it. diff --git a/roadmap/kinematics/ProductOfExponentials/explanation.md b/roadmap/kinematics/ProductOfExponentials/explanation.md index 884ed93..d397e5e 100644 --- a/roadmap/kinematics/ProductOfExponentials/explanation.md +++ b/roadmap/kinematics/ProductOfExponentials/explanation.md @@ -30,6 +30,6 @@ orders screws `(ω; v)`, this library `(v; ω)`); R. W. Brockett, "Robotic Manip of Exponentials Formula," 1984. ## See also -`SE3Transform` (M6, `Exp` and `Adjoint`), `ChainPoseKinematics` (M30, `FrameChain`), +`math::SE3Transform` (NT53, `Exp` and `Adjoint`), `ChainPoseKinematics` (M30, `FrameChain`), `GeometricJacobian` (M8), `DenavitHartenberg` (M7, the frame-based alternative), `MatrixExponential` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), test cross-check). diff --git a/roadmap/kinematics/ProductOfExponentials/implementation.md b/roadmap/kinematics/ProductOfExponentials/implementation.md index 8e23172..c49227b 100644 --- a/roadmap/kinematics/ProductOfExponentials/implementation.md +++ b/roadmap/kinematics/ProductOfExponentials/implementation.md @@ -2,7 +2,7 @@ > Roadmap ref: #M15 (Tier 3) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) -Screws follow the library convention (M6): `S = (v; ω)`, linear part first. +Screws follow the library convention (NT53): `S = (v; ω)`, linear part first. ## Data structures @@ -32,7 +32,7 @@ static Matrix SpaceToGeometric(const Matrix& Js, const Vector3 ```text function Compute(q): # OPTIMIZE_FOR_SPEED A = Identity - for i in 0..N-1: A = A * SE3Transform::Exp(screws[i], q[i]) # M6 + for i in 0..N-1: A = A * SE3Transform::Exp(screws[i], q[i]) # NT53 return A * home # e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M function SpaceJacobian(q): @@ -70,7 +70,7 @@ function SpaceToGeometric(Js, pTool): # J_geo = [[I, −Skew(pTool)], velocity, used by M11/M13/M14 and the controllers — is `J_geo = [[I, −Skew(p_tool)],[0, I]]·J_space`, equal column by column to `GeometricJacobian::Compute(Frames(q))`. - No per-link frames: every screw is written once in the base frame (the main advantage over DH). -- `SE3Transform::Exp` handles the prismatic (`ω = 0`) branch and folds a non-unit `ω` into the angle; +- `math::SE3Transform::Exp` handles the prismatic (`ω = 0`) branch and folds a non-unit `ω` into the angle; still author unit `ω` so `qᵢ` is a true angle. Pitched screws (`ω·v ≠ 0`) cannot be expressed as a `FrameChain` and are rejected at construction. - Body form (`T = M·e^{[B₁]q₁}···`, `Bᵢ = Ad(M⁻¹)·Sᵢ`) is not needed: `J_body = Ad(T⁻¹)·J_space`. @@ -89,5 +89,5 @@ function SpaceToGeometric(Js, pTool): # J_geo = [[I, −Skew(pTool)], - Doc: `doc/kinematics/ProductOfExponentials.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestProductOfExponentials.cpp` → the `_test` target. -- Depends on: M6 (`SE3Transform::Exp`, `Adjoint`), M30 (`FrameChain`), M8 (`GeometricJacobian`). +- Depends on: NT53 (`math::SE3Transform::Exp`, `Adjoint`), M30 (`FrameChain`), M8 (`GeometricJacobian`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/SE3Transform/explanation.md b/roadmap/kinematics/SE3Transform/explanation.md deleted file mode 100644 index 5fe5037..0000000 --- a/roadmap/kinematics/SE3Transform/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# SE(3) Transform, Twists and Wrenches — Overview - -## What it is -The algebra of rigid-body motion: a transform is a rotation plus a translation; a *twist* is a rigid -body's instantaneous velocity (linear and angular); a *wrench* is a force together with a moment. The -adjoint map moves twists and wrenches between frames, and the exponential/logarithm maps convert -between a constant twist held for some time and the finite motion it produces. - -## Why it matters (embedded) -Every pose-level algorithm in the roadmap — full-pose forward kinematics, the 6×N Jacobian, pose -inverse kinematics, product-of-exponentials kinematics, Cartesian interpolation and task-space -control — needs the same small set of operations. Defining them once, with one ordering convention, -prevents the silent sign and ordering mismatches that otherwise appear when independently written -modules exchange 6-vectors. - -## How it works (intuition) -A transform stores a 3×3 rotation and a 3-vector translation; composing two transforms rotates and -shifts the second by the first. A twist written in one frame is re-expressed in another with the -adjoint: rotate both parts, then add the "lever-arm" effect of the frame offset on the linear part. -Wrenches transform with the dual rule so that power (force times velocity) is the same in every -frame. The exponential turns "rotate about this line while sliding along it" into a finite motion — -Rodrigues' formula for the rotation plus a matching integral for the translation — and the logarithm -recovers that motion, which is exactly what pose-error feedback needs. - -## Key parameters -- **Ordering convention** — linear part first for twists `(v; ω)` and wrenches `(f; n)`. -- **Singular regions** — small angles and angles near 180° in the logarithm. - -## 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), Ch. 2. (Both use `(ω; v)` ordering; this -library uses `(v; ω)`, which only permutes the blocks.) - -## See also -`GeometricJacobian` (M8), `PoseInverseKinematics` (M13), `ProductOfExponentials` (M15), -`CartesianSlerpInterpolation` (M10), `ChainPoseKinematics` (M30). diff --git a/roadmap/kinematics/SE3Transform/implementation.md b/roadmap/kinematics/SE3Transform/implementation.md deleted file mode 100644 index 3da3ea1..0000000 --- a/roadmap/kinematics/SE3Transform/implementation.md +++ /dev/null @@ -1,112 +0,0 @@ -# SE(3) Transform, Twists and Wrenches — Implementation Pseudocode - -> Roadmap ref: #M6 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -numerical-toolbox provides `Matrix`, `Quaternion` and the 3D geometry helpers but no rigid-body -transform, so this type lives here, in `kinematics`. It fixes the **one convention used by every -other spec**: 6-vectors are ordered **linear part first** — twists `(v; ω)`, wrenches `(f; n)`. - -## Data structures - -```cpp -template using Vector3 = math::Vector -template using Matrix3 = math::SquareMatrix -template using Vector6 = math::Vector # twist (v; ω) or wrench (f; n) -template using Matrix6 = math::SquareMatrix - -template # static_assert(std::is_floating_point_v); instantiated for float -struct SE3Transform: - Matrix3 R # rotation, orthonormal, det = +1 - Vector3 p # translation -``` - -## Interface - -```text -static SE3Transform Identity() -SE3Transform operator*(const SE3Transform& rhs) const # composition -SE3Transform Inverse() const -Vector3 Apply(const Vector3& point) const # R·x + p -Vector3 Rotate(const Vector3& direction) const # R·x -Matrix6 Adjoint() const # twist map for (v; ω) -Vector6 TransformTwist(const Vector6& twist) const # Adjoint()·twist -Vector6 TransformWrench(const Vector6& wrench) const # dual map, see below - -static SE3Transform Exp(const Vector6& twist, T theta) # e^{[ξ]θ}; hot path for PoE -Vector6 Log() const # inverse of Exp with theta = 1 -static Vector6 PoseError(const SE3Transform& target, const SE3Transform& current) -``` - -## Algorithm (pseudocode) - -```text -function operator*(rhs): return { R·rhs.R, R·rhs.p + p } -function Inverse(): return { Rᵀ, −Rᵀ·p } - -function Adjoint(): # V_A = Ad_T · V_B for V = (v; ω) - return [[ R, Skew(p)·R ], - [ 0, R ]] - -function TransformWrench(w = (f; n)): # power-consistent dual of Adjoint - fA = R·f - nA = R·n + CrossProduct(p, R·f) - return (fA; nA) - -function Exp(ξ = (v; ω), θ): # OPTIMIZE_FOR_SPEED - if ‖ω‖ < ε: # prismatic / pure translation - return { I, v·θ } - # general twist: fold the magnitude of ω into θ so the axis is unit - s = ‖ω‖; ω̂ = ω / s; v̂ = v / s; φ = θ·s - R = RotationAboutAxis(ω̂, φ) # reuse Geometry3D (Rodrigues) - G = I·φ + (1 − cos φ)·Skew(ω̂) + (φ − sin φ)·Skew(ω̂)² - return { R, G·v̂ } - -function Log(): # returns (v; ω) with ‖ω‖ = rotation angle - c = clamp((trace(R) − 1) / 2, −1, 1); φ = acos(c) - if φ < ε: # small angle: first-order - ω = Vee(R − Rᵀ) / 2; return (p; ω) - if π − φ < ε: # near π: sin φ → 0, use the diagonal form - ω̂ = axis from the largest of sqrt((R_ii + 1) / 2), signs from the off-diagonals - else: - ω̂ = Vee(R − Rᵀ) / (2 sin φ) - G⁻¹ = I/φ − Skew(ω̂)/2 + (1/φ − cot(φ/2)/2)·Skew(ω̂)² - return (G⁻¹·p·φ; ω̂·φ) - -function PoseError(target, current): # base-frame error, matches the geometric Jacobian (M8) - eP = target.p − current.p - eR = (SE3Transform{ target.R · current.Rᵀ, 0 }).Log().angular # rotation vector, |eR| ≤ π - return (eP; eR) -``` - -## Complexity & memory - -- Compose / inverse / apply: `O(1)` — a handful of 3×3 products. -- `Exp` / `Log`: `O(1)` — one `sin`/`cos` pair (plus `acos` for `Log`). -- Memory: 12 scalars per transform; `Matrix6` only when an adjoint is requested. No heap. - -## Numerical / embedded notes - -- **Convention (normative for all specs):** twists are `(v; ω)` and wrenches `(f; n)`; the geometric - Jacobian (M8) has linear rows first; `PoseError` returns `(position; rotation-vector)`. -- `Log` must be the true logarithm: `2·vec(quaternion)` equals `2 sin(φ/2)·ω̂`, which is only a - small-angle approximation of `φ·ω̂`. For `φ > π` the rotation is re-expressed the short way, so - `|eR| ≤ π`. -- Handle both singular regions of `Log` (`φ → 0` and `φ → π`) explicitly; `acos` near ±1 loses - precision in `float`, so derive `φ` from `atan2(‖Vee(R − Rᵀ)‖/2, (trace R − 1)/2)` when accuracy - near 0 matters. -- Composition drifts off SO(3) over long chains of products in `float`; re-orthonormalize (Gram–Schmidt - or via `Quaternion` normalization) when a transform is integrated over time, not after single - compositions. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/SE3Transform.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Exp`/`operator*`, and - `extern template struct SE3Transform;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/SE3Transform.cpp` → `template struct SE3Transform;` -- Test: `robotics/kinematics/test/TestSE3Transform.cpp` -- Doc: `doc/kinematics/SE3Transform.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestSE3Transform.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/SE3Transform/tests.md b/roadmap/kinematics/SE3Transform/tests.md deleted file mode 100644 index 9f01231..0000000 --- a/roadmap/kinematics/SE3Transform/tests.md +++ /dev/null @@ -1,50 +0,0 @@ -# SE(3) Transform — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSE3Transform : public ::testing::Test: - SE3Transform a{ RotationAboutAxis(ẑ, π/2), (1, 0, 0) } - SE3Transform b{ RotationAboutAxis(ŷ, π/3), (0, 2, 0) } -# each case below is a TEST_F(TestSE3Transform, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -composition_applies_right_operand_first: - Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2, 0.5) -inverse_composes_to_identity: - Assert: a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity -adjoint_maps_twists_consistently_with_composition: - Arrange: twist V in the frame of a - Assert: Exp(a.TransformTwist(V), t) ≈ a * Exp(V, t) * a.Inverse() (t = 0.4) -wrench_and_twist_power_is_frame_invariant: - Arrange: twist V, wrench W in the same frame - Assert: a.TransformWrench(W) · a.TransformTwist(V) ≈ W · V -exp_of_pure_rotation_about_offset_axis: - Arrange: screw about ẑ through (1, 0, 0): ξ = (−ẑ × (1,0,0); ẑ) = ((0,−1,0); ẑ) - Assert: Exp(ξ, π/2).p ≈ (1, −1, 0) and R ≈ Rz(π/2) -exp_of_prismatic_twist_is_translation: - Assert: Exp(((1,0,0); 0), 0.3) ≈ { I, (0.3, 0, 0) } -log_inverts_exp: - Assert: Exp(Log(T), 1) ≈ T for T = a, b, and a rotation of 179.9° -log_near_zero_and_near_pi_stays_finite: - Assert: Log of rotations of 1e-6 rad and π − 1e-4 rad are finite with the expected angle -pose_error_takes_the_short_way: - Arrange: target = current rotated by +190° about ẑ - Assert: ‖PoseError(target, current).angular‖ ≈ 170° (2.967 rad), direction −ẑ -``` - -## Reference vectors - -- `Rz(π/2)·(1,0,0) = (0,1,0)`; screw about `ẑ` through `(1,0,0)` by `π/2` maps the origin to `(1,−1,0)`. -- Rotation error of `+190°` about `ẑ` ⇒ rotation vector `−170°·ẑ`. - -## Edge cases - -- `Exp` with `ω = 0` (prismatic) and with non-unit `ω` (magnitude folded into the angle). -- `Log` at `φ = π` exactly: either axis sign is valid; the test accepts both. -- Repeated composition in `float` keeps `RᵀR ≈ I` to `1e-5` after 1000 random products. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md index cd3dc22..7155884 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md @@ -32,5 +32,5 @@ K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH* 1985 (SLERP ## See also `Quaternion` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) -(provides `Slerp`), `SE3Transform` (M6, pose and twist conventions), +(provides `Slerp`), `math::SE3Transform` (NT53, pose and twist conventions), `TrapezoidalProfile` / `PolynomialTrajectory` / `SCurveProfile` (the `s(t)` drivers). diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md index 4dcd070..f5aab44 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md @@ -4,7 +4,7 @@ ## Data structures -Poses are `kinematics::SE3Transform` (M6); twists are `kinematics::Vector6` ordered linear-first +Poses are `math::SE3Transform` (NT53); twists are `math::Vector6` ordered linear-first `(v; ω)`. Orientation is interpolated with the upstream `math::Quaternion` (`numerical/math/Quaternion.hpp`, [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp): `FromRotationMatrix`, `Slerp`, `ToRotationMatrix`, `Conjugate`). `TrajectoryState` comes from @@ -13,8 +13,8 @@ Poses are `kinematics::SE3Transform` (M6); twists are `kinematics::Vector6 ```cpp template # static_assert(std::is_floating_point_v); instantiated for float struct CartesianState: - kinematics::SE3Transform pose - kinematics::Vector6 twist # (v; ω), base frame — feed-forward for task-space control + math::SE3Transform pose + math::Vector6 twist # (v; ω), base frame — feed-forward for task-space control # TimeLaw requirement (static polymorphism, no virtual call on the hot path): # TrajectoryState Sample(T t) const — position = progress s ∈ [0, 1], velocity = ṡ @@ -32,8 +32,8 @@ class CartesianSlerpInterpolation: ## Interface ```text -CartesianSlerpInterpolation(const kinematics::SE3Transform& start, - const kinematics::SE3Transform& goal, const TimeLaw& timeLaw) +CartesianSlerpInterpolation(const math::SE3Transform& start, + const math::SE3Transform& goal, const TimeLaw& timeLaw) CartesianState Sample(T t) # hot path T Duration() # = timeLaw.Duration() ``` @@ -91,7 +91,7 @@ including the sign-flipped (obtuse) case. `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class CartesianSlerpInterpolation>;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp`, `robotics/kinematics/SE3Transform.hpp` (M6) and +- Uses `robotics/trajectory/TrajectoryTypes.hpp`, `numerical/math/SE3Transform.hpp` (NT53) and `numerical/math/Quaternion.hpp`; `robotics.trajectory` links `robotics.kinematics`. - Coverage: `robotics/trajectory/CartesianSlerpInterpolation.cpp` → `template class CartesianSlerpInterpolation>;` @@ -103,5 +103,5 @@ including the sign-flipped (obtuse) case. - New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: M6, M2 (any other `TimeLaw`, e.g. M3/M9, works without extra code). +- Depends on: NT53, M2 (any other `TimeLaw`, e.g. M3/M9, works without extra code). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md index 73ac2a6..1275e73 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md @@ -8,8 +8,8 @@ class TestCartesianSlerp : public ::testing::Test: using Law = PolynomialTrajectory Law linearLaw{ { .q0 = 0, .qf = 1, .v0 = 1, .vf = 1 }, 1.0f, Degree::Cubic } # s(t) = t, ṡ = 1 - kinematics::SE3Transform start{ .R = I, .p = {0,0,0} } - kinematics::SE3Transform goal { .R = Rz(90°), .p = {1,0,0} } + math::SE3Transform start{ .R = I, .p = {0,0,0} } + math::SE3Transform goal { .R = Rz(90°), .p = {1,0,0} } CartesianSlerpInterpolation path{ start, goal, linearLaw } # each case below is a TEST_F(TestCartesianSlerp, ) ``` From db74fb5d1e7a0ac597822f852d2f4960e9714ee2 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 14:07:53 +0000 Subject: [PATCH 4/4] =?UTF-8?q?docs(roadmap):=20move=20generic=20algorithm?= =?UTF-8?q?s=20to=20numerical-toolbox=20(NT54=E2=80=93NT65)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Generic parts of this roadmap are now specified upstream in numerical-toolbox-cpp; robotics items keep only the robot-specific part and depend on NT: - Moved whole: M2 polynomial, M3 trapezoidal and M9 S-curve profiles (NT55, NT56, NT60, new upstream motion_profiles module) and M4 friction compensation (NT57). Their specs and numbers are retired. - M33 is a thin joint-space wrapper over math::CubicSpline (NT61, with the Thomas solve NT59) and now owns JointTrajectoryState. - M14 binds the task Jacobian to solvers::DampedPseudoInverse (NT62). - M22 feeds regressor rows to IncrementalQrLeastSquares (NT63). - M34 adapts ABA to SemiImplicitEuler (NT58) and RungeKutta4. - M13 and M23 run their damped Newton iterations on LevenbergMarquardt (NT64): pose IK keeps its manipulability damping as a damping policy, Stewart FK its SE(3) retraction. - M21 wraps joint angles with WrapToPi (NT54). - M25 gains DistributeExact on ActiveSetQp (NT65) for the workspace boundary, where the improved closed form fails (spatial 8-cable fixture: 90 of 4000 boundary-biased feasible wrenches); M27 keeps its two-variable stage LPs, which a strictly convex QP cannot express. ROADMAP.md lists NT53–NT65 with spec links and consumers; M14, M22 and M33 drop one tier now that the numerics live upstream. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- ROADMAP.md | 153 ++++++++++-------- TESTING.md | 5 +- roadmap/README.md | 6 +- .../CableTensionDistribution/explanation.md | 9 +- .../implementation.md | 26 ++- .../CableTensionDistribution/tests.md | 26 ++- .../PdGravityCompensation/explanation.md | 2 +- .../ChainDynamicsModel/implementation.md | 2 +- .../explanation.md | 9 +- .../implementation.md | 43 ++--- .../DynamicParameterIdentification/tests.md | 5 +- .../ForwardDynamicsIntegrator/explanation.md | 4 +- .../implementation.md | 43 ++--- .../ForwardDynamicsIntegrator/tests.md | 8 +- .../FrictionCompensation/explanation.md | 34 ---- .../FrictionCompensation/implementation.md | 84 ---------- .../dynamics/FrictionCompensation/tests.md | 66 -------- .../GenericJointLink/implementation.md | 2 +- .../dynamics/MomentumObserver/explanation.md | 2 +- .../MomentumObserver/implementation.md | 2 +- .../AnalyticalIkOpw/implementation.md | 4 +- .../explanation.md | 4 +- .../implementation.md | 29 ++-- .../PoseInverseKinematics/explanation.md | 3 +- .../PoseInverseKinematics/implementation.md | 59 ++++--- .../RedundancyResolution/explanation.md | 5 +- .../RedundancyResolution/implementation.md | 58 +++---- .../kinematics/RedundancyResolution/tests.md | 25 ++- .../explanation.md | 2 +- .../implementation.md | 22 +-- .../CartesianSlerpInterpolation/tests.md | 2 +- .../CubicSplineTrajectory/explanation.md | 26 ++- .../CubicSplineTrajectory/implementation.md | 106 ++++-------- .../trajectory/CubicSplineTrajectory/tests.md | 69 +++----- .../PolynomialTrajectory/explanation.md | 31 ---- .../PolynomialTrajectory/implementation.md | 116 ------------- .../trajectory/PolynomialTrajectory/tests.md | 63 -------- .../trajectory/SCurveProfile/explanation.md | 35 ---- .../SCurveProfile/implementation.md | 122 -------------- roadmap/trajectory/SCurveProfile/tests.md | 87 ---------- .../explanation.md | 2 +- .../implementation.md | 5 +- .../TrapezoidalProfile/explanation.md | 34 ---- .../TrapezoidalProfile/implementation.md | 108 ------------- .../trajectory/TrapezoidalProfile/tests.md | 71 -------- 45 files changed, 378 insertions(+), 1241 deletions(-) delete mode 100644 roadmap/dynamics/FrictionCompensation/explanation.md delete mode 100644 roadmap/dynamics/FrictionCompensation/implementation.md delete mode 100644 roadmap/dynamics/FrictionCompensation/tests.md delete mode 100644 roadmap/trajectory/PolynomialTrajectory/explanation.md delete mode 100644 roadmap/trajectory/PolynomialTrajectory/implementation.md delete mode 100644 roadmap/trajectory/PolynomialTrajectory/tests.md delete mode 100644 roadmap/trajectory/SCurveProfile/explanation.md delete mode 100644 roadmap/trajectory/SCurveProfile/implementation.md delete mode 100644 roadmap/trajectory/SCurveProfile/tests.md delete mode 100644 roadmap/trajectory/TrapezoidalProfile/explanation.md delete mode 100644 roadmap/trajectory/TrapezoidalProfile/implementation.md delete mode 100644 roadmap/trajectory/TrapezoidalProfile/tests.md diff --git a/ROADMAP.md b/ROADMAP.md index bc03fab..2a8315d 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -4,9 +4,9 @@ Prioritized backlog for the robot-manipulator stack (kinematics, dynamics, traje and manipulator control). Shared numerical primitives (`math`, `solvers`, …) are consumed from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp); upstream already provides `Matrix`, `Quaternion`, `MatrixExponential`, Gaussian elimination, Cholesky, LU, QR, SVD -(tall and wide), Jacobi eigen-solver and Runge-Kutta integrators, but no general QP solver. Generic -pieces this roadmap needs are specified and built upstream; see -[Upstream prerequisites](#upstream-prerequisites-numerical-toolbox). +(tall and wide), Jacobi eigen-solver and Runge-Kutta integrators. Generic pieces this roadmap needs that +upstream does not ship yet — SE(3), motion profiles, a dense QP solver and others — are specified and +built upstream; see [Upstream prerequisites](#upstream-prerequisites-numerical-toolbox). ## Robot manipulators & other manipulator types @@ -38,76 +38,97 @@ Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a numerical-toolbox pin that ships it. -| # | Component | Upstream module | Spec | Needed by | -|------|-------------------------------------------------------|-----------------|----------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------| -| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md) | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30 | +| # | Component | Upstream module | Spec | Needed by | +|------|-------------------------------------------------------|----------------------|-----------------------------------|------------------------------------------------| +| NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30 | +| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21 | +| NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10 | +| NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional) | +| NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | +| NT58 | Symplectic integrators (semi-implicit Euler, Verlet) | `solvers` | [SymplecticIntegrators][nt58] | M34 | +| NT59 | Tridiagonal (Thomas) solver | `math` | [TridiagonalSolver][nt59] | NT61 | +| NT60 | S-curve (jerk-limited) profile | `motion_profiles` | [SCurveProfile][nt60] | M10 (time law, optional) | +| NT61 | Cubic spline interpolation (vector-valued) | `math` | [CubicSpline][nt61] | M33 | +| NT62 | Damped pseudo-inverse + null-space projector | `solvers` | [DampedPseudoInverse][nt62] | M14 | +| NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | +| NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23 | +| NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path) | + +[nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md +[nt54]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/AngleUtilities/implementation.md +[nt55]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/PolynomialProfile/implementation.md +[nt56]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/TrapezoidalProfile/implementation.md +[nt57]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/controllers/FrictionCompensation/implementation.md +[nt58]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/SymplecticIntegrators/implementation.md +[nt59]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/TridiagonalSolver/implementation.md +[nt60]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/SCurveProfile/implementation.md +[nt61]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/CubicSpline/implementation.md +[nt62]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/DampedPseudoInverse/implementation.md +[nt63]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md +[nt64]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/optimization/LevenbergMarquardt/implementation.md +[nt65]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/ActiveSetQp/implementation.md ### Manipulator list (by priority) -| # | Component | Target module | Depends on | Difficulty | -|-----|-----------------------------------------------------------------|---------------------------|------------|------------| -| M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | -| M2 | Cubic / quintic polynomial joint trajectory | `trajectory` (new) | — | ★☆☆☆☆ | -| M3 | Trapezoidal (LSPB) velocity profile + synchronization | `trajectory` (new) | — | ★☆☆☆☆ | -| M4 | Friction compensation (Coulomb + viscous + Stribeck) | `dynamics` | — | ★☆☆☆☆ | -| M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | -| M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M28 | Composite Rigid Body Algorithm (mass matrix) | `dynamics` | — | ★★☆☆☆ | -| M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | -| M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | -| M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | -| M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | NT53, M30 | ★★☆☆☆ | -| M9 | S-curve (jerk-limited) trajectory | `trajectory` (new) | M3 | ★★☆☆☆ | -| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53 | ★★☆☆☆ | -| M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | M8 | ★★☆☆☆ | -| M33 | Cubic spline through via points | `trajectory` (new) | — | ★★☆☆☆ | -| M34 | Forward-dynamics integrator (simulation step) | `dynamics` | — | ★★☆☆☆ | -| M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` | M29 | ★★★☆☆ | -| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, M8 | ★★★☆☆ | -| M14 | Redundancy resolution / null-space projection | `kinematics` | M8 | ★★★☆☆ | -| M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | -| M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | -| M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | -| M31 | Coriolis matrix and inertial-parameter regressor | `dynamics` | M28 | ★★★☆☆ | -| M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | -| M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | -| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | -| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | NT53 | ★★★★☆ | -| M22 | Dynamic (base-parameter) identification | `dynamics` | M31 | ★★★★☆ | -| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | NT53 | ★★★★☆ | -| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14 | ★★★★☆ | -| M25 | Cable-driven tension distribution | `controllers/manipulator` | NT53 | ★★★★☆ | -| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | NT53 | ★★★★☆ | -| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | +| # | Component | Target module | Depends on | Difficulty | +|-----|-----------------------------------------------------------------|---------------------------|----------------|------------| +| M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | +| M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | +| M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | +| M33 | Joint-space cubic spline through via points | `trajectory` (new) | NT61 | ★☆☆☆☆ | +| M28 | Composite Rigid Body Algorithm (mass matrix) | `dynamics` | — | ★★☆☆☆ | +| M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | +| M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | +| M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | +| M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | NT53, M30 | ★★☆☆☆ | +| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53, NT55 | ★★☆☆☆ | +| M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | M8 | ★★☆☆☆ | +| M14 | Redundancy resolution / null-space projection | `kinematics` | M8, NT62 | ★★☆☆☆ | +| M34 | Forward-dynamics integrator (simulation step) | `dynamics` | NT58 | ★★☆☆☆ | +| M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` | M29 | ★★★☆☆ | +| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | +| M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | +| M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | +| M17 | Impedance control | `controllers/manipulator` | M8, M29 | ★★★☆☆ | +| M31 | Coriolis matrix and inertial-parameter regressor | `dynamics` | M28 | ★★★☆☆ | +| M22 | Dynamic (base-parameter) identification | `dynamics` | M31, NT63 | ★★★☆☆ | +| M18 | Operational-space (task-space) control | `controllers/manipulator` | M8, M29 | ★★★★☆ | +| M19 | Hybrid position/force control | `controllers/manipulator` | M8, M29 | ★★★★☆ | +| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` | M31 | ★★★★☆ | +| M21 | Analytical IK for ortho-parallel 6R arms with a spherical wrist | `kinematics` | NT53, NT54 | ★★★★☆ | +| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | NT53, NT64 | ★★★★☆ | +| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14 | ★★★★☆ | +| M25 | Cable-driven tension distribution | `controllers/manipulator` | NT53, NT65 | ★★★★☆ | +| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | NT53 | ★★★★☆ | +| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | + +M2 (polynomial profile), M3 (trapezoidal profile), M4 (friction compensation), M6 (SE(3)) and M9 +(S-curve profile) moved to numerical-toolbox as NT55, NT56, NT57, NT53 and NT60; their numbers are +retired here. ### Tier 1 — Trivial ★☆☆☆☆ **M1. Generic joint link.** Evolve `RevoluteJointLink` (compatibly, via trailing defaulted fields) into a link with a joint type (revolute/prismatic), joint limits and armature, and add the prismatic branches to FK, RNEA, ABA, CRBA and the Jacobian. - *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; Featherstone (2008), Ch. 4. -**M2. Cubic / quintic polynomial joint trajectory.** Point-to-point motion with matched position/velocity(/acceleration) boundary conditions via closed-form polynomial coefficients. -- *Algorithm / paper:* Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control*, Ch. 5. - -**M3. Trapezoidal (LSPB) velocity profile.** Linear-segment-with-parabolic-blends profile respecting velocity/acceleration limits, with duration-constrained planning for multi-axis synchronization. -- *Algorithm / paper:* L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 3. - -**M4. Friction compensation model.** Feedforward Coulomb + viscous + Stribeck joint-friction term, clamped, added to any torque controller or folded into the chain dynamics model. -- *Algorithm / paper:* B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, *Automatica*, 30(7), 1994. - **M5. PD + gravity compensation control.** The simplest globally-stable set-point regulator: `τ = Kp·e − Kd·q̇ + g(q)`. - *Algorithm / paper:* M. Takegaki, S. Arimoto, *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* `g(q)` from M29. +- *Reuses / builds on:* `g(q)` from M29; optional friction feedforward from NT57. **M32. Inverse dynamics with an external tool wrench.** RNEA overload taking the environment's wrench on the tool; satisfies `τ(w) − τ(0) = −Jᵀw`. - *Algorithm / paper:* Luh, Walker, Paul (1980). +**M33. Joint-space cubic spline through via points.** Timed via points to a C² joint trajectory, rest-to-rest by default; defines the shared `JointTrajectoryState`. +- *Algorithm / paper:* Biagiotti & Melchiorri (2008), Ch. 4. +- *Reuses / builds on:* upstream `math::CubicSpline` (NT61), which carries the tridiagonal solve. + ### Tier 2 — Easy ★★☆☆☆ **M28. Composite Rigid Body Algorithm.** `O(n²)` joint-space mass matrix, sharing the spatial algebra of ABA. - *Algorithm / paper:* Walker & Orin (1982); Featherstone (2008), Ch. 6. **M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers. -- *Reuses / builds on:* RNEA, M28. +- *Reuses / builds on:* RNEA, M28; optional friction model from NT57. **M30. Chain pose kinematics.** Full tool pose and a model-independent frame chain (joint origins/axes in the base frame) for the link model. - *Algorithm / paper:* Siciliano et al. (2009), Ch. 2. @@ -118,20 +139,20 @@ numerical-toolbox pin that ships it. **M8. Geometric Jacobian (6×N).** Jacobian and `J̇q̇` from any frame chain, statics `τ = Jᵀw`, and the `JacobianProvider` seam used by IK and the task-space controllers; replaces IK's private 3×N Jacobian. - *Algorithm / paper:* Siciliano et al. (2009), Ch. 3; Lynch & Park (2017), Ch. 5. -**M9. S-curve (jerk-limited) trajectory.** Seven-segment jerk-bounded rest-to-rest profile with all short-move cases in closed form. -- *Algorithm / paper:* Biagiotti & Melchiorri (2008), Ch. 3. - **M10. Cartesian path + orientation interpolation.** Straight-line position with SLERP orientation, one shared time law, twist feed-forward. - *Algorithm / paper:* Lynch & Park, Ch. 9; K. Shoemake, *SIGGRAPH* 1985. +- *Reuses / builds on:* NT53 poses; any upstream motion profile (NT55, NT56, NT60) as the time law. **M11. Manipulability ellipsoid / Yoshikawa index.** `√det(JJᵀ)` (or `√det(JᵀJ)` when the task has more rows than joints), condition number and ellipsoid axes via SVD, translational and rotational parts separately. - *Algorithm / paper:* T. Yoshikawa, *Int. J. Robotics Research*, 4(2), 1985. -**M33. Cubic spline through via points.** C² multi-joint spline with clamped or natural ends, Thomas-algorithm solve. -- *Algorithm / paper:* Biagiotti & Melchiorri (2008), Ch. 4. +**M14. Redundancy resolution / null-space projection.** `q̇ = J⁺_λ ẋ + (I − J⁺J)·q̇₀` with an exact (undamped, rank-thresholded) projector, bound to the arm's task Jacobian. +- *Algorithm / paper:* A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977. +- *Reuses / builds on:* upstream `solvers::DampedPseudoInverse` (NT62). **M34. Forward-dynamics integrator.** Fixed-step semi-implicit Euler / RK4 simulation step on ABA for model-in-the-loop tests. - *Algorithm / paper:* Hairer, Lubich, Wanner (2006); Featherstone (2008), Ch. 7. +- *Reuses / builds on:* upstream `solvers::SemiImplicitEuler` (NT58) and `solvers::RungeKutta4`. ### Tier 3 — Moderate ★★★☆☆ @@ -140,9 +161,7 @@ numerical-toolbox pin that ships it. **M13. Full 6-DOF pose IK.** Damped least squares on the SE(3) log-map error with position/rotation weighting, adaptive damping, step and joint-limit clamping. - *Algorithm / paper:* S. R. Buss (2004); Nakamura & Hanafusa (1986); Chiaverini (1997). - -**M14. Redundancy resolution / null-space projection.** `q̇ = J⁺_λ ẋ + (I − J⁺J)·q̇₀` with an exact (undamped, rank-thresholded) projector. -- *Algorithm / paper:* A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977. +- *Reuses / builds on:* upstream `optimization::LevenbergMarquardt` (NT64) with a manipulability damping policy. **M15. Product-of-Exponentials forward kinematics.** Screw-theory FK and the space Jacobian, with conversion to the geometric Jacobian. - *Algorithm / paper:* Lynch & Park, *Modern Robotics*, Ch. 4. @@ -156,6 +175,10 @@ numerical-toolbox pin that ships it. **M31. Coriolis matrix and inertial regressor.** Modified RNEA giving the Christoffel `C(q,q̇)q̇_r` (skew-symmetric `Ṁ − 2C`), `Cᵀq̇`, and the 10-parameter-per-link regressor. - *Algorithm / paper:* Echeandia & Wensing (2021); Niemeyer & Slotine (1991); Atkeson, An, Hollerbach (1986). +**M22. Dynamic (base-parameter) identification.** Regressor rows streamed into a rank-revealing least-squares solve; the kept directions are the base parameters. +- *Algorithm / paper:* Atkeson, An, Hollerbach (1986); Gautier & Khalil (1990). +- *Reuses / builds on:* M31; upstream `estimators::IncrementalQrLeastSquares` (NT63). + ### Tier 4 — Advanced ★★★★☆ **M18. Operational-space control.** Task-space inertia `Λ = (J M⁻¹ Jᵀ)⁻¹`, `J̇q̇` compensation, full joint-space gravity/Coriolis compensation and a dynamically-consistent null space. @@ -169,25 +192,25 @@ numerical-toolbox pin that ships it. **M21. Analytical IK for ortho-parallel 6R arms with a spherical wrist (OPW).** Closed-form, all eight solutions, covering shoulder, lateral and elbow offsets of common industrial arms. - *Algorithm / paper:* M. Brandstötter, A. Angerer, M. Hofbaur, Austrian Robotics Workshop, 2014; D. Pieper, PhD thesis, Stanford, 1968. - -**M22. Dynamic (base-parameter) identification.** Streaming QR least squares on the regressor with SVD rank reduction to the base parameters. -- *Algorithm / paper:* Atkeson, An, Hollerbach (1986); Gautier & Khalil (1990). +- *Reuses / builds on:* NT53; joint wrapping from NT54. **M23. Parallel-manipulator kinematics.** Stewart-Gough (leg-length IK, Newton FK) and Delta (per-arm closed-form IK, three-sphere FK). - *Algorithm / paper:* J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel (1990). +- *Reuses / builds on:* NT53 retraction; the Stewart Newton FK runs on upstream `LevenbergMarquardt` (NT64). **M24. Mobile-manipulator / nonholonomic-base kinematics.** Combined base + arm Jacobian in the world frame with differential-drive constraints. - *Algorithm / paper:* Y. Yamamoto, X. Yun, *IEEE Trans. Automatic Control*, 39(6), 1994. -**M25. Cable-driven tension distribution.** Structure matrix from cable geometry and Pott's closed-form (improved) tension distribution within `[tMin, tMax]`. +**M25. Cable-driven tension distribution.** Structure matrix from cable geometry, Pott's closed-form (improved) tension distribution within `[tMin, tMax]`, and an exact QP path for the workspace boundary. - *Algorithm / paper:* Pott, Bruckmann, Mikelsons (2009); A. Pott (2014). +- *Reuses / builds on:* upstream `solvers::ActiveSetQp` (NT65) for the exact path. **M26. Continuum / soft constant-curvature kinematics.** Piecewise-constant-curvature FK and single-section IK. - *Algorithm / paper:* R. Webster, B. Jones, *Int. J. Robotics Research*, 29(13), 2010. ### Tier 5 — Hard / research-grade ★★★★★ -**M27. Time-optimal path parameterization (TOPP-RA).** Minimum-time traversal of a fixed path under joint velocity and torque limits via reachability analysis (two-variable LPs per grid stage). +**M27. Time-optimal path parameterization (TOPP-RA).** Minimum-time traversal of a fixed path under joint velocity and torque limits via reachability analysis (two-variable LPs per grid stage, solved locally by Fourier–Motzkin elimination). - *Algorithm / paper:* J. Bobrow, S. Dubowsky, J. Gibson, *IJRR*, 4(3), 1985; H. Pham, Q.-C. Pham, "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis," *IEEE T-RO*, 34(3), 2018. --- diff --git a/TESTING.md b/TESTING.md index 1ba8cf1..9d491f2 100644 --- a/TESTING.md +++ b/TESTING.md @@ -79,7 +79,7 @@ manipulability, redundancy resolution, PoE, analytical IK, parallel/mobile/conti ### 2. Dynamics — `dynamics/` `RecursiveNewtonEuler`, `ArticulatedBodyAlgorithm`, `EulerLagrangeSolver`, `NewtonEulerSolver`; -planned: CRBA, chain-dynamics model, regressor, friction, generic joints. +planned: CRBA, chain-dynamics model, regressor, generic joints (friction compensation is upstream, NT57). - **M1 analytic references** — single rod: holding torque `−mgl/2`, `τ = (ml²/3)·q̈`, free fall `q̈ = 3g/2l`; 2-link uniform rods: closed-form `M(q)`, `C(q,q̇)q̇`, `g(q)` and the horizontal @@ -94,7 +94,8 @@ planned: CRBA, chain-dynamics model, regressor, friction, generic joints. motion up to the integrator's error; `Ṁ − 2C` skew-symmetric for the Christoffel form. ### 3. Trajectory generation — `trajectory/` (planned) -Polynomial, trapezoidal, S-curve, Cartesian SLERP, TOPP. +Joint-space spline, Cartesian SLERP, TOPP (the scalar polynomial, trapezoidal and S-curve profiles are +upstream, NT55/NT56/NT60, and tested there with the same metrics). - **M1 boundary conditions** — endpoint position/velocity/acceleration matched exactly. - **M6 limits** — velocity, acceleration and jerk never exceed their bounds; degenerate short moves diff --git a/roadmap/README.md b/roadmap/README.md index b157f98..0cc0f2b 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -21,7 +21,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[trajectory/PolynomialTrajectory](trajectory/PolynomialTrajectory/implementation.md). +[controllers/manipulator/PdGravityCompensation](controllers/manipulator/PdGravityCompensation/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md) (NT53); dynamics @@ -89,10 +89,10 @@ prerequisites. `DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) ### `dynamics` -`GenericJointLink` (M1) · `FrictionCompensation` (M4) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CompositeRigidBodyAlgorithm` (M28) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) +`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CompositeRigidBodyAlgorithm` (M28) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) ### `trajectory` -`PolynomialTrajectory` (M2) · `TrapezoidalProfile` (M3) · `SCurveProfile` (M9) · `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) · `CubicSplineTrajectory` (M33) +`CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) · `CubicSplineTrajectory` (M33) ### `controllers/manipulator` `PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md index 663ea02..48a8587 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md @@ -22,7 +22,9 @@ to the mid-range value `(tMin + tMax)/2` that still reproduces the wrench, which comfortably away from slack and overload. If some cable still falls outside its range, the improved method pins the worst one at its limit, subtracts its pull from the wrench, and re-solves with the remaining cables — repeating at most once per cable, and reporting "not feasible" if too few cables are -left. +left. Pinning the worst cable is a greedy choice that can be wrong near the edge of the feasible +workspace; there, the exact variant solves the same "closest to mid-range" problem as a small quadratic +program, which finds a solution whenever one exists. ## Key parameters - **baseAnchors, platformAnchors** — winch exit points (world) and attachment points (platform frame). @@ -36,5 +38,6 @@ Over-Constrained Cable-Driven Parallel Robots," *Computational Kinematics*, 2014 ## See also `math::SE3Transform` (NT53, platform pose and wrench convention); `ParallelManipulatorKinematics` (M23, -platform pose from actuator lengths); `solvers::TrySolveSystem` (the small wrench-space solve, from -[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)). +platform pose from actuator lengths); `solvers::TrySolveSystem` (the small wrench-space solve) and +`solvers::ActiveSetQp` (NT65, the exact fallback), from +[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md index b75e7ca..50ac039 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md @@ -4,7 +4,9 @@ Closed-form force distribution for cable-driven parallel robots (Pott, Bruckmann, Mikelsons 2009) with Pott's improved iteration (2014). The structure matrix comes from the **cable geometry** — never from a -serial-arm Jacobian — and no QP solver is needed. +serial-arm Jacobian. The closed form is the deterministic fast path; near the boundary of the +wrench-feasible workspace, where it can fail, `DistributeExact` solves the same least-distance problem as a +QP with upstream `solvers::ActiveSetQp` (NT65). ## Data structures @@ -33,6 +35,10 @@ std::optional ComputeStructureMatrix(const math::SE3Transform Distribute(const WrenchVector& wrench, const math::SE3Transform& platformPose) const # hot path + +# exact minimum of ‖t − tm·1‖ subject to A·t = w, tMin ≤ t ≤ tMax; nullopt iff infeasible: +std::optional DistributeExact(const WrenchVector& wrench, + const math::SE3Transform& platformPose) const ``` ## Algorithm (pseudocode) @@ -64,12 +70,23 @@ function Distribute(w, pose): # OPTIMIZE_FOR_SPEED t[k] = clamp(t[k], tMin, tMax); fixed[k] = true; free −= 1 wFree = wFree − A[:, k]·t[k] # fixed cable's contribution moves to the wrench side return nullopt # fewer than WrenchDim free cables remain + +function DistributeExact(w, pose): # QP, NT65: ActiveSetQp + A = ComputeStructureMatrix(pose); if not A: return nullopt + qp = { hessian = I, gradient = −tm·1, + equalityMatrix = A, equalityVector = w, + inequalityMatrix = [I; −I], inequalityVector = (tMin·1; −tMax·1) } # t ≥ tMin, −t ≥ −tMax + r = ActiveSetQp.Solve(qp) + return r.status == Optimal ? r.x : nullopt # Infeasible: w outside the wrench-feasible set ``` ## Complexity & memory - Time: per pass `O(WrenchDim²·NumCables)` for `Af·Afᵀ` plus an `O(WrenchDim³)` solve; at most `NumCables − WrenchDim + 1` passes — deterministic, bounded, no iterative optimiser. +- `DistributeExact`: one Cholesky of the `NumCables×NumCables` identity (trivial) plus at most the QP's + iteration cap of `O(NumCables²)` steps; bounded but slower than the closed form — call it when + `Distribute` returns `nullopt`, not every cycle. - Memory: one `WrenchDim×NumCables` matrix, one `WrenchDim×WrenchDim` system, a `NumCables` flag array; stack only, no heap. @@ -82,8 +99,9 @@ function Distribute(w, pose): # OPTIMIZE_FOR_SPEED `NumCables = WrenchDim` it reduces to the unique `A⁻¹w` (`tm` cancels). - **Improved closed form (Pott 2014).** Fixing the worst-violating cable at its bound and re-solving the reduced system enlarges the usable workspace over the plain closed form. It can still return `nullopt` - close to the wrench-feasible workspace boundary where an exact LP/QP would succeed — treat `nullopt` as a - workspace-boundary event (no exceptions). + close to the wrench-feasible workspace boundary where the exact QP succeeds (spatial 8-cable fixture: + 90 of 4000 boundary-biased feasible wrenches); fall back to `DistributeExact` there, and treat its + `nullopt` as a true workspace-boundary event (no exceptions). - `TrySolveSystem` (upstream, pivot-threshold Gaussian elimination) reports rank deficiency — near-parallel cables or a degenerate pose — as `nullopt`; `Af·Afᵀ` is SPD otherwise, so `math::CholeskyDecomposition` (`numerical/math/CholeskyDecomposition.hpp`) is an equivalent choice. @@ -107,6 +125,6 @@ function Distribute(w, pose): # OPTIMIZE_FOR_SPEED - New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/controllers/manipulator/` folder. -- Depends on: NT53 (`math::SE3Transform`); `solvers::TrySolveSystem` from +- Depends on: NT53 (`math::SE3Transform`), NT65 (`solvers::ActiveSetQp`); `solvers::TrySolveSystem` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md index 3f4dafa..288a2cc 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md @@ -1,6 +1,6 @@ # Cable Tension Distribution — Unit Test Plan (Pseudocode) -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. No mocks needed: pure geometry + closed form. +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. No mocks needed: pure geometry, closed form and QP. ## Fixture @@ -20,6 +20,12 @@ class TestCableTensionDistributionSquare : public ::testing::Test: # 2 cables, class TestCableTensionDistributionSpatial : public ::testing::Test: # 8 cables, WrenchDim = 6 base[0] = (2, 2, 2), platform[0] = (0.2, 0, 0); remaining anchors: cube / box corners CableTensionDistribution distributor{ base, platform, tMin, tMax } + +class TestCableTensionDistributionCrossed : public ::testing::Test: # 8 cables, WrenchDim = 6, crossed + base = (±2, ±2, {0, 3}) in the order (−,−,0) (−,−,3) (−,+,0) (−,+,3) (+,−,0) (+,−,3) (+,+,0) (+,+,3) + platform = same order with x negated: (0.2,−0.2,−0.1) (0.2,−0.2,0.1) (0.2,0.2,−0.1) (0.2,0.2,0.1) + (−0.2,−0.2,−0.1) (−0.2,−0.2,0.1) (−0.2,0.2,−0.1) (−0.2,0.2,0.1) + pose = { I, p = (0.3, −0.2, 1.4) }, tMin = 10, tMax = 200 # rank(A) = 6 # each case below is a TEST_F(, ); default fixture TestCableTensionDistribution ``` @@ -65,6 +71,19 @@ square_case_unique_solution (TestCableTensionDistributionSquare): collinear_cables_return_nullopt (TestCableTensionDistributionSquare): Arrange: pose.p = (0, 1, 0) (both cables horizontal and opposite ⇒ rank(A) = 1) Assert: Distribute(w, pose) == nullopt + +exact_distribution_matches_closed_form_with_active_bound: + Arrange: w = (0, 150) + Assert: DistributeExact(w, pose) ≈ (200, 50, 50) == Distribute(w, pose) + +exact_distribution_succeeds_where_closed_form_fails (TestCableTensionDistributionCrossed): + Arrange: w = (56.8, −183.5, −35.3, −2.0, 41.7, −2.7) + Assert: Distribute(w, pose) == nullopt; + DistributeExact(w, pose) ≈ (200, 21.0187, 23.9864, 10, 88.1208, 197.9432, 76.4249, 23.7712), A·t ≈ w + +exact_distribution_reports_infeasible_wrench: + Arrange: w = (0, 300) + Assert: DistributeExact(w, pose) == nullopt ``` ## Reference vectors @@ -73,6 +92,11 @@ collinear_cables_return_nullopt (TestCableTensionDistributionSquare): `t = 105 + (2W/3)·(1, −½, −½)` — symmetric tensions on cables 1 and 2. - Upper-bound pass: fix `t₀ = 200` ⇒ `w' = (0, −50)` ⇒ square pair gives `t₁ = t₂ = 50`. - Square fixture: `u = (∓1, 1)/√2`, vertical `w = (0, W)` ⇒ `t₀ = t₁ = W/√2`. +- Planar `w = (0, 150)`: null line `t = (100, −50, −50) + β·(1, 1, 1)`, bounds ⇒ `β ∈ [60, 100]`, unconstrained + `β* = 105` ⇒ `β = 100` ⇒ `(200, 50, 50)` — the closed form is exact here. +- Crossed fixture: the exact optimum has cables 0 (`tMax`) and 3 (`tMin`) at bounds; found by enumerating + the `3⁸` bound patterns (python3/numpy). The improved closed form fixes cable 3 at `tMin`, then cable 5 at + `tMax` (the optimum keeps it at 197.9432) and cable 0, and runs out of free cables (6 < WrenchDim + 1). ## Edge cases diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md index f56b1cd..93d3426 100644 --- a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md +++ b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md @@ -28,5 +28,5 @@ M. Takegaki, S. Arimoto, "A New Feedback Method for Dynamic Control of Manipulat ## See also `ComputedTorqueControl` (full inverse-dynamics tracking); `ImpedanceControl` (compliant contact); -`FrictionCompensation` (adds a friction feedforward); `dynamics/EulerLagrangeDynamics` (the injected +upstream `controllers::FrictionCompensation` (NT57, adds a friction feedforward); `dynamics/EulerLagrangeDynamics` (the injected interface); `ChainDynamicsModel` (M29, its link-chain implementation). diff --git a/roadmap/dynamics/ChainDynamicsModel/implementation.md b/roadmap/dynamics/ChainDynamicsModel/implementation.md index 9e68fce..8ba9753 100644 --- a/roadmap/dynamics/ChainDynamicsModel/implementation.md +++ b/roadmap/dynamics/ChainDynamicsModel/implementation.md @@ -44,7 +44,7 @@ ComputeMassMatrix(q) = crba.Compute(links, q) ComputeCoriolisTerms(q, q̇) = rnea.InverseDynamics(links, q, q̇, 0, 0) # no gravity, no q̈ ComputeGravityTerms(q) = rnea.InverseDynamics(links, q, 0, 0, gravity) ComputeInverseDynamics(q,q̇,q̈) = rnea.InverseDynamics(links, q, q̇, q̈, gravity) # one O(n) pass -# with M1/M4 fields present: + armature ⊙ q̈ + friction(q̇) in ComputeInverseDynamics, +# with M1 fields and an NT57 friction model present: + armature ⊙ q̈ + friction(q̇) in ComputeInverseDynamics, # + diag(armature) in ComputeMassMatrix, friction reported separately (it is not part of C q̇) ``` diff --git a/roadmap/dynamics/DynamicParameterIdentification/explanation.md b/roadmap/dynamics/DynamicParameterIdentification/explanation.md index e8b9ba8..8695895 100644 --- a/roadmap/dynamics/DynamicParameterIdentification/explanation.md +++ b/roadmap/dynamics/DynamicParameterIdentification/explanation.md @@ -15,8 +15,9 @@ every model-based controller more accurate. For each recorded sample the dynamics give one linear equation per joint in the unknown parameters. Stacking many samples gives an over-determined least-squares problem. Some parameters never affect the torques (or only in fixed combinations), so the problem is rank-deficient; a rank-revealing -solution keeps only the identifiable combinations, the "base parameters". Processing samples one at a -time with orthogonal rotations keeps memory fixed and numerics well-conditioned. +solution keeps only the identifiable combinations, the "base parameters". The numerical toolbox's +incremental QR least squares processes samples one at a time with orthogonal rotations, which keeps memory +fixed and numerics well-conditioned; this item supplies the regressor rows. ## Key parameters - **Excitation trajectory** — decides how well-conditioned the problem is. @@ -28,5 +29,5 @@ C. Atkeson, C. An, J. Hollerbach, "Estimation of Inertial Parameters of Manipula Inertial Parameters of Serial Robots," *IEEE Trans. Robotics and Automation*, 6(3), 1990. ## See also -`CoriolisMatrixAndRegressor` (M31), `SlotineLiAdaptiveControl` (M20, on-line counterpart), -`FrictionCompensation` (M4). +`CoriolisMatrixAndRegressor` (M31), `SlotineLiAdaptiveControl` (M20, on-line counterpart), upstream +`IncrementalQrLeastSquares` (NT63, the solver) and `FrictionCompensation` (NT57). diff --git a/roadmap/dynamics/DynamicParameterIdentification/implementation.md b/roadmap/dynamics/DynamicParameterIdentification/implementation.md index b6360e5..0791493 100644 --- a/roadmap/dynamics/DynamicParameterIdentification/implementation.md +++ b/roadmap/dynamics/DynamicParameterIdentification/implementation.md @@ -1,12 +1,14 @@ # Dynamic (Base-Parameter) Identification — Implementation Pseudocode -> Roadmap ref: #M22 (Tier 4) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) +> Roadmap ref: #M22 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) Estimates the inertial parameters of a real arm from measured joint positions, velocities, accelerations and torques recorded along an exciting trajectory. The rigid-body model is linear in the parameters, `τ = Y(q, q̇, q̈)·π` (M31 regressor with `q̇r = q̇`, `q̈r = q̈`), so this is a linear least-squares problem — but only certain linear combinations of `π` (the **base parameters**) are -identifiable, so the solver must handle a rank-deficient regressor. +identifiable, so the solver must handle a rank-deficient regressor. The streaming, rank-revealing least +squares is upstream `estimators::IncrementalQrLeastSquares` (NT63); this item feeds it the regressor and +interprets the result. ## Data structures @@ -15,10 +17,7 @@ template # static_assert(std::is_ class DynamicParameterIdentification: static constexpr std::size_t P = 10 * Dof # full parameter count (M31 ordering) const InertialRegressor& regressor # ChainInertialRegressor (M31) - math::SquareMatrix R # streaming upper-triangular factor (STATE) - math::Vector d # Qᵀ·τ accumulated (STATE) - T residualSquares # ‖τ − Yπ‖² part outside range(R) (STATE) - std::size_t samples + estimators::IncrementalQrLeastSquares leastSquares # NT63 (STATE: P×P factor, no samples stored) struct IdentificationResult: math::Vector parameters # minimum-norm solution (identifiable projection of π) @@ -40,41 +39,33 @@ void Reset() ## Algorithm (pseudocode) ```text -function AddSample(q, q̇, q̈, τ): # streaming Givens QR, no sample storage - Y = regressor.Compute(q, q̇, q̇, q̈) # Dof × P - for each row k of Y (with right-hand side τ[k]): - (row, rhs) = (Y[k], τ[k]) - for j in 0..P−1: # rotate the row into R - if row[j] == 0: continue - (c, s) = Givens(R[j][j], row[j]) - rotate (R[j][j..P−1], d[j]) with (row[j..P−1], rhs) - residualSquares += rhs² - samples += 1 +function AddSample(q, q̇, q̈, τ): + leastSquares.AddRows(regressor.Compute(q, q̇, q̇, q̈), τ) # Dof rows of the P-column regressor function Solve(tol): - (U, σ, V) = SingularValueDecomposition(R) # upstream numerical-toolbox SVD, P×P - rank = #{ σ_i > tol·σ_max } - π̂ = Σ_{i)`. ## Deployment @@ -85,5 +76,5 @@ function Solve(tol): - Coverage: `robotics/dynamics/DynamicParameterIdentification.cpp` → the same instantiation. - Test: `robotics/dynamics/test/TestDynamicParameterIdentification.cpp` - Doc: `doc/dynamics/DynamicParameterIdentification.md` (per `doc/TEMPLATE.md`) -- Depends on: M31 (regressor); upstream SVD (numerical/solvers/SingularValueDecomposition.hpp). +- Depends on: M31 (regressor); NT63 (`estimators::IncrementalQrLeastSquares`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/DynamicParameterIdentification/tests.md b/roadmap/dynamics/DynamicParameterIdentification/tests.md index ab2f167..5d1282e 100644 --- a/roadmap/dynamics/DynamicParameterIdentification/tests.md +++ b/roadmap/dynamics/DynamicParameterIdentification/tests.md @@ -15,6 +15,9 @@ class TestDynamicParameterIdentification : public ::testing::Test: ## Test cases (Arrange / Act / Assert) +The streaming least-squares mechanics (order independence, block rows, minimum norm) are tested upstream +with `estimators::IncrementalQrLeastSquares` (NT63); these cases cover the robot model. + ```cpp base_parameter_count_with_gravity_is_six: Arrange: joint axes horizontal (gravity acts) @@ -26,8 +29,6 @@ noiseless_data_reproduces_torques: Assert: Predict(result, q, q̇, q̈) ≈ τ on held-out samples (≈ 1e-3 relative) identifiable_combinations_match_the_true_parameters: Assert: baseDirections·π̂ ≈ baseDirections·π_true -streaming_is_order_independent: - Assert: shuffling the sample order gives the same result no_samples_returns_nullopt: Assert: Solve(tol) == std::nullopt after Reset() ``` diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md index eed1bb0..f20fff2 100644 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md @@ -24,5 +24,5 @@ E. Hairer, C. Lubich, G. Wanner, *Geometric Numerical Integration* (2006), Ch. I R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 7. ## See also -`ArticulatedBodyAlgorithm`, upstream Runge-Kutta integrators (numerical-toolbox-cpp), the Robot Arm -simulator. +`ArticulatedBodyAlgorithm`, upstream `SymplecticIntegrators` (NT58) and Runge-Kutta integrators +(numerical-toolbox-cpp), the Robot Arm simulator. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md index e2bdd78..7c0e5f8 100644 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md @@ -4,7 +4,8 @@ A fixed-step simulator for a link chain: state `(q, q̇)`, dynamics `q̈ = ABA(q, q̇, τ)`. The simulator application currently hand-rolls this; a library version makes model-in-the-loop tests of the -controllers (M5, M12, M16–M20) reproducible. +controllers (M5, M12, M16–M20) reproducible. The integrators are upstream — `solvers::SemiImplicitEuler` +(NT58) and `solvers::RungeKutta4` — and this item is the adapter that exposes ABA as their system. ## Data structures @@ -15,45 +16,50 @@ template # static_assert(std::is_floati struct ChainState: JointVector q, qDot, qDDot # qDDot of the last evaluation (for logging / RNEA checks) -template -class ForwardDynamicsIntegrator: +template # implements solvers::SecondOrderSystem (NT58) +class ChainAccelerationSystem: # and solvers::OdeSystem for RK4 const LinkArray& links Vector3 gravity ArticulatedBodyAlgorithm aba + Acceleration(q, q̇, τ) = aba.ForwardDynamics(links, q, q̇, τ, gravity) + Derivative(x = (q; q̇), τ, t) = (q̇; Acceleration(q, q̇, τ)) + +template +class ForwardDynamicsIntegrator: + ChainAccelerationSystem system + solvers::SemiImplicitEuler or solvers::RungeKutta4 # by Method ``` ## Interface ```text -ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity) -ChainState Step(const ChainState& state, const JointVector& tau, T dt) const # hot path, τ held over dt +ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity, T dt) +ChainState Step(const ChainState& state, const JointVector& tau) # hot path, τ held over dt ``` ## Algorithm (pseudocode) ```cpp -function Step(x, τ, dt): # OPTIMIZE_FOR_SPEED - if Method == SemiImplicitEuler: # symplectic: bounded energy error - a = aba.ForwardDynamics(links, x.q, x.q̇, τ, gravity) - q̇ = x.q̇ + a·dt - q = x.q + q̇·dt - else: # RungeKutta4 on (q, q̇), zero-order-hold τ - f(q, q̇) = (q̇, aba.ForwardDynamics(links, q, q̇, τ, gravity)) - k1 = f(x); k2 = f(x + k1·dt/2); k3 = f(x + k2·dt/2); k4 = f(x + k3·dt) - (q, q̇) = x + (k1 + 2k2 + 2k3 + k4)·dt/6 - a = k1.acceleration # acceleration at the step start +function Step(x, τ): # OPTIMIZE_FOR_SPEED + a = system.Acceleration(x.q, x.q̇, τ) # acceleration at the step start (logged) + if Method == SemiImplicitEuler: # symplectic (NT58): bounded energy error + (q, q̇) = semiImplicitEuler.Step({ x.q, x.q̇ }, τ) + else: # RungeKutta4 on (q; q̇), zero-order-hold τ + (q, q̇) = rungeKutta4.Step((x.q; x.q̇), τ, 0) # time-invariant system return { q, q̇, a } ``` ## Complexity & memory -- Semi-implicit Euler: one ABA pass (`O(N)`) per step; RK4: four. +- Semi-implicit Euler: one ABA pass (`O(N)`) per step plus the logged one; RK4: four plus one. - Memory: two state copies; no heap. ## Numerical / embedded notes -- RK4 can reuse the upstream `solvers::RungeKutta4` with an `OdeSystem` adapter whose - `Derivative` evaluates ABA (numerical-toolbox-cpp); the virtual call is acceptable off the real-time path. +- Both integrators call the system through a virtual interface; acceptable off the real-time path + (simulation, MIL tests). +- The logged `qDDot` costs one extra ABA pass; drop it (or reuse the integrator's first evaluation) where + cycles matter. - Semi-implicit Euler keeps energy error bounded (no secular drift) at first-order accuracy; RK4 is fourth-order but slowly dissipative/drifting over long runs — choose by use case. - Joint friction or damping belongs in `τ` (e.g. `τ − b·q̇`), not as ad-hoc velocity scaling. @@ -66,6 +72,7 @@ function Step(x, τ, dt): # OPTIMIZE_FOR_SPEED `` and `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/ForwardDynamicsIntegrator.cpp` → the same instantiations. +- Depends on: NT58 (`solvers::SemiImplicitEuler`, `SecondOrderSystem`); upstream `solvers::RungeKutta4`. - Test: `robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp` - Doc: `doc/dynamics/ForwardDynamicsIntegrator.md` (per `doc/TEMPLATE.md`) - The simulator's `RobotArmSimulator::StepChain` should switch to this integrator once it exists. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md b/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md index 41ef93a..90e78f8 100644 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md @@ -7,8 +7,8 @@ ```cpp class TestForwardDynamicsIntegrator : public ::testing::Test: # single uniform rod about ŷ (pendulum) and a 2-link rod chain, gravity (0, 0, −9.81) - ForwardDynamicsIntegrator pendulum{ rod, gravity } - ForwardDynamicsIntegrator doubleRod{ rods, gravity } + ForwardDynamicsIntegrator pendulum{ rod, gravity, 1e-3f } + ForwardDynamicsIntegrator doubleRod{ rods, gravity, 1e-3f } # each case below is a TEST_F(TestForwardDynamicsIntegrator, ) ``` @@ -23,7 +23,7 @@ rk4_energy_drift_is_small: semi_implicit_euler_energy_stays_bounded: Assert: energy error over 10 s never exceeds its maximum over the first second by more than 2× step_acceleration_equals_articulated_body_algorithm: - Assert: Step(x, τ, dt).qDDot == ABA(links, x.q, x.q̇, τ, gravity) + Assert: Step(x, τ).qDDot == ABA(links, x.q, x.q̇, τ, gravity) constant_torque_on_free_link_gives_quadratic_angle: Arrange: z-axis rod (gravity ⟂ motion), τ constant Assert: q(t) ≈ ½·(τ / (mL²/3))·t² @@ -35,5 +35,5 @@ constant_torque_on_free_link_gives_quadratic_angle: ## Edge cases -- `dt = 0` ⇒ state unchanged. +- `dt` is fixed at construction (the upstream integrators take `h` there); `dt ≤ 0` is a precondition violation. - Zero gravity, zero torque ⇒ constant velocity. diff --git a/roadmap/dynamics/FrictionCompensation/explanation.md b/roadmap/dynamics/FrictionCompensation/explanation.md deleted file mode 100644 index 1a4bd92..0000000 --- a/roadmap/dynamics/FrictionCompensation/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Friction Compensation — Overview - -## What it is -A feedforward model of joint friction — the sum of **Coulomb** (constant drag), **viscous** -(speed-proportional drag), and **Stribeck** (extra "stiction" that fades as the joint starts -moving) terms — evaluated from joint velocity and added to a torque command to cancel real friction. - -## Why it matters (embedded) -Friction is the dominant un-modeled effect in geared robot joints: it causes stick-slip, tracking -lag, and steady-state error that pure PD/PID struggles to remove. A few multiplies, one `exp`, and -one `tanh` per joint — evaluated in the control loop — recover much of that lost accuracy without a -larger, slower feedback gain that would risk instability. - -## How it works (intuition) -Plot friction torque against velocity and you get a characteristic curve: a tall "breakaway" spike -near zero speed (stiction), dropping into a Coulomb plateau, then rising linearly (viscous) as speed -increases. The model reproduces that curve. Because the ideal curve is discontinuous at zero -velocity (the `sign` flip), a smooth `tanh` boundary layer replaces the hard sign so the feedforward -command does not chatter as the joint reverses direction. - -## Key parameters -- **F_c (Coulomb)** — constant drag magnitude once sliding. -- **F_s (stiction)** — higher breakaway level at near-zero speed (`F_s ≥ F_c`). -- **F_v (viscous)** — drag proportional to velocity. -- **v_s (Stribeck velocity)** — how quickly stiction fades into the Coulomb plateau. -- **ε (smoothing width)** — trades zero-crossing smoothness against compensation sharpness. - -## Reference -B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, "A survey of models, analysis tools and -compensation methods for the control of machines with friction," *Automatica*, 30(7), 1994. - -## See also -PD + gravity compensation (M5) and computed-torque control (M12), which add this term to their -control law; `RecursiveNewtonEuler` for the model-based torque it complements. diff --git a/roadmap/dynamics/FrictionCompensation/implementation.md b/roadmap/dynamics/FrictionCompensation/implementation.md deleted file mode 100644 index bfeeeaa..0000000 --- a/roadmap/dynamics/FrictionCompensation/implementation.md +++ /dev/null @@ -1,84 +0,0 @@ -# Friction Compensation (Coulomb + Viscous + Stribeck) — Implementation Pseudocode - -> Roadmap ref: #M4 (Tier 1) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct JointFrictionParameters: # one per joint - T coulomb # F_c — kinetic friction magnitude - T stiction # F_s — static/breakaway level (>= F_c) - T viscous # F_v — velocity-proportional coefficient - T stribeckVelocity # v_s — width of the Stribeck dip (> 0) - T stribeckShape # delta — exponent, typically 1 or 2 - T smoothingVelocity# eps — zero-crossing smoothing width (> 0) - T maxCompensation # |τ_f| cap — over-compensation can drive the joint (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -class FrictionCompensation: - std::array, NumJoints> params -``` - -## Interface - -```cpp -FrictionCompensation(const std::array, NumJoints>& params) - -# Feedforward friction torque to add to the control law: -Vector Compute(const Vector& qDot) const # hot path - -# Single-joint helper (also reused by the vector form): -static T JointTorque(const JointFrictionParameters& p, T qDot) -``` - -## Algorithm (pseudocode) - -```text -function JointTorque(p, v): # OPTIMIZE_FOR_SPEED - # Stribeck curve: stiction blends down to Coulomb as |v| grows - fall = exp( -(|v| / p.stribeckVelocity) ^ p.stribeckShape ) - level = p.coulomb + (p.stiction - p.coulomb) * fall - # smooth sign (tanh boundary layer) avoids chattering at v = 0 - dir = tanh(v / p.smoothingVelocity) - return clamp(level * dir + p.viscous * v, −p.maxCompensation, p.maxCompensation) - -function Compute(qDot): # OPTIMIZE_FOR_SPEED - for i in 0 .. NumJoints-1: - tau[i] = JointTorque(params[i], qDot[i]) - return tau -``` - -## Complexity & memory - -- `Compute`: `O(NumJoints)` — one `exp` and one `tanh` per joint, no allocation. -- Memory: `O(NumJoints)` parameter structs, all static/stack-resident. - -## Numerical / embedded notes - -- **Feedforward, not feedback:** add the result to the controller output - (`τ_cmd = τ_control + Compute(q̇)`); it composes with PD+gravity (M5) and computed-torque (M12). -- Feed **desired** velocity for a noise-free command, or **measured** velocity for accuracy — the - smoothing width `eps` trades tracking sharpness against chattering either way. -- The ideal `sign(v)` is replaced by `tanh(v/eps)`: too small an `eps` reintroduces limit cycles - near zero velocity; too large under-compensates stiction on slow moves. -- Over-compensation is worse than under (it can drive the joint), so the output is clamped to - `maxCompensation`; validate `stiction >= coulomb`, `stribeckVelocity > 0`, `smoothingVelocity > 0` - and `maxCompensation > 0` at construction. -- The same model can be folded into the dynamics: `ChainDynamicsModel` (M29) adds it to inverse - dynamics, and its parameters can be identified together with the inertial ones (M22). -- 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/dynamics/FrictionCompensation.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorque`, and - `extern template class FrictionCompensation;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/FrictionCompensation.cpp` → - `template class FrictionCompensation;` and ``. -- Test: `robotics/dynamics/test/TestFrictionCompensation.cpp` -- Doc: `doc/dynamics/FrictionCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestFrictionCompensation.cpp` → the `_test` target. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/dynamics/FrictionCompensation/tests.md b/roadmap/dynamics/FrictionCompensation/tests.md deleted file mode 100644 index 15c92d3..0000000 --- a/roadmap/dynamics/FrictionCompensation/tests.md +++ /dev/null @@ -1,66 +0,0 @@ -# Friction Compensation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestFrictionCompensation : public ::testing::Test: - # single-joint params: Fc=0.5, Fs=0.8, Fv=0.1, vs=0.05, delta=2, eps=1e-3, maxCompensation=10 - JointFrictionParameters p = MakeParams() - FrictionCompensation friction{ {p} } -# each case below is a TEST_F(TestFrictionCompensation, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -positive_velocity_positive_torque: - Arrange: qDot = +1.0 - Act: tau = Compute(qDot) - Assert: tau[0] > 0 and tau[0] ≈ Fc + Fv*1.0 (dip negligible at high v) - -sign_flips_with_velocity: - Arrange: compare Compute(+v) and Compute(-v) - Assert: tau(+v) ≈ -tau(-v) (odd symmetry) - -viscous_dominates_at_high_speed: - Arrange: sweep large |qDot| - Assert: tau grows ~linearly with slope Fv - -stiction_exceeds_coulomb_near_zero: - Arrange: v = 10·eps (tanh ≈ 1) and v << v_s - Assert: JointTorque(p, v) − Fv·v ≈ Fs (Stribeck peak) - -stribeck_decays_to_coulomb: - Arrange: v >> v_s - Assert: JointTorque(p, v) − Fv·v → Fc within tol - -smooth_through_zero: - Arrange: v = 0 - Assert: tau[0] == 0 (tanh(0)=0), continuous, no jump - -multi_joint_elementwise: - Arrange: FrictionCompensation, distinct params, qDot vector - Assert: each output equals its per-joint JointTorque - -feedforward_additive_cancels_model: - Arrange: plant friction torque = −model(q̇) (opposes motion); command = τ_control + Compute(q̇) - Assert: net torque reaching the rigid body ≈ τ_control (compensation cancels) -output_is_clamped_to_max_compensation: - Arrange: maxCompensation = 0.6, v large - Assert: |Compute(v)| == 0.6 -``` - -## Reference vectors - -- High speed `v=1`, dip≈0: `tau ≈ Fc·tanh(1/eps) + Fv·1 ≈ Fc + Fv = 0.6` (fixture `maxCompensation = 10`). -- `v=0`: `tau = 0` exactly (odd, smoothed). -- `v = v_s`: `fall = e^{-1}`, `level = Fc + (Fs−Fc)/e`. - -## Edge cases - -- `qDot = 0` — no torque, no NaN from the `^delta` power. -- Very small `eps` — steep but finite slope; assert no overflow. -- `Fs == Fc` — Stribeck term vanishes, pure Coulomb+viscous. -- Negative-going zero crossing — output stays continuous (no chatter). diff --git a/roadmap/dynamics/GenericJointLink/implementation.md b/roadmap/dynamics/GenericJointLink/implementation.md index db03819..e4ac05f 100644 --- a/roadmap/dynamics/GenericJointLink/implementation.md +++ b/roadmap/dynamics/GenericJointLink/implementation.md @@ -55,7 +55,7 @@ RNEA backward pass: child offset is r_{i+1} (= parentToJoint + z· ABA (Featherstone, S = (0; z) for prismatic): U = I^A·S, D = SᵀU + armature, u = τ − Sᵀp^A, c_i = v_i × S·q̇_i, transforms use r_i(q) CRBA (M28): S as above; M_ii += armature_i -Limits: consumed by IK clamping (M13), trajectory planners (M2/M3/M9/M33), TOPP (M27) +Limits: consumed by IK clamping (M13), trajectory planners (NT55/NT56/NT60, M33), TOPP (M27) ``` ## Complexity & memory diff --git a/roadmap/dynamics/MomentumObserver/explanation.md b/roadmap/dynamics/MomentumObserver/explanation.md index fb936f0..2bc813f 100644 --- a/roadmap/dynamics/MomentumObserver/explanation.md +++ b/roadmap/dynamics/MomentumObserver/explanation.md @@ -27,5 +27,5 @@ A. Albu-Schäffer, "Robot Collisions: A Survey on Detection, Isolation, and Iden Trans. Robotics*, 33(6), 2017. ## See also -`ChainDynamicsModel` (M29), `CoriolisMatrixAndRegressor` (M31), `FrictionCompensation` (M4), +`ChainDynamicsModel` (M29), `CoriolisMatrixAndRegressor` (M31), upstream `FrictionCompensation` (NT57), `RneaExternalWrench` (M32). diff --git a/roadmap/dynamics/MomentumObserver/implementation.md b/roadmap/dynamics/MomentumObserver/implementation.md index 809cdb8..4aa782c 100644 --- a/roadmap/dynamics/MomentumObserver/implementation.md +++ b/roadmap/dynamics/MomentumObserver/implementation.md @@ -47,7 +47,7 @@ function Update(q, q̇, τ, dt): # OPTIMIZE_FOR_SPEED ## Numerical / embedded notes -- Subtract a friction model (M4) from `τ` if friction is not part of `M, C, g`; otherwise friction +- Subtract a friction model (NT57) from `τ` if friction is not part of `M, C, g`; otherwise friction shows up as a (false) external torque. - The residual is a first-order low-pass of `τ_ext` with bandwidth `K_O`; larger gains detect faster but amplify model error and velocity noise. Thresholds are set above the residual seen in diff --git a/roadmap/kinematics/AnalyticalIkOpw/implementation.md b/roadmap/kinematics/AnalyticalIkOpw/implementation.md index e8eb4bd..3290eeb 100644 --- a/roadmap/kinematics/AnalyticalIkOpw/implementation.md +++ b/roadmap/kinematics/AnalyticalIkOpw/implementation.md @@ -86,7 +86,7 @@ function Solve(flange): # OPTIMIZE_FOR_SPEED — closed return slot function acosV(c): zero denominator or |c| > 1 + tol ⇒ invalid (branch unreachable); else acos(clamp(c, −1, 1)) -function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each wrapped to (−π, π] +function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each through math::WrapToPi (NT54, (−π, π]) ``` ## Complexity & memory @@ -121,5 +121,5 @@ function Emit(arm, w, singular): q = signs ⊙ ((arm, w) + offsets), each wrapp - Doc: `doc/kinematics/AnalyticalIkOpw.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestAnalyticalIkOpw.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`), NT54 (`math::WrapToPi`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md index 579ef85..6d4ccf9 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md @@ -34,6 +34,6 @@ J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, "Delta, a Fast Robot Geometry," *Int. Symp. on Industrial Robots*, 1988. ## See also -`math::SE3Transform` (NT53, platform pose and retraction), `LuDecomposition` -([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), the Newton step), +`math::SE3Transform` (NT53, platform pose and retraction), upstream `LevenbergMarquardt` (NT64, the +Newton step), `ManipulabilityIndex` (M11, singularity monitoring). diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md index 49b70cc..e87033c 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md @@ -22,6 +22,10 @@ template struct StewartForwardResult: SE3Transform pose; std::size_t iterations; bool converged +template # implements optimization::LeastSquaresProblem, 6, 6> +class StewartLegResidual: # used by Forward with upstream LevenbergMarquardt (NT64) + const StewartGoughKinematics& kinematics; std::array lengths + template class StewartGoughKinematics: StewartGoughGeometry geometry @@ -66,16 +70,16 @@ function LegJacobian(pose): # row i = (nᵢᵀ, (R·pᵢ × for i: a = pose.R·pᵢ; n = Normalize(a + pose.p − bᵢ); row i = (n ; CrossProduct(a, n)) # (v; ω): platform-origin velocity and angular velocity, base frame (NT53 order); statics: wrench (f; n) = Jᵀ·legForces -function Forward(L, guess): # Newton on SE(3) - x = guess - for iter in 0..maxIterations-1: - f = Inverse(x) − L - if ‖f‖ < tolerance: return { x, iter, true } - J = LegJacobian(x) - δ = −Jᵀ·LuDecomposition(J·Jᵀ + λ²·I₆).Solve(f) # λ = 0: δ = −J⁻¹f - x.p = x.p + δ.linear - x.R = SE3Transform::Exp((0; δ.angular), 1).R · x.R # left (base-frame) retraction - return { x, maxIterations, ‖Inverse(x) − L‖ < tolerance } +# Newton on SE(3) through upstream optimization::LevenbergMarquardt (NT64): +function StewartLegResidual::Residual(x): return kinematics.Inverse(x) − lengths +function StewartLegResidual::Jacobian(x): return kinematics.LegJacobian(x) # (v; ω) increments +function StewartLegResidual::Retract(x, δ): # left (base-frame) retraction + return { math::SE3Transform::Exp((0; δ.angular), 1).R · x.R, x.p + δ.linear } + +function Forward(L, guess): + r = optimization::LevenbergMarquardt{ StewartLegResidual{ *this, L }, ConstantDamping{ λ² }, + { tolerance, +∞, maxIterations } }.Solve(guess) + return { r.x, r.iterations, r.converged } # λ = 0: δ = −J⁻¹f (plain Newton) # ---- Delta ---- function ArmAngle(i, P): # circle (elbow) ∩ sphere (forearm) in arm i's plane @@ -113,8 +117,7 @@ function Forward(θ): # three-sphere intersection, c the leg residual still converges but the pose along the singular direction is ill-determined. - Delta branch choice: `atan2(b, a) ± acos(·)` are elbow-out / elbow-in; for `z < 0`, `b > 0` so the minus root has the larger `cos θ` (elbow out). The lower sphere intersection is the physical effector. -- Upstream `solvers::LuDecomposition` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)) - for the 6×6 solve. +- The 6×6 step solve (`LuDecomposition`) happens inside the upstream solver (NT64). - Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment @@ -128,5 +131,5 @@ function Forward(θ): # three-sphere intersection, c - Tests: `robotics/kinematics/test/TestStewartGoughKinematics.cpp`, `robotics/kinematics/test/TestDeltaKinematics.cpp` - Doc: `doc/kinematics/ParallelManipulatorKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; both tests → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`). +- Depends on: NT53 (`math::SE3Transform`), NT64 (`optimization::LevenbergMarquardt`, `ConstantDamping`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/explanation.md b/roadmap/kinematics/PoseInverseKinematics/explanation.md index 2ca0ccb..a9b47d4 100644 --- a/roadmap/kinematics/PoseInverseKinematics/explanation.md +++ b/roadmap/kinematics/PoseInverseKinematics/explanation.md @@ -38,5 +38,4 @@ Least Squares methods," 2004. ## See also `JacobianProvider` / `GeometricJacobian` (M8, the linearization), `math::SE3Transform::PoseError` (NT53, the orientation error), `ManipulabilityIndex` (M11), `AnalyticalIkOpw` (M21, the closed-form alternative -for ortho-parallel 6R arms), `LuDecomposition` -([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), the 6×6 solve). +for ortho-parallel 6R arms), upstream `LevenbergMarquardt` (NT64, the damped iteration). diff --git a/roadmap/kinematics/PoseInverseKinematics/implementation.md b/roadmap/kinematics/PoseInverseKinematics/implementation.md index 025a1c4..7a63f62 100644 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ b/roadmap/kinematics/PoseInverseKinematics/implementation.md @@ -3,7 +3,9 @@ > Roadmap ref: #M13 (Tier 3) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) Model-agnostic: the arm is a `JacobianProvider` (M8), so the same solver serves the link -chain (`ChainTaskJacobian`, M30/M8), DH (`DhTaskJacobian`, M7) and PoE (M15). +chain (`ChainTaskJacobian`, M30/M8), DH (`DhTaskJacobian`, M7) and PoE (M15). The iteration itself is +upstream `optimization::LevenbergMarquardt` (NT64); this item supplies the IK problem — weighted log-map +pose residual, weighted Jacobian, joint-limit retraction — and the singularity-robust damping policy. ## Data structures @@ -25,6 +27,14 @@ struct PoseIkResult: std::size_t iterations bool converged +template # implements optimization::LeastSquaresProblem +class PoseIkProblem: + const JacobianProvider& arm; math::SE3Transform target; T ρ; JointVector lower, upper + +template # implements optimization::DampingPolicy +class ManipulabilityDamping: + T λ₀, w₀ # w₀ = 0 ⇒ constant λ₀² + template class PoseInverseKinematics: const JacobianProvider& arm # Jacobian(q), ToolPose(q); injected @@ -41,36 +51,32 @@ PoseIkResult Solve(const SE3Transform& target, const JointVector& q0 ## Algorithm (pseudocode) ```text -function WeightedError(target, q): - e = SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), NT53 - return (e.linear; ρ·e.angular) - -function Solve(target, q0): # OPTIMIZE_FOR_SPEED - q = q0 - for iter in 0..maxIterations-1: - eW = WeightedError(target, q) - if ‖eW‖ < tolerance: return { q, ‖eW‖, iter, true } - JW = arm.Jacobian(q) with rows 3–5 scaled by ρ # W·J - A0 = JW·JWᵀ # 6×6 - λ² = Damping(A0) - y = LuDecomposition(A0 + λ²·I₆).Solve(eW) # never invert explicitly - Δq = JWᵀ·y # = Jᵀ W (W J Jᵀ W + λ² I)⁻¹ W e - if ‖Δq‖ > maxStep: Δq = Δq · maxStep / ‖Δq‖ - q = clamp(q + Δq, lowerLimits, upperLimits) - eFinal = ‖WeightedError(target, q)‖ # recomputed at the returned q - return { q, eFinal, maxIterations, eFinal < tolerance } - -function Damping(A0): # Nakamura–Hanafusa / Chiaverini adaptive form - if manipulabilityThreshold == 0: return λ₀² +# --- the IK problem handed to the upstream solver (residual r = −W·e, so δ = Jᵀ W (W J Jᵀ W + λ²I)⁻¹ W e) --- +function PoseIkProblem::Residual(q): + e = math::SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), NT53 + return −(e.linear; ρ·e.angular) +function PoseIkProblem::Jacobian(q): return arm.Jacobian(q) with rows 3–5 scaled by ρ # W·J +function PoseIkProblem::Retract(q, δ): return clamp(q + δ, lower, upper) # projected iteration + +function ManipulabilityDamping::DampingSquared(JW): # Nakamura–Hanafusa / Chiaverini adaptive form + if w₀ == 0: return λ₀² + A0 = JW·JWᵀ # 6×6 w = LuDecomposition(A0) singular ? 0 : sqrt(max(0, Determinant(A0))) # weighted manipulability - # (≡ 0 when Dof < 6 or near-singular ⇒ λ = λ₀) + # (≡ 0 when Dof < 6 or near-singular ⇒ λ = λ₀) return w < w₀ ? λ₀²·(1 − (w / w₀)²) : 0 + +function Solve(target, q0): # OPTIMIZE_FOR_SPEED + problem = PoseIkProblem{ arm, target, ρ, lowerLimits, upperLimits } + damping = ManipulabilityDamping{ λ₀, w₀ } + r = optimization::LevenbergMarquardt{ problem, damping, { tolerance, maxStep, maxIterations } }.Solve(q0) + return { r.x, r.residualNorm, r.iterations, r.converged } # residual recomputed at the returned q ``` ## Complexity & memory -- Per iteration: one `ToolPose` + `Jacobian` (`O(Dof)`), `O(36·Dof)` for `JW·JWᵀ`, `O(6³)` LU. -- One extra `ToolPose` after the loop for the final error. +- Per iteration: one `ToolPose` + `Jacobian` (`O(Dof)`), `O(36·Dof)` for `JW·JWᵀ`, `O(6³)` LU (in the + upstream wide-branch step), plus one more `JW·JWᵀ` and 6×6 LU in the damping policy when `w₀ > 0`. +- One extra `ToolPose` after the loop for the final error (upstream). - Memory: one `6×Dof` Jacobian and a `6×6` system; stack only. ## Numerical / embedded notes @@ -105,5 +111,6 @@ function Damping(A0): # Nakamura–Hanafusa / Chiaveri - Doc: `doc/kinematics/PoseInverseKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestPoseInverseKinematics.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform::PoseError`), M8 (`JacobianProvider`); upstream `solvers::LuDecomposition`. +- Depends on: NT53 (`math::SE3Transform::PoseError`), NT64 (`optimization::LevenbergMarquardt`), + M8 (`JacobianProvider`); upstream `solvers::LuDecomposition` (damping policy). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/RedundancyResolution/explanation.md b/roadmap/kinematics/RedundancyResolution/explanation.md index 7052850..6e9317c 100644 --- a/roadmap/kinematics/RedundancyResolution/explanation.md +++ b/roadmap/kinematics/RedundancyResolution/explanation.md @@ -33,6 +33,5 @@ Mechanisms," *IEEE Trans. Systems, Man, and Cybernetics*, 7(12), 1977; S. Chiave Task-Priority Redundancy Resolution," *IEEE Trans. Robotics and Automation*, 13(3), 1997. ## See also -`JacobianProvider` (M8, supplies `J`), `SingularValueDecomposition` -([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)), `ManipulabilityIndex` -(M11, a common secondary objective), `PoseInverseKinematics` (M13). +`JacobianProvider` (M8, supplies `J`), upstream `DampedPseudoInverse` (NT62, the damped inverse and +projector), `ManipulabilityIndex` (M11, a common secondary objective), `PoseInverseKinematics` (M13). diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md index 492b551..a94c850 100644 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ b/roadmap/kinematics/RedundancyResolution/implementation.md @@ -1,6 +1,10 @@ # Redundancy Resolution (Null-Space Projection) — Implementation Pseudocode -> Roadmap ref: #M14 (Tier 3) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) +> Roadmap ref: #M14 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) + +`q̇ = J⁺_λ·ẋ + (I − J⁺J)·q̇₀` for a redundant arm. The linear algebra — damped pseudo-inverse and exact +null-space projector from one SVD, tall or wide — is upstream `solvers::DampedPseudoInverse` (NT62); this +item binds it to the arm's task Jacobian. ## Data structures @@ -8,9 +12,7 @@ template # static_assert(std::is_floating_point_v); static_assert(Dof > TaskDim) class RedundancyResolution: # instantiated for float const JacobianProvider& jacobian # M8 seam; TaskDim 3 (position) or 6 (pose) - T damping # λ — primary term only - T rankTolerance # relative: σᵢ > rankTolerance·σ₀ counts toward rank - solvers::SingularValueDecomposition svd # of J (wide: U TaskDim×TaskDim, V Dof×TaskDim) + solvers::DampedPseudoInverse inverse # NT62: damping λ (primary only), rank tolerance ``` ## Interface @@ -26,48 +28,29 @@ Matrix NullSpaceProjector(const JointVector& q) # P = ## Algorithm (pseudocode) ```cpp -function Factor(q): - J = jacobian.Jacobian(q) # TaskDim × Dof - svd.Decompose(J) # J = U·Σ·Vᵀ - # uᵢ = column i of U (TaskDim): left singular vectors; vᵢ = column i of V (Dof): right singular vectors - τ = rankTolerance · σ₀; r = #{ σᵢ > τ } - -function Gain(σ): # damped inverse of one singular value - λ > 0 ? σ / (σ² + λ²) : (σ > τ ? 1/σ : 0) - function Resolve(q, ẋ, q̇₀): # OPTIMIZE_FOR_SPEED - Factor(q) - q̇ = Σᵢ Gain(σᵢ)·(uᵢ·ẋ)·vᵢ # primary: J⁺_λ·ẋ, lies in the row space of J - q̇ = q̇ + q̇₀ − Σ_{i 0` the primary task is met only - approximately: `‖J·q̇ − ẋ‖ ≤ λ²/(σ_min² + λ²)·‖ẋ‖` for `ẋ` in the range of `J`. The secondary term - adds nothing to the task velocity regardless of `λ` or `‖q̇₀‖`. -- The primary term lies in the row space (`P·J⁺_λ·ẋ = 0`), so it is the minimum-norm solution. -- Near a singularity `σ_min → 0`: the damped gain stays bounded (`≤ 1/(2λ)`), and the rank threshold - grows the null space (the lost task direction is not "protected" — by design). +- **What the tool sees:** the secondary term adds nothing to the task velocity regardless of `λ` or + `‖q̇₀‖`, because the projector is built undamped from the thresholded SVD (the damped `I − J⁺_λJ` would + leak: 7-DOF fixture, `λ = 0.01`: `‖J·P_λ·q̇₀‖ = 2.8e-4`). With `λ > 0` the primary task is met only + approximately: `‖J·q̇ − ẋ‖ ≤ λ²/(σ_min² + λ²)·‖ẋ‖`. +- Near a singularity `σ_min → 0` the damped gain stays bounded (`≤ 1/(2λ)`) and the rank threshold grows + the null space (the lost task direction is not "protected" — by design). - Common `q̇₀`: gradient of manipulability (M11), distance to joint limits, obstacle potentials. -- SVD: upstream `numerical/solvers/SingularValueDecomposition.hpp` - ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)). - Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment @@ -81,6 +64,5 @@ function NullSpaceProjector(q): Factor(q); return I_Dof − Σ_{i GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. +Projector algebra (idempotence, symmetry, right inverse, minimum norm, damping bound) is tested upstream +with `solvers::DampedPseudoInverse` (NT62). These cases cover the binding to a real redundant arm. + ## Fixture ```cpp @@ -23,18 +26,12 @@ class TestRedundancyResolution : public ::testing::Test: undamped_primary_task_is_exact: Arrange: RedundancyResolution{ pose, λ = 0 } Assert: pose.Jacobian(q)·Resolve(q, xDot, qDot0) ≈ xDot (tol 1e-5) -damped_primary_task_error_is_bounded: +damped_resolution_matches_reference: Arrange: λ = 0.01 - Assert: ‖J·Resolve(q, xDot, qDot0) − xDot‖ ≤ λ²/σ_min²·‖xDot‖ = 8.5e-4 (observed 4.4e-4) + Assert: Resolve(q, xDot, qDot0) ≈ (−0.093934, 0.325324, −0.092001, 0.551001, 0.088089, 0.448837, 0.140035) null_space_motion_is_task_invisible_despite_damping: Arrange: λ = 0.01 Assert: J·NullSpaceProjector(q)·qDot0 ≈ 0 (tol 1e-5) -projector_is_an_orthogonal_projector: - Assert: P·P ≈ P, Pᵀ ≈ P (tol 1e-5), trace P ≈ 1 (= Dof − rank) -undamped_pseudo_inverse_is_a_right_inverse: - Assert: J·DampedPseudoInverse(q)|_{λ=0} ≈ I₆ (tol 1e-5) -primary_term_is_minimum_norm: - Assert: P·Resolve(q, xDot, 0) ≈ 0 (no null-space component) secondary_objective_reduces_cost_without_moving_the_tool: Arrange: H(q) = ½Σ(qᵢ/2.9)²; qDot0 = −∇H; xDot = 0; λ = 0 Act: q̇ = Resolve(q, 0, qDot0) @@ -44,22 +41,18 @@ position_task_has_four_dimensional_null_space: Assert: trace NullSpaceProjector(q) ≈ 4; J_pos·P ≈ 0 rank_loss_grows_the_null_space: Arrange: stretched q_s = (0.1, 0, −0.3, 0, 0.4, 0, −0.2) (σ = 2, 1.950561, 0.498565, 0.311084, 0, 0) - Assert: trace P ≈ 3; P·P ≈ P; J(q_s)·P ≈ 0; Resolve finite with λ = 0.01 -zero_inputs_give_zero_motion: - Assert: Resolve(q, 0, 0) == 0 + Assert: trace P ≈ 3; J(q_s)·P ≈ 0; Resolve finite with λ = 0.01 ``` ## Reference vectors - 7R fixture at `q`: `σ(J) = (1.851950, 1.718255, 1.317197, 0.446306, 0.267222, 0.178251)`. -- `λ = 0`: `q̇ = (−0.094193, 0.326285, −0.092053, 0.552967, 0.088112, 0.449943, 0.140364)`. -- `λ = 0.01`: `q̇ = (−0.093934, 0.325324, −0.092001, 0.551001, 0.088089, 0.448837, 0.140035)`, - `‖Jq̇ − ẋ‖ = 4.44e-4`, `‖J·P·q̇₀‖` at rounding level. The old damped projector `I − J⁺_λJ` would give - `‖J·P_λ·q̇₀‖ = 2.85e-4` and `‖P_λ² − P_λ‖ = 1.98e-3` — do not use it. +- `λ = 0`: `Resolve(q, xDot, qDot0) = (−0.094193, 0.326285, −0.092053, 0.552967, 0.088112, 0.449943, 0.140364)`. +- `λ = 0.01`: `Resolve(q, xDot, qDot0) = (−0.093934, 0.325324, −0.092001, 0.551001, 0.088089, 0.448837, 0.140035)`, + `‖Jq̇ − ẋ‖ = 4.44e-4`, `‖J·P·q̇₀‖` at rounding level (python3/numpy, standard DH). - Position rows only: `σ = (0.839288, 0.805624, 0.257020)`, `trace P = 4`. ## Edge cases - `Dof = TaskDim` rejected at compile time (`static_assert(Dof > TaskDim)`); use M13 instead. - Conflicting or over-scaled `q̇₀` ⇒ primary task unaffected (projector exact); only the secondary objective suffers. -- Exact singularity with `λ = 0` ⇒ zero gain on lost directions (no blow-up), task partially met. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md index 7155884..ef24411 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md @@ -33,4 +33,4 @@ K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH* 1985 (SLERP ## See also `Quaternion` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) (provides `Slerp`), `math::SE3Transform` (NT53, pose and twist conventions), -`TrapezoidalProfile` / `PolynomialTrajectory` / `SCurveProfile` (the `s(t)` drivers). +upstream `TrapezoidalProfile` / `PolynomialProfile` / `SCurveProfile` (NT56/NT55/NT60, the `s(t)` drivers). diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md index f5aab44..194fb8d 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md @@ -7,8 +7,8 @@ Poses are `math::SE3Transform` (NT53); twists are `math::Vector6` ordered linear-first `(v; ω)`. Orientation is interpolated with the upstream `math::Quaternion` (`numerical/math/Quaternion.hpp`, [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp): -`FromRotationMatrix`, `Slerp`, `ToRotationMatrix`, `Conjugate`). `TrajectoryState` comes from -`robotics/trajectory/TrajectoryTypes.hpp`. +`FromRotationMatrix`, `Slerp`, `ToRotationMatrix`, `Conjugate`). `TrajectoryState` comes from upstream +`numerical/motion_profiles/MotionProfileTypes.hpp` (NT55). ```cpp template # static_assert(std::is_floating_point_v); instantiated for float @@ -19,7 +19,8 @@ struct CartesianState: # TimeLaw requirement (static polymorphism, no virtual call on the hot path): # TrajectoryState Sample(T t) const — position = progress s ∈ [0, 1], velocity = ṡ # T Duration() const — s(0) = 0, s(Duration()) = 1 -# e.g. TrapezoidalProfile{0, 1, limits} or PolynomialTrajectory{{.q0 = 0, .qf = 1}, tf, Degree::Quintic} +# e.g. upstream motion_profiles::TrapezoidalProfile{0, 1, limits} (NT56) or +# motion_profiles::PolynomialProfile{{.q0 = 0, .qf = 1}, tf, Degree::Quintic} (NT55) template # static_assert(std::is_floating_point_v); instantiated for float class CartesianSlerpInterpolation: @@ -89,19 +90,20 @@ including the sign-flipped (obtuse) case. - Header: `robotics/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class CartesianSlerpInterpolation>;` under + `extern template class CartesianSlerpInterpolation>;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp`, `numerical/math/SE3Transform.hpp` (NT53) and - `numerical/math/Quaternion.hpp`; `robotics.trajectory` links `robotics.kinematics`. +- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (NT55), `numerical/math/SE3Transform.hpp` (NT53) + and `numerical/math/Quaternion.hpp`; `robotics.trajectory` links `robotics.kinematics` and + `numerical.motion_profiles`. - Coverage: `robotics/trajectory/CartesianSlerpInterpolation.cpp` → - `template class CartesianSlerpInterpolation>;` + `template class CartesianSlerpInterpolation>;` - Test: `robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp` - Doc: `doc/trajectory/CartesianSlerpInterpolation.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. - CMake: `.hpp` → `target_sources(robotics.trajectory …)`; `robotics_add_coverage_sources(robotics.trajectory CartesianSlerpInterpolation.cpp)`; `TestCartesianSlerpInterpolation.cpp` → `robotics.trajectory_test`. - New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: NT53, M2 (any other `TimeLaw`, e.g. M3/M9, works without extra code). + `robotics_add_header_library(robotics.trajectory)`, a `test/` subdir, `add_subdirectory(trajectory)` in + `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. +- Depends on: NT53, NT55 (any other `TimeLaw`, e.g. NT56/NT60, works without extra code). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md index 1275e73..0db2ad0 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md @@ -6,7 +6,7 @@ ```cpp class TestCartesianSlerp : public ::testing::Test: - using Law = PolynomialTrajectory + using Law = motion_profiles::PolynomialProfile # upstream NT55 Law linearLaw{ { .q0 = 0, .qf = 1, .v0 = 1, .vf = 1 }, 1.0f, Degree::Cubic } # s(t) = t, ṡ = 1 math::SE3Transform start{ .R = I, .p = {0,0,0} } math::SE3Transform goal { .R = Rz(90°), .p = {1,0,0} } diff --git a/roadmap/trajectory/CubicSplineTrajectory/explanation.md b/roadmap/trajectory/CubicSplineTrajectory/explanation.md index d0a9e9a..1235e1c 100644 --- a/roadmap/trajectory/CubicSplineTrajectory/explanation.md +++ b/roadmap/trajectory/CubicSplineTrajectory/explanation.md @@ -2,24 +2,21 @@ ## What it is A joint-space trajectory through a list of **via points** reached at given times. Between consecutive -knots each joint follows a cubic polynomial; the pieces are chosen so position, velocity **and** -acceleration are continuous everywhere (C²). The ends are either **clamped** (prescribed velocities, -zero by default ⇒ rest-to-rest) or **natural** (zero end acceleration). +knots each joint follows a cubic polynomial; position, velocity **and** acceleration are continuous +everywhere (C²). The ends are either **clamped** (prescribed velocities, zero by default ⇒ rest-to-rest) +or **natural** (zero end acceleration). ## Why it matters (embedded) Teach-in programs, planner outputs and resampled time-optimal profiles all arrive as a list of waypoints. A single cubic spline turns them into one smooth reference without stopping at every point, -and without the ringing of high-order polynomials. Planning is one linear-time pass per joint on fixed -bounded storage; each servo tick is a binary search plus a cubic evaluation — cheap and deterministic. +and without the ringing of high-order polynomials. Planning is one linear-time pass on fixed bounded +storage; each servo tick is a binary search plus a cubic evaluation — cheap and deterministic. ## How it works (intuition) -Unknown are the accelerations at the knots. Once they are known, each segment is fixed: it must hit -both knot positions and match the two knot accelerations. Requiring the velocities of neighbouring -segments to agree at every interior knot gives one linear equation per knot that couples only the knot -and its two neighbours — a **tridiagonal** system. The end conditions add the first and last equations. -That system is strongly diagonally dominant, so the Thomas algorithm (a two-sweep Gaussian elimination) -solves it in linear time with no pivoting; the elimination factors depend only on the knot times and are -shared by all joints. +The generic part — fitting a smooth vector-valued curve through knots by solving one tridiagonal system +for the knot curvatures — is the shared cubic-spline interpolant of the numerical toolbox. This item uses +time as the abscissa and the joint vector as the value, so the curve's slope and curvature are the joint +velocity and acceleration, and it defaults the end slopes to zero so a move starts and ends at rest. ## Key parameters - **Knot times `t_k`** — strictly increasing; they set the speed between via points (stretching all of @@ -33,5 +30,6 @@ L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Rob (cubic splines: continuity conditions, clamped/natural ends, tridiagonal solution). ## See also -`PolynomialTrajectory` (the two-knot special case), `TimeOptimalPathParameterization` (its output can be -resampled into a spline), `TrapezoidalProfile` / `SCurveProfile` (single-segment, limit-driven moves). +Upstream `CubicSpline` (NT61, the interpolant) and `PolynomialProfile` (NT55, the two-knot special case); +`TimeOptimalPathParameterization` (M27, its output can be resampled into a spline); upstream +`TrapezoidalProfile` / `SCurveProfile` (NT56/NT60, single-segment, limit-driven moves). diff --git a/roadmap/trajectory/CubicSplineTrajectory/implementation.md b/roadmap/trajectory/CubicSplineTrajectory/implementation.md index 77c596e..2e7ee96 100644 --- a/roadmap/trajectory/CubicSplineTrajectory/implementation.md +++ b/roadmap/trajectory/CubicSplineTrajectory/implementation.md @@ -1,35 +1,40 @@ # Multi-Joint Cubic Spline Trajectory — Implementation Pseudocode -> Roadmap ref: #M33 (Tier 2) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) +> Roadmap ref: #M33 (Tier 1) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) + +Joint-space trajectory through timed via points. The interpolation itself — the C² vector-valued spline, +its tridiagonal solve and the clamped/natural ends — is upstream `math::CubicSpline` (NT61); this item +adds the trajectory semantics: knot times as the abscissa, rest-to-rest by default, joint-space state. ## Data structures -`JointTrajectoryState` comes from `robotics/trajectory/TrajectoryTypes.hpp` (canonical -definition: PolynomialTrajectory spec → Data structures). +Shared joint-space trajectory types — **canonical definition**, created once in +`robotics/trajectory/TrajectoryTypes.hpp` by the first of M33/M27 deployed; every other trajectory spec +includes it and never redefines these names (scalar `TrajectoryState`/`MotionLimits` are upstream, NT55): ```cpp -enum class EndCondition : uint8_t { Clamped, Natural } +template # static_assert(std::is_floating_point_v); multi-joint samplers (M27, M33) +struct JointTrajectoryState: + math::Vector position, velocity, acceleration +``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct SplineBoundary: - EndCondition condition = EndCondition::Clamped - math::Vector startVelocity{}, endVelocity{} # Clamped only; default zero (rest-to-rest) +This item: +```cpp template # static_assert(std::is_floating_point_v); class CubicSplineTrajectory: # static_assert(MaxPoints >= 2) using JointVector = math::Vector - array knotTimes # t_0 < t_1 < … < t_{K−1} - array knotPositions # q_k - array knotAccelerations # M_k = q̈(t_k), solved by Plan - array inverseSpan # 1/h_k, h_k = t_{k+1} − t_k - std::size_t count # K, 2 ≤ K ≤ MaxPoints + math::CubicSpline spline # upstream NT61; abscissa = time ``` +`SplineBoundary` / `SplineEnd` (upstream) are used as is: `Clamped` with zero slopes — the +default — is a rest-to-rest move. + ## Interface ```cpp -bool Plan(const array& times, const array& positions, - std::size_t count, const SplineBoundary& boundary = {}) +bool Plan(const std::array& times, const std::array& positions, + std::size_t count, const math::SplineBoundary& boundary = {}) # false if count ∉ [2, MaxPoints] or times not strictly increasing JointTrajectoryState Sample(T t) const # hot path; absolute time, precondition: Plan() == true T StartTime() const; T Duration() const # t_0, t_{K−1} − t_0 @@ -38,80 +43,38 @@ T StartTime() const; T Duration() const # t_0, t_{K−1} − t_0 ## Algorithm (pseudocode) ```text -# Per joint, unknowns M_k = q̈(t_k). On interval k (τ = t − t_k ∈ [0, h_k]): -# q(τ) = q_k + β_k·τ + (M_k/2)·τ² + d_k·τ³ -# β_k = (q_{k+1} − q_k)/h_k − h_k·(2M_k + M_{k+1})/6 # velocity at t_k -# d_k = (M_{k+1} − M_k)/(6h_k) -# Position interpolates the knots and acceleration is shared at knots ⇒ C⁰ and C² by construction. -# Velocity continuity at interior knots k = 1..K−2 gives the tridiagonal rows -# h_{k−1}·M_{k−1} + 2(h_{k−1} + h_k)·M_k + h_k·M_{k+1} = 6·[(q_{k+1} − q_k)/h_k − (q_k − q_{k−1})/h_{k−1}] -# End rows: -# Clamped: 2h_0·M_0 + h_0·M_1 = 6·[(q_1 − q_0)/h_0 − v_start] -# h_{K−2}·M_{K−2} + 2h_{K−2}·M_{K−1} = 6·[v_end − (q_{K−1} − q_{K−2})/h_{K−2}] -# Natural: M_0 = 0, M_{K−1} = 0 -# Every row is strictly diagonally dominant (|diag| = 2·Σ|off-diag|, or 1 vs 0) ⇒ Thomas, no pivoting. - function Plan(times, positions, count, boundary): - if count < 2 or count > MaxPoints: return false - for k in 0..count−2: if times[k+1] − times[k] ≤ ε: return false - store times, positions, count; inverseSpan[k] = 1/h_k - build sub[k], diag[k], sup[k] from h and boundary.condition # stack arrays of MaxPoints - # forward elimination factors depend only on the knot times ⇒ computed once for all joints - invPivot[0] = 1/diag[0]; supPrime[0] = sup[0]·invPivot[0] - for k = 1..K−1: - invPivot[k] = 1/(diag[k] − sub[k]·supPrime[k−1]); supPrime[k] = sup[k]·invPivot[k] - for j in 0..Dof−1: # O(K) per joint - r = right-hand side of joint j (rows above, v_start_j / v_end_j when Clamped, 0 when Natural) - y[0] = r[0]·invPivot[0] - for k = 1..K−1: y[k] = (r[k] − sub[k]·y[k−1])·invPivot[k] - M[K−1][j] = y[K−1] - for k = K−2 down to 0: M[k][j] = y[k] − supPrime[k]·M[k+1][j] - return true - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, t_0, t_{K−1}) - k = (upper_bound(knotTimes[1 .. K−1], t) index) clamped to [0, K−2] # binary search, O(log K) - τ = t − t_k; h = t_{k+1} − t_k; invH = inverseSpan[k] - for j in 0..Dof−1: - d = (M_{k+1,j} − M_{k,j})·invH/6 - β = (q_{k+1,j} − q_{k,j})·invH − h·(2M_{k,j} + M_{k+1,j})/6 - position_j = q_{k,j} + τ·(β + τ·(M_{k,j}/2 + τ·d)) - velocity_j = β + τ·(M_{k,j} + 3d·τ) - acceleration_j = M_{k,j} + 6d·τ - return { position, velocity, acceleration } -``` + return spline.Fit(times, positions, count, boundary) # end slopes = end joint velocities -Verified with python3: knots reproduced exactly, one-sided velocity/acceleration limits at interior knots -agree to `2e-15`, -clamped end velocities exact, natural end accelerations zero, collinear knots ⇒ `M ≡ 0`, two clamped -knots ⇒ exactly the `PolynomialTrajectory` cubic. +function Sample(t): # OPTIMIZE_FOR_SPEED + p = spline.Evaluate(t) # clamps t to [t_0, t_{K−1}] + return { p.value, p.firstDerivative, p.secondDerivative } # position, velocity, acceleration + +StartTime() = spline.First(); Duration() = spline.Last() − spline.First() +``` ## Complexity & memory -- `Plan`: `O(K)` shared factorization + `O(K)` per joint ⇒ `O(Dof·K)`; no pivoting, no iteration. -- `Sample`: `O(log K)` interval search + `O(Dof)` Horner evaluations. -- Memory: `MaxPoints·(2 + 2·Dof)` scalars in bounded `std::array`s; `Plan` uses `O(MaxPoints)` stack - scratch (tridiagonal bands, pivots, `y`). No heap. +- As upstream: `Plan` `O(Dof·K)`, `Sample` `O(log K + Dof)`; storage `MaxPoints·(2 + 2·Dof)` scalars. No heap. ## Numerical / embedded notes - Plan once (planning time), then `Sample` is deterministic: one binary search plus fixed arithmetic. -- Reject `h_k ≤ ε`: near-coincident knot times make the rows ill-conditioned and the accelerations huge. -- Clamped zero velocities (default) give a rest-to-rest move, but end accelerations `M_0`, `M_{K−1}` are - generally non-zero (an acceleration step at start/stop); Natural ends zero the accelerations but leave the - end velocities free — pick per application. +- Clamped zero velocities (default) give a rest-to-rest move, but end accelerations are generally non-zero + (an acceleration step at start/stop); Natural ends zero the accelerations but leave the end velocities + free — pick per application. - The spline does not enforce joint limits and may overshoot between knots; verify with `Sample`, or stretch all knot times by `λ ≥ 1` (velocities `/λ`, accelerations `/λ²`). - Good default knot times: proportional to the largest joint displacement per segment (or chord length). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/trajectory/CubicSplineTrajectory.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class CubicSplineTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). +- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it from the block above if this is the first of + M33/M27) and `numerical/math/CubicSpline.hpp` (NT61). - Coverage: `robotics/trajectory/CubicSplineTrajectory.cpp` → `template class CubicSplineTrajectory;` - Test: `robotics/trajectory/test/TestCubicSplineTrajectory.cpp` - Doc: `doc/trajectory/CubicSplineTrajectory.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. @@ -121,4 +84,5 @@ knots ⇒ exactly the `PolynomialTrajectory` cubic. - New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. +- Depends on: NT61 (`math::CubicSpline`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CubicSplineTrajectory/tests.md b/roadmap/trajectory/CubicSplineTrajectory/tests.md index 9e4abf5..d98663d 100644 --- a/roadmap/trajectory/CubicSplineTrajectory/tests.md +++ b/roadmap/trajectory/CubicSplineTrajectory/tests.md @@ -2,6 +2,9 @@ > GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. +The interpolation math (knot values, continuity, clamped/natural ends, reference second derivatives) is +tested upstream with `math::CubicSpline` (NT61). These cases cover only the trajectory mapping. + ## Fixture ```cpp @@ -11,74 +14,40 @@ class TestCubicSplineTrajectory : public ::testing::Test: std::array, 8> positions{ (0, 0.5, 0), (1, 0.2, 0), (−0.5, 0.2, 0), (0.3, −0.4, 0), (2, 0, 0) } Spline spline - void SetUp(): ASSERT_TRUE(spline.Plan(times, positions, 5)) # clamped, zero end velocities + void SetUp(): ASSERT_TRUE(spline.Plan(times, positions, 5)) # default: clamped, zero end velocities # each case below is a TEST_F(TestCubicSplineTrajectory, ) ``` ## Test cases (Arrange / Act / Assert) ```cpp -passes_through_every_knot: - Assert: Sample(t_k).position ≈ q_k for k = 0..4, every joint - -velocity_and_acceleration_continuous_at_interior_knots: - Assert: for k = 1..3: Sample(t_k − δ) ≈ Sample(t_k + δ) in velocity and acceleration - (δ = 1e-4, tol 1e-2 ≥ |jerk|·2δ), every joint - -knot_accelerations_match_reference: - Assert: joint 0: Sample(t_k).acceleration ≈ (5.665169, −5.330337, 5.991011, −0.737079, −1.898127); - joint 2 (all knots 0) ≡ 0 - -clamped_end_velocities_are_honoured: - Arrange: Plan(..., { Clamped, startVelocity = (0.4, 0, 0), endVelocity = (−0.7, 0, 0) }) - Assert: Sample(0).velocity_0 ≈ 0.4, Sample(4.5).velocity_0 ≈ −0.7; - Sample(0).acceleration_0 ≈ 4.296629, Sample(4.5).acceleration_0 ≈ −3.637453 - (fixture default: both end velocities ≈ 0) +default_plan_is_rest_to_rest: + Assert: Sample(0).position ≈ q_0, Sample(4.5).position ≈ q_4; Sample(0).velocity ≈ 0, Sample(4.5).velocity ≈ 0 -natural_ends_have_zero_acceleration: - Arrange: Plan(..., { Natural }) - Assert: Sample(0).acceleration ≈ 0, Sample(4.5).acceleration ≈ 0; - joint 0 end velocities ≈ 1.680287 and 0.783154 +state_maps_value_and_derivatives_to_position_velocity_acceleration: + Assert: joint 0 knot accelerations Sample(t_k).acceleration_0 ≈ (5.665169, −5.330337, 5.991011, + −0.737079, −1.898127) (the upstream second derivatives); Sample(0.5).velocity is the spline slope -collinear_knots_reproduce_straight_line: - Arrange: times {0, 0.5, 1, 1.5, 2}, q_k = 1 + 0.8·t_k (all joints), Clamped with v = 0.8 - Assert: Sample(0.7) ≈ { 1.56, 0.8, 0 }; acceleration ≈ 0 at every sampled t +start_time_and_duration_follow_the_knots: + Arrange: times shifted by +2 s + Assert: StartTime() ≈ 2, Duration() ≈ 4.5, Sample(2).position ≈ q_0 -two_knots_reduce_to_cubic_polynomial: +two_knots_reduce_to_the_cubic_polynomial_profile: Arrange: times {0, 2}, q {0, 1}, Clamped startVelocity 0.5, endVelocity −0.5; - PolynomialTrajectory{ { .q0 = 0, .qf = 1, .v0 = 0.5, .vf = −0.5 }, 2, Degree::Cubic } + motion_profiles::PolynomialProfile{ { .q0 = 0, .qf = 1, .v0 = 0.5, .vf = −0.5 }, 2, Degree::Cubic } Assert: position, velocity, acceleration equal at t ∈ {0.3, 1.0, 1.7}; knot accelerations (1, −2) -three_knot_reference_values: - Arrange: times {0, 1, 2}, q {0, 1, 0}, clamped zero - Assert: Sample(0.5) ≈ { 0.5, 1.5, 0 }; knot accelerations (6, −6, 6); Sample(1).velocity ≈ 0 - -sample_clamps_outside_domain: - Assert: Sample(−1) == Sample(0) and Sample(5.5) == Sample(4.5) - plan_rejects_invalid_knots: Assert: Plan(count = 1) == false; Plan(count = 9) == false; times {0, 1, 1, 2} == false ``` ## Reference vectors -Computed with python3 (tridiagonal system + Thomas solve, checked against the defining conditions). - -- Fixture joint 0 (`t = 0, 1, 2.5, 3, 4.5`; `q = 0, 1, −0.5, 0.3, 2`): - - clamped zero ⇒ `M = (5.665169, −5.330337, 5.991011, −0.737079, −1.898127)`; - - clamped `(0.4, −0.7)` ⇒ `M = (4.296629, −4.993258, 5.779775, −0.058427, −3.637453)`; - - natural ⇒ `M = (0, −4.081720, 5.605735, −1.400717, 0)`, end velocities `1.680287`, `0.783154`. -- Fixture joint 1 (`q = 0.5, 0.2, 0.2, −0.4, 0`), clamped zero ⇒ - `M = (−1.666292, 1.532584, −2.797753, 3.384270, −2.225468)`. -- Three knots `(0,0), (1,1), (2,0)`, clamped zero ⇒ `M = (6, −6, 6)`, `q(0.5) = 0.5`, `v(0.5) = 1.5` - (first interval is the rest-to-rest cubic `3τ² − 2τ³`); natural ⇒ `M = (0, −3, 0)`, `q(0.5) = 0.6875`. -- Two knots, `v0 = 0.5, vf = −0.5, tf = 2` ⇒ `M = (2a2, 2a2 + 6a3·tf) = (1, −2)`; rest-to-rest - `q0=0, qf=1, tf=2` ⇒ `q(1) = 0.5`, `v(1) = 0.75` (same as PolynomialTrajectory). -- Collinear knots with matching clamped velocity ⇒ right-hand side ≡ 0 ⇒ `M ≡ 0`. +- Fixture joint 0, clamped zero ⇒ knot accelerations `(5.665169, −5.330337, 5.991011, −0.737079, −1.898127)` + (python3; same values as the upstream spline test). +- Two knots, `v0 = 0.5, vf = −0.5, tf = 2` ⇒ `M = (2a2, 2a2 + 6a3·tf) = (1, −2)`. ## Edge cases -- `K = 2`, Natural ⇒ `M = 0`: straight line at constant velocity `(q1 − q0)/h` (end velocities free). -- Unequal spacing (fixture) — the formulation never assumes uniform `h`. -- Near-coincident knot times (`h ≤ ε`) ⇒ rejected by `Plan`, no division blow-up. -- `K = MaxPoints` (8) ⇒ accepted; no out-of-bounds access in the binary search at `t = t_{K−1}`. +- `K = MaxPoints` (8) ⇒ accepted; `Sample` at `t = t_{K−1}` in range. +- `t` outside `[t_0, t_{K−1}]` ⇒ clamped by the spline (end state). diff --git a/roadmap/trajectory/PolynomialTrajectory/explanation.md b/roadmap/trajectory/PolynomialTrajectory/explanation.md deleted file mode 100644 index 6d77ac7..0000000 --- a/roadmap/trajectory/PolynomialTrajectory/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Overview - -## What it is -A closed-form time law that drives a joint from a start state to a goal state over a fixed -duration. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches -endpoint acceleration, giving a smoother (bounded-jerk) motion. - -## Why it matters (embedded) -Point-to-point moves are the bread and butter of manipulator motion. The coefficients are solved -**once** from a handful of algebraic expressions, after which each servo tick is just a Horner -polynomial evaluation — no iteration, no lookup table, fully deterministic cycle count. - -## How it works (intuition) -A degree-`n` polynomial has `n+1` free coefficients. A cubic (4 coefficients) exactly satisfies -four boundary conditions — start/end position and start/end velocity. A quintic (6 coefficients) -adds start/end acceleration. Because the boundary equations are linear in the coefficients and the -time basis is fixed, the solution is a plug-in formula rather than a matrix solve. - -## Key parameters -- **Boundary conditions** — start/end position, velocity, and (quintic) acceleration. -- **Duration `tf`** — the move time; sets the peak velocity/acceleration for a given distance. -- **Degree** — cubic (velocity-matched) or quintic (acceleration-matched, jerk-limited endpoints). - -## Reference -M. W. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5 (polynomial -trajectories). - -## See also -`TrapezoidalProfile` (limit-respecting, not fixed-time), `SCurveProfile` (jerk-bounded), -`CubicSplineTrajectory` (many via points, C² — reduces to this cubic for two knots), -`CartesianSlerpInterpolation` (task-space paths; a 0 → 1 polynomial can drive its time law). diff --git a/roadmap/trajectory/PolynomialTrajectory/implementation.md b/roadmap/trajectory/PolynomialTrajectory/implementation.md deleted file mode 100644 index 9185d70..0000000 --- a/roadmap/trajectory/PolynomialTrajectory/implementation.md +++ /dev/null @@ -1,116 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Implementation Pseudocode - -> Roadmap ref: #M2 (Tier 1) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -Shared trajectory types — **canonical definition**, created once in -`robotics/trajectory/TrajectoryTypes.hpp` by the first trajectory item deployed; every other trajectory -spec includes it and never redefines these names: - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: # one scalar axis - T position{}, velocity{}, acceleration{}, jerk{} # jerk = 0 where a profile leaves it undefined - -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax{}, aMax{} # > 0 for every limit-driven profile - T jMax{} # > 0 required only by SCurveProfile - -template # static_assert(std::is_floating_point_v); multi-joint samplers (M27, M33) -struct JointTrajectoryState: - math::Vector position, velocity, acceleration -``` - -This item: - -```cpp -enum class Degree : uint8_t { Cubic = 3, Quintic = 5 } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct BoundaryConditions: # per joint - T q0, qf # start / end position - T v0 = 0, vf = 0 # start / end velocity - T a0 = 0, af = 0 # start / end acceleration (quintic only) - -template # static_assert(std::is_floating_point_v); instantiated for float -class PolynomialTrajectory: - array coeff # a0..a5 (cubic uses a0..a3, a4 = a5 = 0) - T duration # tf > 0 - Degree degree -``` - -## Interface - -```text -PolynomialTrajectory(BoundaryConditions bc, T tf, Degree degree) -TrajectoryState Sample(T t) # hot path -T Duration() -void Reset(BoundaryConditions bc, T tf) # keeps the degree -``` - -## Algorithm (pseudocode) - -```text -function solveCubic(bc, tf): # closed form, no linear solve - a0 = bc.q0 - a1 = bc.v0 - a2 = ( 3*(bc.qf-bc.q0) - (2*bc.v0 + bc.vf)*tf) / tf^2 - a3 = (-2*(bc.qf-bc.q0) + ( bc.v0 + bc.vf)*tf) / tf^3 - a4 = a5 = 0 - -function solveQuintic(bc, tf): # six matched boundary conditions - a0 = bc.q0; a1 = bc.v0; a2 = bc.a0 / 2 - a3 = ( 20*Δq - (8*bc.vf + 12*bc.v0)*tf - (3*bc.a0 - bc.af)*tf^2) / (2*tf^3) - a4 = (-30*Δq + (14*bc.vf + 16*bc.v0)*tf + (3*bc.a0 - 2*bc.af)*tf^2) / (2*tf^4) - a5 = ( 12*Δq - ( 6*bc.vf + 6*bc.v0)*tf - ( bc.a0 - bc.af)*tf^2) / (2*tf^5) - # Δq = qf - q0 - -constructor / Reset: - degree == Degree::Cubic ? solveCubic(bc, tf) : solveQuintic(bc, tf) - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, duration) - pos = Horner(coeff, t) # a0 + a1 t + ... + a5 t^5 - vel = Horner(derivative(coeff), t) - acc = Horner(secondDerivative(coeff), t) - jerk = Horner(thirdDerivative(coeff), t) # 6 a3 + 24 a4 t + 60 a5 t^2 - return { pos, vel, acc, jerk } -``` - -## Complexity & memory - -- Coefficient solve: `O(1)` once at construction (closed-form expressions). -- `Sample`: `O(degree)` — four Horner evaluations, ≤ 14 multiply-adds. -- Memory: `O(1)` — six coefficients, a duration and the degree; all stack-resident, no heap. - -## Numerical / embedded notes - -- Solve coefficients **once** at construction; `Sample` is then pure Horner — deterministic cycles. -- Multi-joint moves: hold one instance per joint; for many via points use `CubicSplineTrajectory` (M33). -- Guard `tf > 0`; the `1/tf^k` terms blow up for a zero-duration segment. -- Quintic gives continuous acceleration (zero jerk endpoints); prefer it when actuators are jerk-sensitive. -- A 0 → 1 instance (e.g. `v0 = vf = 1`, `tf = 1` cubic ⇒ `s = t`) is a valid `TimeLaw` for - `CartesianSlerpInterpolation`. -- 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/trajectory/PolynomialTrajectory.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class PolynomialTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it from the block above if this is the first - trajectory item; header-only aggregates, no coverage `.cpp`). -- Coverage: `robotics/trajectory/PolynomialTrajectory.cpp` → `template class PolynomialTrajectory;` -- Test: `robotics/trajectory/test/TestPolynomialTrajectory.cpp` -- Doc: `doc/trajectory/PolynomialTrajectory.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory PolynomialTrajectory.cpp)`; - `TestPolynomialTrajectory.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)` (links `numerical.math`), `TrajectoryTypes.hpp` in - `target_sources`, a `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, - and a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/PolynomialTrajectory/tests.md b/roadmap/trajectory/PolynomialTrajectory/tests.md deleted file mode 100644 index fe6c9ef..0000000 --- a/roadmap/trajectory/PolynomialTrajectory/tests.md +++ /dev/null @@ -1,63 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPolynomialTrajectory : public ::testing::Test: - BoundaryConditions restToRest{ .q0 = 0, .qf = 1 } # v0=vf=0 - PolynomialTrajectory cubic{ restToRest, 2.0f, Degree::Cubic } -# each case below is a TEST_F(TestPolynomialTrajectory, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -endpoints_match_boundary_positions: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 1 - -endpoint_velocities_are_matched: - Arrange: rest-to-rest cubic - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cubic_midpoint_is_symmetric: - Arrange: q0=0, qf=1, tf=2 rest-to-rest - Assert: Sample(1.0).position ≈ 0.5 (half distance at half time) - -cubic_peak_velocity_matches_formula: - Arrange: rest-to-rest, distance d, duration tf - Assert: max |velocity| ≈ 1.5 * d / tf (at midpoint) - -cubic_jerk_is_constant_third_derivative: - Arrange: q0=0, qf=1, tf=2 rest-to-rest cubic - Assert: Sample(t).jerk ≈ 6·a3 = -1.5 for t ∈ {0, 1, 2} - -quintic_endpoint_accelerations_are_zero: - Arrange: quintic, a0 = af = 0 - Assert: Sample(0).acceleration ≈ 0 and Sample(tf).acceleration ≈ 0 - -nonzero_boundary_velocity_is_honoured: - Arrange: v0 = 0.5, vf = -0.5 - Assert: Sample(0).velocity ≈ 0.5 and Sample(tf).velocity ≈ -0.5 - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) - -reset_recomputes_coefficients: - Arrange: Reset to q0=1, qf=3, tf=1 - Assert: new endpoints reproduced exactly -``` - -## Reference vectors - -- Rest-to-rest cubic, `q0=0, qf=1, tf=2`: `q(t) = 3(t/2)² − 2(t/2)³`; `q(1) = 0.5`, `v(1) = 0.75`. -- Peak velocity of a rest-to-rest cubic `= 1.5·(qf−q0)/tf`. -- Same cubic: `a3 = −2·(qf−q0)/tf³ = −0.25` ⇒ constant jerk `6·a3 = −1.5`. -- `q0=0, qf=1, v0=vf=1, tf=1` cubic ⇒ `a2 = a3 = 0`, i.e. `s(t) = t` (linear time law). - -## Edge cases - -- Very small `tf` ⇒ large coefficients; assert no overflow / `inf`. -- `q0 == qf` with zero boundary velocities ⇒ constant, zero velocity everywhere. -- Cubic vs quintic on the same move ⇒ quintic has smoother (bounded-jerk) acceleration. diff --git a/roadmap/trajectory/SCurveProfile/explanation.md b/roadmap/trajectory/SCurveProfile/explanation.md deleted file mode 100644 index 0ff0c53..0000000 --- a/roadmap/trajectory/SCurveProfile/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Overview - -## What it is -A **seven-segment, jerk-bounded** motion profile (a "double-S" velocity curve). It extends the -trapezoid by ramping acceleration in and out with a bounded rate of change (jerk), so acceleration -is continuous rather than stepping instantaneously. - -## Why it matters (embedded) -Sudden acceleration jumps excite structural resonances, wear gearing, and spill liquids or wobble -payloads. Limiting **jerk** smooths those transitions, cutting vibration and mechanical stress at -the cost of a slightly longer move. It is the profile of choice for high-precision or -vibration-sensitive machines (pick-and-place, CNC, semiconductor handling). - -## How it works (intuition) -The velocity curve is built from seven phases: jerk up, hold acceleration, jerk down (to reach -cruise velocity), cruise, then the mirror-image deceleration triple. Depending on the distance and -limits, some phases shrink to zero — a short move may never reach `aMax` or `vMax`. For rest-to-rest -moves the phase durations follow from a three-step closed form: assume `vMax` is reached; if the move is -too short, drop the cruise and assume only `aMax` is reached; if still too short, neither is reached and -the acceleration ramps are pure triangles. Several axes finish together by stretching the faster ones in -time by a common factor `λ` (velocity `/λ`, acceleration `/λ²`, jerk `/λ³`). - -## Key parameters -- **`vMax`** — cruise-velocity ceiling. -- **`aMax`** — acceleration ceiling (flat top of the acceleration curve). -- **`jMax`** — jerk ceiling; the slope of the acceleration ramps and the vibration knob. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -§3.4 (double-S / jerk-limited profiles; the non-zero boundary-velocity case there needs an iterative -`aMax` reduction and is future work). - -## See also -`TrapezoidalProfile` (jerk-unbounded predecessor), `PolynomialTrajectory` (fixed-time quintic), -`TimeOptimalPathParameterization` (limits along a geometric path). diff --git a/roadmap/trajectory/SCurveProfile/implementation.md b/roadmap/trajectory/SCurveProfile/implementation.md deleted file mode 100644 index 2e4b715..0000000 --- a/roadmap/trajectory/SCurveProfile/implementation.md +++ /dev/null @@ -1,122 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Implementation Pseudocode - -> Roadmap ref: #M9 (Tier 2) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `robotics/trajectory/TrajectoryTypes.hpp` -(canonical definition: PolynomialTrajectory spec → Data structures); not redefined here. This profile -requires `vMax`, `aMax`, `jMax > 0`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class SCurveProfile: - T q0, direction, jMax - T Tj, Ta, Tv # jerk sub-phase, whole accel phase, cruise - array segT # duration of each of the 7 phases - array segStart # start time of each phase (+ tf) - array segPos, segVel, segAcc # state at each phase boundary - T tf - bool reachesMaxAccel, reachesMaxVel -``` - -## Interface - -```text -SCurveProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path -T Duration() -bool ReachesMaxAccel() / ReachesMaxVel() -``` - -## Algorithm (pseudocode) - -Rest-to-rest closed form (Biagiotti & Melchiorri §3.4, `v0 = v1 = 0`), `h = |qf − q0|`: - -```cpp -# Seven phases: [+j][a=const][-j][v=const][-j][a=const][+j] -function plan(q0, qf, lim): - h = |qf - q0|; direction = sign(qf - q0); jMax = lim.jMax - if h == 0: all durations 0; return - # Step 1 — assume vMax is reached - if lim.vMax * lim.jMax >= lim.aMax^2: # aMax reached on the way to vMax - Tj = lim.aMax / lim.jMax - Ta = Tj + lim.vMax / lim.aMax - else: # triangular accel, peak jMax·Tj < aMax - Tj = sqrt(lim.vMax / lim.jMax) - Ta = 2*Tj - Tv = h / lim.vMax - Ta - if Tv < 0: - # Step 2 — vMax not reached (Tv = 0), assume aMax still reached - Tv = 0 - Tj = lim.aMax / lim.jMax - Δ = lim.aMax^4 / lim.jMax^2 + 4*lim.aMax*h - Ta = (lim.aMax^2 / lim.jMax + sqrt(Δ)) / (2*lim.aMax) - if Ta < 2*Tj: - # Step 3 — neither aMax nor vMax reached - Tj = cbrt(h / (2*lim.jMax)) - Ta = 2*Tj - aLim = jMax*Tj; vLim = aLim*(Ta - Tj) # peak |acc|, peak |vel| - reachesMaxAccel = (Ta > 2*Tj); reachesMaxVel = (Tv > 0) - segT = [Tj, Ta-2*Tj, Tj, Tv, Tj, Ta-2*Tj, Tj] # tf = 2·Ta + Tv - jerkOfPhase = [+jMax, 0, -jMax, 0, -jMax, 0, +jMax] - segPos[0] = segVel[0] = segAcc[0] = 0 - for i in 0..6: # exact constant-jerk integration - τ = segT[i]; j = jerkOfPhase[i] - segPos[i+1] = segPos[i] + segVel[i]*τ + segAcc[i]*τ^2/2 + j*τ^3/6 - segVel[i+1] = segVel[i] + segAcc[i]*τ + j*τ^2/2 - segAcc[i+1] = segAcc[i] + j*τ - segStart[i+1] = segStart[i] + τ - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - i = last phase with segStart[i] ≤ t (i ≤ 6); tau = t - segStart[i] - j = jerkOfPhase[i] - acc = segAcc[i] + j*tau - vel = segVel[i] + segAcc[i]*tau + 0.5*j*tau^2 - s = segPos[i] + segVel[i]*tau + 0.5*segAcc[i]*tau^2 + (1/6)*j*tau^3 - return { q0 + direction*s, direction*vel, direction*acc, direction*j } -``` - -Branch consistency (checked numerically on 20 000 random `h, vMax, aMax, jMax`): every branch ends at `h` -with zero velocity and acceleration, `|v| ≤ vMax`, `|a| ≤ aMax`, `|j| = jMax`; no phase duration is -negative (`Ta − 2Tj = vMax/aMax − aMax/jMax ≥ 0` in step 1a; the step-2 test guards it otherwise). - -## Complexity & memory - -- Planning: `O(1)` — at most three closed-form candidates (one `sqrt`, one `cbrt`), no iteration. -- `Sample`: `O(1)` — locate one of seven phases, evaluate a cubic-in-time (Horner). -- Memory: `O(1)` — four 8-entry tables plus seven durations; stack only, no heap. - -## Numerical / embedded notes - -- Precompute the seven durations and boundary states **once**; each tick is one cubic evaluation. -- Short moves collapse segments to zero length (`Tv = 0`, `Ta = 2·Tj`) — never a negative duration. -- Bounded jerk means acceleration is **continuous** ⇒ far less vibration than a trapezoid. -- Profile is symmetric about its midpoint for rest-to-rest moves (`Td = Ta`). -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1`: velocity, acceleration and jerk become `v/λ, a/λ², j/λ³`, sampled at - `t/λ`. Replanning with limits `(vMax/λ, aMax/λ², jMax/λ³)` gives exactly `Duration() = λ·T_axis` in every - branch (branch tests and the step formulas are invariant under this scaling) — planning stays `O(1)`. -- **Non-rest boundary velocities** (`v0, v1 ≠ 0`, B&M §3.4 general case) need the iterative `aMax` - reduction (`aMax ← γ·aMax`, `0 < γ < 1`) plus a feasibility pre-check; not covered — future work. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/trajectory/SCurveProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class SCurveProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). -- Coverage: `robotics/trajectory/SCurveProfile.cpp` → `template class SCurveProfile;` -- Test: `robotics/trajectory/test/TestSCurveProfile.cpp` -- Doc: `doc/trajectory/SCurveProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory SCurveProfile.cpp)`; - `TestSCurveProfile.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: M3 (shares the synchronization scheme). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/SCurveProfile/tests.md b/roadmap/trajectory/SCurveProfile/tests.md deleted file mode 100644 index bd35c63..0000000 --- a/roadmap/trajectory/SCurveProfile/tests.md +++ /dev/null @@ -1,87 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSCurveProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f, .jMax = 10.0f } - SCurveProfile profile{ 0.0f, 5.0f, limits } # full 7-segment move (step 1, aMax reached) -# each case below is a TEST_F(TestSCurveProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_at_rest_and_zero_accel: - Assert: Sample(0){vel,acc} ≈ 0 and Sample(tf){vel,acc} ≈ 0 - -velocity_never_exceeds_vmax: - Assert: max |velocity| ≤ vMax (1.0) across the profile - -acceleration_never_exceeds_amax: - Assert: max |acceleration| ≤ aMax (2.0) across the profile - -jerk_never_exceeds_jmax: - Assert: every sampled |jerk| ∈ {0, jMax}, ≤ jMax (10.0) - -acceleration_is_continuous: - Arrange: dense sampling across phase joins - Assert: no acceleration step between adjacent samples (bounded by jMax·dt) - -step1_full_profile_durations: # vMax·jMax ≥ aMax², vMax and aMax reached - Assert: Duration() ≈ 5.7; Sample(0.2).acceleration ≈ 2 (end of jerk phase, Tj = 0.2); - Sample(0.7).velocity ≈ 1 (end of accel phase, Ta = 0.7); - ReachesMaxAccel() and ReachesMaxVel() - -step1_triangular_acceleration_when_jerk_low: # vMax·jMax < aMax² - Arrange: limits { vMax=1, aMax=2, jMax=1 }, q0=0, qf=5 - Assert: Duration() ≈ 7; Sample(1).acceleration ≈ 1 (peak jMax·Tj < aMax); - Sample(3.5).velocity ≈ 1; !ReachesMaxAccel() and ReachesMaxVel() - -step2_short_move_reaches_amax_not_vmax: - Arrange: q0=0, qf=0.3 (fixture limits) - Assert: Duration() ≈ 1.0; Sample(0.5).velocity ≈ 0.6 (vLim, at tf/2); - max |acceleration| ≈ 2; ReachesMaxAccel() and !ReachesMaxVel(); Sample(tf).position ≈ 0.3 - -step3_very_short_move_reaches_neither: - Arrange: q0=0, qf=0.02 (fixture limits) - Assert: Duration() ≈ 0.4; Sample(0.1).acceleration ≈ 1 (aLim = jMax·Tj); - Sample(0.2).velocity ≈ 0.1 (vLim); !ReachesMaxAccel() and !ReachesMaxVel(); - Sample(tf).position ≈ 0.02 - -negative_direction_mirrors_profile: - Arrange: q0=5, qf=0 - Assert: Sample(t) = { 5 − p(t), −v(t), −a(t), −j(t) } of the fixture profile - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -Computed by integrating the 7-segment constant-jerk profile (python3, exact): each ends at `h` with -zero velocity/acceleration and respects `vMax`, `aMax`, `jMax`. - -| Case | Limits `(vMax, aMax, jMax)` | `h` | Branch | `Tj` | `Ta` | `Tv` | `tf` | `aLim` | `vLim` | -|------------|-----------------------------|------|---------------|------|------|------|------|--------|--------| -| full | (1, 2, 10) | 5 | 1 (`vj ≥ a²`) | 0.2 | 0.7 | 4.3 | 5.7 | 2 | 1 | -| low jerk | (1, 2, 1) | 5 | 1 (`vj < a²`) | 1.0 | 2.0 | 3.0 | 7.0 | 1 | 1 | -| short | (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | 2 | 0.6 | -| very short | (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | 1 | 0.1 | - -- Step 2 for `h = 0.3`: `Δ = 2⁴/10² + 4·2·0.3 = 2.56`, `Ta = (0.4 + 1.6)/4 = 0.5 ≥ 2·Tj = 0.4`. -- Step 3 for `h = 0.02`: `Tj = (0.02/20)^{1/3} = 0.1`. -- Branch thresholds for `(1, 2, 10)`: step 1 iff `h ≥ vMax·Ta = 0.7`; step 2 iff `h ≥ 2·aMax³/jMax² = 0.16`. -- Synchronization: replanning with `(vMax/λ, aMax/λ², jMax/λ³)` scales `tf` by exactly `λ` in every branch - (e.g. full case, `λ = 2` ⇒ `11.4`). - -## Edge cases - -- `jMax → ∞` ⇒ jerk phases vanish, profile degenerates to a trapezoid (cross-check limit). -- `h` exactly at a branch threshold (0.7 or 0.16 above) ⇒ neighbouring branches give the same `tf`. -- `d == 0` ⇒ zero-length profile; `Sample(0)` is the start, fully at rest. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md b/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md index 1fa8b94..4b11602 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md @@ -48,5 +48,5 @@ Implementation of the Time-Optimal Path Parameterization Algorithm," *IEEE Trans ## See also `CartesianSlerpInterpolation` (task-space path, mapped to `q(s)` through IK), `ChainDynamicsModel` / `InverseDynamicsModel` (M29, torque coefficients), -`CubicSplineTrajectory` (resample the result for the servo loop), `SCurveProfile` (heuristic +`CubicSplineTrajectory` (resample the result for the servo loop), upstream `SCurveProfile` (NT60, heuristic limit-respecting alternative). diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md index 033a49d..9d3e89e 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md @@ -8,7 +8,7 @@ is streamed with `Sample`. ## Data structures `JointTrajectoryState` comes from `robotics/trajectory/TrajectoryTypes.hpp` (canonical -definition: PolynomialTrajectory spec → Data structures). +definition: CubicSplineTrajectory spec → Data structures). ```cpp template using JointVector = math::Vector @@ -156,6 +156,9 @@ optima `min x` / `max x` without an iterative solver. - **Planning-time only**, never in an ISR. For a hard real-time loop, pre-sample the result into a joint-space `CubicSplineTrajectory` (M33) instead of calling the virtual path geometry per tick. +- The two-variable stage LPs stay local on purpose: they are linear (upstream `ActiveSetQp`, NT65, needs a + strictly convex cost) and the Fourier–Motzkin projection solves them exactly in `O(m²)` with no + iteration. - Stage time `Δt_i = 2Δ_i / (sqrt(x_i) + sqrt(x_{i+1}))` is exact for constant `u_i` and finite at the rest endpoints; never integrate `ds / sqrt(x)` (singular at `x = 0`). - Zero-inertia points (`a_ij = 0`) and `q'_j = 0` need no special case: the row becomes a pure `x` bound or diff --git a/roadmap/trajectory/TrapezoidalProfile/explanation.md b/roadmap/trajectory/TrapezoidalProfile/explanation.md deleted file mode 100644 index c586dfa..0000000 --- a/roadmap/trajectory/TrapezoidalProfile/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Overview - -## What it is -A **linear-segment-with-parabolic-blends** (LSPB) motion profile: accelerate at a constant rate, -cruise at a constant velocity, then decelerate at a constant rate. The velocity-vs-time graph is a -trapezoid (or a triangle for short moves that never reach cruise speed). - -## Why it matters (embedded) -It is the standard "get there as fast as the actuator allows" profile. Instead of fixing the -duration, you fix the **velocity and acceleration limits** and the profile computes the minimum -time that respects them — exactly what a motion controller needs to drive real hardware without -saturating drives. - -## How it works (intuition) -Time splits into three phases with a simple symmetry: the accel and decel ramps are mirror images. -If the move is long enough, velocity saturates at `vMax` and coasts; if it is short, the profile -peaks below `vMax` and immediately decelerates — a triangle. A single distance test -(`d ≥ vMax²/aMax`) decides which case applies. - -## Key parameters -- **`vMax`** — cruise velocity ceiling; caps the flat top of the trapezoid. -- **`aMax`** — ramp acceleration; sets the blend duration `vMax/aMax`. -- **Distance `qf − q0`** — decides trapezoid vs triangle and the total time. -- **Duration `T` (fixed-time variant)** — for synchronizing several axes: keep `aMax`, solve for the - lower cruise speed that finishes exactly at `T` (feasible iff `aMax·T² ≥ 4·distance`). - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (trapezoidal / LSPB profiles). - -## See also -`PolynomialTrajectory` (fixed-time, smooth), `SCurveProfile` (jerk-limited upgrade), -`SaturationRateLimiter` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) -(online slew limiting). diff --git a/roadmap/trajectory/TrapezoidalProfile/implementation.md b/roadmap/trajectory/TrapezoidalProfile/implementation.md deleted file mode 100644 index c601ecd..0000000 --- a/roadmap/trajectory/TrapezoidalProfile/implementation.md +++ /dev/null @@ -1,108 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Implementation Pseudocode - -> Roadmap ref: #M3 (Tier 1) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `robotics/trajectory/TrajectoryTypes.hpp` -(canonical definition: PolynomialTrajectory spec → Data structures); not redefined here. This profile -reads `vMax`, `aMax` and ignores `jMax`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class TrapezoidalProfile: - T q0, direction, distance # start, sign(qf - q0), |qf - q0| - T vPeak, aMax # cruise velocity actually reached, ramp acceleration - T tAccel, tCruise, tf # phase boundaries (blend, flat, total) -``` - -## Interface - -```cpp -TrapezoidalProfile(T q0, T qf, MotionLimits limits) # minimum-time plan -static std::optional PlanWithDuration(T q0, T qf, T aMax, T duration) - # fixed-time plan (synchronization) -TrajectoryState Sample(T t) # hot path; jerk = 0 -T Duration() -bool IsTriangular() # true when no cruise phase exists -``` - -## Algorithm (pseudocode) - -```cpp -function plan(q0, qf, limits): - distance = |qf - q0|; direction = sign(qf - q0); aMax = limits.aMax - dBlend = limits.vMax^2 / limits.aMax # distance used by accel + decel - if distance >= dBlend: # trapezoid: cruise phase exists - vPeak = limits.vMax - tAccel = vPeak / aMax - tCruise = (distance - dBlend) / vPeak - else: # triangle: peak below vMax - vPeak = sqrt(distance * aMax) - tAccel = vPeak / aMax - tCruise = 0 - tf = 2*tAccel + tCruise - -function PlanWithDuration(q0, qf, aMax, duration): - # d = v·(duration - v/aMax) ⇒ v² - aMax·duration·v + aMax·d = 0, take the smaller root - d = |qf - q0| - disc = aMax^2 * duration^2 - 4*aMax*d - if aMax <= 0 or duration <= 0 or disc < 0: # feasible iff aMax·duration² ≥ 4d - return nullopt - vPeak = 2*aMax*d / (aMax*duration + sqrt(disc)) # = (aMax·duration − sqrt(disc))/2, cancellation-free - tAccel = vPeak / aMax - tCruise = duration - 2*tAccel - tf = duration - return profile{ q0, sign(qf - q0), d, vPeak, aMax, tAccel, tCruise, tf } - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - if t < tAccel: # parabolic ramp-up - acc = aMax; vel = aMax*t; s = 0.5*aMax*t^2 - else if t < tAccel + tCruise: # linear cruise - acc = 0; vel = vPeak; s = vPeak*(t - 0.5*tAccel) - else: # parabolic ramp-down - td = tf - t - acc = -aMax; vel = aMax*td; s = distance - 0.5*aMax*td^2 - return { q0 + direction*s, direction*vel, direction*acc, 0 } -``` - -## Complexity & memory - -- Planning (both variants): `O(1)` — one `sqrt` and a branch. -- `Sample`: `O(1)` — one phase branch, a couple of multiply-adds. -- Memory: `O(1)` — eight scalars; entirely stack-resident, no heap. - -## Numerical / embedded notes - -- Precompute the three phase boundaries **once**; `Sample` then reduces to one branch per tick. -- Velocity is continuous but acceleration is **discontinuous** at blend joins (bounded jerk spikes) — - upgrade to `SCurveProfile` when that excites structural modes. -- Guard degenerate inputs: `distance == 0` ⇒ zero-length profile; `aMax <= 0` or `vMax <= 0` ⇒ reject. -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1` (`T_sync` = largest `Duration()`): its velocity, acceleration and - jerk become `v/λ, a/λ², j/λ³` and it is sampled at `t/λ`. Replanning that axis with limits - `(vMax/λ, aMax/λ²)` yields exactly `Duration() = λ·T_axis` (same branch, same shape). - `PlanWithDuration(q0, qf, aMax, T_sync)` is the alternative that keeps `aMax` and only lowers the cruise - speed; for `T_sync ≥` the minimum-time duration its `vPeak ≤ vMax` (the root decreases with duration). -- `PlanWithDuration` uses the conjugate form of the smaller root — the textbook - `(aMax·T − sqrt(disc))/2` cancels catastrophically in `float` when `4·aMax·d ≪ aMax²·T²`. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/trajectory/TrapezoidalProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TrapezoidalProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it if this is the first trajectory item). -- Coverage: `robotics/trajectory/TrapezoidalProfile.cpp` → `template class TrapezoidalProfile;` -- Test: `robotics/trajectory/test/TestTrapezoidalProfile.cpp` -- Doc: `doc/trajectory/TrapezoidalProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory TrapezoidalProfile.cpp)`; - `TestTrapezoidalProfile.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/TrapezoidalProfile/tests.md b/roadmap/trajectory/TrapezoidalProfile/tests.md deleted file mode 100644 index 60c59bd..0000000 --- a/roadmap/trajectory/TrapezoidalProfile/tests.md +++ /dev/null @@ -1,71 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestTrapezoidalProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f } - TrapezoidalProfile profile{ 0.0f, 5.0f, limits } # long move ⇒ trapezoid -# each case below is a TEST_F(TestTrapezoidalProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_are_at_rest: - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cruise_velocity_is_clamped_to_vmax: - Arrange: long move - Assert: max velocity over the profile ≈ vMax (1.0), never exceeds it - -acceleration_is_within_limit: - Assert: |acceleration| ≤ aMax at all sampled instants - -short_move_is_triangular: - Arrange: q0=0, qf=0.1 (d < vMax²/aMax) - Assert: IsTriangular() == true and peak velocity < vMax - -phase_durations_match_trapezoid_formula: - Arrange: d=5, vMax=1, aMax=2 - Assert: tAccel ≈ 0.5, tCruise ≈ 4.5, Duration ≈ 5.5 - -negative_direction_move: - Arrange: q0=2, qf=0 - Assert: velocity ≤ 0 throughout, endpoints (2 → 0) reached - -plan_with_duration_lowers_cruise_speed: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=5) - Assert: has_value; Duration() ≈ 5; Sample(0.5).velocity ≈ 0.5 (ramp at aMax); - Sample(2.5).velocity ≈ 1 (cruise vPeak = 1 < aMax·T/2); Sample(5).position ≈ 4 - -plan_with_duration_rejects_infeasible_time: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=3) # aMax·T² = 9 < 4d = 16 - Assert: == nullopt - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -- `d=5, vMax=1, aMax=2`: `tAccel = vMax/aMax = 0.5`, `tCruise = (d − vMax²/aMax)/vMax = 4.5`, - `tf = 5.5`. -- Triangular threshold distance `= vMax²/aMax = 0.5`. -- `PlanWithDuration`, `d=4, aMax=1, T=5`: `v = (aMax·T − sqrt(aMax²T² − 4·aMax·d))/2 = (5 − 3)/2 = 1`, - `tAccel = 1`, `tCruise = 3`. `d=5, aMax=2, T=6`: `v = 0.900980`, `tAccel = 0.450490`, - `tCruise = 5.099020`. Feasible iff `aMax·T² ≥ 4d` (equality ⇒ triangle, `v = aMax·T/2`). -- Synchronization: replanning with `(vMax/λ, aMax/λ²)` scales `Duration()` by exactly `λ` - (e.g. fixture, `λ = 2` ⇒ `11.0`). - -## Edge cases - -- `d` exactly equal to `vMax²/aMax` ⇒ zero cruise time (trapezoid degenerates to triangle). -- `d == 0` ⇒ `tf == 0`, `Sample(0)` is the start at rest. -- `aMax` very large ⇒ ramps vanish, profile approaches a pure velocity step. -- `PlanWithDuration` with `d == 0` ⇒ `vPeak = 0`, holds `q0` at rest for the whole duration.