diff --git a/.github/agents/algo-implementer.agent.md b/.github/agents/algo-implementer.agent.md index 1433d9d..97b5705 100644 --- a/.github/agents/algo-implementer.agent.md +++ b/.github/agents/algo-implementer.agent.md @@ -8,6 +8,8 @@ handoffs: prompt: "Review the deployed algorithm against AGENTS.md: float-only, no heap, tests, doc, CMake wiring." --- +# Algorithm Implementer + You deploy ONE algorithm at a time from its `roadmap///` spec into the codebase. Authoritative rules: `AGENTS.md`. Recipe: `roadmap/DEPLOYMENT.md`. Follow both exactly. diff --git a/.github/agents/executor.agent.md b/.github/agents/executor.agent.md index 91570a6..09c58f4 100644 --- a/.github/agents/executor.agent.md +++ b/.github/agents/executor.agent.md @@ -8,6 +8,8 @@ handoffs: prompt: "Review the implementation changes made above against robotics-toolbox project standards." --- +# Executor + Canonical rules: `AGENTS.md`. Implement exactly what's asked — nothing more. ## Workflow diff --git a/.github/agents/modernizer.agent.md b/.github/agents/modernizer.agent.md index f264cbc..b01d414 100644 --- a/.github/agents/modernizer.agent.md +++ b/.github/agents/modernizer.agent.md @@ -8,6 +8,8 @@ handoffs: prompt: "Review the refactoring changes made above against robotics-toolbox project standards." --- +# Modernizer + Canonical rules: `AGENTS.md`. You modernize ONE pre-roadmap algorithm at a time so it matches current roadmap conventions. Refactor is **behavior-preserving** — no new features, no public-API change unless required to remove duplication. @@ -39,15 +41,12 @@ The `test/Test*.cpp` is a first-class deliverable of every modernization. Audit - [ ] `EXPECT_NEAR` + `math::Tolerance()` for float comparisons. - [ ] Anonymous-namespace fixture; macros outside; no heap; no comments. -Keep `TYPED_TEST` where the algorithm is multi-type; behavior and assertions stay identical. +Behavior and assertions stay identical when restructuring a test. ## Preserve types — hard rule -Keep existing `Q15`/`Q31` support and its `TYPED_TEST` where the algorithm already has it — do -**NOT** strip multi-type. The multi-type guard -(`static_assert(math::is_qnumber::value || std::is_floating_point_v, ...)`) stays for those. -Float-only migration applies ONLY to algorithms that are already float-only; those follow -`static_assert(std::is_floating_point_v)` + `TEST_F` on `float`. +Every algorithm in this repository is float-only: `static_assert(std::is_floating_point_v)` + +`TEST_F` on `float`. Do not introduce `Q15`/`Q31` or `TYPED_TEST` while modernizing. ## Memory — quick reference diff --git a/.github/agents/orchestrator.agent.md b/.github/agents/orchestrator.agent.md index 7547913..77f122d 100644 --- a/.github/agents/orchestrator.agent.md +++ b/.github/agents/orchestrator.agent.md @@ -18,6 +18,8 @@ handoffs: prompt: "Refactor the pre-roadmap algorithm described above to reuse shared utilities and simplify tests, following robotics-toolbox conventions." --- +# Orchestrator + Triage requests and route to the right specialist. Do NOT implement or plan yourself. ## Workflow diff --git a/.github/agents/planner.agent.md b/.github/agents/planner.agent.md index ad081f4..b60576c 100644 --- a/.github/agents/planner.agent.md +++ b/.github/agents/planner.agent.md @@ -8,6 +8,8 @@ handoffs: prompt: "Implement the plan outlined above, following all project conventions strictly." --- +# Planner + Canonical rules: `AGENTS.md`. Produce plans only — no code edits. ## Workflow diff --git a/.github/agents/reviewer.agent.md b/.github/agents/reviewer.agent.md index b9396f7..ead8416 100644 --- a/.github/agents/reviewer.agent.md +++ b/.github/agents/reviewer.agent.md @@ -11,6 +11,8 @@ handoffs: prompt: "Revise the implementation plan based on the review feedback above." --- +# Reviewer + Canonical rules: `AGENTS.md`. Review only — no file modifications. ## Workflow diff --git a/.github/agents/unit-tester.agent.md b/.github/agents/unit-tester.agent.md index cf9180b..d164642 100644 --- a/.github/agents/unit-tester.agent.md +++ b/.github/agents/unit-tester.agent.md @@ -8,6 +8,8 @@ handoffs: prompt: "Review the authored unit tests against AGENTS.md, testing.instructions.md, and TESTING.md: TEST_F on float, StrictMock only, no heap, one behaviour per test, independent reference values, no redundant cases, CMake wiring, tests green." --- +# Unit Tester + You add or extend the unit tests for ONE algorithm at a time in `robotics//test/Test.cpp`. Authoritative rules: `AGENTS.md`. Testing rules: `.github/instructions/testing.instructions.md`. Metric families per algorithm: diff --git a/.github/instructions/robotics-cpp.instructions.md b/.github/instructions/robotics-cpp.instructions.md index 20b1645..809c3b4 100644 --- a/.github/instructions/robotics-cpp.instructions.md +++ b/.github/instructions/robotics-cpp.instructions.md @@ -67,6 +67,8 @@ Apply `OPTIMIZE_FOR_SPEED` (from `numerical/math/CompilerOptimizations.hpp`) on ## Documentation — MANDATORY -For every algorithm added or modified, update the corresponding `doc/{domain}/{AlgorithmName}.md` file. Follow `doc/TEMPLATE.md` exactly. Documentation is **design-first**: cover mathematical background, algorithm behaviour, complexity, pitfalls, and connections. Do **not** include implementation details, class names, template parameters, or usage code examples — docs describe the algorithm design; code follows from it. +For every algorithm added or modified, update the corresponding `doc/{domain}/{AlgorithmName}.md` file. Follow `doc/TEMPLATE.md` exactly. +Documentation is **design-first**: cover mathematical background, algorithm behaviour, complexity, pitfalls, and connections. +Do **not** include implementation details, class names, template parameters, or usage code examples — docs describe the algorithm design; code follows from it. Canonical rules: [AGENTS.md](../../AGENTS.md). diff --git a/.github/instructions/testing.instructions.md b/.github/instructions/testing.instructions.md index a108a1e..c69313d 100644 --- a/.github/instructions/testing.instructions.md +++ b/.github/instructions/testing.instructions.md @@ -20,21 +20,23 @@ applyTo: "**/test/**" ## Fixture Test Pattern (float) ```cpp -#include "numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp" +#include "robotics/kinematics/ForwardKinematics.hpp" +#include "numerical/math/Tolerance.hpp" #include namespace { - class TestDare : public ::testing::Test + class TestForwardKinematics + : public ::testing::Test { protected: - solvers::DiscreteAlgebraicRiccatiEquation solver; + kinematics::ForwardKinematics fk{ math::Vector{ 0.8f, 0.0f, 0.0f } }; }; } -TEST_F(TestDare, solves_simple_system) +TEST_F(TestForwardKinematics, elbow_at_ninety_degrees_places_tool_above_elbow) { - // Arrange, Act, Assert + // Arrange, Act, Assert — EXPECT_NEAR(actual, expected, math::Tolerance()) } ``` diff --git a/.github/prompts/orchestrate.prompt.md b/.github/prompts/orchestrate.prompt.md index ae6d249..41ec37b 100644 --- a/.github/prompts/orchestrate.prompt.md +++ b/.github/prompts/orchestrate.prompt.md @@ -5,7 +5,11 @@ argument-hint: "Describe the algorithm, feature, bug fix, or change you want to model: "Claude Sonnet 4.6" --- -Analyze the following task for the **robotics-toolbox** project — a robot-manipulator algorithms library (kinematics, dynamics, trajectories, and manipulator control) targeting resource-constrained embedded systems, consuming shared numerical primitives from numerical-toolbox-cpp via FetchContent. Gather relevant context from the codebase — identify affected modules, existing patterns, the float-only numeric policy, and documentation requirements. Then provide a brief scope summary and use the handoff buttons to route to the appropriate specialist: +# Orchestrate + +Analyze the following task for the **robotics-toolbox** project — a robot-manipulator algorithms library (kinematics, dynamics, trajectories, and manipulator control) targeting resource-constrained embedded systems, consuming shared numerical primitives from numerical-toolbox-cpp via FetchContent. +Gather relevant context from the codebase — identify affected modules, existing patterns, the float-only numeric policy, and documentation requirements. +Then provide a brief scope summary and use the handoff buttons to route to the appropriate specialist: - **Plan Implementation**: For complex tasks needing detailed upfront design (new algorithms, architectural changes, multi-file modifications) - **Execute Directly**: For straightforward changes with a clear path (bug fixes, small improvements) diff --git a/.github/prompts/unit-test.prompt.md b/.github/prompts/unit-test.prompt.md index 502d135..dc46e33 100644 --- a/.github/prompts/unit-test.prompt.md +++ b/.github/prompts/unit-test.prompt.md @@ -5,6 +5,8 @@ argument-hint: "Name the robotics/ algorithm to author unit tests for (e.g. Biqu model: "Claude Sonnet 5" --- +# Unit Test + Author (or extend) the unit tests for the named **robotics-toolbox** algorithm in `robotics//test/Test.cpp`. Follow the `unit-tester` workflow: read the algorithm's public interface and doc; look up its family in `TESTING.md` and select the metric diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0337084..49783a8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -59,13 +59,24 @@ jobs: name: test-logs path: build/host/Testing/Temporary/ host_build_test: - name: Host Build & Test + name: Host Build & Test (${{ matrix.os }}) runs-on: ${{ matrix.os }} strategy: matrix: - os: [macos-latest, windows-latest] + include: + - os: macos-latest + cache-variant: ccache + cmake-args: "['-DCMAKE_C_COMPILER_LAUNCHER=ccache', '-DCMAKE_CXX_COMPILER_LAUNCHER=ccache']" + # The Visual Studio generator ignores compiler launchers; sccache cannot cache /Zi, so debug info is embedded (/Z7). + # CC/CXX make run-cmake skip its vcpkg-based MSVC setup; msvc-dev-cmd already did it. + - os: windows-latest + cache-variant: sccache + cmake-args: "['-GNinja', '-DCMAKE_C_COMPILER_LAUNCHER=sccache', '-DCMAKE_CXX_COMPILER_LAUNCHER=sccache', '-DCMAKE_POLICY_DEFAULT_CMP0141=NEW', '-DCMAKE_MSVC_DEBUG_INFORMATION_FORMAT=Embedded']" + cc: cl + fail-fast: false steps: - name: Configure Git + shell: bash run: | git config --global url."https://${{ secrets.PRIVATE_REPO_TOKEN }}@github.com/".insteadOf "https://github.com/" git config --global url."https://github.com/".insteadOf "git@github.com:" @@ -73,20 +84,32 @@ jobs: - uses: actions/checkout@3d3c42e5aac5ba805825da76410c181273ba90b1 # v7.0.1 with: persist-credentials: false + - if: ${{ runner.os == 'Windows' }} + uses: ilammy/msvc-dev-cmd@0b201ec74fa43914dc39ae48a89fd1d8cb592756 # v1.13.0 + - if: ${{ runner.os == 'Windows' }} + uses: seanmiddleditch/gha-setup-ninja@3b1f8f94a2f8254bd26914c4ab9474d4f0015f67 # v6 - uses: hendrikmuhs/ccache-action@f09c25b45002a07be2955cbe52e8cee55643f89d # v1.2.24 with: + variant: ${{ matrix.cache-variant }} key: ${{ github.job }}-${{ matrix.os }} max-size: 2G save: ${{ github.ref == 'refs/heads/main' }} - uses: lukka/run-cmake@5d55ea7949e25f69f0ecb516d8d572297e03a956 # v10.9 + env: + CC: ${{ matrix.cc }} + CXX: ${{ matrix.cc }} with: configurePreset: "host-single-Debug" buildPreset: "host-single-Debug" testPreset: "host-single-Debug" - configurePresetAdditionalArgs: "['-DCMAKE_C_COMPILER_LAUNCHER=ccache', '-DCMAKE_CXX_COMPILER_LAUNCHER=ccache']" + configurePresetAdditionalArgs: ${{ matrix.cmake-args }} + - name: Re-run failed tests + if: ${{ failure() }} + run: ctest --preset host-single-Debug --rerun-failed --output-on-failure + continue-on-error: true - name: Upload test logs if: ${{ failure() }} uses: actions/upload-artifact@043fb46d1a93c77aae656e7c1c64a875d1fc6a0a # v7.0.1 with: - name: test-logs - path: build/host/Testing/Temporary/ + name: test-logs-${{ matrix.os }} + path: build/host-single-Debug/Testing/Temporary/ diff --git a/.vscode/launch.json b/.vscode/launch.json index 1476310..ad281ea 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,86 +1,6 @@ { "version": "0.2.0", "configurations": [ - { - "name": "FFT Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/analysis/FastFourierTransform/Debug/robotics.simulator.analysis.fast_fourier_transform", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "PSD Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/analysis/PowerDensitySpectrum/Debug/robotics.simulator.analysis.power_density_spectrum", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "PID Controller Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/controllers/PidController/Debug/robotics.simulator.controllers.pid_controller", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "LQR Cart-Pole Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/controllers/LqrCartPole/Debug/robotics.simulator.controllers.lqr_cart_pole", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, { "name": "Robot Arm Simulator", "type": "cppdbg", @@ -101,166 +21,6 @@ } ] }, - { - "name": "Kalman Filter Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/filters/KalmanFilter/Debug/robotics.simulator.filters.kalman", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "MPC Controller Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/controllers/Mpc/Debug/robotics.simulator.controllers.mpc", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "FIR Filter Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/filters/FirFilter/Debug/robotics.simulator.filters.fir_filter", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "IIR Filter Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/filters/IirFilter/Debug/robotics.simulator.filters.iir_filter", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "RLS Estimator Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/estimators/RecursiveLeastSquares/Debug/robotics.simulator.estimators.rls", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "Neural Network Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/neural_network/NeuralNetwork/Debug/robotics.simulator.neural_network.nn", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "Bayesian MPC Calibration Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/controllers/BayesianMpcCalibration/Debug/robotics.simulator.controllers.bayesian_mpc_calibration", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, - { - "name": "Lqg Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/controllers/Lqg/Debug/robotics.simulator.controllers.lqg", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, { "name": "Linux Debug", "type": "cppdbg", diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 2e2375b..c73c1d1 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -20,16 +20,6 @@ "group": "build", "problemMatcher": [], "detail": "Run formatter on entire codebase" - }, - { - "type": "cmake", - "label": "Antora", - "command": "build", - "targets": ["generate_antora_docs"], - "preset": "${command:cmake.activeBuildPresetName}", - "group": "build", - "problemMatcher": [], - "detail": "Generate Antora documentation" } ] } diff --git a/AGENTS.md b/AGENTS.md index e71e2ec..e691d7c 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -55,6 +55,18 @@ and included as `numerical//…`. `dynamics`, `kinematics`, and new: `trajectory`, `controllers` (manipulator control). Shared primitives keep their upstream namespaces (`math`, `solvers`, …) from numerical-toolbox. +## 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 + 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 + center of mass. +- Controllers receive dynamics and Jacobians through small interfaces (`EulerLagrangeDynamics`, + `InverseDynamicsModel`, `JacobianProvider`); hard real-time code may bind the concrete types instead. +- Spatial fixtures in tests must exercise non-parallel axes and gravity across the joint axes. + ## Testing - GoogleTest. **`TEST_F` on `float`** — no `TYPED_TEST`, no multi-type. **Never plain `TEST()`**. @@ -81,4 +93,5 @@ no code, no class names, no usage examples. Update `doc//README.md` when - Minimal prose. No preamble/postamble, no restating the plan, no summaries unless asked. - Report results as file paths + pass/fail. Don't narrate routine tool calls. - Don't re-read files already read; batch reads; prefer targeted edits. -- Build: `cmake --preset host && cmake --build --preset host` · Test: `ctest --preset host`. +- Build: `cmake --preset host && cmake --build --preset host` · Test: `ctest --preset host` + (the `host` preset builds the Qt simulator; without Qt6 use the `host-single-Debug` presets). diff --git a/CMakeLists.txt b/CMakeLists.txt index 551bc2c..3eb54b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,6 @@ if (CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) endif() option(CMAKE_COMPILE_WARNING_AS_ERROR "Enable warnings-as-error" On) -option(ROBOTICS_TOOLBOX_INCLUDE_DEFAULT_INIT "Include default initialization code; turn off when providing custom initialization" On) option(ROBOTICS_TOOLBOX_BUILD_SIMULATOR "Enable build of the Qt simulator applications" Off) option(ROBOTICS_TOOLBOX_BUILD_TESTS "Enable build of the tests" Off) option(ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS "Enable compiler optimizations for numerical routines" On) @@ -14,11 +13,11 @@ if (ROBOTICS_TOOLBOX_BUILD_TESTS) # CTest cannot be included before the first project() statement, but Embedded Infastructure # Library needs to see that test utilities need to be built. set(BUILD_TESTING On) - add_definitions(-DROBOTICS_TOOLBOX_ENABLE_ASSERTIONS=1) + add_definitions(-DNUMERICAL_TOOLBOX_ENABLE_ASSERTIONS=1) endif() if (ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS) - add_definitions(-DRoboticsToolbox_ENABLE_OPTIMIZATIONS=1) + add_definitions(-DNumericalToolbox_ENABLE_OPTIMIZATIONS=1) endif() if (EMIL_ENABLE_COVERAGE) @@ -30,9 +29,9 @@ include(RoboticsHeaderLibrary) add_definitions(-DEMIL_ENABLE_TRACING=1) -if (ROBOTICS_TOOLBOX_STANDALONE) - include(FetchContent) +include(FetchContent) +if (NOT TARGET infra.util) FetchContent_Declare( emil GIT_REPOSITORY https://github.com/embedded-pro/embedded-infra-lib.git @@ -46,28 +45,28 @@ if (ROBOTICS_TOOLBOX_STANDALONE) set(EMIL_ENABLE_DOCKER_TOOLS Off CACHE BOOL "" FORCE) FetchContent_MakeAvailable(emil) +endif() - if (ROBOTICS_TOOLBOX_BUILD_SIMULATOR) - FetchContent_Declare( - ui - GIT_REPOSITORY https://github.com/embedded-pro/ui-cpp.git - GIT_TAG main - ) - - set(UI_BUILD_QT_BACKEND On CACHE BOOL "" FORCE) - endif() - +if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG main + GIT_TAG 1225b3c422beab93e0ff8fd7231f8d7e087f6da4 # main ) FetchContent_MakeAvailable(numerical_toolbox) +endif() - if (ROBOTICS_TOOLBOX_BUILD_SIMULATOR) - FetchContent_MakeAvailable(ui) - endif() +if (ROBOTICS_TOOLBOX_BUILD_SIMULATOR AND NOT TARGET ui.model) + FetchContent_Declare( + ui + GIT_REPOSITORY https://github.com/embedded-pro/ui-cpp.git + GIT_TAG 13406639f7ffb6ca8811865b0a8b35034f46ebaf # main + ) + + set(UI_BUILD_QT_BACKEND On CACHE BOOL "" FORCE) + + FetchContent_MakeAvailable(ui) endif() project(robotics-toolbox LANGUAGES C CXX ASM VERSION 1.0.0) # x-release-please-version @@ -110,11 +109,9 @@ if (ROBOTICS_TOOLBOX_BUILD_SIMULATOR) add_subdirectory(simulator) endif() -emil_clangformat_directories(robotics DIRECTORIES .) - -emil_coverage_targets(DIRECTORIES .) - if (ROBOTICS_TOOLBOX_STANDALONE) + emil_clangformat_directories(robotics DIRECTORIES .) + emil_coverage_targets(DIRECTORIES .) emil_folderize_all_targets() endif() diff --git a/CMakePresets.json b/CMakePresets.json index ba291ce..1e902e4 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -49,6 +49,11 @@ } ], "buildPresets": [ + { + "name": "host", + "configuration": "Debug", + "configurePreset": "host" + }, { "name": "host-RelWithDebInfo", "configuration": "RelWithDebInfo", diff --git a/README.md b/README.md index 78b8259..00e92ea 100644 --- a/README.md +++ b/README.md @@ -18,22 +18,25 @@ include(FetchContent) FetchContent_Declare( robotics_toolbox GIT_REPOSITORY https://github.com/embedded-pro/robotics-toolbox-cpp.git - GIT_TAG main + GIT_TAG ) FetchContent_MakeAvailable(robotics_toolbox) target_link_libraries(my_app PRIVATE robotics.kinematics robotics.dynamics) ``` -The numerical-toolbox dependency is fetched automatically. Includes are namespaced by domain, e.g. +Pin `GIT_TAG` to a commit SHA for reproducible builds. The dependencies +[embedded-infra-lib](https://github.com/embedded-pro/embedded-infra-lib) (`infra.util`) and +numerical-toolbox (`numerical.math`, `numerical.solver`) are fetched automatically unless the parent +project already defines those targets. Includes are namespaced by domain, e.g. `#include "robotics/kinematics/ForwardKinematics.hpp"` and — for shared primitives — `#include "numerical/math/Matrix.hpp"`. ## Documentation -| Category | Description | -|--------------------------------------|-----------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| Category | Description | +|----------------------------------------|-----------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA) | Each category page lists its algorithms with a brief description and links to the detailed @@ -43,7 +46,7 @@ documentation. The entire documentation set is also published as a single book — read it online as a [GitHub Pages site](https://embedded-pro.github.io/robotics-toolbox-cpp/) or download the latest -PDF from the [Releases page](../../releases/latest). Both are generated automatically from `doc/` +PDF from the [Releases page](https://github.com/embedded-pro/robotics-toolbox-cpp/releases/latest). Both are generated automatically from `doc/` (cover, Summary/table of contents, one chapter per category, consolidated references, back cover). Build it locally with [Pandoc](https://pandoc.org) + XeLaTeX installed: @@ -91,10 +94,19 @@ cmake --preset host && cmake --build --preset host ctest --preset host ``` +The `host` preset also builds the simulator and therefore needs Qt6. Without Qt6, use the +library-only preset: + +```bash +cmake --preset host-single-Debug && cmake --build --preset host-single-Debug +ctest --preset host-single-Debug +``` + ## Contributing -Contributions, issues, and feature requests are welcome. Please check the contributing guidelines -before submitting pull requests. +Contributions, issues, and feature requests are welcome. Before opening a pull request, read +[AGENTS.md](AGENTS.md) (coding, numeric and testing rules) and +[roadmap/DEPLOYMENT.md](roadmap/DEPLOYMENT.md) (how a roadmap specification becomes shipped code). ## License diff --git a/ROADMAP.md b/ROADMAP.md index 4d72b3c..43bb337 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -1,174 +1,186 @@ # Robotics Toolbox — Roadmap Prioritized backlog for the robot-manipulator stack (kinematics, dynamics, trajectories, -and manipulator control). Shared numerical primitives (`math`, `solvers`, `controllers`) -are consumed from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). +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. ## Robot manipulators & other manipulator types -The library already has the hard parts of manipulator **modelling**: forward/inverse dynamics +The library already has the core of manipulator **modelling**: forward/inverse dynamics ([RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp), [ABA](robotics/dynamics/ArticulatedBodyAlgorithm.hpp), -[Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp) giving $M$, $C$, $g$) plus -damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is the -**control, planning, and full-pose kinematics layer** that turns those models into a usable -manipulator stack. Two current limitations gate most of the items below: +[Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), position forward kinematics with base and +tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is +the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a +usable manipulator stack. Two current limitations gate many items below: > **Position-only, revolute-only.** [ForwardKinematics.hpp](robotics/kinematics/ForwardKinematics.hpp) -> returns joint *positions* (a 3×N Jacobian lives privately inside IK), and only -> [RevoluteJointLink](robotics/dynamics/RevoluteJointLink.hpp) exists. Items **M1** (prismatic/generic -> joints) and **M8** (full 6×N spatial Jacobian) lift these limits and unblock the rest. +> returns joint and tool *positions* (a 3×N Jacobian lives privately inside IK), and only +> [RevoluteJointLink](robotics/dynamics/RevoluteJointLink.hpp) exists. Items **M1** (prismatic joints, +> limits, armature), **M30** (full-pose kinematics) and **M8** (6×N Jacobian) lift these limits. -All manipulator items are *(float-first)* — torques, lengths, and inertias exceed the `Q15`/`Q31` -range, matching the existing `dynamics/` convention. +**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. + +All items are *float-only* — torques, lengths, and inertias exceed the `Q15`/`Q31` range, matching the +existing `dynamics/` convention. ### Manipulator list (by priority) -| # | Component | Target module | Difficulty | -|-----|---------------------------------------------------------|---------------------------------|------------| -| M1 | Prismatic / generic joint link | `dynamics` + `kinematics` | ★☆☆☆☆ | -| M2 | Cubic / quintic polynomial joint trajectory | `trajectory` (new) | ★☆☆☆☆ | -| M3 | Trapezoidal (LSPB) velocity profile | `trajectory` (new) | ★☆☆☆☆ | -| M4 | Friction compensation (Coulomb + viscous + Stribeck) | `dynamics` | ★☆☆☆☆ | -| M5 | PD + gravity compensation control | `controllers/manipulator` (new) | ★☆☆☆☆ | -| M6 | Homogeneous transform / SE(3) + adjoint (twists) | `math` | ★★☆☆☆ | -| M7 | Denavit-Hartenberg parameters | `kinematics` | ★★☆☆☆ | -| M8 | Geometric / analytic Jacobian (6×N) | `kinematics` | ★★☆☆☆ | -| M9 | S-curve (jerk-limited) trajectory | `trajectory` (new) | ★★☆☆☆ | -| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | ★★☆☆☆ | -| M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | ★★☆☆☆ | -| M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` (new) | ★★★☆☆ | -| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | ★★★☆☆ | -| M14 | Redundancy resolution / null-space projection | `kinematics` | ★★★☆☆ | -| M15 | Product-of-Exponentials forward kinematics | `kinematics` | ★★★☆☆ | -| M16 | Momentum-based collision-detection observer | `estimators/online` | ★★★☆☆ | -| M17 | Impedance / admittance control | `controllers/manipulator` (new) | ★★★☆☆ | -| M18 | Operational-space (task-space) control | `controllers/manipulator` (new) | ★★★★☆ | -| M19 | Hybrid position/force control | `controllers/manipulator` (new) | ★★★★☆ | -| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` (new) | ★★★★☆ | -| M21 | Analytical IK (Pieper, wrist-partitioned 6R) | `kinematics` | ★★★★☆ | -| M22 | Dynamic (base-parameter) identification | `estimators/offline` | ★★★★☆ | -| M23 | Parallel-manipulator kinematics (Delta / Stewart-Gough) | `kinematics` | ★★★★☆ | -| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | ★★★★☆ | -| M25 | Cable-driven tension distribution | `controllers/manipulator` (new) | ★★★★☆ | -| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | ★★★★☆ | -| M27 | Time-optimal path parameterization (TOPP) | `trajectory` (new) | ★★★★★ | +| # | 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` | — | ★☆☆☆☆ | +| 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 | ★★☆☆☆ | +| M9 | S-curve (jerk-limited) trajectory | `trajectory` (new) | M3 | ★★☆☆☆ | +| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | M6 | ★★☆☆☆ | +| 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 | ★★★☆☆ | +| M14 | Redundancy resolution / null-space projection | `kinematics` | M8 | ★★★☆☆ | +| M15 | Product-of-Exponentials forward kinematics | `kinematics` | M6 | ★★★☆☆ | +| 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 | ★★★★☆ | +| M22 | Dynamic (base-parameter) identification | `dynamics` | M31 | ★★★★☆ | +| M23 | Parallel-manipulator kinematics (Stewart-Gough, Delta) | `kinematics` | M6 | ★★★★☆ | +| 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 | ★★★★☆ | +| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | ### Tier 1 — Trivial ★☆☆☆☆ -**M1. Prismatic / generic joint link.** Generalize `RevoluteJointLink` to a joint type carrying an axis + type (revolute/prismatic), so FK/IK/dynamics handle sliding joints (SCARA, gantries, hydraulic actuators). -- *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3. -- *Reuses / builds on:* [RevoluteJointLink.hpp](robotics/dynamics/RevoluteJointLink.hpp); unblocks FK, IK, RNEA for mixed chains. +**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 (polynomial trajectories). -- *Reuses / builds on:* scalar `math`; new `trajectory/` module. +- *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. +**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. -- *Reuses / builds on:* new `trajectory/` module. -**M4. Friction compensation model.** Feedforward Coulomb + viscous + Stribeck joint-friction term added to any torque controller. -- *Algorithm / paper:* 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. -- *Reuses / builds on:* `dynamics`; composes with M5/M12. +**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, "A New Feedback Method for Dynamic Control of Manipulators," *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* $g(q)$ from [RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp) / Euler-Lagrange; new `controllers/manipulator/` module. +- *Algorithm / paper:* M. Takegaki, S. Arimoto, *ASME J. Dyn. Sys. Meas. Control*, 1981. +- *Reuses / builds on:* `g(q)` from M29. + +**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). ### Tier 2 — Easy ★★☆☆☆ -**M6. Homogeneous transform / SE(3) + adjoint.** Rigid-body transforms (4×4), twists/wrenches (6-vectors), and the adjoint map — the algebra all modern manipulator code is built on. -- *Algorithm / paper:* K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation* (1994). -- *Reuses / builds on:* [Geometry3D.hpp](numerical/math/Geometry3D.hpp), item 18 (Quaternion). +**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. + +**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. -**M7. Denavit-Hartenberg parameters.** Standard `(a, α, d, θ)` link description and per-joint transform generation. -- *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3 (DH convention). -- *Reuses / builds on:* M6, `math::Matrix`. +**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. -**M8. Geometric / analytic Jacobian (6×N).** Full spatial Jacobian mapping joint rates → end-effector linear + angular velocity (and its transpose for force mapping). -- *Algorithm / paper:* Lynch & Park, *Modern Robotics*, Ch. 5 (velocity kinematics). -- *Reuses / builds on:* promotes the private 3×N Jacobian in [InverseKinematics.hpp](robotics/kinematics/InverseKinematics.hpp); unblocks M11, M13, M14, M17, M18. +**M7. Denavit-Hartenberg parameters.** Standard and modified `(a, α, d, θ)` descriptions with joint offsets, producing the M30 frame chain. +- *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3. -**M9. S-curve (jerk-limited) trajectory.** Seven-segment jerk-bounded profile for smooth, low-vibration motion. -- *Algorithm / paper:* Biagiotti & Melchiorri, *Trajectory Planning*, Ch. 3 (double-S profiles). -- *Reuses / builds on:* M3; new `trajectory/` module. +**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. -**M10. Cartesian path + orientation interpolation.** Straight-line/screw position paths with SLERP orientation blending for task-space moves. -- *Algorithm / paper:* Lynch & Park, Ch. 9; K. Shoemake, SLERP, *SIGGRAPH* 1985. -- *Reuses / builds on:* item 18 (Quaternion), M6. +**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. -**M11. Manipulability ellipsoid / Yoshikawa index.** Scalar dexterity/singularity measure `√det(J Jᵀ)` for posture optimization and singularity avoidance. -- *Algorithm / paper:* T. Yoshikawa, "Manipulability of Robotic Mechanisms," *Int. J. Robotics Research*, 4(2), 1985. -- *Reuses / builds on:* M8, item 43 (SVD) or determinant of `math::Matrix`. +**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. + +**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. + +**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. ### Tier 3 — Moderate ★★★☆☆ -**M12. Computed-torque (inverse-dynamics) control.** Feedback-linearizing manipulator law `τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q,q̇)q̇ + g(q)` yielding decoupled error dynamics. +**M12. Computed-torque (inverse-dynamics) control.** `τ = ID(q, q̇, q̈_d + Kd·ė + Kp·e)` in one `O(n)` call, yielding decoupled error dynamics. - *Algorithm / paper:* Spong et al., *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980). -- *Reuses / builds on:* **directly leverages existing [RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp) / [Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)** for $M$, $C$, $g$; item 40 (feedback linearization). -**M13. Full 6-DOF pose IK.** Extend damped-least-squares IK to a position **and** orientation target using the 6×N Jacobian and a quaternion/log orientation error. -- *Algorithm / paper:* S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods," 2004; Nakamura & Hanafusa (1986). -- *Reuses / builds on:* [InverseKinematics.hpp](robotics/kinematics/InverseKinematics.hpp), M8, item 18. +**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.** Exploit extra DOF (7-DOF arms) via `q̇ = J⁺ẋ + (I − J⁺J)·q̇₀` for secondary objectives (joint-limit / obstacle avoidance). -- *Algorithm / paper:* A. Liégeois, "Automatic supervisory control of the configuration and behavior of multibody mechanisms," *IEEE Trans. SMC*, 7(12), 1977. -- *Reuses / builds on:* M8, item 27 (QR) / 43 (SVD) for the pseudo-inverse. +**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. -**M15. Product-of-Exponentials forward kinematics.** Screw-theory FK (`T = e^{[S₁]θ₁}···e^{[Sₙ]θₙ}·M`), avoiding DH frame bookkeeping. +**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. -- *Reuses / builds on:* M6 (SE(3)/twists), item 29 (matrix exponential). -**M16. Momentum-based collision-detection observer.** Estimate external joint torques from generalized-momentum residual — no joint-torque sensors or acceleration needed. -- *Algorithm / paper:* A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, "Collision Detection and Safe Reaction with the DLR-III Lightweight Manipulator Arm," *IROS*, 2006. -- *Reuses / builds on:* RNEA, `estimators/online`. +**M16. Momentum-based collision-detection observer.** External joint-torque estimate from the generalized-momentum residual, using `Cᵀq̇`. +- *Algorithm / paper:* A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, *IROS*, 2006. + +**M17. Impedance control.** Stiffness/damping impedance via `Jᵀ` (no force sensor), and inertia-shaping impedance with measured contact wrench via the task-space inertia. +- *Algorithm / paper:* N. Hogan, *ASME J. Dyn. Sys. Meas. Control*, 1985. -**M17. Impedance / admittance control.** Render a programmable mass-spring-damper at the end-effector for safe contact and compliant assembly. -- *Algorithm / paper:* N. Hogan, "Impedance Control: An Approach to Manipulation, Parts I–III," *ASME J. Dyn. Sys. Meas. Control*, 1985. -- *Reuses / builds on:* M8, M12, `dynamics`; new `controllers/manipulator/` module. +**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). ### Tier 4 — Advanced ★★★★☆ -**M18. Operational-space (task-space) control.** Control directly in Cartesian space using the task-space inertia `Λ = (J M⁻¹ Jᵀ)⁻¹` and dynamically-consistent null-space projection. -- *Algorithm / paper:* O. Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation," *IEEE J. Robotics and Automation*, 3(1), 1987. -- *Reuses / builds on:* M8, M12, item 28 (LU) for the $M^{-1}$ solve. +**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. +- *Algorithm / paper:* O. Khatib, *IEEE J. Robotics and Automation*, 3(1), 1987. -**M19. Hybrid position/force control.** Partition task directions into force-controlled and motion-controlled subspaces via a selection matrix. -- *Algorithm / paper:* M. Raibert, J. Craig, "Hybrid Position/Force Control of Manipulators," *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* M8, M17, `controllers/manipulator/`. +**M19. Hybrid position/force control.** Selection matrix in a constraint frame, PI force loop with anti-windup and damping, motion PD. +- *Algorithm / paper:* M. Raibert, J. Craig, 1981; An & Hollerbach, 1987. -**M20. Passivity-based adaptive control (Slotine-Li).** Track trajectories while online-estimating inertial parameters, exploiting linearity-in-parameters `Y(q,q̇,q̈)·a = τ`. -- *Algorithm / paper:* J.-J. Slotine, W. Li, "On the Adaptive Control of Robot Manipulators," *Int. J. Robotics Research*, 6(3), 1987. -- *Reuses / builds on:* RNEA regressor form, `estimators/online`; item 47 (MRAC) kinship. +**M20. Passivity-based adaptive control (Slotine-Li).** Tracks trajectories while estimating inertial parameters on-line through the regressor `Y(q,q̇,q̇r,q̈r)`. +- *Algorithm / paper:* J.-J. Slotine, W. Li, *Int. J. Robotics Research*, 6(3), 1987. -**M21. Analytical IK (Pieper, wrist-partitioned 6R).** Closed-form inverse kinematics for the common 6R arm with a spherical wrist (all real solutions, no iteration). -- *Algorithm / paper:* D. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD thesis, Stanford, 1968. -- *Reuses / builds on:* M6/M7, item 23 (CORDIC) or trig for the closed-form angles. +**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.** Least-squares estimation of link inertial parameters from excitation trajectories via the linear regressor. -- *Algorithm / paper:* C. Atkeson, C. An, J. Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads and Links," *Int. J. Robotics Research*, 5(3), 1986. -- *Reuses / builds on:* RNEA regressor, item 27 (QR) / 12 (poly LS), `estimators/offline`. +**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). -**M23. Parallel-manipulator kinematics (Delta / Stewart-Gough).** Closed-form inverse kinematics and iterative forward kinematics for parallel platforms (pick-and-place Delta, 6-DOF hexapods). -- *Algorithm / paper:* J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, delta robot (1990). -- *Reuses / builds on:* M6, item 28 (LU) / Newton iteration for the forward solve. +**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). -**M24. Mobile-manipulator / nonholonomic-base kinematics.** Combined base + arm Jacobian with nonholonomic (differential-drive) constraints. -- *Algorithm / paper:* Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. Automatic Control*, 39(6), 1994. -- *Reuses / builds on:* M8, M14 (redundancy). +**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.** Compute non-negative cable tensions realizing a desired wrench (cable robots, tendon-driven hands) via a bounded QP/LP. -- *Algorithm / paper:* T. Bruckmann, A. Pott (eds.), *Cable-Driven Parallel Robots* (2013); Pott tension-distribution methods. -- *Reuses / builds on:* [MPC](numerical/controllers/implementations/Mpc.hpp) QP machinery, M8. +**M25. Cable-driven tension distribution.** Structure matrix from cable geometry and Pott's closed-form (improved) tension distribution within `[tMin, tMax]`. +- *Algorithm / paper:* Pott, Bruckmann, Mikelsons (2009); A. Pott (2014). -**M26. Continuum / soft constant-curvature kinematics.** Piecewise-constant-curvature FK/IK for tendon/pneumatic continuum arms. -- *Algorithm / paper:* R. Webster, B. Jones, "Design and Kinematic Modeling of Constant Curvature Continuum Robots: A Review," *Int. J. Robotics Research*, 29(13), 2010. -- *Reuses / builds on:* M6 (SE(3)), item 18 (Quaternion). +**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).** Minimum-time traversal of a fixed geometric path subject to joint torque/velocity limits. -- *Algorithm / paper:* J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along Specified Paths," *IJRR*, 4(3), 1985; Q.-C. Pham, TOPP-RA, *IEEE T-RO*, 2014. -- *Reuses / builds on:* RNEA (torque limits along path), M10 (path), new `trajectory/` module. +**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). +- *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 118608b..1ba8cf1 100644 --- a/TESTING.md +++ b/TESTING.md @@ -9,15 +9,14 @@ properties must a correct implementation satisfy, and how do we assert them?* A numerical algorithm is not validated by "it compiles and doesn't crash." Every family has a small set of **characteristic invariants** — properties that hold for any correct implementation -regardless of parameters (a low-pass filter must attenuate above cutoff; an ODE integrator must -reproduce a known analytic solution to its order; a Kalman filter's covariance must stay -positive-definite). A unit test earns its place by pinning one such invariant against a **known +regardless of parameters (a rotation must preserve lengths; inverse dynamics composed with forward +dynamics must be the identity; a mass matrix must stay symmetric positive-definite). A unit test earns its place by pinning one such invariant against a **known ground truth**, not by re-running the implementation and trusting its own output. This file groups the library by **metric family** so a test author picks the right invariants fast: -FFT needs spectral/energy metrics, a PID needs transient-response metrics, a solver needs -residual/convergence metrics. Without this, tests drift toward shallow "golden output" snapshots that -pass while the math is wrong. +kinematics needs geometric/round-trip metrics, dynamics needs cross-method and conservation metrics, +trajectories need boundary/limit metrics, controllers need closed-loop metrics. Without this, tests +drift toward shallow "golden output" snapshots that pass while the math is wrong. Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](.github/instructions/testing.instructions.md)): `TEST_F` on `float`, one behaviour per test, no redundant cases, **no heap in tests**, assert with @@ -29,41 +28,33 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. 2. From that family's row, take the **applicable metric types** and author **one `TEST_F` per distinct property** — not per parameter permutation. 3. Prefer **analytic ground truth** (closed-form response, known transform pair, hand-solved system) - over self-consistency. Fall back to a cross-method check (e.g. FFT vs direct DFT) only when no - closed form exists. + over self-consistency. Fall back to a cross-method check (e.g. RNEA vs an Euler-Lagrange model, + ABA ∘ RNEA = identity) only when no closed form exists. 4. Always include the cross-cutting metrics (accuracy, boundary, determinism/reset) plus the - family-specific ones. Keep signal/data generation on the stack (`std::array`, bounded buffers). + family-specific ones. Keep reference data on the stack (`std::array`, bounded buffers). ## Metric types (the vocabulary) | # | Metric type | What it asserts | Typical assertion | |----|-------------------------------|--------------------------------------------------------------|-------------------------------------------------------------------------| | M1 | **Numerical accuracy** | output matches a closed-form / reference value | `EXPECT_NEAR(out, ref, tol)`; ULP error for math funcs | -| M2 | **Frequency response** | magnitude / phase / group delay vs analytic `H(e^{jω})` | error in dB at DC, cutoff, Nyquist; passband ripple; stopband floor | | M3 | **Time / transient response** | step & impulse behaviour | rise time, settling time, % overshoot, steady-state error | | M4 | **Stability** | poles/eigenvalues inside unit circle; BIBO; Riccati/Lyapunov | pole radius < 1; bounded long run; residual of Lyapunov/Riccati eq | | M5 | **Convergence** | iterative process reaches the answer | iterations-to-tolerance; monotonic objective/residual; contraction rate | | M6 | **Boundary / edge** | zero, saturation, extreme magnitude, min sizes | clamp limits; zero-in→zero-out; no NaN/Inf at extremes | -| M7 | **Invariants & conservation** | energy/Parseval, norm, probability mass, orthogonality | Parseval residual; ‖q‖=1; Σsoftmax=1; energy drift bound | +| M7 | **Invariants & conservation** | energy, norm, orthogonality, symmetry / definiteness | ‖q‖=1; RᵀR=I; energy drift bound; M = Mᵀ ≻ 0 | | M8 | **Statistical consistency** | estimator bias / error / covariance sanity | RMSE vs truth; NEES/NIS in χ² band; R²; unbiasedness | -| M9 | **Conditioning / robustness** | behaviour under ill-conditioning & quantization | residual growth vs condition number; quantization-error bound | +| M9 | **Conditioning / robustness** | behaviour near singularities & under ill-conditioning | bounded steps near singular Jacobians; residual vs condition number | ## Family → metric-type matrix -| Family | M1 | M2 | M3 | M4 | M5 | M6 | M7 | M8 | M9 | -|-------------------------------------------------|:--:|:--:|:--:|:--:|:--:|:--:|:--:|:--:|:--:| -| Signal transforms (analysis) | ● | ● | | | | ● | ● | | ● | -| Passive filters | ● | ● | ● | ● | | ● | | | | -| Stochastic/adaptive filters & online estimators | ● | | ● | ● | ● | ● | ● | ● | | -| Controllers | ● | ○ | ● | ● | | ● | | | | -| Control analysis | ● | ● | ● | ● | | ● | | | | -| Offline estimators / regression | ● | | | | ● | ● | | ● | ● | -| Optimization | ● | | | | ● | ● | | | | -| Regularization | ● | | | | | ● | ● | | | -| Solvers (linear / ODE / roots) | ● | | ○ | ● | ● | ● | ● | | ● | -| Dynamics & kinematics | ● | | ● | | ● | ● | ● | | | -| Neural network | ● | | | | ○ | ● | ● | | | -| Math foundation | ● | | | | | ● | ● | | ● | +| Family | M1 | M3 | M4 | M5 | M6 | M7 | M8 | M9 | +|---------------------------------------|:--:|:--:|:--:|:--:|:--:|:--:|:--:|:--:| +| Kinematics | ● | | | ● | ● | ● | | ○ | +| Dynamics | ● | ○ | | | ● | ● | | ○ | +| Trajectory generation (planned) | ● | ● | | | ● | ● | | | +| Manipulator control (planned) | ● | ● | ● | ○ | ● | | | | +| Estimation & identification (planned) | ● | ● | ● | ● | ● | | ● | ● | ● primary ○ situational @@ -71,164 +62,64 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. ## Per-family detail -### 1. Signal transforms — `analysis/` -`FastFourierTransformRadix2Impl`, `RealFastFourierTransform`, `DiscreteCosineTransform`, `GoertzelAlgorithm`, -`ConvolutionCorrelation`, `PowerDensitySpectrum`, `SignalDetectors`, `windowing/`. - -- **M1 accuracy** — known transform pairs: δ[n] → flat spectrum; single sinusoid → single bin at its - frequency with correct magnitude; DC → energy only in bin 0. -- **M7 Parseval / energy** — `Σ|x|² ≈ (1/N)·Σ|X|²`; assert residual near 0. -- **M1 linearity** — `F(a·x + b·y) = a·F(x) + b·F(y)`. -- **M1 round-trip** — `Inverse(Forward(x)) ≈ x`; assert reconstruction RMSE. -- **M7 symmetry** — real input ⇒ conjugate-symmetric spectrum (RealFastFourierTransform): `X[N-k] = conj(X[k])`. -- **Convolution** — matches the direct sum; `x * δ = x`; commutativity; output length. -- **PSD** — non-negative; total power = signal variance; spectral peak at the tone's frequency. -- **Goertzel** — single-bin magnitude equals the full-FFT bin. -- **Windowing** — coherent gain `Σw`, symmetry, endpoint values, main-lobe width / peak side-lobe level. -- **SignalDetectors** — true/false detection on labelled known signals (threshold behaviour). - -### 2. Passive filters — `filters/passive/` -`Fir`, `Iir`, `BiquadCascade`, `CicFilter`, `MovingAverage`, `ExponentialMovingAverage`, -`MedianFilter`, `NotchCombFilter`, `SavitzkyGolayFilter`. - -- **M2 frequency response** — magnitude at DC, cutoff (−3 dB), and Nyquist vs analytic `H(e^{jω})`; - passband ripple; stopband attenuation. Drive steady-state sinusoids and compare RMS ratios. -- **M3 impulse/step** — FIR impulse response equals its coefficients; step steady-state = DC gain - = `H(1)`; EMA/MovingAverage reach the input mean. -- **M4 stability (IIR/Biquad)** — poles inside the unit circle (pole radius < 1); bounded output over - a long run; impulse response decays to zero. -- **M6 boundary** — disabled ⇒ pass-through; `Reset()` restores initial state; zero-in ⇒ zero-out. -- **Specialised** — Median rejects isolated impulses (order-statistic correctness); Savitzky-Golay - reproduces polynomials up to its order exactly and estimates derivatives; CIC gain `= (R·M)^N` - with expected pass-band droop; Notch/Comb null depth at target frequencies. - -### 3. Stochastic / adaptive filters & online estimators — `filters/active/`, `estimators/online/` -`KalmanFilter`, `ExtendedKalmanFilter`, `UnscentedKalmanFilter`, `KalmanSmoother`, -`ComplementaryFilter`, `AlphaBetaFilter`, `LmsAdaptiveFilter`, `RecursiveLeastSquares`. - -- **M8 estimate error** — state estimate converges to ground truth on a simulated known system - (RMSE below a bound). -- **M8 covariance consistency** — covariance stays symmetric positive-definite; normalised error - (NEES) / innovation (NIS) within the χ² confidence band; innovations approximately white. -- **M4/M1 optimality** — steady-state Kalman gain matches the `DiscreteAlgebraicRiccatiEquation` - solution for the linear-Gaussian case. -- **M5 convergence (LMS/RLS)** — error/MSE decreases monotonically toward the Wiener/LS solution; - RLS matches the batch least-squares fit after processing all samples; forgetting factor behaves. -- **UKF** — sigma-point set recovers the mean and covariance of a known distribution. -- **Complementary/AlphaBeta** — bounded tracking lag; noise-reduction ratio vs raw signal. - -### 4. Controllers — `controllers/` -`PidIncremental`, `BangBangHysteresis`, `LeadLagCompensator`, `Lqr`, `Lqg`, -`IntegralStateFeedbackLqi`, `Mpc`, `LuenbergerObserver`, `GainScheduledController`, -`Feedforward2Dof`, `SaturationRateLimiter`. - -- **M3 transient response** — closed-loop step response: rise time, settling time, % overshoot, - steady-state error. Integral action ⇒ zero steady-state error to a step. -- **M4 stability** — closed-loop poles/eigenvalues inside the unit circle; bounded state on a - reference plant. -- **M6 saturation / anti-windup** — output stays within configured limits; no wind-up after - prolonged saturation; `SaturationRateLimiter` honours magnitude and slew limits. -- **M1 gain optimality (LQR/LQG/LQI)** — feedback gain matches the Riccati-derived gain; observer - poles at the designed locations (`LuenbergerObserver`). -- **MPC** — respects input/state constraints over the horizon; recovers the unconstrained LQR law - when constraints are inactive. -- **BangBang** — switches on the hysteresis band edges; no chattering inside the band. -- **LeadLag / Feedforward** — DC gain and phase lead/lag at the design frequency (M2). - -### 5. Control analysis — `control_analysis/` -`ControllabilityObservability`, `FrequencyResponse`, `RootLocus`. - -- **M1 rank correctness** — controllable/observable flag and Gramian rank for hand-built - controllable and deliberately uncontrollable systems. -- **M2 frequency response** — magnitude/phase at sample frequencies vs the analytic transfer - function; correct gain and phase margins. -- **M4/M1 root locus** — branch points for a known plant: loci start at poles and end at zeros/∞; - asymptote angles and breakaway points match closed-form values. - -### 6. Offline estimators / regression — `estimators/offline/` -`LinearRegression`, `PolynomialFitting`, `YuleWalker`, `ExpectationMaximization`. - -- **M1 exact fit** — noiseless data on a line/polynomial ⇒ coefficients recovered to tolerance, - residual ≈ 0. -- **M8 statistical quality** — on noisy data: R²/RMSE within bounds; estimator unbiased across seeds; - overdetermined fit equals the normal-equation solution. -- **YuleWalker** — recovers the AR coefficients of a known AR process; reflection coefficients - `|k| < 1`. -- **EM (M5)** — log-likelihood increases monotonically each iteration; converges to known mixture - parameters. -- **M9 conditioning** — behaviour on near-collinear features (ill-conditioned design matrix). - -### 7. Optimization — `optimization/` -`GradientDescent`, `BayesianOptimization`. - -- **M1/M5 convergence to optimum** — reaches the exact minimum of a quadratic; nears the minimum of - a standard non-convex test function (e.g. Rosenbrock) within the iteration budget. -- **M5 monotonicity** — objective decreases each step for a suitable step size; diverges/oscillates - for too-large steps (documented boundary). -- **M1 gradient check** — analytic gradient matches a finite-difference estimate. -- **Bayesian** — best-observed value improves over iterations and locates the optimum of a cheap - known function within the budget. - -### 8. Regularization — `regularization/` -`L1`, `L2`. - -- **M1 closed form** — L2 shrinks a coefficient by `1/(1+λ)`; L1 soft-thresholds by `λ` (drives - small coefficients to exactly zero). -- **M7 penalty value & gradient** — penalty equals `λ·‖w‖₁` / `λ·‖w‖₂²`; sub/gradient correct. -- **M6 boundary** — `λ = 0` ⇒ identity; large `λ` ⇒ coefficients → 0. - -### 9. Solvers — `solvers/` -`GaussianElimination`, `CholeskyDecomposition`, `DiscreteAlgebraicRiccatiEquation`, `LevinsonDurbin`, -`DurandKerner`, `RungeKuttaIntegrators`, `DormandPrince45`, `OdeSystem`. - -- **M1 residual** — linear solves: `‖A·x − b‖` below tolerance on a hand-solved system; Cholesky - reconstructs `A = L·Lᵀ`; assert SPD precondition handling. -- **M4/M1 DARE** — solution `P` symmetric positive-definite and the Riccati residual ≈ 0; matches a - known small-system solution. -- **LevinsonDurbin** — solution equals the direct Toeplitz solve; reflection coefficients `|k| < 1`. -- **DurandKerner (M5)** — each returned root satisfies `p(root) ≈ 0`; recovers known roots of a - factored polynomial; converges within max iterations. -- **ODE integrators (M1/M5)** — reproduce analytic solutions (exponential decay, harmonic - oscillator) to tolerance; **order of accuracy**: global error scales as `h^p` (assert the slope); - exact for polynomials up to the method order; adaptive step (DormandPrince45) keeps local error - within the requested tolerance. -- **M7 conservation** — energy drift bounded for a conservative system over many steps. - -### 10. Dynamics & kinematics — `dynamics/`, `kinematics/` -`ForwardKinematics`, `InverseKinematics`, `NewtonEulerSolver`, `RecursiveNewtonEuler`, -`EulerLagrangeSolver`, `ArticulatedBodyAlgorithm`. - -- **M1 forward kinematics** — end-effector pose matches known geometry for canonical joint angles. -- **M5 inverse kinematics round-trip** — `FK(IK(pose)) ≈ pose`; converges within iteration budget; - handles reachable vs unreachable targets (M6). -- **M1 cross-method consistency** — `RecursiveNewtonEuler` and `EulerLagrange` produce the same joint - torques for the same state; both match the analytic torque of a simple pendulum / 2-link arm. -- **M7 energy** — conservation in free (unforced) motion; passivity of the mass matrix (SPD). - -### 11. Neural network — `neural_network/` -`activation/*`, `layer/Dense`, `losses/*`, `model/Model`. - -- **M1 activation values** — reference points: `sigmoid(0)=0.5`, `tanh(0)=0`, `relu(−x)=0`, - `leaky_relu` slope; output range bounds; monotonicity where expected. -- **M7 softmax** — outputs sum to 1 and are non-negative; shift-invariance. -- **M1 loss values & gradients** — MSE of identical vectors = 0; loss non-negative; analytic gradient - matches finite-difference (M1 gradient check). -- **M1 dense layer** — `output = W·x + b`; back-prop gradient check. -- **Model (M6)** — forward pass is deterministic and equals the manual layer composition. - -### 12. Math foundation — `math/` -`Matrix`, `ComplexNumber`, `Quaternion`, `Cordic`, `TrigonometricFunctions`, `HyperbolicFunctions`, -`AdvancedFunctions`, `Statistics`, `LinearTimeInvariant`, `Toeplitz`, `QNumber`. - -- **M1 accuracy vs `std::`** — trig/hyperbolic/CORDIC absolute (and where relevant ULP) error across - the input range, including range-reduction boundaries. -- **M7 identities** — `sin²+cos² = 1`; `cosh²−sinh² = 1`; quaternion `‖q‖` preserved under - multiplication; rotation composition; `q·q⁻¹ = 1`. -- **M1 matrix algebra** — `A·A⁻¹ = I` residual; determinant / transpose / multiply vs known results; - associativity. -- **M9 conditioning / quantization** — `QNumber` quantization-error bound and saturation behaviour; - `Statistics` numerically stable mean/variance (Welford) vs the naive formula. -- **LTI** — step/impulse response and pole/zero placement vs analytic (M3/M4). +### 1. Kinematics — `kinematics/` +`ForwardKinematics`, `InverseKinematics`; planned: SE(3) transforms, DH, chain Jacobian, pose IK, +manipulability, redundancy resolution, PoE, analytical IK, parallel/mobile/continuum kinematics. + +- **M1 known geometry** — joint and tool positions for canonical angles (0, ±90°, 180°), including a + **base offset** and a **tool offset** that differs from the center of mass. +- **M1 conventions** — right-hand rule on a non-z axis (`R_y(+90°)` maps `x → −z`) and rotation + **composition order** with mixed axes at non-zero angles (parallel-axis tests cannot detect it). +- **M5 inverse kinematics round-trip** — `FK(IK(FK(q_ref))) ≈ FK(q_ref)`; converges within the + iteration budget; an initial guess at the solution returns without iterating. +- **M6 boundary** — unreachable target reports non-convergence after exactly the iteration budget; + near-full-extension and near-base targets still converge. +- **M7 invariants** — rotations stay orthonormal; Jacobian columns match a finite difference of FK; + manipulability vanishes exactly at the known singular postures. + +### 2. Dynamics — `dynamics/` +`RecursiveNewtonEuler`, `ArticulatedBodyAlgorithm`, `EulerLagrangeSolver`, `NewtonEulerSolver`; +planned: CRBA, chain-dynamics model, regressor, friction, generic joints. + +- **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 + release `q̈ = g·[9/7, −12/7]`. +- **M1 cross-method consistency** — `RNEA` equals an independent Euler-Lagrange model under gravity + and motion; `ABA(RNEA(q̈)) = q̈` on a **non-planar** chain (skewed axes, full inertia tensors, + off-axis centers of mass) with gravity active. Gravity must act across the joint axes — a chain + whose axes are parallel to gravity does not exercise it. +- **M6 boundary** — zero input gives zero output (no gravity, no motion); constant spin about the + joint axis needs no torque. +- **M7 energy / passivity** — mass matrix symmetric positive definite; energy conserved in free + 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. + +- **M1 boundary conditions** — endpoint position/velocity/acceleration matched exactly. +- **M6 limits** — velocity, acceleration and jerk never exceed their bounds; degenerate short moves + collapse phases without negative durations. +- **M7 continuity** — position and velocity continuous; acceleration continuous for jerk-limited + profiles; SLERP output stays unit-norm. +- **M3 timing** — phase durations and total time match the closed-form formulas. + +### 4. Manipulator control — `controllers/` (planned) +PD + gravity, computed torque, impedance, operational space, hybrid force/position, Slotine–Li. + +- **M1 control law** — the computed torque equals the documented law term by term (StrictMock + dynamics/Jacobian providers). +- **M3/M4 closed loop** — on a simulated plant the tracking error decays as the designed error + dynamics predict; equilibrium at the set-point; rendered stiffness/compliance matches the design. +- **M6 boundary** — singular Jacobians stay finite (transpose-based laws, damped inverses). + +### 5. Estimation & identification (planned) +Momentum observer, dynamic parameter identification. + +- **M3 observer response** — the residual follows a step external torque with the designed + first-order dynamics. +- **M8 identification** — recovers known base parameters from noiseless excitation; bounded bias + under noise. +- **M9 conditioning** — regressor conditioning reported for the excitation trajectory. --- @@ -237,6 +128,8 @@ Canonical rules still apply ([AGENTS.md](AGENTS.md), [testing.instructions.md](. - Golden-output snapshots with no independent reference ("the output is whatever it printed"). - One test per parameter value instead of one per property (violates *no redundant tests*). - Asserting only "no NaN / no crash" without a numerical reference. -- Heap-allocated signal buffers in tests — use `std::array` / bounded buffers. +- Heap-allocated buffers in tests — use `std::array` / bounded buffers. +- Planar-only fixtures for spatial algorithms (all joint axes parallel, or parallel to gravity) — they + cannot detect rotation-composition or gravity-propagation errors. - Re-deriving the algorithm inside the test as the "reference" — the reference must be independent (closed form, hand computation, or a distinct method). diff --git a/cmake/RoboticsHeaderLibrary.cmake b/cmake/RoboticsHeaderLibrary.cmake index cda16f0..13ca102 100644 --- a/cmake/RoboticsHeaderLibrary.cmake +++ b/cmake/RoboticsHeaderLibrary.cmake @@ -3,13 +3,13 @@ function(robotics_add_header_library target) if (EMIL_ENABLE_COVERAGE) if (ARG_STATIC) - add_library(${target} ${EMIL_EXCLUDE_FROM_ALL} STATIC) + add_library(${target} ${ROBOTICS_TOOLBOX_EXCLUDE_FROM_ALL} STATIC) else() - add_library(${target} ${EMIL_EXCLUDE_FROM_ALL}) + add_library(${target} ${ROBOTICS_TOOLBOX_EXCLUDE_FROM_ALL}) endif() set(ROBOTICS_VISIBILITY PUBLIC PARENT_SCOPE) else() - add_library(${target} ${EMIL_EXCLUDE_FROM_ALL} INTERFACE) + add_library(${target} ${ROBOTICS_TOOLBOX_EXCLUDE_FROM_ALL} INTERFACE) set(ROBOTICS_VISIBILITY INTERFACE PARENT_SCOPE) endif() endfunction() diff --git a/doc/dynamics/ArticulatedBodyAlgorithm.md b/doc/dynamics/ArticulatedBodyAlgorithm.md index fa3a29c..8eff629 100644 --- a/doc/dynamics/ArticulatedBodyAlgorithm.md +++ b/doc/dynamics/ArticulatedBodyAlgorithm.md @@ -6,7 +6,7 @@ The Articulated Body Algorithm (ABA) is the most efficient method for computing This is the forward-dynamics counterpart of the [Recursive Newton-Euler Algorithm](RecursiveNewtonEuler.md) (which solves inverse dynamics). Together, they form the two fundamental $O(n)$ algorithms in rigid-body dynamics. -Alternatives like Euler-Lagrange require explicitly forming and inverting the $n \times n$ mass matrix ($O(n^3)$), making ABA significantly faster for chains with more than 3-4 links. +The mass-matrix alternative forms $M(q)$ (for example with the Composite Rigid Body Algorithm, $O(n^2)$) and factorizes it ($O(n^3)$). For short chains the two approaches cost about the same; in Featherstone's operation counts ABA becomes the cheaper option somewhere around $n \approx 6$–$9$ bodies, and its advantage grows linearly beyond that. ## Mathematical Theory @@ -44,7 +44,13 @@ with $U_i = I^A_i s_i$, $D_i = s_i^T U_i$, and $s_i$ the joint motion subspace ( $$\ddot{q}_i = \frac{u_i - U_i^T \hat{a}_i}{D_i}$$ -where $u_i = \tau_i - s_i^T p^A_i$ and $\hat{a}_i$ is the spatial acceleration contribution from the parent. +where $u_i = \tau_i - s_i^T p^A_i$ and $\hat{a}_i = {}^{i}X_{\lambda(i)}\, a_{\lambda(i)} + c_i$ is the parent's acceleration expressed in link $i$ plus the velocity-product acceleration $c_i = v_i \times s_i \dot{q}_i$. The link's own acceleration is then $a_i = \hat{a}_i + s_i \ddot{q}_i$. + +**Bias-force propagation**: the articulated bias force passed to the parent is + +$$p^a_i = p^A_i + \hat{I}^A_i c_i + U_i \frac{u_i}{D_i}, \qquad p^A_{\lambda(i)} \leftarrow p^A_{\lambda(i)} + {}^{\lambda(i)}X^*_i\, p^a_i$$ + +where $p^A_i$ starts as the rigid-body bias force $v_i \times^* I_i v_i$ (gyroscopic and centripetal terms). ## Complexity Analysis @@ -56,7 +62,7 @@ Each pass visits every link exactly once, performing constant-time spatial algeb ## Step-by-Step Walkthrough -Consider a 2-link planar arm with unit masses, unit lengths, Y-axis joints, zero velocities, and zero applied torques under gravity $g = [0, 0, -9.81]^T$. +Consider a 2-link arm of uniform rods with unit masses and unit lengths, both lying along $\hat{x}$ at $q = 0$, rotating about $\hat{y}$, with zero velocities and zero applied torques under gravity $g = [0, 0, -9.81]^T$. **Pass 1** — Forward kinematics: both links at $q = [0, 0]^T$ with $\dot{q} = [0, 0]^T$, so all velocities and velocity-product terms are zero. @@ -64,11 +70,13 @@ Consider a 2-link planar arm with unit masses, unit lengths, Y-axis joints, zero **Pass 3** — Forward pass: at the base, the gravitational acceleration is transformed into the base frame and used to compute $\ddot{q}_1$. The resulting acceleration is propagated to link 2 to compute $\ddot{q}_2$. -The result: both joints accelerate downward due to gravity, with the base joint carrying the larger moment from the full chain. +The result, for uniform rods ($I_{\text{CoM}} = ml^2/12$): $\ddot{q} = g\,[\,9/7,\ -12/7\,]^T \approx [12.61, -16.82]^T$. +The shoulder accelerates downward, but the elbow accelerates the *other* way: the outer link initially rotates upward relative to the inner one (its absolute angular acceleration is $\ddot{q}_1 + \ddot{q}_2 = -3g/7$), the classic whip-like start of a double pendulum released from rest. ## Pitfalls & Edge Cases -- **Singular configurations**: When $D_i \to 0$, the joint becomes locked. This should not happen for well-formed revolute joint models with positive inertia. +- **Vanishing articulated inertia**: $D_i$ is the effective inertia about joint $i$ of everything outboard of it. $D_i \to 0$ means a torque on that joint meets no resistance, so $\ddot{q}_i$ becomes unbounded (it does not "lock"). + This happens only for degenerate models, e.g. a massless outboard chain or a point mass lying on the joint axis; valid links with positive inertia about the joint axis keep $D_i > 0$. - **Numerical precision**: The division by $D_i$ amplifies errors if $D_i$ is small. Use `float` (not fixed-point) for dynamics computations. - **Floating-point only**: The algorithm involves trigonometric functions, divisions, and large dynamic ranges that are unsuitable for Q15/Q31 fixed-point. @@ -88,7 +96,7 @@ The result: both joints accelerate downward due to gravity, with the base joint - [Recursive Newton-Euler](RecursiveNewtonEuler.md): solves the inverse problem ($q, \dot{q}, \ddot{q} \to \tau$). ABA and RNEA are duals: `RNEA(q, qDot, ABA(q, qDot, tau)) ≈ tau`. - [Euler-Lagrange](EulerLagrange.md): equivalent $O(n^3)$ formulation using the mass matrix. -- [Gaussian Elimination](../solvers/GaussianElimination.md): used if the mass matrix approach is taken instead. +- Gaussian elimination or Cholesky factorization ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)): used if the mass-matrix approach is taken instead. ## References & Further Reading diff --git a/doc/dynamics/EulerLagrange.md b/doc/dynamics/EulerLagrange.md index 93c16fd..a65bc20 100644 --- a/doc/dynamics/EulerLagrange.md +++ b/doc/dynamics/EulerLagrange.md @@ -106,10 +106,13 @@ For a 2-DOF arm with equal links ($m_1 = m_2 = 1\,\text{kg}$, $l_1 = l_2 = 1\,\t ## Pitfalls & Edge Cases -- **Singular mass matrix**: Should not happen for physically valid systems, but numerical conditioning degrades near kinematic singularities (e.g., fully extended arm). The Gaussian elimination solver uses partial pivoting to help. +- **Mass-matrix conditioning**: The joint-space mass matrix of a physically valid model is always symmetric positive definite, including at kinematic singularities (those make the *task-space* inertia singular, not $M$). + Its conditioning is governed by the inertia ratio between proximal and distal links — a heavy shoulder with a light wrist gives a large condition number. + A Cholesky factorization exploits the symmetry; Gaussian elimination with partial pivoting also works. - **Fixed-point arithmetic**: Dynamics values (torques, accelerations) typically exceed the $[-1, 1)$ range of Q15/Q31 formats. Use `float` unless inputs are carefully scaled. -- **Coriolis computation**: The choice of Coriolis parameterization (Christoffel vs. factored form) affects the skew-symmetry property of $\dot{M} - 2C$. The interface returns the product $C(q,\dot{q})\dot{q}$ directly, leaving the parameterization choice to the user. -- **Energy consistency**: For simulation, ensure the model matrices satisfy the skew-symmetry property; otherwise, energy may not be conserved in numerical integration. +- **Coriolis computation**: Many matrices $C(q,\dot{q})$ produce the same vector $C(q,\dot{q})\dot{q}$, but only the Christoffel-symbol choice makes $\dot{M} - 2C$ skew-symmetric. + Working with the vector $C(q,\dot{q})\dot{q}$ (as forward and inverse dynamics do) is independent of that choice; controllers that need the matrix itself (passivity-based and adaptive control, momentum observers) must use the Christoffel form. +- **Energy consistency**: The exact equations conserve energy for free motion whatever factorization of $C$ is used, because $\dot{q}^T(\dot{M} - 2C)\dot{q} = 0$ holds for every valid one. Energy drift observed in simulation comes from the integrator (step size and method), not from the model. - **Units**: Be explicit about whether angles are in radians (standard) and torques in N·m. ## Variants & Generalizations @@ -132,10 +135,12 @@ For a 2-DOF arm with equal links ($m_1 = m_2 = 1\,\text{kg}$, $l_1 = l_2 = 1\,\t ## Connections to Other Algorithms -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): Used internally to solve $M\ddot{q} = f$ in forward dynamics -- **LQR Controller** ([Lqr.md](../controllers/Lqr.md)): Euler-Lagrange dynamics can be linearized to produce the $(A, B)$ state-space matrices needed for LQR design -- **Kalman Filter** (../filters/active/): State estimation for partially observed dynamical systems derived from Euler-Lagrange models -- **DARE** ([DiscreteAlgebraicRiccatiEquation.md](../solvers/DiscreteAlgebraicRiccatiEquation.md)): Discretized Euler-Lagrange systems feed into DARE for optimal control gain computation +- **Recursive Newton-Euler** ([RecursiveNewtonEuler.md](RecursiveNewtonEuler.md)) and **Articulated Body Algorithm** ([ArticulatedBodyAlgorithm.md](ArticulatedBodyAlgorithm.md)): compute the same $\tau$ and $\ddot{q}$ in $O(n)$ from a link description; + RNEA also supplies $g(q)$ (zero velocity and acceleration) and $C(q,\dot{q})\dot{q}$ (zero acceleration, zero gravity), which is how a link-based model can implement this formulation. +- The following live in [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp): + - **Linear solvers** (Gaussian elimination, Cholesky): solve $M\ddot{q} = f$ in forward dynamics. + - **LQR / DARE**: linearizing the manipulator equation around an operating point gives the $(A, B)$ matrices for optimal state feedback. + - **Kalman filters**: state estimation for partially observed systems modelled with these equations. ## References & Further Reading diff --git a/doc/dynamics/NewtonEuler.md b/doc/dynamics/NewtonEuler.md index e0d3e3d..ea96188 100644 --- a/doc/dynamics/NewtonEuler.md +++ b/doc/dynamics/NewtonEuler.md @@ -104,7 +104,9 @@ The body accelerates about the z-axis despite no external torque — this is the ## Pitfalls & Edge Cases - **Singular inertia**: An inertia tensor must be positive definite. Zero or negative eigenvalues indicate a non-physical body definition. -- **Body-frame vs. inertial frame**: The equations assume all quantities (forces, torques, velocities) are expressed in the body-fixed frame. Users must transform between frames externally. +- **Body-frame vs. inertial frame**: The equations assume all quantities (forces, torques, velocities) are expressed in the body-fixed frame. Users must transform between frames externally, including gravity, which must be rotated into the body frame and added to the applied force. +- **Frame origin at the center of mass**: The decoupled form above holds only when the body frame's origin is the center of mass and the inertia tensor is taken about it. With any other reference point the translational and rotational equations couple through $m\,c \times$ terms (the spatial-inertia form used by the recursive algorithms). +- **Meaning of $\dot{v}$**: $\dot{v}$ is the rate of change of the body-frame velocity components, not the inertial acceleration of the center of mass (which is $F/m$ rotated into the inertial frame). - **Spherical inertia**: When $I = c \cdot \mathbf{I}_3$, the gyroscopic term $\omega \times (I\omega) = c(\omega \times \omega) = 0$, simplifying Euler's equation to $\tau = I\dot{\omega}$. - **Fixed-point arithmetic**: Like Euler-Lagrange, Newton-Euler involves physical quantities (forces in Newtons, torques in N·m) that typically exceed Q15/Q31 range. Use `float`. - **Energy conservation**: For torque-free motion, kinetic energy $T = \frac{1}{2}\omega^T I \omega$ and angular momentum $L = I\omega$ magnitude should be conserved. Numerical integration may violate this. @@ -128,7 +130,7 @@ The body accelerates about the z-axis despite no external torque — this is the ## Connections to Other Algorithms -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): Used to solve the $3 \times 3$ inertia system $I\dot{\omega} = \tau_{net}$ in forward dynamics +- **Linear solvers** ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)): solve the $3 \times 3$ inertia system $I\dot{\omega} = \tau_{net}$ in forward dynamics - **Euler-Lagrange** ([EulerLagrange.md](EulerLagrange.md)): Joint-space counterpart; Euler-Lagrange derives the same dynamics from energy principles in generalized coordinates - **Recursive Newton-Euler (RNEA)**: Extends single-body Newton-Euler to kinematic chains by propagating velocities/accelerations forward and forces/torques backward through the chain diff --git a/doc/dynamics/RecursiveNewtonEuler.md b/doc/dynamics/RecursiveNewtonEuler.md index dca5e33..0cf1407 100644 --- a/doc/dynamics/RecursiveNewtonEuler.md +++ b/doc/dynamics/RecursiveNewtonEuler.md @@ -4,7 +4,8 @@ The Recursive Newton-Euler Algorithm (RNEA) is the most efficient method for computing **inverse dynamics** of serial kinematic chains (robot arms, manipulators). Given joint positions, velocities, and desired accelerations, it computes the required joint torques in $O(n)$ time — linear in the number of links. -This contrasts with the Euler-Lagrange approach, which requires explicitly forming and multiplying $n \times n$ matrices ($O(n^3)$). For a 6-DOF robot arm, RNEA is roughly 10× faster than the matrix-based approach. +This contrasts with evaluating the Euler-Lagrange equations directly: a straightforward Lagrangian evaluation grows as $O(n^4)$, and even with a precomputed mass matrix the product $M(q)\ddot{q}$ alone is $O(n^2)$. +Hollerbach's comparative study (1980) puts the recursive Newton-Euler formulation at $150n - 48$ multiplications and $131n - 48$ additions — 852 and 738 for a 6-DOF arm — against tens of thousands of multiplications for the direct Lagrangian form. The algorithm has two passes over the kinematic chain: @@ -75,7 +76,7 @@ where $[\hat{u}]_\times$ is the skew-symmetric matrix of $\hat{u}$. | Per-link backward pass | $O(1)$ | $O(1)$ | Cross products and additions | | Rotation matrix | $O(1)$ | $O(1)$ | Rodrigues' formula: trig functions + 3×3 matrix | -Compared to Euler-Lagrange $O(n^3)$ inverse dynamics, RNEA is dramatically faster for large $n$. For $n = 6$ (typical robot arm), RNEA performs roughly 780 floating-point operations vs. ~14,000 for the matrix approach. +Compared to evaluating inverse dynamics through an explicit mass matrix ($O(n^2)$ to form and apply) or a direct Lagrangian evaluation ($O(n^4)$), RNEA's linear cost dominates for every practical $n$; for $n = 6$ it needs 852 multiplications and 738 additions (Hollerbach, 1980). ## Step-by-Step Walkthrough @@ -130,8 +131,9 @@ This matches $I_{\text{end}} \cdot \ddot{q} = \frac{ml^2}{3} \cdot 1 = 0.667$. ## Connections to Other Algorithms - **Newton-Euler** ([NewtonEuler.md](NewtonEuler.md)): RNEA applies the single-body Newton-Euler equations recursively across the chain. The per-link force/torque computation is identical. -- **Euler-Lagrange** ([EulerLagrange.md](EulerLagrange.md)): Both compute the same dynamics ($M\ddot{q} + C\dot{q} + g = \tau$) but RNEA is $O(n)$ vs. $O(n^3)$. RNEA is preferred for computation; Euler-Lagrange for analytical derivation. -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): When using RNEA for forward dynamics, the mass matrix built via RNEA is solved with Gaussian elimination. +- **Euler-Lagrange** ([EulerLagrange.md](EulerLagrange.md)): Both compute the same dynamics ($M\ddot{q} + C\dot{q} + g = \tau$) but RNEA is $O(n)$, whereas working through $M(q)$ costs at least $O(n^2)$. RNEA is preferred for computation; Euler-Lagrange for analytical derivation. +- **Linear solvers** ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)): When RNEA is used for forward dynamics, the mass matrix built column by column is factorized (Cholesky, since it is symmetric positive definite) and solved. +- **Articulated Body Algorithm** ([ArticulatedBodyAlgorithm.md](ArticulatedBodyAlgorithm.md)): the $O(n)$ forward-dynamics dual; composing the two is the identity, which makes it a strong cross-check. ## References & Further Reading @@ -139,3 +141,4 @@ This matches $I_{\text{end}} \cdot \ddot{q} = \frac{ml^2}{3} \cdot 1 = 0.667$. - Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7. - Luh, J. Y. S., Walker, M. W., & Paul, R. P. C. (1980). On-line computational scheme for mechanical manipulators. *Journal of Dynamic Systems, Measurement, and Control*, 102(2), 69–76. - Craig, J. J. (2005). *Introduction to Robotics: Mechanics and Control* (3rd ed.). Pearson. Chapter 6. +- Hollerbach, J. M. (1980). A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation complexity. *IEEE Transactions on Systems, Man, and Cybernetics*, 10(11), 730–736. diff --git a/doc/kinematics/ForwardKinematics.md b/doc/kinematics/ForwardKinematics.md index 9a89252..9511e8f 100644 --- a/doc/kinematics/ForwardKinematics.md +++ b/doc/kinematics/ForwardKinematics.md @@ -2,70 +2,86 @@ ## Overview & Motivation -Forward Kinematics computes the 3D Cartesian positions of all joints in a serial kinematic chain, given the joint angles. For a chain of $n$ revolute joints, it produces $n + 1$ position vectors (base through end-effector) in $O(n)$ time. +Forward Kinematics computes the 3D Cartesian positions of all joints of a serial kinematic chain, plus the tool (end-effector) point, given the joint angles. For a chain of $n$ revolute joints it produces $n + 1$ position vectors — the origin of every joint followed by the tool point — in $O(n)$ time. -This is the geometric foundation for visualization, collision detection, and workspace analysis. It answers the question: "given these joint angles, where is each joint in world space?" +This is the geometric foundation for visualization, collision detection, workspace analysis and Jacobian-based inverse kinematics. It answers the question: "given these joint angles, where is each joint, and where is the tool, in the base frame?" ## Mathematical Theory -### Position Computation +### Chain Description -Starting from the base at the origin, the position of joint $i + 1$ is: +Each link $i$ carries: -$$p_{i+1} = p_i + R_{\text{world} \to i} \cdot r_{i \to i+1}$$ +- $\hat{z}_i$ — the unit joint axis, expressed in the link frame; +- $r_i$ — the position of joint $i$'s origin expressed in the frame of the preceding link (for the first joint, in the base frame — this is the base mounting offset). -where: +The tool point is described separately by a constant offset $t$ expressed in the frame of the last link. The tool point is a purely kinematic parameter: it is unrelated to where the last link's center of mass happens to be. -- $p_i$ is the position of joint $i$ in world coordinates -- $R_{\text{world} \to i} = \prod_{k=0}^{i} R_k(q_k)$ is the accumulated rotation from world to link $i$ -- $r_{i \to i+1}$ is the offset from joint $i$ to joint $i + 1$ in the link $i$ frame +### Orientation Composition -Each rotation $R_k(q_k)$ is computed via Rodrigues' formula from the joint axis and angle. +The orientation of link $i$ relative to the base is the ordered product -### End-Effector Approximation +$$^{0}R_{i} = \prod_{k=0}^{i} R_k(q_k) = R_0(q_0)\,R_1(q_1)\cdots R_i(q_i)$$ -For the last link, the offset to the end-effector is approximated as $2 \cdot r_{j \to \text{CoM}}$ (assuming the center of mass is at the midpoint of the link). +where each factor is the rotation by angle $q_k$ about $\hat{z}_k$, obtained from Rodrigues' formula -## Complexity Analysis +$$R(\hat{u}, \theta) = \cos\theta\,\mathbf{I}_3 + (1 - \cos\theta)\,\hat{u}\hat{u}^T + \sin\theta\,[\hat{u}]_\times$$ -| Case | Time | Space | Notes | -|------|--------|--------|------------------------| -| All | $O(n)$ | $O(n)$ | Single pass over chain | +The product is taken left to right because every axis is expressed in its own link frame: $^{0}R_{i}$ maps vectors written in link $i$ coordinates into base coordinates. Rotations are right-handed: a positive angle about $\hat{y}$ carries $\hat{x}$ towards $-\hat{z}$. -## Variants & Generalizations +### Position Recursion -- **Planar vs. spatial chains**: In purely planar manipulators, all joint axes are parallel and rotations reduce to 2D trigonometry, but the algorithmic structure (one pass accumulating transforms) remains the same. -- **Prismatic joints**: For prismatic joints, $R_k(q_k)$ stays constant while $r_{i \to i+1}$ becomes a function of the joint displacement; the same forward sweep still applies. -- **Different parameterizations**: Denavit–Hartenberg, modified DH, or product-of-exponentials (PoE) formulations all map to the same core idea: recursively compose transforms along the chain. -- **Multiple end-effectors**: For branched chains, the same routine can be run per branch by choosing different terminal joints while reusing shared prefixes. +$$p_0 = r_0, \qquad p_{i+1} = p_i + {}^{0}R_{i}\, r_{i+1} \quad (0 \le i < n - 1), \qquad p_{\text{tool}} = p_{n-1} + {}^{0}R_{n-1}\, t$$ -## Applications +where $p_i$ is the base-frame position of joint $i$. The tool point is stored as the last element of the result. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|------|--------|--------|-------------------------------------------------------------------------------------| +| All | $O(n)$ | $O(n)$ | One Rodrigues rotation, one 3×3 product and one 3×3 matrix–vector product per joint | -- **Visualization**: Rendering joint frames and links in 3D for debugging controllers, planners, and estimators. -- **Collision detection & workspace analysis**: Computing link poses to test against environment geometry and to sample reachable workspaces. -- **Control & planning**: Providing end-effector pose and intermediate joint positions to inverse kinematics solvers, trajectory planners, and constraint checkers. -- **Dynamics algorithms**: Supplying link transforms to dynamics routines such as ABA and RNEA that require consistent kinematic states. ## Step-by-Step Walkthrough -Consider a 2-link planar arm with link lengths $L_1 = 1$, $L_2 = 0.8$, Y-axis joints, and $q = [\pi/4, -\pi/6]$. +Consider a 2-link arm with $y$-axis joints, joint 2 located $1$ along $\hat{x}$ of link 1, a tool offset $t = [0.8, 0, 0]^T$, no base offset, and $q = [\pi/4, -\pi/6]$. -1. $p_0 = [0, 0, 0]^T$ (base at origin) -2. $R_0 = R_y(\pi/4)$, link extent $= [1, 0, 0]^T$, so $p_1 = R_0 \cdot [1, 0, 0]^T = [0.707, 0, 0.707]^T$ -3. $R_{01} = R_0 \cdot R_y(-\pi/6)$, link extent $= [0.8, 0, 0]^T$, so $p_2 = p_1 + R_{01} \cdot [0.8, 0, 0]^T$ +1. $p_0 = [0, 0, 0]^T$. +2. $^{0}R_{0} = R_y(\pi/4)$, so $p_1 = R_y(\pi/4)\,[1, 0, 0]^T = [0.707, 0, -0.707]^T$ (a positive rotation about $\hat{y}$ tips $\hat{x}$ downwards). +3. $^{0}R_{1} = R_y(\pi/4)\,R_y(-\pi/6) = R_y(\pi/12)$, so $p_{\text{tool}} = p_1 + R_y(\pi/12)\,[0.8, 0, 0]^T = [0.707 + 0.773,\ 0,\ -0.707 - 0.207]^T = [1.480, 0, -0.914]^T$. ## Pitfalls & Edge Cases -- **Zero-length links**: If `parentToJoint` and `jointToCoM` are both zero, consecutive joints collapse to the same position. -- **Floating-point only**: Uses trigonometric functions, unsuitable for fixed-point types. -- **Approximation at tip**: The end-effector position uses `jointToCoM * 2`, which is exact only for uniform-density links with the CoM at the geometric center. +- **Base offset**: the first joint's offset is a real mounting offset; ignoring it shifts every result by a constant vector and makes inverse kinematics converge to the wrong joint angles. +- **Tool point**: the tool offset must be supplied explicitly. Approximating it from the center of mass (e.g. "twice the center-of-mass distance") is only correct for uniform links and is wrong for typical robot links whose motors sit near the joints. +- **Composition order**: multiplying the joint rotations in the wrong order produces correct results for parallel axes (planar arms) but wrong results as soon as the axes differ. +- **Non-unit axes**: Rodrigues' formula assumes a unit axis; a non-unit axis silently scales the rotation. +- **Zero-length links**: consecutive joints collapse onto the same point, which is valid but makes the corresponding Jacobian columns dependent. +- **Floating-point only**: the trigonometric evaluation is unsuitable for fixed-point types. + +## Variants & Generalizations + +- **Planar vs. spatial chains**: in purely planar manipulators all axes are parallel and the rotations reduce to 2D trigonometry, but the algorithmic structure (one pass accumulating transforms) is the same. +- **Full pose**: returning $^{0}R_{n-1}$ together with $p_{\text{tool}}$ gives the complete tool pose needed for orientation control. +- **Prismatic joints**: $R_k$ stays constant while the joint offset becomes a function of the joint displacement; the same forward sweep applies. +- **Different parameterizations**: Denavit–Hartenberg, modified DH, or product-of-exponentials formulations all recursively compose rigid transforms along the chain. +- **Multiple end-effectors**: for branched chains the routine runs per branch, reusing shared prefixes. + +## Applications + +- **Visualization**: rendering joint frames and links for debugging controllers, planners and estimators. +- **Collision detection & workspace analysis**: computing link positions to test against environment geometry and to sample reachable workspaces. +- **Control & planning**: providing the tool position and intermediate joint positions to inverse kinematics solvers, trajectory planners and constraint checkers. +- **Dynamics algorithms**: the same orientation recursion appears in the forward passes of the recursive dynamics algorithms. ## Connections to Other Algorithms -- [Articulated Body Algorithm](../dynamics/ArticulatedBodyAlgorithm.md): ABA's first pass performs a similar forward kinematics sweep to compute velocities and rotation matrices. -- [Recursive Newton-Euler](../dynamics/RecursiveNewtonEuler.md): RNEA's forward pass also propagates rotations along the chain. -- The shared rotation computation uses `math::RotationAboutAxis` from [Geometry3D](../math/Geometry3D.md). +- [Inverse Kinematics](InverseKinematics.md): evaluates forward kinematics every iteration and builds its Jacobian from the joint positions. +- [Articulated Body Algorithm](../dynamics/ArticulatedBodyAlgorithm.md): its first pass performs the same orientation recursion to propagate velocities. +- [Recursive Newton-Euler](../dynamics/RecursiveNewtonEuler.md): its forward pass also propagates rotations along the chain. +- The axis–angle rotation comes from the shared geometry primitives of [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). ## References & Further Reading - Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Chapters 2–3. - Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Chapter 2. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Chapter 4. diff --git a/doc/kinematics/InverseKinematics.md b/doc/kinematics/InverseKinematics.md index 307a871..f09131a 100644 --- a/doc/kinematics/InverseKinematics.md +++ b/doc/kinematics/InverseKinematics.md @@ -21,7 +21,7 @@ $$J_i = a_i^w \times (p_{\text{ee}} - p_i)$$ where: - $a_i^w = R_{0 \to i} \, a_i^{\text{link}}$ is the joint axis in world frame (accumulated rotation applied to the link-frame axis) - $p_i$ is the world-frame position of joint $i$ (from Forward Kinematics) -- $p_{\text{ee}}$ is the current end-effector position +- $p_{\text{ee}}$ is the current tool (end-effector) point, i.e. the last joint position plus the rotated tool offset ### DLS Update Rule @@ -37,7 +37,7 @@ This is equivalent to solving the $3 \times 3$ linear system: $$\underbrace{\bigl(J J^\top + \lambda^2 I\bigr)}_{A} \, y = e, \quad \Delta q = J^\top y$$ -where $A \in \mathbb{R}^{3 \times 3}$ is always symmetric positive definite (the damping $\lambda^2 I$ guarantees this), so `GaussianElimination` solves it reliably. +where $A \in \mathbb{R}^{3 \times 3}$ is always symmetric positive definite (the damping $\lambda^2 I$ guarantees this), so a small Gaussian elimination (or Cholesky factorization) solves it reliably. The updated joint angles are: @@ -53,9 +53,13 @@ The loop terminates when: At a kinematic singularity (e.g., fully extended arm), $J J^\top$ becomes rank-deficient. The pseudoinverse $J^+ = J^\top (J J^\top)^{-1}$ would produce infinite joint velocities. The damping term $\lambda^2 I$ bounds the solution: -$$\|\Delta q\| \leq \frac{\|e\|}{\lambda}$$ +$$\|\Delta q\| \leq \frac{\|e\|}{2\lambda}$$ -The cost is a small positional bias proportional to $\lambda$: the algorithm converges to within approximately $\lambda^2 \|e\|$ rather than exact zero. Choosing $\lambda \in [0.01, 0.2]$ balances stability and accuracy for typical robot geometries. +because each singular value $\sigma$ of $J$ is mapped to $\sigma / (\sigma^2 + \lambda^2) \le 1/(2\lambda)$. + +Damping does **not** bias the converged solution of the iteration. A fixed point requires $J^\top (J J^\top + \lambda^2 I)^{-1} e = 0$, which for a full-rank $J$ implies $e = 0$: a reachable, non-singular target is reached exactly, and larger $\lambda$ only shortens the steps and slows convergence. +The damping trade-off is therefore speed and step size versus robustness near singularities; only at a singular configuration, or for an unreachable target, does the iteration stop at a least-squares stationary point with $e \neq 0$. +Choosing $\lambda \in [0.01, 0.2]$ (scaled to the robot's link lengths) is a common starting point. ## Complexity Analysis @@ -82,16 +86,17 @@ Consider a 2-link planar arm with $l_1 = 1.0$, $l_2 = 0.8$, both joints rotating 5. Solve $Ay = e$, compute $\Delta q = J^\top y$ 6. Update both joint angles toward a bent configuration -After ~40 iterations $q$ converges to joints that produce $p_{\text{ee}} \approx (0.5, 1.0, 0)$. +After 7 iterations the error drops below $10^{-4}$ and $p_{\text{ee}} \approx (0.5, 1.0, 0)$; with $\lambda = 0.5$ the same target still converges exactly, in 13 iterations. ## Pitfalls & Edge Cases -- **Unreachable targets**: If $\|p_{\text{target}}\|$ exceeds the total arm length, the algorithm reaches maximum iterations without converging. The returned result has `converged = false`; the end-effector will be as close as geometrically possible. -- **Singularity handling**: Near singularities (e.g., fully extended arm), the damping $\lambda$ prevents numerical blow-up but introduces a small steady-state error. Reduce $\lambda$ for higher accuracy away from singularities. +- **Unreachable targets**: If the target lies outside the workspace, the algorithm reaches the iteration limit and reports non-convergence; the iterate approaches a least-squares stationary point (typically the closest reachable point), but this is not guaranteed for non-convex workspaces. +- **Singularity handling**: Near singularities (e.g., fully extended arm), the damping $\lambda$ bounds the step size and prevents numerical blow-up at the cost of slower convergence. Adaptive or selectively damped schemes raise $\lambda$ only near singularities. - **Local minima in redundant chains**: For $n > 3$ (redundant), DLS finds *a* solution (minimum-norm joint motion) but not necessarily the one with the best posture. Null-space techniques (projecting secondary objectives into $\ker(J)$) can improve this. - **Initial guess sensitivity**: Starting closer to the expected solution reduces iterations and avoids winding through joint-limit regions. For repeated calls (e.g., tracking), use the previous solution as the initial guess. -- **Float-only**: Uses `std::cos`, `std::sin`, and `std::sqrt`; Q15/Q31 fixed-point types are not supported. -- **Configuration preconditions**: `dampingFactor` and `tolerance` must both be strictly positive. The damping term $\lambda^2 I$ ensures $A$ is symmetric positive definite only when $\lambda > 0$; both values are asserted in debug builds via `really_assert`. +- **Float-only**: The trigonometric and square-root evaluations make fixed-point types unsuitable. +- **Configuration preconditions**: The damping factor, the tolerance and the iteration budget must all be strictly positive. The damping term $\lambda^2 I$ makes $A$ symmetric positive definite only when $\lambda > 0$; these preconditions are checked when the solver is constructed, in every build type. +- **Tool and base geometry**: The target is compared against the tool point, so the tool offset and the base mounting offset must match the physical robot; a wrong tool offset makes the solver converge exactly to the wrong joint angles. ## Variants & Generalizations @@ -111,8 +116,8 @@ After ~40 iterations $q$ converges to joints that produce $p_{\text{ee}} \approx ## Connections to Other Algorithms - [Forward Kinematics](ForwardKinematics.md): Called internally every iteration to compute current end-effector position and joint positions for the Jacobian. -- [GaussianElimination](../solvers/GaussianElimination.md): Solves the $3 \times 3$ DLS system $Ay = e$ at the core of each iteration. -- `numerical/math/Geometry3D.hpp`: Provides `CrossProduct` for Jacobian column computation, `RotationAboutAxis` for accumulated rotation, and `VectorNorm` for convergence check. +- Gaussian elimination ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)): Solves the $3 \times 3$ DLS system $Ay = e$ at the core of each iteration. +- 3D geometry primitives (cross product, axis–angle rotation, vector norm) from numerical-toolbox-cpp build the Jacobian columns, the accumulated rotations and the convergence test. ## References & Further Reading diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index 4393d6f..be323f0 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,7 +4,7 @@ Geometry and motion algorithms for serial kinematic chains. ## Algorithms -| Algorithm | Description | -|--------------------------------------------|-------------------------------------------------------------------------------------------------------------| -| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions from joint angles for serial chains — $O(n)$ complexity | -| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles from a target end-effector position via Damped Least Squares — $O(k \cdot n)$ iterative | +| Algorithm | Description | +|--------------------------------------------|---------------------------------------------------------------------------------------------------------------| +| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions and the tool point from joint angles for serial chains — $O(n)$ complexity | +| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | diff --git a/release-please-config.json b/release-please-config.json index 28c8fba..2211d83 100644 --- a/release-please-config.json +++ b/release-please-config.json @@ -1,15 +1,11 @@ { "include-component-in-tag": false, - "plugins": [ - "sentence-case" - ], + "plugins": ["sentence-case"], "packages": { ".": { "release-type": "simple", "package-name": "robotics-toolbox", - "extra-files": [ - "CMakeLists.txt" - ], + "extra-files": ["CMakeLists.txt", "sonar-project.properties"], "changelog-sections": [ { "type": "feat", diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index 00aa6af..40c2385 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -22,7 +22,7 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation 4. **CMake** - Add `.hpp` to `target_sources(...)`, `.cpp` to `robotics_add_coverage_sources(...)`, `Test.cpp` to the `_test` target's `target_sources`. - - New module (`trajectory`, `robust_control`, `nonlinear_control`, `controllers/manipulator`): + - New module (`trajectory`, `controllers/manipulator`): create `robotics//CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in the parent `CMakeLists.txt`, and add a `doc//` folder. @@ -33,9 +33,12 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation 6. **Build & test**, fix until green: `cmake --preset host && cmake --build --preset host && ctest --preset host` - (scope to the target/test where possible). + (needs Qt6 for the simulator; without it use the `host-single-Debug` configure/build/test presets). + Scope to the target/test where possible. -7. **Remove roadmap spec** — delete the entire `roadmap///` directory once all tests are green. +7. **Remove roadmap spec** — delete the entire `roadmap///` directory once all tests are + green, and in the same change remove its row from `ROADMAP.md` (or mark it done) and its entry from the + `roadmap/README.md` index. **Report**: the file paths created/edited/deleted + the test result. Nothing else. diff --git a/roadmap/README.md b/roadmap/README.md index c559383..e45a69a 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -8,7 +8,7 @@ the algorithm works, so an implementer can produce the real templated C++ afterw The tree mirrors the `robotics/` layout. Each algorithm gets its own folder with **three files**: -``` +```text roadmap/// ├── implementation.md # data structures, interface, algorithm pseudocode, complexity, float notes, deployment ├── tests.md # GoogleTest test plan in pseudocode (TEST_F on float, StrictMock, no heap) @@ -21,19 +21,25 @@ 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 -[filters/passive/ExponentialMovingAverage](filters/passive/ExponentialMovingAverage/implementation.md). +[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 +and Jacobians reach controllers through interfaces implemented by +[dynamics/ChainDynamicsModel](dynamics/ChainDynamicsModel/implementation.md) and +[kinematics/GeometricJacobian](kinematics/GeometricJacobian/implementation.md). ## Deployment shape (float-only) Each spec maps to this concrete artifact set when deployed into `robotics/`: -| Spec file | Deploys to | -|---------------------|-------------------------------------------------------------------------| +| Spec file | Deploys to | +|---------------------|------------------------------------------------------------------------| | `implementation.md` | `robotics//.hpp` + `.cpp` (coverage instantiation) | | `tests.md` | `robotics//test/Test.cpp` | -| `explanation.md` | `doc//.md` (expanded to follow `doc/TEMPLATE.md`) | +| `explanation.md` | `doc//.md` (expanded to follow `doc/TEMPLATE.md`) | -Header shape (mirroring existing components such as `Fir.hpp`): +Header shape (mirroring existing components such as `ForwardKinematics.hpp`): ```cpp #pragma once @@ -73,54 +79,19 @@ CMake wiring: Tests: single-type **`TEST_F` on `float`**, `StrictMock` only, no heap; validate reference vectors with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). -> **Float-only note.** This diverges from the repo's historical "all three numeric types" mandate -> in `copilot-instructions.md` / `CLAUDE.md`. The `template` signature is retained so -> `Q15`/`Q31` can be re-enabled per algorithm later by relaxing the `static_assert` and adding -> instantiations. +> **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. ## Index -### `filters/passive` -`ExponentialMovingAverage` (1) · `MovingAverage` (2) · `MedianFilter` (6) · `CicFilter` (14) · `BiquadCascade` (15) · `NotchCombFilter` (16) · `SavitzkyGolayFilter` (22) · `IirFilterDesign` (45) - -### `filters/active` -`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) - -### `controllers` -`SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) - -### `robust_control` -`SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) - -### `nonlinear_control` -`FeedbackLinearization` (40) · `BacksteppingControl` (41) · `ModelReferenceAdaptiveControl` (47) - -### `analysis` -`SignalDetectors` (5) · `ConvolutionCorrelation` (11) · `GoertzelAlgorithm` (13) · `RealFastFourierTransform` (25) · `HilbertTransform` (37) · `DiscreteWaveletTransform` (38) - -### `estimators/offline` -`PolynomialFitting` (12) · `TotalLeastSquares` (44) · `DynamicParameterIdentification` (M22) - -### `estimators/online` -`LmsAdaptiveFilter` (21) · `MomentumObserver` (M16) - -### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (M6) - -### `control_analysis` -`ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) +### `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) -### `solvers` -`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) +### `dynamics` +`GenericJointLink` (M1) · `FrictionCompensation` (M4) · `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) +`PolynomialTrajectory` (M2) · `TrapezoidalProfile` (M3) · `SCurveProfile` (M9) · `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) · `CubicSplineTrajectory` (M33) ### `controllers/manipulator` `PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) - -### `kinematics` -`DenavitHartenberg` (M7) · `SpatialJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkPieper` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) - -### `dynamics` -`GenericJointLink` (M1) · `FrictionCompensation` (M4) diff --git a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md index cd1bffe..602c3fe 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/explanation.md @@ -1,34 +1,40 @@ # Cable Tension Distribution — Overview ## What it is -The force-allocation step for a cable-driven robot: given a desired wrench (force + torque) on the -moving platform, compute a set of **non-negative** cable tensions that produce exactly that wrench. -Because cables can only *pull*, and there are usually more cables than task dimensions, this is a -constrained optimisation, not a plain linear solve. +The force-allocation step for a cable-driven parallel robot: given a desired wrench (force + torque) +on the moving platform, compute a set of cable tensions — each between a positive minimum and a +maximum — that produce exactly that wrench. Because cables can only *pull*, and there are usually more +cables than platform degrees of freedom, this is a constrained problem, not a plain linear solve. ## Why it matters (embedded) Cable robots — warehouse cranes, camera rigs (SkyCam), tendon-driven hands, large 3D printers — actuate through tension only. Every control cycle must hand the winches a feasible, bounded tension vector; a negative or over-limit request is physically impossible and can slacken a cable or snap -it. The allocator runs in the real-time loop on the same controller that computes the wrench. +it. A closed-form allocator with a known worst-case cost fits the same real-time loop that computes +the wrench. ## How it works (intuition) -The **structure matrix** `A` maps cable tensions to platform wrench (`A·t = w`). With more cables -than task DOF, infinitely many tension sets produce the same wrench — the extra freedom is *internal -pretension*. A small quadratic program picks the tension vector closest to the mid-range value while -satisfying `A·t = w` and staying within `[tMin, tMax]`. Centring the tensions keeps every cable -comfortably taut, maximising the margin against both going slack and overloading. +From the platform pose, each cable's direction is the unit vector from its platform attachment point +to its winch; together with the attachment offsets (moment arms) these form the **structure matrix** +`A`, and the cables apply `A·t` to the platform. With more cables than DOF, many tension sets give the +same wrench — the extra freedom is internal *pretension*. The closed form takes the tension set closest +to the mid-range value `(tMin + tMax)/2` that still reproduces the wrench, which keeps every cable +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. ## Key parameters +- **baseAnchors, platformAnchors** — winch exit points (world) and attachment points (platform frame). - **tMin** — minimum tension (> 0) so cables never go slack. - **tMax** — maximum tension set by winch/cable strength. -- **structure matrix A = −Jᵀ** — geometry of cable directions and attachment points. -- **objective centre (tMid)** — value the QP biases toward for maximum disturbance margin. ## Reference -T. Bruckmann, A. Pott (eds.), *Cable-Driven Parallel Robots*, Springer, 2013 (Pott -tension-distribution methods). +A. Pott, T. Bruckmann, L. Mikelsons, "Closed-form Force Distribution for Parallel Wire Robots," +*Computational Kinematics*, 2009. A. Pott, "An Improved Force Distribution Algorithm for +Over-Constrained Cable-Driven Parallel Robots," *Computational Kinematics*, 2014. ## See also -`SpatialJacobian` (#M8, supplies `A = −Jᵀ`), `Mpc` (the reused bounded-QP solver), -`OperationalSpaceControl` (#M18, wrench-to-torque mapping for rigid arms). +`SE3Transform` (M6, 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 f784227..fc71626 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/implementation.md @@ -2,63 +2,93 @@ > Roadmap ref: #M25 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) +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. + ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -class CableTensionDistribution: # WrenchDim = 6 (or 3 planar) - const kinematics::SpatialJacobian& structure # J → A = −Jᵀ (WrenchDim×NumCables), injected (#M8) - controllers::Mpc qpSolver # reused bounded-QP machinery - T tMin # minimum cable tension (> 0: cables never go slack) - T tMax # maximum cable tension (actuator / cable limit) +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 ≤ 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 + T tMin # minimum tension (> 0: cables never go slack) + T tMax # maximum tension (winch / cable limit) + +using StructureMatrix = math::Matrix +using TensionVector = math::Vector +using WrenchVector = math::Vector # wrench the cables must apply to the platform ``` ## Interface -``` -CableTensionDistribution(const SpatialJacobian& structure, T tMin, T tMax) +```cpp +CableTensionDistribution(const AnchorArray& baseAnchors, const AnchorArray& platformAnchors, T tMin, T tMax) -# returns nullopt when the pose is outside the wrench-feasible workspace: -std::optional> - Distribute(const WrenchVector& wDesired, const StateVector& pose) # hot path +std::optional ComputeStructureMatrix(const kinematics::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 ``` ## Algorithm (pseudocode) -``` -function Distribute(wDesired, pose): # OPTIMIZE_FOR_SPEED - A = -transpose(structure.Compute(pose)) # WrenchDim×NumCables structure matrix - # pull-only, bounded tensions that realise the wrench, kept away from the limits: - # minimise ½‖t − tMid‖² (closest-to-centre ⇒ max disturbance margin) - # s.t. A·t = wDesired (exact wrench, equality) - # tMin ≤ t ≤ tMax (cables pull only, t > 0) - tMid = 0.5*(tMin + tMax) * Ones(NumCables) - result = qpSolver.Solve(H = Identity(NumCables), gradient = -tMid, - equality = { A, wDesired }, - lower = tMin, upper = tMax) - if not result.feasible: - return nullopt # pose outside wrench-feasible workspace - return result.tension +```cpp +function ComputeStructureMatrix(pose): # wrench balance A·t = w + for i in 0..NumCables-1: + r = pose.R·bᵢ # platform anchor offset, world frame + l = aᵢ − (pose.p + r) # platform anchor → base anchor + if ‖l‖ ≤ ε: return nullopt # zero-length cable: direction undefined + u = l / ‖l‖ # unit direction in which cable i pulls the platform + column i = (u ; CrossProduct(r, u)) # WrenchDim = 6 + = first WrenchDim entries of u # WrenchDim ≤ 3 (point mass) + return A + +function Distribute(w, pose): # OPTIMIZE_FOR_SPEED + A = ComputeStructureMatrix(pose); if not A: return nullopt + tm = (tMin + tMax) / 2 + fixed = {false…}; free = NumCables; wFree = w; t = 0 + while free ≥ WrenchDim: # ≤ NumCables − WrenchDim + 1 passes + Af = A with the columns of fixed cables zeroed + tmF = tm on free cables, 0 on fixed ones + # closed form t = tm + A⁺(w − A·tm), A⁺ = Aᵀ(AAᵀ)⁻¹ — min ‖t − tm‖ subject to A·t = w + y = solvers::TrySolveSystem(Af·Afᵀ, wFree − Af·tmF) # WrenchDim×WrenchDim + if not y: return nullopt # free cables do not span the wrench space + t[i] = tm + (Afᵀ·y)[i] for every free i + k = argmax over free i of violation(t[i]) # violation = max(tMin − t, t − tMax, 0); ties → lowest i + if violation(t[k]) == 0: return t # all tensions within bounds + 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 ``` ## Complexity & memory -- Time: bounded active-set / interior-point QP with `NumCables` variables and `WrenchDim` equalities: - `O(NumCables³)` worst case; small (`NumCables ≤ 8`). -- Memory: `O(NumCables²)` working matrices; all bounded/static, no heap. +- 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. +- Memory: one `WrenchDim×NumCables` matrix, one `WrenchDim×WrenchDim` system, a `NumCables` flag array; + stack only, no heap. ## Numerical / embedded notes -- **Cables pull only** (`t ≥ tMin > 0`) — the defining constraint. A rigid-robot statics solve can - return compression, which is physically impossible here, so the bounded QP is mandatory. -- Redundancy (`NumCables > WrenchDim`) leaves a null space; **centring** tensions at `tMid` keeps - them away from slack (`tMin`) and snap (`tMax`), maximising the wrench-disturbance margin (Pott). -- **Infeasible ⇒ `nullopt`, never an exception** — the caller treats it as a workspace-boundary - event; matches the no-exception embedded convention. -- Keep `tMin > 0` so cables stay taut (avoids backlash / control loss); size `tMax` to the winch. -- A 2-norm (or Δt-regularised) objective gives **continuous** tensions between cycles — no chatter - when the desired wrench moves smoothly. -- Structure matrix `A = −Jᵀ` reuses the spatial Jacobian (#M8); the QP reuses the `Mpc` solver. +- **Cables pull only** (`t ≥ tMin > 0`) — the defining constraint; `tMin > 0` keeps cables taut + (no backlash / loss of control), `tMax` is sized to the winch. +- **Centring.** With redundancy (`NumCables > WrenchDim`) the closed form is the tension closest to + `tm·1` in the 2-norm, keeping cables away from slack and overload (Pott 2009). With + `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). +- `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. +- The wrench is `(f; n)` about the platform origin `pose.p` in the world frame; `w` is what the cables + must supply (e.g. `−(m·g_vec)` plus the dynamic wrench from the platform controller). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -66,14 +96,17 @@ function Distribute(wDesired, pose): # OPTIMIZE_FOR_SPEED - Header: `robotics/controllers/manipulator/CableTensionDistribution.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Distribute`, and - `extern template class CableTensionDistribution;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class CableTensionDistribution;` / `` / `` + under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/CableTensionDistribution.cpp` → - `template class CableTensionDistribution;` + `template class CableTensionDistribution;` / `` / `` - Test: `robotics/controllers/manipulator/test/TestCableTensionDistribution.cpp` - Doc: `doc/controllers/manipulator/CableTensionDistribution.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestCableTensionDistribution.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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 + [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 206ad74..3f4dafa 100644 --- a/roadmap/controllers/manipulator/CableTensionDistribution/tests.md +++ b/roadmap/controllers/manipulator/CableTensionDistribution/tests.md @@ -1,65 +1,81 @@ # Cable Tension Distribution — Unit Test Plan (Pseudocode) -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. No mocks needed: pure geometry + closed form. ## Fixture -``` -class TestCableTensionDistribution : public ::testing::Test: - # structure Jacobian injected & mocked; planar WrenchDim = 2, NumCables = 3: - StrictMock> structure - float tMin = 10 - float tMax = 200 - CableTensionDistribution distributor{ structure, tMin, tMax } - # concrete Mpc QP solver used internally (not mocked) -# each case below is a TEST_F(TestCableTensionDistribution, ) +```cpp +class TestCableTensionDistribution : public ::testing::Test: # planar point mass, 3 cables, WrenchDim = 2 + std::array,3> base = { (0, 2, 0), (−√3, −1, 0), (√3, −1, 0) } # 120° apart, radius 2 + std::array,3> platform = { 0, 0, 0 } + float tMin = 10, tMax = 200 # tm = 105 + CableTensionDistribution distributor{ base, platform, tMin, tMax } + SE3Transform pose = Identity # point mass at the origin + # u = (0, 1), (−√3/2, −1/2), (√3/2, −1/2); A·Aᵀ = 1.5·I; A·(1,1,1) = 0 + +class TestCableTensionDistributionSquare : public ::testing::Test: # 2 cables, WrenchDim = 2 + base = { (−1, 1, 0), (1, 1, 0) }, platform = { 0, 0 }, tMin = 10, tMax = 200 + CableTensionDistribution distributor{ base, platform, tMin, tMax } + +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 } +# each case below is a TEST_F(, ); default fixture TestCableTensionDistribution ``` ## Test cases (Arrange / Act / Assert) -``` -square_case_unique_solution: - Arrange: NumCables == WrenchDim, invertible A (mock structure) - Act: t = Distribute(wDesired, pose) - Assert: t == A⁻¹ · wDesired +```text +structure_matrix_from_geometry: + Act: A = ComputeStructureMatrix(pose) + Assert: columns == (0, 1), (−0.866025, −0.5), (0.866025, −0.5) -redundant_case_centres_tensions: - Arrange: A = 2×3, one-DOF redundancy - Assert: t is the min-norm-to-tMid solution satisfying A·t = wDesired +spatial_structure_matrix_includes_moment_arm (TestCableTensionDistributionSpatial): + Arrange: pose = { Rz(π/2), p = 0 } ⇒ R·b₀ = (0, 0.2, 0) + Assert: column 0 == (u₀; (R·b₀) × u₀) = (0.59655, 0.53689, 0.59655, 0.11931, 0, −0.11931) -all_tensions_nonnegative: - Arrange: any feasible wrench - Assert: t[i] >= tMin > 0 for all i +zero_wrench_gives_centred_pretension: + Arrange: w = 0 + Assert: t == (I − A⁺A)·tm·1 = (105, 105, 105) (tm·1 already lies in null(A) here) -respects_upper_bound: - Arrange: large wrench pushing one cable toward the limit - Assert: t[i] <= tMax for all i +redundant_case_centres_tensions: + Arrange: w = (0, 60) + Assert: t == tm·1 + A⁺·w = (145, 85, 85) (min ‖t − tm‖, all within bounds) wrench_is_reproduced: - Arrange: feasible wrench - Assert: A · t ≈ wDesired (equality constraint satisfied) + Arrange: w = (30, −40) + Assert: A·t ≈ w and tMin ≤ t ≤ tMax; t ≈ (78.333, 101.013, 135.654) + +upper_bound_fixes_worst_cable: + Arrange: w = (0, 150) (plain closed form: t₀ = 205 > tMax) + Assert: t == (200, 50, 50), A·t ≈ w + +lower_bound_fixes_slack_cable: + Arrange: w = (0, −150) (plain closed form: t₀ = 5 < tMin) + Assert: t == (10, 160, 160), A·t ≈ w -infeasible_pose_returns_nullopt: - Arrange: wrench outside the feasible cone (no pull-only solution) - Assert: Distribute(...) == nullopt +infeasible_wrench_returns_nullopt: + Arrange: w = (0, 300) (t₀ fixed at 200 ⇒ remaining pair needs −100 ⇒ < WrenchDim free cables) + Assert: Distribute(w, pose) == nullopt -zero_wrench_uses_internal_pretension: - Arrange: wDesired = 0 - Assert: t in null(A), all >= tMin (taut, no external load) +square_case_unique_solution (TestCableTensionDistributionSquare): + Arrange: w = (0, 100) + Assert: t == A⁻¹·w = (70.7107, 70.7107) (tm cancels) -structure_matrix_queried_once: - Arrange: any pose - Assert: structure.Compute called exactly once (StrictMock) +collinear_cables_return_nullopt (TestCableTensionDistributionSquare): + Arrange: pose.p = (0, 1, 0) (both cables horizontal and opposite ⇒ rank(A) = 1) + Assert: Distribute(w, pose) == nullopt ``` ## Reference vectors -- Planar 3-cable point mass, symmetric angles: hand-computed `A` (2×3); a vertical wrench ⇒ symmetric - tensions, exactly reproducible. -- Square `A` (NumCables = WrenchDim): unique `t = A⁻¹w` — golden check. +- Planar fixture: `A⁺ = (2/3)·Aᵀ`, so `t = 105 + (2/3)·Aᵀw` while unclamped; `w = (0, W)` ⇒ + `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`. ## Edge cases -- Wrench on the workspace boundary: one tension saturates at `tMax`, still feasible. -- Beyond the boundary: `nullopt`. -- Near-parallel cables (`A` ill-conditioned): QP regularisation / `tMid` centring keeps `t` finite. +- Platform anchor coincident with a base anchor (zero-length cable): `nullopt`. +- Wrench exactly on the workspace boundary: one tension equals `tMin` or `tMax`, still feasible. +- Near-parallel cables (`Af·Afᵀ` ill-conditioned): `TrySolveSystem` pivot threshold ⇒ `nullopt`, never `inf`. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md index 16cd87f..067bc03 100644 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md +++ b/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md @@ -7,24 +7,27 @@ PD correction linearises and decouples the arm into independent unit double inte ## Why it matters (embedded) A fixed PID tuned at one posture misbehaves at another because a robot's inertia and gravity change -with configuration. Computed-torque uses the `M`, `C`, `g` you already evaluate with RNEA to erase -that variation, so a *single* gain set tracks fast trajectories across the entire workspace — no -gain scheduling, no lookup tables — at a deterministic `O(n)` cost. +with configuration. Computed-torque erases that variation with a single inverse-dynamics call — one +recursive Newton–Euler pass evaluated at the commanded acceleration, which never forms the mass +matrix. A *single* gain set therefore tracks fast trajectories across the entire workspace — no gain +scheduling, no lookup tables — at a deterministic `O(n)` cost (with diagonal gains). ## How it works (intuition) Work out the acceleration you actually want: the desired trajectory acceleration plus a PD term that corrects position and velocity error. Then ask the dynamics model, "what joint torque produces -exactly that acceleration *right now*?" Because the answer multiplies by the mass matrix `M(q)`, the -arm's inertial coupling, Coriolis, and gravity are all cancelled, leaving clean, identical -second-order error dynamics on every joint. +exactly that acceleration *right now*?" The answer, `M(q)·a + C(q,q̇)q̇ + g(q)`, cancels the arm's +inertial coupling, Coriolis, and gravity, leaving clean, identical second-order error dynamics on +every joint. ## Key parameters -- **model** — injected dynamics supplying `M(q)`, `C(q,q̇)q̇`, `g(q)`. +- **model** — injected inverse-dynamics model returning `M(q)q̈ + C(q,q̇)q̇ + g(q)` in one call + (`ChainDynamicsModel`, M29). - **Kp, Kd** — error gains for the linearised double integrator; pick `Kd = 2√Kp` for critical damping. ## Reference M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980). ## See also -`PdGravityCompensation` (set-point-only special case); `FeedbackLinearization` (same idea for general -plants); `SlotineLiAdaptiveControl` (adapts unknown parameters); `dynamics/RecursiveNewtonEuler` (term source). +`PdGravityCompensation` (set-point-only special case); `SlotineLiAdaptiveControl` (adapts unknown +parameters); `ChainDynamicsModel` (M29, the one-pass RNEA model); general-plant feedback linearization in +[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md index 1316100..04f5423 100644 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md +++ b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md @@ -4,19 +4,19 @@ ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float class ComputedTorqueControl: - const dynamics::EulerLagrangeDynamics& model # M(q), C(q,q̇)q̇, g(q) + const dynamics::InverseDynamicsModel& model # τ = M(q)q̈ + C(q,q̇)q̇ + g(q) in one call (M29) math::SquareMatrix Kp # position-error gain math::SquareMatrix Kd # velocity-error gain ``` ## Interface -``` -# Full dynamics model injected (DIP); gains chosen for the resulting double integrator: -ComputedTorqueControl(const EulerLagrangeDynamics& model, +```cpp +# Inverse-dynamics model injected (DIP); gains chosen for the resulting double integrator: +ComputedTorqueControl(const dynamics::InverseDynamicsModel& model, const SquareMatrix& Kp, const SquareMatrix& Kd) Vector ComputeTorque(const StateVector& q, const StateVector& qDot, @@ -26,33 +26,33 @@ Vector ComputeTorque(const StateVector& q, const StateVector& qDot, ## Algorithm (pseudocode) -``` +```text function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED e = qd - q eDot = qdDot - qDot # inner-loop joint-space acceleration command (feedforward + PD): aq = qdDdot + Kd * eDot + Kp * e - # inverse-dynamics torque that realises aq exactly: - # τ = M(q)·aq + C(q,q̇)q̇ + g(q) - M = model.ComputeMassMatrix(q) - Cqd = model.ComputeCoriolisTerms(q, qDot) - g = model.ComputeGravityTerms(q) - return M * aq + Cqd + g + # inverse dynamics at the measured state and the commanded acceleration: + # τ = M(q)·aq + C(q,q̇)q̇ + g(q) — ONE O(n) RNEA pass, M is never formed + return model.ComputeInverseDynamics(q, qDot, aq) ``` ## Complexity & memory -- Time: `O(Dof²)` for `M·aq`; model evaluation `O(Dof)`–`O(Dof²)` (RNEA inverse dynamics is `O(Dof)`). +- Time: one `ComputeInverseDynamics` call — `O(Dof)` with `ChainDynamicsModel` (single RNEA pass) — + plus `O(Dof²)` for the gain products (`O(Dof)` when the gains are diagonal). No `Dof×Dof` mass matrix + is built or multiplied. - Memory: `O(Dof²)` for the two gains; no dynamic state, no heap. ## Numerical / embedded notes - Substituting `τ` into `M q̈ + Cq̇ + g = τ` gives the **decoupled** linear error dynamics `ë + Kd·ė + Kp·e = 0` — every joint becomes an independent, tunable second-order system. -- The law **multiplies** by `M(q)` (SPD) — it never inverts it, so the hot path stays - well-conditioned (unlike forward dynamics). -- RNEA supplies `M`, `C·q̇`, `g` without forming `C` explicitly; the injected model wraps that - detail (DIP), so the controller is agnostic to how the terms are produced. +- `M(q)` is neither formed nor inverted: RNEA evaluated with `q̈ = aq` returns `M·aq + Cq̇ + g` + directly, so the hot path is `O(n)` and well-conditioned (unlike forward dynamics). +- The model is `dynamics::InverseDynamicsModel` (M29), implemented by `ChainDynamicsModel`. The interface + is a virtual seam for StrictMock tests; hard real-time builds may template the controller on the + concrete `ChainDynamicsModel` to avoid the virtual call (AGENTS.md: no virtual calls in real-time paths). - Model error leaves a residual (`ë + Kd·ė + Kp·e = M⁻¹Δ`); pair with an integral, robust (sliding-mode), or adaptive (`SlotineLiAdaptiveControl`) term to reject it. - Choose `Kd = 2√Kp` per channel for a critically-damped response. @@ -63,14 +63,16 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - Header: `robotics/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class ComputedTorqueControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class ComputedTorqueControl;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/ComputedTorqueControl.cpp` → - `template class ComputedTorqueControl;` + `template class ComputedTorqueControl;` / `` - Test: `robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp` - Doc: `doc/controllers/manipulator/ComputedTorqueControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestComputedTorqueControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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: M29 (`InverseDynamicsModel`, `ChainDynamicsModel`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md b/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md index af77784..17ac4ba 100644 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md +++ b/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md @@ -4,65 +4,51 @@ ## Fixture -``` +```cpp class TestComputedTorqueControl : public ::testing::Test: - StrictMock> model + StrictMock> model SquareMatrix Kp = diag(100, 100) SquareMatrix Kd = diag( 20, 20) ComputedTorqueControl controller{ model, Kp, Kd } # each case below is a TEST_F(TestComputedTorqueControl, ) +# single-step cases set EXPECT_CALL(model, ComputeInverseDynamics(q, qDot, aqExpected)).Times(1) ``` ## Test cases (Arrange / Act / Assert) -``` -pure_feedforward_tracks_acceleration: - Arrange: model M=I, C=0, g=0; all errors 0, qdDdot = a - Act: τ = ComputeTorque(...) - Assert: τ == a (M·aq reduces to the commanded acceleration) - -gravity_compensation_at_rest: - Arrange: model g(q) = [0, mgL]; all setpoints match state, aq = 0 - Assert: τ == g +```cpp +pure_feedforward_passes_desired_acceleration: + Arrange: q = qd, qDot = qdDot, qdDdot = a; model returns r + Act: τ = ComputeTorque(q, qDot, qd, qdDot, qdDdot) + Assert: called once with (q, qDot, a); τ == r (model output returned unchanged) -coriolis_terms_added: - Arrange: model C(q,q̇)q̇ = c; aq = 0 - Assert: τ == c +position_error_enters_command: + Arrange: e = qd − q != 0, eDot = 0, qdDdot = 0 + Assert: called once with aq == Kp · e -mass_matrix_shapes_command: - Arrange: M = diag(2,3), aq = [1,1] (via qdDdot, errors 0) - Assert: τ == [2,3] +velocity_error_enters_command: + Arrange: eDot = qdDot − qDot != 0, e = 0, qdDdot = 0 + Assert: called once with aq == Kd · eDot -position_error_maps_through_mass: - Arrange: e = qd - q != 0, others 0, M = I - Assert: τ == Kp · e - -velocity_error_maps_through_mass: - Arrange: eDot != 0, e = 0, M = I - Assert: τ == Kd · eDot - -full_law_superposition: - Arrange: nonzero M, C, g, and errors - Assert: τ == M·(qdDdot + Kd·ė + Kp·e) + Cq̇ + g +full_command_superposition: + Arrange: e, eDot, qdDdot all nonzero + Assert: called once with aq == qdDdot + Kd·eDot + Kp·e, and with the MEASURED q, qDot decoupled_error_dynamics: - Arrange: wrap plant q̈ = M⁻¹(τ − Cq̇ − g); run K steps - Assert: ||qd − q|| -> 0 matching ë + Kd·ė + Kp·e = 0 - -all_three_model_terms_queried: - Arrange: any state - Assert: ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms each called once - (StrictMock) + Arrange: 2-link plant M(q)q̈ + C(q,q̇)q̇ + g(q) = τ; mock Invoke returns M(q)·aq + C(q,q̇)q̇ + g(q) + of the same plant (exact model); run K steps from e(0) != 0 + Assert: e(t) matches the solution of ë + Kd·ė + Kp·e = 0 (same integrator), ||e|| -> 0 ``` ## Reference vectors -- With the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — a linear ODE whose decay - rate is hand-computable from `Kp`, `Kd`. -- 2-link at rest with all setpoints matched: golden `τ = g(q)`. +- With the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — for `Kp = 100`, `Kd = 20` + critically damped: `e(t) = (e₀ + (ė₀ + 10e₀)t)·e^{−10t}` per joint. +- `e = [0.1, −0.2]`, `ė = 0`, `q̈d = 0` ⇒ expected `aq = [10, −20]` passed to the model. ## Edge cases -- Near-singular `M` (mock): still multiplied, never inverted ⇒ no torque blow-up. -- Model mismatch (mock `M` scaled 1.2): bounded tracking error, closed loop stays stable. +- Only `ComputeInverseDynamics` exists on the mock — no mass-matrix query is possible (StrictMock + fails on any unexpected call), pinning the single `O(n)` call. +- Model mismatch (plant `M` scaled 1.2 vs the mocked model): bounded tracking error, closed loop stays stable. - Large `qdDdot`: torque stays within the documented actuator model. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md index 9f617c5..6ce3664 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/explanation.md @@ -2,8 +2,9 @@ ## What it is A task-space controller that splits the end-effector's directions into two disjoint groups: some -axes are **position-controlled**, the rest are **force-controlled**. A diagonal selection matrix -`S` decides which is which, and its complement `I−S` handles the others. +axes are **position-controlled**, the rest are **force-controlled**. The axes are those of a +*constraint frame* attached to the contact surface; a diagonal selection matrix `S` decides which is +which, and its complement `I−S` handles the others. ## Why it matters (embedded) Many contact tasks are naturally hybrid: sliding a tool on a surface, you want to *track a path* @@ -13,21 +14,26 @@ axis partitioning is essential for deburring, polishing, assembly, and grinding controllers. ## How it works (intuition) -Along motion axes a PD law pulls the tool toward the reference path. Along force axes a PI law drives -the measured contact force to the desired force. The two commands live in orthogonal subspaces, are -summed into a single Cartesian wrench, then mapped to joint torques by `Jᵀ`. Because `S` and `I−S` -never overlap, the loops do not fight each other. +Rotate the pose error, velocity and forces into the constraint frame. Along motion axes a PD law pulls +the tool toward the reference path. Along force axes a PI law drives the measured contact force to the +desired force, with a little velocity damping to calm impacts and a clamped integral so it cannot wind +up when contact is lost. The two commands live in orthogonal subspaces, are summed, rotated back to the +base frame, and mapped to joint torques by `Jᵀ`, with gravity and Coriolis compensated. Because `S` and +`I−S` never overlap, the loops do not fight. Purely kinematic hybrid schemes can go unstable (An & +Hollerbach); the dynamic compensation here — or the fully decoupled operational-space form — avoids it. ## Key parameters -- **S (selection matrix)** — which task axes are motion (1) vs force (0), set in the constraint frame. +- **Rc (constraint rotation)** — orientation of the contact frame relative to the base. +- **S (selection matrix)** — which constraint axes are motion (1) vs force (0). - **Kp, Kd** — motion-subspace position/velocity gains. -- **Kf, Ki** — force-subspace proportional/integral gains (integral removes steady force error). -- **dt** — sample period for the force integral (needs anti-windup). +- **Kf, Ki, Kdf** — force-subspace proportional, integral and velocity-damping gains. +- **integralLimit, dt** — anti-windup clamp and sample period of the force integral. ## Reference M. Raibert, J. Craig, "Hybrid Position/Force Control of Manipulators," *ASME J. Dyn. Sys. Meas. -Control*, 1981. +Control*, 1981. C. An, J. Hollerbach, "Kinematic Stability Issues in Force Control of Manipulators," +*Proc. IEEE ICRA*, 1987. ## See also -`ImpedanceControl` (#M17, compliant unified motion/force), `OperationalSpaceControl` (#M18, -task-space dynamics), `SpatialJacobian` (#M8, the `Jᵀ` wrench map). +`ImpedanceControl` (M17, compliant unified motion/force), `OperationalSpaceControl` (M18, +dynamically decoupled task space), `GeometricJacobian` (M8, the `Jᵀ` wrench map). diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md index 34b1c01..e797942 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/implementation.md @@ -2,68 +2,83 @@ > Roadmap ref: #M19 (Tier 4) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) +Force convention: `fMeasured` and `fd` are the wrench the **tool applies on the environment** (base +frame, `(f; n)` about the tool point) — i.e. `−fExternal` of `ImpedanceControl`. + ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -class HybridPositionForceControl: - const dynamics::EulerLagrangeDynamics& model # g(q), C(q,q̇)q̇ - const kinematics::SpatialJacobian& jacobian # 6×Dof, injected (#M8) - math::SquareMatrix S # selection: 1 = motion axis, 0 = force axis - math::SquareMatrix Kp, Kd # motion-subspace PD gains - math::SquareMatrix Kf, Ki # force-subspace P / I gains - math::Vector forceIntegral # accumulated force error (STATE) +class HybridPositionForceControl: # TaskDim = 6 (pose) or ≤ 3 (position; 2 = planar xy) + const dynamics::EulerLagrangeDynamics& model # C q̇, g — typically ChainDynamicsModel (M29) + const kinematics::JacobianProvider& jacobian # J, tool pose — ChainTaskJacobian (M8) + math::SquareMatrix Rc # base ← constraint rotation: blkdiag(R, R) for 6, R for 3, 2-D rotation for 2 + math::SquareMatrix S # diagonal selection in the constraint frame: 1 = motion, 0 = force + Gains gains # { Kp, Kd (motion); Kf, Ki, Kdf (force); integralLimit } + math::Vector forceIntegral # ∫eF dt in the constraint frame (STATE) T dt ``` ## Interface -``` -HybridPositionForceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& S, const SquareMatrix& Kp, const SquareMatrix& Kd, - const SquareMatrix& Kf, const SquareMatrix& Ki, T dt) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& fMeasured, - const TaskVector& fd) # hot path -void Reset() # clears force integral +```cpp +struct Gains { SquareMatrix Kp, Kd, Kf, Ki, Kdf; TaskVector integralLimit; } # integralLimit ≥ 0 + +HybridPositionForceControl(const dynamics::EulerLagrangeDynamics& model, + const kinematics::JacobianProvider& jacobian, + 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 TaskVector& fMeasured, const TaskVector& fd) # hot path +void Reset() # clears force integral ``` ## Algorithm (pseudocode) -``` -function ComputeTorque(q, qDot, x, xd, xdDot, fMeasured, fd): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) - xDot = J * qDot - # --- motion subspace (S selects position-controlled axes) --- - Fmotion = Kp*(xd - x) + Kd*(xdDot - xDot) - # --- force subspace (I−S selects force-controlled axes), PI on force error --- - eF = fd - fMeasured - forceIntegral = forceIntegral + eF * dt - Fforce = fd + Kf*eF + Ki*forceIntegral - # --- complementary partition: an axis is motion- XOR force-controlled --- - F = S*Fmotion + (Identity(TaskDim) - S)*Fforce - # map task wrench to joint torque; cancel arm gravity + Coriolis: - return transpose(J)*F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) +```text +function ComputeTorque(q, qDot, desiredPose, xdDot, fMeasured, fd): # OPTIMIZE_FOR_SPEED; gains.* unqualified + J = jacobian.Jacobian(q) + xDot = J·qDot + # --- base-frame errors (TaskError.hpp, see ImpedanceControl) rotated into the constraint frame --- + eX = Rcᵀ·TaskError(desiredPose, jacobian.ToolPose(q)) + eXDot = Rcᵀ·(xdDot − xDot) + vC = Rcᵀ·xDot + fdC = Rcᵀ·fd + eF = fdC − Rcᵀ·fMeasured + # --- motion subspace: PD --- + Fmotion = Kp·eX + Kd·eXDot + # --- force subspace: PI + velocity damping, integral clamped (anti-windup) --- + forceIntegral = clamp(forceIntegral + (I − S)·eF·dt, −integralLimit, +integralLimit) # element-wise + Fforce = fdC + Kf·eF + Ki·forceIntegral − Kdf·vC + # --- complementary partition in the constraint frame, rotate back, map to joints --- + F = Rc·(S·Fmotion + (I − S)·Fforce) + return Jᵀ·F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) ``` ## Complexity & memory -- Time: `O(TaskDim·Dof)` for `J·qDot` and `Jᵀ·F`; model terms `O(Dof)`–`O(Dof²)`. +- Time: `O(TaskDim·Dof)` for `J·q̇` and `Jᵀ·F`; `O(TaskDim²)` for the rotations; model terms `O(Dof)` each. - Memory: `O(TaskDim²)` gains + `O(TaskDim)` integral state; no heap. ## Numerical / embedded notes -- `S` is a **complementary orthogonal projector**: `S² = S` and `S·(I−S) = 0`, so the motion and - force loops act on disjoint task axes and never fight (Raibert–Craig). -- Axes are expressed in a **constraint frame** aligned with the contact surface — e.g. peg-in-hole: - normal = force-controlled, insertion/tangential = motion-controlled. -- The force loop is **PI** (not PD): integral action drives steady-state force error to zero on a - stiff environment. Add anti-windup / `Reset()` on contact loss to stop integral run-off. -- `Jᵀ` mapping only (no inverse) ⇒ passes through kinematic singularities safely, like impedance. -- Reuses the `Jᵀ` wrench map of `ImpedanceControl` (#M17) and the spatial Jacobian (#M8); the added - force loop is what makes it *hybrid*. +- `S` is diagonal with 0/1 entries (asserted in the constructor), so `S² = S` and `S·(I−S) = 0`: the + motion and force loops act on disjoint **constraint-frame** axes and never fight (Raibert–Craig). + `Rc` must be orthonormal; it aligns those axes with the contact surface (peg-in-hole: normal = + force-controlled, insertion/tangential = motion-controlled). Rotating with `Rc` only (no translation) + keeps the wrench reference point at the tool. +- Force loop = **PI + damping**: the integral removes steady force error on a stiff environment; the + `−Kdf·vC` term damps motion along force axes (contact impacts, stiff-surface oscillation). The integral + accumulates only on force axes and is clamped to `±integralLimit` (anti-windup); call `Reset()` on + contact loss. +- **Stability caution (An & Hollerbach, 1987):** kinematic hybrid control without dynamic compensation + can be unstable. This law compensates `g(q)` and `C(q,q̇)q̇` and maps + with `Jᵀ` only (no inverse, so it passes through singularities); for full dynamic decoupling of the + subspaces use the `Λ`-weighted form of `OperationalSpaceControl` (Khatib 1987). +- The injected interfaces are a virtual seam for StrictMock tests; hard real-time builds may template + the controller on the concrete `ChainDynamicsModel` / `ChainTaskJacobian` to avoid virtual calls + (AGENTS.md: no virtual calls in real-time paths). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -71,14 +86,17 @@ function ComputeTorque(q, qDot, x, xd, xdDot, fMeasured, fd): # OPTIMIZE_FOR_S - Header: `robotics/controllers/manipulator/HybridPositionForceControl.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class HybridPositionForceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class HybridPositionForceControl;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/HybridPositionForceControl.cpp` → - `template class HybridPositionForceControl;` + `template class HybridPositionForceControl;` / `` - Test: `robotics/controllers/manipulator/test/TestHybridPositionForceControl.cpp` - Doc: `doc/controllers/manipulator/HybridPositionForceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestHybridPositionForceControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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` + (specified in M17, deployed with whichever of M17/M18/M19 lands first). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md index e85ee94..898bcb5 100644 --- a/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md +++ b/roadmap/controllers/manipulator/HybridPositionForceControl/tests.md @@ -4,69 +4,79 @@ ## Fixture -``` +```cpp class TestHybridPositionForceControl : public ::testing::Test: # both dependencies mocked; planar TaskDim = 2, Dof = 2: - StrictMock> model - StrictMock> jacobian - SquareMatrix S = diag(1, 0) # axis 0 = motion, axis 1 = force - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - SquareMatrix Kf = diag( 2, 2) - SquareMatrix Ki = diag( 5, 5) + StrictMock> model + StrictMock> jacobian + SquareMatrix Rc = I # constraint frame = base frame + SquareMatrix S = diag(1, 0) # constraint axis 0 = motion, axis 1 = force + Gains gains{ Kp = diag(100,100), Kd = diag(20,20), Kf = diag(2,2), Ki = diag(5,5), + Kdf = diag(3,3), integralLimit = (1, 1) } float dt = 0.001 - HybridPositionForceControl controller{ model, jacobian, S, Kp, Kd, Kf, Ki, dt } + HybridPositionForceControl controller{ model, jacobian, Rc, S, gains, dt } + # ToolPose mocked as { I, (x, y, 0) } # each case below is a TEST_F(TestHybridPositionForceControl, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```cpp motion_axis_does_position_pd: - Arrange: J=I, model 0; only axis-0 position error, force error 0 - Assert: F[0] == Kp[0]·eX[0] + Kd[0]·eXDot[0]; axis-1 unaffected + Arrange: J = I, model 0; only axis-0 position/velocity error, fd = fMeasured = 0 + Assert: τ[0] == Kp[0]·eX[0] + Kd[0]·eXDot[0]; τ[1] == 0 -force_axis_does_force_pi: - Arrange: J=I, model 0; only axis-1 force error, position error 0, one step - Assert: F[1] == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt +force_axis_does_damped_force_pi: + Arrange: J = I, model 0; axis-1 force error eF, qDot[1] = v, position error 0, one step + Assert: τ[1] == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt − Kdf[1]·v -selection_partitions_axes: - Arrange: S = diag(1,0), both motion and force commands nonzero - Assert: F[0] from motion loop, F[1] from force loop (no cross-talk) +selection_masks_cross_terms: + Arrange: position error only on axis 1, force error only on axis 0, fd = 0, qDot = 0 + Assert: τ == 0 (each axis responds to exactly one loop) -complementary_projectors_orthogonal: - Arrange: verify S·(I−S) == 0 and S·S == S - Assert: each axis contributes exactly one loop +constraint_rotation_maps_axes: + Arrange: local controller with Rc = [[0,−1],[1,0]] (constraint x = base y, constraint y = base −x); + J = I, model 0, qDot = 0; eX = (0.02, 0.1), fd = (−5, 0), fMeasured = (−3, 0) (base frame) + Assert: τ == (−9.01, 10) (base-y error → motion loop; base-x error lies on the force axis + and is ignored; force loop acts along base −x) force_integral_accumulates: - Arrange: constant force error, call ComputeTorque N times - Assert: integral term grows as Ki·eF·(N·dt) + Arrange: constant eF[1], N calls with N·dt·|eF[1]| < integralLimit[1] + Assert: integral contribution == Ki[1]·eF[1]·(N·dt) + +force_integral_clamped: + Arrange: constant eF[1] = 10, 1000 calls (unclamped ∫ = 10 > integralLimit = 1) + Assert: τ[1] == fd[1] + Kf[1]·10 + Ki[1]·1 (anti-windup) reset_clears_force_integral: Arrange: accumulate integral, Reset() - Assert: next force term has no integral history + Assert: next force term == fd[1] + Kf[1]·eF[1] + Ki[1]·eF[1]·dt (no history) jacobian_transpose_maps_wrench: Arrange: J = [[1,0],[0,2]], known F - Assert: τ == transpose(J)·F (+ model terms) + Assert: τ == Jᵀ·F gravity_and_coriolis_added: - Arrange: model g, Cq̇ nonzero; F = 0 - Assert: τ == Cq̇ + g + Arrange: model g, C q̇ nonzero; errors 0, fd = fMeasured = 0, qDot = 0 + Assert: τ == C q̇ + g -both_dependencies_queried: +dependencies_queried_once: Arrange: any state - Assert: jacobian.Compute once; model gravity + Coriolis each once; - ComputeMassMatrix never called (StrictMock) + Assert: Jacobian, ToolPose, ComputeCoriolisTerms, ComputeGravityTerms each once; + ComputeMassMatrix, BiasAcceleration never called (StrictMock) ``` ## Reference vectors -- `S=diag(1,0)`, `J=I`, model 0: `F = [Kp·eX[0], fd[1]+Kf·eF[1]+Ki·∫eF[1]]`, `τ = JᵀF` — hand-checkable. -- Contact equilibrium on the force axis: PI drives `fMeasured → fd` ⇒ steady `eF = 0`. +- `Rc = I`, `S = diag(1,0)`, `J = I`, model 0: + `F = [Kp·eX[0] + Kd·eXDot[0], fd[1] + Kf·eF[1] + Ki·∫eF[1] − Kdf·ẋ[1]]`, `τ = JᵀF` — hand-checkable. +- Rotated case: `Rcᵀ·eX = (0.1, −0.02)`, `Rcᵀ·fd = (0, 5)`, `eF = (0, 2)`, `∫eF = (0, 0.002)` ⇒ + constraint wrench `(10, 9.01)` ⇒ base `F = Rc·(10, 9.01) = (−9.01, 10)`. +- Contact equilibrium on a stiff surface: at rest the applied force equals the force-axis wrench, + and the PI drives `fMeasured → fd` ⇒ steady `eF = 0`. ## Edge cases -- Contact loss (`fMeasured → 0`): force integral winds up ⇒ `Reset()` / anti-windup required. -- `S = I` (all motion) reduces to pure Cartesian PD; `S = 0` (all force) to pure force control. +- Contact loss (`fMeasured → 0`): the integral saturates at `±integralLimit` (bounded) — `Reset()` on re-contact. +- `S = I` (all motion) reduces to Cartesian PD + `g` + `Cq̇`; `S = 0` (all force) to damped force PI. - Near-singular `J`: `Jᵀ` stays finite, torque bounded (no inversion). diff --git a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md b/roadmap/controllers/manipulator/ImpedanceControl/explanation.md index 4f9c492..23ab8cd 100644 --- a/roadmap/controllers/manipulator/ImpedanceControl/explanation.md +++ b/roadmap/controllers/manipulator/ImpedanceControl/explanation.md @@ -2,8 +2,8 @@ ## What it is Instead of commanding a position, impedance control makes the end-effector *behave* like a chosen -mass–spring–damper. You program the stiffness, damping, and (optionally) inertia the robot presents -to the world — a tunable "softness" rather than a rigid trajectory. +spring–damper (and, optionally, mass). You program the stiffness, damping, and — with a force sensor — +the inertia the robot presents to the world: a tunable "softness" rather than a rigid trajectory. ## Why it matters (embedded) Rigid position control shatters on contact: the tiniest position error against a hard surface @@ -12,21 +12,30 @@ with people *safely*, with predictable and adjustable compliance. It is the foun collaborative robots, assembly, and teleoperation. ## How it works (intuition) -Measure the Cartesian error between where the tip is and where it should be. Convert that error into -the force a virtual spring–damper would exert, add any commanded inertia and external-force term, -then use the Jacobian *transpose* to turn that tip force into joint torques. Compensating the arm's -own gravity and Coriolis terms ensures the felt impedance is the one you programmed — not the robot's -native dynamics. Because the mapping uses `Jᵀ` (never an inverse), it stays safe near singularities. +Measure the pose error between where the tip is and where it should be (orientation as a rotation +vector, never a difference of angles). The simple law turns that error and the velocity error into +the force of a virtual spring–damper, maps it to joint torques with the Jacobian *transpose*, and +adds the gravity torque. Pushed by the environment, the tip settles where the spring balances the +push — it yields along the force by exactly the programmed compliance. It does **not** change the +inertia you feel: that is still the arm's own task-space inertia `Λ(q)`, which varies with posture. +Because it uses `Jᵀ` (never an inverse) and leaves the arm's natural energy exchange intact, it stays +passive and safe near singularities. To make the tip also *feel* like a chosen mass, the second law +measures the contact wrench, computes the tip acceleration the target mass–spring–damper would have, +and realises it through `Λ(q)` with full gravity/Coriolis compensation — the operational-space +machinery. If the chosen mass equals `Λ(q)`, the sensor drops out and it collapses back to the +simple law plus feed-forward. ## Key parameters -- **Md (rendered inertia), Dd (damping), Kstiff (stiffness)** — the target impedance per Cartesian axis. -- **model, jacobian** — injected dynamics (`g`, `Cq̇`) and 6×N Jacobian. -- **fExternal** — optional measured contact wrench from a wrist force/torque sensor. +- **K (stiffness), D (damping)** — the target spring–damper per task axis. +- **Md (target inertia)** — used only by the inertia-shaping law; needs a measured contact wrench. +- **model, jacobian** — injected dynamics (`g`; plus `M`, `Cq̇` for shaping) and task Jacobian + provider (`J`, `J̇q̇`, tool pose). +- **fExternal** — wrench exerted by the environment on the tool, from a wrist force/torque sensor. ## Reference N. Hogan, "Impedance Control: An Approach to Manipulation, Parts I–III," *ASME J. Dynamic Systems, Measurement, and Control*, 1985. ## See also -`OperationalSpaceControl` (needed to truly reshape inertia via `Λ`); `HybridPositionForceControl` +`OperationalSpaceControl` (the `Λ` machinery behind inertia shaping); `HybridPositionForceControl` (partition force/motion axes); `ComputedTorqueControl` (rigid tracking counterpart). diff --git a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md index 3a44e5f..afaf853 100644 --- a/roadmap/controllers/manipulator/ImpedanceControl/implementation.md +++ b/roadmap/controllers/manipulator/ImpedanceControl/implementation.md @@ -2,81 +2,121 @@ > 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`. +Task error `e = xd ⊖ x` (`TaskError`, below); `x̃ = x − xd = −e`. + ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -class ImpedanceControl: # TaskDim = 6 (pose) or 3 (position-only) - const dynamics::EulerLagrangeDynamics& model # g(q), C(q,q̇)q̇ - const kinematics::SpatialJacobian& jacobian # 6×Dof, injected (#M8) - math::SquareMatrix Md # desired end-effector inertia - math::SquareMatrix Dd # desired damping - math::SquareMatrix Kstiff # desired stiffness +class ImpedanceControl: # TaskDim = 6 (pose) or ≤ 3 (position; 2 = planar xy) + const dynamics::EulerLagrangeDynamics& model # M, C q̇, g — typically ChainDynamicsModel (M29) + const kinematics::JacobianProvider& jacobian # J, J̇q̇, tool pose — ChainTaskJacobian (M8) + math::SquareMatrix K # stiffness (SPD) + math::SquareMatrix D # damping (SPD) — the target Dd in (b) + math::SquareMatrix MdInverse # target inertia Md⁻¹, precomputed once: SolveSystem(Md, I) + +# shared by M17/M18/M19 — robotics/controllers/manipulator/TaskError.hpp +template +math::Vector TaskError(const kinematics::SE3Transform& desired, + const kinematics::SE3Transform& current) ``` ## Interface -``` -# Dynamics model and Jacobian injected (DIP): -ImpedanceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& Md, const SquareMatrix& Dd, const SquareMatrix& Kstiff) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& xdDdot, - const TaskVector& fExternal) # hot path +```text +ImpedanceControl(const dynamics::EulerLagrangeDynamics& model, + const kinematics::JacobianProvider& jacobian, + const SquareMatrix& K, const SquareMatrix& D, const SquareMatrix& Md) + +# (a) stiffness/damping impedance — no force sensor, no mass matrix: +JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, + const kinematics::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 TaskVector& xdDot, const TaskVector& xdDdot, + const TaskVector& fExternal) # hot path ``` ## Algorithm (pseudocode) -``` -function ComputeTorque(q, qDot, x, xd, xdDot, xdDdot, fExt): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) # TaskDim×Dof spatial Jacobian - xDot = J * qDot # measured end-effector twist - eX = xd - x # Cartesian pose error - eXDot = xdDot - xDot # Cartesian velocity error - # desired end-effector wrench rendering Md·ẍ + Dd·ẋ + Kstiff·x = f_ext : - F = Md * xdDdot + Dd * eXDot + Kstiff * eX + fExt - # map task wrench to joint torque (Jᵀ); cancel the arm's own gravity + Coriolis: - return transpose(J) * F - + model.ComputeCoriolisTerms(q, qDot) - + model.ComputeGravityTerms(q) +```text +function TaskError(desired, current): # never subtracts orientations + if TaskDim == 6: return kinematics::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 + J = jacobian.Jacobian(q) + e = TaskError(desiredPose, jacobian.ToolPose(q)) + eDot = xdDot − J·qDot + return Jᵀ·(K·e + D·eDot) + model.ComputeGravityTerms(q) # C q̇ deliberately NOT added + +function ComputeTorqueWithInertiaShaping(q, qDot, desiredPose, xdDot, xdDdot, fExt): # (b) OPTIMIZE_FOR_SPEED + J = jacobian.Jacobian(q) + e = TaskError(desiredPose, jacobian.ToolPose(q)) + eDot = xdDot − J·qDot + # commanded task acceleration realising Md·x̃̈ + D·x̃̇ + K·x̃ = fExt + aX = xdDdot + MdInverse·(D·eDot + K·e + fExt) + # Λ = (J M⁻¹ Jᵀ)⁻¹ applied through solves — M⁻¹ and Λ never formed explicitly + X = solvers::SolveSystem(model.ComputeMassMatrix(q), Jᵀ) # M⁻¹Jᵀ (Dof×TaskDim) + F = solvers::SolveSystem(J·X, aX − jacobian.BiasAcceleration(q, qDot)) − fExt # Λ(aX − J̇q̇) − fExt + return Jᵀ·F + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) ``` ## Complexity & memory -- Time: `O(TaskDim·Dof)` for `J·qDot` and `Jᵀ·F`; model terms `O(Dof)`–`O(Dof²)`. -- Memory: `O(TaskDim²)` for the three impedance matrices; no dynamic state, no heap. +- (a): `O(TaskDim·Dof)` for `J·q̇` and `Jᵀ·F`, plus one `O(Dof)` gravity pass. +- (b): adds `M` (`O(Dof²)`, CRBA), the `M⁻¹Jᵀ` solve (`O(TaskDim·Dof³)` with `SolveSystem`, which eliminates + per column; factor `M` once with `math::CholeskyDecomposition` for `O(Dof³ + TaskDim·Dof²)`) and a + `TaskDim×TaskDim` solve. +- Memory: `O(TaskDim²)` gains, `O(Dof·TaskDim)` workspace; no dynamic state, no heap. ## Numerical / embedded notes -- The `Jᵀ` map is always well-defined (no inverse) ⇒ the controller passes through kinematic - singularities safely; only *inertia shaping* (`Md` ≠ natural inertia) needs the task inertia - `Λ = (J M⁻¹ Jᵀ)⁻¹` — see `OperationalSpaceControl`. Leaving `Md` at the natural inertia gives - the cheap, robust "stiffness control" variant. +- **(a) contact equilibrium.** Closed loop `M q̈ + C q̇ = Jᵀ(K·e + D·ė + fExt)`. At rest with `J` full rank: + `K·e = −fExt` ⇒ `x − xd = K⁻¹·fExt` — the tool yields *along* the external force with the programmed + compliance. The inertia felt at the tool is the arm's own `Λ(q)`; `Md` is unused by (a). +- **(a) passivity.** `C q̇` is not added: with `q̇ᵀ(Ṁ − 2C)q̇ = 0`, `V = ½q̇ᵀMq̇ + ½eᵀKe` gives + `V̇ = −ẋᵀD ẋ + ẋᵀfExt` for constant `xd` (position tasks) ⇒ passive port `(ẋ, fExt)`, stable against + any passive environment. Cancelling `C q̇` would leave the indefinite term `½q̇ᵀṀq̇`. +- **(b) rendered impedance.** `ẍ = J q̈ + J̇q̇ = Λ⁻¹(F + fExt) + J̇q̇ = aX` ⇒ `Md·x̃̈ + D·x̃̇ + K·x̃ = fExt` + exactly for position tasks. For `TaskDim = 6`, `ωd − ω` is the rate of the rotation-vector error only + to first order, so the rotational impedance is exact to first order about `eR = 0`. +- **(b) equivalent classic form.** Equal to `F = Λ(aX − J̇q̇) + J̄ᵀC q̇ + J̄ᵀg − fExt`, `τ = JᵀF` + (`J̄ = M⁻¹JᵀΛ`) plus `N·(C q̇ + g)`, `N = I − JᵀJ̄ᵀ`: that extra term is zero for a non-redundant arm and + compensates null-space gravity/Coriolis on a redundant one (as in `OperationalSpaceControl`). +- **(b) with `Md = Λ(q)`** the `fExt` terms cancel: `τ = Jᵀ(K·e + D·ė) + g + [JᵀΛ(ẍd − J̇q̇) + C q̇]` — + law (a) plus feed-forward, no force sensor needed. Shaping `Md ≠ Λ` requires a measured `fExt` + (low-pass it and match `D` to the sensor bandwidth); sensor bias shifts the rendered equilibrium. +- (b) needs `Λ`, so it inherits the singularity caveat of `OperationalSpaceControl` (damp + `J M⁻¹ Jᵀ + σ²I`); (a) uses only `Jᵀ` and passes through singularities safely. - **Admittance** is the dual: measure `fExt`, integrate to a motion command, feed a position loop — better on stiff/non-backdrivable robots; impedance is better on backdrivable ones. -- Passivity: with SPD `Dd` and `Kstiff` the rendered port is passive ⇒ stable contact with any - passive environment. -- Diagonal `Kstiff`/`Dd` are chosen per Cartesian axis (e.g. stiff normal, compliant tangential for - insertion tasks). -- `fExt` comes from a wrist force/torque sensor; low-pass it and match `Dd` to its bandwidth. Set it - to zero for pure motion impedance without contact sensing. +- Diagonal `K`/`D` are chosen per task axis (e.g. stiff normal, compliant tangential for insertion). +- The injected interfaces are a virtual seam for StrictMock tests; hard real-time builds may template + the controller on the concrete `ChainDynamicsModel` / `ChainTaskJacobian` to avoid virtual calls + (AGENTS.md: no virtual calls in real-time paths). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. ## Deployment -- Header: `robotics/controllers/manipulator/ImpedanceControl.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class ImpedanceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- 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")`, + `OPTIMIZE_FOR_SPEED` on both torque methods, and `extern template class ImpedanceControl;` / + `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/ImpedanceControl.cpp` → - `template class ImpedanceControl;` + `template class ImpedanceControl;` / `` - Test: `robotics/controllers/manipulator/test/TestImpedanceControl.cpp` - Doc: `doc/controllers/manipulator/ImpedanceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestImpedanceControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ImpedanceControl/tests.md b/roadmap/controllers/manipulator/ImpedanceControl/tests.md index 4341109..47fa5fc 100644 --- a/roadmap/controllers/manipulator/ImpedanceControl/tests.md +++ b/roadmap/controllers/manipulator/ImpedanceControl/tests.md @@ -4,62 +4,79 @@ ## Fixture -``` +```cpp class TestImpedanceControl : public ::testing::Test: - # both injected dependencies mocked; TaskDim = 2 (planar xy point), Dof = 2: - StrictMock> model - StrictMock> jacobian - SquareMatrix Md = diag(1, 1) - SquareMatrix Dd = diag(10, 10) - SquareMatrix Kstiff = diag(500, 500) - ImpedanceControl controller{ model, jacobian, Md, Dd, Kstiff } -# each case below is a TEST_F(TestImpedanceControl, ) + # both injected dependencies mocked; Dof = 2, TaskDim = 2 (planar xy point): + StrictMock> model + StrictMock> jacobian + SquareMatrix K = diag(500, 300) + SquareMatrix D = diag( 40, 30) + SquareMatrix Md = diag( 2, 1.5) + ImpedanceControl controller{ model, jacobian, K, D, Md } + # ToolPose mocked as { I, (x, y, 0) }; desiredPose = { I, (xd, yd, 0) } + +class TestImpedanceControlPose : public ::testing::Test: # TaskDim = 6 branch of TaskError + StrictMock> model + StrictMock> jacobian + ImpedanceControl controller{ model, jacobian, K6 = diag(500,500,500,20,20,20), D6, Md6 = I } +# each case below is a TEST_F(, ); default fixture TestImpedanceControl ``` ## Test cases (Arrange / Act / Assert) -``` -stiffness_pulls_toward_target: - Arrange: jacobian J=I, model terms 0; eX != 0, velocities 0, fExt 0 - Assert: τ == Kstiff · eX - -damping_opposes_task_velocity: - Arrange: J=I, qDot != 0 (so xDot != 0), eX 0, xdDot 0 - Assert: τ contribution == -Dd · xDot - -inertia_feedforward_applied: - Arrange: xdDdot != 0, other terms 0 - Assert: F includes Md · xdDdot - -external_force_passthrough: - Arrange: fExt != 0, all errors 0, J=I - Assert: τ == fExt (Jᵀ = I) +```cpp +# ---- (a) ComputeTorque: stiffness/damping, no force sensing ---- +compliant_law_with_identity_jacobian: + Arrange: J = I; e = xd − x != 0; qDot, xdDot != 0; g != 0 + Act: τ = ComputeTorque(q, qDot, desiredPose, xdDot) + Assert: τ == K·e + D·(xdDot − qDot) + g jacobian_transpose_maps_wrench: - Arrange: J = [[1,0],[0,2]], known F, model 0 - Assert: τ == transpose(J) · F - -gravity_and_coriolis_added: - Arrange: model g, Cq̇ nonzero; F = 0 - Assert: τ == Cq̇ + g - -zero_error_no_contact_holds_gravity: - Arrange: all errors 0, fExt 0 - Assert: τ == Cq̇ + g + Arrange: J = [[1,0],[0,2]], e != 0, qDot = 0, xdDot = 0, g = 0 + Assert: τ == Jᵀ·(K·e) -both_dependencies_queried: +compliant_law_queries_only_gravity: Arrange: any state - Assert: jacobian.Compute called once; model gravity + Coriolis each once; - ComputeMassMatrix never called (StrictMock) + Assert: Jacobian, ToolPose, ComputeGravityTerms each once; ComputeMassMatrix, + ComputeCoriolisTerms, BiasAcceleration never called (StrictMock) + +static_deflection_equals_compliance: + Arrange: simulated 2-DOF Cartesian plant M q̈ + g = τ + fExt with M = diag(1, 2), g = (0, 19.62), + J = I and x = q via Invoke; constant fExt = (3, −4); xd constant; semi-implicit Euler, + dt = 1 ms, 5 s from x = xd + Assert: x − xd → K⁻¹·fExt = (0.006, −0.013333), q̇ → 0 (tool yields along fExt) + +pose_error_uses_rotation_vector (TestImpedanceControlPose): + Arrange: J = I; ToolPose = { Rz(0.2), 0 }, desiredPose = Identity; qDot = 0, xdDot = 0, g = 0 + Assert: τ == K6·(0, 0, 0, 0, 0, −0.2) (PoseError, never a difference of orientations) + +# ---- (b) ComputeTorqueWithInertiaShaping ---- +inertia_shaping_with_unit_model: + Arrange: M = I, J = I, J̇q̇ = 0, C q̇ = 0, g = 0; e, eDot, xdDdot, fExt != 0 + Assert: τ == aX − fExt, aX = xdDdot + Md⁻¹·(D·eDot + K·e + fExt) + +inertia_shaping_uses_task_inertia_bias_and_model: + Arrange: M = diag(2, 3), J = I (Λ = diag(2, 3)), J̇q̇ = b, C q̇ = c, g != 0 + Assert: τ == Λ·(aX − b) − fExt + c + g; each model / provider method called once + +matched_inertia_reduces_to_compliant_law: + Arrange: local controller with Md = Λ = diag(2, 3) (M = diag(2, 3), J = I); J̇q̇ = b, C q̇ = c; fExt != 0 + Assert: ComputeTorqueWithInertiaShaping == ComputeTorque + Λ·(xdDdot − b) + c, independent of fExt + +rendered_impedance_matches_target: + Arrange: plant of static_deflection (M = diag(1, 2) ≠ Md, C = 0, J̇q̇ = 0), step fExt, xd constant; + integrate the plant and the target Md·x̃̈ + D·x̃̇ + K·x̃ = fExt side by side (same integrator) + Assert: x − xd tracks x̃(t) within integration tolerance for the whole run ``` ## Reference vectors -- Pure spring (`J=I`, model 0): `F = Kstiff·eX`, `τ = JᵀF` — hand-checkable. -- Contact equilibrium: at steady state `Kstiff·eX = −fExt` ⇒ `eX_ss = −Kstiff⁻¹·fExt` (rendered compliance). +- (a) equilibrium: `K = diag(500, 300)`, `fExt = (3, −4)` ⇒ `x − xd = (0.006, −0.013333)`. +- (b) `M = I`, `J = I`: `Λ = I`, `τ = aX − fExt + C q̇ + g`. +- `Rz(0.2)` current vs identity desired ⇒ `e = (0, 0, 0, 0, 0, −0.2)`. ## Edge cases -- Near-singular `J`: `Jᵀ` stays finite, torque bounded (no inversion) — the key robustness property. -- Very high `Kstiff`: approaches rigid position control; watch contact instability vs sample rate. -- Noisy `fExt`: `Dd` filters the response; document the sensor bandwidth. +- Near-singular `J`: (a) torque stays finite (`Jᵀ` only); (b) needs the damped `Λ` (see implementation). +- Very high `K`: approaches rigid position control; watch contact instability vs sample rate. +- Sensor bias `β` in (b): static `x̃ = K⁻¹·(fExt + (I − Md·Λ⁻¹)·β)` — no effect only when `Md = Λ`. diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md index 755a819..f52733b 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/explanation.md @@ -14,16 +14,18 @@ rigidly holding a Cartesian pose. ## How it works (intuition) Reflect the joint-space mass matrix through the Jacobian to obtain the effective end-effector inertia -`Λ = (J M⁻¹ Jᵀ)⁻¹`. Command a Cartesian acceleration with a task-space PD law, convert it to a wrench -by multiplying with `Λ`, and map that wrench to joint torque with `Jᵀ`. Finally, inject any secondary -joint torque through a null-space projector chosen so it is *invisible* to the task. The one delicate -step is the `M⁻¹Jᵀ` solve, handled by Gaussian elimination rather than an explicit inverse. +`Λ = (J M⁻¹ Jᵀ)⁻¹`. Command a Cartesian acceleration with a task-space PD law on the pose error +(orientation as a rotation vector), remove the velocity-product term `J̇q̇`, convert it to a wrench by +multiplying with `Λ`, and map that wrench to joint torque with `Jᵀ`. Gravity and Coriolis are cancelled +in *joint* space, so a redundant arm's self-motion does not sag. Finally, inject any secondary joint +torque through a null-space projector chosen so it is *invisible* to the task. The one delicate step is +the `M⁻¹Jᵀ` solve, handled by Gaussian elimination rather than an explicit inverse. ## Key parameters - **Kp, Kd** — task-space position and damping gains. -- **model, jacobian** — injected dynamics and 6×N Jacobian. +- **model, jacobian** — injected dynamics (`M`, `Cq̇`, `g`) and task Jacobian provider (`J`, `J̇q̇`, tool pose). - **tauSecondary** — secondary-objective joint torque (joint-limit / obstacle avoidance). -- **damping factor** — regularises `Λ` near singularities. +- **σ (damping factor)** — adds `σ²I` inside the `Λ` inverse near singularities. ## Reference O. Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational @@ -31,4 +33,5 @@ Space Formulation," *IEEE J. Robotics and Automation*, 3(1), 1987. ## See also `ImpedanceControl` (`Λ` enables true inertia shaping); `HybridPositionForceControl` (adds force axes); -`RedundancyResolution` (kinematic null-space); `solvers/GaussianElimination` (the `M⁻¹Jᵀ` solve). +`RedundancyResolution` (kinematic null-space); `solvers::SolveSystem` (the `M⁻¹Jᵀ` solve, from +[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)). diff --git a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md index 184f5c0..490ff8f 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/implementation.md @@ -4,67 +4,77 @@ ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -class OperationalSpaceControl: - const dynamics::EulerLagrangeDynamics& model # M(q), C(q,q̇)q̇, g(q) - const kinematics::SpatialJacobian& jacobian # J (TaskDim×Dof), injected (#M8) +class OperationalSpaceControl: # TaskDim = 6 (pose) or ≤ 3 (position; 2 = planar xy) + const dynamics::EulerLagrangeDynamics& model # M, C q̇, g — typically ChainDynamicsModel (M29) + const kinematics::JacobianProvider& jacobian # J, J̇q̇, tool pose — ChainTaskJacobian (M8) math::SquareMatrix Kp # task-space position gain math::SquareMatrix Kd # task-space damping gain - solvers::GaussianElimination linearSolver # for the M⁻¹Jᵀ solve + T sigma # singularity damping σ (0 ⇒ exact Λ) ``` ## Interface -``` -OperationalSpaceControl(const EulerLagrangeDynamics& model, const SpatialJacobian& jacobian, - const SquareMatrix& Kp, const SquareMatrix& Kd) +```text +OperationalSpaceControl(const dynamics::EulerLagrangeDynamics& model, + const kinematics::JacobianProvider& jacobian, + const SquareMatrix& Kp, const SquareMatrix& Kd, T sigma) -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const TaskVector& x, const TaskVector& xd, - const TaskVector& xdDot, const TaskVector& xdDdot, - const StateVector& tauSecondary) # hot path +JointVector ComputeTorque(const JointVector& q, const JointVector& qDot, + const kinematics::SE3Transform& desiredPose, + const TaskVector& xdDot, const TaskVector& xdDdot, + const JointVector& tauSecondary) # hot path ``` ## Algorithm (pseudocode) -``` -function ComputeTorque(q, qDot, x, xd, xdDot, xdDdot, tauSecondary): # OPTIMIZE_FOR_SPEED - J = jacobian.Compute(q) # TaskDim×Dof - M = model.ComputeMassMatrix(q) # Dof×Dof (SPD) - # --- task-space (operational-space) inertia Λ = (J M⁻¹ Jᵀ)⁻¹ --- - MinvJt = linearSolver.SolveColumns(M, transpose(J)) # solve M·X = Jᵀ (no explicit inverse) - Lambda = inverse(J * MinvJt) # TaskDim×TaskDim (small: TaskDim ≤ 6) - # --- dynamically-consistent inverse and null-space projector --- - Jbar = MinvJt * Lambda # M⁻¹JᵀΛ (Dof×TaskDim) - N = Identity(Dof) - transpose(J) * transpose(Jbar) - # --- task command wrench --- - xDot = J * qDot - aX = xdDdot + Kd*(xdDot - xDot) + Kp*(xd - x) - mu = transpose(Jbar) * model.ComputeCoriolisTerms(q, qDot) # task-space Coriolis/centrifugal - p = transpose(Jbar) * model.ComputeGravityTerms(q) # task-space gravity - F = Lambda * aX + mu + p - # --- joint torque: task term + dynamically-consistent secondary term --- - return transpose(J) * F + N * tauSecondary +```text +function ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, tauSecondary): # OPTIMIZE_FOR_SPEED + J = jacobian.Jacobian(q) # TaskDim×Dof + M = model.ComputeMassMatrix(q) # Dof×Dof (SPD) + # --- task command (TaskError.hpp, see ImpedanceControl: PoseError for 6, position difference for ≤ 3) --- + eX = TaskError(desiredPose, jacobian.ToolPose(q)) + eXDot = xdDot − J·qDot + aX = xdDdot + Kd·eXDot + Kp·eX + # --- task-space inertia Λ = (J M⁻¹ Jᵀ + σ²I)⁻¹ --- + X = solvers::SolveSystem(M, Jᵀ) # M⁻¹Jᵀ, no explicit inverse + Lambda = solvers::SolveSystem(J·X + σ²·I, I) # TaskDim×TaskDim (small) + # --- dynamically-consistent null-space projector --- + JbarT = Lambda·Xᵀ # J̄ᵀ = Λ J M⁻¹ (M symmetric) + N = I − Jᵀ·JbarT # Dof×Dof + # --- torque: task term + FULL joint-space C q̇ + g + projected secondary torque --- + return Jᵀ·Lambda·(aX − jacobian.BiasAcceleration(q, qDot)) # J̇q̇ + + model.ComputeCoriolisTerms(q, qDot) + + model.ComputeGravityTerms(q) + + N·tauSecondary ``` ## Complexity & memory -- Time: `O(Dof³)` for the `M`-solve (LU/Gaussian); `O(TaskDim³)` for the `Λ` inverse (small); +- Time: `O(TaskDim·Dof³)` for the `M⁻¹Jᵀ` solve with `SolveSystem` (it eliminates per column; factoring + `M` once with `math::CholeskyDecomposition` gives `O(Dof³ + TaskDim·Dof²)`); `O(TaskDim³)` for `Λ`; `O(Dof²·TaskDim)` for the projector. - Memory: `O(Dof²)` working matrices; all stack/static, no heap. ## Numerical / embedded notes -- Solve `M·X = Jᵀ` with `solvers::GaussianElimination` (or LU) rather than forming `M⁻¹` — - cheaper and better-conditioned. -- `Λ = (J M⁻¹ Jᵀ)⁻¹` blows up at kinematic singularities (`J` loses rank); near `det(J M⁻¹ Jᵀ)→0` - switch to a **damped** inverse (add `σ²I`). -- The dynamically-consistent null-space `N = I − Jᵀ J̄ᵀ` guarantees `tauSecondary` produces **no** - task-space acceleration — clean priority stacking for redundant arms. -- The `−Λ J̇ q̇` centrifugal correction is added when the Jacobian derivative is available; omitting - it costs a small high-speed tracking error. -- `Λ` and `Jbar` are small (`TaskDim`-wide) — cache them when the task dimension ≪ `Dof`. +- **Task dynamics.** With `σ = 0`: `ẍ = J q̈ + J̇q̇ = aX` exactly, and `J·M⁻¹·N = 0`, so `tauSecondary` + produces **no** task-space acceleration. +- **Why `C q̇ + g` in joint space.** The classic form `F = Λ(aX − J̇q̇) + J̄ᵀC q̇ + J̄ᵀg`, `τ = JᵀF + Nτ₀` + compensates only `JᵀJ̄ᵀ(C q̇ + g)`; on a redundant arm (`Dof > TaskDim`) the null-space part + `N·(C q̇ + g)` is left uncompensated and the self-motion sags. The law above equals the classic form + plus `N·(C q̇ + g)`; task behaviour is unchanged, and at rest with zero error `τ = g` exactly. +- Solve `M·X = Jᵀ` with `solvers::SolveSystem` (multi-column Gaussian elimination) rather than forming + `M⁻¹` — cheaper and better-conditioned. +- `Λ = (J M⁻¹ Jᵀ)⁻¹` blows up at kinematic singularities (`J` loses rank); near + `det(J M⁻¹ Jᵀ) → 0` use σ > 0 (adds `σ²I` inside the inverse). With σ > 0 the task is tracked only + approximately and `J·M⁻¹·N ≈ 0`; with σ = 0 a rank-deficient `J` violates `SolveSystem`'s pivot assert. +- For `TaskDim = 6` the rotation-vector error rate equals `ωd − ω` only to first order about `eR = 0`. +- `Λ` and `J̄` are small (`TaskDim`-wide) — cache them when the task dimension ≪ `Dof`. +- The injected interfaces are a virtual seam for StrictMock tests; hard real-time builds may template + the controller on the concrete `ChainDynamicsModel` / `ChainTaskJacobian` to avoid virtual calls + (AGENTS.md: no virtual calls in real-time paths). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -72,14 +82,18 @@ function ComputeTorque(q, qDot, x, xd, xdDot, xdDdot, tauSecondary): # OPTIMIZ - Header: `robotics/controllers/manipulator/OperationalSpaceControl.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class OperationalSpaceControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class OperationalSpaceControl;` / `` / `` + under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/OperationalSpaceControl.cpp` → - `template class OperationalSpaceControl;` + `template class OperationalSpaceControl;` / `` / `` - Test: `robotics/controllers/manipulator/test/TestOperationalSpaceControl.cpp` - Doc: `doc/controllers/manipulator/OperationalSpaceControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestOperationalSpaceControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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` + (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/controllers/manipulator/OperationalSpaceControl/tests.md b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md index d002d8c..24b8222 100644 --- a/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md +++ b/roadmap/controllers/manipulator/OperationalSpaceControl/tests.md @@ -4,62 +4,73 @@ ## Fixture -``` -class TestOperationalSpaceControl : public ::testing::Test: - StrictMock> model - StrictMock> jacobian +```cpp +class TestOperationalSpaceControl : public ::testing::Test: # Dof = 2, TaskDim = 2 + StrictMock> model + StrictMock> jacobian SquareMatrix Kp = diag(100, 100) SquareMatrix Kd = diag( 20, 20) - OperationalSpaceControl controller{ model, jacobian, Kp, Kd } -# each case below is a TEST_F(TestOperationalSpaceControl, ) + OperationalSpaceControl controller{ model, jacobian, Kp, Kd, 0 } # σ = 0 + # ToolPose mocked as { I, (x, y, 0) }; desiredPose = { I, (xd, yd, 0) } + +class TestOperationalSpaceControlRedundant : public ::testing::Test: # Dof = 3, TaskDim = 2 + StrictMock> model # M = [[3,.5,.2],[.5,2,.3],[.2,.3,1]], g = (4,−2,1.5) + StrictMock> jacobian # J = [[1,.5,.2],[0,1,.7]] + OperationalSpaceControl controller{ model, jacobian, Kp, Kd, 0 } +# each case below is a TEST_F(, ); default fixture TestOperationalSpaceControl ``` ## Test cases (Arrange / Act / Assert) -``` +```text unit_inertia_gives_task_pd: - Arrange: model M=I, J=I, g=0, C=0; eX, eXDot given - Act: τ = ComputeTorque(...) - Assert: Λ=I ⇒ τ == xdDdot + Kd·eXDot + Kp·eX + Arrange: M = I, J = I, J̇q̇ = 0, C q̇ = 0, g = 0; eX, eXDot, xdDdot given + Act: τ = ComputeTorque(q, qDot, desiredPose, xdDot, xdDdot, 0) + Assert: τ == xdDdot + Kd·eXDot + Kp·eX + +task_inertia_scales_command: + Arrange: M = diag(2, 3), J = I (Λ = diag(2, 3)), J̇q̇ = 0, model terms 0 + Assert: τ == diag(2, 3)·aX -task_inertia_computed_from_mass: - Arrange: M = diag(2,2), J=I - Assert: Λ == diag(2,2); F scaled accordingly +bias_acceleration_subtracted: + Arrange: M = I, J = I, all errors 0, xdDdot = 0, J̇q̇ = b + Assert: τ == −b -gravity_mapped_to_task_and_back: - Arrange: model g != 0, M=I, J=I, errors 0 - Assert: τ == g (Jᵀ J̄ᵀ g collapses to g) +coriolis_and_gravity_added_in_joint_space: + Arrange: M = I, J = I, aX = 0, J̇q̇ = 0, C q̇ = c, g != 0 + Assert: τ == c + g jacobian_transpose_realises_wrench: - Arrange: nontrivial J, known F components - Assert: τ == transpose(J)·F (+ null-space term) + Arrange: M = I, J = [[1,0],[1,2]], errors 0, xdDdot = aX, model terms 0 + Assert: τ == Jᵀ·(J Jᵀ)⁻¹·aX (= J⁻¹·aX) -secondary_torque_projected_to_nullspace: - Arrange: redundant case (Dof>TaskDim), tauSecondary != 0 - Assert: J·M⁻¹·(N·tauSecondary) ≈ 0 (no task-space acceleration) +redundant_at_rest_holds_gravity_exactly (TestOperationalSpaceControlRedundant): + Arrange: q̇ = 0 (C q̇ = 0, J̇q̇ = 0), ToolPose = desiredPose, xdDot = xdDdot = 0, tauSecondary = 0 + Assert: τ == g = (4, −2, 1.5) (null-space gravity compensated) -nullspace_orthogonality: - Arrange: any tauSecondary - Assert: J · Jbar == Identity(TaskDim) and J·M⁻¹·N ≈ 0 +secondary_torque_invisible_to_task (TestOperationalSpaceControlRedundant): + Arrange: same state; τ₀ = (1, −2, 0.5); Δτ = τ(τ₀) − τ(0) + Assert: Δτ == N·τ₀ = (0.38863, −1.32780, 1.06224) != 0 and J·M⁻¹·Δτ ≈ 0 singular_jacobian_uses_damping: - Arrange: J rank-deficient - Assert: damped Λ stays finite (no NaN / inf) + Arrange: local controller with σ = 0.1; M = I, J = [[1,1],[1,1]] (rank 1), aX != 0 + Assert: τ finite (no NaN / inf) -msolve_and_model_terms_queried: +dependencies_queried_once: Arrange: any state - Assert: ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms each once; - jacobian.Compute once (StrictMock) + Assert: ComputeMassMatrix, ComputeCoriolisTerms, ComputeGravityTerms, Jacobian, BiasAcceleration, + ToolPose each called exactly once (StrictMock) ``` ## Reference vectors -- `M=I`, `J=I`: `Λ=I`, controller collapses to task-space PD `τ = Kp·eX + Kd·eXDot + xdDdot + g`. -- Consistency: `J·M⁻¹·N ≈ 0` — the null-space produces no task motion (verify numerically ≈ 0). +- `M = I`, `J = I`: `Λ = I`, controller collapses to task-space PD `τ = xdDdot + Kd·eXDot + Kp·eX + C q̇ + g`. +- Redundant fixture: the classic `J̄ᵀg` form would return `JᵀJ̄ᵀg = (3.3365, 0.2670, −0.3136) ≠ g`; + the law returns `g` exactly. `J·M⁻¹·N·τ₀ ≈ 0` (verified ≈ 1e-16 in double). ## Edge cases - Redundant arm (`Dof > TaskDim`): non-trivial null space; secondary objective realised without - disturbing the task. -- Singularity: damped inverse keeps torque bounded. + disturbing the task, and self-motion does not sag under gravity. +- Singularity: σ > 0 keeps torque bounded; task tracking becomes approximate. - Model mismatch: task decoupling degrades gracefully, stays stable. diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md index c05441f..f56b1cd 100644 --- a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md +++ b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md @@ -18,7 +18,7 @@ the spring no longer fights it and the joint lands precisely on the set-point. A energy argument (kinetic + spring potential) guarantees the arm always converges. ## Key parameters -- **model** — injected dynamics object supplying the gravity term `g(q)`. +- **model** — injected dynamics object supplying the gravity term `g(q)` (typically `ChainDynamicsModel`, M29). - **Kp (stiffness)** — how hard the controller pulls toward the target. - **Kd (damping)** — how strongly it resists velocity; pick `Kd ≈ 2√(Kp·inertia)` for critical damping. @@ -28,4 +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 model). +`FrictionCompensation` (adds a friction feedforward); `dynamics/EulerLagrangeDynamics` (the injected +interface); `ChainDynamicsModel` (M29, its link-chain implementation). diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md index 5e2bd45..3d84042 100644 --- a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md +++ b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md @@ -4,7 +4,7 @@ ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float class PdGravityCompensation: const dynamics::EulerLagrangeDynamics& model # only g(q) is queried @@ -14,7 +14,7 @@ class PdGravityCompensation: ## Interface -``` +```cpp # Dynamics model injected (DIP); the set-point regulator needs only its gravity term: PdGravityCompensation(const EulerLagrangeDynamics& model, const SquareMatrix& Kp, const SquareMatrix& Kd) @@ -26,7 +26,7 @@ Vector ComputeTorque(const StateVector& q, ## Algorithm (pseudocode) -``` +```text function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED # set-point regulation: qd is constant, so the desired velocity is zero e = qd - q @@ -49,8 +49,11 @@ function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED steady offset `e_ss = Kp⁻¹·g`. - **Set-point regulator only** — `qd` is constant. For trajectory tracking use `ComputedTorqueControl`. - Diagonal `Kp`, `Kd` decouple the joints and keep the hot path to `Dof` multiplies. -- Only `g(q)` is drawn from the injected model — the same interface used by the full computed-torque - law, so a single dynamics object serves both controllers (ISP / DIP). +- Only `g(q)` is drawn from the injected `EulerLagrangeDynamics`, typically a `dynamics::ChainDynamicsModel` + (M29), which also implements the `InverseDynamicsModel` used by `ComputedTorqueControl` — one model + object serves both controllers (ISP / DIP). +- The interface is a virtual seam for StrictMock tests; hard real-time builds may template the controller + on the concrete `ChainDynamicsModel` to avoid the virtual call (AGENTS.md: no virtual calls in real-time paths). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -58,14 +61,16 @@ function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED - Header: `robotics/controllers/manipulator/PdGravityCompensation.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class PdGravityCompensation;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class PdGravityCompensation;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/controllers/manipulator/PdGravityCompensation.cpp` → - `template class PdGravityCompensation;` + `template class PdGravityCompensation;` / `` - Test: `robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp` - Doc: `doc/controllers/manipulator/PdGravityCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestPdGravityCompensation.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". +- Depends on: shipped `dynamics::EulerLagrangeDynamics`; model typically M29 (`ChainDynamicsModel`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md index 3c96d4f..8e4136b 100644 --- a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md +++ b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md @@ -4,7 +4,7 @@ ## Fixture -``` +```cpp class TestPdGravityCompensation : public ::testing::Test: # Injected model mocked so torque math is verified in isolation: StrictMock> model @@ -16,7 +16,7 @@ class TestPdGravityCompensation : public ::testing::Test: ## Test cases (Arrange / Act / Assert) -``` +```cpp gravity_only_at_matched_setpoint: Arrange: q = qd, qDot = 0, model g(q) = [0, mgL] Act: τ = ComputeTorque(q, 0, qd) diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md index 928e568..508e5f0 100644 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/explanation.md @@ -3,8 +3,8 @@ ## What it is A trajectory-tracking manipulator controller that **learns the robot's inertial parameters online** while it moves. It exploits the fact that rigid-body dynamics are *linear in the inertial -parameters*: `M(q)q̈ + C(q,q̇)q̇ + g(q) = Y(q,q̇,q̈)·a`, so the unknown masses/inertias `a` can be -estimated by a simple adaptation law. +parameters*: `M(q)q̈r + C(q,q̇)q̇r + g(q) = Y(q,q̇,q̇r,q̈r)·a` (with the Christoffel-consistent Coriolis +matrix `C`), so the unknown masses/inertias `a` can be estimated by a simple adaptation law. ## Why it matters (embedded) Real robots carry unknown or changing payloads. Rather than re-identifying the model offline, this @@ -29,5 +29,6 @@ J.-J. Slotine, W. Li, "On the Adaptive Control of Robot Manipulators," *Int. J. 6(3), 1987. ## See also -`ComputedTorqueControl` (#M12, exact-model sibling), `ModelReferenceAdaptiveControl` (#47), -`DynamicParameterIdentification` (#M22, offline regressor identification). +`ComputedTorqueControl` (M12, exact-model sibling), `CoriolisMatrixAndRegressor` (M31, supplies `Y`), +`DynamicParameterIdentification` (M22, offline regressor identification); generic-plant model-reference +adaptive control lives in [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md index 9bbf119..8db7605 100644 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/implementation.md @@ -4,10 +4,10 @@ ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float class SlotineLiAdaptiveControl: - const dynamics::InertialRegressor& regressor # Y(q,q̇,q̇r,q̈r) + const dynamics::InertialRegressor& regressor # Y(q,q̇,q̇r,q̈r) — M31 math::SquareMatrix Lambda # sliding-surface slope (SPD) math::SquareMatrix Kd # sliding-variable gain (SPD) math::SquareMatrix Gamma # adaptation gain (SPD) @@ -17,9 +17,10 @@ class SlotineLiAdaptiveControl: ## Interface -``` -# Regressor injected (DIP): supplies Y such that M(q)q̈r + C(q,q̇)q̇r + g(q) = Y·a -SlotineLiAdaptiveControl(const InertialRegressor& regressor, const SquareMatrix& Lambda, +```text +# Regressor injected (DIP): supplies Y such that M(q)q̈r + C(q,q̇)q̇r + g(q) = Y·a, +# C the Christoffel-consistent Coriolis matrix (M31, dynamics::InertialRegressor) +SlotineLiAdaptiveControl(const dynamics::InertialRegressor& regressor, const SquareMatrix& Lambda, const SquareMatrix& Kd, const SquareMatrix& Gamma, const Vector& aHat0, T dt) @@ -32,7 +33,7 @@ void Reset(const Vector& aHat0) ## Algorithm (pseudocode) -``` +```text function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED qTilde = q - qd qTildeDot = qDot - qdDot @@ -51,7 +52,8 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED ## Complexity & memory -- Time: `O(Dof·NumParams)` for `Y·aHat` and `Yᵀ·s`; regressor build is `O(Dof·NumParams)` (RNEA form). +- Time: `O(Dof·NumParams)` for `Y·aHat` and `Yᵀ·s`; regressor build is `O(Dof·NumParams)` + (`ChainInertialRegressor`, M31, `NumParams = 10·N`). - Memory: `O(NumParams)` parameter state + `O(Dof²)` gains; no heap. ## Numerical / embedded notes @@ -60,12 +62,17 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED key robustness advantage over direct inverse-dynamics identification. - Global tracking convergence via Lyapunov `V = ½sᵀM(q)s + ½ãᵀΓ⁻¹ã` (SPD `M`, `ã = âHat − a`): `s → 0` hence `q̃ → 0`; parameters stay **bounded** but converge only under persistent excitation. +- **The proof needs `Ṁ − 2C` skew-symmetric**, so `Y` must use the *Christoffel-consistent* `C(q,q̇)` + applied to the reference velocity, `C(q,q̇)q̇r`. Plain RNEA (`ChainDynamicsModel`, M29) only returns the + vector `C(q,q̇)q̇` for a single velocity and cannot provide it; use the regressor of + `roadmap/dynamics/CoriolisMatrixAndRegressor` (M31). - Add a **projection** (or dead-zone) so `aHat` stays in a physically valid set (positive masses) and does not drift under sensor noise or unmodelled dynamics. - `Λ` sets the error-manifold bandwidth, `Kd` damps `s`, `Γ` sets adaptation speed — too large `Γ` causes estimate oscillation and can excite unmodelled modes. -- Reuses the RNEA regressor form; sibling of `ComputedTorqueControl` (#M12, non-adaptive) and - `ModelReferenceAdaptiveControl` (#47). +- Sibling of `ComputedTorqueControl` (M12, non-adaptive). The interface is a virtual seam for StrictMock + tests; hard real-time builds may template the controller on the concrete `ChainInertialRegressor` + to avoid the virtual call (AGENTS.md: no virtual calls in real-time paths). - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -73,14 +80,16 @@ function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - Header: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class SlotineLiAdaptiveControl;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. + `extern template class SlotineLiAdaptiveControl;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD` (`20 = 10·N` matches `ChainInertialRegressor`). - Coverage: `robotics/controllers/manipulator/SlotineLiAdaptiveControl.cpp` → - `template class SlotineLiAdaptiveControl;` + `template class SlotineLiAdaptiveControl;` / `` - Test: `robotics/controllers/manipulator/test/TestSlotineLiAdaptiveControl.cpp` - Doc: `doc/controllers/manipulator/SlotineLiAdaptiveControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestSlotineLiAdaptiveControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/controllers/CMakeLists.txt`, and add a +- 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: M31 (`InertialRegressor`, `ChainInertialRegressor`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md index 577f263..9e9e95e 100644 --- a/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md +++ b/roadmap/controllers/manipulator/SlotineLiAdaptiveControl/tests.md @@ -4,7 +4,7 @@ ## Fixture -``` +```cpp class TestSlotineLiAdaptiveControl : public ::testing::Test: # regressor injected & mocked; Dof = 2, NumParams = 3: StrictMock> regressor @@ -19,7 +19,7 @@ class TestSlotineLiAdaptiveControl : public ::testing::Test: ## Test cases (Arrange / Act / Assert) -``` +```cpp zero_error_uses_feedforward_only: Arrange: q=qd, qDot=qdDot ⇒ s=0; regressor Y given Act: τ = ComputeTorque(...) @@ -50,7 +50,8 @@ estimate_persists_across_calls: Assert: aHat integrates twice; ParameterEstimate() reflects both convergence_tracks_trajectory: - Arrange: wrap a plant M q̈ + Cq̇ + g = τ with unknown a; run K steps + Arrange: horizontal 2-link plant M q̈ + C q̇ = τ with true a (3 params); mock Invoke returns its + regressor Y(q,q̇,q̇r,q̈r) built with the Christoffel C; aHat0 = 0 ≠ a; run K steps Assert: ||q − qd|| -> 0 (tracking) even though aHat != a reset_restores_initial_estimate: @@ -67,6 +68,9 @@ regressor_queried_once_per_call: - By construction `s = q̃̇ + Λq̃`; single joint, `Λ=λ`, constant `q̃=e`, `q̃̇=0` ⇒ `s = λe` (hand-checkable). - With `aHat = a` (true params) the law reduces to computed-torque-like `τ = Y·a − Kd·s`. - One adaptation step: `Δa = −Γ·Yᵀ·s·dt` — exact, hand-verifiable. +- Horizontal 2-link arm, `a = (a1, a2, a3)` (Spong, Hutchinson, Vidyasagar): + `M = [[a1 + 2a2c₂, a3 + a2c₂], [a3 + a2c₂, a3]]`, Christoffel `C = a2s₂·[[−q̇₂, −(q̇₁+q̇₂)], [q̇₁, 0]]` ⇒ + `Y = [[q̈r₁, c₂(2q̈r₁ + q̈r₂) − s₂(q̇₂q̇r₁ + (q̇₁+q̇₂)q̇r₂), q̈r₂], [0, c₂q̈r₁ + s₂q̇₁q̇r₁, q̈r₁ + q̈r₂]]`. ## Edge cases diff --git a/roadmap/dynamics/ChainDynamicsModel/explanation.md b/roadmap/dynamics/ChainDynamicsModel/explanation.md new file mode 100644 index 0000000..290f2aa --- /dev/null +++ b/roadmap/dynamics/ChainDynamicsModel/explanation.md @@ -0,0 +1,31 @@ +# Chain Dynamics Model — Overview + +## What it is +An adapter that turns a robot described link by link into the "manipulator equation" +`M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` that model-based controllers speak: it answers "what is the mass +matrix?", "what are the Coriolis/centrifugal torques?", "what are the gravity torques?" and "what +torque produces this acceleration?". + +## Why it matters (embedded) +Gravity compensation, computed-torque, impedance and operational-space controllers all need these +terms. Without an adapter every controller would re-derive them or require hand-written analytic +models; with it, one link description drives all of them, and the one-call inverse-dynamics path +keeps computed-torque control at linear cost per cycle. + +## How it works (intuition) +The recursive Newton-Euler algorithm already computes joint torques for any state; zeroing the +acceleration and gravity isolates the velocity-dependent torques, zeroing velocity and acceleration +isolates gravity, and passing everything gives the full inverse dynamics in one pass. The composite +rigid body algorithm supplies the mass matrix. + +## Key parameters +- **Link array** — the same inertial and geometric description the dynamics algorithms use. +- **Gravity vector** — expressed in the base frame. + +## Reference +R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 5–6; M. W. Spong, S. Hutchinson, +M. Vidyasagar, *Robot Modeling and Control* (2020), Ch. 6. + +## See also +`RecursiveNewtonEuler`, `CompositeRigidBodyAlgorithm` (M28), `EulerLagrangeSolver`, +`PdGravityCompensation` (M5), `ComputedTorqueControl` (M12), `CoriolisMatrixAndRegressor` (M31). diff --git a/roadmap/dynamics/ChainDynamicsModel/implementation.md b/roadmap/dynamics/ChainDynamicsModel/implementation.md new file mode 100644 index 0000000..9677f0b --- /dev/null +++ b/roadmap/dynamics/ChainDynamicsModel/implementation.md @@ -0,0 +1,78 @@ +# Chain Dynamics Model (Link Chain → Manipulator Equation) — Implementation Pseudocode + +> Roadmap ref: #M29 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +The manipulator controllers (M5, M12, M16–M20) consume dynamics through interfaces, but nothing in the +library turns a link description into those interfaces. This adapter does: it implements the shipped +`EulerLagrangeDynamics` interface (`M`, `C q̇`, `g`) with CRBA (M28) and RNEA, and a new one-call +`InverseDynamicsModel` interface so computed-torque control can run in `O(n)`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class InverseDynamicsModel: # new interface, virtual ~InverseDynamicsModel() = default + virtual StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, + const StateVector& qDDot) const = 0 + +template +class ChainDynamicsModel + : public EulerLagrangeDynamics # shipped interface + , public InverseDynamicsModel: + LinkArray links # owned copy (payload updates via SetLinks) + Vector3 gravity # base-frame gravity, e.g. (0, 0, −9.81) + RecursiveNewtonEuler rnea + CompositeRigidBodyAlgorithm crba # M28 +``` + +## Interface + +```text +ChainDynamicsModel(const LinkArray& links, const Vector3& gravity) +void SetLinks(const LinkArray& links) # e.g. payload change +MassMatrix ComputeMassMatrix(const StateVector& q) const override +StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override +StateVector ComputeGravityTerms(const StateVector& q) const override +StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, + const StateVector& qDDot) const override # hot path +``` + +## Algorithm (pseudocode) + +```text +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, +# + diag(armature) in ComputeMassMatrix, friction reported separately (it is not part of C q̇) +``` + +## Complexity & memory + +- `ComputeInverseDynamics`, `ComputeCoriolisTerms`, `ComputeGravityTerms`: `O(n)` each (one RNEA pass). +- `ComputeMassMatrix`: `O(n²)` (CRBA). +- Memory: one copy of the link array; no heap. + +## Numerical / embedded notes + +- `C(q,q̇)q̇` obtained from RNEA is the correct Coriolis/centrifugal *vector*; controllers that need + the Coriolis *matrix* (Slotine–Li, momentum observer) must use M31 instead. +- The interfaces are virtual so controllers can be tested with StrictMock models. On a hard real-time + path, template the controller on `ChainDynamicsModel` directly to devirtualize (AGENTS.md: no virtual + calls in real-time paths). +- Must satisfy `ComputeInverseDynamics(q, q̇, q̈) = M q̈ + C q̇ + g` exactly (up to rounding) — the + cross-check test pins it. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Headers: `robotics/dynamics/InverseDynamicsModel.hpp` (pure interface) and + `robotics/dynamics/ChainDynamicsModel.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, + `OPTIMIZE_FOR_SPEED` on `ComputeInverseDynamics`, and `extern template class ChainDynamicsModel;` + / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/dynamics/ChainDynamicsModel.cpp` → the same instantiations. +- Test: `robotics/dynamics/test/TestChainDynamicsModel.cpp` +- Doc: `doc/dynamics/ChainDynamicsModel.md` (per `doc/TEMPLATE.md`) +- Depends on: M28. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/ChainDynamicsModel/tests.md b/roadmap/dynamics/ChainDynamicsModel/tests.md new file mode 100644 index 0000000..29ee47c --- /dev/null +++ b/roadmap/dynamics/ChainDynamicsModel/tests.md @@ -0,0 +1,40 @@ +# Chain Dynamics Model — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestChainDynamicsModel : public ::testing::Test: + # 2-link uniform rods about ŷ, gravity (0, 0, −9.81), q measured from horizontal + ChainDynamicsModel model{ MakeRodChain(1.2, 0.6, 0.7, 0.45), (0, 0, −9.81) } +# each case below is a TEST_F(TestChainDynamicsModel, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +gravity_terms_match_closed_form: + Assert: g(q) = −g·[m1l1/2·c1 + m2(l1c1 + l2/2·c12), m2l2/2·c12] +coriolis_terms_match_closed_form: + Assert: C q̇ = [−h(2q̇1q̇2 + q̇2²), h q̇1²], h = m2 l1 l2/2 · sin q2 +mass_matrix_matches_crba: + Assert: ComputeMassMatrix(q) == CompositeRigidBodyAlgorithm::Compute(links, q) +inverse_dynamics_equals_manipulator_equation: + Assert: ComputeInverseDynamics(q, q̇, q̈) ≈ M q̈ + C q̇ + g +euler_lagrange_forward_dynamics_matches_articulated_body_algorithm: + Assert: EulerLagrangeSolver::ForwardDynamics(model, q, q̇, τ) ≈ ABA(links, q, q̇, τ, gravity) +set_links_updates_every_term: + Arrange: double the last link mass via SetLinks + Assert: g(q) changes by the closed-form payload contribution +``` + +## Reference vectors + +- `m1=1.2, l1=0.6, m2=0.7, l2=0.45`, `q=(0.4,−0.9)`, `q̇=(1.1,−0.6)`, `q̈=(0.8,−1.7)` ⇒ + `τ = (−8.2064, −1.4410)` (analytic, matches RNEA). + +## Edge cases + +- Zero gravity ⇒ `g(q) = 0` for every `q`. +- `q̇ = 0` ⇒ `C q̇ = 0`. diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md new file mode 100644 index 0000000..61cd65b --- /dev/null +++ b/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md @@ -0,0 +1,31 @@ +# Composite Rigid Body Algorithm — Overview + +## What it is +An `O(n²)` algorithm that builds the joint-space mass (inertia) matrix `M(q)` of a serial arm — the +matrix in `M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` that says how joint accelerations turn into joint torques. + +## Why it matters (embedded) +Several controllers need `M(q)` itself rather than just torques: operational-space control reflects +it into task space, momentum observers integrate `M(q)q̇`, and some forward-dynamics schemes factor +it. Building it column by column with inverse dynamics costs `O(n²)` too but with a larger constant; +CRBA does it directly and cheaply. + +## How it works (intuition) +Treat everything outboard of a joint as one rigid "composite" body. Accelerating that joint by one +unit with all other joints held still requires a wrench equal to the composite inertia times the +joint's motion. Projecting that wrench onto the joint's own axis gives the diagonal entry; carrying +it inward and projecting on each ancestor axis gives the off-diagonal entries of the same column. + +## Key parameters +- **Link inertial parameters** — mass, center of mass, inertia tensor. +- **Joint axes and offsets** — the chain geometry. + +## Reference +R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 6; M. W. Walker, D. E. Orin, +"Efficient Dynamic Computer Simulation of Robotic Mechanisms," *ASME J. Dyn. Sys. Meas. Control*, +104(3), 1982. + +## See also +`ArticulatedBodyAlgorithm` (shares the spatial algebra), `RecursiveNewtonEuler` (column-by-column +alternative and cross-check), `ChainDynamicsModel` (M29), `OperationalSpaceControl` (M18), +`MomentumObserver` (M16). diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md new file mode 100644 index 0000000..d92f754 --- /dev/null +++ b/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md @@ -0,0 +1,72 @@ +# Composite Rigid Body Algorithm (CRBA) — Implementation Pseudocode + +> Roadmap ref: #M28 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +Computes the joint-space mass matrix `M(q)` of a serial chain described by the shipped link model. +`M(q)` is needed by operational-space control (M18), the momentum observer (M16), the chain dynamics +model (M29) and any forward-dynamics path that solves `M q̈ = τ − h`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class CompositeRigidBodyAlgorithm: # stateless + using MassMatrix = math::SquareMatrix + # reuses the spatial-inertia block form {rot, cross, lin} of ArticulatedBodyAlgorithm: + # I = [[rot, cross], [crossᵀ, lin]] about the joint origin, link frame, acting on (ω; v) +``` + +## Interface + +```text +MassMatrix Compute(const LinkArray& links, const JointVector& q) const # hot path +``` + +## Algorithm (pseudocode) + +```text +function Compute(links, q): # OPTIMIZE_FOR_SPEED + for i: R[i] = RotationAboutAxis(links[i].jointAxis, q[i]) + for i: Ic[i] = RigidBodyInertia(links[i]) # rot = I_c + m·Skew(c)ᵀSkew(c), cross = m·Skew(c), lin = m·I + # 1. composite inertias, tip → base (same transform as ABA's TransformInertiaToParent) + for i = N−1 .. 1: + Ic[i−1] += TransformInertiaToParent(Ic[i], R[i], links[i].parentToJoint) + # 2. one column per joint: the composite body's wrench for a unit joint acceleration, + # carried towards the base and projected on every ancestor axis + for i = N−1 .. 0: + z = links[i].jointAxis + n = Ic[i].rot · z; f = Ic[i].crossᵀ · z # wrench (n; f) about joint i, frame i + M[i][i] = z · n + j = i + while j > 0: + f = R[j] · f; n = R[j] · n + CrossProduct(links[j].parentToJoint, f) + j = j − 1 + M[i][j] = M[j][i] = links[j].jointAxis · n + return M +``` + +## Complexity & memory + +- `O(N²)`: `O(N)` composite-inertia pass plus `N(N−1)/2` wrench transforms (each `O(1)`). +- Memory: `N` spatial inertias (three 3×3 blocks each) + the `N×N` result; stack only; no recursion. + +## Numerical / embedded notes + +- Factor the spatial helpers now private to `ArticulatedBodyAlgorithm` (rigid-body inertia, inertia + and wrench transforms) into a shared internal header so ABA and CRBA cannot drift apart. +- `M` is symmetric positive definite for valid links; fill both triangles from one computation so the + result is exactly symmetric in `float`. +- Joint armature / reflected rotor inertia (M1 fields) is added to the diagonal: `M[i][i] += armature[i]`. +- To solve `M q̈ = b`, prefer a Cholesky factorization (numerical-toolbox `CholeskyDecomposition`). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/dynamics/CompositeRigidBodyAlgorithm.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and + `extern template class CompositeRigidBodyAlgorithm;` / `` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/dynamics/CompositeRigidBodyAlgorithm.cpp` → the same instantiations. +- Test: `robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp` +- Doc: `doc/dynamics/CompositeRigidBodyAlgorithm.md` (per `doc/TEMPLATE.md`) +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md new file mode 100644 index 0000000..2c7bd5b --- /dev/null +++ b/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md @@ -0,0 +1,41 @@ +# Composite Rigid Body Algorithm — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestCompositeRigidBodyAlgorithm : public ::testing::Test: + CompositeRigidBodyAlgorithm crba2 + CompositeRigidBodyAlgorithm crba3 + # uniform rods: inertia diag(0, ml²/12, ml²/12), CoM at (l/2, 0, 0) +# each case below is a TEST_F(TestCompositeRigidBodyAlgorithm, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +single_rod_about_end_is_ml2_over_3: + Assert: M = m·l²/3 for a z-axis rod +two_link_planar_matches_closed_form: + Arrange: z-axis rods m1, l1, m2, l2; q2 = −0.5 + Assert: M11 = m1l1²/3 + m2(l1² + l2²/3 + l1l2 cos q2), M12 = m2(l2²/3 + l1l2 cos q2 / 2), M22 = m2l2²/3 +matrix_is_independent_of_first_joint_angle: + Assert: M(q1 = 0.3, q2) ≈ M(q1 = −1.2, q2) for the planar arm +columns_match_recursive_newton_euler: + Arrange: 3-link chain with skewed axes, full inertia tensors, off-axis CoM + Assert: M·e_j ≈ RNEA(q, 0, e_j, gravity = 0) for every unit vector e_j +matrix_is_symmetric_positive_definite: + Assert: M == Mᵀ and all leading principal minors > 0 on the 3-link chain +kinetic_energy_matches_link_sum: + Assert: ½ q̇ᵀ M q̇ ≈ Σ ½(m‖v_c‖² + ωᵀ I_c ω) using link velocities from forward propagation +``` + +## Reference vectors + +- Unit rods, `q2 = −0.5`: `M11 = 5/3 + cos(0.5)`, `M12 = 1/3 + cos(0.5)/2`, `M22 = 1/3`. + +## Edge cases + +- One link ⇒ `1×1` matrix, no ancestor loop. +- Point-mass link (zero `I_c`) ⇒ still SPD while the mass is off the joint axis. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md new file mode 100644 index 0000000..ce5cbd9 --- /dev/null +++ b/roadmap/dynamics/CoriolisMatrixAndRegressor/explanation.md @@ -0,0 +1,37 @@ +# Coriolis Matrix and Inertial Regressor — Overview + +## What it is +Two views of the same rigid-body dynamics. The **Coriolis matrix** `C(q,q̇)` is the matrix whose +product with the joint velocity gives the centrifugal and Coriolis torques — chosen in the specific +(Christoffel) form that makes `Ṁ − 2C` skew-symmetric. The **inertial regressor** `Y` rewrites the +torques as a known matrix of motion terms times an unknown vector of inertial parameters (masses, +first moments, inertia-tensor entries). + +## Why it matters (embedded) +Adaptive controllers learn payloads online by adjusting the parameter vector, which only works with +the regressor form and with a skew-symmetric `Ṁ − 2C`. Collision-detection observers need `Cᵀq̇`. +Offline identification of a real robot's inertial parameters is a least-squares fit on the +regressor. All of these fall out of one modified recursive pass, at a few times the cost of plain +inverse dynamics. + +## How it works (intuition) +Run the recursive Newton-Euler sweep with two velocities side by side — the actual joint velocity and +a "reference" one. The actual velocity decides how the bodies rotate; the reference velocity is what +those rotating bodies are asked to carry. Splitting each body's gyroscopic term symmetrically between +the two produces the Coriolis matrix with the passivity property. Because every body's contribution +is linear in its ten inertial parameters, replacing the numbers by the columns that multiply them +gives the regressor. + +## Key parameters +- **Reference velocity/acceleration** — the Slotine–Li reference motion (or the actual motion). +- **Parameter vector** — ten numbers per link, inertia taken about the joint origin. + +## Reference +S. Echeandia, P. M. Wensing, "Numerical Methods to Compute the Coriolis Matrix and Christoffel +Symbols for Rigid-Body Systems," *J. Computational and Nonlinear Dynamics*, 16(9), 2021; +G. Niemeyer, J.-J. Slotine, "Performance in Adaptive Manipulator Control," *Int. J. Robotics +Research*, 10(2), 1991; C. Atkeson, C. An, J. Hollerbach, *Int. J. Robotics Research*, 5(3), 1986. + +## See also +`SlotineLiAdaptiveControl` (M20), `MomentumObserver` (M16), `DynamicParameterIdentification` (M22), +`CompositeRigidBodyAlgorithm` (M28), `RecursiveNewtonEuler`. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md new file mode 100644 index 0000000..da7beef --- /dev/null +++ b/roadmap/dynamics/CoriolisMatrixAndRegressor/implementation.md @@ -0,0 +1,100 @@ +# Coriolis Matrix and Inertial Regressor — Implementation Pseudocode + +> Roadmap ref: #M31 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +Plain RNEA only yields the Coriolis *vector* `C(q,q̇)q̇`. Passivity-based control (Slotine–Li, M20) +needs `C(q,q̇)q̇_r` for a reference velocity `q̇_r ≠ q̇`, the momentum observer (M16) needs `Cᵀ(q,q̇)q̇`, +and identification (M22) plus adaptive control need the dynamics written linearly in the inertial +parameters, `Y·π`. One modified RNEA pass provides all three, with the **Christoffel-consistent** +`C` (so `Ṁ − 2C` is skew-symmetric). + +## Data structures + +```cpp +# internal spatial algebra uses Featherstone ordering, motion (ω; v), force (n; f), link frames, +# exactly like the shipped ArticulatedBodyAlgorithm; nothing here is exposed as a public 6-vector. + +template # static_assert(std::is_floating_point_v); instantiated for float +class ModifiedRecursiveNewtonEuler: # stateless + JointVector InverseDynamics(links, q, q̇, q̇r, q̈r, gravity) const # = M q̈r + C(q,q̇) q̇r + g + +template +class CoriolisMatrix: # stateless + SquareMatrix Compute(links, q, q̇) const # Christoffel C(q, q̇) + JointVector TransposeTimesVelocity(links, q, q̇) const # Cᵀ(q, q̇) q̇ + +template +class InertialRegressor: # interface, virtual ~InertialRegressor() = default + virtual Matrix Compute(q, q̇, q̇r, q̈r) const = 0 # Y with Y·π = M q̈r + C q̇r + g + +template +class ChainInertialRegressor : public InertialRegressor: + const LinkArray& links; Vector3 gravity + static Vector Parameters(const LinkArray& links) + # per link π_i = (m, m·c_x, m·c_y, m·c_z, I_xx, I_xy, I_xz, I_yy, I_yz, I_zz), + # I = inertia about the JOINT origin: I_c + m·Skew(c)·Skew(c)ᵀ (link frame) +``` + +## Algorithm (pseudocode) + +```cpp +# helpers (6-vectors in (ω; v) / (n; f) ordering, 6×6 in 3×3 blocks): +# crm(v) = [[ω×, 0], [v×, ω×]] crf(v) = −crm(v)ᵀ = [[ω×, v×], [0, ω×]] +# barcrf(h) for force h = (n; f): barcrf(h)·w = crf(w)·h ⇒ barcrf(h) = [[−n×, −f×], [−f×, 0]] +# X_i = link-i-from-parent motion transform (same as ABA); S_i = (z_i; 0) +# bodyC(I, v) = ½·( crf(v)·I − I·crm(v) + barcrf(I·v) ) # skew-compatible body Coriolis + +function InverseDynamics(links, q, q̇, q̇r, q̈r, g): # OPTIMIZE_FOR_SPEED + v_−1 = vr_−1 = 0; a_−1 = (0; −g) # gravity as base acceleration + for i in 0..N−1: + v_i = X_i·v_{i−1} + S_i·q̇[i] + vr_i = X_i·vr_{i−1} + S_i·q̇r[i] + a_i = X_i·a_{i−1} + S_i·q̈r[i] + crm(v_i)·S_i·q̇r[i] # J_i q̈r + J̇_i q̇r (J̇ built with q̇) + f_i = I_i·a_i + bodyC(I_i, v_i)·vr_i + for i = N−1 .. 0: + τ[i] = S_iᵀ·f_i + if i > 0: f_{i−1} += X_iᵀ·f_i + return τ + +function CoriolisMatrix.Compute(links, q, q̇): # O(N²): one pass per column + for j: column j = InverseDynamics(links, q, q̇, e_j, 0, g = 0) +function TransposeTimesVelocity(links, q, q̇): return Transpose(Compute(links, q, q̇))·q̇ + +# regressor: bodyC and I·v are linear in π_i, via A(v)·π_i = I·v with v = (ω; u): +# A(v) = [[ 0₃ₓ₁, −u×, L(ω) ], +# [ u, ω×, 0₃ₓ₆ ]], L(ω) = [[ωx, ωy, ωz, 0, 0, 0], [0, ωx, 0, ωy, ωz, 0], [0, 0, ωx, 0, ωy, ωz]] +function ChainInertialRegressor.Compute(q, q̇, q̇r, q̈r): # O(N²) + forward pass for v_i, vr_i, a_i as above + for i in 0..N−1: + K = A(a_i) + ½·( crf(v_i)·A(vr_i) − A(crm(v_i)·vr_i) + crf(vr_i)·A(v_i) ) # 6×10 + for j = i .. 0: # carry towards the base + Y[j, 10i .. 10i+9] = S_jᵀ·K + if j > 0: K = X_jᵀ·K + return Y +``` + +## Complexity & memory + +- `InverseDynamics`: `O(N)`, about twice an RNEA pass. +- `CoriolisMatrix::Compute` and the regressor: `O(N²)`. +- Memory: `N` spatial velocities/accelerations and one `6×10` block; stack only; no recursion. + +## Numerical / embedded notes + +- With `q̇r = q̇, q̈r = q̈` the pass reduces to ordinary inverse dynamics — test against RNEA. +- `C` is the Christoffel form: `C(q,q̇)q̇` equals the RNEA bias and `Ṁ − 2C` is skew-symmetric; both + were verified numerically against finite-difference Christoffel symbols (residual ~1e-11 in double). +- The 10-parameter-per-link vector is not identifiable as a whole; M22 reduces it to base parameters. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## 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` + 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` +- Doc: `doc/dynamics/CoriolisMatrixAndRegressor.md` (per `doc/TEMPLATE.md`) +- Shares the spatial helpers factored out for M28 (CRBA) and the shipped ABA. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md b/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md new file mode 100644 index 0000000..a10675d --- /dev/null +++ b/roadmap/dynamics/CoriolisMatrixAndRegressor/tests.md @@ -0,0 +1,47 @@ +# Coriolis Matrix and Inertial Regressor — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestCoriolisMatrix : public ::testing::Test: + # 3-link chain: skewed unit axes, full SPD inertia tensors, off-axis CoM, arbitrary offsets + std::array, 3> chain{ ... } + CoriolisMatrix coriolis + ModifiedRecursiveNewtonEuler modified + RecursiveNewtonEuler rnea + CompositeRigidBodyAlgorithm crba +# each case below is a TEST_F(TestCoriolisMatrix, ) (regressor cases in TestChainInertialRegressor) +``` + +## Test cases (Arrange / Act / Assert) + +```text +reduces_to_rnea_when_reference_equals_actual: + Assert: modified.InverseDynamics(q, q̇, q̇, q̈, g) ≈ rnea.InverseDynamics(q, q̇, q̈, g) +coriolis_times_velocity_equals_rnea_bias: + Assert: C(q, q̇)·q̇ ≈ rnea.InverseDynamics(q, q̇, 0, 0) +matches_christoffel_symbols_of_the_mass_matrix: + Arrange: Γ_ijk = ½(∂M_ij/∂q_k + ∂M_ik/∂q_j − ∂M_jk/∂q_i) by central differences of crba.Compute + Assert: C_ij ≈ Σ_k Γ_ijk q̇_k +mass_matrix_derivative_minus_twice_coriolis_is_skew: + Assert: N = Ṁ − 2C (Ṁ by finite difference along q̇) satisfies N + Nᵀ ≈ 0 +two_link_planar_closed_form: + Assert: C = [[−h q̇2, −h(q̇1 + q̇2)], [h q̇1, 0]], h = m2 l1 lc2 sin q2 (uniform rods, lc2 = l2/2) +regressor_times_parameters_reproduces_dynamics: + Arrange: π = ChainInertialRegressor::Parameters(chain) + Assert: Y(q, q̇, q̇r, q̈r)·π ≈ modified.InverseDynamics(q, q̇, q̇r, q̈r, g) +regressor_is_independent_of_parameters: + Arrange: second chain with different masses/inertias but the same geometry + Assert: Y is identical for both chains (only π differs) +``` + +## Reference vectors + +- Planar 2R with uniform rods: `C` as above; `C q̇ = [−h(2q̇1q̇2 + q̇2²), h q̇1²]`. + +## Edge cases + +- `q̇ = 0` ⇒ `C = 0`. +- Single link ⇒ `C = 0` for any `q̇` (no velocity coupling about a fixed axis). diff --git a/roadmap/dynamics/DynamicParameterIdentification/explanation.md b/roadmap/dynamics/DynamicParameterIdentification/explanation.md new file mode 100644 index 0000000..e8b9ba8 --- /dev/null +++ b/roadmap/dynamics/DynamicParameterIdentification/explanation.md @@ -0,0 +1,32 @@ +# Dynamic Parameter Identification — Overview + +## What it is +A procedure that estimates a robot's inertial parameters (masses, centers of mass, inertia tensors) +from torque and motion measurements, by exploiting the fact that rigid-body dynamics are linear in +those parameters. + +## Why it matters (embedded) +Model-based control (gravity compensation, computed torque, impedance, collision detection) is only +as good as the model. CAD data rarely matches the real arm with cables, motors and grippers. +Identifying the parameters on the real robot — during commissioning or after a tool change — makes +every model-based controller more accurate. + +## How it works (intuition) +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. + +## Key parameters +- **Excitation trajectory** — decides how well-conditioned the problem is. +- **Rank tolerance** — which singular values count as identifiable. + +## Reference +C. Atkeson, C. An, J. Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads and Links," +*Int. J. Robotics Research*, 5(3), 1986; M. Gautier, W. Khalil, "Direct Calculation of Minimum Set of +Inertial Parameters of Serial Robots," *IEEE Trans. Robotics and Automation*, 6(3), 1990. + +## See also +`CoriolisMatrixAndRegressor` (M31), `SlotineLiAdaptiveControl` (M20, on-line counterpart), +`FrictionCompensation` (M4). diff --git a/roadmap/dynamics/DynamicParameterIdentification/implementation.md b/roadmap/dynamics/DynamicParameterIdentification/implementation.md new file mode 100644 index 0000000..0e15732 --- /dev/null +++ b/roadmap/dynamics/DynamicParameterIdentification/implementation.md @@ -0,0 +1,89 @@ +# 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) + +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. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +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 + +struct IdentificationResult: + math::Vector parameters # minimum-norm solution (identifiable projection of π) + std::size_t rank # number of base parameters + math::Matrix baseDirections # first `rank` rows span the identifiable combinations + T residualRms +``` + +## Interface + +```cpp +void AddSample(const JointVector& q, const JointVector& qDot, const JointVector& qDDot, + const JointVector& tauMeasured) # streaming, O(Dof·P²) +std::optional Solve(T relativeTolerance) const # nullopt if no samples +JointVector Predict(const IdentificationResult&, q, q̇, q̈) const # Y·π̂ for validation +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 Solve(tol): + (U, σ, V) = SingularValueDecomposition(R) # upstream numerical-toolbox SVD, P×P + rank = #{ σ_i > tol·σ_max } + π̂ = Σ_{i)`. + +## Deployment + +- Header: `robotics/dynamics/DynamicParameterIdentification.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `extern template class DynamicParameterIdentification;` + under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- 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). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/DynamicParameterIdentification/tests.md b/roadmap/dynamics/DynamicParameterIdentification/tests.md new file mode 100644 index 0000000..ab2f167 --- /dev/null +++ b/roadmap/dynamics/DynamicParameterIdentification/tests.md @@ -0,0 +1,43 @@ +# Dynamic Parameter Identification — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestDynamicParameterIdentification : public ::testing::Test: + # planar 2R uniform rods; "true" links generate noiseless torques with RNEA + ChainInertialRegressor regressor{ links, gravity } + DynamicParameterIdentification identification{ regressor } + # excitation: 200 samples of q = Σ Fourier terms, q̇, q̈ analytic +# each case below is a TEST_F(TestDynamicParameterIdentification, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +base_parameter_count_with_gravity_is_six: + Arrange: joint axes horizontal (gravity acts) + Assert: Solve().rank == 6 +base_parameter_count_without_gravity_is_four: + Arrange: joint axes parallel to gravity + Assert: Solve().rank == 4 +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() +``` + +## Reference vectors + +- Planar 2R: 6 base parameters with gravity in the plane of motion, 4 without (e.g. `ZZ1R, ZZ2, + MX2, MY2` plus `MX1R, MY1R` when gravity acts) — rank checked numerically. + +## Edge cases + +- Non-exciting data (constant pose) ⇒ rank drops; residual still finite. +- Additive torque noise ⇒ bounded bias; RMS residual ≈ noise level. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md new file mode 100644 index 0000000..eed1bb0 --- /dev/null +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md @@ -0,0 +1,28 @@ +# Forward-Dynamics Integrator — Overview + +## What it is +A fixed-step time-stepping routine that advances a robot arm's joint positions and velocities under +given joint torques, using the articulated body algorithm for the accelerations. + +## Why it matters (embedded) +Controllers are best tested against a simulated plant before touching hardware. A deterministic, +heap-free step function lets the same code run in unit tests, on a host simulator and in +hardware-in-the-loop rigs. + +## How it works (intuition) +Each step asks the dynamics for the joint accelerations at the current state and torque, then moves +the state forward in time. Semi-implicit Euler updates the velocity first and uses the new velocity +for the position, which keeps the energy error bounded; the classical fourth-order Runge-Kutta +method samples the dynamics four times per step for much higher accuracy. + +## Key parameters +- **Step size** — accuracy and stability versus cost. +- **Method** — semi-implicit Euler (cheap, bounded energy error) or RK4 (accurate). + +## Reference +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. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md new file mode 100644 index 0000000..56c8de8 --- /dev/null +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md @@ -0,0 +1,72 @@ +# Forward-Dynamics Integrator (Simulation Step) — Implementation Pseudocode + +> Roadmap ref: #M34 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +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. + +## Data structures + +```cpp +enum class IntegrationMethod : uint8_t { SemiImplicitEuler, RungeKutta4 } + +template # static_assert(std::is_floating_point_v); instantiated for float +struct ChainState: + JointVector q, qDot, qDDot # qDDot of the last evaluation (for logging / RNEA checks) + +template +class ForwardDynamicsIntegrator: + const LinkArray& links + Vector3 gravity + ArticulatedBodyAlgorithm aba +``` + +## 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 +``` + +## 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 + return { q, q̇, a } +``` + +## Complexity & memory + +- Semi-implicit Euler: one ABA pass (`O(N)`) per step; RK4: four. +- 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. +- 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. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/dynamics/ForwardDynamicsIntegrator.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Step`, `extern template` for + `` and `` + under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/dynamics/ForwardDynamicsIntegrator.cpp` → the same instantiations. +- 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. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md b/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md new file mode 100644 index 0000000..41ef93a --- /dev/null +++ b/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md @@ -0,0 +1,39 @@ +# Forward-Dynamics Integrator — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```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 } +# each case below is a TEST_F(TestForwardDynamicsIntegrator, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +small_oscillation_period_matches_physical_pendulum: + Arrange: rod length L hanging at q = π/2, released from π/2 + 0.05 rad, dt = 1 ms + Assert: measured period ≈ 2π·sqrt(2L/(3g)) within 0.5 % +rk4_energy_drift_is_small: + Assert: |E(10 s) − E(0)| / |E(0)| < 1e-4 for the pendulum at dt = 1 ms +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) +constant_torque_on_free_link_gives_quadratic_angle: + Arrange: z-axis rod (gravity ⟂ motion), τ constant + Assert: q(t) ≈ ½·(τ / (mL²/3))·t² +``` + +## Reference vectors + +- Uniform rod pendulum: `ω₀ = sqrt(3g / (2L))`; `L = 1 m`, `g = 9.81` ⇒ period `1.638 s`. + +## Edge cases + +- `dt = 0` ⇒ state unchanged. +- Zero gravity, zero torque ⇒ constant velocity. diff --git a/roadmap/dynamics/FrictionCompensation/implementation.md b/roadmap/dynamics/FrictionCompensation/implementation.md index 9c7107a..cb43e39 100644 --- a/roadmap/dynamics/FrictionCompensation/implementation.md +++ b/roadmap/dynamics/FrictionCompensation/implementation.md @@ -4,7 +4,7 @@ ## 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 @@ -13,6 +13,7 @@ struct JointFrictionParameters: # one per joint 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: @@ -21,7 +22,7 @@ class FrictionCompensation: ## Interface -``` +```cpp FrictionCompensation(const std::array, NumJoints>& params) # Feedforward friction torque to add to the control law: @@ -33,14 +34,14 @@ 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 level * dir + p.viscous * v + return clamp(level * dir + p.viscous * v, −p.maxCompensation, p.maxCompensation) function Compute(qDot): # OPTIMIZE_FOR_SPEED for i in 0 .. NumJoints-1: @@ -61,8 +62,11 @@ function Compute(qDot): # OPTIMIZE_FOR_SPEED 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 cap each term and validate - `stiction >= coulomb`, `stribeckVelocity > 0`, `smoothingVelocity > 0` at construction. +- 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. @@ -70,9 +74,9 @@ function Compute(qDot): # OPTIMIZE_FOR_SPEED - Header: `robotics/dynamics/FrictionCompensation.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorque`, and - `extern template class FrictionCompensation;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. + `extern template class FrictionCompensation;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. - Coverage: `robotics/dynamics/FrictionCompensation.cpp` → - `template class FrictionCompensation;` + `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`; diff --git a/roadmap/dynamics/FrictionCompensation/tests.md b/roadmap/dynamics/FrictionCompensation/tests.md index 958f852..15c92d3 100644 --- a/roadmap/dynamics/FrictionCompensation/tests.md +++ b/roadmap/dynamics/FrictionCompensation/tests.md @@ -4,9 +4,9 @@ ## 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 + # 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, ) @@ -14,7 +14,7 @@ class TestFrictionCompensation : public ::testing::Test: ## Test cases (Arrange / Act / Assert) -``` +```text positive_velocity_positive_torque: Arrange: qDot = +1.0 Act: tau = Compute(qDot) @@ -29,12 +29,12 @@ viscous_dominates_at_high_speed: Assert: tau grows ~linearly with slope Fv stiction_exceeds_coulomb_near_zero: - Arrange: small v just above eps - Assert: |level| closer to Fs than Fc (Stribeck peak) + 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: level -> Fc within tol + Assert: JointTorque(p, v) − Fv·v → Fc within tol smooth_through_zero: Arrange: v = 0 @@ -45,13 +45,16 @@ multi_joint_elementwise: Assert: each output equals its per-joint JointTorque feedforward_additive_cancels_model: - Arrange: plant friction = model; command = -Compute(qDot) - Assert: net joint friction ≈ 0 (compensation cancels) + 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`. +- 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`. diff --git a/roadmap/dynamics/GenericJointLink/explanation.md b/roadmap/dynamics/GenericJointLink/explanation.md index 66a0b82..118ff62 100644 --- a/roadmap/dynamics/GenericJointLink/explanation.md +++ b/roadmap/dynamics/GenericJointLink/explanation.md @@ -1,33 +1,32 @@ # Generic Joint Link — Overview ## What it is -A single struct that describes **either** a revolute (rotating) **or** a prismatic (sliding) joint, -by carrying a joint *type* alongside a unit *axis* and the usual inertial parameters. It generalizes -the revolute-only link so one chain can mix hinges and slides. +One link description that covers both revolute (rotating) and prismatic (sliding) joints and also +carries joint limits and the actuator's reflected inertia (armature). ## Why it matters (embedded) -Real machines are rarely all-revolute: SCARA arms, gantry/Cartesian robots, hydraulic rams, and -3-D printers all contain sliding axes. Encoding the joint type once — as a byte-sized enum plus a -shared code path — lets forward kinematics, the Jacobian, and inverse dynamics handle mixed chains -without duplicating an entire link/algorithm family per joint type. +Real machines are rarely all-revolute: SCARA arms, gantries, linear axes and hydraulic rams contain +sliding joints. Limits are needed by every planner and inverse-kinematics solver, and the armature of +geared actuators often dominates the effective inertia of small arms — ignoring it makes +model-based control markedly worse. ## How it works (intuition) -Every 1-DOF joint moves along a **screw axis**. A revolute joint spends its motion in the *angular* -part of that screw (it rotates by `q` about the axis); a prismatic joint spends it in the *linear* -part (it slides by `q` along the axis). The link therefore exposes two things: the relative -transform produced by `q`, and which channel — angular or linear — the axis occupies. Downstream -algorithms read those and stay joint-type-agnostic. +Every one-degree-of-freedom joint moves along a screw axis: a revolute joint spends its motion in +the angular channel, a prismatic joint in the linear channel. The kinematic and dynamic recursions +only need to know which channel the joint drives; for prismatic joints the torque projection becomes +a force projection and an extra Coriolis term appears when the slider rides on a rotating link. +Armature adds a rotor inertia that only the joint itself feels. ## Key parameters -- **type** — `Revolute` or `Prismatic`. -- **axis** — unit vector; rotation axis (revolute) or slide direction (prismatic). -- **parentToJoint / jointToCoM** — link geometry, unchanged from the revolute link. -- **mass / inertia** — rigid-body inertial parameters. +- **type** — revolute or prismatic. +- **axis** — unit rotation axis or slide direction. +- **limits** — position, velocity and effort bounds. +- **armature** — reflected actuator inertia. ## Reference -J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 -(link description and joint transforms). +J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; +R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 4 (joint models). ## See also -`RevoluteJointLink` (the specialization it generalizes), `RecursiveNewtonEuler` (consumes the -motion subspace), Denavit-Hartenberg parameters (M7), spatial Jacobian (M8). +`RecursiveNewtonEuler`, `ArticulatedBodyAlgorithm`, `CompositeRigidBodyAlgorithm` (M28), +`ChainPoseKinematics` (M30), `DenavitHartenberg` (M7). diff --git a/roadmap/dynamics/GenericJointLink/implementation.md b/roadmap/dynamics/GenericJointLink/implementation.md index 41713c0..cc23f53 100644 --- a/roadmap/dynamics/GenericJointLink/implementation.md +++ b/roadmap/dynamics/GenericJointLink/implementation.md @@ -1,79 +1,86 @@ -# Generic (Revolute / Prismatic) Joint Link — Implementation Pseudocode +# Generic (Revolute / Prismatic) Joint Link with Limits and Armature — Implementation Pseudocode -> Roadmap ref: #M1 (Tier 1) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) +> Roadmap ref: #M1 (Tier 1) · Target: `robotics/dynamics` (+ changes in `kinematics`) · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) ## Data structures -``` -enum class JointType : uint8_t { Revolute, Prismatic } +```cpp +enum class JointType : uint8_t { Revolute, Prismatic } # shared with M7 / M30 + +template # static_assert(std::is_floating_point_v); instantiated for float +struct JointLimits: + T positionMin = −∞, positionMax = +∞ # rad or m + T velocityMax = +∞ # rad/s or m/s + T effortMax = +∞ # N·m or N -template # static_assert(std::is_floating_point_v); instantiated for float -struct GenericJointLink: - JointType type # revolute (rotate) or prismatic (slide) +template +struct JointLink: # evolves the shipped RevoluteJointLink T mass - math::SquareMatrix inertia # inertia tensor at CoM, link frame - math::Vector axis # unit joint axis in link frame - math::Vector parentToJoint # fixed joint origin in parent frame - math::Vector jointToCoM # CoM position in link frame + math::SquareMatrix inertia # at the CoM, link frame + math::Vector jointAxis # unit, link frame (rotation axis or slide direction) + math::Vector parentToJoint # joint origin at q = 0, parent frame + math::Vector jointToCoM # link frame + JointType type = JointType::Revolute # new fields are trailing and defaulted, + JointLimits limits = {} # so every existing aggregate + T armature = 0 # initializer keeps compiling + +template using RevoluteJointLink = JointLink # migration alias ``` -## Interface +`armature` is the reflected actuator inertia (`G²·I_rotor`, kg·m² for revolute, kg for prismatic). -``` -# Relative transform produced by the joint variable q: -(Matrix3 R, Vector3 p) JointTransform(T q) const # hot path - -# Motion-subspace (screw) split — exactly one is the axis, the other is zero: -Vector3 AngularAxis() const # axis if Revolute else 0 -Vector3 LinearAxis() const # axis if Prismatic else 0 +## Interface -# Adapter so existing revolute-only code keeps compiling: -static GenericJointLink FromRevolute(const RevoluteJointLink& link) +```text +(Matrix3 R, Vector3 offset) JointTransform(T q) const # link frame relative to parent; hot path +Vector3 AngularAxis() const # axis if Revolute else 0 # motion subspace S = (AngularAxis; LinearAxis) +Vector3 LinearAxis() const # axis if Prismatic else 0 # in the dynamics' (ω; v) ordering ``` ## Algorithm (pseudocode) -``` -function JointTransform(q): # OPTIMIZE_FOR_SPEED - if type == Revolute: - R = math::RotationAboutAxis(axis, q) # rotate by angle q about axis - p = parentToJoint # origin fixed - else: # Prismatic - R = Identity3 # no rotation - p = parentToJoint + axis * q # slide distance q along axis - return (R, p) - -function AngularAxis(): return (type == Revolute) ? axis : 0 -function LinearAxis(): return (type == Prismatic) ? axis : 0 +```cpp +function JointTransform(q): # OPTIMIZE_FOR_SPEED + if type == Revolute: return (RotationAboutAxis(jointAxis, q), parentToJoint) + else: return (Identity, parentToJoint + jointAxis·q) + +# Required changes in the shipped algorithms (per joint, link frame = parent rotated by R, offset r): +FK / ChainPoseKinematics (M30): origin_i = origin_{i−1} + R_{i−1}·r_i ; R_i = R_{i−1}·R(q_i) + Jacobian column (M8): revolute (z × (p − o); z), prismatic (z; 0) +RNEA forward pass, prismatic i: ω_i = Rᵀω_{i−1} = ω_{i−1}; ω̇_i = ω̇_{i−1} + a_i = a_{i−1} + ω̇_{i−1}×r_i + ω_{i−1}×(ω_{i−1}×r_i) + 2·ω_i×(z·q̇_i) + z·q̈_i +RNEA backward pass: child offset is r_{i+1} (= parentToJoint + z·q for a prismatic child) + τ_i = z·n_i (revolute) | τ_i = z·f_i (prismatic) + τ_i += armature_i·q̈_i +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) ``` ## Complexity & memory -- `JointTransform`: `O(1)` — one axis-angle rotation (revolute) or one scaled add (prismatic). -- Memory: one enum byte + the existing inertial fields; no growth over `RevoluteJointLink`. +- `JointTransform`: `O(1)`. The algorithm changes keep every pass `O(N)` (`O(N²)` for CRBA). +- Memory: one enum byte, four limit scalars and the armature per link. ## Numerical / embedded notes -- The `(AngularAxis, LinearAxis)` pair *is* the motion-subspace `S`: FK multiplies `JointTransform` - down the chain; the 6×N Jacobian (M8) fills column `i` from these two vectors; RNEA injects - `axis·q̇` into the angular channel (revolute) or the linear channel (prismatic). -- Keep `axis` **unit-normalized** at construction; a non-unit axis silently rescales both the - rotation angle and the slide distance. -- `RotationAboutAxis` (Rodrigues form) avoids gimbal issues and is branch-light — good for the FK loop. -- A `Prismatic` joint contributes no rotation, so its inertia only shifts via the parallel-axis - term along `axis·q`; revolute links keep the existing propagation unchanged. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Migration:** keep the aggregate order of the shipped struct and append defaulted fields; alias + `RevoluteJointLink` so FK/IK/RNEA/ABA and their tests compile unchanged, then add the prismatic + branches behind `type`. +- The prismatic `2ω×(z·q̇)` Coriolis term and the `z·f` projection are the two places a revolute-only + implementation silently goes wrong — both are pinned by tests. +- Keep `jointAxis` unit-normalized (assert at construction); a non-unit axis rescales both the angle + and the slide. +- Armature adds to the joint-space inertia only; it does not change the kinematics or gravity. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment -- Header: `robotics/dynamics/GenericJointLink.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `JointTransform`, and - `extern template struct GenericJointLink;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/GenericJointLink.cpp` → - `template struct GenericJointLink;` -- Test: `robotics/dynamics/test/TestGenericJointLink.cpp` -- Doc: `doc/dynamics/GenericJointLink.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestGenericJointLink.cpp` → the `_test` target. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". +- Header: `robotics/dynamics/JointLink.hpp` (and `RevoluteJointLink.hpp` becomes the alias) — + `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `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. +- Doc: `doc/dynamics/JointLink.md` (per `doc/TEMPLATE.md`) and updates to the RNEA/ABA/FK docs. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/GenericJointLink/tests.md b/roadmap/dynamics/GenericJointLink/tests.md index f2041ec..166ff27 100644 --- a/roadmap/dynamics/GenericJointLink/tests.md +++ b/roadmap/dynamics/GenericJointLink/tests.md @@ -4,61 +4,44 @@ ## Fixture -``` -class TestGenericJointLink : public ::testing::Test: - # unit axes for readable expectations - Vector3 z = {0, 0, 1} - Vector3 x = {1, 0, 0} - GenericJointLink MakeRevolute(axis, parentToJoint) - GenericJointLink MakePrismatic(axis, parentToJoint) -# each case below is a TEST_F(TestGenericJointLink, ) +```cpp +class TestJointLink : public ::testing::Test: + Vector3 x = {1, 0, 0}, z = {0, 0, 1} + JointLink MakeRevolute(axis, parentToJoint), MakePrismatic(axis, parentToJoint) +# each case below is a TEST_F(TestJointLink, ) (algorithm cases live in the RNEA/ABA/FK tests) ``` ## Test cases (Arrange / Act / Assert) -``` -revolute_rotates_about_axis: - Arrange: revolute about z, q = pi/2 - Act: (R, p) = JointTransform(q) - Assert: R maps x -> y (±tol), p == parentToJoint - -revolute_zero_angle_is_identity: - Arrange: revolute about z, q = 0 - Assert: R == I, p == parentToJoint - -prismatic_translates_along_axis: - Arrange: prismatic along x, parentToJoint = {0,0,0}, q = 0.3 - Act: (R, p) = JointTransform(q) - Assert: R == I, p == {0.3, 0, 0} - -prismatic_keeps_orientation: - Arrange: prismatic along z, sweep q - Assert: R == I for all q (no rotation) - -revolute_motion_subspace: - Arrange: revolute about axis a - Assert: AngularAxis() == a, LinearAxis() == 0 - -prismatic_motion_subspace: - Arrange: prismatic along axis a - Assert: LinearAxis() == a, AngularAxis() == 0 - -from_revolute_matches_legacy: - Arrange: RevoluteJointLink r; g = FromRevolute(r) - Assert: same mass/inertia/axis; JointTransform is a pure rotation - -offset_joint_origin_applied: - Arrange: prismatic along x, parentToJoint = {0,1,0}, q = 0.5 - Assert: p == {0.5, 1, 0} +```text +existing_aggregate_initializers_default_to_revolute: + Arrange: RevoluteJointLink{ m, I, axis, offset, com } + Assert: type == Revolute, armature == 0, limits unbounded +revolute_transform_rotates_about_axis: + Assert: JointTransform(π/2) about z maps x → y, offset == parentToJoint +prismatic_transform_slides_along_axis: + Assert: JointTransform(0.3) along x == (I, parentToJoint + (0.3, 0, 0)) +motion_subspace_selects_one_channel: + Assert: revolute → (axis; 0), prismatic → (0; axis) +vertical_slider_needs_weight_plus_inertial_force: # RNEA test file + Arrange: prismatic along z, mass m, gravity (0, 0, −g), q̈ = a + Assert: τ = m·(g + a) +prismatic_coriolis_term_on_rotating_base: # RNEA test file + Arrange: revolute base about z spinning at ω, prismatic slider along x at q = r, q̇ = v + Assert: base torque = 2·m·r·v·ω (Coriolis) + inertial terms per closed form +mixed_chain_round_trips_through_aba: # ABA test file + Assert: ABA(RNEA(q̈)) ≈ q̈ for a revolute–prismatic–revolute chain under gravity +armature_adds_to_diagonal_inertia: + Assert: single rod: τ = (m l²/3 + armature)·q̈ in RNEA; ABA inverts it; CRBA M = m l²/3 + armature ``` ## Reference vectors -- Revolute z, q = π/2: `R·[1,0,0]ᵀ = [0,1,0]ᵀ`. -- Prismatic x, q = d: `p = parentToJoint + [d,0,0]ᵀ`, `R = I`. +- Slider: `m = 2`, `a = 1`: `τ = 2·(9.81 + 1) = 21.62 N`. +- Rotating slider (point mass at `r` on an arm spinning at `ω`, extending at `v`): Coriolis torque + `2 m r v ω` about the base axis. ## Edge cases -- Negative `q` (reverse rotation / retracting slide) mirrors the positive case. -- Non-unit `axis` — assert construction normalizes (or a guard rejects it). -- Full `2π` revolute wrap returns to identity within tolerance. +- Negative `q` retracts the slider / reverses the rotation. +- Full `2π` revolute wrap returns to identity. diff --git a/roadmap/dynamics/MomentumObserver/explanation.md b/roadmap/dynamics/MomentumObserver/explanation.md new file mode 100644 index 0000000..fb936f0 --- /dev/null +++ b/roadmap/dynamics/MomentumObserver/explanation.md @@ -0,0 +1,31 @@ +# Generalized-Momentum Observer — Overview + +## What it is +A disturbance observer that estimates the torque an unexpected contact applies to each joint, using +only the motor command and the measured joint positions and velocities. + +## Why it matters (embedded) +Collaborative robots must stop or react within milliseconds of hitting something or someone. The +observer needs no joint-torque sensors and no differentiated velocities, runs at the control rate +and turns a model the controller already has into a safety signal. + +## How it works (intuition) +The robot's generalized momentum `M(q)q̇` changes only because of applied torques, gravity, a +velocity-dependent term, and any external torque. The observer integrates everything it knows and +compares the result with the momentum it actually measures; the mismatch, fed back through a gain, +converges to the external torque with a first-order lag. Using momentum instead of acceleration is +what avoids differentiating noisy velocities. + +## Key parameters +- **Observer gain** — bandwidth of the estimate (detection speed versus noise). +- **Thresholds** — per-joint residual levels that declare a collision. + +## Reference +A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, "Collision Detection and Safe Reaction with +the DLR-III Lightweight Manipulator Arm," *IEEE/RSJ IROS*, 2006; S. Haddadin, A. De Luca, +A. Albu-Schäffer, "Robot Collisions: A Survey on Detection, Isolation, and Identification," *IEEE +Trans. Robotics*, 33(6), 2017. + +## See also +`ChainDynamicsModel` (M29), `CoriolisMatrixAndRegressor` (M31), `FrictionCompensation` (M4), +`RneaExternalWrench` (M32). diff --git a/roadmap/dynamics/MomentumObserver/implementation.md b/roadmap/dynamics/MomentumObserver/implementation.md new file mode 100644 index 0000000..61c71da --- /dev/null +++ b/roadmap/dynamics/MomentumObserver/implementation.md @@ -0,0 +1,67 @@ +# Generalized-Momentum Observer (Collision Detection) — Implementation Pseudocode + +> Roadmap ref: #M16 (Tier 3) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +Estimates the external joint torque from commanded torque and joint state only — no joint-torque +sensors and no acceleration measurement — so contacts and collisions can be detected on-line. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class MomentumObserver: + const EulerLagrangeDynamics& model # M(q), g(q) — typically ChainDynamicsModel (M29) + const CoriolisMatrix& coriolis # Cᵀ(q, q̇) q̇ (M31); the RNEA vector C q̇ is NOT enough + const LinkArray& links + math::Vector gain # K_O (diagonal), 1/s + math::Vector initialMomentum, integral, residual # STATE +``` + +## Interface + +```text +MomentumObserver(model, coriolis, links, gain) +void Reset(const JointVector& q, const JointVector& qDot) # p(0) +const JointVector& Update(const JointVector& q, const JointVector& qDot, + const JointVector& tauCommanded, T dt) # hot path +bool Collision(const JointVector& threshold) const # |r_i| > threshold_i +``` + +## Algorithm (pseudocode) + +```text +# dynamics: ṗ = τ + Cᵀ(q,q̇) q̇ − g(q) + τ_ext, p = M(q) q̇ (uses Ṁ = C + Cᵀ) +function Reset(q, q̇): + initialMomentum = M(q)·q̇; integral = 0; residual = 0 + +function Update(q, q̇, τ, dt): # OPTIMIZE_FOR_SPEED + β = g(q) − coriolis.TransposeTimesVelocity(links, q, q̇) + integral = integral + (τ − β + residual)·dt + residual = gain ⊙ (M(q)·q̇ − initialMomentum − integral) + return residual # ṙ = K_O (τ_ext − r): first-order estimate of τ_ext +``` + +## Complexity & memory + +- `Update`: `O(N²)` (mass matrix and `Cᵀq̇`); memory `O(N)` state + `O(N²)` scratch; no heap. + +## Numerical / embedded notes + +- Subtract a friction model (M4) 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 + collision-free runs. +- Forward-Euler integration of the observer at the control rate is standard; keep `K_O·dt ≪ 1`. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/dynamics/MomentumObserver.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, + `OPTIMIZE_FOR_SPEED` on `Update`, `extern template class MomentumObserver;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/dynamics/MomentumObserver.cpp` → the same instantiations. +- Test: `robotics/dynamics/test/TestMomentumObserver.cpp` +- Doc: `doc/dynamics/MomentumObserver.md` (per `doc/TEMPLATE.md`) +- Depends on: M28/M29 (M, g), M31 (`Cᵀq̇`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/MomentumObserver/tests.md b/roadmap/dynamics/MomentumObserver/tests.md new file mode 100644 index 0000000..3e1db3a --- /dev/null +++ b/roadmap/dynamics/MomentumObserver/tests.md @@ -0,0 +1,42 @@ +# Generalized-Momentum Observer — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestMomentumObserver : public ::testing::Test: + # 2-link uniform rods about ŷ, gravity on; plant simulated with ABA at dt = 1 ms + ChainDynamicsModel model{ ... } + CoriolisMatrix coriolis + MomentumObserver observer{ model, coriolis, links, gain = (50, 50) } +# each case below is a TEST_F(TestMomentumObserver, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +free_motion_keeps_residual_near_zero: + Arrange: gravity-compensated motion with commanded torque, no external torque, 1 s + Assert: max |r| stays below 1e-2 N·m +step_external_torque_is_recovered_with_first_order_response: + Arrange: τ_ext = (0, 0.8) N·m applied to the plant from t = 0.2 s + Assert: r_2(0.2 + 1/K) ≈ 0.8·(1 − e^{−1}) and r_2 → 0.8 +collision_flag_uses_per_joint_thresholds: + Assert: Collision((0.5, 0.5)) is false before and true after the step settles +reset_restarts_from_current_momentum: + Arrange: run with non-zero velocity, Reset(q, q̇) + Assert: residual == 0 immediately after +uses_coriolis_transpose_not_the_rnea_vector: + Arrange: fast motion where Cᵀq̇ ≠ C q̇ + Assert: residual stays near zero (a C q̇ implementation drifts — documents why M31 is required) +``` + +## Reference vectors + +- First-order response: `r(t) = τ_ext (1 − e^{−K_O (t − t0)})`. + +## Edge cases + +- `gain = 0` ⇒ residual identically zero. +- `dt` large relative to `1/K_O` ⇒ document instability; the test uses `K_O·dt = 0.05`. diff --git a/roadmap/dynamics/RneaExternalWrench/explanation.md b/roadmap/dynamics/RneaExternalWrench/explanation.md new file mode 100644 index 0000000..99b553b --- /dev/null +++ b/roadmap/dynamics/RneaExternalWrench/explanation.md @@ -0,0 +1,28 @@ +# Inverse Dynamics with an External Tool Wrench — Overview + +## What it is +Inverse dynamics that also accounts for a force and moment applied to the tool by the environment — +a payload's weight, a contact force, a tool reaction. + +## Why it matters (embedded) +Force-controlled tasks and payload handling need torques that include the contact wrench, and +collision observers need a model to compare against. Adding the wrench inside the recursion keeps +the cost linear and avoids building the Jacobian just to map one wrench. + +## How it works (intuition) +The backward pass of the recursive Newton-Euler algorithm accumulates, from the tip inward, the +wrench each joint must transmit. An external wrench on the last link simply offsets that +accumulation at the tip: the environment already supplies part of what the joints would otherwise +have to provide. + +## Key parameters +- **Wrench** — force and moment on the tool, in the base frame, with a fixed sign convention. +- **Tool offset** — where the wrench acts, in the last link's frame. + +## Reference +J. Y. S. Luh, M. W. Walker, R. P. C. Paul, "On-Line Computational Scheme for Mechanical Manipulators," +*ASME J. Dyn. Sys. Meas. Control*, 102(2), 1980; B. Siciliano et al., *Robotics* (2009), Ch. 7. + +## See also +`RecursiveNewtonEuler`, `GeometricJacobian` (M8, `Jᵀw` equivalence), `ImpedanceControl` (M17), +`HybridPositionForceControl` (M19), `MomentumObserver` (M16). diff --git a/roadmap/dynamics/RneaExternalWrench/implementation.md b/roadmap/dynamics/RneaExternalWrench/implementation.md new file mode 100644 index 0000000..3ed6ba4 --- /dev/null +++ b/roadmap/dynamics/RneaExternalWrench/implementation.md @@ -0,0 +1,63 @@ +# Inverse Dynamics with an External Tool Wrench — Implementation Pseudocode + +> Roadmap ref: #M32 (Tier 1) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) + +Extends the shipped `RecursiveNewtonEuler` so contact forces at the tool enter inverse dynamics: +`τ = M q̈ + C q̇ + g − Jᵀ w_ext`. Needed by force control (M17, M19), by payload/contact estimation, +and to cross-check the momentum observer (M16). + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct ToolWrench: + Vector3 force # f, exerted BY the environment ON the tool, base frame + Vector3 moment # n, about the tool point, base frame + Vector3 toolOffset # tool point in the last link frame (same as FK) +``` + +## Interface + +```text +# new overload next to the shipped InverseDynamics(links, q, q̇, q̈, gravity): +JointVector InverseDynamics(const LinkArray& links, const JointVector& q, const JointVector& qDot, + const JointVector& qDDot, const Vector3& gravity, + const ToolWrench& wrench) const # hot path +``` + +## Algorithm (pseudocode) + +```text +function InverseDynamics(links, q, q̇, q̈, g, w): # OPTIMIZE_FOR_SPEED + states = ComputeForwardPass(links, q, q̇, q̈, g) # unchanged + Rlast = R_0 · R_1 ··· R_{N−1} # base ← last link (product of states[i].R) + fExt = Rlastᵀ · w.force # into the last link frame + nExt = Rlastᵀ · w.moment + CrossProduct(w.toolOffset, fExt) # moment about the last joint origin + # backward pass: the environment pushes on the last link, so the joints need less wrench + force[N−1] −= fExt + torque[N−1] −= nExt + ... continue the shipped backward pass unchanged ... + return τ +``` + +## Complexity & memory + +- `O(N)`; one extra rotation product chain (already available from the forward pass) and two cross + products. No additional storage. + +## Numerical / embedded notes + +- Sign convention (normative): `w` is the wrench the environment applies to the robot. Holding a + 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). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Extend `robotics/dynamics/RecursiveNewtonEuler.hpp` (overload) and add `ToolWrench` to it; the + coverage `.cpp` already instantiates ``. +- Test: new cases in `robotics/dynamics/test/TestRecursiveNewtonEuler.cpp` +- Doc: extend `doc/dynamics/RecursiveNewtonEuler.md` (external forces section). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/RneaExternalWrench/tests.md b/roadmap/dynamics/RneaExternalWrench/tests.md new file mode 100644 index 0000000..2abf8cb --- /dev/null +++ b/roadmap/dynamics/RneaExternalWrench/tests.md @@ -0,0 +1,36 @@ +# Inverse Dynamics with an External Tool Wrench — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestRecursiveNewtonEuler : public ::testing::Test: # extends the shipped fixture + RecursiveNewtonEuler rnea3 + std::array, 3> chain{ ... } # skewed axes, tool offset (0.3, 0.05, 0) +# each case below is a TEST_F(TestRecursiveNewtonEuler, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +zero_wrench_matches_shipped_overload: + Assert: InverseDynamics(..., ToolWrench{0, 0, tool}) == InverseDynamics(...) +wrench_enters_as_minus_jacobian_transpose: + Arrange: w = ((1, −2, 0.5); (0.1, 0, −0.2)), J from GeometricJacobian (M8) at the same tool point + Assert: τ(w) − τ(0) ≈ −Jᵀ·w +horizontal_link_holding_a_payload: + Arrange: single y-axis rod length l, payload force (0, 0, −m_p·g) at the tip, static + Assert: τ = −(m l/2 + m_p l)·g (both moments add) +pure_moment_projects_on_the_axis: + Arrange: single z-axis link, w.moment = (0, 0, 2) + Assert: τ = −2 +``` + +## Reference vectors + +- Rod `m = 1, l = 1`, payload `0.5 kg`: `τ = −(0.5 + 0.5)·9.81 = −9.81 N·m`. + +## Edge cases + +- Tool offset zero ⇒ the force acts at the last joint origin (no moment arm from the offset). diff --git a/roadmap/kinematics/AnalyticalIkOpw/explanation.md b/roadmap/kinematics/AnalyticalIkOpw/explanation.md new file mode 100644 index 0000000..88d3cc4 --- /dev/null +++ b/roadmap/kinematics/AnalyticalIkOpw/explanation.md @@ -0,0 +1,41 @@ +# Analytical IK for ortho-parallel 6R arms with a spherical wrist (OPW) — Overview + +## What it is +A **closed-form** inverse-kinematics solver for the six-revolute layout used by most industrial arms: +the second and third axes are parallel to each other and perpendicular to the first, and the last +three axes meet at one point (a spherical wrist). Seven lengths — shoulder offset, lateral offset, +elbow offset, and the base, upper-arm, forearm and wrist lengths — plus a zero offset and a direction +sign per joint describe the arm. It returns every posture — up to eight — that reaches a flange pose. + +## Why it matters (embedded) +Closed-form IK is exact, has no convergence loop and runs in constant time — ideal for a hard real-time +controller that cannot afford an iterative solver's worst case. Getting *all* solutions lets a planner +pick the posture that respects joint limits or stays close to the current one. The seven-parameter +description is read straight off a datasheet drawing, avoiding DH frame-placement mistakes. + +## How it works (intuition) +Pieper's result is the theoretical basis: when the last three axes intersect, position and +orientation decouple. The wrist centre sits a fixed distance behind the flange along the approach axis, +and it depends only on the first three joints. The base joint turns the arm plane toward the wrist +centre — facing it or facing away, correcting for the lateral offset — giving two shoulder branches. +Inside that plane the shoulder offset shifts the triangle's corner, and the forearm offset turns the +forearm into a slightly longer virtual link at a fixed angle; the law of cosines on that triangle gives +elbow-up and elbow-down. With the arm placed, the remaining rotation is exactly a Z-Y-Z rotation of the +wrist, read off with `atan2` in two mirror-image flavours. When the wrist's middle joint is straight, +the first and last wrist axes line up and only their sum is determined, so one is fixed by convention. + +## Key parameters +- **a1, a2, b, c1, c2, c3, c4** — shoulder, elbow and lateral offsets; base, upper-arm, forearm and wrist lengths. +- **joint offsets and signs** — map the manufacturer's joint zero and direction onto the model. +- **branch selection** — shoulder front/back, elbow up/down, wrist flip. + +## Reference +M. Brandstötter, A. Angerer, M. Hofbaur, "An Analytical Solution of the Inverse Kinematics Problem of +Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist," *Proc. Austrian +Robotics Workshop*, 2014; D. L. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD +thesis, Stanford University, 1968. + +## See also +`SE3Transform` (M6, 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 new file mode 100644 index 0000000..dc4d931 --- /dev/null +++ b/roadmap/kinematics/AnalyticalIkOpw/implementation.md @@ -0,0 +1,125 @@ +# Analytical IK — OPW (Ortho-Parallel 6R with Spherical Wrist) — Implementation Pseudocode + +> Roadmap ref: #M21 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) + +Closed-form IK of Brandstötter, Angerer & Hofbaur (2014) for the industrial 6R layout (KUKA, ABB, +Fanuc, Stäubli, …): axes 2 and 3 parallel and orthogonal to axis 1, spherical wrist. Seven lengths plus +per-joint offsets and signs describe the arm; no DH table and no frame convention to pin. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct OpwParameters: # model zero pose: upper arm and forearm point along +z + T a1 # axis 1 → axis 2, radial (x) + T a2 # forearm offset ⊥ forearm, axis 3 → wrist line (x at zero pose) + T b # lateral offset of the arm plane (y) + T c1 # base → axis 2, along axis 1 (z) + T c2 # upper arm, axis 2 → axis 3 + T c3 # forearm, axis 3 → wrist centre + T c4 # wrist centre → flange, along axis 6 + std::array offsets # θ = sign·q − offset (model angle from joint value) + std::array signs # ±1 + +template +struct OpwSolution: + math::Vector q # joint values, wrapped to (−π, π] + bool wristSingular # sin θ5 ≈ 0: only θ4 ± θ6 is determined + +template +using OpwSolutions = std::array>, 8> + # slot j (0..3): arm branch j with wrist θ5 ≥ 0; slot j + 4: same arm, flipped wrist + # arm j: 0/1 shoulder front (elbow a/b), 2/3 shoulder back (elbow a/b) + +template +class AnalyticalIkOpw: + OpwParameters parameters +``` + +Forward model (defines the parameters; `Forward` implements it): +`T(q) = Rz(θ1)·Trans(a1, b, c1)·Ry(θ2)·Trans(0, 0, c2)·Ry(θ3)·Trans(a2, 0, c3)·Rz(θ4)·Ry(θ5)·Rz(θ6)·Trans(0, 0, c4)`. + +## Interface + +```text +explicit AnalyticalIkOpw(const OpwParameters& parameters) +OpwSolutions Solve(const SE3Transform& flange) const # all real branches; hot path +SE3Transform Forward(const JointVector& q) const # closed-form FK (round trips, tests) +``` + +## Algorithm (pseudocode) + +```cpp +function Forward(q): + θ = signs ⊙ q − offsets; k = √(a2² + c3²); ψ3 = atan2(a2, c3) + x = c2·sin θ2 + k·sin(θ2 + θ3 + ψ3) + a1; z = c2·cos θ2 + k·cos(θ2 + θ3 + ψ3) + C = (x·cos θ1 − b·sin θ1, x·sin θ1 + b·cos θ1, z + c1) # wrist centre + R = Rz(θ1)·Ry(θ2 + θ3)·Rz(θ4)·Ry(θ5)·Rz(θ6) + return { R, C + c4·R·ẑ } + +function Solve(flange): # OPTIMIZE_FOR_SPEED — closed form, no iteration + (R, p) = flange; C = p − c4·R·ẑ + ρ² = Cx² + Cy² − b²; if ρ² < 0: return all empty + n = √ρ² − a1; z = Cz − c1; k² = a2² + c3²; ψ3 = atan2(a2, c3) + θ1ᶠ = atan2(Cy, Cx) − atan2(b, n + a1) # shoulder front + θ1ᵇ = atan2(Cy, Cx) + atan2(b, n + a1) − π # shoulder back + s1² = n² + z²; s2² = (n + 2·a1)² + z² + β1 = acosV((s1² + c2² − k²) / (2·s1·c2)); γ1 = acosV((s1² − c2² − k²) / (2·c2·k)) + β2 = acosV((s2² + c2² − k²) / (2·s2·c2)); γ2 = acosV((s2² − c2² − k²) / (2·c2·k)) + arm[0] = (θ1ᶠ, atan2(n, z) − β1, γ1 − ψ3) + arm[1] = (θ1ᶠ, atan2(n, z) + β1, −γ1 − ψ3) + arm[2] = (θ1ᵇ, −atan2(n + 2·a1, z) − β2, γ2 − ψ3) + arm[3] = (θ1ᵇ, −atan2(n + 2·a1, z) + β2, −γ2 − ψ3) + for j in 0..3 with arm[j] valid (its β, γ defined): + (θ1, θ2, θ3) = arm[j] + W = Transpose(Rz(θ1)·Ry(θ2 + θ3)) · R # = Rz(θ4)·Ry(θ5)·Rz(θ6) + s5 = √max(0, 1 − W₂₂²) + if s5 > ε: # regular wrist: two branches + w = (atan2(W₁₂, W₀₂), atan2(s5, W₂₂), atan2(W₂₁, −W₂₀)) + slot[j] = Emit(arm[j], w, false) + slot[j + 4] = Emit(arm[j], (w0 + π, −w1, w2 − π), false) + else: # axes 4 and 6 aligned + θ4 = 0 # tie-break; caller may redistribute + w = W₂₂ > 0 ? (θ4, 0, atan2(W₁₀, W₁₁) − θ4) # θ4 + θ6 fixed + : (θ4, π, θ4 − atan2(−W₁₀, W₁₁)) # θ4 − θ6 fixed + slot[j] = Emit(arm[j], w, true); slot[j + 4] = empty + 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 (−π, π] +``` + +## Complexity & memory + +- `O(1)`: a fixed set of `sqrt`/`acos`/`atan2` for the arm (`k`, `ψ3` precomputed at construction), + then 3 `atan2` and one 3×3 product per arm branch. +- Memory: 8 optional solutions on the stack; no iteration, no Jacobian, no linear solve. + +## Numerical / embedded notes + +- **Why not a planar two-link elbow:** `a1`, `b` and the forearm offset `a2` break the planar + law-of-cosines triangle used for offset-free arms; OPW folds them into `n`, `n + 2a1` and the virtual + forearm length `k = √(a2² + c3²)` with angle `ψ3`, which is why it covers KUKA/ABB/Fanuc arms. +- **Reachability:** `|acos argument| > 1` prunes that arm branch; `ρ² < 0` (wrist centre inside the + `b`-cylinder) prunes all. Generic poses yield 8 or 4 solutions (shoulder-back branch often out of reach). +- **Boundary precision:** at full stretch `acos` of ≈ 1 amplifies rounding (`δθ ≈ √(2ε)`, ≈ 5e-4 rad in + `float`); tolerate it in tests, never feed `acos` an unclamped argument. +- **Wrist singularity** (`sin θ5 ≈ 0`): the two wrist branches coincide; one solution is emitted with + `θ4 = 0` and `wristSingular = true`. **Shoulder singularity** (`Cx = Cy = 0`, `b = 0`): `atan2(0, 0) = 0`, + any `θ1` is valid — returned as `θ1 = 0`. +- Joint limits are not applied; the caller filters/chooses (e.g. nearest to the current `q`). +- The target is the **flange** pose; for a tool use `target · tool⁻¹`. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/kinematics/AnalyticalIkOpw.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `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` +- 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`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/AnalyticalIkOpw/tests.md b/roadmap/kinematics/AnalyticalIkOpw/tests.md new file mode 100644 index 0000000..f35002e --- /dev/null +++ b/roadmap/kinematics/AnalyticalIkOpw/tests.md @@ -0,0 +1,81 @@ +# Analytical IK (OPW) — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestAnalyticalIkOpw : public ::testing::Test: + # KUKA KR6 R700 sixx + OpwParameters kr6{ a1 = 0.025, a2 = −0.035, b = 0, c1 = 0.400, c2 = 0.315, c3 = 0.365, c4 = 0.080, + offsets = (0, −π/2, 0, 0, 0, 0), signs = (−1, 1, 1, −1, 1, −1) } + AnalyticalIkOpw ik{ kr6 } + JointVector qTrue{ 0.3, −1.0, 1.2, 0.5, 0.6, 0.7 } +# each case below is a TEST_F(TestAnalyticalIkOpw, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +forward_matches_elementary_chain: + Assert: ik.Forward(q) ≈ Rz(θ1)·Trans(a1,b,c1)·Ry(θ2)·Trans(0,0,c2)·Ry(θ3)·Trans(a2,0,c3) + ·Rz(θ4)·Ry(θ5)·Rz(θ6)·Trans(0,0,c4) (θ = signs⊙q − offsets), for qTrue and q = 0 +home_flange_pose: + Assert: ik.Forward(0) ≈ { [[0,0,1],[0,1,0],[−1,0,0]], (0.785, 0, 0.435) } +every_solution_reproduces_target: + Arrange: target = ik.Forward(qTrue) + Act: sols = ik.Solve(target) + Assert: all 8 slots valid; Forward(sols[k].q) ≈ target (position and R, tol 1e-4) +finds_the_known_configuration: + Assert: slot 0 ≈ qTrue (mod 2π) +wrist_flip_pairs: + Assert: slot j+4 = slot j with q4 + π, −θ5, q6 − π (in model angles), for j = 0..3 +random_poses_round_trip: + Arrange: 200 deterministic pseudo-random q ∈ [−π, π]⁶ (fixed LCG seed, std::array) + Assert: every valid slot reproduces Forward(q) (tol 1e-3); some slot ≈ q (mod 2π, tol 1e-3) +general_offsets_signs_and_lateral_offset: + Arrange: a1=0.10 a2=−0.05 b=0.07 c1=0.50 c2=0.60 c3=0.55 c4=0.10, + offsets=(0.1,−0.2,0.3,0,0.4,−0.5), signs=(1,−1,1,1,−1,1) + Assert: as random_poses_round_trip (exercises b ≠ 0 and every offset/sign path) +wrist_singularity_keeps_the_sum: + Arrange: q = (0.3, −1.0, 1.2, 0.5, 0, 0.7); target = Forward(q) + Assert: slot 0 = (0.3, −1.0, 1.2, 0, 0, 1.2), wristSingular; slot 4 empty; Forward(slot 0) ≈ target +stretched_arm_elbows_coincide: + Arrange: q = (0.3, −0.3, −ψ3 = 0.095598, 0.5, 0.6, 0.7) + Assert: slots 0 and 1 agree to 1e-3; both reproduce the target (tol 1e-3) +unreachable_target_returns_no_solution: + Arrange: flange at (1.5, 0, 0.4) + Assert: all 8 slots empty +``` + +## Reference vectors + +- `Forward(qTrue)`: `p = (0.582764, −0.202939, 0.574882)`, `R = [[−0.707382, −0.375709, 0.598710], + [−0.689753, 0.551987, −0.468563], [−0.154437, −0.744415, −0.649612]]`. +- The 8 solutions for that target (joint space, wrapped): + + | slot | q1 | q2 | q3 | q4 | q5 | q6 | + |------|---------|---------|---------|---------|---------|---------| + | 0 | 0.3 | −1.0 | 1.2 | 0.5 | 0.6 | 0.7 | + | 1 | 0.3 | 0.1977 | −1.0088 | 0.2742 | 1.5525 | 1.1184 | + | 2 | −2.8416 | 3.0760 | 0.9021 | −2.8675 | 1.5770 | 1.1253 | + | 3 | −2.8416 | −2.3360 | −0.7109 | −2.7791 | 0.8687 | 0.8834 | + | 4 | 0.3 | −1.0 | 1.2 | −2.6416 | −0.6 | −2.4416 | + | 5 | 0.3 | 0.1977 | −1.0088 | −2.8674 | −1.5525 | −2.0232 | + | 6 | −2.8416 | 3.0760 | 0.9021 | 0.2741 | −1.5770 | −2.0163 | + | 7 | −2.8416 | −2.3360 | −0.7109 | 0.3625 | −0.8687 | −2.2582 | + +- Wrist-singular target `p = (0.609771, −0.188624, 0.610958)`: 7 slots valid (slot 4 empty). +- Verified in double over 2000 random poses each for KR6 R700, ABB IRB2400 (a1=0.100, a2=−0.135, + c1=0.615, c2=0.705, c3=0.755, c4=0.085, offset₃ = −π/2), Fanuc R2000iB (0.720, −0.225, 0.600, 1.075, + 1.280, 0.235, offset₃ = −π/2) and the `b ≠ 0` set: max pose error `2.3e-12`, the true `q` always found, + 8 or 4 solutions per pose. A `float32`-emulated solver (every operation rounded) over 200 random + KR6 poses: worst pose error `4.3e-5`, worst recovery of the true `q` `2.7e-4` rad; at `qTrue` `3.7e-7`. + +## Edge cases + +- `|acos argument| > 1` ⇒ that arm branch (and its wrist flip) empty. +- Wrist centre inside the `b`-cylinder ⇒ every slot empty. +- Shoulder singularity (`Cx = Cy = 0`) ⇒ `θ1 = 0` returned, solutions still valid. +- `θ5 = ±π` ⇒ singular branch with `θ4 − θ6` fixed. +- Angle wrap ⇒ compare modulo `2π`. diff --git a/roadmap/kinematics/AnalyticalIkPieper/explanation.md b/roadmap/kinematics/AnalyticalIkPieper/explanation.md deleted file mode 100644 index 93666b8..0000000 --- a/roadmap/kinematics/AnalyticalIkPieper/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Analytical IK (Pieper) — Overview - -## What it is -A **closed-form** inverse-kinematics solver for the classic six-revolute arm whose last three axes -meet at a point (a spherical wrist). Instead of iterating, it computes every joint angle directly with -trigonometry and returns *all* the arm postures — up to eight — that reach a given pose. - -## Why it matters (embedded) -Closed-form IK is exact, has no convergence loop, and runs in constant time — ideal for a hard -real-time controller that cannot afford an iterative solver's worst-case iteration count. Getting *all* -solutions also lets a planner choose the posture that best avoids joint limits or obstacles, something -a single-answer numerical solver cannot offer. - -## How it works (intuition) -Pieper's insight is that a spherical wrist **decouples** the problem. The wrist centre — the point -where the last three axes cross — depends only on the first three joints, so you first back it out of -the target pose and solve a 3-DOF *position* problem for joints 1-3 using plain planar geometry (an -`atan2` for the base and the law of cosines for the elbow, giving shoulder and elbow-up/down branches). -With the arm placed, the leftover rotation from frame 3 to the tool is handled by wrist joints 4-6, -extracted as a set of Euler angles (two branches for the middle wrist joint). Multiplying the branch -counts gives up to eight complete solutions. - -## Key parameters -- **DH table of the 6R arm** — with the spherical-wrist assumption baked in. -- **target pose** — the tool position and orientation to invert. -- **branch selection** — shoulder, elbow, and wrist flips the caller chooses among. - -## Reference -D. L. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD thesis, Stanford -University, 1968. - -## See also -`DenavitHartenberg` (M7) / `SE3Transform` (M6) for the frames, `Cordic` (item 23) for the trig, -`PoseInverseKinematics` (M13, the iterative fallback for non-spherical wrists). diff --git a/roadmap/kinematics/AnalyticalIkPieper/implementation.md b/roadmap/kinematics/AnalyticalIkPieper/implementation.md deleted file mode 100644 index d2ee652..0000000 --- a/roadmap/kinematics/AnalyticalIkPieper/implementation.md +++ /dev/null @@ -1,77 +0,0 @@ -# Analytical IK — Pieper (Wrist-Partitioned 6R) — Implementation Pseudocode - -> Roadmap ref: #M21 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct IkSolutions: - std::array, 8> q # up to 8 branches - std::size_t count # how many are valid / reachable - -template -class AnalyticalIkPieper: # fixed 6R with a spherical wrist - std::array, 6> dh # last three axes intersect at the wrist -``` - -## Interface - -``` -AnalyticalIkPieper(std::array, 6> dh) -IkSolutions Solve(SE3 target) # all real solutions; hot path -``` - -## Algorithm (pseudocode) - -``` -function Solve(target): # OPTIMIZE_FOR_SPEED (closed form, no iteration) - # 1. wrist centre — subtract the tool offset along the approach axis - p_wc = target.p - d6 * (target.R * ẑ) - - # 2. arm (joints 1-3) from the wrist-centre position — planar geometry - θ1 = atan2(p_wc.y, p_wc.x) # and the θ1 + π branch - (r, s) = planar coords of p_wc in the θ1 plane - c3 = (r² + s² - a2² - a3²) / (2·a2·a3) - if |c3| > 1: this branch is unreachable — skip it - θ3 = ±acos(c3) # elbow-up / elbow-down - θ2 = atan2(s, r) - atan2(a3·sinθ3, a2 + a3·cosθ3) - - # 3. wrist (joints 4-6) from the leftover orientation - R03 = rotation of joints 1-3 - R36 = Transpose(R03) * target.R - (θ4, θ5, θ6) = ZYZ-Euler(R36) # two branches: θ5 = ±acos(R36[2,2]) - - # 4. assemble the (≤ 2×2×2 = 8) combinations into IkSolutions - return solutions -``` - -## Complexity & memory - -- `O(1)` — a fixed number of `atan2` / `acos` calls; **no iteration, no Jacobian, no matrix solve**. -- Enumerates up to `8` postures (2 shoulder × 2 elbow × 2 wrist). -- Memory: the 8-solution array; entirely on the stack. - -## Numerical / embedded notes - -- **Reachability filter:** if the law-of-cosines argument `|c3| > 1` the target is out of reach on that - branch — drop it instead of feeding `acos` an out-of-domain value. -- **Wrist singularity** (`θ5 → 0`, axes 4 and 6 align): `θ4` and `θ6` are individually undefined, only - their sum is fixed — pick a convention (e.g. hold `θ4` at its current value) and flag it. -- Requires a **spherical wrist** (last three axes intersect); the partition into position (1-3) and - orientation (4-6) is what makes the closed form possible — reuse M6/M7 for the transforms. -- Enumerate *all* real solutions and let the caller choose (nearest to current `q`, joint-limit-feasible). -- 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/kinematics/AnalyticalIkPieper.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class AnalyticalIkPieper;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/AnalyticalIkPieper.cpp` → `template class AnalyticalIkPieper;` -- Test: `robotics/kinematics/test/TestAnalyticalIkPieper.cpp` -- Doc: `doc/kinematics/AnalyticalIkPieper.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestAnalyticalIkPieper.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/AnalyticalIkPieper/tests.md b/roadmap/kinematics/AnalyticalIkPieper/tests.md deleted file mode 100644 index 1caade4..0000000 --- a/roadmap/kinematics/AnalyticalIkPieper/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Analytical IK (Pieper) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPieperIk : public ::testing::Test: - std::array, 6> puma{ ... } # PUMA-like 6R, spherical wrist - AnalyticalIkPieper ik{ puma } - DenavitHartenberg fk{ puma } # for round-trips -# each case below is a TEST_F(TestPieperIk, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -every_solution_reproduces_target: - Arrange: q_true; target = fk.Forward(q_true) - Act: sols = ik.Solve(target) - Assert: for each k < count: fk.Forward(sols.q[k]) ≈ target - -finds_the_known_configuration: - Assert: some returned branch ≈ q_true (within angle wrap) - -enumerates_up_to_eight: - Assert: count ≤ 8; a generic reachable pose yields 8 - -elbow_up_and_down_present: - Assert: two solutions share θ1 but differ in θ3 sign - -unreachable_target_returns_zero: - Arrange: target far outside the workspace - Assert: count == 0 - -boundary_pose_is_single_elbow: - Arrange: target at max reach (arm straight) - Assert: elbow-up and elbow-down coincide (c3 = 1) - -wrist_singularity_flagged: - Arrange: pose with θ5 = 0 - Assert: θ4 / θ6 use the documented tie-break; their sum is correct - -no_iteration_side_effects: - Assert: Solve is pure / const; repeated calls identical -``` - -## Reference vectors - -- `target = fk.Forward(q_true)` ⇒ at least one branch matches `q_true`; all `count` branches satisfy FK. -- Fully stretched target ⇒ `c3 = 1`, single elbow solution. - -## Edge cases - -- `|c3| > 1` ⇒ branch pruned (unreachable). -- Wrist singularity `θ5 = 0` ⇒ `θ4 + θ6` fixed, split by convention. -- Shoulder singularity (`p_wc` on axis 1) ⇒ `θ1` free; documented handling. -- Angle wrap ⇒ compare solutions modulo `2π`. diff --git a/roadmap/kinematics/ChainPoseKinematics/explanation.md b/roadmap/kinematics/ChainPoseKinematics/explanation.md new file mode 100644 index 0000000..d9649a5 --- /dev/null +++ b/roadmap/kinematics/ChainPoseKinematics/explanation.md @@ -0,0 +1,29 @@ +# Chain Pose Kinematics — Overview + +## What it is +Forward kinematics that returns the full tool pose (position and orientation) of a serial arm +described link by link, together with every joint's axis and origin in the base frame. + +## Why it matters (embedded) +Position-only kinematics is enough for reaching a point, but grasping, welding or keeping a camera +level needs orientation too, and every Jacobian-based algorithm needs the joint axes in one common +frame. Producing them in the same single pass that computes the positions keeps the cost linear in +the number of joints. + +## How it works (intuition) +Walk the chain from the base: each joint's origin is the previous origin plus the previous +orientation applied to the link offset; each joint's axis is the previous orientation applied to the +axis as written in the link; then the joint's own rotation is folded into the running orientation. +After the last joint the constant tool frame is appended. The collected origins and axes form a +"frame chain" that any Jacobian routine can consume without knowing how the arm was described. + +## Key parameters +- **Link offsets and joint axes** — the chain description. +- **Tool frame** — constant pose of the tool in the last link's frame. + +## Reference +B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, *Robotics: Modelling, Planning and Control* (2009), +Ch. 2 (direct kinematics). + +## See also +`ForwardKinematics` (shipped, position-only), `SE3Transform` (M6), `GeometricJacobian` (M8). diff --git a/roadmap/kinematics/ChainPoseKinematics/implementation.md b/roadmap/kinematics/ChainPoseKinematics/implementation.md new file mode 100644 index 0000000..f148dd1 --- /dev/null +++ b/roadmap/kinematics/ChainPoseKinematics/implementation.md @@ -0,0 +1,75 @@ +# Chain Pose Kinematics (Full-Pose FK + Frame Chain) — Implementation Pseudocode + +> Roadmap ref: #M30 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) + +The shipped `ForwardKinematics` returns joint origins and the tool point only. Pose-level algorithms +(M8, M13, M17–M19) also need the tool **orientation** and every joint **axis** in the base frame. This +spec extends the same recursion over the existing link model (`RevoluteJointLink`, and +`GenericJointLink` once M1 lands) and produces a model-independent `FrameChain` — the single input +the geometric Jacobian (M8) consumes, whether the frames come from this link model, from DH (M7) or +from PoE (M15). + +## Data structures + +```cpp +enum class JointType : uint8_t { Revolute, Prismatic } # shared with M1 / M7 + +template # static_assert(std::is_floating_point_v); instantiated for float +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) + +template +class ChainPoseKinematics: + SE3Transform toolInLastLink # constant tool frame, injected +``` + +## Interface + +```text +explicit ChainPoseKinematics(const SE3Transform& toolInLastLink) +FrameChain Compute(const LinkArray& links, const JointVector& q) const # hot path +SE3Transform ToolPose(const LinkArray& links, const JointVector& q) const +``` + +## Algorithm (pseudocode) + +```text +function Compute(links, q): # OPTIMIZE_FOR_SPEED + R = I; o = links[0].parentToJoint + for i in 0..N-1: + if i > 0: o = o + R · links[i].parentToJoint + frames.jointOrigins[i] = o + frames.jointAxes[i] = R · links[i].jointAxis # axis is invariant under its own rotation + frames.jointTypes[i] = Revolute # (GenericJointLink: links[i].type) + R = R · RotationAboutAxis(links[i].jointAxis, q[i]) # prismatic (M1): R unchanged, o += axis·q + frames.tool = SE3Transform{ R, o } * toolInLastLink + return frames +``` + +## Complexity & memory + +- `Compute`: `O(N)` — one Rodrigues rotation and two 3×3 products per joint. +- Memory: `2N` vectors + one transform; stack only. + +## Numerical / embedded notes + +- Consistent with the shipped `ForwardKinematics`: `jointOrigins[i]` equals its `positions[i]`, and + `tool.p` equals its last entry when `toolInLastLink = { I, toolOffset }` — assert this in a test. +- The base mounting offset is `links[0].parentToJoint`; the tool is a kinematic parameter, never + derived from a center of mass. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/kinematics/ChainPoseKinematics.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `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. +- Test: `robotics/kinematics/test/TestChainPoseKinematics.cpp` +- Doc: `doc/kinematics/ChainPoseKinematics.md` (or extend `ForwardKinematics.md`) +- Depends on: M6 (`SE3Transform`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ChainPoseKinematics/tests.md b/roadmap/kinematics/ChainPoseKinematics/tests.md new file mode 100644 index 0000000..dcfe8b4 --- /dev/null +++ b/roadmap/kinematics/ChainPoseKinematics/tests.md @@ -0,0 +1,40 @@ +# Chain Pose Kinematics — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestChainPoseKinematics : public ::testing::Test: + # 3-link arm: z-axis base joint raised 0.2, then two y-axis joints (0.5 up, 0.4 along x) + std::array, 3> arm{ ... } + ChainPoseKinematics kinematics{ SE3Transform{ I, (0.3, 0, 0) } } +# each case below is a TEST_F(TestChainPoseKinematics, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +origins_and_tool_match_shipped_forward_kinematics: + Assert: jointOrigins[i] and tool.p equal ForwardKinematics{ (0.3,0,0) }.Compute positions +axes_are_expressed_in_the_base_frame: + Arrange: q = (π/2, 0, 0) + Assert: jointAxes[1] ≈ (−1, 0, 0) (y rotated by +90° about z) +tool_orientation_composes_joint_rotations: + Arrange: q = (π/2, π/2, 0) + Assert: tool.R ≈ Rz(π/2)·Ry(π/2) +tool_frame_rotation_is_applied_last: + Arrange: toolInLastLink = { Rx(π/2), 0 } + Assert: tool.R ≈ Rz(q1)·Ry(q2)·Ry(q3)·Rx(π/2) +base_offset_moves_every_origin: + Assert: jointOrigins[0] == links[0].parentToJoint for all q +``` + +## Reference vectors + +- `Rz(+90°)·ŷ = −x̂`; `Rz(90°)Ry(90°)·x̂ = (0, 0, −1)`. + +## Edge cases + +- Zero-length links (coincident origins) — valid, origins repeat. +- Non-unit axis — asserted at construction of the link (M1), not here. diff --git a/roadmap/kinematics/ContinuumKinematics/explanation.md b/roadmap/kinematics/ContinuumKinematics/explanation.md index d5f684e..e4c35a4 100644 --- a/roadmap/kinematics/ContinuumKinematics/explanation.md +++ b/roadmap/kinematics/ContinuumKinematics/explanation.md @@ -2,35 +2,38 @@ ## What it is Kinematics for arms that bend continuously instead of pivoting at discrete joints — tendon-driven, -pneumatic, or "soft" robots shaped like an elephant's trunk. The standard model treats each section as +pneumatic or "soft" robots shaped like an elephant's trunk. The standard model treats each section as a **circular arc of constant curvature**, described by three numbers: how sharply it bends, in which plane, and over what length. ## Why it matters (embedded) Continuum robots reach into cluttered, delicate spaces — inside the body for surgery, around obstacles -for inspection — where a rigid arm cannot go. The constant-curvature model reduces an infinite-DOF -flexible body to a handful of arc parameters per section, small and cheap enough to run the forward -kinematics on the embedded controller driving the tendons or air chambers. +for inspection — where a rigid arm cannot go. The constant-curvature model reduces a flexible body to +three parameters per section, small and cheap enough to evaluate on the embedded controller that +drives the tendons or air chambers. ## How it works (intuition) -Each section is assumed to bow into a perfect circular arc. Three parameters pin it down: the curvature -`κ` (how tight the arc is), the plane angle `φ` (which way it bends), and the arc length `s` (how far -along). From these, the section's tip pose is a closed-form point on that circle, with the frame -rotated by the total bend angle `θ = κ·s`. Chaining the section transforms — exactly like multiplying -joint transforms on a rigid arm — gives the whole robot's shape. The one delicate spot is a nearly -straight section: the formulas divide by `κ`, so as the arc flattens you must switch to a straight-line -limit to avoid dividing by zero. This "robot-independent" arc mapping is kept separate from the -"robot-specific" step that converts actual tendon pulls or chamber pressures into `(κ, φ, s)`. +Each section bows into a circular arc. The curvature `κ` sets how tight the arc is, the plane angle +`φ` which way it bends, and the arc length `s` how far along it runs; the total bend is `θ = κ·s`. The +tip lies on that circle and the tip frame is the base frame rotated by `θ` within the bending plane. +Writing the tip position as `s` times a function of `θ` avoids dividing by the curvature, so a nearly +straight section needs only a short series instead of a special case that blows up. Chaining section +transforms — like multiplying joint transforms on a rigid arm — gives the whole backbone. For one +section the inverse is closed form: the tip's direction gives the plane, the ratio of its sideways to +forward distance gives half the bend angle, and the straight-line distance to the tip — the chord +`2·sin(θ/2)/κ` — gives the curvature. The "robot-independent" arc map stays separate from the +"robot-specific" step that turns tendon pulls or chamber pressures into arc parameters. ## Key parameters -- **curvature `κ`** — inverse bend radius of the section. -- **bending-plane angle `φ`** — the direction the section curves (undefined when straight). -- **arc length `s`** — how long the section is. +- **curvature `κ`** — inverse bend radius; its sign can be folded into the plane angle. +- **bending-plane angle `φ`** — bend direction (undefined when straight). +- **arc length `s`** — section length. ## Reference R. J. Webster III, B. A. Jones, "Design and Kinematic Modeling of Constant Curvature Continuum Robots: A Review," *Int. J. Robotics Research*, 29(13), 2010. ## See also -`SE3Transform` (M6, per-section transform), `Quaternion` (item 18, orientation blending), -`PoseInverseKinematics` (M13, the iterative multi-section inverse). +`SE3Transform` (M6, 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 e1c8687..0c72917 100644 --- a/roadmap/kinematics/ContinuumKinematics/implementation.md +++ b/roadmap/kinematics/ContinuumKinematics/implementation.md @@ -4,11 +4,11 @@ ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -struct ArcParameters: # configuration space of one section - T kappa # curvature (1 / radius) - T phi # bending-plane angle +struct ArcParameters: # configuration of one section + T kappa # curvature κ (1/m); sign folds into φ: (−κ, φ) ≡ (κ, φ + π) + T phi # bending-plane angle about the section's base z T length # arc length s template @@ -18,67 +18,68 @@ class ContinuumKinematics: ## Interface -``` -ContinuumKinematics(std::array, NumSections> sections) -SE3 SectionTransform(ArcParameters arc) # one arc; hot path -SE3 Forward() # tip pose; hot path -ArcParameters InverseSection(SE3 sectionPose) # single-section geometric IK +```cpp +explicit ContinuumKinematics(const std::array, NumSections>& sections) +static SE3Transform SectionTransform(const ArcParameters& arc) # one arc; hot path +SE3Transform Forward() const # tip pose; hot path +static ArcParameters InverseSection(const Vector3& tip) # single section, closed form ``` ## Algorithm (pseudocode) -``` -function SectionTransform(arc): # OPTIMIZE_FOR_SPEED (robot-independent map) - θ = arc.kappa * arc.length # total bend angle - if arc.kappa ≈ 0: # straight section — series fallback - return SE3(I, (0, 0, arc.length)) - # arc lies in a plane rotated by φ about z; circle of radius 1/κ - p = (1/κ) * ( (1 - cosθ)·(cosφ, sinφ, 0) + sinθ·ẑ ) - R = Rotz(φ) * Roty(θ) * Rotz(-φ) # frame swept along the arc - return SE3(R, p) +```text +function SectionTransform(arc): # OPTIMIZE_FOR_SPEED, robot-independent map + θ = arc.kappa · arc.length # total bend angle + if |θ| < θ_series: # straight-ish: no 1/κ, no 0/0 + f1 = θ/2 − θ³/24; f2 = 1 − θ²/6 + else: + f1 = 2·sin²(θ/2) / θ; f2 = sin θ / θ # (1 − cos θ)/θ without cancellation + p = arc.length · ( f1·(cos φ, sin φ, 0) + f2·ẑ ) + R = Rz(φ) · Ry(θ) · Rz(−φ) # tangent R·ẑ = (cos φ sin θ, sin φ sin θ, cos θ) = dp/ds + return { R, p } function Forward(): # OPTIMIZE_FOR_SPEED - T = Identity - for i in 0..NumSections-1: - T = T * SectionTransform(sections[i]) # reuse SE(3) compose (M6) - return T - -function InverseSection(pose): # single section, closed form - # recover (κ, φ, s) from one constant-curvature arc's tip - φ = atan2(pose.p.y, pose.p.x) - θ = 2 * atan2( ‖(pose.p.x, pose.p.y)‖ , pose.p.z ) # from arc geometry - κ = θ / arcLengthFrom(pose.p, θ) - s = θ / κ + A = Identity + for i in 0..NumSections-1: A = A * SectionTransform(sections[i]) # M6 compose + return A + +function InverseSection(tip): # tip position in the section base frame, κs < 2π + ρ = ‖(tip.x, tip.y)‖; L = ‖tip‖ + if ρ < ε: return { 0, 0, tip.z } # straight: φ undefined, returned as 0 + φ = atan2(tip.y, tip.x) + θ = 2·atan2(ρ, tip.z) # tan(θ/2) = ρ / z + κ = 2ρ / L² # from L² = 2ρ/κ + s = L · (θ/2) / sin(θ/2) # = θ/κ, finite as θ → 0 return { κ, φ, s } ``` ## Complexity & memory -- `SectionTransform`: `O(1)` — a few trig calls and one `SE(3)` build. -- `Forward`: `O(NumSections)` `SE(3)` products; `InverseSection`: `O(1)` closed form. -- Memory: the section array plus one accumulator; no heap. +- `SectionTransform`: `O(1)` — two `sin`/`cos` pairs and a 3×3 build. +- `Forward`: `O(NumSections)` SE(3) products; `InverseSection`: `O(1)`. +- Memory: the section array plus one accumulator; stack only. ## Numerical / embedded notes -- **Curvature `κ → 0` is the trap:** the pose uses `1/κ`, which blows up for a straight section — switch - to the `θ → 0` series (`p → (0,0,s)`) below a threshold, and note `φ` is undefined when straight. -- Keep the **robot-independent** map `(κ, φ, s) → SE(3)` separate from the **robot-specific** map +- **Straight limit:** writing the arc as `s·f(θ)` removes the `1/κ` blow-up; below `θ_series ≈ 1e-2` + (`float`) the series is accurate to `θ⁴/120 < 1e-9`. The result is independent of `φ` when straight. +- **Chord identity:** `‖p‖ = 2|sin(θ/2)|/|κ|` — the basis of `InverseSection` and of the tests. +- Keep the robot-independent map `(κ, φ, s) → SE(3)` separate from the robot-specific actuator map (tendon lengths / chamber pressures → `(κ, φ, s)`); only the latter changes between hardware. -- Constant-curvature is a **modeling assumption** (piecewise circular arcs); real gravity/load bending - deviates from it — document it as an approximation, not exact kinematics. -- Multi-section inverse kinematics generally needs iteration (reuse damped least squares, M13-style); - only the single-section case is closed-form here. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- Constant curvature is a modelling assumption; gravity and tip loads bend real sections off-arc. +- The single-section inverse returns `κ ≥ 0`, `φ ∈ (−π, π]`; multi-section inverse needs iteration + (M13-style damped least squares on `(κᵢ, φᵢ, sᵢ)`). +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/kinematics/ContinuumKinematics.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `SectionTransform`/`Forward`, and - `extern template class ContinuumKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ContinuumKinematics.cpp` → `template class ContinuumKinematics;` + `extern template class ContinuumKinematics;` / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/ContinuumKinematics.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestContinuumKinematics.cpp` - 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`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ContinuumKinematics/tests.md b/roadmap/kinematics/ContinuumKinematics/tests.md index c946773..e3eafe5 100644 --- a/roadmap/kinematics/ContinuumKinematics/tests.md +++ b/roadmap/kinematics/ContinuumKinematics/tests.md @@ -4,57 +4,47 @@ ## Fixture -``` -class TestContinuum : public ::testing::Test: - # single unit-length section, straight by default - std::array, 1> sec = { { kappa=0, phi=0, length=1 } } - ContinuumKinematics ck{ sec } -# each case below is a TEST_F(TestContinuum, ) +```cpp +class TestContinuumKinematics : public ::testing::Test: + ArcParameters quarter{ π/2, 0, 1 } # θ = π/2, radius 2/π + ArcParameters general{ 2.0, 0.7, 1.2 } # θ = 2.4 + ContinuumKinematics twoSections{ { general, general } } +# each case below is a TEST_F(TestContinuumKinematics, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```cpp straight_section_is_pure_translation: - Act: T = ck.SectionTransform({0, 0, 1}) - Assert: T ≈ SE3(I, (0, 0, 1)) - + Assert: SectionTransform({0, 0, 1}) ≈ { I, (0, 0, 1) } quarter_circle_bend: - Arrange: κ = π/2, length = 1 ⇒ θ = π/2, radius = 2/π - Assert: tip position ≈ (2/π)·(1, 0, 1) - + Assert: SectionTransform(quarter).p ≈ (2/π)·(1, 0, 1) = (0.636620, 0, 0.636620); R ≈ Ry(π/2) bending_plane_angle_rotates_tip: - Arrange: same κ, s; φ = π/2 - Assert: tip bends in the y-z plane instead of the x-z plane - + Assert: SectionTransform({π/2, π/2, 1}).p ≈ (0, 0.636620, 0.636620) +tip_distance_equals_chord: + Assert: ‖SectionTransform(arc).p‖ ≈ 2·|sin(κs/2)|/|κ| for quarter (0.900316), general (0.932039), + and {−1.5, 0.3, 0.8} (0.752857) +tip_tangent_matches_bend: + Assert: SectionTransform(general).R·ẑ ≈ (cos 0.7·sin 2.4, sin 0.7·sin 2.4, cos 2.4) = (0.516623, 0.435145, −0.737394) forward_composes_sections: - Arrange: two identical bent sections - Assert: Forward = SectionTransform ∘ SectionTransform - + Assert: twoSections.Forward() ≈ SectionTransform(general) * SectionTransform(general); p ≈ (0.348960, 0.293925, −0.498082) inverse_recovers_arc_parameters: - Arrange: known (κ, φ, s); pose = SectionTransform - Act: arc = ck.InverseSection(pose) - Assert: arc ≈ (κ, φ, s) - -curvature_zero_series_no_blowup: - Arrange: κ = 1e-9 - Assert: SectionTransform finite ≈ straight (no 1/κ overflow) - -arc_length_preserved: - Assert: geodesic length of the section == s regardless of κ - -tip_orientation_matches_bend: - Assert: section R rotates the tangent by θ = κ·s + Assert: InverseSection(SectionTransform(a).p) ≈ a for a ∈ {quarter, general}; + {−1.5, 0.3, 0.8} ⇒ (1.5, 0.3 − π, 0.8) (sign folded into φ) +near_zero_curvature_uses_series: + Assert: SectionTransform({1e-9, 0.4, 1}) ≈ { I, (0, 0, 1) }; SectionTransform({1e-5, 0.4, 1}).p ≈ (4.605e-6, 1.947e-6, 1), finite ``` ## Reference vectors -- Straight unit section ⇒ tip `(0, 0, 1)`, `R = I`. -- `κ = π/2`, `s = 1`, `φ = 0` ⇒ `θ = π/2`, radius `2/π`, tip `= (2/π)·(1, 0, 1)`. +- Straight unit section ⇒ `(0, 0, 1)`, `R = I`. +- `κ = π/2, s = 1, φ = 0` ⇒ tip `(0.636620, 0, 0.636620)`, chord `2√2/π = 0.900316`. +- `κ = 2, φ = 0.7, s = 1.2` ⇒ tip `(0.664416, 0.559630, 0.337732)`, chord `sin 1.2 = 0.932039`. +- All closed-form tips agree with a 4000-step numerical integration of the tangent `R(s)·ẑ` to `1e-8`. ## Edge cases -- `κ → 0` ⇒ series fallback; `φ` undefined but the result is independent of `φ`. -- Full loop `θ = 2π` ⇒ tip returns near the base (closed circle). -- Negative `κ` ⇒ bends the opposite way; sign consistent with `φ`. -- Multi-section inverse ⇒ falls back to iteration (documented, not closed-form). +- Full loop `θ = 2π` ⇒ tip at the base `(0, 0, 0)`; `InverseSection` is limited to `κs < 2π`. +- Negative `κ` ⇒ same arc as `(|κ|, φ + π)`. +- Straight tip in `InverseSection` ⇒ `κ = 0`, `φ = 0`, `s = z`. +- Multi-section inverse ⇒ iterative (M13), not here. diff --git a/roadmap/kinematics/DenavitHartenberg/explanation.md b/roadmap/kinematics/DenavitHartenberg/explanation.md index d7f18fb..deb243e 100644 --- a/roadmap/kinematics/DenavitHartenberg/explanation.md +++ b/roadmap/kinematics/DenavitHartenberg/explanation.md @@ -2,30 +2,39 @@ ## What it is A minimal four-number recipe — `(a, α, d, θ)` — that describes where each link of a robot arm sits -relative to the previous one. Each link's four parameters generate one 4×4 homogeneous transform, and -multiplying them down the chain gives the forward kinematics. +relative to the previous one. Each row generates one rigid transform; multiplying them down the chain +gives the forward kinematics, and reading each joint's axis off the intermediate frames gives the input +the Jacobian needs. ## Why it matters (embedded) DH is the *lingua franca* of industrial robotics: nearly every arm's datasheet ships a DH table. Encoding a manipulator as `N×4` constants (plus a joint-type flag) is the most compact possible model, -and the per-link transform is a handful of trig calls — cheap enough for a servo loop on a microcontroller. +and the per-link transform is one sine/cosine pair plus a fixed fill — cheap enough for a servo loop on +a microcontroller. Emitting the shared frame chain lets DH-described arms reuse the library's single +Jacobian, inverse-kinematics and control code. ## How it works (intuition) -The convention forces every joint axis onto its frame's `z`-axis and every common normal onto the +The convention forces every joint axis onto a frame's `z`-axis and every common normal onto an `x`-axis. That discipline collapses the six numbers of a general rigid transform down to four: two -describe the joint (`d` slides along `z`, `θ` rotates about `z`) and two describe the link that follows -(`a` slides along `x`, `α` twists about `x`). One of `θ`/`d` is the moving joint variable; the other -three are fixed geometry. Chaining the link transforms walks the frame from the base out to the tool. +describe the joint (`d` slides along `z`, `θ` rotates about `z`) and two describe the link (`a` slides +along `x`, `α` twists about `x`). The joint variable is *added* to the constant `θ` (revolute) or `d` +(prismatic), because real tables carry offsets. The standard (distal) convention attaches frame `i` at +the far end of link `i`, so joint `i` turns about the `z`-axis of the previous frame; the modified +(proximal, Craig) convention attaches it at the near end, so joint `i` turns about its own frame's +`z`-axis and each row carries the previous link's `a` and `α`. Both describe the same arm. ## Key parameters - **a (link length), α (link twist)** — fixed geometry of the link. -- **d (link offset), θ (joint angle)** — one is the joint variable, chosen by the joint type. -- **convention** — standard (distal) vs modified (proximal) DH place the frame differently. -- **joint type** — revolute (θ varies) or prismatic (d varies). +- **d (link offset), θ (joint angle)** — constant offsets; the joint variable adds to one of them. +- **convention** — standard (distal) or modified (proximal); rows differ between the two. +- **tool transform** — constant pose of the tool in the last DH frame. ## Reference -J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 (the DH convention). +J. Denavit, R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," +*ASME J. Applied Mechanics*, 1955; J. J. Craig, *Introduction to Robotics: Mechanics and Control*, +4th ed., Ch. 3 (modified convention). ## See also -`SE3Transform` (M6, the per-link transform), `SpatialJacobian` (M8, differentiates this chain), -`ProductOfExponentials` (M15, the screw-theory alternative that skips DH bookkeeping). +`SE3Transform` (M6, 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 7fd1bb9..edc02a0 100644 --- a/roadmap/kinematics/DenavitHartenberg/implementation.md +++ b/roadmap/kinematics/DenavitHartenberg/implementation.md @@ -2,87 +2,117 @@ > Roadmap ref: #M7 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) +Produces the tool pose and the M30 `FrameChain` from a DH table, so the geometric Jacobian (M8) and +everything built on `JacobianProvider` work on DH-described arms unchanged. + ## Data structures -``` -enum JointType: Revolute, Prismatic +```cpp +enum class JointType : uint8_t { Revolute, Prismatic } # shared with M30 / M1 +enum class DhConvention : uint8_t { Standard, Modified } # distal (classic) vs proximal (Craig) template # static_assert(std::is_floating_point_v); instantiated for float -struct DhLink: - T a # link length (along xᵢ) - T alpha # link twist (about xᵢ) - T d # link offset (along zᵢ₋₁) - T theta # joint angle (about zᵢ₋₁) - JointType type # which of {theta, d} is the variable - -template +struct DhLink: # Standard row i: (aᵢ, αᵢ, dᵢ, θᵢ); Modified row i: (aᵢ₋₁, αᵢ₋₁, dᵢ, θᵢ) + T a # link length along x + T alpha # link twist about x + T d # constant offset along z (added to q when Prismatic) + T theta # constant offset about z (added to q when Revolute) + JointType type + +template class DenavitHartenberg: - std::array, NumLinks> links # constant frame description - bool modified = false # standard (distal) vs modified (proximal) DH + std::array, N> links + DhConvention convention + SE3Transform tool # constant tool pose in the last DH frame (M6) + std::array cosAlpha, sinAlpha # precomputed at construction + +template # TaskDim ∈ {3, 6} +class DhTaskJacobian : public JacobianProvider: # M8 seam, mirrors ChainTaskJacobian + const DenavitHartenberg& model ``` ## Interface -``` -DenavitHartenberg(std::array, NumLinks> links, bool modified = false) -SE3 LinkTransform(DhLink link, T q) # one Aᵢ; hot path -SE3 Forward(JointVector q) # ⁰Tₙ; hot path -std::array, N + 1> FrameChain(JointVector q) # every ⁰Tᵢ (for Jacobian) +```cpp +DenavitHartenberg(const std::array, N>& links, DhConvention convention, + const SE3Transform& tool = SE3Transform::Identity()) +SE3Transform LinkTransform(std::size_t i, T q) const # one Aᵢ; hot path +SE3Transform Forward(const JointVector& q) const # tool pose ⁰Tₙ·tool; hot path +FrameChain Frames(const JointVector& q) const # M30 frame chain (for M8) + +# DhTaskJacobian: Jacobian(q) = rows of GeometricJacobian::Compute(model.Frames(q)) (TaskDim = 3 → linear rows), +# BiasAcceleration(q, q̇) = rows of GeometricJacobian::BiasAcceleration(model.Frames(q), q̇), +# ToolPose(q) = model.Forward(q) ``` ## Algorithm (pseudocode) -``` -function LinkTransform(link, q): # OPTIMIZE_FOR_SPEED - theta = link.type == Revolute ? q : link.theta - d = link.type == Prismatic ? q : link.d - (ct, st) = (cos theta, sin theta) - (ca, sa) = (cos link.alpha, sin link.alpha) # constant per link — precompute - # standard (distal) DH: Rotz(θ)·Transz(d)·Transx(a)·Rotx(α) - R = [[ ct, -st·ca, st·sa ], - [ st, ct·ca, -ct·sa ], - [ 0, sa, ca ]] - p = (link.a·ct, link.a·st, d) - return SE3(R, p) +```text +function LinkTransform(i, q): # OPTIMIZE_FOR_SPEED + link = links[i] + θ = link.theta + (link.type == Revolute ? q : 0) + d = link.d + (link.type == Prismatic ? q : 0) + (cθ, sθ) = (cos θ, sin θ); (cα, sα) = (cosAlpha[i], sinAlpha[i]) + if convention == Standard: # Rotz(θ)·Transz(d)·Transx(a)·Rotx(α) + R = [[ cθ, −sθ·cα, sθ·sα ], + [ sθ, cθ·cα, −cθ·sα ], + [ 0, sα, cα ]] + p = (a·cθ, a·sθ, d) + else: # Modified: Rotx(αᵢ₋₁)·Transx(aᵢ₋₁)·Rotz(θ)·Transz(d) + R = [[ cθ, −sθ, 0 ], + [ sθ·cα, cθ·cα, −sα ], + [ sθ·sα, cθ·sα, cα ]] + p = (a, −sα·d, cα·d) + return { R, p } function Forward(q): # OPTIMIZE_FOR_SPEED - T = Identity - for i in 0..N-1: - T = T * LinkTransform(links[i], q[i]) # reuse SE3 compose (M6) - return T + A = Identity + for i in 0..N-1: A = A * LinkTransform(i, q[i]) + return A * tool -function FrameChain(q): - frames[0] = Identity +function Frames(q): + A = Identity for i in 0..N-1: - frames[i+1] = frames[i] * LinkTransform(links[i], q[i]) + if convention == Standard: # joint i acts about z of frame i−1 (the frame before its link) + frames.jointOrigins[i] = A.p; frames.jointAxes[i] = A.R · ẑ + A = A * LinkTransform(i, q[i]) + else: # Modified: joint i acts about z of frame i + A = A * LinkTransform(i, q[i]) + frames.jointOrigins[i] = A.p; frames.jointAxes[i] = A.R · ẑ + frames.jointTypes[i] = links[i].type + frames.tool = A * tool return frames ``` ## Complexity & memory -- `LinkTransform`: `O(1)` — four trig calls and a fixed 3×3 fill, no loop. -- `Forward` / `FrameChain`: `O(N)` SE(3) products; `FrameChain` stores `N+1` transforms. -- Memory: the constant `NumLinks` link table plus one `SE(3)` accumulator; all on the stack. +- `LinkTransform`: `O(1)` — one `sin`/`cos` pair and a fixed 3×3 fill (`α` terms precomputed). +- `Forward` / `Frames`: `O(N)` SE(3) products; `Frames` stores `2N` vectors + one transform. +- Memory: the constant link table plus `2N` precomputed trig values; stack only. ## Numerical / embedded notes -- Precompute `sin/cos` of each `alpha` once (constant per link) — only `theta`/`d` vary at runtime. -- Support both **standard** and **modified** DH: the two conventions place the frame differently, so - the factor order changes; expose the flag rather than hard-coding one. -- `JointType` selects whether `theta` or `d` is the variable — one struct serves revolute and prismatic - chains (unblocks mixed SCARA/gantry arms), unlike the revolute-only `RevoluteJointLink`. -- Keep each `R` on `SO(3)`: the factored form is exactly orthonormal, so no reorthonormalization needed. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Offsets are kept:** real DH tables carry constant `θ` offsets on revolute joints and constant `d` + on prismatic ones; the joint variable is *added*, never substituted. +- **Conventions differ in the row layout, not just the factor order:** a modified row holds + `(aᵢ₋₁, αᵢ₋₁)` of the previous link, and the last standard `(aₙ, αₙ)` moves into `tool`. The same + arm described both ways yields identical `Forward` and `Frames`. +- The origin written into `FrameChain` is any point on the joint axis; for revolute columns + `z × (p − o)` is unchanged by sliding `o` along `z`, so the modified-frame origin (which includes `dᵢ`) + is valid. +- Both factored forms are exactly orthonormal — no re-orthonormalization needed. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment -- Header: `robotics/kinematics/DenavitHartenberg.hpp` — `#pragma once` → +- Header: `robotics/kinematics/DenavitHartenberg.hpp` (+ `DhTaskJacobian.hpp`) — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `LinkTransform`/`Forward`, and - `extern template class DenavitHartenberg;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/DenavitHartenberg.cpp` → `template class DenavitHartenberg;` + `extern template class DenavitHartenberg;` / `` / `` plus + `extern template class DhTaskJacobian;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/DenavitHartenberg.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestDenavitHartenberg.cpp` - 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`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/DenavitHartenberg/tests.md b/roadmap/kinematics/DenavitHartenberg/tests.md index e2e00f1..1d86e95 100644 --- a/roadmap/kinematics/DenavitHartenberg/tests.md +++ b/roadmap/kinematics/DenavitHartenberg/tests.md @@ -4,58 +4,61 @@ ## Fixture -``` +```cpp class TestDenavitHartenberg : public ::testing::Test: - # 2-link planar arm: a₁ = a₂ = 1, α = d = 0, both revolute - std::array, 2> planar = { {1,0,0,0,Revolute}, {1,0,0,0,Revolute} } - DenavitHartenberg dh{ planar } + # 2-link unit planar arm, both revolute (a, α, d, θ, type) + DenavitHartenberg standard{ { {1,0,0,0,Revolute}, {1,0,0,0,Revolute} }, Standard } + DenavitHartenberg modified{ { {0,0,0,0,Revolute}, {1,0,0,0,Revolute} }, Modified, + SE3Transform{ I, (1, 0, 0) } } # a₂ moves into the tool # each case below is a TEST_F(TestDenavitHartenberg, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```text zero_config_is_stretched_along_x: - Act: T = dh.Forward({0, 0}) - Assert: T.p ≈ (2, 0, 0), T.R ≈ Identity - -single_link_revolute_rotates: - Arrange: 1-link arm a = 1 - Act: T = dh.Forward({π/2}) - Assert: T.p ≈ (0, 1, 0) - + Assert: standard.Forward({0, 0}).p ≈ (2, 0, 0), R ≈ I planar_elbow_ninety_deg: - Act: T = dh.Forward({0, π/2}) - Assert: T.p ≈ (1, 1, 0) - -prismatic_link_translates_along_z: - Arrange: single Prismatic link, a = α = θ = 0 - Act: T = dh.Forward({0.5}) - Assert: T.p ≈ (0, 0, 0.5) - -link_twist_alpha_reorients_frame: - Arrange: link a = 0, α = π/2 - Assert: LinkTransform.R maps ẑ → -ŷ (axis reorientation) - -frame_chain_last_matches_forward: - Assert: FrameChain(q).back() ≈ Forward(q) - -link_transform_is_orthonormal: - Assert: RᵀR ≈ Identity for arbitrary q - -modified_vs_standard_differ: - Assert: modified-DH Forward ≠ standard Forward for the same table (documented) + Assert: standard.Forward({0, π/2}).p ≈ (1, 1, 0) +single_link_revolute_rotates: + Arrange: 1-link standard arm {1,0,0,0,Revolute} + Assert: Forward({π/2}).p ≈ (0, 1, 0) +theta_offset_is_added_to_the_joint_variable: + Arrange: standard table with link 1 θ = π/2 + Assert: Forward({0, 0}).p ≈ (0, 2, 0); Forward({0, π/2}).p ≈ (−1, 1, 0) +prismatic_variable_adds_to_d: + Arrange: 1-link {0,0,0.2,0,Prismatic} + Assert: Forward({0.5}).p ≈ (0, 0, 0.7) +link_twist_maps_z_to_minus_y: + Arrange: link {0, π/2, 0, 0, Revolute}, both conventions + Assert: LinkTransform(0, 0).R · ẑ ≈ (0, −1, 0) +modified_link_transform_matches_elementary_product: + Arrange: link {0.3, 0.7, 0.2, 0, Revolute}, q = 1.1 + Assert: LinkTransform ≈ Rotx(0.7)·Transx(0.3)·Rotz(1.1)·Transz(0.2) + ⇒ p ≈ (0.3, −0.128844, 0.152968) +standard_and_modified_give_the_same_pose: + Assert: standard.Forward(q) ≈ modified.Forward(q) for q ∈ {(0,0), (0,π/2), (0.4,−0.7), (1.3,2.1)} +standard_and_modified_give_the_same_frame_chain: + Assert: Frames(q).jointOrigins, jointAxes, tool agree; origins = {(0,0,0), (cos q₁, sin q₁, 0)}, axes = ẑ +frame_chain_jacobian_matches_finite_difference: + Arrange: PUMA-560 standard table (below), q = (0.3,−0.4,0.5,0.6,−0.7,0.8), q̇ = (0.3,−0.2,0.5,0.1,−0.4,0.7) + Assert: GeometricJacobian::Compute(Frames(q))·q̇ ≈ (Δp; Log(ΔR)) / ε (tol 1e-3, ε = 1e-3) ``` ## Reference vectors -- 2-link unit planar arm, `q = (0, π/2)` ⇒ tip `(1, 1, 0)`. -- Single revolute `a = 1`, `q = π/2` ⇒ tip `(0, 1, 0)`. -- Prismatic `d = 0.5` ⇒ tip `(0, 0, 0.5)`. +- 2-link unit arm: `q = (0, π/2)` ⇒ tip `(1, 1, 0)`; `q = (0.4, −0.7)` ⇒ `(1.876397, 0.093898, 0)`; + `q = (1.3, 2.1)` ⇒ `(−0.699299, 0.708017, 0)` — identical for both conventions. +- `θ₁` offset `π/2`: `q = (0,0)` ⇒ `(0, 2, 0)`; `q = (0, π/2)` ⇒ `(−1, 1, 0)`. +- `α = π/2` ⇒ `R·ẑ = −ŷ` (both conventions). +- PUMA-560 standard `(a, α, d)`: `(0, π/2, 0)`, `(0.4318, 0, 0)`, `(0.0203, −π/2, 0.15005)`, + `(0, π/2, 0.4318)`, `(0, −π/2, 0)`, `(0, 0, 0)`. At the test `q`: tool `p ≈ (0.402407, −0.032586, + 0.263519)`, `J·q̇ ≈ (−0.146069, 0.072514, −0.086416; −0.005657, 0.296324, 0.946824)`; forward-difference + truncation error at `ε = 1e-3` is `≈ 1e-4`. ## Edge cases -- `alpha = ±π/2` ⇒ frame axis swaps; verify no sign error in `sa`. -- Zero-length links (`a = d = 0`) ⇒ pure rotation joints. -- Mixed revolute/prismatic chain ⇒ correct variable substitution per link. -- Full-turn `theta = 2π` ⇒ same pose as `theta = 0` (wrap-agnostic). +- `α = ±π/2` ⇒ frame axis swaps; sign of `sα` checked by `link_twist_maps_z_to_minus_y`. +- Zero-length links (`a = d = 0`) ⇒ coincident origins. +- Mixed revolute/prismatic chain ⇒ `Frames` reports the per-joint type. +- `θ = 2π` ⇒ same pose as `θ = 0`. diff --git a/roadmap/kinematics/GeometricJacobian/explanation.md b/roadmap/kinematics/GeometricJacobian/explanation.md new file mode 100644 index 0000000..fdbfc19 --- /dev/null +++ b/roadmap/kinematics/GeometricJacobian/explanation.md @@ -0,0 +1,32 @@ +# Geometric Jacobian — Overview + +## What it is +The matrix that maps joint rates to the tool's velocity: for an `N`-joint arm it is `6×N`, the top +three rows giving the linear velocity of the tool point and the bottom three its angular velocity, +both expressed in the base frame. Its transpose maps a force and moment applied at the tool to joint +torques. The companion term `J̇q̇` is the tool acceleration produced by joint velocities alone. + +## Why it matters (embedded) +It is the workhorse of manipulator control: velocity control, force control, singularity detection, +pose inverse kinematics, impedance and operational-space control all need it, and task-space +controllers additionally need `J̇q̇`. Building it from one generic "frame chain" means a single, +well-tested routine serves every kinematic description in the library. + +## How it works (intuition) +Each joint contributes one column. A revolute joint spins everything outboard about its axis `z`, so +the tool gains angular velocity `z` and linear velocity `z × r`, where `r` runs from the joint to the +tool. A prismatic joint slides the tool along `z`. `J̇q̇` follows from differentiating those columns +while the axes and lever arms are carried along by the moving links. + +## Key parameters +- **Frame chain** — joint origins, axes and types plus the tool pose, in the base frame. +- **Ordering** — `(v; ω)`, linear first, as everywhere in the library. + +## Reference +B. Siciliano et al., *Robotics: Modelling, Planning and Control* (2009), Ch. 3 (geometric Jacobian); +K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 5 (for the space/body Jacobian variants). + +## See also +`ChainPoseKinematics` (M30), `DenavitHartenberg` (M7) and `ProductOfExponentials` (M15) produce the +frame chain; `ManipulabilityIndex` (M11) scores the Jacobian; `PoseInverseKinematics` (M13), +`RedundancyResolution` (M14) and the task-space controllers (M17–M19) use it. diff --git a/roadmap/kinematics/GeometricJacobian/implementation.md b/roadmap/kinematics/GeometricJacobian/implementation.md new file mode 100644 index 0000000..1f840fa --- /dev/null +++ b/roadmap/kinematics/GeometricJacobian/implementation.md @@ -0,0 +1,106 @@ +# Geometric Jacobian (6×N) and Jacobian Provider — Implementation Pseudocode + +> 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 +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 +single Jacobian implementation — replacing the private 3×N Jacobian inside the shipped IK. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class GeometricJacobian: # stateless + using Jacobian = math::Matrix + +template # DI seam used by all task-space controllers +class JacobianProvider: + virtual ~JacobianProvider() = default + virtual math::Matrix Jacobian(const JointVector& q) const = 0 + virtual math::Vector BiasAcceleration(const JointVector& q, const JointVector& qDot) const = 0 # J̇·q̇ + virtual SE3Transform ToolPose(const JointVector& q) const = 0 + +template # TaskDim ∈ {3, 6} +class ChainTaskJacobian : public JacobianProvider: + const LinkArray& links # link model (shipped RevoluteJointLink chain) + ChainPoseKinematics kinematics # M30 +``` + +## Interface + +```text +static Jacobian Compute(const FrameChain& frames) # hot path +static Vector6 BiasAcceleration(const FrameChain& frames, const JointVector& qDot) # J̇·q̇ +static JointVector JointTorques(const Jacobian& J, const Vector6& wrench) # Jᵀ·w + +# ChainTaskJacobian: TaskDim = 6 → all rows; TaskDim = 3 → the linear rows only (position tasks) +``` + +## Algorithm (pseudocode) + +```cpp +function Compute(frames): # OPTIMIZE_FOR_SPEED + p = frames.tool.p + for i in 0..N-1: + z = frames.jointAxes[i]; o = frames.jointOrigins[i] + if frames.jointTypes[i] == Revolute: column i = ( CrossProduct(z, p − o) ; z ) + else: column i = ( z ; 0 ) + return J + +function BiasAcceleration(frames, q̇): # O(N) recursion, q̈ = 0 + ω = 0; vOrigin = 0; oPrev = frames.jointOrigins[0] + ṗ = Compute(frames).linearRows · q̇ # tool-point velocity (or recurse, see note) + acc = 0 (6-vector) + for i in 0..N-1: + o = frames.jointOrigins[i]; z = frames.jointAxes[i] + vOrigin = vOrigin + CrossProduct(ω, o − oPrev) # velocity of joint origin i + ż = CrossProduct(ω, z) # axis carried by link i−1 + if Revolute: + acc.linear += q̇[i] · ( CrossProduct(ż, p − o) + CrossProduct(z, ṗ − vOrigin) ) + acc.angular += q̇[i] · ż + ω = ω + z · q̇[i] + else: # Prismatic + acc.linear += q̇[i] · ż + vOrigin = vOrigin + z · q̇[i] + oPrev = o + return acc + +function JointTorques(J, w): # statics: τ = Jᵀ·w, w = (f; n) at the tool point + return Transpose(J) · w +``` + +## Complexity & memory + +- `Compute`: `O(N)` — one cross product per column. +- `BiasAcceleration`: `O(N)` after the `O(6N)` tool-velocity product. +- `JointTorques`: `O(6N)`. +- Memory: one `6×N` matrix; stack only. + +## Numerical / embedded notes + +- **Ordering is normative:** rows `0–2` are linear velocity of the tool point, rows `3–5` angular + velocity, both in the base frame; wrenches passed to `JointTorques` are `(f; n)` about the tool point. +- DH frames (M7) put joint `i`'s axis on `z` of frame `i−1` (standard) or frame `i` (modified); the DH + spec is responsible for filling `FrameChain` correctly — this routine never looks at DH parameters. +- `BiasAcceleration` (`J̇q̇`) is required by operational-space control (M18) and by pose tracking; + check it against a finite difference `(J(q + εq̇) − J(q))/ε · q̇`. +- Near a **singularity** columns become dependent; detect with M11 and never invert `J` directly. +- The provider interface is a virtual seam for testability (StrictMock). Hard real-time users can + template the controllers on the concrete `ChainTaskJacobian` to avoid the virtual call. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/kinematics/GeometricJacobian.hpp` (+ `JacobianProvider.hpp`, `ChainTaskJacobian.hpp`) — + `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `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. +- Test: `robotics/kinematics/test/TestGeometricJacobian.cpp` +- Doc: `doc/kinematics/GeometricJacobian.md` (per `doc/TEMPLATE.md`) +- Depends on: M6 (`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/GeometricJacobian/tests.md b/roadmap/kinematics/GeometricJacobian/tests.md new file mode 100644 index 0000000..222be93 --- /dev/null +++ b/roadmap/kinematics/GeometricJacobian/tests.md @@ -0,0 +1,49 @@ +# Geometric Jacobian — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestGeometricJacobian : public ::testing::Test: + # 2-link unit planar arm, z-axis joints, joint 2 at (1,0,0), tool at (1,0,0) of link 2 + std::array, 2> planar{ ... } + ChainPoseKinematics kinematics{ SE3Transform{ I, (1, 0, 0) } } +# each case below is a TEST_F(TestGeometricJacobian, ) +``` + +## Test cases (Arrange / Act / Assert) + +```text +stretched_planar_arm_columns: + Act: J = GeometricJacobian::Compute(kinematics.Compute(planar, (0, 0))) + Assert: linear rows [[0,0],[2,1],[0,0]], angular rows [[0,0],[0,0],[1,1]] +linear_rows_match_finite_difference_of_tool_position: + Arrange: q = (0.4, −0.7), q̇ = (0.3, −0.5) + Assert: J_linear·q̇ ≈ (p(q + εq̇) − p(q)) / ε +angular_rows_match_finite_difference_of_tool_rotation: + Assert: J_angular·q̇ ≈ Log(R(q + εq̇)·R(q)ᵀ).angular / ε (spatial 3-link arm, mixed axes) +bias_acceleration_matches_finite_difference: + Assert: BiasAcceleration(frames, q̇) ≈ (J(q + εq̇) − J(q)) / ε · q̇ +prismatic_column_is_pure_translation: + Arrange: FrameChain with a prismatic joint along ŷ + Assert: its column = (ŷ; 0) +wrench_along_link_line_needs_no_torque: + Assert: JointTorques(J(0,0), (x̂; 0)) ≈ (0, 0) +link_model_and_dh_frames_give_the_same_jacobian: + Arrange: same 2-link arm described by DH (M7) and by links (M30) + Assert: Compute(dhFrames) ≈ Compute(linkFrames) +task_jacobian_position_rows_match_six_dof_rows: + Assert: ChainTaskJacobian::Jacobian(q) == first three rows of the 6-row version +``` + +## Reference vectors + +- 2-link unit arm at `q = (0,0)`: linear `[[0,0],[2,1],[0,0]]`, angular `[[0,0],[0,0],[1,1]]`. +- Tool force `x̂` on the stretched arm acts along the links ⇒ joint torques `(0, 0)`. + +## Edge cases + +- Stretched arm ⇒ columns linearly dependent (singular). +- `q̇ = 0` ⇒ `BiasAcceleration = 0`. +- Single joint ⇒ `6×1`. diff --git a/roadmap/kinematics/ManipulabilityIndex/explanation.md b/roadmap/kinematics/ManipulabilityIndex/explanation.md index 60b051b..f067812 100644 --- a/roadmap/kinematics/ManipulabilityIndex/explanation.md +++ b/roadmap/kinematics/ManipulabilityIndex/explanation.md @@ -1,31 +1,34 @@ # Manipulability Index — Overview ## What it is -A single number that scores how well an arm can move in its current posture. Yoshikawa's measure, -`w = √det(J Jᵀ)`, is the volume of the "velocity ellipsoid" — the set of tool velocities reachable by -unit-norm joint rates. Big `w` means dexterous; `w = 0` means the arm is at a singularity. +A single number that scores how well an arm can move in its current posture. Yoshikawa's measure is +the volume of the "velocity ellipsoid" — the set of tool velocities reachable with unit-norm joint +rates. Big means dexterous; zero means the arm is at a singularity. Its companions are the ellipsoid's +axis lengths (the singular values) and their ratio (the condition number). ## Why it matters (embedded) Singularities are where Jacobian-based controllers blow up: joint rates rocket toward infinity for a finite tool motion. A cheap scalar that flags "you are getting close" lets a real-time controller slow -down, switch strategies, or damp the inverse *before* the actuators saturate. It is also the objective -a redundant arm optimizes in its null space to stay out of trouble. +down, raise damping in its inverse, or switch strategy *before* the actuators saturate. It is also the +objective a redundant arm maximizes in its null space. ## How it works (intuition) -Map the unit sphere of joint velocities through `J` and it becomes an ellipsoid in task space. The -ellipsoid's principal axes are the singular values of `J`; its volume is their product, which equals -`√det(J Jᵀ)`. When the arm nears a singularity one axis collapses — one singular value goes to zero — -so the volume, and hence `w`, drops to zero. The ratio of the longest to shortest axis (the condition -number) tells you how *lopsided* the reachable set is, a finer warning than volume alone. +Map the unit sphere of joint velocities through the Jacobian and it becomes an ellipsoid; its axes are +the singular values and its volume their product. If there are at least as many joints as measured +task directions, the volume is `√det(J·Jᵀ)`. If there are fewer joints — a two-joint planar arm measured +in 3-D position — the ellipsoid is flat in the task space, `det(J·Jᵀ)` is always zero, and the +meaningful volume is that of the lower-dimensional ellipsoid, `√det(Jᵀ·J)`. Linear and angular rows +have different units, so for full-pose tasks the translational and rotational volumes are reported +separately rather than multiplied into one unit-dependent number. ## Key parameters -- **the Jacobian `J`** — supplied by `SpatialJacobian` at the current joint configuration. +- **Jacobian provider and task dimension** — which rows are measured (position only, planar, or full pose). - **singularity threshold `eps`** — below which `NearSingular` trips. -- **square vs redundant** — picks `|det J|` versus the Gram-determinant form. ## Reference T. Yoshikawa, "Manipulability of Robotic Mechanisms," *Int. J. Robotics Research*, 4(2), 1985. ## See also -`SpatialJacobian` (M8, the input), `SingularValueDecomposition` (item 43, robust axes + conditioning), -`RedundancyResolution` (M14, which maximizes this in the null space). +`GeometricJacobian` / `JacobianProvider` (M8, the input), `SingularValueDecomposition` and +`LuDecomposition` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)), +`PoseInverseKinematics` (M13, adaptive damping), `RedundancyResolution` (M14, a null-space objective). diff --git a/roadmap/kinematics/ManipulabilityIndex/implementation.md b/roadmap/kinematics/ManipulabilityIndex/implementation.md index d83ab51..91c39fa 100644 --- a/roadmap/kinematics/ManipulabilityIndex/implementation.md +++ b/roadmap/kinematics/ManipulabilityIndex/implementation.md @@ -4,40 +4,52 @@ ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float class ManipulabilityIndex: - SpatialJacobian jac - # scratch: SquareMatrix JJt (task-space Gram, m = 6) + const JacobianProvider& jacobian # M8 seam, injected + # measured blocks share one physical unit: + static constexpr std::size_t Rows = (TaskDim == 6) ? 3 : TaskDim # TaskDim = 6 → linear / angular blocks + static constexpr std::size_t Axes = min(Rows, Dof) ``` ## Interface -``` -ManipulabilityIndex(SpatialJacobian jac) -T Compute(JointVector q) # w = √det(J Jᵀ); hot path -T ConditionNumber(JointVector q) # σ_max / σ_min -std::array EllipsoidAxes(JointVector q) # singular values (semi-axis lengths) -bool NearSingular(JointVector q, T eps) # w < eps +```cpp +explicit ManipulabilityIndex(const JacobianProvider& jacobian) +T Compute(const JointVector& q) const # w of rows 0..Rows−1 (translational for TaskDim 3/6); hot path +T ComputeRotational(const JointVector& q) const requires (TaskDim == 6) # rows 3–5 +std::array EllipsoidAxes(const JointVector& q) const # singular values of the Compute block, descending +T ConditionNumber(const JointVector& q) const # σ_max / σ_min of that block; +∞ when σ_min ≤ ε +bool NearSingular(const JointVector& q, T eps) const # Compute(q) < eps ``` ## Algorithm (pseudocode) -``` +```text +function GramRoot(B): # B: Rows × Dof, one unit + if Rows ≤ Dof: G = B·Bᵀ # Rows × Rows: task-space ellipsoid volume + else: G = Bᵀ·B # Dof × Dof: B·Bᵀ would be rank ≤ Dof ⇒ det ≡ 0 + lu = solvers::LuDecomposition + if !lu.Decompose(G): return 0 # upstream flags pivot < 1e-6·max ⇒ σ_min/σ_max ≲ 1e-3 + return sqrt(max(0, lu.Determinant())) # clamp −0 from rounding + function Compute(q): # OPTIMIZE_FOR_SPEED - J = jac.Compute(q) # 6×N - JJt = J * Transpose(J) # 6×6, symmetric PSD - return sqrt( determinant(JJt) ) # Yoshikawa measure w - # square non-redundant arm (N = 6): w = |det J| directly — skip the product + J = jacobian.Jacobian(q) # TaskDim × Dof + return GramRoot(J.rows(0 .. Rows−1)) + +function ComputeRotational(q): # TaskDim = 6 + return GramRoot(jacobian.Jacobian(q).rows(3 .. 5)) function EllipsoidAxes(q): - J = jac.Compute(q) - σ = SingularValues(J) # reuse SVD (item 43) - return σ # w = ∏ σᵢ ; axes of the velocity ellipsoid + B = jacobian.Jacobian(q).rows(0 .. Rows−1) + svd = solvers::SingularValueDecomposition + svd.Decompose(Rows ≥ Dof ? B : Bᵀ) # upstream requires Rows ≥ Cols; σ(Bᵀ) = σ(B) + return svd.SingularValues() # descending; ∏ σᵢ = GramRoot(B) function ConditionNumber(q): - σ = SingularValues(jac.Compute(q)) - return σ_max / σ_min # → ∞ at a singularity + σ = EllipsoidAxes(q) + return σ_min ≤ ε·σ_max ? +∞ : σ_max / σ_min # upstream ConditionNumber() returns 0 when singular — map to +∞ function NearSingular(q, eps): return Compute(q) < eps @@ -45,29 +57,39 @@ function NearSingular(q, eps): ## Complexity & memory -- `Compute` via `J Jᵀ` + determinant: `O(N·36 + 6³)` — cheap for small `N`. -- `EllipsoidAxes` / `ConditionNumber` via SVD: `O(6²·N)` but more numerically robust. -- Memory: one `6×6` scratch matrix (or the SVD factors); no heap. +- `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. +- Memory: one `TaskDim×Dof` Jacobian, one Gram matrix or SVD factors; stack only. ## Numerical / embedded notes -- `w = √det(J Jᵀ)` is the **volume** of the velocity ellipsoid: large = dexterous, `w → 0` = singular. -- Prefer **SVD** (item 43) over an explicit determinant when you also need conditioning or the - ellipsoid axes — `det(J Jᵀ) = (∏ σᵢ)²`, but the product of singular values avoids cancellation. -- For a **redundant** arm (`N > 6`) the Gram-determinant root is the only valid form — `det J` does - not exist for a non-square `J`. -- Use `NearSingular` as a guard *before* any Jacobian inverse; do not wait for the solve to blow up. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Branch choice matters:** with fewer joints than measured rows (`Dof < Rows`), `det(B·Bᵀ)` is + identically zero; `√det(Bᵀ·B)` is the volume of the `Dof`-dimensional ellipsoid actually reachable. + For the 2-link planar arm with position rows this gives `|sin q₂|`. +- **Units:** linear rows are in m/s per unit rate, angular rows in rad/s. For `TaskDim = 6` the two + blocks are measured separately; a single 6-row determinant mixes units and depends on the length unit. +- A planar arm with `TaskDim = 3` and `Dof ≥ 3` has a zero `z` row ⇒ `w ≡ 0`; use a `TaskDim = 2` + provider (planar position rows) for planar tasks. +- `det(G) = (∏ σᵢ)²`; when conditioning or axes are needed use the SVD (upstream + `numerical/solvers/SingularValueDecomposition.hpp`, + [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)) — it avoids the + squaring of `G` (the Gram/LU path reports `0` once `σ_min/σ_max ≲ 1e-3`; `∏ EllipsoidAxes` stays + accurate below that). For `Rows = Dof`, `w = |det B|` directly. +- Use `NearSingular` as a guard *before* any Jacobian inverse. +- `ComputeRotational` is constrained with `requires` (C++20), not `static_assert`, so explicit + coverage instantiations with `TaskDim ≠ 6` still compile. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/kinematics/ManipulabilityIndex.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ManipulabilityIndex;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ManipulabilityIndex.cpp` → `template class ManipulabilityIndex;` + `extern template class ManipulabilityIndex;` / `` / `` / + `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/ManipulabilityIndex.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestManipulabilityIndex.cpp` - 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`. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ManipulabilityIndex/tests.md b/roadmap/kinematics/ManipulabilityIndex/tests.md index 01fb5aa..9256974 100644 --- a/roadmap/kinematics/ManipulabilityIndex/tests.md +++ b/roadmap/kinematics/ManipulabilityIndex/tests.md @@ -4,53 +4,53 @@ ## Fixture -``` -class TestManipulability : public ::testing::Test: - DenavitHartenberg arm{ ... } # 2-link unit planar arm - SpatialJacobian jac{ arm } - ManipulabilityIndex w{ jac } -# each case below is a TEST_F(TestManipulability, ) +```cpp +class TestManipulabilityIndex : public ::testing::Test: + # 2-link unit planar arm (z-axis joints, link lengths 1), link model M30 + std::array, 2> planar{ ... } + ChainTaskJacobian position{ planar, SE3Transform{ I, (1, 0, 0) } } + ChainTaskJacobian full{ planar, SE3Transform{ I, (1, 0, 0) } } + ManipulabilityIndex w{ position } + StrictMock> planarTask # 3-link planar arm, xy rows + ManipulabilityIndex wRedundant{ planarTask } +# each case below is a TEST_F(TestManipulabilityIndex, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```cpp +position_manipulability_is_abs_sin_elbow: + Assert: w.Compute({0, q₂}) ≈ |sin q₂| for q₂ ∈ {0.3, 1.2, 2.5} (Dof < Rows ⇒ Jᵀ·J branch) right_angle_elbow_is_most_dexterous: - Act: value = w.Compute({0, π/2}) - Assert: value is the local maximum over q₂ - -stretched_arm_is_singular: - Act: value = w.Compute({0, 0}) - Assert: value ≈ 0 (elbow straight ⇒ lost a DOF) - -folded_arm_is_singular: - Assert: w.Compute({0, π}) ≈ 0 - -index_is_nonnegative: - Assert: Compute(q) ≥ 0 across a sweep of q - -near_singular_flag_trips: - Arrange: q close to stretched - Assert: NearSingular(q, eps) == true - + Assert: w.Compute({0, π/2}) ≈ 1 and > w.Compute({0, π/2 ± 0.1}) +stretched_and_folded_arm_are_singular: + Assert: w.Compute({0, 0}) ≈ 0 and w.Compute({0, π}) ≈ 0 +redundant_task_uses_j_jt_branch: + Arrange: EXPECT_CALL(planarTask, Jacobian) returns [[−1, −1, 0], [0, −1, −1]] # q = (0, π/2, π/2) + Assert: wRedundant.Compute(q) ≈ √3 = 1.732051; EllipsoidAxes ≈ (1.732051, 1) +six_row_task_measures_units_separately: + Arrange: ManipulabilityIndex{ full } + Assert: Compute({0, 1.2}) ≈ 0.932039 (= |sin 1.2|); ComputeRotational({0, 1.2}) ≈ 0 (parallel axes) +ellipsoid_axes_are_sorted_singular_values: + Assert: w.EllipsoidAxes({0, π/2}) ≈ (1.618034, 0.618034); product ≈ w.Compute({0, π/2}) condition_number_grows_near_singularity: - Assert: ConditionNumber → large as q → stretched - -ellipsoid_axes_match_singular_values: - Assert: EllipsoidAxes(q) are sorted σ; their product ≈ Compute(q) - -symmetry_about_elbow_sign: - Assert: Compute({0, +β}) ≈ Compute({0, -β}) + Assert: ConditionNumber({0, π/2}) ≈ 2.618034; ({0, 0.1}) ≈ 49.963; ({0, 0.01}) ≈ 499.996 (rel tol 1e-3) +exactly_singular_condition_number_is_infinite: + Assert: ConditionNumber({0, 0}) == +∞ +near_singular_flag_trips: + Assert: NearSingular({0, 0.01}, 0.05) == true; NearSingular({0, π/2}, 0.05) == false ``` ## Reference vectors -- 2-link unit arm: `w(q₂) = |sin q₂|` (planar case) ⇒ max at `q₂ = π/2`, zero at `q₂ ∈ {0, π}`. -- Condition number `→ ∞` as `q₂ → 0`. +- 2-link unit arm, position rows: `w(q₂) = |sin q₂|` — `0.295520` (0.3), `0.932039` (1.2), `1` (π/2), + `0.598472` (2.5), `0` at `{0, π}`. The full 6-row `√det(J·Jᵀ)` is `0` for every `q` (rank ≤ 2). +- `q = (0, π/2)`: `σ = (φ, 1/φ) = (1.618034, 0.618034)`, `κ = φ² = 2.618034`. +- 3-link unit planar arm, `q = (0, π/2, π/2)`: `J_xy = [[−1,−1,0],[0,−1,−1]]`, `J_xy·J_xyᵀ = [[2,1],[1,2]]` + ⇒ `w = √3`, `σ = (√3, 1)`. ## Edge cases -- Square arm (`N = 6`) ⇒ `w = |det J|`, skip the Gram product. -- Redundant arm (`N > 6`) ⇒ `√det(J Jᵀ)` only; `det J` undefined. -- Exactly singular `J` ⇒ `ConditionNumber` division guarded (returns ∞ sentinel). -- Tiny `w` below `float` epsilon ⇒ `NearSingular` still deterministic. +- `Rows = Dof` ⇒ `w = |det B|`. +- Planar arm with `TaskDim = 3`, `Dof ≥ 3` ⇒ zero `z` row, `w ≡ 0` (documented, use `TaskDim = 2`). +- Tiny `w` below `float` epsilon ⇒ `NearSingular` still deterministic; `Determinant` clamped at 0. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md index e2ccdbb..493ef3d 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md @@ -1,36 +1,37 @@ # Mobile-Manipulator Kinematics — Overview ## What it is -Kinematics for an arm mounted on a driving base — a robot that can both reach and roam. It combines the -arm's Jacobian with the base's motion into one map from "wheel commands plus joint rates" to -end-effector velocity, while respecting that a wheeled base cannot move sideways. +Kinematics for an arm mounted on a differential-drive base — a robot that can both reach and roam. It +combines the arm's Jacobian with the base's motion into one map from "drive speed, turn rate and joint +rates" to the tool's linear and angular velocity in the world, respecting that wheels cannot slide +sideways. ## Why it matters (embedded) -Mobile manipulators — warehouse robots, service robots, rovers with arms — must coordinate base and arm -as a single system, not two. A unified Jacobian lets one controller command the whole platform, and the -extra freedom of "drive there *and* reach" is what gives these robots their large effective workspace. -Doing it on-board in real time needs a compact, constraint-aware model. +Warehouse, service and field robots must coordinate base and arm as one system. A unified Jacobian +lets a single controller command the whole platform, and the extra freedom of "drive there *and* +reach" gives these robots their large workspace. Computing it on board each cycle needs a compact, +frame-consistent model. ## How it works (intuition) -The tool's velocity is the sum of two contributions: what the arm does on top of a stationary base, and -what the base does while carrying a frozen arm. The arm part is its ordinary Jacobian. The base part -depends on the lever arm from the base to the tool — driving forward translates the tool, spinning the -base swings it around, and the farther out the tool, the more a base rotation moves it. The catch is the -**nonholonomic** constraint: a differential-drive base has three planar coordinates but only two -controls (drive speed and turn rate), because it physically cannot slip sideways. A small matrix `S(φ)` -encodes that, mapping the two real controls into allowed base motion. Stacking the constrained base -block beside the arm Jacobian gives the combined map; because base-plus-arm usually over-provides DOF, -null-space resolution decides how much each should contribute. +The tool velocity is the sum of what the arm does on a stationary base and what the base does while +carrying a frozen arm. The arm's Jacobian is known in the arm's own base frame, so it is first rotated +into the world by the base heading and the mounting orientation. The base contributes two columns: +driving forward translates the tool along the heading, and turning swings it about the vertical axis +through the axle midpoint — the farther the tool from that axis, the faster it moves. Because a +differential drive cannot slip sideways, only these two base motions exist; there is no lateral +column to command. Base plus arm usually provides more freedom than the task needs, and null-space +resolution decides how much each contributes. ## Key parameters -- **base pose `(x, y, φ)`** and **drive controls `(v, ω)`** — the constrained base freedom. -- **arm configuration `q`** — the joint angles feeding the arm Jacobian. -- **base-to-arm mount** — the fixed transform placing the arm on the base. +- **base pose `(x, y, φ)`** and **controls `(v, ω)`** — axle-midpoint pose, drive speed and turn rate. +- **arm joints `q`** and the arm's Jacobian provider. +- **mount transform** — arm base in the mobile-base frame. +- **wheel base** — converts `(v, ω)` to wheel speeds. ## Reference Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. Automatic Control*, 39(6), 1994. ## See also -`SpatialJacobian` (M8, the arm block), `RedundancyResolution` (M14, splitting base vs arm), -`SE3Transform` (M6, base pose composition). +`JacobianProvider` (M8, the arm block), `SE3Transform` (M6, pose composition), `RedundancyResolution` +(M14, splitting base vs arm). diff --git a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md index ccc6ab3..265f357 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md @@ -1,79 +1,84 @@ -# Mobile-Manipulator Kinematics (Nonholonomic Base) — Implementation Pseudocode +# Mobile-Manipulator Kinematics (Differential-Drive Base) — Implementation Pseudocode > Roadmap ref: #M24 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) ## Data structures -``` +```cpp template # static_assert(std::is_floating_point_v); instantiated for float -struct BaseState: # differential-drive base pose - T x, y, phi # planar position + heading +struct BaseState: # planar pose of the axle midpoint in the world frame + T x, y, phi -template +template class MobileManipulatorKinematics: - SpatialJacobian armJac - SE3 baseToArm # arm mount on the base - T wheelBase # for the drive model + const JacobianProvider& arm # M8: J and ToolPose in the arm-base frame + SE3Transform mount # arm base in the mobile-base frame (M6) + T wheelBase # L, axle length ``` ## Interface -``` -MobileManipulatorKinematics(armJac, SE3 baseToArm, T wheelBase) -Matrix Compute(BaseState base, JointVector q) # combined J; hot path -SE3 EndEffectorPose(BaseState base, JointVector q) -Matrix BaseConstraint(BaseState base) # S(φ): (v,ω) → q̇_base +```cpp +MobileManipulatorKinematics(const JacobianProvider& arm, const SE3Transform& mount, T wheelBase) +Matrix Compute(const BaseState& base, const JointVector& q) const # hot path +SE3Transform ToolPose(const BaseState& base, const JointVector& q) const +static Matrix BaseConstraint(T phi) # (ẋ, ẏ, φ̇) = S(φ)·(v, ω) +std::array WheelSpeeds(T v, T omega) const # (left, right) rim speeds ``` +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). + ## Algorithm (pseudocode) -``` -function BaseConstraint(base): # nonholonomic: no side-slip - # Pfaffian constraint [-sinφ, cosφ, 0]·q̇_base = 0 ⇒ parameterize by (v, ω) - return [[ cosφ, 0 ], - [ sinφ, 0 ], - [ 0, 1 ]] # q̇_base = S(φ)·(v, ω) +```cpp +function ToolPose(base, q): + T_wb = { Rz(base.phi), (base.x, base.y, 0) } + return T_wb * mount * arm.ToolPose(q) function Compute(base, q): # OPTIMIZE_FOR_SPEED - # end-effector velocity = base contribution + arm contribution - J_arm = armJac.Compute(q) # 6×NumArm, in the base frame - r = EndEffectorPose(base, q).p - basePosition # lever arm base → tool - J_base = [[ I₃ , -SkewSymmetric(r) ], # planar base twist → tool twist - [ 0 , ẑ ]] # keep the (v, ω) columns only - J_base_reduced = J_base * lift(S(φ)) # apply nonholonomic S(φ) ⇒ 6×2 - return [ J_base_reduced | J_arm ] # 6 × (2 + NumArm) - -function EndEffectorPose(base, q): - T_base = SE3(Rotz(base.phi), (base.x, base.y, 0)) - return T_base * baseToArm * armForward(q) + R_wa = Rz(base.phi) · mount.R # arm-base orientation in the world + J_arm = arm.Jacobian(q) # 6×ArmDof, arm-base frame + r = ToolPose(base, q).p − (base.x, base.y, 0) # lever arm, world frame + column 0 (v) = ( (cos φ, sin φ, 0) ; 0 ) + column 1 (ω) = ( CrossProduct(ẑ, r) ; ẑ ) + columns 2.. = ( R_wa · J_arm.linear ; R_wa · J_arm.angular ) # blockdiag(R_wa, R_wa)·J_arm + return J + +function BaseConstraint(φ): # no side-slip: [−sin φ, cos φ, 0]·(ẋ, ẏ, φ̇) = 0 + return [[cos φ, 0], [sin φ, 0], [0, 1]] + +function WheelSpeeds(v, ω): + return (v − ω·L/2, v + ω·L/2) ``` ## Complexity & memory -- `Compute`: `O(NumArm)` — arm Jacobian plus a fixed base block and one `S(φ)` multiply. -- Memory: one `6 × (2 + NumArm)` combined Jacobian; bounded, stack-allocated. +- `Compute`: one arm `Jacobian` + `ToolPose`, `2·ArmDof` 3×3 rotations, two fixed base columns. +- Memory: one `6 × (2 + ArmDof)` matrix; stack only. ## Numerical / embedded notes -- The **nonholonomic constraint** (a differential-drive base cannot slide sideways) drops the base from - 3 planar DOF to 2 controls `(v, ω)`; `S(φ)` enforces it — never command lateral base velocity directly. -- Base and arm together are **redundant** for a 6-DOF task — resolve the split with null-space - projection (M14), e.g. prefer arm motion for fine moves and base motion for gross reach. -- Compute the tool lever arm `r` in the **base frame** so the `[r]×` block and the arm Jacobian share - one convention; reuse `SkewSymmetric` (Geometry3D) and `SpatialJacobian` (M8). -- Watch coupling: base rotation `ω` moves a far-out tool a lot (`|r|` large) — scale the two blocks so - the solver does not over-use the base. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Frames:** the arm Jacobian is expressed in the arm-base frame; it must be rotated by + `R_wa = Rz(φ)·R_mount` before being stacked with the world-frame base columns (skipping it gives an + error of order ‖J_arm·q̇‖ — 0.52 in the test configuration). No lever-arm correction is needed for the + arm block: its linear rows already describe the tool point. +- **Nonholonomic base:** a differential drive has three planar coordinates but two controls; the base + columns are already the constrained ones, so no lateral-velocity column exists to command. +- Base plus arm is redundant for a 6-DOF task — split the motion with M14 (e.g. arm for fine motion, + base for reach); scale the base columns (`|r|` amplifies yaw) before resolving. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/kinematics/MobileManipulatorKinematics.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class MobileManipulatorKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/MobileManipulatorKinematics.cpp` → `template class MobileManipulatorKinematics;` + `extern template class MobileManipulatorKinematics;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/MobileManipulatorKinematics.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestMobileManipulatorKinematics.cpp` - 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`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/tests.md b/roadmap/kinematics/MobileManipulatorKinematics/tests.md index 13743dd..4466135 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/tests.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/tests.md @@ -4,55 +4,49 @@ ## Fixture -``` -class TestMobileManipulator : public ::testing::Test: - DenavitHartenberg arm{ ... } - SpatialJacobian armJac{ arm } - MobileManipulatorKinematics mm{ armJac, mount, wheelBase=0.3f } -# each case below is a TEST_F(TestMobileManipulator, ) +```cpp +class TestMobileManipulatorKinematics : public ::testing::Test: + # 3-link arm (M30 link model): z-axis joint at (0,0,0.1); y-axis joint +(0,0,0.2); y-axis joint +(0.3,0,0) + std::array, 3> links{ ... } + ChainTaskJacobian arm{ links, SE3Transform{ I, (0.25, 0, 0) } } + MobileManipulatorKinematics mm{ arm, SE3Transform{ Rz(π/2), (0.1, 0, 0.3) }, 0.3 } + BaseState base{ 1, 2, π/6 }; JointVector q{ 0.4, −0.6, 0.9 } + u = (v, ω, q̇) = (0.3, −0.5, 0.2, −0.1, 0.4) +# each case below is a TEST_F(TestMobileManipulatorKinematics, ) ``` ## Test cases (Arrange / Act / Assert) -``` -combined_jacobian_dimensions: - Act: J = mm.Compute(base0, q0) - Assert: J is 6 × (2 + 3) - -base_pose_composes_with_arm: - Assert: EndEffectorPose = T_base · mount · FK_arm(q) - -nonholonomic_constraint_blocks_sideslip: - Assert: BaseConstraint(base)·(v,ω) has zero lateral component in the world frame - -pure_forward_drive_moves_tool: - Arrange: (v=1, ω=0), q̇ = 0 - Assert: tool linear velocity ≈ the forward heading direction - -base_yaw_moves_far_tool_more: - Arrange: ω = 1, large lever arm r - Assert: tool linear-velocity magnitude scales with |r| - -arm_only_motion_when_base_still: - Arrange: (v, ω) = 0 - Assert: tool velocity = armJac·q̇ - -redundancy_split_base_and_arm: - Arrange: task rate reachable by base or arm - Assert: null-space resolution distributes the motion (M14) - +```cpp +tool_pose_composes_base_mount_and_arm: + Assert: ToolPose(base, q) = T_wb·mount·arm.ToolPose(q); p ≈ (0.698536, 2.343297, 0.695513) twist_matches_finite_difference: - Assert: J·[v, ω, q̇] ≈ (pose(t + ε) ⊖ pose(t)) / ε + Arrange: integrate base (x += εv cos φ, y += εv sin φ, φ += εω) and q += εq̇, ε = 1e-3 + Assert: Compute(base, q)·u ≈ (Δp; Log(ΔR)) / ε (tol 1e-3); + Compute·u ≈ (0.403993, 0.199541, −0.046890; −0.180886, −0.239333, −0.3) +arm_block_is_rotated_into_the_world: + Arrange: u = (0, 0, q̇) + Assert: columns 2..4 = blockdiag(R_wa, R_wa)·arm.Jacobian(q), R_wa = Rz(π/6)·Rz(π/2) +forward_drive_moves_tool_along_heading: + Arrange: base φ = 0, u = (1, 0, 0, 0, 0) + Assert: tool twist = ((1, 0, 0); 0) +yaw_moves_tool_by_lever_arm: + Arrange: base φ = 0, u = (0, 1, 0, 0, 0) + Assert: linear = ẑ × r, ‖linear‖ = ‖r_xy‖ = 0.456874; angular = ẑ +base_constraint_blocks_sideslip: + Assert: (−sin φ, cos φ, 0)·BaseConstraint(φ)·(v, ω) = 0 for φ ∈ {0, π/6, π} +wheel_speeds_from_unicycle_command: + Assert: WheelSpeeds(1, 2) = (0.7, 1.3) (L = 0.3) ``` ## Reference vectors -- Base heading `φ = 0`, `(v=1, ω=0)` ⇒ tool linear velocity `(1, 0, 0)` plus any arm term. -- Nonholonomic `S(φ)` ⇒ `[-sinφ, cosφ, 0]·q̇_base = 0` for all `(v, ω)`. +- Fixture: tool world `p = (0.698536, 2.343297, 0.695513)`, `r = (−0.301464, 0.343297, 0.695513)`. +- `J·u` agrees with the finite difference to `3.6e-8` (ε = 1e-6) / `3.6e-5` (ε = 1e-3) in double; + stacking the unrotated arm block instead gives an error of `0.516`. ## Edge cases -- Zero lever arm (tool over the base axis) ⇒ base yaw adds no tool translation. -- Pure spin (`v=0, ω≠0`) ⇒ tool moves only if `r ≠ 0`. -- Heading wrap (`φ = ±π`) ⇒ `S(φ)` still consistent. -- Non-redundant case (task = base + arm DOF) ⇒ unique split, empty null space. +- Tool above the axle midpoint (`r_xy = 0`) ⇒ yaw adds no tool translation. +- Heading wrap `φ = ±π` ⇒ columns continuous. +- Redundancy split (base vs arm) is M14's job, not tested here. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md index d8e0a35..ebc92be 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md @@ -1,35 +1,39 @@ # Parallel Manipulator Kinematics — Overview ## What it is -Kinematics for robots whose moving platform is held by several legs in parallel — the pick-and-place -**Delta** (three arms) and the six-legged **Stewart-Gough** hexapod. Given a platform pose it finds the -leg lengths (inverse), and given the leg lengths it finds the platform pose (forward). +Kinematics for robots whose moving platform is held by several chains in parallel: the six-legged +**Stewart–Gough** hexapod, driven by leg lengths, and the pick-and-place **Delta**, driven by three +rotary arms whose parallelogram forearms keep the effector level. Inverse kinematics maps a platform +pose to actuator values; forward kinematics maps actuator values back to the pose. ## Why it matters (embedded) -Parallel robots are stiff, fast, and accurate — the workhorses of high-speed packaging and motion -platforms. Their control flips the usual serial-arm effort: computing the leg commands from a desired -pose is a trivial, closed-form, per-leg calculation that fits any real-time loop, exactly what the -servo layer needs on every tick. +Parallel robots are stiff, fast and accurate — the workhorses of high-speed packaging and motion +platforms. Their servo layer needs actuator commands from a desired pose on every tick, and both +mechanisms make that a short, closed-form, per-chain computation. Forward kinematics, needed for state +estimation, is cheap for the Delta and a few Newton steps for the hexapod. ## How it works (intuition) -For the **inverse** problem each leg is independent: transform its platform anchor into the base frame, -subtract its base anchor, and the length is just the distance between those two points — no coupling, no -iteration. The **forward** problem is the hard one, because moving one leg moves the whole platform and -thus every other leg. It has no general closed form, so you solve it with Newton's method: guess a -pose, compute what leg lengths it *would* produce, and correct the pose until those match the measured -lengths. Several platform poses can produce the same leg lengths (assembly modes), so the initial guess -picks which one you land on. The three-legged Delta is special enough to admit a closed-form forward -solve by intersecting spheres. +For the **hexapod**, each leg is independent in the inverse problem: move the platform anchor into the +base frame and measure its distance to the base anchor. The forward problem couples all legs, so it is +solved by Newton's method on the pose: predict the leg lengths, compare, and correct position and +orientation using the leg Jacobian (each row is the leg direction and its moment about the platform +origin). Several poses share the same leg lengths, so the initial guess picks the assembly mode; +near singular poses a little damping keeps the correction finite. +For the **Delta**, each arm's elbow moves on a circle, and the forearm end must lie on a sphere around +the effector attachment. Rotating the target into the arm's plane reduces that to one equation +`a·cos θ + b·sin θ = c`, solved with one `atan2` and one `acos`. Going forward, each elbow (shifted +inward by the effector radius) is the centre of a sphere of forearm length, and the effector centre is +the lower of the two points where the three spheres meet. ## Key parameters -- **base and platform anchor points** — the fixed geometry of the mechanism. -- **leg lengths** — the actuated variables (prismatic legs). -- **forward-solve guess** — selects the assembly mode and warm-starts Newton. +- **Hexapod:** base and platform anchors; Newton tolerance, damping and iteration limit. +- **Delta:** base radius, effector radius, upper-arm length, forearm length. ## Reference J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, "Delta, a Fast Robot with Parallel -Geometry," *Int. Symp. on Industrial Robots*, 1990. +Geometry," *Int. Symp. on Industrial Robots*, 1988. ## See also -`SE3Transform` (M6, platform pose), `LuDecomposition` / `GaussianElimination` (item 28, the Newton -step), `ManipulabilityIndex` (M11, singularity monitoring). +`SE3Transform` (M6, 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 09a98f5..9753db9 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md @@ -1,82 +1,132 @@ -# Parallel Manipulator Kinematics (Delta / Stewart-Gough) — Implementation Pseudocode +# Parallel Manipulator Kinematics (Stewart–Gough / Delta) — Implementation Pseudocode > Roadmap ref: #M23 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) +Two mechanisms, two classes: the Stewart–Gough hexapod (six prismatic legs, 6-DOF platform) and the +Delta robot (three revolute actuated arms with parallelogram forearms, 3-DOF translation). Their +actuated variables differ (leg lengths vs arm angles), so they share no data model. + ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct PlatformGeometry: - std::array, NumLegs> baseAnchors # bᵢ in the fixed frame - std::array, NumLegs> platformAnchors # pᵢ in the moving frame +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct StewartGoughGeometry: + std::array, 6> baseAnchors # bᵢ, base frame + std::array, 6> platformAnchors # pᵢ, platform frame + +template +struct StewartForwardConfig: + T tolerance; T damping; std::size_t maxIterations # damping λ: 0 ⇒ plain Newton + +template +struct StewartForwardResult: + SE3Transform pose; std::size_t iterations; bool converged + +template +class StewartGoughKinematics: + StewartGoughGeometry geometry + StewartForwardConfig config template -struct ForwardConfig: - T tolerance - std::size_t maxIterations - -template -class ParallelManipulatorKinematics: - PlatformGeometry geom - ForwardConfig config +struct DeltaGeometry: # base plane z = 0, effector below (z < 0) + T baseRadius # R: centre → shoulder axis (triangle side = 2√3·R) + T effectorRadius # r: effector centre → forearm attachment + T upperArm # rf + T forearm # re (parallelogram length) + # arm i shoulder azimuth φᵢ = 0°, 120°, 240°; θᵢ = 0 ⇒ upper arm horizontal, outward; θ > 0 ⇒ down + +template +class DeltaKinematics: + DeltaGeometry geometry + std::array cosPhi, sinPhi # precomputed ``` ## Interface -``` -ParallelManipulatorKinematics(geom, ForwardConfig cfg = {}) -std::array Inverse(SE3 platformPose) # leg lengths; closed form, hot path -SE3 Forward(std::array legLengths, - SE3 guess) # Newton iteration +```cpp +StewartGoughKinematics(const StewartGoughGeometry& geometry, const StewartForwardConfig& config) +std::array Inverse(const SE3Transform& pose) const # leg lengths; hot path +Matrix6 LegJacobian(const SE3Transform& pose) const # l̇ = J·(v; ω) +StewartForwardResult Forward(const std::array& lengths, const SE3Transform& guess) const + +explicit DeltaKinematics(const DeltaGeometry& geometry) +std::optional> Inverse(const Vector3& effector) const # returns (θ₁, θ₂, θ₃); hot path +std::optional> Forward(const Vector3& angles) const # effector position ``` ## Algorithm (pseudocode) -``` -function Inverse(pose): # OPTIMIZE_FOR_SPEED (closed form) - for i in 0..NumLegs-1: - legVec = pose.R * platformAnchors[i] + pose.p - baseAnchors[i] - length[i] = VectorNorm(legVec) # each leg independent - return length - -function Forward(measured, guess): # iterative — multiple assembly modes - x = guess # pose as (p, small-rotation) +```cpp +# ---- Stewart–Gough ---- +function Inverse(pose): # OPTIMIZE_FOR_SPEED, closed form + for i: lengths[i] = ‖pose.R·pᵢ + pose.p − bᵢ‖ + return lengths + +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 + +function Forward(L, guess): # Newton on SE(3) + x = guess for iter in 0..maxIterations-1: - f = Inverse(x) - measured # residual per leg - if norm(f) < tolerance: return x - J = ∂(legLengths)/∂x # NumLegs × 6 (Stewart), analytic or numeric - Δx = solve(J, -f) # LU (item 28); least squares if NumLegs ≠ 6 - x = x ⊕ Δx # retract onto SE(3) - return x # best effort (may not converge) + 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 } + +# ---- Delta ---- +function ArmAngle(i, P): # circle (elbow) ∩ sphere (forearm) in arm i's plane + x' = cosφᵢ·P.x + sinφᵢ·P.y; y' = −sinφᵢ·P.x + cosφᵢ·P.y; z = P.z + X = x' + r − R + a = 2·rf·X; b = −2·rf·z; c = X² + y'² + z² + rf² − re² # a·cos θ + b·sin θ = c + ρ = √(a² + b²); if |c| > ρ: return nullopt # out of reach + return atan2(b, a) − acos(c / ρ) # elbow-out root (larger cos θ) for z < 0 + +function Inverse(P): # OPTIMIZE_FOR_SPEED + return (ArmAngle(0, P), ArmAngle(1, P), ArmAngle(2, P)) # nullopt if any arm fails + +function Forward(θ): # three-sphere intersection, closed form + Cᵢ = Rz(φᵢ)·(R − r + rf·cos θᵢ, 0, −rf·sin θᵢ) # elbow shifted by −effector radius + ex = Normalize(C₂ − C₁); d = ‖C₂ − C₁‖; i = ex·(C₃ − C₁) + ey = Normalize(C₃ − C₁ − i·ex); j = ey·(C₃ − C₁); ez = ex × ey + x = d/2; y = (i² + j² − 2·i·x) / (2j); h² = re² − x² − y² # equal radii re + if h² < 0: return nullopt + return the one of C₁ + x·ex + y·ey ± √h²·ez with the smaller z # effector below the base ``` ## Complexity & memory -- `Inverse`: `O(NumLegs)` — one transform + norm per leg, fully parallel and branch-free. -- `Forward`: `O(iters · 6³)` — a `6×6` (or least-squares) solve per Newton step. -- Memory: geometry arrays plus one `NumLegs×6` Jacobian; bounded, stack-allocated. +- Stewart `Inverse` / `LegJacobian`: `O(6)`; `Forward`: `O(iters·6³)` (one 6×6 LU per step). +- Delta `Inverse`: 3 × (`sqrt`, `acos`, `atan2`); `Forward`: `O(1)`, one `sqrt`. +- Memory: geometry + one `6×6` matrix; stack only. ## Numerical / embedded notes -- Parallel robots invert the usual difficulty: **inverse** kinematics is trivial and closed-form, while - **forward** kinematics needs Newton iteration (the opposite of a serial arm). -- Forward FK has **multiple assembly modes** (the platform can sit in several poses for the same leg - lengths) — the `guess` selects the branch; seed it from the previous cycle. -- The **Delta** (3 legs, parallelogram arms) admits a closed-form forward solve via three-sphere - intersection — specialize it rather than iterating. -- Near a platform singularity the leg Jacobian is ill-conditioned — monitor it (M11 idea) and fall back - to a damped step; reuse `GaussianElimination` / LU (item 28) for the linear solve. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- Parallel robots invert the serial difficulty: Stewart IK is closed form, FK needs Newton and has up to + 40 assembly modes — `guess` selects one (seed from the previous cycle). The Delta's special + geometry (parallelograms keep the effector parallel to the base) makes both directions closed form. +- `LegJacobian` is the inverse Jacobian of the platform; it is singular where the platform gains + uncontrollable motion (the symmetric fixture at yaw 90°). `λ > 0` keeps the step finite there; + 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. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment -- Header: `robotics/kinematics/ParallelManipulatorKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Inverse`, and - `extern template class ParallelManipulatorKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ParallelManipulatorKinematics.cpp` → `template class ParallelManipulatorKinematics;` -- Test: `robotics/kinematics/test/TestParallelManipulatorKinematics.cpp` +- Headers: `robotics/kinematics/StewartGoughKinematics.hpp`, `robotics/kinematics/DeltaKinematics.hpp` — + `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `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` + → `template class StewartGoughKinematics;` / `template class DeltaKinematics;` +- 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`; - `TestParallelManipulatorKinematics.cpp` → the `_test` target. +- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; both tests → the `_test` target. +- Depends on: M6 (`SE3Transform`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md b/roadmap/kinematics/ParallelManipulatorKinematics/tests.md index 31f617c..29427f1 100644 --- a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md +++ b/roadmap/kinematics/ParallelManipulatorKinematics/tests.md @@ -2,58 +2,79 @@ > GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. -## Fixture - +## Fixture — Stewart–Gough + +```cpp +class TestStewartGoughKinematics : public ::testing::Test: + # leg k = 0..5, j = k/2, s = (k even ? −1 : +1) + # bₖ = 1.0·(cos β, sin β, 0), β = 120°·j + s·15°; pₖ = 0.6·(cos γ, sin γ, 0), γ = 120°·j + s·45° + StewartGoughKinematics hexapod{ geometry, { tol = 1e-5, λ = 0, 50 } } + SE3Transform home{ I, (0, 0, 0.8) } + SE3Transform tilted{ Rz(0.1)·Rx(0.05), (0.05, −0.03, 0.85) } +# each case below is a TEST_F(TestStewartGoughKinematics, ) ``` -class TestStewartGough : public ::testing::Test: - PlatformGeometry hexapod{ ... } # symmetric 6-6 platform - ParallelManipulatorKinematics kin{ hexapod, { 1e-6, 50 } } -# each case below is a TEST_F(TestStewartGough, ) + +## Test cases — Stewart–Gough + +```text +home_legs_are_equal: + Assert: Inverse(home) ≈ 0.980189 for all six legs +raising_the_platform_lengthens_every_leg: + Assert: Inverse({ I, (0, 0, 0.9) }) ≈ 1.063376 for all six legs +inverse_of_a_general_pose: + Assert: Inverse(tilted) ≈ (0.955703, 1.037816, 1.049122, 1.083232, 0.981789, 1.042429) +leg_jacobian_matches_finite_difference: + Arrange: twist (v; ω) = (0.1, −0.2, 0.05; 0.3, −0.1, 0.2), pose perturbed as p + εv, Exp(εω)·R + Assert: LegJacobian(tilted)·twist ≈ (Inverse(perturbed) − Inverse(tilted)) / ε (ε = 1e-3, tol 1e-3) +forward_recovers_the_pose: + Act: r = Forward(Inverse(tilted), { Rz(0.15), (0, 0, 0.8) }) + Assert: r.converged; r.iterations ≤ 6; r.pose ≈ tilted (tol 1e-4) +assembly_mode_is_selected_by_the_guess: + Arrange: L = Inverse(home) + Assert: Forward(L, { I, (0,0,0.7) }).pose.p ≈ (0,0,0.8); Forward(L, { I, (0,0,−0.7) }).pose.p ≈ (0,0,−0.8) +singular_pose_step_stays_finite: + Arrange: target { Rz(90°), (0,0,0.8) } (det LegJacobian = 0); λ = 1e-3; guess yaw 80° + Assert: pose finite; ‖Inverse(r.pose) − L‖ < 1e-4 ``` -## Test cases (Arrange / Act / Assert) +## Fixture — Delta +```cpp +class TestDeltaKinematics : public ::testing::Test: + DeltaKinematics delta{ { R = 0.1, r = 0.03, rf = 0.2, re = 0.45 } } +# each case below is a TEST_F(TestDeltaKinematics, ) ``` -inverse_of_home_pose: - Act: L = kin.Inverse(homePose) - Assert: all legs equal the nominal home length -inverse_is_closed_form_deterministic: - Assert: Inverse(pose) is repeatable, no iteration state +## Test cases — Delta +```text +zero_angles_put_the_effector_on_the_axis: + Assert: Forward((0,0,0)) ≈ (0, 0, −0.36) (√(re² − (R − r + rf)²) = √(0.2025 − 0.0729)) +inverse_of_known_points: + Assert: Inverse((0.05, −0.03, −0.40)) ≈ (0.042468, 0.371178, 0.205749) + Inverse((−0.08, 0.06, −0.32)) ≈ (0.195443, −0.538250, −0.084938) +inverse_forward_round_trip: + Assert: Forward(Inverse(P)) ≈ P for P ∈ {(0.05,−0.03,−0.40), (−0.08,0.06,−0.32), (0.12,0.10,−0.45)} (tol 1e-5) forward_inverse_round_trip: - Arrange: pose_true; L = kin.Inverse(pose_true) - Act: pose = kin.Forward(L, pose_true + small_perturbation) - Assert: pose ≈ pose_true - -forward_converges_from_nearby_guess: - Assert: Newton reaches tolerance in few iterations (warm start) - -leg_length_monotone_with_height: - Arrange: raise the platform along +z - Assert: each leg length increases - -assembly_mode_selected_by_guess: - Arrange: two guesses bracketing different modes - Assert: Forward returns the pose near each guess - -singular_pose_illconditioned: - Arrange: geometry at a platform singularity - Assert: Forward step damped; no NaN - -delta_closed_form_forward: - Arrange: 3-leg Delta geometry - Assert: closed-form Forward matches the Inverse round-trip + Assert: Inverse(Forward(θ)) ≈ θ over the grid θᵢ ∈ {−0.6, 0, 0.6, 1.2} (all 64 assemblable; tol 1e-4) +forearm_constraint_holds: + Assert: ‖elbowᵢ − (P + Rz(φᵢ)·(r,0,0))‖ ≈ re for each arm at Inverse(P) +unreachable_point_has_no_solution: + Assert: Inverse((0, 0, −0.9)) == nullopt ``` ## Reference vectors -- Home pose ⇒ all six legs at nominal length `L₀`. -- Pure `+z` translation `Δh` ⇒ symmetric increase in every leg length. +- Hexapod: `L₀ = √(1 + 0.36 − 1.2·cos 30° + 0.64) = 0.980189`; singular at yaw `90°` + (`det J` from `−0.8417` at 0° through `0`), legs there `(1.183216, 1.612452) × 3`. +- Newton from the stated guess converges in 4 iterations (double); with `λ = 1e-3` near the + singularity the leg residual reaches `7e-7` while the yaw is only fixed to `≈ 0.1°`. +- Delta: home `(0, 0, −0.36)`; `θ → Forward → Inverse` reproduces `θ` to `2e-15` (double) over 1000 + random `θ ∈ [−0.6, 1.2]³`; the minus root was the elbow-out root in all 46 265 sampled arm solves. ## Edge cases -- Platform singularity ⇒ leg Jacobian rank-deficient; damped fallback. -- Wrong assembly-mode guess ⇒ converges to a different valid pose (documented). -- Unreachable leg lengths ⇒ Forward fails gracefully, returns best effort. -- Delta (3 legs) vs Stewart (6 legs) ⇒ different `NumLegs` template instantiations. +- Stewart: wrong assembly-mode guess converges to a different valid pose (documented). +- Stewart: non-converging lengths ⇒ `converged == false`, best-effort pose, finite. +- Delta: `h² < 0` in `Forward` ⇒ nullopt (angles not assemblable). +- Delta: target on the reach boundary ⇒ `|c| = ρ`, single (tangent) solution; clamp `acos` argument. diff --git a/roadmap/kinematics/PoseInverseKinematics/explanation.md b/roadmap/kinematics/PoseInverseKinematics/explanation.md index 579faf5..69c6700 100644 --- a/roadmap/kinematics/PoseInverseKinematics/explanation.md +++ b/roadmap/kinematics/PoseInverseKinematics/explanation.md @@ -1,35 +1,42 @@ # Pose Inverse Kinematics — Overview ## What it is -Given a desired **pose** — position *and* orientation — of the tool, find joint angles that reach it. -This extends position-only IK to all six task dimensions by driving a 6-vector error (three for -translation, three for rotation) to zero with a damped-least-squares Newton step. +Given a desired **pose** — position *and* orientation — of the tool, find joint values that reach it. +It extends position-only IK to all six task dimensions by driving a 6-vector error (three for +translation, three for rotation) to zero with weighted, damped least-squares Newton steps. ## Why it matters (embedded) Real tasks are pose tasks: point a welding torch, align a gripper, hold a camera level. A numerically -robust solver that survives singularities without commanding infinite joint speeds is what makes an -arm safe to run in a tight real-time loop, warm-started from the previous cycle for a handful of -iterations. +robust solver that survives singularities without commanding huge joint jumps, respects joint limits +and runs a bounded number of iterations is what makes an arm safe to drive from a real-time loop, +warm-started from the previous cycle. ## How it works (intuition) -Each step linearizes the arm about the current joints with the Jacobian, then asks "what joint change -best cancels the current pose error?" The plain least-squares answer, `J⁺e`, explodes near -singularities; adding a damping term `λ²I` inside the inverse tames it — the arm gives up a little -tracking accuracy in exchange for never blowing up. The subtle part is the **orientation** error: -subtracting Euler angles wraps and stalls, so the error is computed as the *rotation vector* (the log -of the relative rotation, or twice the vector part of the error quaternion), which always points the -short way around. +Each step linearizes the arm with the Jacobian and asks "what joint change best cancels the current +pose error?" The plain least-squares answer explodes near singularities; adding `λ²I` inside the +inverse keeps the step bounded. Because the method *iterates*, damping does not leave an error +behind: whenever the target is reachable and the arm is not singular, the only fixed point is zero +error — damping just makes each step shorter. Adaptive damping switches it on only when the +manipulability drops, so steps are full Newton steps elsewhere. Two details keep the error honest: +rotation is measured with the true logarithm of the relative rotation (always the short way, exact +angle), and radians are converted to metres with a characteristic length so that position and +orientation are weighed fairly in both the step and the stopping test. ## Key parameters -- **damping λ** — larger = more stable near singularities, less accurate away from them. -- **tolerance** — the 6-vector error norm at which the solve stops. -- **max iterations** and **seed `q0`** — warm-starting from the last cycle keeps counts low. +- **damping λ₀ and threshold w₀** — maximum damping and the manipulability below which it engages. +- **characteristic length ρ** — how many metres one radian of orientation error is worth. +- **tolerance, max iterations, max step, joint limits** — stopping rule and safety bounds. +- **seed `q0`** — warm-starting from the last cycle keeps iteration counts low. ## Reference +Y. Nakamura, H. Hanafusa, "Inverse Kinematic Solutions with Singularity Robustness for Robot +Manipulator Control," *ASME J. Dyn. Sys. Meas. Control*, 108, 1986; S. Chiaverini, "Singularity-Robust +Task-Priority Redundancy Resolution," *IEEE Trans. Robotics and Automation*, 13(3), 1997; S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped -Least Squares methods," 2004; Y. Nakamura, H. Hanafusa, "Inverse Kinematic Solutions with Singularity -Robustness," *ASME J. Dyn. Sys. Meas. Control*, 1986. +Least Squares methods," 2004. ## See also -`SpatialJacobian` (M8, the linearization), `Quaternion` (item 18, the orientation error), -`GaussianElimination` (the 6×6 solve), `AnalyticalIkPieper` (M21, the closed-form alternative). +`JacobianProvider` / `GeometricJacobian` (M8, the linearization), `SE3Transform::PoseError` (M6, 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 0ee4cff..9ac657c 100644 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ b/roadmap/kinematics/PoseInverseKinematics/implementation.md @@ -1,87 +1,109 @@ -# Pose Inverse Kinematics (Damped Least Squares) — Implementation Pseudocode +# Pose Inverse Kinematics (Weighted Damped Least Squares) — Implementation Pseudocode > 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). + ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float struct PoseIkConfig: - T damping # λ (Levenberg-Marquardt) - T tolerance # 6-vector error norm to declare success + T damping # λ₀ + T manipulabilityThreshold # w₀; 0 ⇒ constant damping λ₀ (adaptive otherwise) + T characteristicLength # ρ (m): W = diag(1,1,1, ρ,ρ,ρ) makes rad commensurate with m + T tolerance # on ‖W·e‖ (m) + T maxStep # Δmax on ‖Δq‖; +∞ disables std::size_t maxIterations + JointVector lowerLimits, upperLimits # ∓∞ disables -template +template struct PoseIkResult: - Vector q - T finalError - std::size_t iterations - bool converged + JointVector q + T finalError # ‖W·PoseError(target, ToolPose(q))‖ at the returned q + std::size_t iterations + bool converged -template +template class PoseInverseKinematics: - DenavitHartenberg fk - SpatialJacobian jac - PoseIkConfig config + const JacobianProvider& arm # Jacobian(q), ToolPose(q); injected + PoseIkConfig config ``` ## Interface -``` -PoseInverseKinematics(model, PoseIkConfig cfg = {}) -PoseIkResult Solve(SE3 target, JointVector q0) # hot path -Vector PoseError(SE3 target, SE3 current) # [Δp; Δω] +```text +PoseInverseKinematics(const JacobianProvider& arm, const PoseIkConfig& config) +PoseIkResult Solve(const SE3Transform& target, const JointVector& q0) const # hot path ``` ## Algorithm (pseudocode) -``` -function PoseError(target, current): - e_p = target.p - current.p # position error - R_err = target.R * Transpose(current.R) # relative rotation - e_ω = AxisAngle(R_err) # log map → rotation vector - # equivalently 2·(vector part of quaternion(R_err)); reuse item 18 - return concat(e_p, e_ω) # 6-vector +```text +function WeightedError(target, q): + e = SE3Transform::PoseError(target, arm.ToolPose(q)) # (Δp; true log-map rotation vector), M6 + return (e.linear; ρ·e.angular) function Solve(target, q0): # OPTIMIZE_FOR_SPEED q = q0 for iter in 0..maxIterations-1: - T = fk.Forward(q) - e = PoseError(target, T) # 6-vector - if norm(e) < tolerance: return { q, norm(e), iter, true } - J = jac.Compute(q) # 6×N - # damped least squares: Δq = Jᵀ (J Jᵀ + λ²I)⁻¹ e - A = J * Transpose(J) + λ² * I₆ # 6×6 SPD - y = GaussianElimination(A, e) # solve A y = e (reuse solver) - q = q + Transpose(J) * y - return { q, norm(e), maxIterations, false } + 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 λ₀² + w = LuDecomposition(A0) singular ? 0 : sqrt(max(0, Determinant(A0))) # weighted manipulability + # (≡ 0 when Dof < 6 or near-singular ⇒ λ = λ₀) + return w < w₀ ? λ₀²·(1 − (w / w₀)²) : 0 ``` ## Complexity & memory -- Per iteration: `O(6²N)` to form `J Jᵀ` plus `O(6³)` for the 6×6 solve. -- Total: `O(iters · (6²N + 6³))`; iteration count depends on `λ` and start pose. -- Memory: one `6×N` Jacobian and a `6×6` system; no heap, no dynamic sizing. +- 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. +- Memory: one `6×Dof` Jacobian and a `6×6` system; stack only. ## Numerical / embedded notes -- **Damping `λ`** trades accuracy for stability: it keeps `(J Jᵀ + λ²I)` invertible *through* - singularities (Nakamura's singularity-robust inverse) at the cost of a small steady-state error. -- Orientation error **must** use the log/quaternion form — naive Euler-angle subtraction wraps and - stalls; reuse `Quaternion` (item 18) and fix the double-cover sign so the arm takes the short way. -- Solve `A y = e` with `GaussianElimination`; never form `(J Jᵀ)⁻¹` explicitly. +- **Damping does not bias the answer:** at a fixed point `JWᵀ·y = 0` with `JW` of full row rank forces + `y = 0`, hence `e = 0`. For a reachable, non-singular target iterative DLS converges to the exact pose; + `λ` only slows convergence (linear instead of quadratic). The residual is non-zero only for + unreachable targets, at singularities (DLS returns the damped least-squares compromise) or when a + joint limit is active. +- **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. + `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 + `λ₀` as `w → 0`. Step clamping bounds the joint jump per iteration; joint limits are enforced by + clamping (projected iteration). +- Upstream `LuDecomposition` reports singular below a relative pivot of `1e-6`; keep `λ₀² ≫ 1e-6·‖W·J‖²` + (assert `λ₀ > 0`) so the damped system never trips it — a tripped `A0` simply means `w = 0`, `λ = λ₀`. - Seed `q0` from the previous control cycle for warm-started, few-iteration convergence. -- 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/kinematics/PoseInverseKinematics.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class PoseInverseKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/PoseInverseKinematics.cpp` → `template class PoseInverseKinematics;` + `extern template class PoseInverseKinematics;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/PoseInverseKinematics.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestPoseInverseKinematics.cpp` - 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`. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/tests.md b/roadmap/kinematics/PoseInverseKinematics/tests.md index 23a79b1..23e6272 100644 --- a/roadmap/kinematics/PoseInverseKinematics/tests.md +++ b/roadmap/kinematics/PoseInverseKinematics/tests.md @@ -4,57 +4,70 @@ ## Fixture -``` -class TestPoseIk : public ::testing::Test: - DenavitHartenberg arm{ ... } # 6R with spherical wrist - SpatialJacobian jac{ arm } - PoseInverseKinematics ik{ arm, { λ=0.05, tol=1e-4, 100 } } -# each case below is a TEST_F(TestPoseIk, ) +```cpp +class TestPoseInverseKinematics : public ::testing::Test: + DenavitHartenberg puma{ pumaTable, Standard } # PUMA-560, see M7 tests + DhTaskJacobian arm{ puma } + StrictMock> mock # scripted J and ToolPose + PoseIkConfig config{ λ₀ = 0.05, w₀ = 0, ρ = 0.3, tol = 1e-4, maxStep = ∞, 100, ∓∞ } + JointVector qTrue{ 0.3, −0.4, 0.5, 0.6, −0.7, 0.8 } +# each case below is a TEST_F(TestPoseInverseKinematics, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```cpp round_trip_reachable_pose: - Arrange: q_true random; target = FK(q_true) - Act: r = ik.Solve(target, q_true + small_perturbation) - Assert: r.converged; FK(r.q) ≈ target (position + orientation) - -position_and_orientation_both_met: - Assert: PoseError(target, FK(r.q)) < tol in all 6 components - -converges_from_nearby_seed: - Assert: iterations small when q0 near solution (warm start) - -pure_orientation_change: - Arrange: target = same position, rotated tool - Assert: solver reorients without large base-joint motion - -damping_survives_singularity: - Arrange: target forcing a wrist singularity - Assert: Δq stays bounded; no NaN / Inf - -unreachable_target_fails_gracefully: - Arrange: target beyond arm reach - Assert: r.converged == false, finalError > tol, q finite - -orientation_error_takes_short_path: - Arrange: target rotation of +190° about ẑ - Assert: ‖e_ω‖ ≈ 170° (short way), not 190° - + Arrange: target = puma.Forward(qTrue); q0 = qTrue + 0.1 + Act: r = PoseInverseKinematics{ arm, config }.Solve(target, q0) + Assert: r.converged; r.iterations ≤ 10; ‖W·PoseError(target, puma.Forward(r.q))‖ < 1e-4 +damping_slows_but_does_not_bias: + Arrange: λ₀ = 0.2, tol = 1e-5, maxIterations = 200 + Assert: r.converged; ‖r.q − qTrue‖∞ < 1e-3; r.iterations > the λ₀ = 0.05 count zero_error_is_immediate: - Arrange: target = FK(q0) - Assert: iterations == 0, converged + Assert: Solve(puma.Forward(q0), q0) → iterations == 0, converged +final_error_is_measured_after_the_last_update: + Arrange: maxIterations = 1, λ₀ = 0, ρ = 1; target = { Rz(0.1), (0.1, 0, 0) } + mock.ToolPose → Identity (1st call), { Rz(0.08), (0.08, 0, 0) } (2nd call); mock.Jacobian → I₆ + Assert: r.q ≈ q0 + (0.1,0,0,0,0,0.1); r.finalError ≈ 0.028284 (not 0.141421); converged == false +rotation_rows_are_weighted_in_the_step: + Arrange: maxIterations = 1, λ₀ = 0.1, ρ = 0.1, target = { Rz(0.1), (0.1, 0, 0) }; + mock.ToolPose → Identity (twice); mock.Jacobian → I₆ + Assert: r.q − q0 ≈ (0.0990099, 0, 0, 0, 0, 0.05) [= wᵢ²/(wᵢ² + λ²)·eᵢ]; r.finalError ≈ 0.100499 +adaptive_damping_vanishes_away_from_singularity: + Arrange: w₀ = 0.5, λ₀ = 0.1, ρ = 1, maxIterations = 1; mock.Jacobian → I₆ (w = 1) + Assert: Δq == e exactly (λ = 0) +adaptive_damping_engages_near_singularity: + Arrange: as above, mock.Jacobian → diag(1,1,1,1,1,0.1) (w = 0.1 ⇒ λ² = 0.0096), e = 0.01·𝟙 + Assert: Δq ≈ (0.0099049 ×5, 0.0510204) +step_is_clamped: + Arrange: maxStep = 0.1, mock J = I₆, e = (0.3, 0.4, 0, 0, 0, 0) + Assert: Δq ≈ (0.06, 0.08, 0, 0, 0, 0) +joint_limits_are_enforced: + Arrange: upperLimits[0] = q0[0] + 0.05, mock J = I₆, e = (0.3, 0, 0, 0, 0, 0) + Assert: r.q[0] == upperLimits[0] +damping_survives_wrist_singularity: + Arrange: q0 = (0.3, −0.4, 0.5, 0.6, 0, 0.8) (θ₅ = 0: wrist axes aligned, σ_min ≈ 2e-9); + target = puma.Forward((0.3, −0.4, 0.5, 0.9, 0.3, 0.5)); w₀ = 1e-3, maxStep = 0.2 + Assert: r.converged; every r.q component finite +unreachable_target_fails_gracefully: + Arrange: target.p = (3, 0, 0) (beyond reach ≈ 0.9 m) + Assert: !r.converged; r.finalError > tol; q finite ``` ## Reference vectors -- Reachable `target = FK(q_true)` ⇒ recovered `q` with `‖e‖ < 10⁻⁴`. -- Orientation error of `R` vs `R·Rot(ẑ, 190°)` ⇒ `‖e_ω‖ ≈ 170° = 2.967 rad ≤ π`. +- PUMA-560 at `qTrue`: tool `p = (0.402407, −0.032586, 0.263519)`; from `qTrue + 0.1` (double): + `λ = 0.05` reaches `1e-4` in 3 iterations; `λ = 0` / `0.05` / `0.2` reach `‖W e‖ < 1e-12` in 4 / 12 / + 64 iterations, all to `qTrue` within `1e-11` (damping is unbiased). +- Weighted manipulability `√det(W J Jᵀ W)` at `qTrue` with `ρ = 0.3` is `1.09e-3` — pick `w₀` per arm + 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). ## Edge cases -- Wrist singularity (aligned axes) ⇒ damping keeps the solve finite. -- Target exactly at reach boundary ⇒ slow but bounded convergence. -- Antipodal orientation (180°) ⇒ axis ambiguous; document the tie-break. -- Quaternion double cover ⇒ `q` and `−q` give the same error direction. +- Antipodal orientation (180°) ⇒ `PoseError` axis ambiguous (either sign valid, M6). +- 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 f48a673..884ed93 100644 --- a/roadmap/kinematics/ProductOfExponentials/explanation.md +++ b/roadmap/kinematics/ProductOfExponentials/explanation.md @@ -1,32 +1,35 @@ # Product of Exponentials — Overview ## What it is -A forward-kinematics formula built from screw theory: `T = e^{[S₁]θ₁}···e^{[Sₙ]θₙ}·M`. Each joint is a -**screw axis** `Sᵢ` fixed in the base frame; turning the joint by `θᵢ` applies the matrix exponential of +A forward-kinematics formula built from screw theory: `T = e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M`. Each joint is a +**screw axis** `Sᵢ` written once in the base frame; moving the joint by `qᵢ` applies the exponential of that screw, and `M` is the tool's pose when every joint is at zero. ## Why it matters (embedded) -PoE describes an arm with a single home pose `M` and one screw axis per joint — all in the *base* -frame — instead of a chain of intermediate DH frames. There is nothing to line up between links, so it -is less error-prone to author, and the same screw axes give the Jacobian almost for free. For code that -already has an `SE(3)` type, it is the most direct FK you can write. +PoE describes an arm with one home pose and one screw per joint — all in the *base* frame — instead of +a chain of intermediate DH frames. There is nothing to line up between links, so it is less +error-prone to author from a CAD model, and the same screws give the Jacobian and the frame chain in +the same pass. With an SE(3) type already in the library it is the most direct FK to write. ## How it works (intuition) -A screw axis packages "rotate about this line while sliding along it" into one 6-vector. Its matrix -exponential is the finite rigid motion produced by riding that screw for a parameter `θ` — the -rigid-body version of Rodrigues' rotation formula. Forward kinematics is then just: start at the home -pose, and for each joint multiply in the motion its screw produces. Because every axis is written in -the fixed base frame, you never rebuild intermediate frames; you only compose `SE(3)` exponentials. +A screw axis packages "rotate about this line while sliding along it" into one 6-vector: the unit +direction `ω` of the line and the velocity `v = −ω × a` that a point at the base origin would have when +the body spins about the line through `a`. Its exponential is the finite rigid motion produced by +riding that screw for `qᵢ`. Forward kinematics starts at the home pose and multiplies in each joint's +motion. Carrying each screw through the motions of the joints before it gives the "space Jacobian", +whose linear part is the velocity of the point at the base origin; shifting that reference point to +the tool gives the geometric Jacobian used everywhere else in the library. ## Key parameters -- **screw axes `Sᵢ = (ωᵢ; vᵢ)`** — one per joint, expressed in the base frame. +- **screw axes `Sᵢ = (vᵢ; ωᵢ)`** — one per joint, base frame, at `q = 0`. - **home configuration `M`** — the tool pose at `q = 0`. -- **space vs body form** — whether the screws are read in the fixed or the tool frame. ## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 4 (forward kinematics via the product of -exponentials). +K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 4–5 (PoE and the space Jacobian; the book +orders screws `(ω; v)`, this library `(v; ω)`); R. W. Brockett, "Robotic Manipulators and the Product +of Exponentials Formula," 1984. ## See also -`SE3Transform` (M6, the `Exp` and adjoint), `MatrixExponential` (item 29, the `se(3)→SE(3)` map), -`DenavitHartenberg` (M7, the frame-based alternative), `SpatialJacobian` (M8). +`SE3Transform` (M6, `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 15f3ba5..81e01bb 100644 --- a/roadmap/kinematics/ProductOfExponentials/implementation.md +++ b/roadmap/kinematics/ProductOfExponentials/implementation.md @@ -2,68 +2,92 @@ > 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. + ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float class ProductOfExponentials: - std::array, NumLinks> screws # space-frame screw axes Sᵢ = (ωᵢ; vᵢ) - SE3 home # M: tool pose at q = 0 - bool bodyForm = false # space vs body PoE + std::array, N> screws # space-frame screw axes Sᵢ = (vᵢ; ωᵢ) at q = 0 + SE3Transform home # M: tool pose at q = 0 ``` +Screw construction (asserted at construction): +- revolute about unit `ω` through point `a`: `S = (−ω × a; ω)` (pitch 0 ⇒ `ω·v = 0`); +- prismatic along unit `v`: `S = (v; 0)`. + ## Interface -``` -ProductOfExponentials(std::array, N> screws, SE3 home) -SE3 Compute(JointVector q) # ⁰Tₙ; hot path -Matrix SpaceJacobian(JointVector q) # columns = Adⱼ·Sᵢ +```cpp +ProductOfExponentials(const std::array, N>& screws, const SE3Transform& home) +SE3Transform Compute(const JointVector& q) const # tool pose; hot path +Matrix SpaceJacobian(const JointVector& q) const # Lynch–Park space Jacobian, (v; ω) rows +FrameChain Frames(const JointVector& q) const # M30 frame chain for M8 +static Matrix SpaceToGeometric(const Matrix& Js, const Vector3& pTool) ``` ## Algorithm (pseudocode) -``` -function Compute(q): # OPTIMIZE_FOR_SPEED (space form) - T = Identity - for i in 0..N-1: - T = T * SE3::Exp(screws[i], q[i]) # e^{[Sᵢ]qᵢ} (reuse M6 / item 29) - return T * home # T = e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M +```text +function Compute(q): # OPTIMIZE_FOR_SPEED + A = Identity + for i in 0..N-1: A = A * SE3Transform::Exp(screws[i], q[i]) # M6 + return A * home # e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M function SpaceJacobian(q): - Js[0] = screws[0] - T = Identity - for i in 1..N-1: - T = T * SE3::Exp(screws[i-1], q[i-1]) - Js[i] = Adjoint(T) * screws[i] # transform screw into the current frame + A = Identity + for i in 0..N-1: + column i = A.Adjoint() · screws[i] # Ad(e^{[S₁]q₁}···e^{[Sᵢ₋₁]qᵢ₋₁})·Sᵢ, Ad = [[R, Skew(p)R],[0, R]] + A = A * SE3Transform::Exp(screws[i], q[i]) return Js + +function Frames(q): # same loop, one pass + A = Identity + for i in 0..N-1: + (v; ω) = A.Adjoint() · screws[i] # current screw of joint i in the base frame + if ‖ω‖ > 0: jointAxes[i] = ω/‖ω‖; jointOrigins[i] = (ω × v)/‖ω‖²; jointTypes[i] = Revolute + # = foot of the perpendicular from the base origin to the axis + else: jointAxes[i] = v; jointOrigins[i] = A.p; jointTypes[i] = Prismatic + A = A * SE3Transform::Exp(screws[i], q[i]) + frames.tool = A * home + return frames + +function SpaceToGeometric(Js, pTool): # J_geo = [[I, −Skew(pTool)], [0, I]] · J_space + for each column (v; ω): (v − Skew(pTool)·ω ; ω) = (v + ω × pTool ; ω) ``` ## Complexity & memory -- `Compute`: `O(N)` — one `SE(3)` exponential and one compose per joint. -- `SpaceJacobian`: `O(N)` — one adjoint (6×6) per column, reusing the running product. -- Memory: the `N` screw axes plus `M` and one accumulator; `O(1)` working set, no heap. +- `Compute`: `O(N)` — one `Exp` and one compose per joint. +- `SpaceJacobian` / `Frames`: `O(N)` — one adjoint–vector product per joint, reusing the running product. +- Memory: `N` screws + `M` + one accumulator; stack only. ## Numerical / embedded notes -- PoE needs **no per-link frames** — every screw axis is expressed once in the fixed base frame, so - there is no DH bookkeeping and no accumulated convention error (its main advantage over M7). -- Each `SE3::Exp` uses the closed-form `se(3)` exponential (Rodrigues + left Jacobian); guard the - `θ → 0` case with a series expansion to avoid `sinθ/θ` division (reuse item 29 / M6). -- **Space vs body** form differ only in factor order and where `M` sits — expose the flag; space - Jacobian columns are `Adⱼ·Sᵢ`, body Jacobian columns are `Adⱼ⁻¹·Bᵢ`. -- Normalize each `ωᵢ` to unit length (or zero for a prismatic screw) so `qᵢ` is a true angle/length. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Which Jacobian:** the space Jacobian's linear part is the velocity of the (virtual) body point that + currently coincides with the **base origin**, not of the tool. The geometric Jacobian (M8) — tool-point + 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; + 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`. +- Upstream `math::MatrixExponential` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)) + on the 4×4 `[S]θ` is a valid cross-check of `Exp` in tests, not a runtime dependency. +- Float-only: `static_assert(std::is_floating_point_v)`. ## Deployment - Header: `robotics/kinematics/ProductOfExponentials.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ProductOfExponentials;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ProductOfExponentials.cpp` → `template class ProductOfExponentials;` + `extern template class ProductOfExponentials;` / `` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/ProductOfExponentials.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestProductOfExponentials.cpp` - 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`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ProductOfExponentials/tests.md b/roadmap/kinematics/ProductOfExponentials/tests.md index 4855d81..57937ef 100644 --- a/roadmap/kinematics/ProductOfExponentials/tests.md +++ b/roadmap/kinematics/ProductOfExponentials/tests.md @@ -4,54 +4,55 @@ ## Fixture -``` -class TestPoE : public ::testing::Test: - # 2-link unit planar arm as screws about ẑ at x = 0 and x = 1 - std::array, 2> screws = { (ẑ; 0,0,0), (ẑ; 0,-1,0) } - SE3 home = SE3(I, (2,0,0)) - ProductOfExponentials poe{ screws, home } -# each case below is a TEST_F(TestPoE, ) +```cpp +class TestProductOfExponentials : public ::testing::Test: + # 2-link unit planar arm: revolute about ẑ through (0,0,0) and through (1,0,0); screws (v; ω) + std::array, 2> screws{ ((0,0,0); ẑ), ((0,−1,0); ẑ) } + ProductOfExponentials poe{ screws, SE3Transform{ I, (2, 0, 0) } } + # UR5 (Lynch–Park Ex. 4.5, lengths W1=0.109 W2=0.082 L1=0.425 L2=0.392 H1=0.089 H2=0.095), (v; ω): + # ((0,0,0);ẑ) ((−H1,0,0);ŷ) ((−H1,0,L1);ŷ) ((−H1,0,L1+L2);ŷ) ((−W1,L1+L2,0);−ẑ) ((H2−H1,0,L1+L2);ŷ) + # M = { [[−1,0,0],[0,0,1],[0,1,0]], (L1+L2, W1+W2, H1−H2) } + ProductOfExponentials ur5{ ... } +# each case below is a TEST_F(TestProductOfExponentials, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```text zero_config_returns_home: - Assert: poe.Compute({0, 0}) ≈ home (tip at (2,0,0)) - + Assert: poe.Compute({0, 0}) ≈ home (tip (2, 0, 0)); ur5.Compute(0).p ≈ (0.817, 0.191, −0.006) single_joint_rotation: - Act: T = poe.Compute({π/2, 0}) - Assert: T.p ≈ (0, 2, 0) - -elbow_rotation_matches_dh: - Act: T = poe.Compute({0, π/2}) - Assert: T.p ≈ (1, 1, 0) - + Assert: poe.Compute({π/2, 0}).p ≈ (0, 2, 0) +elbow_rotation: + Assert: poe.Compute({0, π/2}).p ≈ (1, 1, 0) agrees_with_denavit_hartenberg: - Assert: poe.Compute(q) ≈ dh.Forward(q) over a sweep of q (same arm) - -space_jacobian_first_column_is_screw: - Assert: SpaceJacobian(q) column 0 == screws[0] - -jacobian_matches_finite_difference: - Assert: SpaceJacobian(q)·q̇ ≈ twist from (Compute(q + εq̇) ⊖ Compute(q)) / ε - + Assert: poe.Compute(q) ≈ DH 2-link unit arm Forward(q) for q ∈ {(0.4,−0.7), (1.3,2.1)} +space_jacobian_linear_part_is_base_origin_velocity: + Arrange: q = (0.4, −0.7) + Assert: SpaceJacobian(q) = [[0, 0.389418], [0, −0.921061], [0,0], [0,0], [0,0], [1,1]] +space_to_geometric_matches_frame_chain_jacobian: + Arrange: ur5, q = (0.3,−0.5,0.8,−0.2,0.6,1.1) + Assert: SpaceToGeometric(SpaceJacobian(q), Compute(q).p) ≈ GeometricJacobian::Compute(Frames(q)) +geometric_jacobian_matches_finite_difference: + Arrange: ur5, q as above, q̇ = (0.2,−0.4,0.3,0.5,−0.1,0.7), ε = 1e-3 + Assert: J_geo·q̇ ≈ (Δp; Log(ΔR)) / ε (tol 1e-3) +frames_place_joint_origins_on_the_axes: + Assert: poe.Frames({0.4, −0.7}).jointOrigins ≈ {(0,0,0), (0.921061, 0.389418, 0)}, axes = ẑ prismatic_screw_translates: - Arrange: screw with ω = 0, v = x̂ - Assert: Compute({s}) translates the tool by s·x̂ - -exp_zero_angle_is_identity: - Assert: SE3::Exp(S, 0) ≈ Identity for each screw + Arrange: ProductOfExponentials{ { ((1,0,0); 0) }, Identity } + Assert: Compute({0.3}).p ≈ (0.3, 0, 0); Frames type Prismatic, axis x̂ ``` ## Reference vectors -- 2-link unit arm, `q = (0, π/2)` ⇒ tip `(1, 1, 0)` (identical to the DH result). -- `q = (π/2, 0)` ⇒ tip `(0, 2, 0)`. +- Planar arm: `q = (π/2, 0)` ⇒ `(0, 2, 0)`; `q = (0, π/2)` ⇒ `(1, 1, 0)`; `q = (0.4, −0.7)` ⇒ + `(1.876397, 0.093898, 0)`, `J_geo = [[−0.093898, 0.295520], [1.876397, 0.955336], 0, 0, 0, [1, 1]]`. +- UR5 at the test `q`: tool `p = (0.696820, 0.400489, 0.077764)`; + `J_space·q̇ = (−0.170343, −0.064298, 0.868026; 0.096307, 1.053237, 0.260041)`, + `J_geo·q̇ = (−0.192582, 0.109415, 0.172680; 0.096307, 1.053237, 0.260041)`. ## Edge cases -- `q → 0` per joint ⇒ series fallback in `Exp`, no `sinθ/θ` blow-up. -- Prismatic screw (`ω = 0`) ⇒ pure translation, `Exp` skips the rotation branch. -- Non-unit `ω` fed in ⇒ document the normalization expectation. -- Long chain ⇒ compose order fixed (space form left-to-right) to match `M`. +- `q = 0` ⇒ `Exp` returns identity, `Compute = M`. +- Prismatic screw (`ω = 0`) ⇒ pure translation branch of `Exp`. +- Pitched screw (`ω·v ≠ 0`) ⇒ rejected at construction (`really_assert`). diff --git a/roadmap/kinematics/RedundancyResolution/explanation.md b/roadmap/kinematics/RedundancyResolution/explanation.md index ffefef6..7052850 100644 --- a/roadmap/kinematics/RedundancyResolution/explanation.md +++ b/roadmap/kinematics/RedundancyResolution/explanation.md @@ -1,34 +1,38 @@ # Redundancy Resolution — Overview ## What it is -A way to use a robot that has *more* joints than the task needs. A 7-DOF arm reaching a 6-DOF pose has -one spare degree of freedom; redundancy resolution splits the joint motion into a **primary** part that -achieves the task and a **secondary** part, living in the Jacobian's null space, that pursues an extra -goal without disturbing the tool. +A way to use a robot that has *more* joints than the task needs. A 7-DOF arm holding a 6-DOF pose — or +any arm tracking only a 3-D position — has spare freedom; redundancy resolution splits the joint motion +into a **primary** part that achieves the task and a **secondary** part, living in the Jacobian's null +space, that pursues an extra goal without disturbing the tool. ## Why it matters (embedded) -Extra DOF are what let a robot dodge its own joint limits, avoid obstacles, or stay away from -singularities *while still doing the job*. The formula `q̇ = J⁺ẋ + (I − J⁺J)q̇₀` packages that into one -matrix expression cheap enough to run every control tick — the difference between an arm that jams at a -joint stop and one that gracefully reconfigures around it. +Extra joints let a robot dodge its own joint limits, avoid obstacles, or stay away from singularities +*while still doing the job*. The formula `q̇ = J⁺ẋ + (I − J⁺J)q̇₀` is cheap enough to run every control +tick — the difference between an arm that jams at a joint stop and one that reconfigures around it. ## How it works (intuition) -The pseudo-inverse `J⁺` gives the smallest joint motion that produces the desired tool velocity — the -primary term. The projector `I − J⁺J` filters *any* vector down to the component the tool cannot -feel: push the joints with it and the end-effector holds still. So you pick a secondary joint velocity -`q̇₀` — typically the downhill direction of some cost like "distance from joint limits" or -"manipulability" — run it through the projector, and add it on. The task is untouched; the spare +Factor the Jacobian into singular directions. The primary term inverts it along each direction it can +move the tool in — with a little damping so that a nearly-lost direction does not demand huge joint +rates — which gives the smallest joint motion producing (almost exactly) the desired tool velocity. +The projector removes from any secondary joint velocity every component the tool can feel; it must be +built from the *undamped*, rank-thresholded factorization, because a damped projector leaks a small +amount of motion into the task. Pick a secondary velocity — typically the downhill direction of a cost +such as distance to joint limits — project it, and add it on: the tool does not notice, and the spare freedom does useful work. ## Key parameters -- **task rate `ẋ`** — the desired 6-vector end-effector velocity (primary objective). -- **secondary rate `q̇₀`** — gradient of the objective the null space should optimize. -- **damping λ** — singularity-robustness of the pseudo-inverse. +- **task rate `ẋ`** and **task dimension** (position only or full pose). +- **secondary rate `q̇₀`** — gradient of the objective optimized in the null space. +- **damping λ** — singularity robustness of the primary term (task error `≈ λ²/σ_min²`). +- **rank tolerance** — which singular values count as lost directions. ## Reference A. Liégeois, "Automatic Supervisory Control of the Configuration and Behavior of Multibody -Mechanisms," *IEEE Trans. Systems, Man, and Cybernetics*, 7(12), 1977. +Mechanisms," *IEEE Trans. Systems, Man, and Cybernetics*, 7(12), 1977; S. Chiaverini, "Singularity-Robust +Task-Priority Redundancy Resolution," *IEEE Trans. Robotics and Automation*, 13(3), 1997. ## See also -`SpatialJacobian` (M8, supplies `J`), `SingularValueDecomposition` / `QrDecomposition` (items 43/27, -robust `J⁺`), `ManipulabilityIndex` (M11, a common secondary objective). +`JacobianProvider` (M8, supplies `J`), `SingularValueDecomposition` +([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)), `ManipulabilityIndex` +(M11, a common secondary objective), `PoseInverseKinematics` (M13). diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md index 9459ed6..0f07afb 100644 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ b/roadmap/kinematics/RedundancyResolution/implementation.md @@ -4,71 +4,81 @@ ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class RedundancyResolution: # NumLinks > 6 (redundant) - SpatialJacobian jac - T damping # λ for the damped pseudo-inverse - # scratch: JJt (6×6), Jpinv (N×6), P (N×N) +```cpp +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ᵀ (upstream needs Rows ≥ Cols) ``` ## Interface -``` -RedundancyResolution(SpatialJacobian jac, T damping = 0) -Matrix PseudoInverse(JointVector q) # J⁺ -Matrix NullSpaceProjector(JointVector q) # I − J⁺J -JointVector Resolve(JointVector q, Vector xdot, - JointVector qdot0) # hot path +```text +RedundancyResolution(const JacobianProvider& jacobian, T damping, T rankTolerance = 1e-4) +JointVector Resolve(const JointVector& q, const TaskVector& xDot, + const JointVector& qDot0) # hot path +Matrix DampedPseudoInverse(const JointVector& q) # J⁺_λ (λ = 0 ⇒ rank-thresholded J⁺) +Matrix NullSpaceProjector(const JointVector& q) # P = I − J⁺J, undamped ``` ## Algorithm (pseudocode) -``` -function PseudoInverse(q): # right inverse, wide J - J = jac.Compute(q) # 6×N - JJt = J * Transpose(J) + λ² I₆ # damped ⇒ singularity-robust - # J⁺ = Jᵀ (JJt)⁻¹ via 6×6 solves (reuse GaussianElimination / QR item 27) - return Transpose(J) * Inverse(JJt) - -function NullSpaceProjector(q): - J = jac.Compute(q) - Jp = PseudoInverse(q) - return I_N - Jp * J # N×N, idempotent - -function Resolve(q, xdot, qdot0): # OPTIMIZE_FOR_SPEED - Jp = PseudoInverse(q) - qdot_task = Jp * xdot # minimum-norm primary solution - P = I_N - Jp * jac.Compute(q) # null-space projector - return qdot_task + P * qdot0 # secondary objective in the null space +```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) + τ = rankTolerance · σ₀; r = #{ σᵢ > τ } + +function Gain(σ): # damped inverse of one singular value + λ > 0 ? σ / (σ² + λ²) : (σ > τ ? 1/σ : 0) + +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)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. +- **Why the projector is undamped:** with the damped inverse, `J·(I − J⁺_λJ) = λ²(JJᵀ + λ²I)⁻¹J ≠ 0`, so + `I − J⁺_λJ` is neither idempotent nor task-invisible (7-DOF fixture, `λ = 0.01`: `‖J·P_λ·q̇₀‖ = 2.8e-4`, + `‖P_λ² − P_λ‖ = 2.0e-3`). Built from the thresholded SVD, `P` is an exact orthogonal projector: + `P² = P`, `Pᵀ = P`, `J·P = Σ_{σᵢ ≤ τ} σᵢvᵢuᵢᵀ` (zero for exact rank loss, rounding-level at full rank). +- **What the tool sees:** `J·q̇ = Σᵢ σᵢ·Gain(σᵢ)·vᵢvᵢᵀ·ẋ`. 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. +- 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 - Header: `robotics/kinematics/RedundancyResolution.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Resolve`, and - `extern template class RedundancyResolution;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/RedundancyResolution.cpp` → `template class RedundancyResolution;` + `extern template class RedundancyResolution;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/kinematics/RedundancyResolution.cpp` → the same instantiations. - Test: `robotics/kinematics/test/TestRedundancyResolution.cpp` - Doc: `doc/kinematics/RedundancyResolution.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestRedundancyResolution.cpp` → the `_test` target. +- Depends on: M8 (`JacobianProvider`); upstream `solvers::SingularValueDecomposition`. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/RedundancyResolution/tests.md b/roadmap/kinematics/RedundancyResolution/tests.md index 2699318..f6d7dad 100644 --- a/roadmap/kinematics/RedundancyResolution/tests.md +++ b/roadmap/kinematics/RedundancyResolution/tests.md @@ -4,56 +4,62 @@ ## Fixture -``` -class TestRedundancy : public ::testing::Test: - DenavitHartenberg arm{ ... } # 7-DOF redundant arm - SpatialJacobian jac{ arm } - RedundancyResolution rr{ jac, 0.01f } -# each case below is a TEST_F(TestRedundancy, ) +```cpp +class TestRedundancyResolution : public ::testing::Test: + # 7R arm (iiwa-like), standard DH (a, α, d): (0,−π/2,0.34) (0,π/2,0) (0,π/2,0.4) (0,−π/2,0) + # (0,−π/2,0.4) (0,π/2,0) (0,0,0.126) + DenavitHartenberg arm{ iiwaTable, Standard } + DhTaskJacobian pose{ arm } + DhTaskJacobian position{ arm } + JointVector q { 0.1, 0.5, −0.3, −1.2, 0.4, 0.8, −0.2 } + TaskVector xDot{ 0.1, −0.05, 0.02, 0.1, 0.2, −0.1 } + JointVector qDot0{ 1, −1, 0.5, 0.3, −0.2, 0.7, −0.4 } +# each case below is a TEST_F(TestRedundancyResolution, ) ``` ## Test cases (Arrange / Act / Assert) -``` -primary_task_is_satisfied: - Arrange: desired ẋ; qdot0 = 0 - Act: q̇ = rr.Resolve(q, ẋ, 0) - Assert: jac.Compute(q) * q̇ ≈ ẋ - -null_space_motion_is_task_invisible: - Arrange: arbitrary qdot0 - Act: q̇ = P * qdot0 (task rate zero) - Assert: jac.Compute(q) * q̇ ≈ 0 - -projector_is_idempotent: - Assert: P * P ≈ P - -minimum_norm_primary_solution: - Assert: ‖J⁺ẋ‖ ≤ ‖any other q̇ solving Jq̇ = ẋ‖ - -secondary_objective_reduces_cost: - Arrange: qdot0 = -∇(joint-limit cost) - Assert: cost decreases along q̇ while task still met - -pseudo_inverse_right_identity: - Assert: J * J⁺ ≈ I₆ (right inverse for wide J) - -damping_bounds_near_singularity: - Arrange: near-singular q - Assert: ‖J⁺‖ stays finite - +```cpp +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: + Arrange: λ = 0.01 + Assert: ‖J·Resolve(q, xDot, qDot0) − xDot‖ ≤ λ²/σ_min²·‖xDot‖ = 8.5e-4 (observed 4.4e-4) +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) + Assert: ∇H·q̇ ≈ −0.0051809 (< 0); ‖J·q̇‖ ≈ 0 +position_task_has_four_dimensional_null_space: + Arrange: RedundancyResolution{ position, λ = 0 } + 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: Resolve(q, 0, 0) == 0 ``` ## Reference vectors -- Wide `J` (`6×7`): `J·J⁺ ≈ I₆`, but `J⁺·J ≠ I₇` (rank 6). -- Null-space rate: `J·(P q̇₀) = 0` for any `q̇₀`. +- 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. +- Position rows only: `σ = (0.839288, 0.805624, 0.257020)`, `trace P = 4`. ## Edge cases -- Non-redundant arm (`N = 6`) ⇒ `P ≈ 0`, no null space. -- Fully singular `J` ⇒ damping prevents blow-up, task partially met. -- Conflicting secondary objective ⇒ primary task still exact, secondary compromised. -- Over-scaled `q̇₀` ⇒ document that the projector still protects the task. +- `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/kinematics/SE3Transform/explanation.md b/roadmap/kinematics/SE3Transform/explanation.md new file mode 100644 index 0000000..5fe5037 --- /dev/null +++ b/roadmap/kinematics/SE3Transform/explanation.md @@ -0,0 +1,36 @@ +# 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 new file mode 100644 index 0000000..2b92c00 --- /dev/null +++ b/roadmap/kinematics/SE3Transform/implementation.md @@ -0,0 +1,112 @@ +# 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` → + `#pragma GCC optimize("O3","fast-math")`, `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 new file mode 100644 index 0000000..9f01231 --- /dev/null +++ b/roadmap/kinematics/SE3Transform/tests.md @@ -0,0 +1,50 @@ +# 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/kinematics/SpatialJacobian/explanation.md b/roadmap/kinematics/SpatialJacobian/explanation.md deleted file mode 100644 index b5bd56a..0000000 --- a/roadmap/kinematics/SpatialJacobian/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Spatial Jacobian — Overview - -## What it is -The Jacobian is the matrix that links how fast the joints move to how fast the end-effector moves. -For an `N`-joint arm it is `6×N`: the top three rows give the tool's linear velocity, the bottom three -its angular velocity, as a linear function of the joint-rate vector `q̇`. - -## Why it matters (embedded) -It is the workhorse of manipulator control. Velocity control, force control, singularity detection, -and every Jacobian-based inverse-kinematics solver need it. Its transpose maps end-effector -forces/torques back to joint torques — so the same matrix does velocity kinematics *and* statics, -exactly the reuse an embedded arm controller wants. - -## How it works (intuition) -Each joint contributes one column. A **revolute** joint spins the tool about its own axis `zᵢ`, so it -adds angular velocity `zᵢ` and linear velocity `zᵢ × r`, where `r` is the lever arm from the joint to -the tool. A **prismatic** joint slides the tool along `zᵢ`, adding pure linear velocity `zᵢ` and no -rotation. Stack those columns and you have the map `twist = J·q̇`. When two columns line up the arm is -**singular** — it has locally lost a direction of motion, and `J` can no longer be inverted safely. - -## Key parameters -- **joint axes `zᵢ`** and **origins `oᵢ`** — read off the forward-kinematics frame chain. -- **joint types** — revolute vs prismatic pick the column formula. -- **twist ordering** — `(v; ω)` (linear first) here; must be consistent everywhere downstream. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 5 (velocity kinematics and the Jacobian). - -## See also -`DenavitHartenberg` (M7) / `ProductOfExponentials` (M15) supply the frames; `ManipulabilityIndex` -(M11) scores it; `PoseInverseKinematics` (M13) and `RedundancyResolution` (M14) invert it. diff --git a/roadmap/kinematics/SpatialJacobian/implementation.md b/roadmap/kinematics/SpatialJacobian/implementation.md deleted file mode 100644 index 9109b6b..0000000 --- a/roadmap/kinematics/SpatialJacobian/implementation.md +++ /dev/null @@ -1,76 +0,0 @@ -# Spatial Jacobian (6×N) — Implementation Pseudocode - -> Roadmap ref: #M8 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SpatialJacobian: - # geometric Jacobian in the base frame: - # Matrix columns = [ linear (3); angular (3) ] - DenavitHartenberg model # or a screw / SE(3) chain -``` - -## Interface - -``` -SpatialJacobian(DenavitHartenberg model) -Matrix Compute(JointVector q) # geometric J; hot path -Vector EndEffectorTwist(JointVector q, JointVector qdot) # J·q̇ -JointVector JointTorques(JointVector q, Vector wrench) # Jᵀ·w (statics) -``` - -## Algorithm (pseudocode) - -``` -function Compute(q): # OPTIMIZE_FOR_SPEED - frames = model.FrameChain(q) # every ⁰Tᵢ (reuse M7/M6) - oₙ = frames[N].p # end-effector origin - for i in 0..N-1: - zᵢ = frames[i].R * ẑ # joint axis in base frame - oᵢ = frames[i].p - if joint i is Revolute: - Jv = CrossProduct(zᵢ, oₙ - oᵢ) # reuse Geometry3D - Jw = zᵢ - else: # Prismatic - Jv = zᵢ - Jw = 0 - column i = concat(Jv, Jw) # 6-vector - return J - -function EndEffectorTwist(q, qdot): - return Compute(q) * qdot # (v; ω) - -function JointTorques(q, w): # OPTIMIZE_FOR_SPEED - return Transpose(Compute(q)) * w # τ = Jᵀ w -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one `FrameChain` pass plus a cross product per column. -- `EndEffectorTwist` / `JointTorques`: `O(6N)` matrix-vector products. -- Memory: one `6×N` matrix plus the `N+1` cached frames; all stack-allocated. - -## Numerical / embedded notes - -- This **promotes** the private 3×N position Jacobian inside `InverseKinematics.hpp` to the full 6×N - form — the single dependency that unblocks pose-IK, manipulability, and redundancy. -- Twist ordering is `(v; ω)` (linear first) here; downstream consumers must match this block layout. -- Near a **singularity** two columns become linearly dependent (rank drops) — detectable via the - manipulability index (M11); do not invert `J` directly, use damped least squares. -- `Jᵀ` gives the statics/force map for free — the transpose needs no recomputation. -- 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/kinematics/SpatialJacobian.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorques`, and - `extern template class SpatialJacobian;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/SpatialJacobian.cpp` → `template class SpatialJacobian;` -- Test: `robotics/kinematics/test/TestSpatialJacobian.cpp` -- Doc: `doc/kinematics/SpatialJacobian.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestSpatialJacobian.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/SpatialJacobian/tests.md b/roadmap/kinematics/SpatialJacobian/tests.md deleted file mode 100644 index 3f25c5c..0000000 --- a/roadmap/kinematics/SpatialJacobian/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# Spatial Jacobian — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSpatialJacobian : public ::testing::Test: - # 2-link unit planar arm, both revolute, in the x-y plane - DenavitHartenberg arm{ ... } - SpatialJacobian jac{ arm } -# each case below is a TEST_F(TestSpatialJacobian, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -planar_arm_column_dimensions: - Act: J = jac.Compute({0, 0}) - Assert: J is 6×2 - -stretched_arm_linear_block: - Arrange: q = (0, 0), tip at (2,0,0) - Act: J = jac.Compute(q) - Assert: Jv column0 ≈ (0, 2, 0), Jv column1 ≈ (0, 1, 0) - -revolute_angular_block_is_axis: - Assert: Jw columns ≈ ẑ for both joints (rotation about z) - -twist_matches_finite_difference: - Arrange: q̇ = (1, 0) - Assert: EndEffectorTwist ≈ (FK(q + εq̇) - FK(q)) / ε - -transpose_maps_wrench_to_torque: - Arrange: unit force f = x̂ at the tip - Assert: JointTorques ≈ hand-computed moment arms - -prismatic_column_is_pure_translation: - Arrange: chain with a prismatic joint - Assert: that column = (axis; 0) - -singular_configuration_drops_rank: - Arrange: fully stretched arm (q = (0,0)) - Assert: columns linearly dependent ⇒ rank < 2 - -zero_config_is_deterministic: - Assert: Compute is repeatable and side-effect free -``` - -## Reference vectors - -- 2-link unit arm at `q = (0,0)`: `Jv = [[0,0],[2,1],[0,0]]`, `Jw = [[0,0],[0,0],[1,1]]`. -- Unit tip force `x̂` on that arm ⇒ joint torques `(0, 0)` (force along the link line). - -## Edge cases - -- Fully stretched / folded arm ⇒ rank-deficient Jacobian (singularity). -- Prismatic-only chain ⇒ zero angular block. -- Single-joint arm ⇒ `6×1` Jacobian, no cross-column coupling. -- Twist ordering `(v; ω)` vs `(ω; v)` ⇒ pin the convention in one assertion. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md index 908d2ea..cd3dc22 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md @@ -1,7 +1,7 @@ # Cartesian Path + Orientation (SLERP) Interpolation — Overview ## What it is -A task-space motion generator: it moves the end-effector along a **straight line** (or screw) in +A task-space motion generator: it moves the end-effector along a **straight line** in Cartesian position while blending orientation with **SLERP** (spherical linear interpolation of unit quaternions). A single scalar progress variable `s(t) ∈ [0, 1]` drives both channels so they start and finish together. @@ -17,16 +17,20 @@ Position is a plain linear blend between the two endpoints. Orientation is trick averaging quaternions leaves the sphere and changes speed. SLERP instead walks the **great-circle arc** on the unit sphere at constant angular rate, giving the shortest, smoothest rotation. Feeding `s(t)` from a trapezoidal or polynomial time law shapes how fast the pose advances along the path. +Because the path is fixed and only `s` moves, the end-effector twist is simply `ṡ` times the path +tangent: linear velocity `ṡ·(p1 − p0)` and angular velocity `ṡ·θ` about the fixed axis of the relative +rotation — a free feed-forward term for task-space controllers. ## Key parameters -- **Start / goal pose** — Cartesian position plus a unit quaternion orientation. -- **Duration `tf`** — total move time. -- **Time scaling `s(t)`** — linear, trapezoidal, or polynomial progress law. +- **Start / goal pose** — rigid transforms (position + rotation); orientation handled as unit quaternions. +- **Time law `s(t)`** — any 0 → 1 profile (polynomial, trapezoidal, S-curve); it fixes the duration and + bounds the linear and angular speeds through `ṡ`. ## Reference K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 9 (trajectory generation); K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH* 1985 (SLERP). ## See also -`Quaternion` (item #18, provides `Slerp`), `SE3Transform` (item #M6), -`TrapezoidalProfile` / `PolynomialTrajectory` (the `s(t)` drivers). +`Quaternion` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) +(provides `Slerp`), `SE3Transform` (M6, 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 e54f27d..794f3d3 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md @@ -4,68 +4,84 @@ ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct Pose: # reuses existing types - math::Vector position # Cartesian point (m) - math::Quaternion orientation # unit quaternion (item #18) +Poses are `kinematics::SE3Transform` (M6); twists are `kinematics::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`. +```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 + +# 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} + +template # static_assert(std::is_floating_point_v); instantiated for float class CartesianSlerpInterpolation: - Pose start, goal - T duration # tf > 0 - T dot, theta, sinTheta # precomputed SLERP geometry - bool useNlerp # near-parallel fallback flag + math::Vector p0, deltaP # start position, goal − start + math::Quaternion q0, q1 # start / goal orientation, q1 sign-fixed (q0·q1 ≥ 0) + math::Vector rotationVector # φ·k of R1·R0ᵀ (base frame), |φ| ≤ π + TimeLaw timeLaw ``` ## Interface -``` -CartesianSlerpInterpolation(Pose start, Pose goal, T tf) -Pose Sample(T t) # hot path -T Duration() -void SetTimeScaling(scaling) # linear, or a Trapezoidal/Polynomial s(t) driver +```text +CartesianSlerpInterpolation(const kinematics::SE3Transform& start, + const kinematics::SE3Transform& goal, const TimeLaw& timeLaw) +CartesianState Sample(T t) # hot path +T Duration() # = timeLaw.Duration() ``` ## Algorithm (pseudocode) -``` -function plan(): # precompute orientation geometry once - if dot(start.q, goal.q) < 0: # pick the shortest of the two arcs - goal.q = -goal.q - dot = clamp(dot(start.q, goal.q), -1, 1) - theta = acos(dot) - useNlerp = (dot > 0.9995) # arc too small ⇒ linear blend + normalize - sinTheta = sin(theta) +```cpp +function plan(start, goal): # orientation geometry once + p0 = start.p; deltaP = goal.p - start.p + q0 = Quaternion::FromRotationMatrix(start.R) + q1 = Quaternion::FromRotationMatrix(goal.R) + if dot4(q0, q1) < 0: q1 = -q1 # shortest of the two arcs (same rotation) + d = q1 * q0.Conjugate() # R1·R0ᵀ, base frame; d.w = dot4(q0, q1) ≥ 0 + n = |(d.x, d.y, d.z)| + if n < ε: rotationVector = 2·(d.x, d.y, d.z) # small angle, no division + else: rotationVector = 2·atan2(n, d.w)·(d.x, d.y, d.z)/n # φ·k, φ = 2·acos(d.w) function Sample(t): # OPTIMIZE_FOR_SPEED - s = timeScaling(clamp(t, 0, duration) / duration) # s ∈ [0, 1] - # position: straight line in Cartesian space - p = start.position + s * (goal.position - start.position) - # orientation: spherical linear interpolation - if useNlerp: - q = normalize( (1 - s)*start.q + s*goal.q ) - else: - w0 = sin((1 - s)*theta) / sinTheta - w1 = sin( s *theta) / sinTheta - q = w0*start.q + w1*goal.q # already unit-length - return Pose{ p, q } + law = timeLaw.Sample(clamp(t, 0, Duration())) + s = clamp(law.position, 0, 1); sDot = law.velocity + p = p0 + s*deltaP # straight line + q = Quaternion::Slerp(q0, q1, s) # great-circle arc; nlerp fallback for dot > 0.9995 upstream + v = sDot * deltaP # linear velocity + ω = sDot * rotationVector # angular velocity ṡ·φ·k, constant axis + return { SE3Transform{ q.ToRotationMatrix(), p }, (v; ω) } ``` +Verified (python3, finite differences of `R(s(t))`): `vee(Ṙ·Rᵀ) = ṡ·φ·k` for arbitrary start/goal, +including the sign-flipped (obtuse) case. + ## Complexity & memory -- Planning: `O(1)` — one `dot`, one `acos`, one `sin`. -- `Sample`: `O(1)` — a 3-vector lerp plus two `sin` (or an nlerp + one normalize). -- Memory: `O(1)` — two poses and a few cached scalars; stack-resident, no heap. +- Planning: `O(1)` — two matrix → quaternion conversions, one quaternion product, one `atan2`. +- `Sample`: `O(1)` — one time-law sample, a 3-vector lerp, one upstream `Slerp` (`acos` + 3 `sin`, or an + nlerp) and one quaternion → matrix conversion. +- Memory: `O(1)` — two quaternions, three 3-vectors and the time law; stack-resident, no heap. ## Numerical / embedded notes -- Reuse `math::Quaternion::Slerp` / `Normalize` (item #18) rather than re-deriving the blend. -- **Shortest-path fix:** negate `goal.q` when `dot < 0`; antipodal quaternions are the same rotation - but the long way round. -- **`nlerp` fallback** when `dot → 1` avoids the `1/sin θ` singularity for near-parallel orientations. -- Decouple position and orientation timing by sharing one scalar `s(t)` from a - `TrapezoidalProfile`/`PolynomialTrajectory`, so both reach the goal simultaneously. +- Reuse upstream `math::Quaternion::Slerp` (shortest-path flip + nlerp fallback built in) rather than + re-deriving the blend. +- **Shortest-path fix:** negate `q1` when `dot < 0` *before* computing `rotationVector`, so the twist + matches the arc `Slerp` actually takes. +- `rotationVector` uses `φ = 2·atan2(n, w)` — well-conditioned near `φ → 0` (where `acos` loses `float` + precision) and finite at `φ = π`; the angular part of `SE3Transform::PoseError(goal, start)` equals it. +- Position and orientation are **coupled** by sharing one scalar `s(t)` from the time law, so both reach + the goal simultaneously; limits of the time law (on `s`) bound `|v| = ṡ·|Δp|` and `|ω| = ṡ·|φ|`. +- `TimeLaw` is a template parameter, not an interface: no virtual call on the hot path. - Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a `Q15`/`Q31` specialisation cheap to add later. @@ -73,13 +89,19 @@ function Sample(t): # OPTIMIZE_FOR_SPEED - Header: `robotics/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class CartesianSlerpInterpolation;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. + `extern template class CartesianSlerpInterpolation>;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Uses `robotics/trajectory/TrajectoryTypes.hpp`, `robotics/kinematics/SE3Transform.hpp` (M6) and + `numerical/math/Quaternion.hpp`; `robotics.trajectory` links `robotics.kinematics`. - Coverage: `robotics/trajectory/CartesianSlerpInterpolation.cpp` → - `template class CartesianSlerpInterpolation;` + `template class CartesianSlerpInterpolation>;` - Test: `robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp` -- Doc: `doc/trajectory/CartesianSlerpInterpolation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestCartesianSlerpInterpolation.cpp` → the `_test` target. -- New module: create `robotics/trajectory/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/trajectory/` folder. +- 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: M6, 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 cb0fe85..73ac2a6 100644 --- a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md +++ b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md @@ -4,54 +4,63 @@ ## Fixture -``` +```cpp class TestCartesianSlerp : public ::testing::Test: - Pose start{ .position = {0,0,0}, .orientation = Identity } - Pose goal { .position = {1,0,0}, .orientation = rot(ẑ, 90°) } - CartesianSlerpInterpolation path{ start, goal, 1.0f } + 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} } + CartesianSlerpInterpolation path{ start, goal, linearLaw } # each case below is a TEST_F(TestCartesianSlerp, ) ``` ## Test cases (Arrange / Act / Assert) -``` +```text endpoints_match_start_and_goal: - Assert: Sample(0) ≈ start and Sample(tf) ≈ goal + Assert: Sample(0).pose ≈ start and Sample(tf).pose ≈ goal (R and p element-wise) position_is_straight_line: - Arrange: goal.position = (1,0,0) - Assert: Sample(tf/2).position ≈ (0.5, 0, 0) + Assert: Sample(tf/2).pose.p ≈ (0.5, 0, 0) -orientation_is_unit_at_midpoint: - Assert: |Sample(tf/2).orientation| ≈ 1 (SLERP preserves unit norm) +rotation_stays_orthonormal: + Assert: Sample(tf/2).pose.R · Sample(tf/2).pose.Rᵀ ≈ I, det ≈ +1 slerp_midpoint_is_half_angle: - Arrange: 90° rotation about ẑ - Assert: Sample(tf/2).orientation ≈ rot(ẑ, 45°) + Assert: Sample(tf/2).pose.R ≈ Rz(45°) + +shortest_path_takes_short_arc: + Arrange: goal.R = Rz(270°) (= Rz(−90°)) + Assert: Sample(tf/2).pose.R ≈ Rz(−45°) and twist ω ≈ (0, 0, −π/2) -shortest_path_negates_goal: - Arrange: goal.q with negative dot to start.q (obtuse) - Assert: interpolation takes the < 180° arc (angle decreases monotonically) +near_parallel_orientation_is_finite: + Arrange: goal.R = Rz(0.01°) + Assert: no NaN/inf in pose or twist; pose.R ≈ I; ω_z ≈ 0.01°·π/180 (ṡ = 1) -near_parallel_uses_nlerp: - Arrange: goal.q within 0.01° of start.q - Assert: no NaN/inf (no 1/sinθ blow-up), result ≈ start.q +constant_angular_rate_for_linear_time_law: + Assert: angle(start.R, Sample(t).pose.R) ≈ t·90° for t ∈ {0.25, 0.5, 0.75} -constant_angular_rate_for_linear_scaling: - Arrange: linear s(t) - Assert: angle(start, Sample(t)) grows linearly in t +twist_is_sdot_times_path_tangent: + Arrange: quintic law { .q0 = 0, .qf = 1 }, tf = 2 ⇒ ṡ(1) = 0.9375 + Assert: Sample(1).twist ≈ (0.9375, 0, 0; 0, 0, 0.9375·π/2 = 1.472622) + i.e. v = ṡ·(p1 − p0), |ω| = ṡ·θ with θ = 90°, axis ẑ; Sample(0).twist ≈ 0 sample_clamps_outside_domain: - Assert: Sample(-1) ≈ start and Sample(tf+1) ≈ goal + Assert: Sample(-1).pose ≈ start and Sample(tf+1).pose ≈ goal ``` ## Reference vectors -- SLERP of Identity → `rot(ẑ, 90°)` at `s = 0.5` ⇒ `rot(ẑ, 45°)`. +- SLERP of `I → Rz(90°)` at `s = 0.5` ⇒ `Rz(45°)` (quaternion `(0.923880, 0, 0, 0.382683)`). - Straight-line position `p(s) = p0 + s(p1 − p0)`; midpoint of `(0,0,0)→(1,0,0)` is `(0.5,0,0)`. +- Twist `(v; ω) = ṡ·(p1 − p0; φ·k)`, `φ·k` = rotation vector of `R1·R0ᵀ`; for `Rz(90°)` and `ṡ = 1.3`, + `ω = (0, 0, 2.042035)` (matches finite differences of `R(s(t))`). +- Rest-to-rest quintic `0 → 1`, `tf = 2`: `ṡ(tf/2) = (15/8)/tf = 0.9375`. ## Edge cases -- Identical start and goal orientation ⇒ constant quaternion, `nlerp` path, no division by zero. -- Antipodal quaternions (`dot ≈ −1`) ⇒ shortest-path negation, well-defined 180° arc. -- Zero-length Cartesian move with pure rotation ⇒ position constant, orientation still slerps. +- Identical start and goal orientation ⇒ constant rotation, `ω = 0`, no division by zero. +- Antipodal quaternions (`dot ≈ −1`, from the sign ambiguity of `FromRotationMatrix`) ⇒ sign fix, + identical rotation, `ω = 0`. +- 180° rotation ⇒ either arc is shortest; `|ω| = ṡ·π`, pose and twist stay consistent. +- Zero-length Cartesian move with pure rotation ⇒ position constant, `v = 0`, orientation still slerps. diff --git a/roadmap/trajectory/CubicSplineTrajectory/explanation.md b/roadmap/trajectory/CubicSplineTrajectory/explanation.md new file mode 100644 index 0000000..d0a9e9a --- /dev/null +++ b/roadmap/trajectory/CubicSplineTrajectory/explanation.md @@ -0,0 +1,37 @@ +# Multi-Joint Cubic Spline Trajectory — Overview + +## 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). + +## 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. + +## 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. + +## Key parameters +- **Knot times `t_k`** — strictly increasing; they set the speed between via points (stretching all of + them by `λ` divides velocities by `λ` and accelerations by `λ²`). +- **Knot positions `q_k`** — one joint vector per via point. +- **End condition** — clamped (start/end velocity, default zero) or natural (zero end acceleration). +- **`MaxPoints`** — compile-time capacity of the bounded storage. + +## Reference +L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 4 +(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). diff --git a/roadmap/trajectory/CubicSplineTrajectory/implementation.md b/roadmap/trajectory/CubicSplineTrajectory/implementation.md new file mode 100644 index 0000000..9925ad1 --- /dev/null +++ b/roadmap/trajectory/CubicSplineTrajectory/implementation.md @@ -0,0 +1,124 @@ +# 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) + +## Data structures + +`JointTrajectoryState` comes from `robotics/trajectory/TrajectoryTypes.hpp` (canonical +definition: PolynomialTrajectory spec → Data structures). + +```cpp +enum class EndCondition : uint8_t { Clamped, Natural } + +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) + +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 +``` + +## Interface + +```cpp +bool Plan(const array& times, const array& positions, + std::size_t count, const 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 +``` + +## 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 } +``` + +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. + +## 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. + +## 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. +- 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. + +## Deployment + +- Header: `robotics/trajectory/CubicSplineTrajectory.hpp` — `#pragma once` → + `#pragma GCC optimize("O3","fast-math")`, `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;` +- Test: `robotics/trajectory/test/TestCubicSplineTrajectory.cpp` +- Doc: `doc/trajectory/CubicSplineTrajectory.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. +- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; + `robotics_add_coverage_sources(robotics.trajectory CubicSplineTrajectory.cpp)`; + `TestCubicSplineTrajectory.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/CubicSplineTrajectory/tests.md b/roadmap/trajectory/CubicSplineTrajectory/tests.md new file mode 100644 index 0000000..9e4abf5 --- /dev/null +++ b/roadmap/trajectory/CubicSplineTrajectory/tests.md @@ -0,0 +1,84 @@ +# Multi-Joint Cubic Spline Trajectory — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestCubicSplineTrajectory : public ::testing::Test: + using Spline = CubicSplineTrajectory + std::array times{ 0, 1, 2.5, 3, 4.5 } # K = 5 used + 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 +# 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) + +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 + +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 + +two_knots_reduce_to_cubic_polynomial: + 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 } + 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`. + +## 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}`. diff --git a/roadmap/trajectory/PolynomialTrajectory/explanation.md b/roadmap/trajectory/PolynomialTrajectory/explanation.md index 83584be..6d77ac7 100644 --- a/roadmap/trajectory/PolynomialTrajectory/explanation.md +++ b/roadmap/trajectory/PolynomialTrajectory/explanation.md @@ -27,4 +27,5 @@ trajectories). ## See also `TrapezoidalProfile` (limit-respecting, not fixed-time), `SCurveProfile` (jerk-bounded), -`CartesianSlerpInterpolation` (task-space paths). +`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 index 19abce2..b91b033 100644 --- a/roadmap/trajectory/PolynomialTrajectory/implementation.md +++ b/roadmap/trajectory/PolynomialTrajectory/implementation.md @@ -4,41 +4,61 @@ ## 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 -struct TrajectoryState: - T position, velocity, acceleration - template # static_assert(std::is_floating_point_v); instantiated for float class PolynomialTrajectory: - array coeff # a0..a5 (cubic uses a0..a3) + array coeff # a0..a5 (cubic uses a0..a3, a4 = a5 = 0) T duration # tf > 0 - uint8_t degree # 3 = cubic, 5 = quintic + 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) +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 @@ -47,26 +67,32 @@ function solveQuintic(bc, tf): # six matched boundary conditions 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) - return { pos, vel, acc } + 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)` — three Horner evaluations, ≤ 15 multiply-adds. -- Memory: `O(1)` — six coefficients plus a duration; all stack-resident, no heap. +- `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, or template the state on `math::Vector`. +- 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. @@ -75,12 +101,16 @@ function Sample(t): # OPTIMIZE_FOR_SPEED - Header: `robotics/trajectory/PolynomialTrajectory.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class PolynomialTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/trajectory/PolynomialTrajectory.cpp` → - `template class PolynomialTrajectory;` +- 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` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestPolynomialTrajectory.cpp` → the `_test` target. -- New module: create `robotics/trajectory/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". +- 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 index 727fdeb..fe6c9ef 100644 --- a/roadmap/trajectory/PolynomialTrajectory/tests.md +++ b/roadmap/trajectory/PolynomialTrajectory/tests.md @@ -4,7 +4,7 @@ ## Fixture -``` +```cpp class TestPolynomialTrajectory : public ::testing::Test: BoundaryConditions restToRest{ .q0 = 0, .qf = 1 } # v0=vf=0 PolynomialTrajectory cubic{ restToRest, 2.0f, Degree::Cubic } @@ -13,7 +13,7 @@ class TestPolynomialTrajectory : public ::testing::Test: ## Test cases (Arrange / Act / Assert) -``` +```text endpoints_match_boundary_positions: Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 1 @@ -29,6 +29,10 @@ 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 @@ -49,6 +53,8 @@ reset_recomputes_coefficients: - 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 diff --git a/roadmap/trajectory/SCurveProfile/explanation.md b/roadmap/trajectory/SCurveProfile/explanation.md index 9f5042f..0ff0c53 100644 --- a/roadmap/trajectory/SCurveProfile/explanation.md +++ b/roadmap/trajectory/SCurveProfile/explanation.md @@ -14,8 +14,11 @@ 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`. All phase -durations follow from closed-form algebra on the velocity/acceleration/jerk ceilings. +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. @@ -24,7 +27,8 @@ durations follow from closed-form algebra on the velocity/acceleration/jerk ceil ## Reference L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (double-S / jerk-limited profiles). +§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), diff --git a/roadmap/trajectory/SCurveProfile/implementation.md b/roadmap/trajectory/SCurveProfile/implementation.md index 751bda0..f780a00 100644 --- a/roadmap/trajectory/SCurveProfile/implementation.md +++ b/roadmap/trajectory/SCurveProfile/implementation.md @@ -4,26 +4,25 @@ ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax, aMax, jMax # velocity, acceleration, jerk ceilings (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: - T position, velocity, acceleration, jerk +`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 + 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() @@ -32,47 +31,75 @@ 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): - d = |qf - q0|; direction = sign(qf - q0) - # --- acceleration sub-phase durations (rest-to-rest) --- - if (lim.vMax * lim.jMax) >= lim.aMax^2: # aMax is reached + 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 (aMax not reached) + else: # triangular accel, peak jMax·Tj < aMax Tj = sqrt(lim.vMax / lim.jMax) Ta = 2*Tj - # --- constant-velocity phase --- - Tv = d / lim.vMax - Ta # shrink/kill accel phase if Tv < 0 - if Tv < 0: recompute Ta, Tj for a short move (no cruise) - segT = [Tj, Ta-2*Tj, Tj, Tv, Tj, Ta-2*Tj, Tj] - integrate boundary states segPos/segVel/segAcc from constant-jerk kinematics + 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, tau = locateSegment(t) # phase index + local time - j = jerkOfPhase(i) # +jMax, 0, or -jMax + 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)` — a fixed set of algebraic phase-duration formulas, no iteration. +- 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)` — three 8-entry boundary tables plus seven durations; stack only, no heap. +- 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. -- Handle the degenerate cases: short moves where `aMax` and/or `vMax` are never reached collapse - segments to zero length — never emit a negative duration. -- Bounded jerk means acceleration is **continuous** ⇒ far less vibration than a trapezoid; this is - the reason to pay for the extra bookkeeping. -- Profile is symmetric about its midpoint for rest-to-rest moves — reuse the accel table, mirrored. +- 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. @@ -81,12 +108,15 @@ function Sample(t): # OPTIMIZE_FOR_SPEED - Header: `robotics/trajectory/SCurveProfile.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class SCurveProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/trajectory/SCurveProfile.cpp` → - `template class SCurveProfile;` +- 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` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestSCurveProfile.cpp` → the `_test` target. -- New module: create `robotics/trajectory/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/trajectory/` folder. +- 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 index 6cc4e45..bd35c63 100644 --- a/roadmap/trajectory/SCurveProfile/tests.md +++ b/roadmap/trajectory/SCurveProfile/tests.md @@ -4,16 +4,16 @@ ## 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 + 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 @@ -33,9 +33,30 @@ acceleration_is_continuous: Arrange: dense sampling across phase joins Assert: no acceleration step between adjacent samples (bounded by jMax·dt) -short_move_skips_cruise: - Arrange: q0=0, qf=0.05 (very short) - Assert: ReachesMaxVel() == false, endpoints still reached +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) @@ -43,12 +64,24 @@ sample_clamps_outside_domain: ## Reference vectors -- Full profile `vMax=1, aMax=2, jMax=10`: jerk phase `Tj = aMax/jMax = 0.2 s`; - accel phase `Ta = Tj + vMax/aMax = 0.7 s`. -- Rest-to-rest S-curve is symmetric ⇒ velocity peak occurs at `tf/2`. +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). -- `vMax`/`aMax` unreachable for a short move ⇒ 4- or 2-segment degenerate profile, still valid. +- `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 ac85faa..1fa8b94 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md @@ -10,26 +10,43 @@ geometry is frozen, only the speed along it is optimized. Separating *where* to go (path planning) from *how fast* to go (parameterization) is a powerful decomposition. Once a collision-free path exists, TOPP squeezes maximum throughput out of the hardware — critical for cycle-time-bound industrial robots — while provably respecting drive -saturation, so the plan never asks for torque the motors cannot deliver. +saturation, so the plan never asks for torque the motors cannot deliver. The reachability variant +used here is a fixed number of closed-form steps per grid stage: bounded memory, no iterative solver, +deterministic planning time. ## How it works (intuition) -Reparameterize dynamics in terms of the scalar path coordinate `s`: each joint torque becomes affine -in `s̈` and `ṡ²`. This collapses the high-dimensional problem to a 2-D `(s, ṡ)` phase plane. The -**maximum velocity curve** (MVC) marks the fastest `ṡ` still feasible at each `s`. Modern TOPP-RA -then runs a backward "controllable set" pass and a forward "reachable set" pass over a grid, each -step solving a tiny 1-D linear program — numerically robust, linear-time, and free of the fragile -switch-point search in Bobrow's original phase-plane method. +Reparameterize the dynamics in terms of the scalar path coordinate `s`: with `x = ṡ²` and `u = s̈`, +every joint torque becomes affine, `τ = a(s)·u + b(s)·x + c(s)`, and a joint speed limit becomes an upper +bound on `x`. On a grid of `s`, holding `u` constant per stage makes `x` evolve linearly +(`x_{i+1} = x_i + 2Δ·u_i`). TOPP-RA then works on **sets of admissible speeds**: +- a **backward pass** computes, from the end at rest, the *controllable set* `K_i = [lo_i, hi_i]` — the + speeds at `s_i` from which the rest of the path can still be completed. Each `K_i` is the range of `x` + over a small polygon in the `(x, u)` plane (stage constraints plus "land inside `K_{i+1}`"), i.e. two + tiny **2-variable** linear programs, solved exactly by eliminating `u`; +- a **forward pass** starts at rest and greedily picks the largest acceleration that keeps the next + speed inside `K_{i+1}` — a 1-D linear program per stage. + +The result is bang-bang-like (maximum acceleration, then maximum deceleration, with speed-limited +plateaus), and the stage times follow exactly from the average of the speeds at both ends. It avoids +the fragile switch-point search of the classical phase-plane (Bobrow) and numerical-integration methods. ## Key parameters - **Path geometry** — `q(s)` and its first/second derivatives along `s ∈ [0, 1]`. -- **Velocity & torque limits** — per-joint bounds, torques via inverse dynamics (RNEA). -- **Grid resolution** — number of `s` samples; trades accuracy against planning cost. +- **Velocity & torque limits** — per-joint bounds; the path coefficients `a, b, c` come from inverse + dynamics (three RNEA passes per grid point). +- **Grid resolution** — number of `s` samples; constraints hold exactly at grid points, with an + `O(Δ)` excursion between them. ## Reference -J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along Specified -Paths," *IJRR* 4(3), 1985; Q.-C. Pham, "A General, Fast, and Robust Implementation of the -Time-Optimal Path Parameterization Algorithm" (TOPP-RA), *IEEE T-RO*, 2014. +H. Pham, Q.-C. Pham, "A New Approach to Time-Optimal Path Parameterization Based on Reachability +Analysis," *IEEE Trans. Robotics* 34(3), 2018 (TOPP-RA — the algorithm specified here). +Background: J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along +Specified Paths," *IJRR* 4(3), 1985 (phase-plane method); Q.-C. Pham, "A General, Fast, and Robust +Implementation of the Time-Optimal Path Parameterization Algorithm," *IEEE Trans. Robotics* 30(6), 2014 +(numerical-integration TOPP, not TOPP-RA). ## See also -`CartesianSlerpInterpolation` (supplies the path), `RecursiveNewtonEuler` (torque limits), -`SCurveProfile` (heuristic limit-respecting alternative). +`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 +limit-respecting alternative). diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md index a3b4256..454d5a9 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md @@ -1,95 +1,194 @@ -# Time-Optimal Path Parameterization (TOPP) — Implementation Pseudocode +# Time-Optimal Path Parameterization (TOPP-RA) — Implementation Pseudocode > Roadmap ref: #M27 (Tier 5) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) +Reachability-analysis TOPP (H. Pham, Q.-C. Pham, IEEE T-RO 34(3), 2018). Planning-time only; the result +is streamed with `Sample`. + ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -interface PathGeometry: # DI — the fixed geometric path q(s), s ∈ [0,1] - math::Vector Position(T s) - math::Vector FirstDerivative(T s) # q'(s) - math::Vector SecondDerivative(T s) # q''(s) - -template # static_assert(std::is_floating_point_v); instantiated for float -interface JointLimits: # DI — inverse dynamics + bounds (reuse RNEA) - void Coefficients(T s, out a, out b, out c) # τ = a(s)·s̈ + b(s)·ṡ² + c(s) - Bounds TorqueBounds(), VelocityBounds() - -template # static_assert(std::is_floating_point_v); instantiated for float +`JointTrajectoryState` comes from `robotics/trajectory/TrajectoryTypes.hpp` (canonical +definition: PolynomialTrajectory spec → Data structures). + +```cpp +template using JointVector = math::Vector + +template # static_assert(std::is_floating_point_v); instantiated for float +class PathGeometry: # DI, virtual ~PathGeometry() = default — fixed path q(s), s ∈ [0, 1] + virtual JointVector Position(T s) const = 0 # q(s) + virtual JointVector FirstDerivative(T s) const = 0 # q'(s) + virtual JointVector SecondDerivative(T s) const = 0 # q''(s) + +template +struct PathCoefficients: # joint torque along the path: τ = a·s̈ + b·ṡ² + c + JointVector a, b, c + +template +struct JointBounds: + JointVector torqueMin, torqueMax # τmin < τmax + JointVector velocityMax # q̇max > 0 + +template +class JointLimits: # DI, virtual ~JointLimits() = default + virtual PathCoefficients Coefficients(T s) const = 0 + virtual const JointBounds& Bounds() const = 0 + +template # production JointLimits, built on M29 +class InverseDynamicsJointLimits : public JointLimits: + const PathGeometry& path + const dynamics::InverseDynamicsModel& dynamics # e.g. ChainDynamicsModel + JointBounds bounds + +template +struct StageRow: # one linear constraint α·x + β·u ≤ γ (x = ṡ², u = s̈) + T alpha, beta, gamma + +template # static_assert(Grid >= 3); gridpoints s_0..s_N, N = Grid − 1 stages class TimeOptimalPathParameterization: - array sGrid # discretized path parameter - array xMax # MVC: max ṡ² per gridpoint - array xProfile # controllable ṡ² after reachability pass + const PathGeometry& path + const JointLimits& limits + array, Grid> coefficients # a_i, b_i, c_i at s_i + array xMax # velocity bound on x at s_i + array lo, hi # controllable sets K_i = [lo_i, hi_i] + array sDot # sqrt(x_i) of the chosen profile + array u # constant s̈ on stage i (entry N unused) + array stageStartTime # time at s_i; entry N = TotalTime + bool parameterized ``` ## Interface -``` -TimeOptimalPathParameterization(PathGeometry&, JointLimits&) -bool Parameterize() # build ṡ²(s); false if infeasible -Optional> Sample(T t) # hot path, after parameterization -T TotalTime() +```cpp +TimeOptimalPathParameterization(const PathGeometry& path, const JointLimits& limits) +bool Parameterize() # false if infeasible +std::optional> Sample(T t) const # nullopt until Parameterize() succeeds +T TotalTime() const + +InverseDynamicsJointLimits(const PathGeometry&, const dynamics::InverseDynamicsModel&, + const JointBounds&) ``` ## Algorithm (pseudocode) +```cpp +# Along the path q̇ = q'·ṡ, q̈ = q'·u + q''·x, so every joint torque is affine in (x, u): +# τ(s) = a(s)·u + b(s)·x + c(s), a = M q', b = M q'' + C(q,q') q', c = g(q) +# Grid s_i = i/N, Δ_i = s_{i+1} − s_i (uniform 1/N). Stage i holds u_i constant on [s_i, s_{i+1}]: +# x_{i+1} = x_i + 2Δ_i·u_i + +function InverseDynamicsJointLimits::Coefficients(s): # three O(n) RNEA passes (M29 includes gravity) + q = path.Position(s); q1 = path.FirstDerivative(s); q2 = path.SecondDerivative(s) + c = dynamics.ComputeInverseDynamics(q, 0, 0) # RNEA(q, 0, 0, g) = g(q) + a = dynamics.ComputeInverseDynamics(q, 0, q1) − c # RNEA(q, 0, q', 0) = M q' + b = dynamics.ComputeInverseDynamics(q, q1, q2) − c # RNEA(q, q', q'', 0) = M q'' + C(q,q') q' + return { a, b, c } # C(q, q'ṡ) q'ṡ = ṡ²·C(q,q') q' + +function stageRows(i, targetLo, targetHi): # m = 2·Dof + 4 rows, bounded array + for j in 0..Dof−1: + { b_ij, a_ij, τmax_j − c_ij } # τ_j ≤ τmax_j + { −b_ij, −a_ij, c_ij − τmin_j } # τ_j ≥ τmin_j + { 1, 0, xMax_i }, { −1, 0, 0 } # 0 ≤ x ≤ xMax_i + { 1, 2Δ_i, targetHi }, { −1, −2Δ_i, −targetLo } # x + 2Δ_i·u ∈ [targetLo, targetHi] + +function tighten(K, α, γ): # apply α·x ≤ γ to K = [lo, hi] + if α > ε: K.hi = min(K.hi, γ/α) + else if α < −ε: K.lo = max(K.lo, γ/α) + else if γ < −ε: K.empty = true # row violated for every x + +function projectOntoX(rows): # min / max x of the 2-D polygon {(x,u) : rows} + K = [0, xCeiling] + for r in rows with |r.β| ≤ ε: tighten(K, r.α, r.γ) + for p in rows with p.β > ε: # u ≤ (γ_p − α_p·x)/β_p + for n in rows with n.β < −ε: # u ≥ (γ_n − α_n·x)/β_n + tighten(K, p.β·n.α − n.β·p.α, p.β·n.γ − n.β·p.γ) # eliminate u (Fourier–Motzkin) + return (K.empty or K.lo > K.hi + ε) ? empty : K + +function maxControl(rows, x): # 1-D LP: largest u satisfying every row at this x + return min over rows with r.β > ε of (r.γ − r.α·x)/r.β # finite: transition row has β = 2Δ_i + +function Parameterize(): + parameterized = false + for i in 0..N: + q1 = path.FirstDerivative(s_i); coefficients[i] = limits.Coefficients(s_i) + xMax[i] = min over j with |q1_j| > ε of (q̇max_j / q1_j)² # |q'_j·ṡ| ≤ q̇max_j + (xCeiling when every q1_j ≈ 0) + if every q1_j(s_i) ≈ 0: # zero-length path + stageStartTime, sDot, u = 0; parameterized = true; return true + lo[N] = hi[N] = 0 # K_N = [0, 0]: rest at the end + for i = N−1 down to 0: # backward pass: controllable sets + K = projectOntoX(stageRows(i, lo[i+1], hi[i+1])) + if K empty: return false + lo[i], hi[i] = K.lo, K.hi + if lo[0] > ε: return false # 0 ∉ K_0: cannot start from rest + x = 0; sDot[0] = 0; stageStartTime[0] = 0 + for i = 0 .. N−1: # forward pass: greedy, stays inside K_{i+1} + xNext = clamp(x + 2Δ_i·maxControl(stageRows(i, lo[i+1], hi[i+1]), x), lo[i+1], hi[i+1]) + u[i] = (xNext − x) / (2Δ_i) # re-derived after the clamp ⇒ stage ends exactly at s_{i+1} + sDot[i+1] = sqrt(xNext) + if sDot[i] + sDot[i+1] ≤ ε: return false # stalled at rest + stageStartTime[i+1] = stageStartTime[i] + 2Δ_i / (sDot[i] + sDot[i+1]) + x = xNext + parameterized = true; return true + +function Sample(t): # OPTIMIZE_FOR_SPEED + if !parameterized: return nullopt + t = clamp(t, 0, stageStartTime[N]) + i = binary search: last stage i ∈ [0, N−1] with stageStartTime[i] ≤ t + τ = t − stageStartTime[i] + s = s_i + sDot[i]·τ + u[i]·τ²/2; sd = sDot[i] + u[i]·τ; sdd = u[i] + q1 = path.FirstDerivative(s) + return { path.Position(s), q1·sd, q1·sdd + path.SecondDerivative(s)·sd² } ``` -# Per gridpoint the joint torque law is affine in (u = s̈, x = ṡ²): -# τ_j(s) = a_j(s)·u + b_j(s)·x + c_j(s) ∈ [τ_min, τ_max] - -function maxVelocityCurve(s): # MVC — tightest ṡ² still feasible - x_v = min_j ( velLimit_j / |q'_j(s)| )^2 # velocity bound - x_a = largest x where some u keeps every τ_j in bounds # accel/torque bound - return min(x_v, x_a) - -function Parameterize(): # TOPP-RA reachability analysis - for i in 0..Grid-1: xMax[i] = maxVelocityCurve(sGrid[i]) - # backward pass: controllable set ending at rest (x_N = 0) - for i = Grid-1 .. 0: - [uLo, uHi] = admissibleAccel(sGrid[i], xProfile[i]) # 1-D LP over joints - xProfile[i] = min(xMax[i], propagateBack(xProfile[i+1], uLo, uHi)) - # forward pass: reachable set starting from rest (x_0 = 0), clipped to controllable - for i = 0 .. Grid-1: - xProfile[i] = min(xProfile[i], propagateForward(xProfile[i-1])) - return all(xProfile >= 0) - -function Sample(t): # OPTIMIZE_FOR_SPEED - # integrate dt = ds / sqrt(x(s)) offline into a time->s table, then look up - s = timeToS(t) - sd = sqrt(interp(xProfile, s)) # ṡ - sdd = admissibleAccelMid(s) # s̈ - return jointState(path, s, sd, sdd) -``` + +`projectOntoX` is the exact projection of the stage polygon onto `x`: every `(p, n)` pair is a candidate +vertex (intersection of an upper and a lower `u`-bound line), so `lo`/`hi` are the two 2-variable LP +optima `min x` / `max x` without an iterative solver. ## Complexity & memory -- Parameterization: `O(Grid · N)` — each gridpoint solves a tiny 1-D LP over `N` joints. -- Two linear passes (backward controllable + forward reachable), no global optimization loop. -- Memory: `O(Grid)` bounded arrays (`sGrid`, `xMax`, `xProfile`); planning-time, stack/static only. +- `Parameterize`: `O(Grid·Dof²)` — per stage one projection over `m = 2·Dof + 4` rows (`O(m²)` pairs) and + one `O(m)` 1-D LP, plus one `Coefficients` call (three `O(Dof)` RNEA passes). No iteration, deterministic. +- `Sample`: `O(log Grid)` binary search + three path evaluations. +- Memory: `(3·Dof + 6)·Grid` scalars in bounded `std::array`s (≈ 3 KB for ``); the rows of one + stage live on the stack. No heap — place the planner in static storage. ## Numerical / embedded notes -- This is a **planning-time** computation, not an ISR path — run offline, then stream `Sample`. -- Reuse **RNEA** (item exists) to evaluate the `a(s), b(s), c(s)` inverse-dynamics coefficients per - gridpoint; inject it behind `JointLimits` so the planner stays dynamics-agnostic (DIP). -- Grid density trades accuracy for memory/time; `Grid` is a compile-time bound (`std::array`). -- Guard **MVC singularities** (a `q'_j(s)=0` "zero-inertia" direction) — clamp, don't divide by zero. -- TOPP-RA's reachability formulation is numerically robust where Bobrow's switch-point search stalls. +- **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. +- 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 + the joint drops out of `xMax` — no division by zero. `xCeiling` (e.g. `1 / math::Tolerance()`) keeps + every stage polygon bounded when no joint limits the speed. +- Constraints are collocated at gridpoints: between them `τ` may exceed a bound by `O(Δ)` — refine `Grid` + or shrink the bounds by a margin. +- `ε`: relative tolerance on the row scale (`math::Tolerance()`·max|coefficient|). +- The affine form holds for `M q̈ + C q̇ + g` (+ armature, Coulomb friction with fixed sign along the path); + **viscous friction is linear in `ṡ`, not `ṡ²`** — use a friction-free model or a torque margin. +- Infeasible iff some `K_i` is empty or `0 ∉ K_0`; both surface as `Parameterize() == false`. - 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/TimeOptimalPathParameterization.hpp` — `#pragma once` → +- 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 - `extern template class TimeOptimalPathParameterization;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. + `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); + `robotics.trajectory` links `robotics.dynamics`. - Coverage: `robotics/trajectory/TimeOptimalPathParameterization.cpp` → - `template class TimeOptimalPathParameterization;` + `template class TimeOptimalPathParameterization;` and + `template class InverseDynamicsJointLimits;` - Test: `robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp` -- Doc: `doc/trajectory/TimeOptimalPathParameterization.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestTimeOptimalPathParameterization.cpp` → the `_test` target. -- New module: create `robotics/trajectory/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/trajectory/` folder. +- Doc: `doc/trajectory/TimeOptimalPathParameterization.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. +- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; + `robotics_add_coverage_sources(robotics.trajectory TimeOptimalPathParameterization.cpp)`; + `TestTimeOptimalPathParameterization.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: M29 (`InverseDynamicsModel`, `ChainDynamicsModel`). - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md b/roadmap/trajectory/TimeOptimalPathParameterization/tests.md index 1c5cf67..12f4ca3 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/tests.md @@ -1,59 +1,103 @@ -# Time-Optimal Path Parameterization (TOPP) — Unit Test Plan (Pseudocode) +# Time-Optimal Path Parameterization (TOPP-RA) — Unit Test Plan (Pseudocode) > GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. ## Fixture -``` +```cpp +class PathGeometryMock : public PathGeometry # MOCK_METHOD Position / FirstDerivative / SecondDerivative +class JointLimitsMock : public JointLimits # MOCK_METHOD Coefficients / Bounds +class InverseDynamicsModelMock : public dynamics::InverseDynamicsModel # MOCK_METHOD ComputeInverseDynamics + class TestTopp : public ::testing::Test: - StrictMock> path # DI: q(s), q'(s), q''(s) - StrictMock> limits # DI: RNEA coeffs + bounds - TimeOptimalPathParameterization topp{ path, limits } + StrictMock path + StrictMock limits + TimeOptimalPathParameterization topp{ path, limits } # N = 63 stages, Δ = 1/63 + JointBounds unitTorque{ .torqueMin = (−1, −1), .torqueMax = (1, 1), .velocityMax = (100, 100) } + + void GivenStraightSingleJointPath(float L, JointBounds bounds): + # q(s) = (L·s, 0), unit inertia, no gravity: a = (L, 0), b = 0, c = 0 + EXPECT_CALL(path, Position(_)).WillRepeatedly(Return((L·s, 0))) + EXPECT_CALL(path, FirstDerivative(_)).WillRepeatedly(Return((L, 0))) + EXPECT_CALL(path, SecondDerivative(_)).WillRepeatedly(Return((0, 0))) + EXPECT_CALL(limits, Coefficients(_)).WillRepeatedly(Return({ (L, 0), (0, 0), (0, 0) })) + EXPECT_CALL(limits, Bounds()).WillRepeatedly(ReturnRef(bounds)) # each case below is a TEST_F(TestTopp, ) ``` ## Test cases (Arrange / Act / Assert) -``` -straight_path_respects_velocity_limit: - Arrange: linear q(s), velocity limit vMax - Act: Parameterize(); sample ṡ along path - Assert: every joint velocity |q'(s)·ṡ| ≤ vMax - -profile_respects_torque_bounds: - Arrange: mock RNEA coeffs a,b,c with symmetric torque limits - Assert: reconstructed τ_j(s) ∈ [τ_min, τ_max] at all gridpoints - -starts_and_ends_at_rest: - Assert: ṡ(0) ≈ 0 and ṡ(1) ≈ 0 (x_0 = x_N = 0) - -is_faster_than_uniform_traversal: - Arrange: same path, fixed slow ṡ - Assert: TotalTime() < uniform-traversal time (optimality sanity check) - -infeasible_path_reports_failure: - Arrange: torque bounds too tight to move - Assert: Parameterize() == false - -bang_bang_structure_on_simple_path: - Arrange: single-joint, constant inertia - Assert: accel is at a bound almost everywhere (max-accel then max-decel) - -denser_grid_refines_time: - Arrange: Grid 32 vs 128 - Assert: TotalTime() converges (monotone, bounded change) +```cpp +bang_bang_total_time_matches_closed_form: + Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) + Act: Parameterize() + Assert: true; TotalTime() ≈ 2·sqrt(L) = 2 (tol 1e-3; exact value 2.000064 for Grid 64) + +bang_bang_acceleration_saturates_torque: + Arrange: as above, L = 4 + Assert: |Sample(t).acceleration_0| ≈ 1 (= |τ|max / inertia) for t in the first and last 45 % of + TotalTime(); positive before TotalTime()/2, negative after + +velocity_limit_caps_path_speed: + Arrange: GivenStraightSingleJointPath(L = 1, { torque ±100, velocityMax = (0.5, 0.5) }) + Assert: every sampled |velocity_0| ≤ 0.5·(1 + tol); Sample(TotalTime()/2).velocity_0 ≈ 0.5; + TotalTime() ≈ 2 + 4Δ = 2.063492 + +profile_respects_torque_and_velocity_bounds: + Arrange: q(s) = (s, 0.4s − 0.4s²) ⇒ q' = (1, 0.4 − 0.8s), q'' = (0, −0.8); + coupled mock coefficients a = (1 + 0.5s, 0.4 − 0.8s), b = (0.3·cos 3s, −0.2 + 0.1s), + c = (0.1, −0.05s); τ ∈ [(−1, −0.6), (1, 0.6)], q̇max = (1.5, 0.8) + Act: Parameterize(); dense Sample(t) (s = position_0, ṡ = velocity_0, s̈ = acceleration_0) + Assert: true; TotalTime() ≈ 2.256496; |q̇_j| ≤ q̇max_j; a(s)·s̈ + b(s)·ṡ² + c(s) ∈ [τmin − 0.02, τmax + 0.02] + (collocation: exact at gridpoints, O(Δ) between them — 0.018 measured) + +starts_and_ends_at_rest_on_path_endpoints: + Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) + Assert: Sample(0) = { q(0), 0, · } and Sample(TotalTime()) = { q(1), ≈0, · } + +sample_is_continuous_across_stages: + Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) + Assert: for every interior stage start time t_i: Sample(t_i − δ) ≈ Sample(t_i + δ) in position and + velocity (δ = 1e-4·TotalTime()) + +infeasible_static_torque_reports_failure: + Arrange: straight path with c = (2, 0) > τmax = 1 (gravity cannot be held) + Assert: Parameterize() == false and Sample(0) == nullopt sample_before_parameterize_returns_nullopt: - Assert: Sample(t) == nullopt until Parameterize() succeeds + Assert: Sample(0) == nullopt + +zero_length_path_takes_zero_time: + Arrange: q'(s) = q''(s) = (0, 0), q(s) = (0.3, −0.2) + Assert: Parameterize() == true, TotalTime() == 0, Sample(0) = { (0.3, −0.2), 0, 0 } + +inverse_dynamics_adapter_builds_coefficients: + Arrange: StrictMock dyn; path mock q = (0.1, 0.2), q' = (1, 2), q'' = (3, 4) + EXPECT_CALL(dyn, ComputeInverseDynamics(q, 0, 0)) → g = (0.5, 0.25) + EXPECT_CALL(dyn, ComputeInverseDynamics(q, 0, q')) → g + Mq' = (1.5, 2.25) + EXPECT_CALL(dyn, ComputeInverseDynamics(q, q', q'')) → g + Mq'' + Cq' = (4.5, 5.25) + Act: InverseDynamicsJointLimits{ path, dyn, unitTorque }.Coefficients(0.5) + Assert: a ≈ (1, 2), b ≈ (4, 5), c ≈ (0.5, 0.25) ``` ## Reference vectors -- Single joint, unit inertia, `|τ| ≤ 1`, path length `L`, rest-to-rest ⇒ bang-bang time `2√L`. -- Velocity-limited straight segment ⇒ `ṡ_max = vMax / |q'(s)|` on the flat portion. +Computed with a python3 implementation of exactly this pseudocode. + +- Single joint, unit inertia, `|τ| ≤ 1`, path length `L`, rest-to-rest ⇒ bang-bang time `2·sqrt(L)`. + Grid 64 (63 stages, odd ⇒ one coast stage at the midpoint): `L = 1 → 2.000064`, `L = 4 → 4.000128` + (relative error `3.2e-5`); Grid 65 (even stage count): exact to `1e-15`; Grid 128: `7.8e-6`. +- Velocity-limited straight segment, `q̇max = 0.5`, `|τ| ≤ 100`, `L = 1`, Grid 64 ⇒ `ṡ = 0.5` on the + interior, one full ramp stage at each end: `TotalTime = (N − 2)·Δ/0.5 + 2·(2Δ/0.5) = 2 + 4/63 = 2.063492`. +- Coupled case above ⇒ feasible, `TotalTime = 2.256496`; torque bound violation `1e-12` at gridpoints, + `0.0178` between them (Grid 64); velocity bounds never violated. +- `c = 2 > τmax = 1` ⇒ every stage forces `u ≤ −1`, so `0 ∉ K_0` ⇒ infeasible. +- Adapter on a 2R planar arm: `τ(q'ṡ, q'u + q''ṡ²) − (a·u + b·ṡ² + c)` ≤ `2e-15` for arbitrary `(ṡ, u)`. ## Edge cases -- Zero-length path (`q(s)` constant) ⇒ `TotalTime() == 0`, trivial success. -- MVC singularity where `q'_j(s) = 0` ⇒ no division blow-up, profile stays finite. -- Velocity-only vs torque-only limiting ⇒ correct binding constraint selected per gridpoint. +- Zero-length path (`q'` ≡ 0) ⇒ `TotalTime() == 0`, trivial success. +- `q'_j(s) = 0` or `a_ij(s) = 0` at a gridpoint ⇒ no division, row becomes a pure `x` bound. +- Velocity-only vs torque-only limiting ⇒ the binding row changes per gridpoint; both cases above. +- `Grid = 2` (one stage) cannot leave and return to rest with one constant `u` ⇒ stalls; hence + `static_assert(Grid >= 3)`. `Grid = 3` on the bang-bang case gives exactly `2·sqrt(L)`. diff --git a/roadmap/trajectory/TrapezoidalProfile/explanation.md b/roadmap/trajectory/TrapezoidalProfile/explanation.md index 6e98ef6..c586dfa 100644 --- a/roadmap/trajectory/TrapezoidalProfile/explanation.md +++ b/roadmap/trajectory/TrapezoidalProfile/explanation.md @@ -21,6 +21,8 @@ peaks below `vMax` and immediately decelerates — a triangle. A single distance - **`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), @@ -28,4 +30,5 @@ Ch. 3 (trapezoidal / LSPB profiles). ## See also `PolynomialTrajectory` (fixed-time, smooth), `SCurveProfile` (jerk-limited upgrade), -`SaturationRateLimiter` (online slew limiting). +`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 index ef7b7dc..4e63412 100644 --- a/roadmap/trajectory/TrapezoidalProfile/implementation.md +++ b/roadmap/trajectory/TrapezoidalProfile/implementation.md @@ -4,48 +4,57 @@ ## Data structures -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax # peak velocity (> 0) - T aMax # peak accel (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: - T position, velocity, acceleration +`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 # start, sign(qf - q0) - T vPeak, aMax # cruise velocity actually reached + 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 -``` -TrapezoidalProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path +```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 vMax is never reached +bool IsTriangular() # true when no cruise phase exists ``` ## Algorithm (pseudocode) -``` +```cpp function plan(q0, qf, limits): - d = |qf - q0|; direction = sign(qf - q0) + distance = |qf - q0|; direction = sign(qf - q0); aMax = limits.aMax dBlend = limits.vMax^2 / limits.aMax # distance used by accel + decel - if d >= dBlend: # trapezoid: cruise phase exists + if distance >= dBlend: # trapezoid: cruise phase exists vPeak = limits.vMax - tAccel = vPeak / limits.aMax - tCruise = (d - dBlend) / vPeak + tAccel = vPeak / aMax + tCruise = (distance - dBlend) / vPeak else: # triangle: peak below vMax - vPeak = sqrt(d * limits.aMax) - tAccel = vPeak / limits.aMax + 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 @@ -54,23 +63,30 @@ function Sample(t): # OPTIMIZE_FOR_SPEED acc = 0; vel = vPeak; s = vPeak*(t - 0.5*tAccel) else: # parabolic ramp-down td = tf - t - acc = -aMax; vel = aMax*td; s = d - 0.5*aMax*td^2 - return { q0 + direction*s, direction*vel, direction*acc } + acc = -aMax; vel = aMax*td; s = distance - 0.5*aMax*td^2 + return { q0 + direction*s, direction*vel, direction*acc, 0 } ``` ## Complexity & memory -- Planning: `O(1)` — one `sqrt` and a branch to pick trapezoid vs triangle. +- Planning (both variants): `O(1)` — one `sqrt` and a branch. - `Sample`: `O(1)` — one phase branch, a couple of multiply-adds. -- Memory: `O(1)` — six scalars; entirely stack-resident, no heap. +- 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: `d == 0` ⇒ zero-length profile; `aMax == 0` or `vMax == 0` ⇒ reject. -- Symmetric ramps mean the decel phase mirrors accel — reuse the same `aMax`, flip the sign. +- 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. @@ -79,12 +95,14 @@ function Sample(t): # OPTIMIZE_FOR_SPEED - Header: `robotics/trajectory/TrapezoidalProfile.hpp` — `#pragma once` → `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and `extern template class TrapezoidalProfile;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/trajectory/TrapezoidalProfile.cpp` → - `template class TrapezoidalProfile;` +- 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` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestTrapezoidalProfile.cpp` → the `_test` target. -- New module: create `robotics/trajectory/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". +- 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 index b5defe4..60c59bd 100644 --- a/roadmap/trajectory/TrapezoidalProfile/tests.md +++ b/roadmap/trajectory/TrapezoidalProfile/tests.md @@ -4,7 +4,7 @@ ## 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 @@ -13,7 +13,7 @@ class TestTrapezoidalProfile : public ::testing::Test: ## Test cases (Arrange / Act / Assert) -``` +```cpp endpoints_reached_exactly: Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 @@ -39,6 +39,15 @@ 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) ``` @@ -48,9 +57,15 @@ sample_clamps_outside_domain: - `d=5, vMax=1, aMax=2`: `tAccel = vMax/aMax = 0.5`, `tCruise = (d − vMax²/aMax)/vMax = 4.5`, `tf = 5.5`. - Triangular threshold distance `= vMax²/aMax = 0.5`. +- `PlanWithDuration`, `d=4, aMax=1, T=5`: `v = (aMax·T − sqrt(aMax²T² − 4·aMax·d))/2 = (5 − 3)/2 = 1`, + `tAccel = 1`, `tCruise = 3`. `d=5, aMax=2, T=6`: `v = 0.900980`, `tAccel = 0.450490`, + `tCruise = 5.099020`. Feasible iff `aMax·T² ≥ 4d` (equality ⇒ triangle, `v = aMax·T/2`). +- Synchronization: replanning with `(vMax/λ, aMax/λ²)` scales `Duration()` by exactly `λ` + (e.g. fixture, `λ = 2` ⇒ `11.0`). ## Edge cases - `d` exactly equal to `vMax²/aMax` ⇒ zero cruise time (trapezoid degenerates to triangle). - `d == 0` ⇒ `tf == 0`, `Sample(0)` is the start at rest. - `aMax` very large ⇒ ramps vanish, profile approaches a pure velocity step. +- `PlanWithDuration` with `d == 0` ⇒ `vPeak = 0`, holds `q0` at rest for the whole duration. diff --git a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp index df89e7c..3d99188 100644 --- a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp +++ b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp @@ -4,17 +4,16 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "infra/util/ReallyAssert.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" #include +#include namespace dynamics { - // Articulated Body Algorithm (ABA): O(n) forward dynamics for serial chains. - // Given joint positions, velocities, and applied torques, computes joint accelerations - // directly without forming or inverting the mass matrix. template class ArticulatedBodyAlgorithm { @@ -65,8 +64,6 @@ namespace dynamics Vector3 UaLin; }; - // ── Pass 1: Forward kinematics ── - static std::array ComputeForwardKinematics( const LinkArray& links, const JointVector& q, const JointVector& qDot); @@ -74,8 +71,6 @@ namespace dynamics LinkKinematics& current, const LinkKinematics& parent, const RevoluteJointLink& link, T qDot_i); - // ── Pass 2: Backward articulated-body inertias ── - static SpatialInertia ComputeRigidBodyInertia( const RevoluteJointLink& link); @@ -116,8 +111,6 @@ namespace dynamics std::array& pA, std::array& proj); - // ── Pass 3: Forward accelerations ── - static T ComputeJointAcceleration( const JointProjection& proj, const Vector3& aHatAng, const Vector3& aHatLin); @@ -154,6 +147,7 @@ namespace dynamics for (std::size_t i = 0; i < NumLinks; ++i) { + assert(HasUnitJointAxis(links[i])); kin[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); if (i == 0) @@ -208,11 +202,10 @@ namespace dynamics proj.Ua = Ia.rot * axis; proj.UaLin = Ia.cross.Transpose() * axis; - auto dVec = axis.Transpose() * proj.Ua; - proj.D = dVec.at(0, 0); + proj.D = math::DotProduct(axis, proj.Ua); + really_assert(proj.D > T(0)); - auto sTpA = axis.Transpose() * pA.torque; - proj.u = tau - sTpA.at(0, 0); + proj.u = tau - math::DotProduct(axis, pA.torque); return proj; } @@ -285,8 +278,7 @@ namespace dynamics const JointProjection& proj, const Vector3& aPrimeAng, const Vector3& aPrimeLin) { - auto UaTa = proj.Ua.Transpose() * aPrimeAng + proj.UaLin.Transpose() * aPrimeLin; - return (proj.u - UaTa.at(0, 0)) / proj.D; + return (proj.u - math::DotProduct(proj.Ua, aPrimeAng) - math::DotProduct(proj.UaLin, aPrimeLin)) / proj.D; } template diff --git a/robotics/dynamics/EulerLagrangeDynamics.hpp b/robotics/dynamics/EulerLagrangeDynamics.hpp index aefccb5..97cbc1d 100644 --- a/robotics/dynamics/EulerLagrangeDynamics.hpp +++ b/robotics/dynamics/EulerLagrangeDynamics.hpp @@ -13,8 +13,7 @@ namespace dynamics { static_assert(std::is_floating_point_v, "EulerLagrangeDynamics only supports floating-point types"); - static_assert(math::detail::is_valid_dimensions_v, - "Degrees of freedom must be positive"); + static_assert(Dof > 0, "Degrees of freedom must be positive"); public: using StateVector = math::Vector; diff --git a/robotics/dynamics/EulerLagrangeSolver.hpp b/robotics/dynamics/EulerLagrangeSolver.hpp index 547135a..7a80a54 100644 --- a/robotics/dynamics/EulerLagrangeSolver.hpp +++ b/robotics/dynamics/EulerLagrangeSolver.hpp @@ -4,10 +4,11 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" -#include "numerical/solvers/GaussianElimination.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" namespace dynamics { @@ -16,8 +17,7 @@ namespace dynamics { static_assert(std::is_floating_point_v, "EulerLagrangeSolver only supports floating-point types"); - static_assert(math::detail::is_valid_dimensions_v, - "Degrees of freedom must be positive"); + static_assert(Dof > 0, "Degrees of freedom must be positive"); public: using StateVector = math::Vector; @@ -25,11 +25,9 @@ namespace dynamics EulerLagrangeSolver() = default; - // Forward dynamics: computes qDDot = M(q)^{-1} * (tau - C(q, qDot) - g(q)) OPTIMIZE_FOR_SPEED StateVector ForwardDynamics(const EulerLagrangeDynamics& model, const StateVector& q, const StateVector& qDot, const StateVector& tau) const; - // Inverse dynamics: computes tau = M(q) * qDDot + C(q, qDot) + g(q) OPTIMIZE_FOR_SPEED StateVector InverseDynamics(const EulerLagrangeDynamics& model, const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const; }; @@ -44,10 +42,10 @@ namespace dynamics auto C = model.ComputeCoriolisTerms(q, qDot); auto g = model.ComputeGravityTerms(q); - auto rhs = tau - C - g; + const auto qDDot{ math::CholeskyDecomposition::Solve(M, StateVector{ tau - C - g }) }; + really_assert(qDDot.has_value()); - solvers::GaussianElimination solver; - return solver.Solve(M, rhs); + return *qDDot; } template diff --git a/robotics/dynamics/NewtonEulerSolver.hpp b/robotics/dynamics/NewtonEulerSolver.hpp index 1fc7f0a..89ad7ec 100644 --- a/robotics/dynamics/NewtonEulerSolver.hpp +++ b/robotics/dynamics/NewtonEulerSolver.hpp @@ -4,27 +4,35 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "robotics/dynamics/NewtonEulerBody.hpp" +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "numerical/solvers/GaussianElimination.hpp" +#include "robotics/dynamics/NewtonEulerBody.hpp" namespace dynamics { template - struct SpatialAcceleration + struct BodyAcceleration { math::Vector linear; math::Vector angular; }; template - struct SpatialForce + struct BodyWrench { math::Vector force; math::Vector torque; }; + template + using SpatialAcceleration [[deprecated("use BodyAcceleration")]] = BodyAcceleration; + + template + using SpatialForce [[deprecated("use BodyWrench")]] = BodyWrench; + template class NewtonEulerSolver { @@ -37,77 +45,49 @@ namespace dynamics NewtonEulerSolver() = default; - // Forward dynamics (body-frame formulation): - // linearAccel = force / mass - angularVelocity x linearVelocity - // angularAccel = I^{-1} (torque - angularVelocity x (I * angularVelocity)) - OPTIMIZE_FOR_SPEED SpatialAcceleration ForwardDynamics(const NewtonEulerBody& body, + OPTIMIZE_FOR_SPEED BodyAcceleration ForwardDynamics(const NewtonEulerBody& body, const Vector3& force, const Vector3& torque, const Vector3& linearVelocity, const Vector3& angularVelocity) const; - // Inverse dynamics (body-frame formulation): - // force = mass * (linearAccel + angularVelocity x linearVelocity) - // torque = I * angularAccel + angularVelocity x (I * angularVelocity) - OPTIMIZE_FOR_SPEED SpatialForce InverseDynamics(const NewtonEulerBody& body, + OPTIMIZE_FOR_SPEED BodyWrench InverseDynamics(const NewtonEulerBody& body, const Vector3& linearAcceleration, const Vector3& angularAcceleration, const Vector3& linearVelocity, const Vector3& angularVelocity) const; - - private: - static Vector3 CrossProduct(const Vector3& a, const Vector3& b); }; - template - typename NewtonEulerSolver::Vector3 - NewtonEulerSolver::CrossProduct(const Vector3& a, const Vector3& b) - { - return Vector3{ - a.at(1, 0) * b.at(2, 0) - a.at(2, 0) * b.at(1, 0), - a.at(2, 0) * b.at(0, 0) - a.at(0, 0) * b.at(2, 0), - a.at(0, 0) * b.at(1, 0) - a.at(1, 0) * b.at(0, 0) - }; - } - template OPTIMIZE_FOR_SPEED - SpatialAcceleration + BodyAcceleration NewtonEulerSolver::ForwardDynamics(const NewtonEulerBody& body, const Vector3& force, const Vector3& torque, const Vector3& linearVelocity, const Vector3& angularVelocity) const { - auto mass = body.ComputeMass(); - auto I = body.ComputeInertia(); - - auto Iw = I * angularVelocity; - auto gyroscopic = CrossProduct(angularVelocity, Iw); - auto rhs = torque - gyroscopic; + const T mass{ body.ComputeMass() }; + const InertiaMatrix inertia{ body.ComputeInertia() }; + really_assert(mass > T(0)); - solvers::GaussianElimination solver; - auto angularAccel = solver.Solve(I, rhs); + const Vector3 gyroscopic{ math::CrossProduct(angularVelocity, Vector3{ inertia * angularVelocity }) }; + const auto angularAcceleration{ math::CholeskyDecomposition::Solve(inertia, Vector3{ torque - gyroscopic }) }; + really_assert(angularAcceleration.has_value()); - auto wCrossV = CrossProduct(angularVelocity, linearVelocity); - T inverseMass = T(1.0f) / mass; - auto linearAccel = force * inverseMass - wCrossV; + const Vector3 linearAcceleration{ force * (T(1) / mass) - math::CrossProduct(angularVelocity, linearVelocity) }; - return SpatialAcceleration{ linearAccel, angularAccel }; + return BodyAcceleration{ linearAcceleration, *angularAcceleration }; } template OPTIMIZE_FOR_SPEED - SpatialForce + BodyWrench NewtonEulerSolver::InverseDynamics(const NewtonEulerBody& body, const Vector3& linearAcceleration, const Vector3& angularAcceleration, const Vector3& linearVelocity, const Vector3& angularVelocity) const { - auto mass = body.ComputeMass(); - auto I = body.ComputeInertia(); - - auto wCrossV = CrossProduct(angularVelocity, linearVelocity); - auto resultForce = (linearAcceleration + wCrossV) * mass; + const T mass{ body.ComputeMass() }; + const InertiaMatrix inertia{ body.ComputeInertia() }; - auto Iw = I * angularVelocity; - auto gyroscopic = CrossProduct(angularVelocity, Iw); - auto resultTorque = I * angularAcceleration + gyroscopic; + const Vector3 resultForce{ (linearAcceleration + math::CrossProduct(angularVelocity, linearVelocity)) * mass }; + const Vector3 resultTorque{ inertia * angularAcceleration + math::CrossProduct(angularVelocity, Vector3{ inertia * angularVelocity }) }; - return SpatialForce{ resultForce, resultTorque }; + return BodyWrench{ resultForce, resultTorque }; } #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD diff --git a/robotics/dynamics/RecursiveNewtonEuler.hpp b/robotics/dynamics/RecursiveNewtonEuler.hpp index f5a3661..e02cd7c 100644 --- a/robotics/dynamics/RecursiveNewtonEuler.hpp +++ b/robotics/dynamics/RecursiveNewtonEuler.hpp @@ -4,11 +4,12 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "robotics/dynamics/RevoluteJointLink.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" #include +#include namespace dynamics { @@ -27,8 +28,6 @@ namespace dynamics RecursiveNewtonEuler() = default; - // Inverse dynamics: given joint positions, velocities, and accelerations, - // compute the required joint torques. O(n) complexity. OPTIMIZE_FOR_SPEED JointVector InverseDynamics(const LinkArray& links, const JointVector& q, const JointVector& qDot, const JointVector& qDDot, const Vector3& gravity) const; @@ -120,6 +119,7 @@ namespace dynamics for (std::size_t i = 0; i < NumLinks; ++i) { + assert(HasUnitJointAxis(links[i])); states[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); if (i == 0) @@ -176,12 +176,9 @@ namespace dynamics states[i + 1].R, links[i + 1].parentToJoint); } - JointVector tau; + JointVector tau{}; for (std::size_t i = 0; i < NumLinks; ++i) - { - auto dotProduct = links[i].jointAxis.Transpose() * torque[i]; - tau.at(i, 0) = dotProduct.at(0, 0); - } + tau.at(i, 0) = math::DotProduct(links[i].jointAxis, torque[i]); return tau; } diff --git a/robotics/dynamics/RevoluteJointLink.hpp b/robotics/dynamics/RevoluteJointLink.hpp index c3ab2a3..9226d62 100644 --- a/robotics/dynamics/RevoluteJointLink.hpp +++ b/robotics/dynamics/RevoluteJointLink.hpp @@ -4,8 +4,10 @@ #pragma GCC optimize("O3", "fast-math") #endif +#include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" #include +#include namespace dynamics { @@ -16,9 +18,15 @@ namespace dynamics "RevoluteJointLink only supports floating-point types"); T mass; - math::SquareMatrix inertia; // inertia tensor at center of mass, in link frame - math::Vector jointAxis; // joint rotation axis in link frame (unit vector) - math::Vector parentToJoint; // position of this joint origin in parent frame - math::Vector jointToCoM; // position of center of mass in link frame + math::SquareMatrix inertia; + math::Vector jointAxis; + math::Vector parentToJoint; + math::Vector jointToCoM; }; + + template + bool HasUnitJointAxis(const RevoluteJointLink& link) + { + return std::abs(math::DotProduct(link.jointAxis, link.jointAxis) - T(1)) < T(1e-3); + } } diff --git a/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp b/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp index adb0076..ab71bb9 100644 --- a/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp +++ b/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp @@ -1,277 +1,136 @@ +#include "numerical/math/Tolerance.hpp" #include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" #include #include namespace { - constexpr float gravity = 9.81f; - const math::Vector gravityVec{ 0.0f, 0.0f, -gravity }; + using Vector3 = math::Vector; + using Link = dynamics::RevoluteJointLink; - dynamics::RevoluteJointLink MakePendulumLink(float mass, float length) + constexpr float gravity{ 9.81f }; + const Vector3 gravityVector{ 0.0f, 0.0f, -gravity }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeRod(float mass, float length, const Vector3& axis, const Vector3& parentToJoint) { - float I = mass * length * length / 12.0f; + const float inertia{ mass * length * length / 12.0f }; - return dynamics::RevoluteJointLink{ + return Link{ mass, math::SquareMatrix{ { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f } + { 0.0f, inertia, 0.0f }, + { 0.0f, 0.0f, inertia } }, + axis, + parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; } - std::array, 2> MakeTwoLinkArm( - float m1, float l1, float m2, float l2) + Vector3 Normalized(float x, float y, float z) { - float I1 = m1 * l1 * l1 / 12.0f; - float I2 = m2 * l2 * l2 / 12.0f; - - dynamics::RevoluteJointLink link1{ - m1, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I1, 0.0f }, - { 0.0f, 0.0f, I1 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f } - }; - - dynamics::RevoluteJointLink link2{ - m2, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I2, 0.0f }, - { 0.0f, 0.0f, I2 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{ l1, 0.0f, 0.0f }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f } - }; - - return { link1, link2 }; + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; } - class TestArticulatedBodyAlgorithm : public ::testing::Test + class TestArticulatedBodyAlgorithm + : public ::testing::Test { protected: dynamics::ArticulatedBodyAlgorithm aba1; dynamics::ArticulatedBodyAlgorithm aba2; + dynamics::ArticulatedBodyAlgorithm aba3; }; } -TEST_F(TestArticulatedBodyAlgorithm, single_link_zero_torque_no_gravity_gives_zero_acceleration) -{ - auto link = MakePendulumLink(1.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; - math::Vector zeroGravity{}; - - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-5f); -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_pure_torque_no_gravity) +TEST_F(TestArticulatedBodyAlgorithm, zero_torque_without_gravity_gives_zero_acceleration) { - float mass = 2.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector zeroGravity{}; + std::array links{ MakeRod(1.0f, 1.0f, zAxis, Vector3{}) }; - // Apply torque = I_end to get qDDot = 1 rad/s^2 - float I_end = mass * length * length / 3.0f; - math::Vector tau{ I_end }; + auto qDDot = aba1.ForwardDynamics(links, math::Vector{ 0.3f }, math::Vector{}, math::Vector{}, Vector3{}); - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 1.0f, 1e-3f); + EXPECT_NEAR(qDDot.at(0, 0), 0.0f, math::Tolerance()); } -TEST_F(TestArticulatedBodyAlgorithm, single_link_rnea_aba_roundtrip_no_gravity) +TEST_F(TestArticulatedBodyAlgorithm, torque_divided_by_end_inertia_gives_acceleration) { - auto link = MakePendulumLink(2.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.5f }; - math::Vector qDot{ 1.5f }; - math::Vector qDDotExpected{ 3.0f }; - math::Vector zeroGravity{}; - - math::Vector tau{ 2.0f }; + const float mass{ 2.0f }; + const float length{ 1.0f }; + std::array links{ MakeRod(mass, length, zAxis, Vector3{}) }; - auto qDDotRecovered = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); + auto qDDot = aba1.ForwardDynamics(links, math::Vector{ 0.0f }, math::Vector{}, math::Vector{ 1.0f }, Vector3{}); - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-3f); + EXPECT_NEAR(qDDot.at(0, 0), 3.0f / (mass * length * length), math::Tolerance()); } -TEST_F(TestArticulatedBodyAlgorithm, single_link_rnea_aba_roundtrip_with_gravity) +TEST_F(TestArticulatedBodyAlgorithm, horizontal_link_released_falls_at_three_g_over_two_l) { - auto link = MakePendulumLink(2.0f, 1.0f); - std::array, 1> links = { link }; + const float length{ 1.0f }; + std::array links{ MakeRod(1.0f, length, yAxis, Vector3{}) }; - math::Vector q{ 0.3f }; - math::Vector qDot{ 0.0f }; - math::Vector qDDotExpected{ 2.0f }; + auto qDDot = aba1.ForwardDynamics(links, math::Vector{ 0.0f }, math::Vector{}, math::Vector{}, gravityVector); - math::Vector tau{ 1.3333333731f }; - auto qDDotRecovered = aba1.ForwardDynamics(links, q, qDot, tau, gravityVec); - - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-2f); + EXPECT_NEAR(qDDot.at(0, 0), 3.0f * gravity / (2.0f * length), math::Tolerance()); } -TEST_F(TestArticulatedBodyAlgorithm, two_link_zero_torque_no_gravity_gives_zero_acceleration) +TEST_F(TestArticulatedBodyAlgorithm, analytic_two_link_torque_produces_commanded_acceleration) { - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_analytical_torque_produces_known_acceleration) -{ - // Textbook 2R planar arm (z-axis joints, uniform rods): - // M(q) = | m1*l1^2/3 + m2*(l1^2 + l2^2/3 + l1*l2*cos(q2)) m2*(l2^2/3 + l1*l2*cos(q2)/2) | - // | m2*(l2^2/3 + l1*l2*cos(q2)/2) m2*l2^2/3 | - // C(q,qd)*qd_0 = h*(2*qd1*qd2 + qd2^2)/2, h = -m2*l1*l2*sin(q2) - // C(q,qd)*qd_1 = -h*qd1^2/2 - // G = 0 (z-axis rotation with z-gravity => no gravitational torque) - // - // tau = M*qDDot + C*qDot => qDDot = M^{-1}*(tau - C*qDot) - - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - float q2 = -0.5f; - float qd1 = 1.0f, qd2 = -0.5f; - float qdd1 = 2.0f, qdd2 = -1.0f; - - float c2 = std::cos(q2); - float s2 = std::sin(q2); - float h = -m2 * l1 * l2 * s2; - - float M00 = m1 * l1 * l1 / 3.0f + m2 * (l1 * l1 + l2 * l2 / 3.0f + l1 * l2 * c2); - float M01 = m2 * (l2 * l2 / 3.0f + l1 * l2 * c2 / 2.0f); - float M11 = m2 * l2 * l2 / 3.0f; - - float Cqd0 = h * (2.0f * qd1 * qd2 + qd2 * qd2) / 2.0f; - float Cqd1 = -h * qd1 * qd1 / 2.0f; - - float tau0 = M00 * qdd1 + M01 * qdd2 + Cqd0; - float tau1 = M01 * qdd1 + M11 * qdd2 + Cqd1; - - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - math::Vector q{ 0.3f, q2 }; - math::Vector qDot{ qd1, qd2 }; - math::Vector tau{ tau0, tau1 }; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), qdd1, 1e-4f); - EXPECT_NEAR(qDDot.at(1, 0), qdd2, 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_coriolis_torque_gives_zero_acceleration) -{ - // If tau = C(q,qDot)*qDot exactly, then M*qDDot = 0 => qDDot = 0 - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - float q2 = -0.5f; - float qd1 = 1.0f, qd2 = -0.5f; - - float h = -m2 * l1 * l2 * std::sin(q2); - float tau0 = h * (2.0f * qd1 * qd2 + qd2 * qd2) / 2.0f; - float tau1 = -h * qd1 * qd1 / 2.0f; - - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - math::Vector q{ 0.3f, q2 }; - math::Vector qDot{ qd1, qd2 }; - math::Vector tau{ tau0, tau1 }; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-5f); - EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-5f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_rnea_aba_roundtrip_no_gravity) -{ - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.3f, -0.5f }; - math::Vector qDot{ 1.0f, -0.5f }; - math::Vector qDDotExpected{ 2.0f, -1.0f }; - math::Vector zeroGravity{}; + const float q2{ -0.5f }; + const float qd1{ 1.0f }; + const float qd2{ -0.5f }; + const float qdd1{ 2.0f }; + const float qdd2{ -1.0f }; + const float c2{ std::cos(q2) }; + const float h{ -std::sin(q2) }; + const float m11{ 1.0f / 3.0f + 1.0f + 1.0f / 3.0f + c2 }; + const float m12{ 1.0f / 3.0f + c2 / 2.0f }; + const float m22{ 1.0f / 3.0f }; + math::Vector tau{ + m11 * qdd1 + m12 * qdd2 + h * (2.0f * qd1 * qd2 + qd2 * qd2) / 2.0f, + m12 * qdd1 + m22 * qdd2 - h * qd1 * qd1 / 2.0f + }; + std::array links{ MakeRod(1.0f, 1.0f, zAxis, Vector3{}), MakeRod(1.0f, 1.0f, zAxis, Vector3{ 1.0f, 0.0f, 0.0f }) }; - math::Vector tau{ 4.1365890503f, 0.9712030888f }; - auto qDDotRecovered = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); + auto qDDot = aba2.ForwardDynamics(links, math::Vector{ 0.3f, q2 }, math::Vector{ qd1, qd2 }, tau, Vector3{}); - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-4f); - EXPECT_NEAR(qDDotRecovered.at(1, 0), qDDotExpected.at(1, 0), 1e-4f); + EXPECT_NEAR(qDDot.at(0, 0), qdd1, math::Tolerance()); + EXPECT_NEAR(qDDot.at(1, 0), qdd2, math::Tolerance()); } -TEST_F(TestArticulatedBodyAlgorithm, two_link_rnea_aba_roundtrip_with_gravity) +TEST_F(TestArticulatedBodyAlgorithm, two_link_released_horizontal_matches_analytic_acceleration) { - auto links = MakeTwoLinkArm(2.0f, 0.5f, 1.0f, 0.8f); - - math::Vector q{ 0.1f, -0.2f }; - math::Vector qDot{ 0.5f, -0.3f }; - math::Vector qDDotExpected{ 1.0f, -2.0f }; + std::array links{ MakeRod(1.0f, 1.0f, yAxis, Vector3{}), MakeRod(1.0f, 1.0f, yAxis, Vector3{ 1.0f, 0.0f, 0.0f }) }; - math::Vector tau{ 0.1949892044f, -0.0272534918f }; - auto qDDotRecovered = aba2.ForwardDynamics(links, q, qDot, tau, gravityVec); + auto qDDot = aba2.ForwardDynamics(links, math::Vector{ 0.0f, 0.0f }, math::Vector{}, math::Vector{}, gravityVector); - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-4f); - EXPECT_NEAR(qDDotRecovered.at(1, 0), qDDotExpected.at(1, 0), 1e-4f); + EXPECT_NEAR(qDDot.at(0, 0), 9.0f * gravity / 7.0f, math::Tolerance()); + EXPECT_NEAR(qDDot.at(1, 0), -12.0f * gravity / 7.0f, math::Tolerance()); } -TEST_F(TestArticulatedBodyAlgorithm, single_link_free_fall_under_gravity) +TEST_F(TestArticulatedBodyAlgorithm, spatial_chain_inverts_recursive_newton_euler) { - // y-axis pendulum at q=0: joint axis = y, CoM at [L/2, 0, 0]. - // Gravity = [0, 0, -g]. At q=0, link frame = world frame. - // Potential energy: V = -mgL*sin(q)/2 (positive y-rotation moves CoM toward -z) - // G(q) = dV/dq = -mgL*cos(q)/2 - // At q=0: G = -mgL/2 - // EoM: I_pivot * qDDot + G = 0 => qDDot = -G / I_pivot = +mgL / (2*I_pivot) - // I_pivot = mL^2/3 - // qDDot = +3g/(2L) - float mass = 1.0f; - float length = 1.0f; - float I = mass * length * length / 12.0f; - - dynamics::RevoluteJointLink link{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f } + const math::SquareMatrix inertia{ + { 0.05f, 0.01f, -0.005f }, + { 0.01f, 0.04f, 0.008f }, + { -0.005f, 0.008f, 0.03f } }; - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; + std::array links{ + Link{ 1.4f, inertia, Normalized(0.0f, 0.2f, 1.0f), Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.02f, -0.01f, 0.15f } }, + Link{ 0.9f, inertia * 0.8f, Normalized(0.1f, 1.0f, 0.0f), Vector3{ 0.05f, 0.0f, 0.3f }, Vector3{ 0.2f, 0.03f, -0.02f } }, + Link{ 0.5f, inertia * 0.5f, Normalized(1.0f, 0.3f, -0.4f), Vector3{ 0.4f, -0.05f, 0.02f }, Vector3{ 0.1f, 0.02f, 0.03f } } + }; + dynamics::RecursiveNewtonEuler rnea; + math::Vector q{ 0.4f, -0.7f, 1.1f }; + math::Vector qDot{ 0.9f, -1.3f, 0.6f }; + math::Vector expected{ 1.5f, -2.0f, 0.7f }; - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, gravityVec); + auto tau = rnea.InverseDynamics(links, q, qDot, expected, gravityVector); + auto qDDot = aba3.ForwardDynamics(links, q, qDot, tau, gravityVector); - float expected = 3.0f * gravity / (2.0f * length); - EXPECT_NEAR(qDDot.at(0, 0), expected, 1e-4f); + EXPECT_NEAR(qDDot.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(qDDot.at(1, 0), expected.at(1, 0), math::Tolerance()); + EXPECT_NEAR(qDDot.at(2, 0), expected.at(2, 0), math::Tolerance()); } diff --git a/robotics/dynamics/test/TestEulerLagrangeSolver.cpp b/robotics/dynamics/test/TestEulerLagrangeSolver.cpp index c68b078..91c3a87 100644 --- a/robotics/dynamics/test/TestEulerLagrangeSolver.cpp +++ b/robotics/dynamics/test/TestEulerLagrangeSolver.cpp @@ -5,10 +5,6 @@ namespace { - // Simple pendulum: 1-DOF - // M(q) = m * l^2 - // C(q, qDot) = 0 - // g(q) = m * gravity * l * sin(q) class SimplePendulum : public dynamics::EulerLagrangeDynamics { public: @@ -32,21 +28,17 @@ namespace } }; - // Two-link planar arm: 2-DOF - // m1, m2: link masses - // l1, l2: link lengths - // lc1, lc2: center-of-mass distances - // I1, I2: link inertias class TwoLinkPlanarArm : public dynamics::EulerLagrangeDynamics { public: static constexpr float m1 = 1.0f; static constexpr float m2 = 1.0f; static constexpr float l1 = 1.0f; + static constexpr float l2 = 1.0f; static constexpr float lc1 = 0.5f; static constexpr float lc2 = 0.5f; - static constexpr float I1 = 0.083f; // m1 * l1^2 / 12 - static constexpr float I2 = 0.083f; + static constexpr float I1 = m1 * l1 * l1 / 12.0f; + static constexpr float I2 = m2 * l2 * l2 / 12.0f; static constexpr float gravity = 9.81f; MassMatrix ComputeMassMatrix(const StateVector& q) const override @@ -98,13 +90,12 @@ namespace TEST_F(TestEulerLagrangeSolver, forward_dynamics_at_rest_returns_gravity_acceleration) { - math::Vector q{ 0.5f }; // 0.5 rad from vertical + math::Vector q{ 0.5f }; math::Vector qDot{}; math::Vector tau{}; auto qDDot = solver1Dof.ForwardDynamics(pendulum, q, qDot, tau); - // qDDot = -g * sin(q) / l = -9.81 * sin(0.5) float expected = -SimplePendulum::gravity * std::sin(0.5f) / SimplePendulum::length; EXPECT_NEAR(qDDot.at(0, 0), expected, 1e-4f); } @@ -117,7 +108,6 @@ TEST_F(TestEulerLagrangeSolver, inverse_dynamics_at_rest_returns_gravity_compens auto tau = solver1Dof.InverseDynamics(pendulum, q, qDot, qDDot); - // tau = g(q) = m * gravity * l * sin(q) float expected = SimplePendulum::mass * SimplePendulum::gravity * SimplePendulum::length * std::sin(0.5f); EXPECT_NEAR(tau.at(0, 0), expected, 1e-4f); } @@ -136,7 +126,6 @@ TEST_F(TestEulerLagrangeSolver, forward_inverse_roundtrip_consistency) TEST_F(TestEulerLagrangeSolver, identity_mass_matrix_forward_dynamics_reduces_to_subtraction) { - // A trivial model: M=I, C=0, g=0 class TrivialModel : public dynamics::EulerLagrangeDynamics { public: @@ -182,13 +171,12 @@ TEST_F(TestEulerLagrangeSolver, two_link_forward_inverse_roundtrip) TEST_F(TestEulerLagrangeSolver, two_link_at_rest_returns_gravity_torques) { - math::Vector q{ 0.0f, 0.0f }; // hanging straight down + math::Vector q{ 0.0f, 0.0f }; math::Vector qDot{}; math::Vector tau{}; auto qDDot = solver2Dof.ForwardDynamics(twoLinkArm, q, qDot, tau); - // At q = [0, 0], sin(q1)=0, sin(q1+q2)=0 → g=[0,0] → qDDot=[0,0] EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-4f); EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-4f); } diff --git a/robotics/dynamics/test/TestNewtonEulerSolver.cpp b/robotics/dynamics/test/TestNewtonEulerSolver.cpp index 0f643ac..644bf3c 100644 --- a/robotics/dynamics/test/TestNewtonEulerSolver.cpp +++ b/robotics/dynamics/test/TestNewtonEulerSolver.cpp @@ -5,7 +5,6 @@ namespace { - // Uniform sphere: I = (2/5) * m * r^2 * Identity class UniformSphere : public dynamics::NewtonEulerBody { public: @@ -19,7 +18,7 @@ namespace InertiaMatrix ComputeInertia() const override { - float i = 0.4f * mass * radius * radius; // (2/5) * m * r^2 + float i = 0.4f * mass * radius * radius; return InertiaMatrix{ { i, 0.0f, 0.0f }, { 0.0f, i, 0.0f }, @@ -28,7 +27,6 @@ namespace } }; - // Asymmetric body with distinct principal moments of inertia class AsymmetricBody : public dynamics::NewtonEulerBody { public: @@ -67,7 +65,6 @@ TEST_F(TestNewtonEulerSolver, forward_dynamics_pure_translation_no_rotation) auto result = solver.ForwardDynamics(sphere, force, torque, zero3, zero3); - // a = F / m = 6 / 2 = 3 EXPECT_NEAR(result.linear.at(0, 0), 3.0f, 1e-4f); EXPECT_NEAR(result.linear.at(1, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.linear.at(2, 0), 0.0f, 1e-4f); @@ -84,7 +81,6 @@ TEST_F(TestNewtonEulerSolver, forward_dynamics_pure_rotation_no_translation) auto result = solver.ForwardDynamics(sphere, force, torque, zero3, zero3); - // alpha = I^{-1} * torque, I = 0.2 * Identity → alpha_z = 1.0 / 0.2 = 5.0 float inertia = 0.4f * UniformSphere::mass * UniformSphere::radius * UniformSphere::radius; EXPECT_NEAR(result.angular.at(2, 0), 1.0f / inertia, 1e-3f); EXPECT_NEAR(result.linear.at(0, 0), 0.0f, 1e-4f); @@ -124,15 +120,12 @@ TEST_F(TestNewtonEulerSolver, forward_inverse_roundtrip_consistency) TEST_F(TestNewtonEulerSolver, gyroscopic_effect_on_asymmetric_body) { - // Spinning about z-axis with no external torque → gyroscopic coupling math::Vector force{}; math::Vector torque{}; math::Vector angularVel{ 0.0f, 0.0f, 1.0f }; auto result = solver.ForwardDynamics(asymmetricBody, force, torque, zero3, angularVel); - // omega x (I * omega) = [0,0,1] x [0,0,3] = [0*3 - 1*0, 1*0 - 0*3, 0*0 - 0*0] = [0,0,0] - // When omega is aligned with a principal axis, gyroscopic term vanishes EXPECT_NEAR(result.angular.at(0, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.angular.at(1, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.angular.at(2, 0), 0.0f, 1e-4f); @@ -140,16 +133,12 @@ TEST_F(TestNewtonEulerSolver, gyroscopic_effect_on_asymmetric_body) TEST_F(TestNewtonEulerSolver, gyroscopic_effect_off_principal_axis) { - // Spinning about combined axes on asymmetric body → non-zero gyroscopic term math::Vector force{}; math::Vector torque{}; math::Vector angularVel{ 1.0f, 1.0f, 0.0f }; auto result = solver.ForwardDynamics(asymmetricBody, force, torque, zero3, angularVel); - // I*omega = [1*1, 2*1, 0] = [1, 2, 0] - // omega x I*omega = [1,1,0] x [1,2,0] = [0-0, 0-0, 2-1] = [0, 0, 1] - // alpha = I^{-1} * (0 - [0,0,1]) = [0, 0, -1/3] EXPECT_NEAR(result.angular.at(0, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.angular.at(1, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.angular.at(2, 0), -1.0f / 3.0f, 1e-3f); @@ -157,7 +146,6 @@ TEST_F(TestNewtonEulerSolver, gyroscopic_effect_off_principal_axis) TEST_F(TestNewtonEulerSolver, body_frame_coriolis_effect) { - // Linear velocity + angular velocity → omega x v contributes to forward dynamics math::Vector force{}; math::Vector torque{}; math::Vector linearVel{ 1.0f, 0.0f, 0.0f }; @@ -165,7 +153,6 @@ TEST_F(TestNewtonEulerSolver, body_frame_coriolis_effect) auto result = solver.ForwardDynamics(sphere, force, torque, linearVel, angularVel); - // a = F/m - omega x v = 0 - [0,0,1] x [1,0,0] = -[0,0,0 x 1,0,0] = -[0*0-1*0, 1*1-0*0, 0*0-0*1] = -[0, 1, 0] EXPECT_NEAR(result.linear.at(0, 0), 0.0f, 1e-4f); EXPECT_NEAR(result.linear.at(1, 0), -1.0f, 1e-4f); EXPECT_NEAR(result.linear.at(2, 0), 0.0f, 1e-4f); diff --git a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp index c861a5a..e33a09d 100644 --- a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp +++ b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp @@ -1,63 +1,87 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "robotics/dynamics/EulerLagrangeSolver.hpp" #include "robotics/dynamics/RecursiveNewtonEuler.hpp" #include #include namespace { - constexpr float gravity = 9.81f; - const math::Vector gravityVec{ 0.0f, 0.0f, -gravity }; + using Vector3 = math::Vector; + using Link = dynamics::RevoluteJointLink; - // 1-DOF simple pendulum: single link rotating about z-axis - // Link hangs along x-axis, joint at origin, CoM at (l/2, 0, 0) - dynamics::RevoluteJointLink MakePendulumLink(float mass, float length) + constexpr float gravity{ 9.81f }; + const Vector3 gravityVector{ 0.0f, 0.0f, -gravity }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeRod(float mass, float length, const Vector3& axis, const Vector3& parentToJoint) { - float I = mass * length * length / 12.0f; // thin rod about CoM + const float inertia{ mass * length * length / 12.0f }; - return dynamics::RevoluteJointLink{ + return Link{ mass, math::SquareMatrix{ { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, // z-axis rotation - math::Vector{}, // joint at parent origin - math::Vector{ length / 2.0f, 0.0f, 0.0f } // CoM at half-length + { 0.0f, inertia, 0.0f }, + { 0.0f, 0.0f, inertia } }, + axis, + parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; } - // 2-DOF planar arm: two links rotating about z-axis in x-y plane - std::array, 2> MakeTwoLinkArm( - float m1, float l1, float m2, float l2) + class UniformRodTwoLinkModel + : public dynamics::EulerLagrangeDynamics { - float I1 = m1 * l1 * l1 / 12.0f; - float I2 = m2 * l2 * l2 / 12.0f; - - dynamics::RevoluteJointLink link1{ - m1, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I1, 0.0f }, - { 0.0f, 0.0f, I1 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f } - }; - - dynamics::RevoluteJointLink link2{ - m2, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I2, 0.0f }, - { 0.0f, 0.0f, I2 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{ l1, 0.0f, 0.0f }, // joint 2 at end of link 1 - math::Vector{ l2 / 2.0f, 0.0f, 0.0f } - }; - - return { link1, link2 }; - } + public: + UniformRodTwoLinkModel(float m1, float l1, float m2, float l2) + : m1{ m1 } + , l1{ l1 } + , m2{ m2 } + , l2{ l2 } + {} + + MassMatrix ComputeMassMatrix(const StateVector& q) const override + { + const float c2{ std::cos(q.at(1, 0)) }; + const float m12{ m2 * (l2 * l2 / 3.0f + l1 * l2 * c2 / 2.0f) }; + + return MassMatrix{ + { m1 * l1 * l1 / 3.0f + m2 * (l1 * l1 + l2 * l2 / 3.0f + l1 * l2 * c2), m12 }, + { m12, m2 * l2 * l2 / 3.0f } + }; + } + + StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override + { + const float h{ m2 * l1 * l2 / 2.0f * std::sin(q.at(1, 0)) }; + const float qd1{ qDot.at(0, 0) }; + const float qd2{ qDot.at(1, 0) }; + + return StateVector{ -h * (2.0f * qd1 * qd2 + qd2 * qd2), h * qd1 * qd1 }; + } + + StateVector ComputeGravityTerms(const StateVector& q) const override + { + const float c1{ std::cos(q.at(0, 0)) }; + const float c12{ std::cos(q.at(0, 0) + q.at(1, 0)) }; + + return StateVector{ + -gravity * (m1 * l1 / 2.0f * c1 + m2 * (l1 * c1 + l2 / 2.0f * c12)), + -gravity * m2 * l2 / 2.0f * c12 + }; + } + + private: + float m1; + float l1; + float m2; + float l2; + }; - class TestRecursiveNewtonEuler : public ::testing::Test + class TestRecursiveNewtonEuler + : public ::testing::Test { protected: dynamics::RecursiveNewtonEuler rnea1; @@ -65,142 +89,75 @@ namespace }; } -TEST_F(TestRecursiveNewtonEuler, single_link_gravity_torque_at_horizontal) +TEST_F(TestRecursiveNewtonEuler, horizontal_link_needs_half_weight_moment_to_hold) { - // Pendulum rotating about y-axis in x-z plane, horizontal at q=0 (link along x) - float mass = 1.0f; - float length = 1.0f; - float I = mass * length * length / 12.0f; - - dynamics::RevoluteJointLink link{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 1.0f, 0.0f }, // y-axis rotation - math::Vector{}, // joint at parent origin - math::Vector{ length / 2.0f, 0.0f, 0.0f } // CoM at half-length - }; - std::array, 1> links = { link }; + const float mass{ 1.0f }; + const float length{ 1.0f }; + std::array links{ MakeRod(mass, length, yAxis, Vector3{}) }; - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{}; + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.0f }, math::Vector{}, math::Vector{}, gravityVector); - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, gravityVec); - - // At q=0: link along x, gravity in -z, rotating about y-axis - // Fictitious base acceleration = [0, 0, g], force on CoM = [0, 0, mg] - // Torque = rCoM x force = [l/2, 0, 0] x [0, 0, mg] = [0, -mg*l/2, 0] - // Projected on y-axis: tau = -m*g*l/2 - float expected = -mass * gravity * (length / 2.0f); - EXPECT_NEAR(tau.at(0, 0), expected, 0.01f); + EXPECT_NEAR(tau.at(0, 0), -mass * gravity * length / 2.0f, math::Tolerance()); } -TEST_F(TestRecursiveNewtonEuler, single_link_zero_gravity_zero_motion_gives_zero_torque) +TEST_F(TestRecursiveNewtonEuler, rest_without_gravity_needs_no_torque) { - auto link = MakePendulumLink(1.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.5f }; - math::Vector qDot{}; - math::Vector qDDot{}; - math::Vector zeroGravity{}; + std::array links{ MakeRod(1.0f, 1.0f, zAxis, Vector3{}) }; - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.5f }, math::Vector{}, math::Vector{}, Vector3{}); - // No gravity, no motion → zero torque - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-5f); + EXPECT_NEAR(tau.at(0, 0), 0.0f, math::Tolerance()); } -TEST_F(TestRecursiveNewtonEuler, single_link_pure_acceleration_no_gravity) +TEST_F(TestRecursiveNewtonEuler, angular_acceleration_needs_end_inertia_torque) { - float mass = 2.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 1.0f }; // 1 rad/s^2 - math::Vector zeroGravity{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // For a thin rod about the end: I_end = m*l^2/3 - // Torque = I_end * qDDot = 2 * 1 / 3 * 1 = 0.6667 - float I_end = mass * length * length / 3.0f; - EXPECT_NEAR(tau.at(0, 0), I_end * 1.0f, 1e-3f); -} + const float mass{ 2.0f }; + const float length{ 1.0f }; + std::array links{ MakeRod(mass, length, zAxis, Vector3{}) }; -TEST_F(TestRecursiveNewtonEuler, two_link_zero_gravity_zero_motion_gives_zero_torque) -{ - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.0f }, math::Vector{}, math::Vector{ 1.0f }, Vector3{}); - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{}; - math::Vector zeroGravity{}; - - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(tau.at(1, 0), 0.0f, 1e-4f); + EXPECT_NEAR(tau.at(0, 0), mass * length * length / 3.0f, math::Tolerance()); } -TEST_F(TestRecursiveNewtonEuler, two_link_joint2_acceleration_only) +TEST_F(TestRecursiveNewtonEuler, constant_spin_needs_no_torque) { - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 0.0f, 1.0f }; // only joint 2 accelerates - math::Vector zeroGravity{}; + std::array links{ MakeRod(1.0f, 1.0f, zAxis, Vector3{}) }; - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.0f }, math::Vector{ 2.0f }, math::Vector{}, Vector3{}); - // Joint 2 torque = I2_end * qDDot2 = m2*l2^2/3 * 1 - float I2_end = m2 * l2 * l2 / 3.0f; - EXPECT_NEAR(tau.at(1, 0), I2_end, 1e-3f); - - // Joint 1 should also feel a reaction torque (coupling term) - EXPECT_TRUE(std::abs(tau.at(0, 0)) > 1e-4f); -} - -TEST_F(TestRecursiveNewtonEuler, single_link_centrifugal_term) -{ - // Link spinning at constant velocity → centrifugal force on CoM - float mass = 1.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{ 2.0f }; // spinning at 2 rad/s - math::Vector qDDot{}; // constant velocity - math::Vector zeroGravity{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // For a single link rotating in a plane, centrifugal force is radial - // and doesn't produce torque about the rotation axis → tau ≈ 0 - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-4f); + EXPECT_NEAR(tau.at(0, 0), 0.0f, math::Tolerance()); } -TEST_F(TestRecursiveNewtonEuler, two_link_symmetry_check) +TEST_F(TestRecursiveNewtonEuler, elbow_acceleration_couples_into_shoulder_through_mass_matrix) { - // Two identical links, both at q=0, both accelerating equally - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); + const float m2{ 1.0f }; + const float l1{ 1.0f }; + const float l2{ 1.0f }; + std::array links{ MakeRod(1.0f, l1, zAxis, Vector3{}), MakeRod(m2, l2, zAxis, Vector3{ l1, 0.0f, 0.0f }) }; - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 1.0f, 1.0f }; - math::Vector zeroGravity{}; + auto tau = rnea2.InverseDynamics(links, math::Vector{ 0.0f, 0.0f }, math::Vector{}, math::Vector{ 0.0f, 1.0f }, Vector3{}); - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); + EXPECT_NEAR(tau.at(0, 0), m2 * (l2 * l2 / 3.0f + l1 * l2 / 2.0f), math::Tolerance()); + EXPECT_NEAR(tau.at(1, 0), m2 * l2 * l2 / 3.0f, math::Tolerance()); +} - // Joint 1 should require more torque than joint 2 (it carries the full chain) - EXPECT_GT(std::abs(tau.at(0, 0)), std::abs(tau.at(1, 0))); +TEST_F(TestRecursiveNewtonEuler, matches_euler_lagrange_model_under_gravity_and_motion) +{ + const float m1{ 1.2f }; + const float l1{ 0.6f }; + const float m2{ 0.7f }; + const float l2{ 0.45f }; + std::array links{ MakeRod(m1, l1, yAxis, Vector3{}), MakeRod(m2, l2, yAxis, Vector3{ l1, 0.0f, 0.0f }) }; + UniformRodTwoLinkModel model{ m1, l1, m2, l2 }; + dynamics::EulerLagrangeSolver eulerLagrange; + math::Vector q{ 0.4f, -0.9f }; + math::Vector qDot{ 1.1f, -0.6f }; + math::Vector qDDot{ 0.8f, -1.7f }; + + auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, gravityVector); + auto reference = eulerLagrange.InverseDynamics(model, q, qDot, qDDot); + + EXPECT_NEAR(tau.at(0, 0), reference.at(0, 0), math::Tolerance()); + EXPECT_NEAR(tau.at(1, 0), reference.at(1, 0), math::Tolerance()); } diff --git a/robotics/kinematics/ForwardKinematics.hpp b/robotics/kinematics/ForwardKinematics.hpp index 98fa376..a8a8bbb 100644 --- a/robotics/kinematics/ForwardKinematics.hpp +++ b/robotics/kinematics/ForwardKinematics.hpp @@ -4,11 +4,12 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "robotics/dynamics/RevoluteJointLink.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" #include +#include namespace kinematics { @@ -26,15 +27,22 @@ namespace kinematics using LinkArray = std::array, NumLinks>; using PositionArray = std::array; - ForwardKinematics() = default; + explicit ForwardKinematics(const Vector3& toolOffset); - // Computes the 3D positions of all joints (including base at index 0 - // and end-effector at index NumLinks) given link descriptions and - // joint angles. OPTIMIZE_FOR_SPEED PositionArray Compute(const LinkArray& links, const JointVector& q) const; + + const Vector3& ToolOffset() const; + + private: + Vector3 toolOffset; }; + template + ForwardKinematics::ForwardKinematics(const Vector3& toolOffset) + : toolOffset{ toolOffset } + {} + template OPTIMIZE_FOR_SPEED typename ForwardKinematics::PositionArray @@ -42,26 +50,28 @@ namespace kinematics const JointVector& q) const { PositionArray positions{}; - positions[0] = Vector3{}; + positions[0] = links[0].parentToJoint; auto R = Matrix3::Identity(); for (std::size_t i = 0; i < NumLinks; ++i) { + assert(dynamics::HasUnitJointAxis(links[i])); R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - Vector3 linkExtent; - if (i + 1 < NumLinks) - linkExtent = links[i + 1].parentToJoint; - else - linkExtent = links[i].jointToCoM * T(2); - - positions[i + 1] = positions[i] + R * linkExtent; + const Vector3& offsetToNext = (i + 1 < NumLinks) ? links[i + 1].parentToJoint : toolOffset; + positions[i + 1] = positions[i] + R * offsetToNext; } return positions; } + template + const typename ForwardKinematics::Vector3& ForwardKinematics::ToolOffset() const + { + return toolOffset; + } + #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD extern template class ForwardKinematics; extern template class ForwardKinematics; diff --git a/robotics/kinematics/InverseKinematics.hpp b/robotics/kinematics/InverseKinematics.hpp index 6de0d52..bfc7af8 100644 --- a/robotics/kinematics/InverseKinematics.hpp +++ b/robotics/kinematics/InverseKinematics.hpp @@ -5,14 +5,13 @@ #endif #include "infra/util/ReallyAssert.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" -#include "robotics/kinematics/ForwardKinematics.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" #include "numerical/solvers/GaussianElimination.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/kinematics/ForwardKinematics.hpp" #include -#include #include namespace kinematics @@ -52,7 +51,7 @@ namespace kinematics using LinkArray = std::array, NumLinks>; using PositionArray = std::array; - explicit InverseKinematics(InverseKinematicsConfig cfg = {}); + explicit InverseKinematics(const Vector3& toolOffset, InverseKinematicsConfig cfg = {}); OPTIMIZE_FOR_SPEED InverseKinematicsResult Solve( const LinkArray& links, @@ -65,17 +64,22 @@ namespace kinematics const JointVector& q, const PositionArray& positions) const; + JointVector DampedLeastSquaresStep(const Jacobian& jacobian, const Vector3& error) const; + + T PositionError(const LinkArray& links, const JointVector& q, const Vector3& target) const; + InverseKinematicsConfig config; ForwardKinematics fk; - mutable solvers::GaussianElimination solver; }; template - InverseKinematics::InverseKinematics(InverseKinematicsConfig cfg) - : config(cfg) + InverseKinematics::InverseKinematics(const Vector3& toolOffset, InverseKinematicsConfig cfg) + : config{ cfg } + , fk{ toolOffset } { really_assert(config.dampingFactor > T(0)); really_assert(config.tolerance > T(0)); + really_assert(config.maxIterations > 0); } template @@ -86,55 +90,21 @@ namespace kinematics const Vector3& target, const JointVector& initialQ) const { - JointVector q = initialQ; - - std::size_t iter = 0; - T error = T(0); + JointVector q{ initialQ }; - for (; iter < config.maxIterations; ++iter) + for (std::size_t iteration = 0; iteration < config.maxIterations; ++iteration) { - auto positions = fk.Compute(links, q); - const Vector3& eePos = positions[NumLinks]; - - Vector3 e = target - eePos; - error = math::VectorNorm(e); - - if (error < config.tolerance) - return { q, error, iter, true }; - - Jacobian J = ComputeJacobian(links, q, positions); - - math::SquareMatrix JJt{}; - for (std::size_t r = 0; r < 3; ++r) - for (std::size_t c = 0; c < 3; ++c) - { - T sum = T(0); - for (std::size_t k = 0; k < NumLinks; ++k) - sum += J.at(r, k) * J.at(c, k); - JJt.at(r, c) = sum; - } - - T lambda2 = config.dampingFactor * config.dampingFactor; - JJt.at(0, 0) += lambda2; - JJt.at(1, 1) += lambda2; - JJt.at(2, 2) += lambda2; - - Vector3 y = solver.Solve(JJt, e); - - for (std::size_t i = 0; i < NumLinks; ++i) - { - T dq = T(0); - for (std::size_t r = 0; r < 3; ++r) - dq += J.at(r, i) * y.at(r, 0); - q.at(i, 0) += dq; - } - } + const auto positions = fk.Compute(links, q); + const Vector3 error{ target - positions[NumLinks] }; + const T errorNorm{ math::VectorNorm(error) }; - auto positions = fk.Compute(links, q); - Vector3 e = target - positions[NumLinks]; - error = math::VectorNorm(e); + if (errorNorm < config.tolerance) + return { q, errorNorm, iteration, true }; + + q = q + DampedLeastSquaresStep(ComputeJacobian(links, q, positions), error); + } - return { q, error, iter, false }; + return { q, PositionError(links, q, target), config.maxIterations, false }; } template @@ -144,20 +114,19 @@ namespace kinematics const JointVector& q, const PositionArray& positions) const { - const Vector3& eePos = positions[NumLinks]; + const Vector3& toolPosition = positions[NumLinks]; - Matrix3 R = Matrix3::Identity(); + Matrix3 R{ Matrix3::Identity() }; Jacobian J{}; for (std::size_t i = 0; i < NumLinks; ++i) { - Vector3 worldAxis = R * links[i].jointAxis; - Vector3 jointToEe = eePos - positions[i]; - Vector3 col = math::CrossProduct(worldAxis, jointToEe); + const Vector3 worldAxis{ R * links[i].jointAxis }; + const Vector3 column{ math::CrossProduct(worldAxis, Vector3{ toolPosition - positions[i] }) }; - J.at(0, i) = col.at(0, 0); - J.at(1, i) = col.at(1, 0); - J.at(2, i) = col.at(2, 0); + J.at(0, i) = column.at(0, 0); + J.at(1, i) = column.at(1, 0); + J.at(2, i) = column.at(2, 0); R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); } @@ -165,6 +134,26 @@ namespace kinematics return J; } + template + typename InverseKinematics::JointVector + InverseKinematics::DampedLeastSquaresStep(const Jacobian& jacobian, const Vector3& error) const + { + const T lambdaSquared{ config.dampingFactor * config.dampingFactor }; + const Matrix3 damped{ jacobian * jacobian.Transpose() + math::ScaledIdentity(lambdaSquared) }; + + solvers::GaussianElimination solver; + const Vector3 y{ solver.Solve(damped, error) }; + + return jacobian.Transpose() * y; + } + + template + T InverseKinematics::PositionError(const LinkArray& links, const JointVector& q, const Vector3& target) const + { + const auto positions = fk.Compute(links, q); + return math::VectorNorm(Vector3{ target - positions[NumLinks] }); + } + #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD extern template class InverseKinematics; extern template class InverseKinematics; diff --git a/robotics/kinematics/test/TestForwardKinematics.cpp b/robotics/kinematics/test/TestForwardKinematics.cpp index 759e6d7..945335d 100644 --- a/robotics/kinematics/test/TestForwardKinematics.cpp +++ b/robotics/kinematics/test/TestForwardKinematics.cpp @@ -1,300 +1,161 @@ +#include "numerical/math/Tolerance.hpp" #include "robotics/kinematics/ForwardKinematics.hpp" -#include #include #include namespace { - constexpr float tolerance = 5e-4f; + using Vector3 = math::Vector; + using Link = dynamics::RevoluteJointLink; + constexpr float pi = std::numbers::pi_v; - dynamics::RevoluteJointLink MakeLink(float mass, float length, - const math::Vector& axis, - const math::Vector& parentToJoint, - const math::Vector& jointToCoM) + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeLink(const Vector3& axis, const Vector3& parentToJoint, const Vector3& jointToCoM) { - float I = mass * length * length / 12.0f; - - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { I, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - axis, - parentToJoint, - jointToCoM - }; + return Link{ 1.0f, math::SquareMatrix::Identity(), axis, parentToJoint, jointToCoM }; } - dynamics::RevoluteJointLink MakePlanarLink(float mass, float length, - const math::Vector& parentToJoint) + Vector3 Along(const Vector3& axis, float length) { - return MakeLink(mass, length, - math::Vector{ 0.0f, 0.0f, 1.0f }, - parentToJoint, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); + return axis * length; } - class TestForwardKinematics : public ::testing::Test + class TestForwardKinematics + : public ::testing::Test { protected: - kinematics::ForwardKinematics fk1; - kinematics::ForwardKinematics fk2; - kinematics::ForwardKinematics fk3; + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + EXPECT_NEAR(actual.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(actual.at(1, 0), expected.at(1, 0), math::Tolerance()); + EXPECT_NEAR(actual.at(2, 0), expected.at(2, 0), math::Tolerance()); + } }; } -TEST_F(TestForwardKinematics, single_link_at_zero_angle) +TEST_F(TestForwardKinematics, single_link_at_zero_angle_places_tool_along_offset) { - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; + std::array links{ MakeLink(zAxis, Vector3{}, Along(xAxis, 0.5f)) }; + kinematics::ForwardKinematics fk{ Along(xAxis, 1.0f) }; - math::Vector q{ 0.0f }; - auto positions = fk1.Compute(links, q); + auto positions = fk.Compute(links, math::Vector{ 0.0f }); - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[0].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[0].at(2, 0), 0.0f, tolerance); - - EXPECT_NEAR(positions[1].at(0, 0), length, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); + ExpectNear(positions[0], Vector3{}); + ExpectNear(positions[1], Vector3{ 1.0f, 0.0f, 0.0f }); } -TEST_F(TestForwardKinematics, single_link_rotated_90_degrees) +TEST_F(TestForwardKinematics, z_axis_rotation_follows_right_hand_rule) { - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; + std::array links{ MakeLink(zAxis, Vector3{}, Along(xAxis, 0.5f)) }; + kinematics::ForwardKinematics fk{ Along(xAxis, 1.0f) }; - math::Vector q{ pi / 2.0f }; - auto positions = fk1.Compute(links, q); + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f }); - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), length, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); + ExpectNear(positions[1], Vector3{ 0.0f, 1.0f, 0.0f }); } -TEST_F(TestForwardKinematics, single_link_rotated_180_degrees) +TEST_F(TestForwardKinematics, y_axis_rotation_follows_right_hand_rule) { - float length = 2.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; + std::array links{ MakeLink(yAxis, Vector3{}, Along(xAxis, 0.5f)) }; + kinematics::ForwardKinematics fk{ Along(xAxis, 1.0f) }; - math::Vector q{ pi }; - auto positions = fk1.Compute(links, q); + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f }); - EXPECT_NEAR(positions[1].at(0, 0), -length, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); + ExpectNear(positions[1], Vector3{ 0.0f, 0.0f, -1.0f }); } -TEST_F(TestForwardKinematics, two_link_planar_both_zero) +TEST_F(TestForwardKinematics, two_link_planar_elbow_at_ninety_degrees) { - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, 0.0f }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 + l2, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_planar_first_joint_90) -{ - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ pi / 2.0f, 0.0f }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), l1, tolerance); - - EXPECT_NEAR(positions[2].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), l1 + l2, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_planar_second_joint_90) -{ - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, pi / 2.0f }; - auto positions = fk2.Compute(links, q); + std::array links{ + MakeLink(zAxis, Vector3{}, Along(xAxis, 0.5f)), + MakeLink(zAxis, Along(xAxis, 1.0f), Along(xAxis, 0.4f)) + }; + kinematics::ForwardKinematics fk{ Along(xAxis, 0.8f) }; - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); + auto positions = fk.Compute(links, math::Vector{ 0.0f, pi / 2.0f }); - EXPECT_NEAR(positions[2].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), l2, tolerance); + ExpectNear(positions[1], Vector3{ 1.0f, 0.0f, 0.0f }); + ExpectNear(positions[2], Vector3{ 1.0f, 0.8f, 0.0f }); } -TEST_F(TestForwardKinematics, two_link_folded_back) +TEST_F(TestForwardKinematics, two_link_planar_folded_back) { - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, pi }; - auto positions = fk2.Compute(links, q); + std::array links{ + MakeLink(zAxis, Vector3{}, Along(xAxis, 0.5f)), + MakeLink(zAxis, Along(xAxis, 1.0f), Along(xAxis, 0.4f)) + }; + kinematics::ForwardKinematics fk{ Along(xAxis, 0.8f) }; - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 - l2, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); -} + auto positions = fk.Compute(links, math::Vector{ 0.0f, pi }); -TEST_F(TestForwardKinematics, y_axis_rotation_single_link) -{ - float length = 1.0f; - auto link = MakeLink(1.0f, length, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); - std::array, 1> links = { link }; - - math::Vector q{ pi / 2.0f }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(std::sqrt(positions[1].at(0, 0) * positions[1].at(0, 0) + - positions[1].at(2, 0) * positions[1].at(2, 0)), - length, tolerance); + ExpectNear(positions[2], Vector3{ 0.2f, 0.0f, 0.0f }); } -TEST_F(TestForwardKinematics, base_position_always_origin) +TEST_F(TestForwardKinematics, base_offset_translates_the_whole_chain) { - float length = 1.5f; - auto link = MakePlanarLink(2.0f, length, math::Vector{}); - std::array, 1> links = { link }; + std::array links{ + MakeLink(zAxis, Along(zAxis, 0.3f), Along(xAxis, 0.5f)), + MakeLink(zAxis, Along(xAxis, 1.0f), Along(xAxis, 0.4f)) + }; + kinematics::ForwardKinematics fk{ Along(xAxis, 0.8f) }; - for (float angle : { 0.0f, pi / 4.0f, pi / 2.0f, pi, -pi / 3.0f }) - { - math::Vector q{ angle }; - auto positions = fk1.Compute(links, q); + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f, 0.0f }); - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, 1e-6f); - EXPECT_NEAR(positions[0].at(1, 0), 0.0f, 1e-6f); - EXPECT_NEAR(positions[0].at(2, 0), 0.0f, 1e-6f); - } + ExpectNear(positions[0], Vector3{ 0.0f, 0.0f, 0.3f }); + ExpectNear(positions[1], Vector3{ 0.0f, 1.0f, 0.3f }); + ExpectNear(positions[2], Vector3{ 0.0f, 1.8f, 0.3f }); } -TEST_F(TestForwardKinematics, end_effector_distance_equals_link_length) +TEST_F(TestForwardKinematics, tool_point_is_independent_of_center_of_mass) { - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; + std::array links{ MakeLink(zAxis, Vector3{}, Along(xAxis, 0.1f)) }; + kinematics::ForwardKinematics fk{ Along(xAxis, 1.0f) }; - for (float angle : { 0.0f, pi / 6.0f, pi / 3.0f, pi / 2.0f, 2.0f * pi / 3.0f }) - { - math::Vector q{ angle }; - auto positions = fk1.Compute(links, q); - - float dx = positions[1].at(0, 0) - positions[0].at(0, 0); - float dy = positions[1].at(1, 0) - positions[0].at(1, 0); - float dz = positions[1].at(2, 0) - positions[0].at(2, 0); - float dist = std::sqrt(dx * dx + dy * dy + dz * dz); + auto positions = fk.Compute(links, math::Vector{ 0.0f }); - EXPECT_NEAR(dist, length, tolerance); - } + ExpectNear(positions[1], Vector3{ 1.0f, 0.0f, 0.0f }); } -TEST_F(TestForwardKinematics, three_link_all_zero) +TEST_F(TestForwardKinematics, tool_offset_is_expressed_in_last_link_frame) { - float l1 = 1.0f, l2 = 0.8f, l3 = 0.6f; + std::array links{ MakeLink(zAxis, Vector3{}, Along(xAxis, 0.5f)) }; + kinematics::ForwardKinematics fk{ Vector3{ 1.0f, 0.2f, 0.0f } }; - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - auto link3 = MakePlanarLink(0.6f, l3, math::Vector{ l2, 0.0f, 0.0f }); - std::array, 3> links = { link1, link2, link3 }; + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f }); - math::Vector q{ 0.0f, 0.0f, 0.0f }; - auto positions = fk3.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 + l2, tolerance); - EXPECT_NEAR(positions[3].at(0, 0), l1 + l2 + l3, tolerance); - - for (int i = 0; i <= 3; ++i) - { - EXPECT_NEAR(positions[i].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[i].at(2, 0), 0.0f, tolerance); - } + ExpectNear(positions[1], Vector3{ -0.2f, 1.0f, 0.0f }); } -TEST_F(TestForwardKinematics, two_link_total_reach) +TEST_F(TestForwardKinematics, mixed_axes_compose_in_joint_order) { - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - for (float q1 : { 0.0f, pi / 4.0f, pi / 2.0f }) - { - for (float q2 : { 0.0f, pi / 3.0f, pi, -pi / 2.0f }) - { - math::Vector q{ q1, q2 }; - auto positions = fk2.Compute(links, q); + std::array links{ + MakeLink(zAxis, Vector3{}, Along(xAxis, 0.25f)), + MakeLink(yAxis, Along(xAxis, 0.5f), Along(xAxis, 0.2f)) + }; + kinematics::ForwardKinematics fk{ Along(xAxis, 0.4f) }; - float x = positions[2].at(0, 0); - float y = positions[2].at(1, 0); - float z = positions[2].at(2, 0); - float dist = std::sqrt(x * x + y * y + z * z); + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f, pi / 2.0f }); - EXPECT_LE(dist, l1 + l2 + 1e-3f); - } - } + ExpectNear(positions[1], Vector3{ 0.0f, 0.5f, 0.0f }); + ExpectNear(positions[2], Vector3{ 0.0f, 0.5f, -0.4f }); } -TEST_F(TestForwardKinematics, spatial_3dof_vertical_base) +TEST_F(TestForwardKinematics, spatial_three_link_vertical_base) { - float l1 = 0.5f, l2 = 0.4f, l3 = 0.3f; - - auto link1 = MakeLink(1.0f, l1, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ 0.0f, 0.0f, l1 / 2.0f }); - - auto link2 = MakeLink(0.8f, l2, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ 0.0f, 0.0f, l1 }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f }); - - auto link3 = MakeLink(0.6f, l3, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l2, 0.0f, 0.0f }, - math::Vector{ l3 / 2.0f, 0.0f, 0.0f }); - - std::array, 3> links = { link1, link2, link3 }; - - math::Vector q{ 0.0f, 0.0f, 0.0f }; - auto positions = fk3.Compute(links, q); - - EXPECT_NEAR(positions[1].at(2, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); + std::array links{ + MakeLink(zAxis, Vector3{}, Along(zAxis, 0.25f)), + MakeLink(yAxis, Along(zAxis, 0.5f), Along(xAxis, 0.2f)), + MakeLink(yAxis, Along(xAxis, 0.4f), Along(xAxis, 0.15f)) + }; + kinematics::ForwardKinematics fk{ Along(xAxis, 0.3f) }; - EXPECT_NEAR(positions[2].at(0, 0), l2, tolerance); - EXPECT_NEAR(positions[2].at(2, 0), l1, tolerance); + auto positions = fk.Compute(links, math::Vector{ pi / 2.0f, -pi / 2.0f, 0.0f }); - EXPECT_NEAR(positions[3].at(0, 0), l2 + l3, tolerance); - EXPECT_NEAR(positions[3].at(2, 0), l1, tolerance); + ExpectNear(positions[1], Vector3{ 0.0f, 0.0f, 0.5f }); + ExpectNear(positions[2], Vector3{ 0.0f, 0.0f, 0.9f }); + ExpectNear(positions[3], Vector3{ 0.0f, 0.0f, 1.2f }); } diff --git a/robotics/kinematics/test/TestInverseKinematics.cpp b/robotics/kinematics/test/TestInverseKinematics.cpp index e939816..7c93f84 100644 --- a/robotics/kinematics/test/TestInverseKinematics.cpp +++ b/robotics/kinematics/test/TestInverseKinematics.cpp @@ -1,3 +1,4 @@ +#include "numerical/math/Tolerance.hpp" #include "robotics/kinematics/ForwardKinematics.hpp" #include "robotics/kinematics/InverseKinematics.hpp" #include @@ -6,265 +7,147 @@ namespace { - constexpr float tolerance = 1e-3f; - constexpr float pi = std::numbers::pi_v; + using Vector3 = math::Vector; + using Link = dynamics::RevoluteJointLink; - dynamics::RevoluteJointLink MakeLink(float mass, float length, - const math::Vector& axis, - const math::Vector& parentToJoint, - const math::Vector& jointToCoM) - { - float I = mass * length * length / 12.0f; + constexpr float pi = std::numbers::pi_v; - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { I, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - axis, - parentToJoint, - jointToCoM - }; - } + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; - dynamics::RevoluteJointLink MakePlanarLink(float mass, float length, - const math::Vector& parentToJoint) + Link MakeLink(const Vector3& axis, const Vector3& parentToJoint, const Vector3& jointToCoM) { - return MakeLink(mass, length, - math::Vector{ 0.0f, 0.0f, 1.0f }, - parentToJoint, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); + return Link{ 1.0f, math::SquareMatrix::Identity(), axis, parentToJoint, jointToCoM }; } - class TestInverseKinematics : public ::testing::Test + class TestInverseKinematics + : public ::testing::Test { protected: - kinematics::InverseKinematics ik1; - kinematics::InverseKinematics ik2; - kinematics::InverseKinematics ik3; - kinematics::ForwardKinematics fk1; - kinematics::ForwardKinematics fk2; - kinematics::ForwardKinematics fk3; + const Vector3 planarTool{ 0.8f, 0.0f, 0.0f }; + const std::array planarArm{ + MakeLink(zAxis, Vector3{}, Vector3{ 0.5f, 0.0f, 0.0f }), + MakeLink(zAxis, Vector3{ 1.0f, 0.0f, 0.0f }, Vector3{ 0.4f, 0.0f, 0.0f }) + }; + + template + void ExpectToolAt(const std::array& links, const Vector3& tool, + const math::Vector& q, const Vector3& target) const + { + kinematics::ForwardKinematics fk{ tool }; + auto positions = fk.Compute(links, q); + + EXPECT_NEAR(positions[N].at(0, 0), target.at(0, 0), math::Tolerance()); + EXPECT_NEAR(positions[N].at(1, 0), target.at(1, 0), math::Tolerance()); + EXPECT_NEAR(positions[N].at(2, 0), target.at(2, 0), math::Tolerance()); + } }; } -TEST_F(TestInverseKinematics, single_link_reaches_target_on_arc) +TEST_F(TestInverseKinematics, single_link_recovers_reference_angle) { - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector referenceQ{ pi / 3.0f }; - auto fkRef = fk1.Compute(links, referenceQ); - math::Vector target = fkRef[1]; + std::array links{ MakeLink(zAxis, Vector3{}, Vector3{ 0.5f, 0.0f, 0.0f }) }; + kinematics::InverseKinematics ik{ xAxis }; + Vector3 target{ std::cos(pi / 3.0f), std::sin(pi / 3.0f), 0.0f }; - math::Vector initialQ{ 0.0f }; - auto result = ik1.Solve(links, target, initialQ); + auto result = ik.Solve(links, target, math::Vector{ 0.0f }); EXPECT_TRUE(result.converged); - EXPECT_NEAR(result.q.at(0, 0), pi / 3.0f, 1e-3f); - - auto positions = fk1.Compute(links, result.q); - EXPECT_NEAR(positions[1].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[1].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[1].at(2, 0), target.at(2, 0), tolerance); + EXPECT_NEAR(result.q.at(0, 0), pi / 3.0f, math::Tolerance()); } -TEST_F(TestInverseKinematics, starts_at_solution_converges_immediately) +TEST_F(TestInverseKinematics, initial_guess_at_solution_returns_without_iterating) { - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; + kinematics::InverseKinematics ik{ planarTool }; + math::Vector initialQ{ pi / 4.0f, -pi / 6.0f }; + kinematics::ForwardKinematics fk{ planarTool }; + Vector3 target{ fk.Compute(planarArm, initialQ)[2] }; - math::Vector initialQ{ pi / 4.0f }; - auto fkPositions = fk1.Compute(links, initialQ); - math::Vector target = fkPositions[1]; - - auto result = ik1.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, target, initialQ); EXPECT_TRUE(result.converged); - EXPECT_LT(result.iterations, 2u); + EXPECT_EQ(result.iterations, 0u); EXPECT_LT(result.finalError, 1e-4f); } -TEST_F(TestInverseKinematics, two_link_planar_reaches_known_position) +TEST_F(TestInverseKinematics, two_link_planar_reaches_reachable_target) { - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; + kinematics::InverseKinematics ik{ planarTool }; + Vector3 target{ 0.5f, 1.0f, 0.0f }; - auto result = ik2.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, target, math::Vector{ 0.0f, 0.0f }); EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[2].at(2, 0), target.at(2, 0), tolerance); + ExpectToolAt(planarArm, planarTool, result.q, target); } -TEST_F(TestInverseKinematics, two_link_planar_fully_extended) +TEST_F(TestInverseKinematics, target_near_full_extension_converges) { - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.1f, 0.1f }; - math::Vector target{ l1 + l2 - 0.05f, 0.0f, 0.0f }; + kinematics::InverseKinematics ik{ planarTool }; + Vector3 target{ 1.75f, 0.0f, 0.0f }; - auto result = ik2.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, target, math::Vector{ 0.1f, 0.1f }); EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); + ExpectToolAt(planarArm, planarTool, result.q, target); } -TEST_F(TestInverseKinematics, two_link_planar_folded_elbow_up) +TEST_F(TestInverseKinematics, target_near_base_converges) { - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ pi / 4.0f, -pi / 4.0f }; - math::Vector target{ 1.2f, 0.5f, 0.0f }; + kinematics::InverseKinematics ik{ planarTool }; + Vector3 target{ 0.4f, 0.0f, 0.0f }; - auto result = ik2.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, target, math::Vector{ pi / 3.0f, -2.0f * pi / 3.0f }); EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); + ExpectToolAt(planarArm, planarTool, result.q, target); } -TEST_F(TestInverseKinematics, unreachable_target_does_not_converge) +TEST_F(TestInverseKinematics, unreachable_target_exhausts_iterations) { - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 10.0f, 10.0f, 10.0f }; + kinematics::InverseKinematicsConfig config; + config.maxIterations = 50; + kinematics::InverseKinematics ik{ planarTool, config }; - auto result = ik2.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, Vector3{ 10.0f, 10.0f, 10.0f }, math::Vector{ 0.0f, 0.0f }); EXPECT_FALSE(result.converged); - EXPECT_GT(result.finalError, 1e-4f); - EXPECT_EQ(result.iterations, 100u); + EXPECT_EQ(result.iterations, 50u); + EXPECT_GT(result.finalError, config.tolerance); } -TEST_F(TestInverseKinematics, result_contains_populated_iteration_count_and_error) +TEST_F(TestInverseKinematics, heavy_damping_still_converges_to_exact_target) { - float l1 = 1.0f, l2 = 0.8f; + kinematics::InverseKinematicsConfig config; + config.dampingFactor = 0.5f; + config.maxIterations = 500; + kinematics::InverseKinematics ik{ planarTool, config }; + Vector3 target{ 0.5f, 1.0f, 0.0f }; - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_GT(result.iterations, 0u); - EXPECT_LT(result.finalError, 1e-4f); -} - -TEST_F(TestInverseKinematics, custom_damping_factor_still_converges) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - kinematics::InverseKinematicsConfig cfg; - cfg.dampingFactor = 0.5f; - cfg.tolerance = 1e-4f; - cfg.maxIterations = 500; - kinematics::InverseKinematics ikHighDamping{ cfg }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; - - auto result = ikHighDamping.Solve(links, target, initialQ); + auto result = ik.Solve(planarArm, target, math::Vector{ 0.0f, 0.0f }); EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); + EXPECT_LT(result.finalError, config.tolerance); } -TEST_F(TestInverseKinematics, three_link_spatial_reaches_target) +TEST_F(TestInverseKinematics, spatial_chain_with_base_and_tool_offsets_reaches_fk_target) { - float l1 = 0.5f, l2 = 0.4f, l3 = 0.3f; - - auto link1 = MakeLink(1.0f, l1, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f }); - auto link2 = MakeLink(0.8f, l2, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l1, 0.0f, 0.0f }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f }); - auto link3 = MakeLink(0.6f, l3, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l2, 0.0f, 0.0f }, - math::Vector{ l3 / 2.0f, 0.0f, 0.0f }); - std::array, 3> links = { link1, link2, link3 }; - - kinematics::InverseKinematicsConfig cfg; - cfg.dampingFactor = 0.05f; - cfg.tolerance = 1e-4f; - cfg.maxIterations = 300; - kinematics::InverseKinematics ik3Spatial{ cfg }; - - math::Vector referenceQ{ pi / 4.0f, pi / 6.0f, -pi / 3.0f }; - auto fkRef = fk3.Compute(links, referenceQ); - math::Vector target = fkRef[3]; - - math::Vector initialQ{ 0.0f, 0.0f, 0.0f }; - auto result = ik3Spatial.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk3.Compute(links, result.q); - EXPECT_NEAR(positions[3].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[3].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[3].at(2, 0), target.at(2, 0), tolerance); -} - -TEST_F(TestInverseKinematics, two_link_target_at_origin_region) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ pi / 3.0f, -2.0f * pi / 3.0f }; - math::Vector target{ 0.4f, 0.0f, 0.0f }; + const Vector3 tool{ 0.25f, 0.0f, 0.05f }; + const std::array links{ + MakeLink(zAxis, Vector3{ 0.0f, 0.0f, 0.2f }, Vector3{ 0.0f, 0.0f, 0.1f }), + MakeLink(yAxis, Vector3{ 0.0f, 0.0f, 0.5f }, Vector3{ 0.05f, 0.0f, 0.0f }), + MakeLink(yAxis, Vector3{ 0.4f, 0.0f, 0.0f }, Vector3{ 0.05f, 0.0f, 0.0f }) + }; + kinematics::InverseKinematicsConfig config; + config.dampingFactor = 0.05f; + config.maxIterations = 300; + kinematics::InverseKinematics ik{ tool, config }; + kinematics::ForwardKinematics fk{ tool }; + Vector3 target{ fk.Compute(links, math::Vector{ pi / 4.0f, pi / 6.0f, -pi / 3.0f })[3] }; - auto result = ik2.Solve(links, target, initialQ); + auto result = ik.Solve(links, target, math::Vector{ 0.0f, 0.0f, 0.0f }); EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); + ExpectToolAt(links, tool, result.q, target); } diff --git a/scripts/validate-docs.py b/scripts/validate-docs.py index 9e9ac4a..b2d6d62 100644 --- a/scripts/validate-docs.py +++ b/scripts/validate-docs.py @@ -2,11 +2,12 @@ """Validate that algorithm documentation files follow the project template. Checks every .md file under doc/ (excluding README.md, TEMPLATE.md, and backups) -for the required section headings defined in the template. +for the required section headings defined in the template, in template order, +and checks that relative links in every .md file under doc/ resolve. Exit codes: 0 — all files pass, or no algorithm documentation files found - 1 — doc directory is missing, or one or more files have missing sections + 1 — doc directory is missing, or one or more files fail a check """ import pathlib @@ -35,11 +36,32 @@ def extract_h2_headings(text: str) -> list[str]: return re.findall(r"^## (.+)$", text, re.MULTILINE) +LINK_PATTERN = re.compile(r"\]\(([^)\s]+)\)") + + def validate_file(path: pathlib.Path) -> list[str]: text = path.read_text(encoding="utf-8") headings = extract_h2_headings(text) - missing = [s for s in REQUIRED_SECTIONS if s not in headings] - return missing + problems = [f"missing: ## {s}" for s in REQUIRED_SECTIONS if s not in headings] + + present = [h for h in headings if h in REQUIRED_SECTIONS] + expected = [s for s in REQUIRED_SECTIONS if s in present] + if present != expected: + problems.append("sections out of template order: " + " → ".join(present)) + + return problems + + +def broken_links(path: pathlib.Path) -> list[str]: + text = path.read_text(encoding="utf-8") + problems = [] + for target in LINK_PATTERN.findall(text): + if target.startswith(("http://", "https://", "mailto:", "#")): + continue + relative = target.split("#", 1)[0] + if relative and not (path.parent / relative).exists(): + problems.append(f"broken link: {target}") + return problems def main() -> int: @@ -60,17 +82,24 @@ def main() -> int: failures: list[tuple[pathlib.Path, list[str]]] = [] for path in md_files: - missing = validate_file(path) - if missing: - failures.append((path, missing)) + problems = validate_file(path) + if problems: + failures.append((path, problems)) + + for path in sorted(DOC_ROOT.rglob("*.md")): + if path.name == "TEMPLATE.md" or any(part in SKIP_DIRS for part in path.parts): + continue + problems = broken_links(path) + if problems: + failures.append((path, problems)) if failures: print(f"FAIL: {len(failures)} file(s) do not follow the template:\n") for path, missing in failures: rel = path.relative_to(DOC_ROOT) print(f" {rel}") - for section in missing: - print(f" - missing: ## {section}") + for problem in missing: + print(f" - {problem}") print() return 1 diff --git a/simulator/dynamics/RobotArm/CMakeLists.txt b/simulator/dynamics/RobotArm/CMakeLists.txt index cda79c7..f122385 100644 --- a/simulator/dynamics/RobotArm/CMakeLists.txt +++ b/simulator/dynamics/RobotArm/CMakeLists.txt @@ -18,8 +18,4 @@ target_sources(robotics.simulator.dynamics.robot_arm PRIVATE if(MSVC) target_compile_options(robotics.simulator.dynamics.robot_arm PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(robotics.simulator.dynamics.robot_arm PRIVATE - -Wno-error - ) endif() diff --git a/simulator/dynamics/RobotArm/application/CMakeLists.txt b/simulator/dynamics/RobotArm/application/CMakeLists.txt index 50bd0d0..aa22ca7 100644 --- a/simulator/dynamics/RobotArm/application/CMakeLists.txt +++ b/simulator/dynamics/RobotArm/application/CMakeLists.txt @@ -20,10 +20,6 @@ target_sources(robotics.simulator.dynamics.robotarm.application PRIVATE if(MSVC) target_compile_options(robotics.simulator.dynamics.robotarm.application PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(robotics.simulator.dynamics.robotarm.application PRIVATE - -Wno-error - ) endif() if (ROBOTICS_TOOLBOX_BUILD_TESTS) diff --git a/simulator/dynamics/RobotArm/application/RobotArmForm.cpp b/simulator/dynamics/RobotArm/application/RobotArmForm.cpp index d8ad244..1c1f758 100644 --- a/simulator/dynamics/RobotArm/application/RobotArmForm.cpp +++ b/simulator/dynamics/RobotArm/application/RobotArmForm.cpp @@ -27,7 +27,7 @@ namespace simulator::dynamics GroupSpec{ field::firstLink, "Link 1", {} }, GroupSpec{ field::secondLink, "Link 2", {} }, GroupSpec{ field::thirdLink, "Link 3", spatialOnly }, - GroupSpec{ field::torques, "Joint Torques (N·m)", {} }, + GroupSpec{ field::torques, "Joint Torques (0.1 N·m)", {} }, GroupSpec{ field::positions, "Initial Position (deg)", {} }, GroupSpec{ field::simulation, "Simulation", {} } }; diff --git a/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp b/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp index 1dd9f12..2d13ed7 100644 --- a/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp +++ b/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp @@ -1,9 +1,46 @@ #include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" +#include "infra/util/ReallyAssert.hpp" namespace simulator::dynamics { + namespace + { + using Vector3 = math::Vector; + using Link = ::dynamics::RevoluteJointLink; + + constexpr float rodAxialInertia{ 0.001f }; + + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeRod(float mass, float length, const Vector3& jointAxis, const Vector3& parentToJoint, const Vector3& direction) + { + const float transverse{ mass * length * length / 12.0f }; + const auto alongRod{ math::OuterProduct(direction, direction) }; + const auto inertia{ (math::SquareMatrix::Identity() - alongRod) * transverse + alongRod * rodAxialInertia }; + + return Link{ mass, inertia, jointAxis, parentToJoint, direction * (length / 2.0f) }; + } + + template + math::Vector ToVector(const std::vector& values) + { + math::Vector result{}; + + for (std::size_t i = 0; i < N; ++i) + result.at(i, 0) = values[i]; + + return result; + } + } + void RobotArmSimulator::Configure(const RobotArmConfig& cfg) { + really_assert(cfg.dof == 2 || cfg.dof == 3); + really_assert(cfg.linkLengths.size() >= static_cast(cfg.dof)); + really_assert(cfg.linkMasses.size() >= static_cast(cfg.dof)); + config = cfg; torques.assign(config.dof, 0.0f); Reset(); @@ -26,13 +63,10 @@ namespace simulator::dynamics void RobotArmSimulator::Step() { - if (config.dof == 2) - StepDof2(); + if (config.dof == 3) + StepChain(BuildLinks3(), aba3, rnea3); else - StepDof3(); - - for (int i = 0; i < config.dof; ++i) - state.qDot[i] *= (1.0f - config.damping * config.dt); + StepChain(BuildLinks2(), aba2, rnea2); state.time += config.dt; UpdateForwardKinematics(); @@ -60,150 +94,68 @@ namespace simulator::dynamics return config; } - void RobotArmSimulator::StepDof2() + template + void RobotArmSimulator::StepChain(const LinkArray& links, const ::dynamics::ArticulatedBodyAlgorithm& aba, + const ::dynamics::RecursiveNewtonEuler& rnea) { - auto links = BuildLinks2(); + const Vector3 g{ 0.0f, 0.0f, -gravity }; + const auto q{ ToVector(state.q) }; + const auto qDot{ ToVector(state.qDot) }; - math::Vector q{ { state.q[0] }, { state.q[1] } }; - math::Vector qDot{ { state.qDot[0] }, { state.qDot[1] } }; - math::Vector tau{ { torques[0] }, { torques[1] } }; - math::Vector g{ { 0.0f }, { 0.0f }, { -gravity } }; + const auto qDDot{ aba.ForwardDynamics(links, q, qDot, ToVector(torques), g) }; + const auto inverseDynamicsTorques{ rnea.InverseDynamics(links, q, qDot, qDDot, g) }; - // ABA: forward dynamics — compute accelerations from applied torques - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, g); - - // Semi-implicit Euler integration - for (int i = 0; i < 2; ++i) + for (std::size_t i = 0; i < N; ++i) { state.qDDot[i] = qDDot.at(i, 0); + state.inverseDynamicsTorques[i] = inverseDynamicsTorques.at(i, 0); state.qDot[i] += qDDot.at(i, 0) * config.dt; state.q[i] += state.qDot[i] * config.dt; + state.qDot[i] *= 1.0f - config.damping * config.dt; } - - // RNEA: inverse dynamics — compute torques required for current motion - math::Vector qNew{ { state.q[0] }, { state.q[1] } }; - math::Vector qDotNew{ { state.qDot[0] }, { state.qDot[1] } }; - auto idTorques = rnea2.InverseDynamics(links, qNew, qDotNew, qDDot, g); - - for (int i = 0; i < 2; ++i) - state.inverseDynamicsTorques[i] = idTorques.at(i, 0); } - void RobotArmSimulator::StepDof3() + RobotArmSimulator::LinkArray<2> RobotArmSimulator::BuildLinks2() const { - auto links = BuildLinks3(); - - math::Vector q{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - math::Vector qDot{ { state.qDot[0] }, { state.qDot[1] }, { state.qDot[2] } }; - math::Vector tau{ { torques[0] }, { torques[1] }, { torques[2] } }; - math::Vector g{ { 0.0f }, { 0.0f }, { -gravity } }; - - // ABA: forward dynamics — compute accelerations from applied torques - auto qDDot = aba3.ForwardDynamics(links, q, qDot, tau, g); + const auto& lengths = config.linkLengths; + const auto& masses = config.linkMasses; - for (int i = 0; i < 3; ++i) - { - state.qDDot[i] = qDDot.at(i, 0); - state.qDot[i] += qDDot.at(i, 0) * config.dt; - state.q[i] += state.qDot[i] * config.dt; - } - - // RNEA: inverse dynamics — compute torques required for current motion - math::Vector qNew{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - math::Vector qDotNew{ { state.qDot[0] }, { state.qDot[1] }, { state.qDot[2] } }; - auto idTorques = rnea3.InverseDynamics(links, qNew, qDotNew, qDDot, g); - - for (int i = 0; i < 3; ++i) - state.inverseDynamicsTorques[i] = idTorques.at(i, 0); + return LinkArray<2>{ + MakeRod(masses[0], lengths[0], yAxis, Vector3{}, xAxis), + MakeRod(masses[1], lengths[1], yAxis, xAxis * lengths[0], xAxis) + }; } - std::array<::dynamics::RevoluteJointLink, 2> RobotArmSimulator::BuildLinks2() const + RobotArmSimulator::LinkArray<3> RobotArmSimulator::BuildLinks3() const { - std::array<::dynamics::RevoluteJointLink, 2> links; - - float L0 = config.linkLengths[0]; - float m0 = config.linkMasses[0]; - float I0 = m0 * L0 * L0 / 12.0f; - - links[0].mass = m0; - links[0].inertia = math::SquareMatrix{ { { I0, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I0 } } }; - links[0].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[0].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { 0.0f } }; - links[0].jointToCoM = math::Vector{ { L0 / 2.0f }, { 0.0f }, { 0.0f } }; - - float L1 = config.linkLengths[1]; - float m1 = config.linkMasses[1]; - float I1 = m1 * L1 * L1 / 12.0f; - - links[1].mass = m1; - links[1].inertia = math::SquareMatrix{ { { I1, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I1 } } }; - links[1].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[1].parentToJoint = math::Vector{ { L0 }, { 0.0f }, { 0.0f } }; - links[1].jointToCoM = math::Vector{ { L1 / 2.0f }, { 0.0f }, { 0.0f } }; - - return links; + const auto& lengths = config.linkLengths; + const auto& masses = config.linkMasses; + + return LinkArray<3>{ + MakeRod(masses[0], lengths[0], zAxis, Vector3{}, zAxis), + MakeRod(masses[1], lengths[1], yAxis, zAxis * lengths[0], xAxis), + MakeRod(masses[2], lengths[2], yAxis, xAxis * lengths[1], xAxis) + }; } - std::array<::dynamics::RevoluteJointLink, 3> RobotArmSimulator::BuildLinks3() const + template + void RobotArmSimulator::UpdateJointPositions(const LinkArray& links) { - std::array<::dynamics::RevoluteJointLink, 3> links; - - float L0 = config.linkLengths[0]; - float m0 = config.linkMasses[0]; - float I0 = m0 * L0 * L0 / 12.0f; - - links[0].mass = m0; - links[0].inertia = math::SquareMatrix{ { { I0, 0, 0 }, { 0, I0, 0 }, { 0, 0, 0.001f } } }; - links[0].jointAxis = math::Vector{ { 0.0f }, { 0.0f }, { 1.0f } }; - links[0].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { 0.0f } }; - links[0].jointToCoM = math::Vector{ { 0.0f }, { 0.0f }, { L0 / 2.0f } }; - - float L1 = config.linkLengths[1]; - float m1 = config.linkMasses[1]; - float I1 = m1 * L1 * L1 / 12.0f; - - links[1].mass = m1; - links[1].inertia = math::SquareMatrix{ { { I1, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I1 } } }; - links[1].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[1].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { L0 } }; - links[1].jointToCoM = math::Vector{ { L1 / 2.0f }, { 0.0f }, { 0.0f } }; - - float L2 = config.linkLengths[2]; - float m2 = config.linkMasses[2]; - float I2 = m2 * L2 * L2 / 12.0f; - - links[2].mass = m2; - links[2].inertia = math::SquareMatrix{ { { I2, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I2 } } }; - links[2].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[2].parentToJoint = math::Vector{ { L1 }, { 0.0f }, { 0.0f } }; - links[2].jointToCoM = math::Vector{ { L2 / 2.0f }, { 0.0f }, { 0.0f } }; - - return links; + const ::kinematics::ForwardKinematics fk{ xAxis * config.linkLengths[N - 1] }; + const auto positions{ fk.Compute(links, ToVector(state.q)) }; + + state.jointPositions.resize(N + 1); + + for (std::size_t i = 0; i <= N; ++i) + state.jointPositions[i] = { positions[i].at(0, 0), positions[i].at(1, 0), positions[i].at(2, 0) }; } void RobotArmSimulator::UpdateForwardKinematics() { - int n = config.dof; - state.jointPositions.resize(n + 1); - - if (n == 2) - { - auto links = BuildLinks2(); - math::Vector q{ { state.q[0] }, { state.q[1] } }; - auto positions = fk2.Compute(links, q); - - for (int i = 0; i <= n; ++i) - state.jointPositions[i] = { positions[i].at(0, 0), positions[i].at(1, 0), positions[i].at(2, 0) }; - } + if (config.dof == 3) + UpdateJointPositions(BuildLinks3()); else - { - auto links = BuildLinks3(); - math::Vector q{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - auto positions = fk3.Compute(links, q); - - for (int i = 0; i <= n; ++i) - state.jointPositions[i] = { positions[i].at(0, 0), positions[i].at(1, 0), positions[i].at(2, 0) }; - } + UpdateJointPositions(BuildLinks2()); } void RobotArmSimulator::UpdateTrail() diff --git a/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp b/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp index 36311fb..f09ffb1 100644 --- a/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp +++ b/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp @@ -24,13 +24,21 @@ namespace simulator::dynamics const RobotArmConfig& GetConfig() const; private: - void StepDof2(); - void StepDof3(); + template + using LinkArray = std::array<::dynamics::RevoluteJointLink, N>; + + template + void StepChain(const LinkArray& links, const ::dynamics::ArticulatedBodyAlgorithm& aba, + const ::dynamics::RecursiveNewtonEuler& rnea); + + template + void UpdateJointPositions(const LinkArray& links); + void UpdateForwardKinematics(); void UpdateTrail(); - std::array<::dynamics::RevoluteJointLink, 2> BuildLinks2() const; - std::array<::dynamics::RevoluteJointLink, 3> BuildLinks3() const; + LinkArray<2> BuildLinks2() const; + LinkArray<3> BuildLinks3() const; static constexpr float gravity = 9.81f; static constexpr std::size_t maxTrailSize = 500; @@ -39,8 +47,6 @@ namespace simulator::dynamics ::dynamics::ArticulatedBodyAlgorithm aba3; ::dynamics::RecursiveNewtonEuler rnea2; ::dynamics::RecursiveNewtonEuler rnea3; - ::kinematics::ForwardKinematics fk2; - ::kinematics::ForwardKinematics fk3; RobotArmConfig config; RobotArmState state; diff --git a/simulator/dynamics/RobotArm/application/RobotArmTypes.hpp b/simulator/dynamics/RobotArm/application/RobotArmTypes.hpp index 891b4dc..5e4944f 100644 --- a/simulator/dynamics/RobotArm/application/RobotArmTypes.hpp +++ b/simulator/dynamics/RobotArm/application/RobotArmTypes.hpp @@ -12,7 +12,7 @@ namespace simulator::dynamics std::vector linkLengths = { 0.5f, 0.4f, 0.3f }; std::vector linkMasses = { 1.0f, 0.8f, 0.6f }; float damping = 0.05f; - float dt = 1.0f / 120.0f; + float dt = 0.008f; }; struct RobotArmState diff --git a/simulator/dynamics/RobotArm/application/test/CMakeLists.txt b/simulator/dynamics/RobotArm/application/test/CMakeLists.txt index e4177b3..826359d 100644 --- a/simulator/dynamics/RobotArm/application/test/CMakeLists.txt +++ b/simulator/dynamics/RobotArm/application/test/CMakeLists.txt @@ -1,5 +1,5 @@ add_executable(robotics.simulator.dynamics.robotarm.application_test) -emil_build_for(robotics.simulator.dynamics.robotarm.application_test HOST All PREREQUISITE_BOOL ROBOTICS_TOOLBOX_BUILD_SIMULATOR) +emil_build_for(robotics.simulator.dynamics.robotarm.application_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) emil_add_test(robotics.simulator.dynamics.robotarm.application_test) target_link_libraries(robotics.simulator.dynamics.robotarm.application_test PUBLIC @@ -9,4 +9,5 @@ target_link_libraries(robotics.simulator.dynamics.robotarm.application_test PUBL target_sources(robotics.simulator.dynamics.robotarm.application_test PRIVATE TestRobotArmForm.cpp + TestRobotArmSimulator.cpp ) diff --git a/simulator/dynamics/RobotArm/application/test/TestRobotArmSimulator.cpp b/simulator/dynamics/RobotArm/application/test/TestRobotArmSimulator.cpp new file mode 100644 index 0000000..c988554 --- /dev/null +++ b/simulator/dynamics/RobotArm/application/test/TestRobotArmSimulator.cpp @@ -0,0 +1,111 @@ +#include "numerical/math/Tolerance.hpp" +#include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" +#include +#include +#include + +namespace +{ + using simulator::dynamics::RobotArmConfig; + using simulator::dynamics::RobotArmSimulator; + + constexpr float gravity{ 9.81f }; + + RobotArmConfig PlanarArm() + { + RobotArmConfig config; + config.dof = 2; + config.linkLengths = { 0.5f, 0.4f }; + config.linkMasses = { 1.0f, 0.8f }; + config.damping = 0.0f; + return config; + } + + RobotArmConfig SpatialArm() + { + RobotArmConfig config; + config.dof = 3; + config.linkLengths = { 0.5f, 0.4f, 0.3f }; + config.linkMasses = { 1.0f, 0.8f, 0.6f }; + config.damping = 0.0f; + return config; + } + + class TestRobotArmSimulator + : public ::testing::Test + { + protected: + RobotArmSimulator simulator; + }; +} + +TEST_F(TestRobotArmSimulator, planar_inverse_dynamics_readout_reproduces_applied_torque) +{ + simulator.Configure(PlanarArm()); + simulator.SetInitialPositions({ 0.3f, -0.4f }); + simulator.SetTorques({ 1.5f, -0.5f }); + + simulator.Step(); + simulator.Step(); + + EXPECT_NEAR(simulator.GetState().inverseDynamicsTorques[0], 1.5f, math::Tolerance()); + EXPECT_NEAR(simulator.GetState().inverseDynamicsTorques[1], -0.5f, math::Tolerance()); +} + +TEST_F(TestRobotArmSimulator, spatial_inverse_dynamics_readout_reproduces_applied_torque) +{ + simulator.Configure(SpatialArm()); + simulator.SetInitialPositions({ 0.2f, 0.3f, -0.4f }); + simulator.SetTorques({ 0.7f, 1.5f, -0.5f }); + + simulator.Step(); + simulator.Step(); + + EXPECT_NEAR(simulator.GetState().inverseDynamicsTorques[0], 0.7f, math::Tolerance()); + EXPECT_NEAR(simulator.GetState().inverseDynamicsTorques[1], 1.5f, math::Tolerance()); + EXPECT_NEAR(simulator.GetState().inverseDynamicsTorques[2], -0.5f, math::Tolerance()); +} + +TEST_F(TestRobotArmSimulator, hanging_arm_without_torque_stays_at_rest) +{ + const float hanging{ std::numbers::pi_v / 2.0f }; + simulator.Configure(PlanarArm()); + simulator.SetInitialPositions({ hanging, 0.0f }); + + for (int step = 0; step < 120; ++step) + simulator.Step(); + + EXPECT_NEAR(simulator.GetState().q[0], hanging, math::Tolerance()); + EXPECT_NEAR(simulator.GetState().q[1], 0.0f, math::Tolerance()); +} + +TEST_F(TestRobotArmSimulator, horizontal_release_matches_analytic_uniform_rod_acceleration) +{ + const float m1{ 1.0f }; + const float m2{ 0.8f }; + const float l1{ 0.5f }; + const float l2{ 0.4f }; + const float m11{ m1 * l1 * l1 / 3.0f + m2 * (l1 * l1 + l2 * l2 / 3.0f + l1 * l2) }; + const float m12{ m2 * (l2 * l2 / 3.0f + l1 * l2 / 2.0f) }; + const float m22{ m2 * l2 * l2 / 3.0f }; + const float g1{ gravity * (m1 * l1 / 2.0f + m2 * l1 + m2 * l2 / 2.0f) }; + const float g2{ gravity * m2 * l2 / 2.0f }; + const float determinant{ m11 * m22 - m12 * m12 }; + simulator.Configure(PlanarArm()); + + simulator.Step(); + + EXPECT_NEAR(simulator.GetState().qDDot[0], (m22 * g1 - m12 * g2) / determinant, 1e-2f); + EXPECT_NEAR(simulator.GetState().qDDot[1], (m11 * g2 - m12 * g1) / determinant, 1e-2f); +} + +TEST_F(TestRobotArmSimulator, tool_point_is_at_the_end_of_the_last_link) +{ + simulator.Configure(SpatialArm()); + + const auto& tool = simulator.GetState().jointPositions.back(); + + EXPECT_NEAR(tool[0], 0.7f, math::Tolerance()); + EXPECT_NEAR(tool[1], 0.0f, math::Tolerance()); + EXPECT_NEAR(tool[2], 0.5f, math::Tolerance()); +} diff --git a/simulator/dynamics/RobotArm/view/CMakeLists.txt b/simulator/dynamics/RobotArm/view/CMakeLists.txt index 8ff6052..5089e3b 100644 --- a/simulator/dynamics/RobotArm/view/CMakeLists.txt +++ b/simulator/dynamics/RobotArm/view/CMakeLists.txt @@ -27,10 +27,6 @@ target_sources(robotics.simulator.dynamics.robotarm.view PRIVATE if(MSVC) target_compile_options(robotics.simulator.dynamics.robotarm.view PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(robotics.simulator.dynamics.robotarm.view PRIVATE - -Wno-error - ) endif() if (ROBOTICS_TOOLBOX_BUILD_TESTS) diff --git a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp index 4f472b8..e1a6fa4 100644 --- a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp +++ b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp @@ -29,6 +29,7 @@ namespace simulator::dynamics::view view3D->SetPanCursorEnabled(true); shell.SetContent(view3D); + simulationTimer->setTimerType(Qt::PreciseTimer); connect(simulationTimer, &QTimer::timeout, this, &RobotArmMainWindow::OnSimulationStep); form.Model().onActionTriggered = [this](ui::model::ActionId action) @@ -42,6 +43,7 @@ namespace simulator::dynamics::view RobotArmMainWindow::~RobotArmMainWindow() { delete view3D; + delete formView; } void RobotArmMainWindow::OnActionTriggered(ui::model::ActionId action) @@ -67,9 +69,11 @@ namespace simulator::dynamics::view { if (!running) { - ApplyConfiguration(); - simulator.SetInitialPositions(form.InitialPositions()); + if (!paused) + ApplyConfiguration(); + running = true; + paused = false; simulationTimer->start(); shell.SetStatus("Simulation running..."); } @@ -77,14 +81,19 @@ namespace simulator::dynamics::view void RobotArmMainWindow::OnStopRequested() { + if (!running) + return; + running = false; + paused = true; simulationTimer->stop(); - shell.SetStatus("Simulation stopped"); + shell.SetStatus("Simulation paused (Start resumes, Reset applies new parameters)"); } void RobotArmMainWindow::OnResetRequested() { running = false; + paused = false; simulationTimer->stop(); ApplyConfiguration(); shell.SetStatus("Simulation reset"); diff --git a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp index 02e2d63..8ba0811 100644 --- a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp +++ b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp @@ -36,5 +36,6 @@ namespace simulator::dynamics::view ui::backend::qt::QtPaintedWidget* view3D; QTimer* simulationTimer; bool running = false; + bool paused = false; }; } diff --git a/simulator/dynamics/RobotArm/view/RobotArmSceneView.cpp b/simulator/dynamics/RobotArm/view/RobotArmSceneView.cpp index 32424da..c54e28b 100644 --- a/simulator/dynamics/RobotArm/view/RobotArmSceneView.cpp +++ b/simulator/dynamics/RobotArm/view/RobotArmSceneView.cpp @@ -3,6 +3,7 @@ #include "ui/theme/Theme.hpp" #include #include +#include namespace simulator::dynamics::view { diff --git a/simulator/dynamics/RobotArm/view/test/CMakeLists.txt b/simulator/dynamics/RobotArm/view/test/CMakeLists.txt index cfd3d52..0b73892 100644 --- a/simulator/dynamics/RobotArm/view/test/CMakeLists.txt +++ b/simulator/dynamics/RobotArm/view/test/CMakeLists.txt @@ -16,6 +16,4 @@ target_sources(robotics.simulator.dynamics.robotarm.view_test PRIVATE if(MSVC) target_compile_options(robotics.simulator.dynamics.robotarm.view_test PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(robotics.simulator.dynamics.robotarm.view_test PRIVATE -Wno-error) endif() diff --git a/sonar-project.properties b/sonar-project.properties index 1bc9274..d41125a 100644 --- a/sonar-project.properties +++ b/sonar-project.properties @@ -3,7 +3,7 @@ sonar.organization=embedded-pro sonar.projectName=robotics-toolbox-cpp # x-release-please-start-version -sonar.projectVersion=0.0.1 +sonar.projectVersion=1.0.0 # x-release-please-end sonar.links.homepage=https://github.com/embedded-pro/robotics-toolbox-cpp