├── .clang-format ├── .github ├── dependabot.yml ├── mergify.yml └── workflows │ ├── humble-abi-compatibility.yml │ ├── humble-build-binary.yml │ ├── humble-build-coverage.yml │ ├── humble-build-downstream.yml │ ├── humble-build-semi-binary.yml │ ├── humble-build-source.yml │ ├── humble-ci-pre-commit.yml │ ├── humble-debian-build.yml │ ├── humble-rhel-semi-binary-build.yml │ ├── jazzy-abi-compatibility.yml │ ├── jazzy-build-binary.yml │ ├── jazzy-build-coverage.yml │ ├── jazzy-build-downstream.yml │ ├── jazzy-build-semi-binary.yml │ ├── jazzy-build-source.yml │ ├── jazzy-ci-pre-commit.yml │ ├── jazzy-debian-build.yml │ ├── jazzy-rhel-semi-binary-build.yml │ ├── reviewer_lottery.yml │ ├── rolling-abi-compatibility.yml │ ├── rolling-build-binary.yml │ ├── rolling-build-coverage.yml │ ├── rolling-build-downstream.yml │ ├── rolling-build-semi-binary.yml │ ├── rolling-build-source.yml │ ├── rolling-ci-pre-commit.yml │ ├── rolling-compatibility-build.yml │ ├── rolling-debian-build.yml │ ├── rolling-rhel-semi-binary-build.yml │ ├── rosdoc2.yml │ ├── stale.yml │ └── update-pre-commit.yml ├── .pre-commit-config.yaml ├── LICENSE ├── LICENSE.BSD-3-clause ├── README.md ├── codecov.yml ├── control_toolbox-not-released.humble.repos ├── control_toolbox-not-released.jazzy.repos ├── control_toolbox-not-released.kilted.repos ├── control_toolbox-not-released.rolling.repos ├── control_toolbox.humble.repos ├── control_toolbox.jazzy.repos ├── control_toolbox.kilted.repos ├── control_toolbox.rolling.repos ├── control_toolbox ├── CHANGELOG.rst ├── CMakeLists.txt ├── control_filters.xml ├── doc │ ├── conf.py │ └── index.rst ├── include │ ├── control_filters │ │ ├── custom_validators.hpp │ │ ├── exponential_filter.hpp │ │ ├── gravity_compensation.hpp │ │ ├── low_pass_filter.hpp │ │ └── rate_limiter.hpp │ └── control_toolbox │ │ ├── dither.hpp │ │ ├── filter_traits.hpp │ │ ├── filters.hpp │ │ ├── limited_proxy.hpp │ │ ├── low_pass_filter.hpp │ │ ├── pid.hpp │ │ ├── pid_ros.hpp │ │ ├── rate_limiter.hpp │ │ ├── sine_sweep.hpp │ │ └── sinusoid.hpp ├── package.xml ├── src │ ├── control_filters │ │ ├── exponential_filter.cpp │ │ ├── exponential_filter_parameters.yaml │ │ ├── gravity_compensation.cpp │ │ ├── gravity_compensation_filter_parameters.yaml │ │ ├── low_pass_filter.cpp │ │ ├── low_pass_filter_parameters.yaml │ │ ├── rate_limiter.cpp │ │ └── rate_limiter_parameters.yaml │ ├── dither.cpp │ ├── limited_proxy.cpp │ ├── pid.cpp │ ├── pid_ros.cpp │ ├── sine_sweep.cpp │ └── sinusoid.cpp └── test │ ├── control_filters │ ├── test_exponential_filter.cpp │ ├── test_exponential_filter_parameters.yaml │ ├── test_filter_util.hpp │ ├── test_gravity_compensation.cpp │ ├── test_gravity_compensation.hpp │ ├── test_gravity_compensation_parameters.yaml │ ├── test_load_exponential_filter.cpp │ ├── test_load_gravity_compensation.cpp │ ├── test_load_low_pass_filter.cpp │ ├── test_load_rate_limiter.cpp │ ├── test_low_pass_filter.cpp │ ├── test_low_pass_filter_parameters.yaml │ ├── test_rate_limiter.cpp │ └── test_rate_limiter_parameters.yaml │ ├── pid_ros_parameters_tests.cpp │ ├── pid_ros_publisher_tests.cpp │ ├── pid_tests.cpp │ └── rate_limiter.cpp ├── doc ├── control_filters.md ├── control_toolbox.md ├── index.rst ├── migration.rst └── release_notes.rst ├── ros_controls.humble.repos ├── ros_controls.jazzy.repos ├── ros_controls.kilted.repos └── ros_controls.rolling.repos /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | ColumnLimit: 100 6 | AccessModifierOffset: -2 7 | AlignAfterOpenBracket: AlwaysBreak 8 | BreakBeforeBraces: Allman 9 | ConstructorInitializerIndentWidth: 0 10 | ContinuationIndentWidth: 2 11 | DerivePointerAlignment: false 12 | PointerAlignment: Middle 13 | ReflowComments: false 14 | IncludeBlocks: Preserve 15 | InsertBraces: true 16 | ... 17 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # To get started with Dependabot version updates, you'll need to specify which 2 | # package ecosystems to update and where the package manifests are located. 3 | # Please see the documentation for all configuration options: 4 | # https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates 5 | 6 | version: 2 7 | updates: 8 | - package-ecosystem: "github-actions" 9 | # Workflow files stored in the 10 | # default location of `.github/workflows` 11 | directory: "/" 12 | schedule: 13 | interval: "weekly" 14 | - package-ecosystem: "github-actions" 15 | # Workflow files stored in the 16 | # default location of `.github/workflows` 17 | directory: "/" 18 | schedule: 19 | interval: "weekly" 20 | target-branch: "humble" 21 | - package-ecosystem: "github-actions" 22 | # Workflow files stored in the 23 | # default location of `.github/workflows` 24 | directory: "/" 25 | schedule: 26 | interval: "weekly" 27 | target-branch: "jazzy" 28 | -------------------------------------------------------------------------------- /.github/mergify.yml: -------------------------------------------------------------------------------- 1 | pull_request_rules: 2 | 3 | - name: Backport to humble at reviewers discretion 4 | conditions: 5 | - base=ros2-master 6 | - "label=backport-humble" 7 | actions: 8 | backport: 9 | branches: 10 | - humble 11 | 12 | - name: Backport to jazzy at reviewers discretion 13 | conditions: 14 | - base=ros2-master 15 | - "label=backport-jazzy" 16 | actions: 17 | backport: 18 | branches: 19 | - jazzy 20 | 21 | - name: Ask to resolve conflict 22 | conditions: 23 | - conflict 24 | - author!=mergify[bot] 25 | - author!=dependabot[bot] 26 | actions: 27 | comment: 28 | message: This pull request is in conflict. Could you fix it @{{author}}? 29 | 30 | - name: Ask to resolve conflict for backports 31 | conditions: 32 | - conflict 33 | - author=mergify[bot] 34 | actions: 35 | comment: 36 | message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? 37 | 38 | - name: development targets ros2-master branch 39 | conditions: 40 | - base!=ros2-master 41 | - author!=bmagyar 42 | - author!=destogl 43 | - author!=christophfroehlich 44 | - author!=saikishor 45 | - author!=mergify[bot] 46 | - author!=dependabot[bot] 47 | actions: 48 | comment: 49 | message: | 50 | @{{author}}, all pull requests must be targeted towards the `ros2-master` development branch. 51 | Once merged into `ros2-master`, it is possible to backport to `{{base}}`, but it must be in `ros2-master` 52 | to have these changes reflected into new distributions. 53 | -------------------------------------------------------------------------------- /.github/workflows/humble-abi-compatibility.yml: -------------------------------------------------------------------------------- 1 | name: Humble - ABI Compatibility Check 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - humble 7 | 8 | jobs: 9 | abi_check: 10 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master 11 | with: 12 | ros_distro: humble 13 | -------------------------------------------------------------------------------- /.github/workflows/humble-build-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Humble - Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - humble 9 | push: 10 | branches: 11 | - humble 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | with: 21 | ros_distro: humble 22 | ros_repo: testing 23 | upstream_workspace: control_toolbox-not-released.humble.repos 24 | ref_for_scheduled_build: humble 25 | binary_clang: 26 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 27 | with: 28 | ros_distro: humble 29 | ros_repo: testing 30 | upstream_workspace: control_toolbox-not-released.humble.repos 31 | ref_for_scheduled_build: humble 32 | additional_debs: clang 33 | c_compiler: clang 34 | cxx_compiler: clang++ 35 | not_test_build: true 36 | -------------------------------------------------------------------------------- /.github/workflows/humble-build-coverage.yml: -------------------------------------------------------------------------------- 1 | name: Humble - Coverage Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - humble 7 | pull_request: 8 | branches: 9 | - humble 10 | 11 | jobs: 12 | coverage_humble: 13 | name: coverage build - humble 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master 15 | secrets: inherit 16 | with: 17 | ros_distro: humble 18 | -------------------------------------------------------------------------------- /.github/workflows/humble-build-downstream.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Humble - Downstream Build 5 | on: 6 | pull_request: 7 | branches: 8 | - humble 9 | push: 10 | branches: 11 | - humble 12 | 13 | 14 | jobs: 15 | build-downstream: 16 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 17 | with: 18 | ros_distro: humble 19 | ros_repo: testing 20 | ref_for_scheduled_build: humble 21 | not_test_build: true 22 | upstream_workspace: control_toolbox.humble.repos 23 | # we test the downstream packages, which are part of our organization 24 | downstream_workspace: ros_controls.humble.repos 25 | not_test_downstream: false 26 | -------------------------------------------------------------------------------- /.github/workflows/humble-build-semi-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Humble - Semi-Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - humble 9 | push: 10 | branches: 11 | - humble 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | semi-binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | with: 21 | ros_distro: humble 22 | ros_repo: testing 23 | upstream_workspace: control_toolbox.humble.repos 24 | ref_for_scheduled_build: humble 25 | -------------------------------------------------------------------------------- /.github/workflows/humble-build-source.yml: -------------------------------------------------------------------------------- 1 | name: Humble - Source Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - humble 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | jobs: 12 | source_build: 13 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master 14 | with: 15 | ros_distro: humble 16 | ref: humble 17 | os_name: ubuntu-22.04 18 | container: "" 19 | -------------------------------------------------------------------------------- /.github/workflows/humble-ci-pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Humble - Pre-Commit 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - humble 8 | 9 | jobs: 10 | pre-commit: 11 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master 12 | with: 13 | ros_distro: humble 14 | -------------------------------------------------------------------------------- /.github/workflows/humble-debian-build.yml: -------------------------------------------------------------------------------- 1 | name: Humble - Debian Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - humble 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 5 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | debian_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master 15 | with: 16 | ros_distro: humble 17 | upstream_workspace: control_toolbox.humble.repos 18 | ref_for_scheduled_build: humble 19 | -------------------------------------------------------------------------------- /.github/workflows/humble-rhel-semi-binary-build.yml: -------------------------------------------------------------------------------- 1 | name: Humble - RHEL Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - humble 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | rhel_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master 15 | with: 16 | ros_distro: humble 17 | upstream_workspace: control_toolbox.humble.repos 18 | ref_for_scheduled_build: humble 19 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-abi-compatibility.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - ABI Compatibility Check 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - jazzy 7 | 8 | jobs: 9 | abi_check: 10 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master 11 | with: 12 | ros_distro: jazzy 13 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-build-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Jazzy - Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - jazzy 9 | push: 10 | branches: 11 | - jazzy 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | ROS_REPO: [main, testing] 24 | with: 25 | ros_distro: jazzy 26 | ros_repo: ${{ matrix.ROS_REPO }} 27 | upstream_workspace: control_toolbox-not-released.jazzy.repos 28 | ref_for_scheduled_build: jazzy 29 | binary_clang: 30 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 31 | with: 32 | ros_distro: jazzy 33 | ros_repo: testing 34 | upstream_workspace: control_toolbox-not-released.jazzy.repos 35 | ref_for_scheduled_build: jazzy 36 | additional_debs: clang 37 | c_compiler: clang 38 | cxx_compiler: clang++ 39 | not_test_build: true 40 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-build-coverage.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - Coverage Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - jazzy 7 | pull_request: 8 | branches: 9 | - jazzy 10 | 11 | jobs: 12 | coverage: 13 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master 14 | secrets: inherit 15 | with: 16 | ros_distro: jazzy 17 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-build-downstream.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Jazzy - Downstream Build 5 | on: 6 | pull_request: 7 | branches: 8 | - jazzy 9 | push: 10 | branches: 11 | - jazzy 12 | 13 | 14 | jobs: 15 | build-downstream: 16 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 17 | with: 18 | ros_distro: jazzy 19 | ros_repo: testing 20 | ref_for_scheduled_build: jazzy 21 | not_test_build: true 22 | upstream_workspace: control_toolbox.jazzy.repos 23 | # we test the downstream packages, which are part of our organization 24 | downstream_workspace: ros_controls.jazzy.repos 25 | not_test_downstream: false 26 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-build-semi-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Jazzy - Semi-Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - jazzy 9 | push: 10 | branches: 11 | - jazzy 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | semi-binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | with: 21 | ros_distro: jazzy 22 | ros_repo: testing 23 | upstream_workspace: control_toolbox.jazzy.repos 24 | ref_for_scheduled_build: jazzy 25 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-build-source.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - Source Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - jazzy 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | jobs: 12 | source_build: 13 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master 14 | with: 15 | ros_distro: jazzy 16 | ref: jazzy 17 | os_name: ubuntu-latest 18 | container: ubuntu:24.04 19 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-ci-pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - Pre-Commit 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - jazzy 8 | 9 | jobs: 10 | pre-commit: 11 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master 12 | with: 13 | ros_distro: jazzy 14 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-debian-build.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - Debian Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - jazzy 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 5 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | debian_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master 15 | with: 16 | ros_distro: jazzy 17 | upstream_workspace: control_toolbox.jazzy.repos 18 | ref_for_scheduled_build: jazzy 19 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-rhel-semi-binary-build.yml: -------------------------------------------------------------------------------- 1 | name: Jazzy - RHEL Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - jazzy 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | rhel_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master 15 | with: 16 | ros_distro: jazzy 17 | upstream_workspace: control_toolbox.jazzy.repos 18 | ref_for_scheduled_build: jazzy 19 | -------------------------------------------------------------------------------- /.github/workflows/reviewer_lottery.yml: -------------------------------------------------------------------------------- 1 | name: Reviewer lottery 2 | # pull_request_target takes the same events as pull_request, 3 | # but it runs on the base branch instead of the head branch. 4 | on: 5 | pull_request_target: 6 | types: [opened, ready_for_review, reopened] 7 | 8 | jobs: 9 | assign_reviewers: 10 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-reviewer-lottery.yml@master 11 | -------------------------------------------------------------------------------- /.github/workflows/rolling-abi-compatibility.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - ABI Compatibility Check 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - ros2-master 7 | 8 | jobs: 9 | abi_check: 10 | strategy: 11 | fail-fast: false 12 | matrix: 13 | ROS_DISTRO: [kilted, rolling] 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master 15 | with: 16 | ros_distro: ${{ matrix.ROS_DISTRO }} 17 | -------------------------------------------------------------------------------- /.github/workflows/rolling-build-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Rolling - Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - ros2-master 9 | push: 10 | branches: 11 | - ros2-master 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | ROS_DISTRO: [kilted, rolling] 24 | ROS_REPO: [main, testing] 25 | with: 26 | ros_distro: ${{ matrix.ROS_DISTRO }} 27 | ros_repo: ${{ matrix.ROS_REPO }} 28 | upstream_workspace: control_toolbox-not-released.${{ matrix.ROS_DISTRO }}.repos 29 | ref_for_scheduled_build: ros2-master 30 | binary_clang: 31 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 32 | with: 33 | ros_distro: rolling 34 | ros_repo: testing 35 | upstream_workspace: control_toolbox-not-released.rolling.repos 36 | ref_for_scheduled_build: ros2-master 37 | additional_debs: clang 38 | c_compiler: clang 39 | cxx_compiler: clang++ 40 | not_test_build: true 41 | -------------------------------------------------------------------------------- /.github/workflows/rolling-build-coverage.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - Coverage Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - ros2-master 7 | pull_request: 8 | branches: 9 | - ros2-master 10 | 11 | jobs: 12 | coverage_rolling: 13 | name: coverage build - rolling 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master 15 | secrets: inherit 16 | with: 17 | ros_distro: rolling 18 | -------------------------------------------------------------------------------- /.github/workflows/rolling-build-downstream.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Rolling - Downstream Build 5 | on: 6 | pull_request: 7 | branches: 8 | - ros2-master 9 | push: 10 | branches: 11 | - ros2-master 12 | 13 | 14 | jobs: 15 | build-downstream: 16 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 17 | strategy: 18 | fail-fast: false 19 | matrix: 20 | ROS_DISTRO: [kilted, rolling] 21 | with: 22 | ros_distro: ${{ matrix.ROS_DISTRO }} 23 | ros_repo: testing 24 | ref_for_scheduled_build: ros2-master 25 | not_test_build: true 26 | upstream_workspace: control_toolbox.${{ matrix.ROS_DISTRO }}.repos 27 | # we test the downstream packages, which are part of our organization 28 | downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos 29 | not_test_downstream: false 30 | -------------------------------------------------------------------------------- /.github/workflows/rolling-build-semi-binary.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: Rolling - Semi-Binary Build 5 | on: 6 | pull_request: 7 | branches: 8 | - ros2-master 9 | push: 10 | branches: 11 | - ros2-master 12 | schedule: 13 | # Run every day to detect flakiness and broken dependencies 14 | - cron: '28 6 * * MON-FRI' 15 | 16 | 17 | jobs: 18 | semi-binary: 19 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | ROS_DISTRO: [kilted, rolling] 24 | with: 25 | ros_distro: ${{ matrix.ROS_DISTRO }} 26 | ros_repo: testing 27 | upstream_workspace: control_toolbox.${{ matrix.ROS_DISTRO }}.repos 28 | ref_for_scheduled_build: ros2-master 29 | -------------------------------------------------------------------------------- /.github/workflows/rolling-build-source.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - Source Build 2 | on: 3 | workflow_dispatch: 4 | push: 5 | branches: 6 | - ros2-master 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | jobs: 12 | source_build: 13 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | include: 18 | - ROS_DISTRO: rolling 19 | CONTAINER: ubuntu:24.04 20 | OS_NAME: ubuntu-latest 21 | - ROS_DISTRO: kilted 22 | CONTAINER: ubuntu:24.04 23 | OS_NAME: ubuntu-latest 24 | with: 25 | ros_distro: ${{ matrix.ROS_DISTRO }} 26 | ref: ros2-master 27 | os_name: ${{ matrix.OS_NAME }} 28 | container: ${{ matrix.CONTAINER }} 29 | -------------------------------------------------------------------------------- /.github/workflows/rolling-ci-pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - Pre-Commit 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | branches: 7 | - ros2-master 8 | 9 | jobs: 10 | pre-commit: 11 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | ROS_DISTRO: [kilted, rolling] 16 | with: 17 | ros_distro: ${{ matrix.ROS_DISTRO }} 18 | -------------------------------------------------------------------------------- /.github/workflows/rolling-compatibility-build.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - Check Compatibility 2 | # author: Christoph Froehlich 3 | # description: 'Build & test the rolling version on earlier distros.' 4 | 5 | on: 6 | workflow_dispatch: 7 | pull_request: 8 | branches: 9 | - ros2-master 10 | push: 11 | branches: 12 | - ros2-master 13 | 14 | concurrency: 15 | # cancel previous runs of the same workflow, except for pushes on ros2-master branch 16 | group: ${{ github.workflow }}-${{ github.ref }} 17 | cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} 18 | 19 | jobs: 20 | build: 21 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master 22 | strategy: 23 | fail-fast: false 24 | matrix: 25 | ROS_DISTRO: [humble, jazzy, kilted] 26 | with: 27 | ros_distro: ${{ matrix.ROS_DISTRO }} 28 | ros_repo: testing 29 | upstream_workspace: control_toolbox.rolling.repos 30 | ref_for_scheduled_build: master 31 | -------------------------------------------------------------------------------- /.github/workflows/rolling-debian-build.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - Debian Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - ros2-master 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 5 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | debian_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | ROS_DISTRO: [kilted, rolling] 19 | with: 20 | ros_distro: ${{ matrix.ROS_DISTRO }} 21 | upstream_workspace: control_toolbox.${{ matrix.ROS_DISTRO }}.repos 22 | ref_for_scheduled_build: ros2-master 23 | -------------------------------------------------------------------------------- /.github/workflows/rolling-rhel-semi-binary-build.yml: -------------------------------------------------------------------------------- 1 | name: Rolling - RHEL Semi-Binary Build 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | branches: 6 | - ros2-master 7 | schedule: 8 | # Run every day to detect flakiness and broken dependencies 9 | - cron: '03 3 * * MON-FRI' 10 | 11 | 12 | jobs: 13 | rhel_semi_binary_build: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | ROS_DISTRO: [kilted, rolling] 19 | with: 20 | ros_distro: ${{ matrix.ROS_DISTRO }} 21 | upstream_workspace: control_toolbox.${{ matrix.ROS_DISTRO }}.repos 22 | ref_for_scheduled_build: ros2-master 23 | -------------------------------------------------------------------------------- /.github/workflows/rosdoc2.yml: -------------------------------------------------------------------------------- 1 | name: rosdoc2 2 | 3 | on: 4 | workflow_dispatch: 5 | pull_request: 6 | paths: 7 | - doc/** 8 | - rosdoc2.yaml 9 | - package.xml 10 | 11 | 12 | jobs: 13 | check: 14 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master 15 | -------------------------------------------------------------------------------- /.github/workflows/stale.yml: -------------------------------------------------------------------------------- 1 | name: 'Stale issues and PRs' 2 | on: 3 | workflow_dispatch: 4 | schedule: 5 | # UTC noon every workday 6 | - cron: '0 12 * * MON-FRI' 7 | 8 | jobs: 9 | stale: 10 | runs-on: ubuntu-latest 11 | permissions: 12 | issues: write 13 | pull-requests: write 14 | steps: 15 | - uses: actions/stale@v9 16 | with: 17 | stale-issue-label: 'stale' 18 | stale-pr-label: 'stale' 19 | stale-issue-message: 'This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.' 20 | close-issue-message: 'This issue was closed because it has been stalled for 45 days with no activity.' 21 | stale-pr-message: 'This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.' 22 | days-before-stale: 45 23 | days-before-close: 45 24 | days-before-pr-close: -1 25 | exempt-all-milestones: true 26 | exempt-issue-labels: good first issue,good second issue,persistent,release,roadmap,Epic 27 | operations-per-run: 100 28 | -------------------------------------------------------------------------------- /.github/workflows/update-pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Auto Update pre-commit 2 | # Update pre-commit config and create PR if changes are detected 3 | # author: Christoph Fröhlich 4 | 5 | on: 6 | workflow_dispatch: 7 | schedule: 8 | - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month 9 | 10 | jobs: 11 | auto_update_and_create_pr: 12 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master 13 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | 2 | # To use: 3 | # 4 | # pre-commit run -a 5 | # 6 | # Or: 7 | # 8 | # pre-commit install # (runs every time you commit in git) 9 | # 10 | # To update this file: 11 | # 12 | # pre-commit autoupdate 13 | # 14 | # See https://github.com/pre-commit/pre-commit 15 | 16 | repos: 17 | # Standard hooks 18 | - repo: https://github.com/pre-commit/pre-commit-hooks 19 | rev: v5.0.0 20 | hooks: 21 | - id: check-added-large-files 22 | - id: check-ast 23 | - id: check-case-conflict 24 | - id: check-docstring-first 25 | - id: check-merge-conflict 26 | - id: check-symlinks 27 | - id: check-xml 28 | - id: check-yaml 29 | - id: debug-statements 30 | - id: end-of-file-fixer 31 | - id: mixed-line-ending 32 | - id: trailing-whitespace 33 | exclude_types: [rst] 34 | - id: fix-byte-order-marker 35 | 36 | 37 | # Python hooks 38 | - repo: https://github.com/asottile/pyupgrade 39 | rev: v3.20.0 40 | hooks: 41 | - id: pyupgrade 42 | args: [--py36-plus] 43 | 44 | # PyDocStyle 45 | - repo: https://github.com/PyCQA/pydocstyle 46 | rev: 6.3.0 47 | hooks: 48 | - id: pydocstyle 49 | args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] 50 | 51 | - repo: https://github.com/psf/black 52 | rev: 25.1.0 53 | hooks: 54 | - id: black 55 | args: ["--line-length=99"] 56 | 57 | - repo: https://github.com/pycqa/flake8 58 | rev: 7.2.0 59 | hooks: 60 | - id: flake8 61 | args: ["--extend-ignore=E501"] 62 | 63 | # CPP hooks 64 | - repo: https://github.com/pre-commit/mirrors-clang-format 65 | rev: v20.1.5 66 | hooks: 67 | - id: clang-format 68 | args: ['-fallback-style=none', '-i'] 69 | 70 | - repo: local 71 | hooks: 72 | - id: ament_cppcheck 73 | name: ament_cppcheck 74 | description: Static code analysis of C/C++ files. 75 | entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck 76 | language: system 77 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 78 | 79 | - repo: local 80 | hooks: 81 | - id: ament_cpplint 82 | name: ament_cpplint 83 | description: Static code analysis of C/C++ files. 84 | entry: ament_cpplint 85 | language: system 86 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 87 | args: ["--linelength=100", "--filter=-whitespace/newline"] 88 | 89 | # Cmake hooks 90 | - repo: local 91 | hooks: 92 | - id: ament_lint_cmake 93 | name: ament_lint_cmake 94 | description: Check format of CMakeLists.txt files. 95 | entry: ament_lint_cmake 96 | language: system 97 | files: CMakeLists\.txt$ 98 | 99 | # Copyright 100 | - repo: local 101 | hooks: 102 | - id: ament_copyright 103 | name: ament_copyright 104 | description: Check if copyright notice is available in all files. 105 | entry: ament_copyright 106 | language: system 107 | exclude: doc/conf.py 108 | 109 | # Docs - RestructuredText hooks 110 | - repo: https://github.com/PyCQA/doc8 111 | rev: v1.1.2 112 | hooks: 113 | - id: doc8 114 | args: [ 115 | '--max-line-length=100', 116 | '--ignore=D001', 117 | '--ignore-path=doc/index.rst' # D000 fails 118 | ] 119 | exclude: CHANGELOG\.rst$ 120 | 121 | - repo: https://github.com/pre-commit/pygrep-hooks 122 | rev: v1.10.0 123 | hooks: 124 | - id: rst-backticks 125 | exclude: CHANGELOG\.rst$ 126 | - id: rst-directive-colons 127 | - id: rst-inline-touching-normal 128 | 129 | # Spellcheck in comments and docs 130 | # skipping of *.svg files is not working... 131 | - repo: https://github.com/codespell-project/codespell 132 | rev: v2.4.1 133 | hooks: 134 | - id: codespell 135 | args: ['--write-changes'] 136 | exclude: CHANGELOG\.rst|\.(svg|pyc)$ 137 | 138 | - repo: https://github.com/python-jsonschema/check-jsonschema 139 | rev: 0.33.0 140 | hooks: 141 | - id: check-github-workflows 142 | args: ["--verbose"] 143 | - id: check-github-actions 144 | args: ["--verbose"] 145 | - id: check-dependabot 146 | args: ["--verbose"] 147 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /LICENSE.BSD-3-clause: -------------------------------------------------------------------------------- 1 | Copyright 2025 ros2_control Development Team 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions are met: 5 | 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 13 | * Neither the name of the copyright holder nor the names of its 14 | contributors may be used to endorse or promote products derived from 15 | this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # control_toolbox 2 | 3 | [![License](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) 4 | [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) 5 | [![codecov](https://codecov.io/gh/ros-controls/control_toolbox/graph/badge.svg?token=0o4dFzADHj)](https://codecov.io/gh/ros-controls/control_toolbox) 6 | 7 | This package contains several C++ classes and filter plugins useful in writing controllers. 8 | 9 | See the documentation of [ros2_control](http://control.ros.org) and release infos on [index.ros.org](http://index.ros.org/p/control_toolbox). 10 | 11 | ## Contributing 12 | 13 | As an open-source project, we welcome each contributor, regardless of their background and experience. Pick a [PR](https://github.com/ros-controls/control_toolbox/pulls) and review it, or [create your own](https://github.com/ros-controls/control_toolbox/contribute)! 14 | If you are new to the project, please read the [contributing guide](https://control.ros.org/rolling/doc/contributing/contributing.html) for more information on how to get started. We are happy to help you with your first contribution. 15 | 16 | ## Build status 17 | 18 | ROS2 Distro | Branch | Build status | Documentation | Package build 19 | :---------: | :----: | :----------: | :-----------: | :---------------: 20 | **Rolling** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | [![Binary Build](https://github.com/ros-controls/control_toolbox/actions/workflows/rolling-build-binary.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/rolling-build-binary.yml?branch=ros2-master)
[![Source Build](https://github.com/ros-controls/control_toolbox/actions/workflows/rolling-build-source.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/rolling-build-source.yml?branch=ros2-master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__control_toolbox__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__control_toolbox__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/control_toolbox/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__control_toolbox__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__control_toolbox__ubuntu_noble_amd64__binary/) 21 | **Kilted** | [`ros2-master`](https://github.com/ros-controls/control_toolbox/tree/ros2-master) | see above
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__control_toolbox__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__control_toolbox__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/rolling/p/control_toolbox/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__control_toolbox__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__control_toolbox__ubuntu_noble_amd64__binary/) 22 | **Jazzy** | [`jazzy`](https://github.com/ros-controls/control_toolbox/tree/jazzy) | [![Jazzy Binary Build](https://github.com/ros-controls/control_toolbox/actions/workflows/jazzy-build-binary.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/jazzy-build-binary.yml?branch=ros2-master)
[![Jazzy Source Build](https://github.com/ros-controls/control_toolbox/actions/workflows/jazzy-build-source.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/jazzy-build-source.yml?branch=ros2-master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__control_toolbox__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__control_toolbox__ubuntu_noble_amd64/) | [API](http://docs.ros.org/en/jazzy/p/control_toolbox/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__control_toolbox__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__control_toolbox__ubuntu_noble_amd64__binary/) 23 | **Humble** | [`humble`](https://github.com/ros-controls/control_toolbox/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/control_toolbox/actions/workflows/humble-build-binary.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/humble-build-binary.yml?branch=ros2-master)
[![Humble Source Build](https://github.com/ros-controls/control_toolbox/actions/workflows/humble-build-source.yml/badge.svg?branch=ros2-master)](https://github.com/ros-controls/control_toolbox/actions/workflows/humble-build-source.yml?branch=ros2-master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__control_toolbox__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__control_toolbox__ubuntu_jammy_amd64/) | [API](http://docs.ros.org/en/humble/p/control_toolbox/) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__control_toolbox__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__control_toolbox__ubuntu_jammy_amd64__binary/) 24 | 25 | ## Acknowledgements 26 | 27 | The project has received major contributions from companies and institutions [listed on control.ros.org](https://control.ros.org/rolling/doc/acknowledgements/acknowledgements.html) 28 | -------------------------------------------------------------------------------- /codecov.yml: -------------------------------------------------------------------------------- 1 | coverage: 2 | precision: 2 3 | round: down 4 | range: "35...100" 5 | status: 6 | project: 7 | default: 8 | informational: true 9 | patch: off 10 | fixes: 11 | - "ros_ws/src/control_toolbox/::" 12 | comment: 13 | layout: "diff, flags, files" 14 | behavior: default 15 | -------------------------------------------------------------------------------- /control_toolbox-not-released.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ## EXAMPLE DEPENDENCY 3 | # : 4 | # type: git 5 | # url: git@github.com:/.git 6 | # version: master 7 | -------------------------------------------------------------------------------- /control_toolbox-not-released.jazzy.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ## EXAMPLE DEPENDENCY 3 | # : 4 | # type: git 5 | # url: git@github.com:/.git 6 | # version: master 7 | -------------------------------------------------------------------------------- /control_toolbox-not-released.kilted.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ## EXAMPLE DEPENDENCY 3 | # : 4 | # type: git 5 | # url: git@github.com:/.git 6 | # version: master 7 | -------------------------------------------------------------------------------- /control_toolbox-not-released.rolling.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ## EXAMPLE DEPENDENCY 3 | # : 4 | # type: git 5 | # url: git@github.com:/.git 6 | # version: master 7 | -------------------------------------------------------------------------------- /control_toolbox.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: humble 6 | realtime_tools: 7 | type: git 8 | url: https://github.com/ros-controls/realtime_tools 9 | version: humble 10 | -------------------------------------------------------------------------------- /control_toolbox.jazzy.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: jazzy 6 | realtime_tools: 7 | type: git 8 | url: https://github.com/ros-controls/realtime_tools 9 | version: jazzy 10 | ros2_control_cmake: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_control_cmake.git 13 | version: master 14 | -------------------------------------------------------------------------------- /control_toolbox.kilted.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: master 6 | realtime_tools: 7 | type: git 8 | url: https://github.com/ros-controls/realtime_tools 9 | version: master 10 | ros2_control_cmake: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_control_cmake.git 13 | version: master 14 | -------------------------------------------------------------------------------- /control_toolbox.rolling.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | control_msgs: 3 | type: git 4 | url: https://github.com/ros-controls/control_msgs.git 5 | version: master 6 | realtime_tools: 7 | type: git 8 | url: https://github.com/ros-controls/realtime_tools 9 | version: master 10 | ros2_control_cmake: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_control_cmake.git 13 | version: master 14 | -------------------------------------------------------------------------------- /control_toolbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(control_toolbox LANGUAGES CXX) 3 | 4 | find_package(ros2_control_cmake REQUIRED) 5 | set_compiler_options() 6 | 7 | if(WIN32) 8 | add_compile_definitions( 9 | # For math constants 10 | # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019 11 | _USE_MATH_DEFINES 12 | # Minimize Windows namespace collision 13 | NOMINMAX 14 | WIN32_LEAN_AND_MEAN 15 | ) 16 | # set the same behavior for windows as it is on linux 17 | export_windows_symbols() 18 | endif() 19 | 20 | set(THIS_PACKAGE_INCLUDE_DEPENDS 21 | control_msgs 22 | rclcpp 23 | rcl_interfaces 24 | rcutils 25 | realtime_tools 26 | ) 27 | 28 | find_package(ament_cmake REQUIRED) 29 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 30 | find_package(${Dependency} REQUIRED) 31 | endforeach() 32 | 33 | add_library(control_toolbox SHARED 34 | src/dither.cpp 35 | src/limited_proxy.cpp 36 | src/pid_ros.cpp 37 | src/pid.cpp 38 | src/sine_sweep.cpp 39 | src/sinusoid.cpp 40 | ) 41 | target_compile_features(control_toolbox PUBLIC cxx_std_17) 42 | target_include_directories(control_toolbox PUBLIC 43 | $ 44 | $ 45 | ) 46 | target_link_libraries(control_toolbox PUBLIC 47 | ${control_msgs_TARGETS} 48 | ${rcl_interfaces_TARGETS} 49 | rclcpp::rclcpp 50 | rcutils::rcutils 51 | realtime_tools::realtime_tools 52 | ) 53 | target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") 54 | 55 | 56 | ######################## 57 | # Build control filters 58 | ######################## 59 | set(CONTROL_FILTERS_INCLUDE_DEPENDS 60 | filters 61 | geometry_msgs 62 | generate_parameter_library 63 | pluginlib 64 | rclcpp 65 | tf2_geometry_msgs 66 | tf2 67 | tf2_ros 68 | ) 69 | set(CONTROL_FILTERS_INCLUDE_TARGETS 70 | filters::filter_base 71 | ${geometry_msgs_TARGETS} 72 | ${generate_parameter_library_TARGETS} 73 | pluginlib::pluginlib 74 | rclcpp::rclcpp 75 | tf2_geometry_msgs::tf2_geometry_msgs 76 | tf2::tf2 77 | tf2_ros::tf2_ros 78 | ) 79 | 80 | foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS}) 81 | find_package(${Dependency} REQUIRED) 82 | endforeach() 83 | find_package(Eigen3 REQUIRED NO_MODULE) 84 | 85 | generate_parameter_library( 86 | gravity_compensation_filter_parameters 87 | src/control_filters/gravity_compensation_filter_parameters.yaml 88 | ) 89 | 90 | add_library(gravity_compensation SHARED 91 | src/control_filters/gravity_compensation.cpp 92 | ) 93 | target_compile_features(gravity_compensation PUBLIC cxx_std_17) 94 | target_include_directories(gravity_compensation PUBLIC 95 | $ 96 | $ 97 | ) 98 | target_link_libraries(gravity_compensation PUBLIC 99 | gravity_compensation_filter_parameters 100 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 101 | ) 102 | 103 | generate_parameter_library( 104 | low_pass_filter_parameters 105 | src/control_filters/low_pass_filter_parameters.yaml 106 | ) 107 | 108 | add_library(low_pass_filter SHARED 109 | src/control_filters/low_pass_filter.cpp 110 | ) 111 | target_compile_features(low_pass_filter PUBLIC cxx_std_17) 112 | target_include_directories(low_pass_filter PUBLIC 113 | $ 114 | $ 115 | $ 116 | ) 117 | target_link_libraries(low_pass_filter PUBLIC 118 | low_pass_filter_parameters 119 | Eigen3::Eigen 120 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 121 | ) 122 | 123 | generate_parameter_library( 124 | rate_limiter_parameters 125 | src/control_filters/rate_limiter_parameters.yaml 126 | include/control_filters/custom_validators.hpp 127 | ) 128 | 129 | add_library(rate_limiter SHARED 130 | src/control_filters/rate_limiter.cpp 131 | ) 132 | target_compile_features(rate_limiter PUBLIC cxx_std_17) 133 | target_include_directories(rate_limiter PUBLIC 134 | $ 135 | $ 136 | $ 137 | ) 138 | target_link_libraries(rate_limiter PUBLIC 139 | rate_limiter_parameters 140 | Eigen3::Eigen 141 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 142 | ) 143 | 144 | generate_parameter_library( 145 | exponential_filter_parameters 146 | src/control_filters/exponential_filter_parameters.yaml 147 | ) 148 | 149 | add_library(exponential_filter SHARED 150 | src/control_filters/exponential_filter.cpp 151 | ) 152 | target_compile_features(exponential_filter PUBLIC cxx_std_17) 153 | target_include_directories(exponential_filter PUBLIC 154 | $ 155 | $ 156 | ) 157 | target_link_libraries(exponential_filter PUBLIC 158 | exponential_filter_parameters 159 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 160 | ) 161 | 162 | # Install pluginlib xml 163 | pluginlib_export_plugin_description_file(filters control_filters.xml) 164 | 165 | ########## 166 | # Testing 167 | ########## 168 | if(BUILD_TESTING) 169 | find_package(ament_cmake_gmock REQUIRED) 170 | find_package(rclcpp_lifecycle REQUIRED) 171 | 172 | ament_add_gmock(pid_tests test/pid_tests.cpp) 173 | target_link_libraries(pid_tests control_toolbox) 174 | 175 | ament_add_gmock(rate_limiter_tests test/rate_limiter.cpp) 176 | target_link_libraries(rate_limiter_tests control_toolbox) 177 | 178 | ament_add_gmock(pid_ros_parameters_tests test/pid_ros_parameters_tests.cpp) 179 | target_link_libraries(pid_ros_parameters_tests control_toolbox) 180 | 181 | ament_add_gmock(pid_ros_publisher_tests test/pid_ros_publisher_tests.cpp) 182 | target_link_libraries(pid_ros_publisher_tests control_toolbox rclcpp_lifecycle::rclcpp_lifecycle) 183 | 184 | ## Control Filters 185 | ### Gravity Compensation 186 | add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp 187 | ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_gravity_compensation_parameters.yaml 188 | ) 189 | target_link_libraries(test_gravity_compensation 190 | gravity_compensation 191 | gravity_compensation_filter_parameters 192 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 193 | ) 194 | 195 | ament_add_gmock(test_load_gravity_compensation test/control_filters/test_load_gravity_compensation.cpp) 196 | target_link_libraries(test_load_gravity_compensation 197 | gravity_compensation 198 | gravity_compensation_filter_parameters 199 | ${CONTROL_FILTERS_INCLUDE_TARGETS} 200 | ) 201 | 202 | # exponential_filter 203 | add_rostest_with_parameters_gmock(test_exponential_filter test/control_filters/test_exponential_filter.cpp 204 | ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_exponential_filter_parameters.yaml 205 | ) 206 | target_link_libraries(test_exponential_filter exponential_filter exponential_filter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 207 | set_tests_properties(test_exponential_filter PROPERTIES TIMEOUT 2) 208 | 209 | ament_add_gmock(test_load_exponential_filter test/control_filters/test_load_exponential_filter.cpp) 210 | target_link_libraries(test_load_exponential_filter exponential_filter exponential_filter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 211 | 212 | # low_pass_filter 213 | add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp 214 | ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml 215 | ) 216 | target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 217 | set_tests_properties(test_low_pass_filter PROPERTIES TIMEOUT 2) 218 | 219 | ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp) 220 | target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 221 | 222 | # rate_limiter 223 | add_rostest_with_parameters_gmock(test_rate_limiter test/control_filters/test_rate_limiter.cpp 224 | ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_rate_limiter_parameters.yaml 225 | ) 226 | target_link_libraries(test_rate_limiter rate_limiter rate_limiter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 227 | set_tests_properties(test_rate_limiter PROPERTIES TIMEOUT 2) 228 | 229 | ament_add_gmock(test_load_rate_limiter test/control_filters/test_load_rate_limiter.cpp) 230 | target_link_libraries(test_load_rate_limiter rate_limiter rate_limiter_parameters ${CONTROL_FILTERS_INCLUDE_TARGETS}) 231 | endif() 232 | 233 | # Install 234 | install( 235 | DIRECTORY include/ 236 | DESTINATION include/control_toolbox 237 | ) 238 | install(TARGETS control_toolbox 239 | low_pass_filter low_pass_filter_parameters 240 | rate_limiter rate_limiter_parameters 241 | exponential_filter exponential_filter_parameters gravity_compensation gravity_compensation_filter_parameters 242 | EXPORT export_control_toolbox 243 | ARCHIVE DESTINATION lib 244 | LIBRARY DESTINATION lib 245 | RUNTIME DESTINATION bin 246 | ) 247 | 248 | ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) 249 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) 250 | ament_package() 251 | -------------------------------------------------------------------------------- /control_toolbox/control_filters.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | This is a gravity compensation filter working with geometry_msgs::WrenchStamped. 8 | It subtracts the influence of a force caused by the gravitational force from force 9 | measurements. 10 | 11 | 12 | 13 | 14 | 17 | 18 | This is a low pass filter working with a double value. 19 | 20 | 21 | 24 | 25 | This is a low pass filter working with a std::vector double value. 26 | 27 | 28 | 31 | 32 | This is a low pass filter working with geometry_msgs::WrenchStamped. 33 | 34 | 35 | 36 | 37 | 40 | 41 | This is an exponential filter working with a double value. 42 | 43 | 44 | 45 | 46 | 49 | 50 | This is a rate limiter working with a double value. 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /control_toolbox/doc/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # settings will be overridden by rosdoc2, so we add here only custom settings 3 | 4 | copyright = "2024, ros2_control development team" 5 | html_logo = "https://control.ros.org/master/_static/logo_ros-controls.png" 6 | -------------------------------------------------------------------------------- /control_toolbox/doc/index.rst: -------------------------------------------------------------------------------- 1 | Welcome to the documentation for control_toolbox 2 | ================================================ 3 | 4 | This package contains several C++ classes useful in writing controllers. 5 | 6 | For more information of the ros2_control framework see `control.ros.org `__. 7 | 8 | 9 | API documentation 10 | ------------------ 11 | 12 | .. toctree:: 13 | :maxdepth: 2 14 | 15 | C++ API 16 | Service Definitions 17 | 18 | 19 | Indices and Search 20 | ================== 21 | 22 | * :ref:`genindex` 23 | * :ref:`search` 24 | -------------------------------------------------------------------------------- /control_toolbox/include/control_filters/custom_validators.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 AIT - Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ 16 | #define CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace control_filters 27 | { 28 | 29 | /** 30 | * @brief gt_eq, but check only if the value is not NaN 31 | */ 32 | template 33 | tl::expected gt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) 34 | { 35 | auto param_value = parameter.as_double(); 36 | if (!std::isnan(param_value)) 37 | { 38 | // check only if the value is not NaN 39 | return rsl::gt_eq(parameter, expected_value); 40 | } 41 | return {}; 42 | } 43 | 44 | /** 45 | * @brief lt_eq, but check only if the value is not NaN 46 | */ 47 | template 48 | tl::expected lt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value) 49 | { 50 | auto param_value = parameter.as_double(); 51 | if (!std::isnan(param_value)) 52 | { 53 | // check only if the value is not NaN 54 | return rsl::lt_eq(parameter, expected_value); 55 | } 56 | return {}; 57 | } 58 | 59 | } // namespace control_filters 60 | 61 | #endif // CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ 62 | -------------------------------------------------------------------------------- /control_toolbox/include/control_filters/exponential_filter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, AIT Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ 16 | #define CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "filters/filter_base.hpp" 23 | 24 | #include "control_toolbox/exponential_filter_parameters.hpp" 25 | #include "control_toolbox/filters.hpp" 26 | 27 | namespace control_filters 28 | { 29 | 30 | /***************************************************/ 31 | /*! \class ExponentialFilter 32 | \brief A exponential filter class. 33 | 34 | This class implements a low-pass filter for 35 | various data types. 36 | 37 | \section Usage 38 | 39 | The ExponentialFilter class is meant to be instantiated as a filter in 40 | a controller but can also be used elsewhere. 41 | For manual instantiation, you should first call configure() 42 | (in non-realtime) and then call update() at every update step. 43 | 44 | */ 45 | /***************************************************/ 46 | 47 | template 48 | class ExponentialFilter : public filters::FilterBase 49 | { 50 | public: 51 | /*! 52 | * \brief Configure the ExponentialFilter (access and process params). 53 | */ 54 | bool configure() override; 55 | 56 | /*! 57 | * \brief Applies one iteration of the exponential filter. 58 | * 59 | * \param data_in input to the filter 60 | * \param data_out filtered output 61 | * 62 | * \returns false if filter is not configured, true otherwise 63 | */ 64 | bool update(const T & data_in, T & data_out) override; 65 | 66 | private: 67 | std::shared_ptr logger_; 68 | std::shared_ptr parameter_handler_; 69 | exponential_filter::Params parameters_; 70 | T last_smoothed_value; 71 | }; 72 | 73 | template 74 | bool ExponentialFilter::configure() 75 | { 76 | logger_.reset( 77 | new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); 78 | 79 | // Initialize the parameters once 80 | if (!parameter_handler_) 81 | { 82 | try 83 | { 84 | parameter_handler_ = std::make_shared( 85 | this->params_interface_, this->param_prefix_); 86 | } 87 | catch (const std::exception & ex) 88 | { 89 | RCLCPP_ERROR( 90 | (*logger_), "Exponential filter cannot be configured: %s (type : %s)", ex.what(), 91 | typeid(ex).name()); 92 | parameter_handler_.reset(); 93 | return false; 94 | } 95 | catch (...) 96 | { 97 | RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Exponential filter"); 98 | parameter_handler_.reset(); 99 | return false; 100 | } 101 | } 102 | parameters_ = parameter_handler_->get_params(); 103 | 104 | last_smoothed_value = std::numeric_limits::quiet_NaN(); 105 | 106 | return true; 107 | } 108 | 109 | template 110 | bool ExponentialFilter::update(const T & data_in, T & data_out) 111 | { 112 | if (!this->configured_) 113 | { 114 | throw std::runtime_error("Filter is not configured"); 115 | } 116 | 117 | // Update internal parameters if required 118 | if (parameter_handler_->is_old(parameters_)) 119 | { 120 | parameters_ = parameter_handler_->get_params(); 121 | } 122 | 123 | if (std::isnan(last_smoothed_value)) 124 | { 125 | last_smoothed_value = data_in; 126 | } 127 | 128 | data_out = last_smoothed_value = 129 | filters::exponentialSmoothing(data_in, last_smoothed_value, parameters_.alpha); 130 | return true; 131 | } 132 | 133 | } // namespace control_filters 134 | 135 | #endif // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ 136 | -------------------------------------------------------------------------------- /control_toolbox/include/control_filters/gravity_compensation.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ 16 | #define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "filters/filter_base.hpp" 24 | #include "geometry_msgs/msg/vector3_stamped.hpp" 25 | #include "tf2_ros/buffer.h" 26 | #include "tf2_ros/transform_listener.h" 27 | 28 | #include "control_toolbox/gravity_compensation_filter_parameters.hpp" 29 | 30 | namespace control_filters 31 | { 32 | 33 | template 34 | class GravityCompensation : public filters::FilterBase 35 | { 36 | public: 37 | /** \brief Constructor */ 38 | GravityCompensation(); 39 | 40 | /** \brief Destructor */ 41 | ~GravityCompensation(); 42 | 43 | /** @brief Configure filter parameters */ 44 | bool configure() override; 45 | 46 | /** \brief Update the filter and return the data separately 47 | * \param data_in T array with length width 48 | * \param data_out T array with length width 49 | */ 50 | bool update(const T & data_in, T & data_out) override; 51 | 52 | protected: 53 | void compute_internal_params() 54 | { 55 | cog_.vector.x = parameters_.tool.CoG[0]; 56 | cog_.vector.y = parameters_.tool.CoG[1]; 57 | cog_.vector.z = parameters_.tool.CoG[2]; 58 | cst_ext_force_.vector.x = parameters_.tool.gravity_field[0] * parameters_.tool.mass; 59 | cst_ext_force_.vector.y = parameters_.tool.gravity_field[1] * parameters_.tool.mass; 60 | cst_ext_force_.vector.z = parameters_.tool.gravity_field[2] * parameters_.tool.mass; 61 | }; 62 | 63 | private: 64 | rclcpp::Clock::SharedPtr clock_; 65 | std::shared_ptr logger_; 66 | std::shared_ptr parameter_handler_; 67 | gravity_compensation_filter::Params parameters_; 68 | 69 | // Frames for Transformation of Gravity / CoG Vector 70 | std::string world_frame_; // frame in which gravity is given 71 | std::string sensor_frame_; // frame in which Cog is given and compution occur 72 | // Storage for Calibration Values 73 | geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt sensor frame) 74 | geometry_msgs::msg::Vector3Stamped cst_ext_force_; // Gravity Force Vector (wrt world frame) 75 | 76 | // Filter objects 77 | std::unique_ptr p_tf_Buffer_; 78 | std::unique_ptr p_tf_Listener_; 79 | geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_, 80 | transform_data_out_sensor_, transform_sensor_world_; 81 | }; 82 | 83 | template 84 | GravityCompensation::GravityCompensation() 85 | { 86 | } 87 | 88 | template 89 | GravityCompensation::~GravityCompensation() 90 | { 91 | } 92 | 93 | template 94 | bool GravityCompensation::configure() 95 | { 96 | clock_ = std::make_shared(RCL_ROS_TIME); 97 | p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); 98 | p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); 99 | 100 | logger_.reset( 101 | new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); 102 | 103 | // Initialize the parameter_listener once 104 | if (!parameter_handler_) 105 | { 106 | try 107 | { 108 | parameter_handler_ = std::make_shared( 109 | this->params_interface_, this->param_prefix_); 110 | } 111 | catch (std::exception & ex) 112 | { 113 | RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); 114 | parameter_handler_.reset(); 115 | throw; 116 | } 117 | } 118 | parameters_ = parameter_handler_->get_params(); 119 | compute_internal_params(); 120 | 121 | return true; 122 | } 123 | 124 | } // namespace control_filters 125 | 126 | #endif // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ 127 | -------------------------------------------------------------------------------- /control_toolbox/include/control_filters/low_pass_filter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ 16 | #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "filters/filter_base.hpp" 23 | #include "geometry_msgs/msg/wrench_stamped.hpp" 24 | 25 | #include "control_toolbox/low_pass_filter.hpp" 26 | #include "control_toolbox/low_pass_filter_parameters.hpp" 27 | 28 | namespace control_filters 29 | { 30 | 31 | /***************************************************/ 32 | /*! \class LowPassFilter 33 | \brief A Low-pass filter class. 34 | 35 | This class implements a low-pass filter for 36 | various data types based on an Infinite Impulse Response Filter. 37 | 38 | In particular, this class implements a simplified version of 39 | an IIR filter equation : 40 | 41 | \f$y(n) = b x(n-1) + a y(n-1)\f$ 42 | 43 | where:
44 |
    45 |
  • \f$ x(n)\f$ is the input signal 46 |
  • \f$ y(n)\f$ is the output signal (filtered) 47 |
  • \f$ b \f$ is the feedforward filter coefficient 48 |
  • \f$ a \f$ is the feedback filter coefficient 49 |
50 | 51 | and the Low-Pass coefficient equation: 52 |
53 |
    54 |
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ 55 |
  • \f$ b = 1 - a \f$ 56 |
57 | 58 | where:
59 |
    60 |
  • \f$ sf \f$ is the sampling frequency 61 |
  • \f$ df \f$ is the damping frequency 62 |
  • \f$ di \f$ is the damping intensity (amplitude) 63 |
64 | 65 | \section Usage 66 | 67 | The LowPassFilter class is meant to be instantiated as a filter in 68 | a controller but can also be used elsewhere. 69 | For manual instantiation, you should first call configure() 70 | (in non-realtime) and then call update() at every update step. 71 | 72 | */ 73 | /***************************************************/ 74 | 75 | template 76 | class LowPassFilter : public filters::FilterBase 77 | { 78 | public: 79 | /*! 80 | * \brief Configure the LowPassFilter (access and process params). 81 | */ 82 | bool configure() override; 83 | 84 | /*! 85 | * \brief Applies one iteration of the IIR filter. 86 | * 87 | * \param data_in input to the filter 88 | * \param data_out filtered output 89 | * 90 | * \returns false if filter is not configured, true otherwise 91 | */ 92 | bool update(const T & data_in, T & data_out) override; 93 | 94 | private: 95 | std::shared_ptr logger_; 96 | std::shared_ptr parameter_handler_; 97 | low_pass_filter::Params parameters_; 98 | std::shared_ptr> lpf_; 99 | }; 100 | 101 | template 102 | bool LowPassFilter::configure() 103 | { 104 | logger_.reset( 105 | new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); 106 | 107 | // Initialize the parameters once 108 | if (!parameter_handler_) 109 | { 110 | try 111 | { 112 | parameter_handler_ = std::make_shared( 113 | this->params_interface_, this->param_prefix_); 114 | } 115 | catch (const std::exception & ex) 116 | { 117 | RCLCPP_ERROR( 118 | (*logger_), "LowPass filter cannot be configured: %s (type : %s)", ex.what(), 119 | typeid(ex).name()); 120 | parameter_handler_.reset(); 121 | return false; 122 | } 123 | catch (...) 124 | { 125 | RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring LowPass filter"); 126 | parameter_handler_.reset(); 127 | return false; 128 | } 129 | } 130 | parameters_ = parameter_handler_->get_params(); 131 | lpf_ = std::make_shared>( 132 | parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); 133 | 134 | return lpf_->configure(); 135 | } 136 | 137 | template <> 138 | inline bool LowPassFilter::update( 139 | const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) 140 | { 141 | if (!this->configured_ || !lpf_ || !lpf_->is_configured()) 142 | { 143 | throw std::runtime_error("Filter is not configured"); 144 | } 145 | 146 | // Update internal parameters if required 147 | if (parameter_handler_->is_old(parameters_)) 148 | { 149 | parameters_ = parameter_handler_->get_params(); 150 | lpf_->set_params( 151 | parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); 152 | } 153 | 154 | return lpf_->update(data_in, data_out); 155 | } 156 | 157 | template <> 158 | inline bool LowPassFilter>::update( 159 | const std::vector & data_in, std::vector & data_out) 160 | { 161 | if (!this->configured_ || !lpf_ || !lpf_->is_configured()) 162 | { 163 | throw std::runtime_error("Filter is not configured"); 164 | } 165 | 166 | // Update internal parameters if required 167 | if (parameter_handler_->is_old(parameters_)) 168 | { 169 | parameters_ = parameter_handler_->get_params(); 170 | lpf_->set_params( 171 | parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); 172 | } 173 | 174 | return lpf_->update(data_in, data_out); 175 | } 176 | 177 | template 178 | bool LowPassFilter::update(const T & data_in, T & data_out) 179 | { 180 | if (!this->configured_ || !lpf_ || !lpf_->is_configured()) 181 | { 182 | throw std::runtime_error("Filter is not configured"); 183 | } 184 | 185 | // Update internal parameters if required 186 | if (parameter_handler_->is_old(parameters_)) 187 | { 188 | parameters_ = parameter_handler_->get_params(); 189 | lpf_->set_params( 190 | parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); 191 | } 192 | 193 | return lpf_->update(data_in, data_out); 194 | } 195 | 196 | } // namespace control_filters 197 | 198 | #endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ 199 | -------------------------------------------------------------------------------- /control_toolbox/include/control_filters/rate_limiter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 AIT - Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__RATE_LIMITER_HPP_ 16 | #define CONTROL_FILTERS__RATE_LIMITER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "filters/filter_base.hpp" 25 | 26 | #include "control_toolbox/rate_limiter.hpp" 27 | #include "control_toolbox/rate_limiter_parameters.hpp" 28 | 29 | namespace control_filters 30 | { 31 | 32 | /***************************************************/ 33 | /*! \class RateLimiter 34 | 35 | \section Usage 36 | 37 | The RateLimiter class is meant to be instantiated as a filter in 38 | a controller but can also be used elsewhere. 39 | For manual instantiation, you should first call configure() 40 | (in non-realtime) and then call update() at every update step. 41 | 42 | */ 43 | /***************************************************/ 44 | 45 | template 46 | class RateLimiter : public filters::FilterBase 47 | { 48 | public: 49 | /*! 50 | * \brief Configure the RateLimiter (access and process params). 51 | */ 52 | bool configure() override; 53 | 54 | /*! 55 | * \brief Applies one step of the rate limiter 56 | * 57 | * \param data_in input to the limiter 58 | * \param data_out limited output 59 | * 60 | * \returns false if filter is not configured, true otherwise 61 | */ 62 | bool update(const T & data_in, T & data_out) override; 63 | 64 | private: 65 | std::shared_ptr logger_; 66 | std::shared_ptr parameter_handler_; 67 | rate_limiter::Params parameters_; 68 | std::shared_ptr> limiter; 69 | 70 | T v1, v0; 71 | }; 72 | 73 | template 74 | bool RateLimiter::configure() 75 | { 76 | logger_.reset( 77 | new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); 78 | 79 | v0 = v1 = std::numeric_limits::quiet_NaN(); 80 | 81 | // Initialize the parameters once 82 | if (!parameter_handler_) 83 | { 84 | try 85 | { 86 | parameter_handler_ = 87 | std::make_shared(this->params_interface_, this->param_prefix_); 88 | } 89 | catch (const std::exception & ex) 90 | { 91 | RCLCPP_ERROR( 92 | (*logger_), "Rate Limiter filter cannot be configured: %s (type : %s)", ex.what(), 93 | typeid(ex).name()); 94 | parameter_handler_.reset(); 95 | return false; 96 | } 97 | catch (...) 98 | { 99 | RCLCPP_ERROR((*logger_), "Caught unknown exception while configuring Rate Limiter filter"); 100 | parameter_handler_.reset(); 101 | return false; 102 | } 103 | } 104 | parameters_ = parameter_handler_->get_params(); 105 | limiter = std::make_shared>( 106 | parameters_.min_value, parameters_.max_value, parameters_.min_first_derivative_neg, 107 | parameters_.max_first_derivative_pos, parameters_.min_first_derivative_pos, 108 | parameters_.max_first_derivative_neg, parameters_.min_second_derivative, 109 | parameters_.max_second_derivative); 110 | 111 | return true; 112 | } 113 | 114 | template 115 | bool RateLimiter::update(const T & data_in, T & data_out) 116 | { 117 | if (!this->configured_ || !limiter) 118 | { 119 | throw std::runtime_error("Filter is not configured"); 120 | } 121 | 122 | // Update internal parameters if required 123 | if (parameter_handler_->is_old(parameters_)) 124 | { 125 | parameters_ = parameter_handler_->get_params(); 126 | limiter->set_params( 127 | parameters_.min_value, parameters_.max_value, parameters_.min_first_derivative_neg, 128 | parameters_.max_first_derivative_pos, parameters_.min_first_derivative_pos, 129 | parameters_.max_first_derivative_neg, parameters_.min_second_derivative, 130 | parameters_.max_second_derivative); 131 | } 132 | T v = data_in; 133 | if (std::isnan(v0)) 134 | { 135 | // not initialized yet 136 | v1 = v0 = v; 137 | } 138 | limiter->limit(v, v0, v1, static_cast(parameters_.sampling_interval)); 139 | // shift the values for the next update call 140 | v1 = v0; 141 | v0 = v; // use the limited value 142 | data_out = v; 143 | return true; 144 | } 145 | 146 | } // namespace control_filters 147 | 148 | #endif // CONTROL_FILTERS__RATE_LIMITER_HPP_ 149 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/dither.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | // \author Kevin Watts 34 | 35 | #ifndef CONTROL_TOOLBOX__DITHER_HPP_ 36 | #define CONTROL_TOOLBOX__DITHER_HPP_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "rcutils/logging_macros.h" 44 | 45 | namespace control_toolbox 46 | { 47 | /***************************************************/ 48 | /*! \class Dither 49 | * 50 | * \brief Gives white noise at specified amplitude. 51 | * 52 | * This class gives white noise at the given amplitude when 53 | * update() is called. It can be used to vibrate joints or 54 | * to break static friction. 55 | * 56 | */ 57 | class Dither 58 | { 59 | public: 60 | Dither(); 61 | 62 | /*! 63 | * \brief Get next Gaussian white noise point. Called in RT loop. 64 | *\return White noise of given amplitude. 65 | */ 66 | double update(); 67 | 68 | /* 69 | *\brief Dither gets an amplitude, must be >0 to initialize 70 | * 71 | *\param amplitude Amplitude of white noise output 72 | *\param seed Random seed for white noise 73 | */ 74 | bool init(const double & amplitude, const double & seed) 75 | { 76 | if (amplitude < 0.0) 77 | { 78 | RCUTILS_LOG_ERROR("Dither amplitude not set properly. Amplitude must be >0."); 79 | return false; 80 | } 81 | 82 | amplitude_ = amplitude; 83 | 84 | // seed generator for reproducible sequence of random numbers 85 | generator_.seed(static_cast(seed)); 86 | 87 | return true; 88 | } 89 | 90 | /* 91 | *\brief Generate a random number with random_device for non-deterministic random numbers 92 | */ 93 | static double generateRandomSeed() 94 | { 95 | std::random_device rdev{}; 96 | return static_cast(rdev()); 97 | } 98 | 99 | private: 100 | double amplitude_; /**< Amplitude of the sweep. */ 101 | double saved_value_; 102 | bool has_saved_value_; 103 | std::mt19937 generator_; /**< random number generator for white noise. */ 104 | }; 105 | } // namespace control_toolbox 106 | 107 | #endif // CONTROL_TOOLBOX__DITHER_HPP_ 108 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/filter_traits.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025, ros2_control development team 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ 16 | #define CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ 17 | 18 | #define EIGEN_INITIALIZE_MATRICES_BY_NAN 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "geometry_msgs/msg/wrench_stamped.hpp" 27 | 28 | namespace control_toolbox 29 | { 30 | 31 | template 32 | struct FilterTraits; 33 | 34 | // Wrapper around std::vector to be used as 35 | // the std::vector StorageType specialization. 36 | // This is a workaround for the fact that 37 | // std::vector's operator* and operator+ cannot be overloaded. 38 | template 39 | struct FilterVector 40 | { 41 | std::vector data; 42 | 43 | FilterVector() = default; 44 | 45 | explicit FilterVector(const std::vector & vec) : data(vec) {} 46 | 47 | explicit FilterVector(size_t size, const U & initial_value = U{}) : data(size, initial_value) {} 48 | 49 | FilterVector operator*(const U & scalar) const 50 | { 51 | FilterVector result = *this; 52 | for (auto & val : result.data) 53 | { 54 | val *= scalar; 55 | } 56 | return result; 57 | } 58 | 59 | FilterVector operator+(const FilterVector & other) const 60 | { 61 | assert(data.size() == other.data.size() && "Vectors must be of the same size for addition"); 62 | FilterVector result = *this; 63 | for (size_t i = 0; i < data.size(); ++i) 64 | { 65 | result.data[i] += other.data[i]; 66 | } 67 | return result; 68 | } 69 | 70 | size_t size() const { return data.size(); } 71 | }; 72 | 73 | // Enable scalar * FilterVector 74 | template 75 | inline FilterVector operator*(const U & scalar, const FilterVector & vec) 76 | { 77 | return vec * scalar; 78 | } 79 | 80 | template 81 | struct FilterTraits 82 | { 83 | using StorageType = T; 84 | 85 | static void initialize(StorageType & storage) 86 | { 87 | storage = T{std::numeric_limits::quiet_NaN()}; 88 | } 89 | 90 | static bool is_nan(const StorageType & storage) { return std::isnan(storage); } 91 | 92 | static bool is_finite(const StorageType & storage) { return std::isfinite(storage); } 93 | 94 | static bool is_empty(const StorageType & storage) 95 | { 96 | (void)storage; 97 | return false; 98 | } 99 | 100 | static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } 101 | 102 | static void validate_input(const T & data_in, const StorageType & filtered_value, T & data_out) 103 | { 104 | (void)data_in; 105 | (void)filtered_value; 106 | (void)data_out; // Suppress unused warnings 107 | } 108 | 109 | static void add_metadata(StorageType & storage, const StorageType & data_in) 110 | { 111 | (void)storage; 112 | (void)data_in; 113 | } 114 | }; 115 | 116 | template <> 117 | struct FilterTraits 118 | { 119 | using StorageType = Eigen::Matrix; 120 | using DataType = geometry_msgs::msg::WrenchStamped; 121 | 122 | static void initialize(StorageType & storage) 123 | { 124 | // Evocation of the default constructor through EIGEN_INITIALIZE_MATRICES_BY_NAN 125 | storage = StorageType(); 126 | } 127 | 128 | static bool is_nan(const StorageType & storage) { return storage.hasNaN(); } 129 | 130 | static bool is_finite(const DataType & data) 131 | { 132 | return std::isfinite(data.wrench.force.x) && std::isfinite(data.wrench.force.y) && 133 | std::isfinite(data.wrench.force.z) && std::isfinite(data.wrench.torque.x) && 134 | std::isfinite(data.wrench.torque.y) && std::isfinite(data.wrench.torque.z); 135 | } 136 | 137 | static bool is_empty(const StorageType & storage) 138 | { 139 | (void)storage; 140 | return false; 141 | } 142 | 143 | static void assign(DataType & data_in, const StorageType & storage) 144 | { 145 | data_in.wrench.force.x = storage[0]; 146 | data_in.wrench.force.y = storage[1]; 147 | data_in.wrench.force.z = storage[2]; 148 | data_in.wrench.torque.x = storage[3]; 149 | data_in.wrench.torque.y = storage[4]; 150 | data_in.wrench.torque.z = storage[5]; 151 | } 152 | 153 | static void assign(StorageType & storage, const DataType & data_in) 154 | { 155 | storage[0] = data_in.wrench.force.x; 156 | storage[1] = data_in.wrench.force.y; 157 | storage[2] = data_in.wrench.force.z; 158 | storage[3] = data_in.wrench.torque.x; 159 | storage[4] = data_in.wrench.torque.y; 160 | storage[5] = data_in.wrench.torque.z; 161 | } 162 | 163 | static void assign(StorageType & storage, const StorageType & data_in) { storage = data_in; } 164 | 165 | static void validate_input( 166 | const DataType & data_in, const StorageType & filtered_value, DataType & data_out) 167 | { 168 | (void)filtered_value; // Not used here 169 | 170 | // Compare new input's frame_id with previous output's frame_id 171 | assert( 172 | data_in.header.frame_id == data_out.header.frame_id && 173 | "Frame ID changed between filter updates"); 174 | } 175 | 176 | static void add_metadata(DataType & data_out, const DataType & data_in) 177 | { 178 | data_out.header = data_in.header; 179 | } 180 | }; 181 | 182 | template 183 | struct FilterTraits> 184 | { 185 | using StorageType = FilterVector; 186 | using DataType = std::vector; 187 | 188 | static void initialize(StorageType & storage) { (void)storage; } 189 | 190 | static bool is_finite(const StorageType & storage) 191 | { 192 | return std::all_of( 193 | storage.data.begin(), storage.data.end(), [](U val) { return std::isfinite(val); }); 194 | } 195 | 196 | static bool is_finite(const DataType & storage) 197 | { 198 | return std::all_of(storage.begin(), storage.end(), [](U val) { return std::isfinite(val); }); 199 | } 200 | 201 | static bool is_empty(const StorageType & storage) { return storage.data.empty(); } 202 | 203 | static bool is_nan(const StorageType & storage) 204 | { 205 | for (const auto & val : storage.data) 206 | { 207 | if (std::isnan(val)) 208 | { 209 | return true; 210 | } 211 | } 212 | 213 | return false; 214 | } 215 | 216 | static void assign(StorageType & storage, const DataType & data_in) { storage.data = data_in; } 217 | 218 | static void assign(DataType & storage, const StorageType & data_in) { storage = data_in.data; } 219 | 220 | static void assign(StorageType & storage, const StorageType & data_in) 221 | { 222 | storage.data = data_in.data; 223 | } 224 | 225 | static void validate_input( 226 | const DataType & data_in, const StorageType & filtered_value, DataType & data_out) 227 | { 228 | assert( 229 | data_in.size() == filtered_value.size() && 230 | "Input vector size does not match internal state size"); 231 | assert(data_out.size() == data_in.size() && "Input and output vectors must be the same size"); 232 | } 233 | 234 | static void add_metadata(DataType & storage, const DataType & data_in) 235 | { 236 | (void)storage; 237 | (void)data_in; 238 | } 239 | }; 240 | 241 | } // namespace control_toolbox 242 | 243 | #endif // CONTROL_TOOLBOX__FILTER_TRAITS_HPP_ 244 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/filters.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef CONTROL_TOOLBOX__FILTERS_HPP_ 34 | #define CONTROL_TOOLBOX__FILTERS_HPP_ 35 | 36 | #include 37 | 38 | namespace filters 39 | { 40 | /** Exponential smoothing filter. Alpha is between 0 and 1. 41 | * Values closer to 0 weight the last smoothed value more heavily */ 42 | 43 | static inline double exponentialSmoothing( 44 | double current_raw_value, double last_smoothed_value, double alpha) 45 | { 46 | return alpha * current_raw_value + (1 - alpha) * last_smoothed_value; 47 | } 48 | } // namespace filters 49 | 50 | #endif // CONTROL_TOOLBOX__FILTERS_HPP_" 51 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/limited_proxy.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef CONTROL_TOOLBOX__LIMITED_PROXY_HPP_ 34 | #define CONTROL_TOOLBOX__LIMITED_PROXY_HPP_ 35 | 36 | namespace control_toolbox 37 | { 38 | class LimitedProxy 39 | { 40 | public: 41 | // Controller parameter values 42 | double mass_; // Estimate of the joint mass 43 | double Kd_; // Damping gain 44 | double Kp_; // Position gain 45 | double Ki_; // Integral gain 46 | double Ficl_; // Integral force clamp 47 | double effort_limit_; // Limit on output force 48 | double vel_limit_; // Limit on velocity 49 | double pos_upper_limit_; // Upper position bound 50 | double pos_lower_limit_; // Lower position bound 51 | double lambda_proxy_; // Bandwidth of proxy reconvergence 52 | double acc_converge_; // Acceleration of proxy reconvergence 53 | 54 | LimitedProxy() 55 | : mass_(0.0), 56 | Kd_(0.0), 57 | Kp_(0.0), 58 | Ki_(0.0), 59 | Ficl_(0.0), 60 | effort_limit_(0.0), 61 | vel_limit_(0.0), 62 | pos_upper_limit_(0.0), 63 | pos_lower_limit_(0.0), 64 | lambda_proxy_(0.0), 65 | acc_converge_(0.0) 66 | { 67 | } 68 | 69 | void reset(double pos_act, double vel_act); 70 | 71 | double update( 72 | double pos_des, double vel_des, double acc_des, double pos_act, double vel_act, double dt); 73 | 74 | private: 75 | // Controller state values 76 | double last_proxy_pos_; // Proxy position 77 | double last_proxy_vel_; // Proxy velocity 78 | double last_proxy_acc_; // Proxy acceleration 79 | 80 | double last_vel_error_; // Velocity error 81 | double last_pos_error_; // Position error 82 | double last_int_error_; // Integral error 83 | }; 84 | 85 | } // namespace control_toolbox 86 | 87 | #endif // CONTROL_TOOLBOX__LIMITED_PROXY_HPP_ 88 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/low_pass_filter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ 16 | #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "control_toolbox/filter_traits.hpp" 27 | 28 | #include "geometry_msgs/msg/wrench_stamped.hpp" 29 | 30 | namespace control_toolbox 31 | { 32 | 33 | /***************************************************/ 34 | /*! \class LowPassFilter 35 | \brief A Low-pass filter class. 36 | 37 | This class implements a low-pass filter for 38 | various data types based on an Infinite Impulse Response Filter. 39 | For vector elements, the filtering is applied separately on 40 | each element of the vector. 41 | 42 | In particular, this class implements a simplified version of 43 | an IIR filter equation : 44 | 45 | \f$y(n) = b x(n-1) + a y(n-1)\f$ 46 | 47 | where:
48 |
    49 |
  • \f$ x(n)\f$ is the input signal 50 |
  • \f$ y(n)\f$ is the output signal (filtered) 51 |
  • \f$ b \f$ is the feedforward filter coefficient 52 |
  • \f$ a \f$ is the feedback filter coefficient 53 |
54 | 55 | and the Low-Pass coefficient equation: 56 |
57 |
    58 |
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ 59 |
  • \f$ b = 1 - a \f$ 60 |
61 | 62 | where:
63 |
    64 |
  • \f$ sf \f$ is the sampling frequency 65 |
  • \f$ df \f$ is the damping frequency 66 |
  • \f$ di \f$ is the damping intensity (amplitude) 67 |
68 | 69 | \section Usage 70 | 71 | For manual instantiation, you should first call configure() 72 | (in non-realtime) and then call update() at every update step. 73 | 74 | */ 75 | /***************************************************/ 76 | 77 | template 78 | class LowPassFilter 79 | { 80 | public: 81 | // Default constructor 82 | LowPassFilter(); 83 | 84 | LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity) 85 | { 86 | set_params(sampling_frequency, damping_frequency, damping_intensity); 87 | } 88 | 89 | /*! 90 | * \brief Destructor of LowPassFilter class. 91 | */ 92 | ~LowPassFilter(); 93 | 94 | /*! 95 | * \brief Configure the LowPassFilter (access and process params). 96 | */ 97 | bool configure(); 98 | 99 | /*! 100 | * \brief Applies one iteration of the IIR filter. 101 | * 102 | * \param data_in input to the filter 103 | * \param data_out filtered output 104 | * 105 | * \returns false if filter is not configured, true otherwise 106 | */ 107 | bool update(const T & data_in, T & data_out); 108 | 109 | bool is_configured() const { return configured_; } 110 | 111 | /*! 112 | * \brief Internal computation of the feedforward and feedbackward coefficients 113 | * according to the LowPassFilter parameters. 114 | */ 115 | void set_params(double sampling_frequency, double damping_frequency, double damping_intensity) 116 | { 117 | a1_ = exp( 118 | -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / 119 | (pow(10.0, damping_intensity / -10.0))); 120 | b1_ = 1.0 - a1_; 121 | }; 122 | 123 | private: 124 | // Filter parameters 125 | double a1_; /** feedbackward coefficient. */ 126 | double b1_; /** feedforward coefficient. */ 127 | 128 | // Define the storage type based on T 129 | using Traits = FilterTraits; 130 | using StorageType = typename Traits::StorageType; 131 | 132 | StorageType filtered_value_, filtered_old_value_, old_value_; 133 | 134 | bool configured_ = false; 135 | }; 136 | 137 | template 138 | LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) 139 | { 140 | } 141 | 142 | template 143 | LowPassFilter::~LowPassFilter() 144 | { 145 | } 146 | 147 | template 148 | bool LowPassFilter::configure() 149 | { 150 | Traits::initialize(filtered_value_); 151 | Traits::initialize(filtered_old_value_); 152 | Traits::initialize(old_value_); 153 | 154 | return configured_ = true; 155 | } 156 | 157 | template 158 | bool LowPassFilter::update(const T & data_in, T & data_out) 159 | { 160 | if (!configured_) 161 | { 162 | throw std::runtime_error("Filter is not configured"); 163 | } 164 | // If this is the first call to update initialize the filter at the current state 165 | // so that we dont apply an impulse to the data. 166 | if (Traits::is_nan(filtered_value_) || Traits::is_empty(filtered_value_)) 167 | { 168 | if (!Traits::is_finite(data_in)) 169 | { 170 | return false; 171 | } 172 | 173 | Traits::assign(filtered_value_, data_in); 174 | Traits::assign(filtered_old_value_, data_in); 175 | Traits::assign(old_value_, data_in); 176 | } 177 | else 178 | { 179 | // Generic validation for all types 180 | Traits::validate_input(data_in, filtered_value_, data_out); 181 | } 182 | 183 | // Filter 184 | filtered_value_ = old_value_ * b1_ + filtered_old_value_ * a1_; 185 | filtered_old_value_ = filtered_value_; 186 | 187 | Traits::assign(old_value_, data_in); 188 | Traits::assign(data_out, filtered_value_); 189 | 190 | if (Traits::is_finite(data_in)) 191 | { 192 | Traits::assign(old_value_, data_in); 193 | } 194 | 195 | Traits::add_metadata(data_out, data_in); 196 | 197 | return true; 198 | } 199 | 200 | } // namespace control_toolbox 201 | 202 | #endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ 203 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/pid_ros.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Open Source Robotics Foundation, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef CONTROL_TOOLBOX__PID_ROS_HPP_ 34 | #define CONTROL_TOOLBOX__PID_ROS_HPP_ 35 | 36 | #include 37 | #include 38 | 39 | #include "control_msgs/msg/pid_state.hpp" 40 | 41 | #include "rclcpp/clock.hpp" 42 | #include "rclcpp/duration.hpp" 43 | #include "rclcpp/node.hpp" 44 | 45 | #include "realtime_tools/realtime_buffer.hpp" 46 | #include "realtime_tools/realtime_publisher.hpp" 47 | 48 | #include "control_toolbox/pid.hpp" 49 | 50 | namespace control_toolbox 51 | { 52 | 53 | class PidROS 54 | { 55 | public: 56 | /*! 57 | * \brief Constructor of PidROS class. 58 | * 59 | * The node is passed to this class to handler the ROS parameters, this class allows 60 | * to add a prefix to the pid parameters 61 | * 62 | * \param node ROS node 63 | * \param prefix prefix to add to the pid parameters. 64 | * Per default is prefix interpreted as prefix for topics. 65 | * \param prefix_is_for_params provided prefix should be interpreted as prefix for parameters. 66 | * If the parameter is `true` then "/" in the middle of the string will not be replaced 67 | * with "." for parameters prefix. "/" or "~/" at the beginning will be removed. 68 | * 69 | */ 70 | template 71 | explicit PidROS( 72 | std::shared_ptr node_ptr, std::string prefix = std::string(""), 73 | bool prefix_is_for_params = false) 74 | : PidROS( 75 | node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(), 76 | node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(), prefix, 77 | prefix_is_for_params) 78 | { 79 | } 80 | 81 | PidROS( 82 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, 83 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, 84 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params, 85 | rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, 86 | std::string prefix = std::string(""), bool prefix_is_for_params = false); 87 | 88 | /*! 89 | * \brief Initialize the PID controller and set the parameters 90 | * \param p The proportional gain. 91 | * \param i The integral gain. 92 | * \param d The derivative gain. 93 | * \param i_max Upper integral clamp. 94 | * \param i_min Lower integral clamp. 95 | * \param antiwindup Antiwindup functionality. When set to true, limits 96 | the integral error to prevent windup; otherwise, constrains the 97 | integral contribution to the control output. i_max and 98 | i_min are applied in both scenarios. 99 | * 100 | * \note New gains are not applied if i_min_ > i_max_ 101 | */ 102 | void initialize_from_args( 103 | double p, double i, double d, double i_max, double i_min, bool antiwindup); 104 | 105 | /*! 106 | * \brief Initialize the PID controller and set the parameters 107 | * \param p The proportional gain. 108 | * \param i The integral gain. 109 | * \param d The derivative gain. 110 | * \param i_max The max integral windup. 111 | * \param i_min The min integral windup. 112 | * \param antiwindup antiwindup. 113 | * \param save_i_term save integrator output between resets. 114 | * 115 | * \note New gains are not applied if i_min_ > i_max_ 116 | */ 117 | void initialize_from_args( 118 | double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_i_term); 119 | 120 | /*! 121 | * \brief Initialize the PID controller based on already set parameters 122 | * \return True if all parameters are set (p, i, d, i_min and i_max), False otherwise 123 | */ 124 | bool initialize_from_ros_parameters(); 125 | 126 | /*! 127 | * \brief Reset the state of this PID controller 128 | * 129 | * @note save_i_term parameter is read from ROS parameters 130 | */ 131 | void reset(); 132 | 133 | /*! 134 | * \brief Reset the state of this PID controller 135 | * 136 | * \param save_i_term boolean indicating if integral term is retained on reset() 137 | */ 138 | void reset(bool save_i_term); 139 | 140 | /*! 141 | * \brief Set the PID error and compute the PID command with nonuniform time 142 | * step size. The derivative error is computed from the change in the error 143 | * and the timestep \c dt. 144 | * 145 | * \param error Error since last call (error = target - state) 146 | * \param dt Change in time since last call in seconds 147 | * 148 | * \returns PID command 149 | */ 150 | double compute_command(double error, const rclcpp::Duration & dt); 151 | 152 | /*! 153 | * \brief Set the PID error and compute the PID command with nonuniform 154 | * time step size. This also allows the user to pass in a precomputed 155 | * derivative error. 156 | * 157 | * \param error Error since last call (error = target - state) 158 | * \param error_dot d(Error)/dt since last call 159 | * \param dt Change in time since last call in seconds 160 | * 161 | * \returns PID command 162 | */ 163 | double compute_command(double error, double error_dot, const rclcpp::Duration & dt); 164 | 165 | /*! 166 | * \brief Get PID gains for the controller. 167 | * \return gains A struct of the PID gain values 168 | */ 169 | Pid::Gains get_gains(); 170 | 171 | /*! 172 | * \brief Set PID gains for the controller. 173 | * \param p The proportional gain. 174 | * \param i The integral gain. 175 | * \param d The derivative gain. 176 | * \param i_max Upper integral clamp. 177 | * \param i_min Lower integral clamp. 178 | * \param antiwindup Antiwindup functionality. When set to true, limits 179 | the integral error to prevent windup; otherwise, constrains the 180 | integral contribution to the control output. i_max and 181 | i_min are applied in both scenarios. 182 | * 183 | * \note New gains are not applied if i_min > i_max 184 | */ 185 | void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false); 186 | 187 | /*! 188 | * \brief Set PID gains for the controller. 189 | * \param gains A struct of the PID gain values 190 | * 191 | * \note New gains are not applied if gains.i_min_ > gains.i_max_ 192 | */ 193 | void set_gains(const Pid::Gains & gains); 194 | 195 | /*! 196 | * \brief Set current command for this PID controller 197 | * \param cmd command to set 198 | */ 199 | void set_current_cmd(double cmd); 200 | 201 | /*! 202 | * \brief Return current command for this PID controller 203 | * \return current cmd 204 | */ 205 | double get_current_cmd(); 206 | 207 | /*! 208 | * \brief Return PID state publisher 209 | * \return shared_ptr to the PID state publisher 210 | */ 211 | std::shared_ptr> get_pid_state_publisher(); 212 | 213 | /*! 214 | * \brief Return PID error terms for the controller. 215 | * \param pe[out] The proportional error. 216 | * \param ie[out] The weighted integral error. 217 | * \param de[out] The derivative error. 218 | */ 219 | void get_current_pid_errors(double & pe, double & ie, double & de); 220 | 221 | /*! 222 | * \brief Print to console the current parameters 223 | */ 224 | void print_values(); 225 | 226 | /*! 227 | * \brief Return PID parameters callback handle 228 | * \return shared_ptr to the PID parameters callback handle 229 | */ 230 | inline rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr 231 | get_parameters_callback_handle() 232 | { 233 | return parameter_callback_; 234 | } 235 | 236 | protected: 237 | std::string topic_prefix_; 238 | std::string param_prefix_; 239 | 240 | private: 241 | void set_parameter_event_callback(); 242 | 243 | void publish_pid_state(double cmd, double error, rclcpp::Duration dt); 244 | 245 | void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value); 246 | 247 | bool get_double_param(const std::string & param_name, double & value); 248 | 249 | bool get_boolean_param(const std::string & param_name, bool & value); 250 | 251 | /*! 252 | * \brief Set prefix for topic and parameter names 253 | * \param[in] topic_prefix prefix to add to the pid parameters. 254 | * Per default is prefix interpreted as prefix for topics. 255 | * If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e., 256 | * "/" will be added in front of topic prefix 257 | */ 258 | void set_prefixes(const std::string & topic_prefix); 259 | 260 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; 261 | 262 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; 263 | rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; 264 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_; 265 | rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface_; 266 | 267 | std::shared_ptr> rt_state_pub_; 268 | std::shared_ptr> state_pub_; 269 | 270 | Pid pid_; 271 | }; 272 | 273 | } // namespace control_toolbox 274 | 275 | #endif // CONTROL_TOOLBOX__PID_ROS_HPP_ 276 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/sine_sweep.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #ifndef CONTROL_TOOLBOX__SINE_SWEEP_HPP_ 34 | #define CONTROL_TOOLBOX__SINE_SWEEP_HPP_ 35 | 36 | #include 37 | 38 | namespace control_toolbox 39 | { 40 | /***************************************************/ 41 | /*! \class SineSweep 42 | \brief Generates a sine sweep for frequency analysis of a joint 43 | 44 | This class basically calculates the output for 45 | a sine sweep. Where the signal is a sine wave, 46 | whose frequency is exponentially increased from 47 | \f$\omega_1\f$ to \f$\omega_2\f$ over \f$T\f$ seconds.
48 | 49 | \f$s(n) = A \sin [ K(e^{\delta t/L} - 1) ]\f$
50 | 51 | where:
52 | \f$K = \frac{\omega_1T}{\ln \frac{\omega_2}{\omega_1} }\f$
53 | \f$L = \frac{T}{\ln \frac{\omega_2}{\omega_1} }\f$.
54 | 55 | */ 56 | /***************************************************/ 57 | 58 | class SineSweep 59 | { 60 | public: 61 | /*! 62 | * \brief Constructor 63 | */ 64 | SineSweep(); 65 | 66 | /*! 67 | * \brief Update the SineSweep loop with nonuniform time step size. 68 | * 69 | * \param dt Change in time since last call 70 | */ 71 | double update(rclcpp::Duration dt); 72 | 73 | /*! 74 | * \brief Initializes everything and calculates the constants for the sweep. 75 | * 76 | * \param start_freq Start frequency of the sweep, \f$\omega_1\f$ . 77 | * \param end_freq End frequency of the sweep, \f$\omega_2\f$. 78 | * \param duration The duration of the sweep, \f$T\f$. 79 | * \param amplitude The amplitude of the sweep, \f$A\f$. 80 | */ 81 | bool init(double start_freq, double end_freq, double duration, double amplitude); 82 | 83 | private: 84 | double amplitude_; /**< Amplitude of the sweep. */ 85 | rclcpp::Duration duration_; /**< Duration of the sweep. */ 86 | double start_angular_freq_; /**< Start angular frequency of the sweep. */ 87 | double end_angular_freq_; /**< End angular frequency of the sweep. */ 88 | double K_; /**< Constant \f$K\f$. */ 89 | double L_; /**< Constant \f$L\f$.*/ 90 | double cmd_; /**< Command to send. */ 91 | }; 92 | } // namespace control_toolbox 93 | 94 | #endif // CONTROL_TOOLBOX__SINE_SWEEP_HPP_ 95 | -------------------------------------------------------------------------------- /control_toolbox/include/control_toolbox/sinusoid.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | /** \author Mrinal Kalakrishnan */ 34 | 35 | #ifndef CONTROL_TOOLBOX__SINUSOID_HPP_ 36 | #define CONTROL_TOOLBOX__SINUSOID_HPP_ 37 | 38 | namespace control_toolbox 39 | { 40 | /** 41 | * \class Sinusoid 42 | * \brief A basic sine class 43 | * 44 | * This class calculates the output for a sine wave and its derivatives, given the amplitude, 45 | * phase, frequency and offset.
46 | * 47 | */ 48 | class Sinusoid 49 | { 50 | public: 51 | /** 52 | * \brief Constructor 53 | */ 54 | Sinusoid(); 55 | 56 | /** 57 | * \brief Constructor which initializes values 58 | * 59 | * \param offset A DC offset to be added to the sine wave 60 | * \param amplitude Amplitude of the sine wave 61 | * \param frequency Frequency of the sine wave 62 | * \param phase Phase (in radians) of the sine wave at t=0 63 | */ 64 | Sinusoid(double offset, double amplitude, double frequency, double phase); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | virtual ~Sinusoid(); 70 | 71 | /** 72 | * Prints the parameters of the sine wave to stdout (for debugging) 73 | */ 74 | void debug(); 75 | 76 | /** 77 | * \brief Gets the value and derivatives of the sinusoid at a given time 78 | * 79 | * \param time Time at which to sample the sine wave 80 | * \param qd (output) The derivative of the sine wave 81 | * \param qdd (output) Second derivative of the sine wave 82 | * \return The sampled value of the sine wave 83 | */ 84 | double update(double time, double & qd, double & qdd); 85 | 86 | private: 87 | double offset_; /**< DC offset of the sine wave. */ 88 | double amplitude_; /**< Amplitude of the sine wave. */ 89 | double frequency_; /**< Frequency of the sine wave. */ 90 | double phase_; /**< Phase of the sine wave at t=0. */ 91 | }; 92 | 93 | } // namespace control_toolbox 94 | 95 | #endif // CONTROL_TOOLBOX__SINUSOID_HPP_ 96 | -------------------------------------------------------------------------------- /control_toolbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_toolbox 4 | 5.4.0 5 | The control toolbox contains modules that are useful across all controllers. 6 | 7 | Bence Magyar 8 | Denis Štogl 9 | Christoph Froehlich 10 | Sai Kishor Kothakota 11 | 12 | Apache License 2.0 13 | 14 | https://control.ros.org 15 | https://github.com/ros-controls/control_toolbox/issues 16 | https://github.com/ros-controls/control_toolbox/ 17 | 18 | Melonee Wise 19 | Sachin Chitta 20 | John Hsu 21 | 22 | ament_cmake 23 | 24 | ros2_control_cmake 25 | 26 | control_msgs 27 | eigen 28 | filters 29 | generate_parameter_library 30 | geometry_msgs 31 | pluginlib 32 | rclcpp 33 | rcutils 34 | realtime_tools 35 | tf2 36 | tf2_ros 37 | tf2_geometry_msgs 38 | 39 | ament_cmake_gmock 40 | rclcpp_lifecycle 41 | 42 | ament_cmake 43 | 44 | 45 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/exponential_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, AIT Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "control_filters/exponential_filter.hpp" 16 | 17 | #include "pluginlib/class_list_macros.hpp" 18 | 19 | PLUGINLIB_EXPORT_CLASS(control_filters::ExponentialFilter, filters::FilterBase) 20 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/exponential_filter_parameters.yaml: -------------------------------------------------------------------------------- 1 | exponential_filter: 2 | alpha: { 3 | type: double, 4 | description: "Exponential decay factor", 5 | validation: { 6 | bounds<>: [0.0, 1.0] 7 | }, 8 | } 9 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/gravity_compensation.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "control_filters/gravity_compensation.hpp" 16 | 17 | #include "geometry_msgs/msg/vector3_stamped.hpp" 18 | #include "geometry_msgs/msg/wrench_stamped.hpp" 19 | #include "tf2/LinearMath/Vector3.hpp" 20 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 21 | 22 | namespace control_filters 23 | { 24 | template <> 25 | bool GravityCompensation::update( 26 | const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) 27 | { 28 | if (!this->configured_) 29 | { 30 | throw std::runtime_error("Filter is not configured"); 31 | } 32 | 33 | // Update internal parameters if required 34 | if (parameter_handler_->is_old(parameters_)) 35 | { 36 | parameters_ = parameter_handler_->get_params(); 37 | compute_internal_params(); 38 | } 39 | 40 | try 41 | { 42 | // transform from data_in frame to sensor_frame 43 | transform_sensor_datain_ = p_tf_Buffer_->lookupTransform( 44 | parameters_.sensor_frame, data_in.header.frame_id, rclcpp::Time()); 45 | 46 | // use data_out frame id for the back transformation, otherwise same is data_in 47 | if (!data_out.header.frame_id.empty()) 48 | { 49 | data_out.header.stamp = data_in.header.stamp; // only copy the timestamp 50 | } 51 | else 52 | { 53 | data_out.header = data_in.header; // keep the same header and same frame_id 54 | } 55 | transform_data_out_sensor_ = p_tf_Buffer_->lookupTransform( 56 | data_out.header.frame_id, parameters_.sensor_frame, rclcpp::Time()); 57 | // transform from world (gravity) frame to sensor frame 58 | transform_sensor_world_ = p_tf_Buffer_->lookupTransform( 59 | parameters_.sensor_frame, parameters_.world_frame, rclcpp::Time()); 60 | } 61 | catch (const tf2::TransformException & ex) 62 | { 63 | std::stringstream frames_sstr; 64 | frames_sstr << "datain:" << data_in.header.frame_id << " dataout:" << data_out.header.frame_id; 65 | frames_sstr << " world:" << parameters_.world_frame << " sensor:" << parameters_.sensor_frame; 66 | RCLCPP_ERROR_SKIPFIRST_THROTTLE( 67 | (*logger_), *clock_, 5000, "GravityCompensation update failed:%s, given frames are %s", 68 | ex.what(), frames_sstr.str().c_str()); 69 | return false; // if cannot transform, result of subsequent computations is invalid 70 | } 71 | 72 | // Transform data_in to sensor_frame frame 73 | geometry_msgs::msg::Wrench wrench_sensor; 74 | tf2::doTransform(data_in.wrench, wrench_sensor, transform_sensor_datain_); 75 | 76 | // CoG is already in sensor_frame 77 | 78 | // Rotate (no wrench, just a force) the gravity force to sensor frame 79 | geometry_msgs::msg::Vector3Stamped cst_ext_force_transformed; 80 | tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, transform_sensor_world_); 81 | 82 | // Compensate for gravity force in sensor frame 83 | wrench_sensor.force.x -= cst_ext_force_transformed.vector.x; 84 | wrench_sensor.force.y -= cst_ext_force_transformed.vector.y; 85 | wrench_sensor.force.z -= cst_ext_force_transformed.vector.z; 86 | // Compensate for torque produced by offset CoG in sensor frame 87 | // result from cross-product of cog Vector and force 88 | tf2::Vector3 cog_vector = {cog_.vector.x, cog_.vector.y, cog_.vector.z}; 89 | auto added_torque = cog_vector.cross( 90 | {cst_ext_force_transformed.vector.x, cst_ext_force_transformed.vector.y, 91 | cst_ext_force_transformed.vector.z}); 92 | wrench_sensor.torque.x -= added_torque.getX(); 93 | wrench_sensor.torque.y -= added_torque.getY(); 94 | wrench_sensor.torque.z -= added_torque.getZ(); 95 | // Transform wrench_world to data_out frame_id if not empty otherwise to data_in frame id 96 | tf2::doTransform(wrench_sensor, data_out.wrench, transform_data_out_sensor_); 97 | 98 | return true; 99 | } 100 | 101 | } // namespace control_filters 102 | 103 | #include "pluginlib/class_list_macros.hpp" 104 | 105 | PLUGINLIB_EXPORT_CLASS( 106 | control_filters::GravityCompensation, 107 | filters::FilterBase) 108 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/gravity_compensation_filter_parameters.yaml: -------------------------------------------------------------------------------- 1 | gravity_compensation_filter: 2 | world_frame: { 3 | type: string, 4 | description: "Fixed world frame in which the constant field-induced force is given.", 5 | read_only: true, 6 | validation: { 7 | not_empty<>: [] 8 | }, 9 | } 10 | sensor_frame: { 11 | type: string, 12 | description: "Sensor frame in which center of gravity (CoG) is measured and computation occur.", 13 | read_only: true, 14 | validation: { 15 | not_empty<>: [] 16 | }, 17 | } 18 | tool: 19 | CoG: { 20 | type: double_array, 21 | description: "Position of the CoG of tool attached to the FT sensor in the sensor frame.", 22 | validation: { 23 | fixed_size<>: 3 24 | }, 25 | } 26 | gravity_field: { 27 | type: double_array, 28 | description: "Specifies the gravity field defined in fixed world frame in m/s^2.", 29 | default_value: [0, 0, -9.81], 30 | validation: { 31 | fixed_size<>: 3 32 | }, 33 | } 34 | mass: { 35 | type: double, 36 | description: "The mass of the tool in kg.", 37 | } 38 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/low_pass_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "control_filters/low_pass_filter.hpp" 16 | 17 | #include "pluginlib/class_list_macros.hpp" 18 | 19 | PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) 20 | 21 | PLUGINLIB_EXPORT_CLASS( 22 | control_filters::LowPassFilter>, filters::FilterBase>) 23 | 24 | PLUGINLIB_EXPORT_CLASS( 25 | control_filters::LowPassFilter, 26 | filters::FilterBase) 27 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/low_pass_filter_parameters.yaml: -------------------------------------------------------------------------------- 1 | low_pass_filter: 2 | sampling_frequency: { 3 | type: double, 4 | description: "Sampling frequency", 5 | validation: { 6 | gt<>: [0.0] 7 | }, 8 | } 9 | damping_frequency: { 10 | type: double, 11 | description: "Damping frequency", 12 | validation: { 13 | gt<>: [0.0] 14 | }, 15 | } 16 | damping_intensity: { 17 | type: double, 18 | description: "Damping intensity", 19 | } 20 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/rate_limiter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 AIT - Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "control_filters/rate_limiter.hpp" 16 | 17 | #include "pluginlib/class_list_macros.hpp" 18 | 19 | PLUGINLIB_EXPORT_CLASS(control_filters::RateLimiter, filters::FilterBase) 20 | -------------------------------------------------------------------------------- /control_toolbox/src/control_filters/rate_limiter_parameters.yaml: -------------------------------------------------------------------------------- 1 | rate_limiter: 2 | sampling_interval: { 3 | type: double, 4 | description: "Sampling interval in seconds", 5 | validation: { 6 | gt<>: [0.0], 7 | }, 8 | } 9 | max_value: { 10 | type: double, 11 | default_value: .NAN, 12 | description: "Maximum value, e.g. [m/s]", 13 | validation: { 14 | "control_filters::gt_eq_or_nan<>": [0.0] 15 | }, 16 | } 17 | min_value: { 18 | type: double, 19 | default_value: .NAN, 20 | description: "Minimum value, e.g. [m/s]", 21 | validation: { 22 | "control_filters::lt_eq_or_nan<>": [0.0] 23 | }, 24 | } 25 | max_first_derivative_pos: { 26 | type: double, 27 | default_value: .NAN, 28 | description: "Maximum value of the first derivative if **value** is positive, e.g. [m/s^2]", 29 | validation: { 30 | "control_filters::gt_eq_or_nan<>": [0.0] 31 | }, 32 | } 33 | min_first_derivative_pos: { 34 | type: double, 35 | default_value: .NAN, 36 | description: "Minimum value of the first derivative if **value** is positive, e.g. [m/s^2]", 37 | validation: { 38 | "control_filters::lt_eq_or_nan<>": [0.0] 39 | }, 40 | } 41 | max_first_derivative_neg: { 42 | type: double, 43 | default_value: .NAN, 44 | description: "Maximum value of the first derivative if **value** is negative, e.g. [m/s^2]", 45 | validation: { 46 | "control_filters::gt_eq_or_nan<>": [0.0] 47 | }, 48 | } 49 | min_first_derivative_neg: { 50 | type: double, 51 | default_value: .NAN, 52 | description: "Minimum value of the first derivative if **value** is negative, e.g. [m/s^2]", 53 | validation: { 54 | "control_filters::lt_eq_or_nan<>": [0.0] 55 | }, 56 | } 57 | max_second_derivative: { 58 | type: double, 59 | default_value: .NAN, 60 | description: "Maximum value of the second derivative, e.g. [m/s^3]", 61 | validation: { 62 | "control_filters::gt_eq_or_nan<>": [0.0] 63 | }, 64 | } 65 | min_second_derivative: { 66 | type: double, 67 | default_value: .NAN, 68 | description: "Minimum value of the second derivative, e.g. [m/s^3]", 69 | validation: { 70 | "control_filters::lt_eq_or_nan<>": [0.0] 71 | }, 72 | } 73 | -------------------------------------------------------------------------------- /control_toolbox/src/dither.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | // Original version: Kevin Watts 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "control_toolbox/dither.hpp" 40 | 41 | namespace control_toolbox 42 | { 43 | Dither::Dither() : amplitude_(0), has_saved_value_(false) {} 44 | 45 | double Dither::update() 46 | { 47 | if (has_saved_value_) 48 | { 49 | has_saved_value_ = false; 50 | return saved_value_; 51 | } 52 | 53 | // Generates gaussian random noise using the polar method. 54 | double v1, v2, r; 55 | // uniform distribution on the interval [-1.0, 1.0] 56 | std::uniform_real_distribution distribution( 57 | -1.0, std::nextafter(1.0, std::numeric_limits::max())); 58 | for (int i = 0; i < 100; ++i) 59 | { 60 | v1 = distribution(generator_); 61 | v2 = distribution(generator_); 62 | r = v1 * v1 + v2 * v2; 63 | if (r <= 1.0) 64 | { 65 | break; 66 | } 67 | } 68 | r = std::min(r, 1.0); 69 | 70 | double f = sqrt(-2.0 * log(r) / r); 71 | double current = amplitude_ * f * v1; 72 | saved_value_ = amplitude_ * f * v2; 73 | has_saved_value_ = true; 74 | 75 | return current; 76 | } 77 | 78 | } // namespace control_toolbox 79 | -------------------------------------------------------------------------------- /control_toolbox/src/pid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | /* 34 | Author: Melonee Wise 35 | Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen 36 | Desc: Implements a standard proportional-integral-derivative controller 37 | */ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "control_toolbox/pid.hpp" 47 | 48 | namespace control_toolbox 49 | { 50 | Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup) 51 | : gains_buffer_() 52 | { 53 | if (i_min > i_max) 54 | { 55 | throw std::invalid_argument("received i_min > i_max"); 56 | } 57 | set_gains(p, i, d, i_max, i_min, antiwindup); 58 | 59 | // Initialize saved i-term values 60 | clear_saved_iterm(); 61 | 62 | reset(); 63 | } 64 | 65 | Pid::Pid(const Pid & source) 66 | { 67 | // Copy the realtime buffer to the new PID class 68 | gains_buffer_ = source.gains_buffer_; 69 | 70 | // Initialize saved i-term values 71 | clear_saved_iterm(); 72 | 73 | // Reset the state of this PID controller 74 | reset(); 75 | } 76 | 77 | Pid::~Pid() {} 78 | 79 | void Pid::initialize(double p, double i, double d, double i_max, double i_min, bool antiwindup) 80 | { 81 | set_gains(p, i, d, i_max, i_min, antiwindup); 82 | 83 | reset(); 84 | } 85 | 86 | void Pid::reset() { reset(false); } 87 | 88 | void Pid::reset(bool save_i_term) 89 | { 90 | p_error_last_ = 0.0; 91 | p_error_ = 0.0; 92 | d_error_ = 0.0; 93 | cmd_ = 0.0; 94 | 95 | // Check to see if we should reset integral error here 96 | if (!save_i_term) 97 | { 98 | clear_saved_iterm(); 99 | } 100 | } 101 | 102 | void Pid::clear_saved_iterm() { i_term_ = 0.0; } 103 | 104 | void Pid::get_gains(double & p, double & i, double & d, double & i_max, double & i_min) 105 | { 106 | bool antiwindup; 107 | get_gains(p, i, d, i_max, i_min, antiwindup); 108 | } 109 | 110 | void Pid::get_gains( 111 | double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) 112 | { 113 | Gains gains = *gains_buffer_.readFromRT(); 114 | 115 | p = gains.p_gain_; 116 | i = gains.i_gain_; 117 | d = gains.d_gain_; 118 | i_max = gains.i_max_; 119 | i_min = gains.i_min_; 120 | antiwindup = gains.antiwindup_; 121 | } 122 | 123 | Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); } 124 | 125 | void Pid::set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup) 126 | { 127 | Gains gains(p, i, d, i_max, i_min, antiwindup); 128 | 129 | set_gains(gains); 130 | } 131 | 132 | void Pid::set_gains(const Gains & gains) 133 | { 134 | if (gains.i_min_ > gains.i_max_) 135 | { 136 | std::cout << "received i_min > i_max, skip new gains\n"; 137 | } 138 | else 139 | { 140 | gains_buffer_.writeFromNonRT(gains); 141 | } 142 | } 143 | 144 | double Pid::compute_command(double error, const double & dt_s) 145 | { 146 | if (std::abs(dt_s) <= std::numeric_limits::epsilon()) 147 | { 148 | // don't update anything 149 | return cmd_; 150 | } 151 | else if (dt_s < 0.0) 152 | { 153 | throw std::invalid_argument("Pid is called with negative dt"); 154 | } 155 | 156 | // don't reset controller but return NaN 157 | if (!std::isfinite(error)) 158 | { 159 | std::cout << "Received a non-finite error value\n"; 160 | return cmd_ = std::numeric_limits::quiet_NaN(); 161 | } 162 | 163 | // Calculate the derivative error 164 | d_error_ = (error - p_error_last_) / dt_s; 165 | p_error_last_ = error; 166 | 167 | return compute_command(error, d_error_, dt_s); 168 | } 169 | 170 | double Pid::compute_command(double error, const rcl_duration_value_t & dt_ns) 171 | { 172 | return compute_command(error, static_cast(dt_ns) / 1.e9); 173 | } 174 | 175 | double Pid::compute_command(double error, const rclcpp::Duration & dt) 176 | { 177 | return compute_command(error, dt.seconds()); 178 | } 179 | 180 | double Pid::compute_command(double error, const std::chrono::nanoseconds & dt_ns) 181 | { 182 | return compute_command(error, static_cast(dt_ns.count()) / 1.e9); 183 | } 184 | 185 | double Pid::compute_command(double error, double error_dot, const rcl_duration_value_t & dt_ns) 186 | { 187 | return compute_command(error, error_dot, static_cast(dt_ns) / 1.e9); 188 | } 189 | 190 | double Pid::compute_command(double error, double error_dot, const rclcpp::Duration & dt) 191 | { 192 | return compute_command(error, error_dot, dt.seconds()); 193 | } 194 | 195 | double Pid::compute_command(double error, double error_dot, const std::chrono::nanoseconds & dt_ns) 196 | { 197 | return compute_command(error, error_dot, static_cast(dt_ns.count()) / 1.e9); 198 | } 199 | 200 | double Pid::compute_command(double error, double error_dot, const double & dt_s) 201 | { 202 | if (std::abs(dt_s) <= std::numeric_limits::epsilon()) 203 | { 204 | // don't update anything 205 | return cmd_; 206 | } 207 | else if (dt_s < 0.0) 208 | { 209 | throw std::invalid_argument("Pid is called with negative dt"); 210 | } 211 | // Get the gain parameters from the realtime buffer 212 | Gains gains = *gains_buffer_.readFromRT(); 213 | 214 | double p_term, d_term; 215 | p_error_ = error; // this is error = target - state 216 | d_error_ = error_dot; 217 | 218 | // don't reset controller but return NaN 219 | if (!std::isfinite(error) || !std::isfinite(error_dot)) 220 | { 221 | std::cout << "Received a non-finite error/error_dot value\n"; 222 | return cmd_ = std::numeric_limits::quiet_NaN(); 223 | } 224 | 225 | // Calculate proportional contribution to command 226 | p_term = gains.p_gain_ * p_error_; 227 | 228 | // Calculate integral contribution to command 229 | if (gains.antiwindup_) 230 | { 231 | // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_ 232 | i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_); 233 | } 234 | else 235 | { 236 | i_term_ += gains.i_gain_ * dt_s * p_error_; 237 | } 238 | 239 | // Calculate derivative contribution to command 240 | d_term = gains.d_gain_ * d_error_; 241 | 242 | // Compute the command 243 | // Limit i_term so that the limit is meaningful in the output 244 | cmd_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term; 245 | 246 | return cmd_; 247 | } 248 | 249 | void Pid::set_current_cmd(double cmd) { cmd_ = cmd; } 250 | 251 | double Pid::get_current_cmd() { return cmd_; } 252 | 253 | void Pid::get_current_pid_errors(double & pe, double & ie, double & de) 254 | { 255 | pe = p_error_; 256 | ie = i_term_; 257 | de = d_error_; 258 | } 259 | 260 | } // namespace control_toolbox 261 | -------------------------------------------------------------------------------- /control_toolbox/src/sine_sweep.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2008, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | // Original version: Melonee Wise 34 | 35 | #include 36 | 37 | #include "control_toolbox/sine_sweep.hpp" 38 | 39 | namespace control_toolbox 40 | { 41 | SineSweep::SineSweep() 42 | : amplitude_(0.0), 43 | duration_(rclcpp::Duration(0, 0)), 44 | start_angular_freq_(0.0), 45 | end_angular_freq_(0.0), 46 | K_(0.0), 47 | L_(0.0), 48 | cmd_(0.0) 49 | { 50 | } 51 | 52 | bool SineSweep::init(double start_freq, double end_freq, double duration, double amplitude) 53 | { 54 | if (start_freq > end_freq) 55 | { 56 | return false; 57 | } 58 | if (duration < 0 || amplitude < 0) 59 | { 60 | return false; 61 | } 62 | 63 | amplitude_ = amplitude; 64 | duration_ = rclcpp::Duration::from_seconds(duration); 65 | // calculate the angular frequencies 66 | start_angular_freq_ = 2 * M_PI * start_freq; 67 | end_angular_freq_ = 2 * M_PI * end_freq; 68 | 69 | // calculate the constants 70 | K_ = (start_angular_freq_ * duration) / log(end_angular_freq_ / start_angular_freq_); 71 | L_ = (duration) / log(end_angular_freq_ / start_angular_freq_); 72 | 73 | // zero out the command 74 | cmd_ = 0.0; 75 | 76 | return true; 77 | } 78 | 79 | double SineSweep::update(rclcpp::Duration dt) 80 | { 81 | if (dt <= duration_) 82 | { 83 | cmd_ = amplitude_ * sin(K_ * (exp((dt.seconds()) / (L_)) - 1)); 84 | } 85 | else 86 | { 87 | cmd_ = 0.0; 88 | } 89 | 90 | return cmd_; 91 | } 92 | } // namespace control_toolbox 93 | -------------------------------------------------------------------------------- /control_toolbox/src/sinusoid.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // All rights reserved. 3 | // 4 | // Software License Agreement (BSD License 2.0) 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions 8 | // are met: 9 | // 10 | // * Redistributions of source code must retain the above copyright 11 | // notice, this list of conditions and the following disclaimer. 12 | // * Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // * Neither the name of the Willow Garage nor the names of its 17 | // contributors may be used to endorse or promote products derived 18 | // from this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | /** \author Mrinal Kalakrishnan */ 34 | 35 | #include 36 | #include 37 | 38 | #include "control_toolbox/sinusoid.hpp" 39 | 40 | namespace control_toolbox 41 | { 42 | Sinusoid::Sinusoid(double offset, double amplitude, double frequency, double phase) 43 | : offset_(offset), amplitude_(amplitude), frequency_(frequency), phase_(phase) 44 | { 45 | } 46 | 47 | Sinusoid::~Sinusoid() {} 48 | 49 | Sinusoid::Sinusoid() {} 50 | 51 | double Sinusoid::update(double time, double & qd, double & qdd) 52 | { 53 | double angular_frequency = 2.0 * M_PI * frequency_; 54 | double p = phase_ + angular_frequency * time; 55 | double sin_p = sin(p); 56 | double cos_p = cos(p); 57 | double q = offset_ + amplitude_ * sin_p; 58 | qd = angular_frequency * amplitude_ * cos_p; 59 | qdd = -angular_frequency * angular_frequency * amplitude_ * sin_p; 60 | return q; 61 | } 62 | 63 | void Sinusoid::debug() 64 | { 65 | std::cout << "offset=" << offset_ << " amplitude=" << amplitude_ << " phase=" << phase_ 66 | << " frequency=" << frequency_ << std::endl; 67 | } 68 | 69 | } // namespace control_toolbox 70 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_exponential_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, AIT Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "test_filter_util.hpp" 16 | 17 | #include 18 | #include "gmock/gmock.h" 19 | 20 | #include "control_filters/exponential_filter.hpp" 21 | 22 | TEST_F(FilterTest, TestExponentialFilterThrowsUnconfigured) 23 | { 24 | std::shared_ptr> filter_ = 25 | std::make_shared>(); 26 | double in, out; 27 | ASSERT_THROW(filter_->update(in, out), std::runtime_error); 28 | } 29 | 30 | TEST_F(FilterTest, TestExponentialFilterInvalidParameterValue) 31 | { 32 | std::shared_ptr> filter_ = 33 | std::make_shared>(); 34 | ASSERT_FALSE(filter_->configure( 35 | "", "TestExponentialFilter", node_->get_node_logging_interface(), 36 | node_->get_node_parameters_interface())); 37 | } 38 | 39 | TEST_F(FilterTest, TestExponentialFilterComputation) 40 | { 41 | // parameters should match the test yaml file 42 | double alpha = 0.7; 43 | 44 | double in = 1.0, calculated, out; 45 | 46 | std::shared_ptr> filter_ = 47 | std::make_shared>(); 48 | 49 | // configure 50 | ASSERT_TRUE(filter_->configure( 51 | "", "TestExponentialFilter", node_->get_node_logging_interface(), 52 | node_->get_node_parameters_interface())); 53 | 54 | // first filter pass, output should be input value as no old value was stored 55 | ASSERT_TRUE(filter_->update(in, out)); 56 | ASSERT_EQ(out, 1.0); 57 | 58 | // second filter pass with same values: no change 59 | // check equality with low-pass-filter 60 | ASSERT_TRUE(filter_->update(in, out)); 61 | calculated = in; 62 | ASSERT_EQ(calculated, out); 63 | 64 | // input change 65 | in = 0.0; 66 | for (int i = 0; i < 100; ++i) 67 | { 68 | ASSERT_TRUE(filter_->update(in, out)); 69 | calculated = alpha * in + (1 - alpha) * calculated; 70 | ASSERT_EQ(calculated, out); 71 | } 72 | } 73 | 74 | int main(int argc, char ** argv) 75 | { 76 | ::testing::InitGoogleMock(&argc, argv); 77 | rclcpp::init(argc, argv); 78 | int result = RUN_ALL_TESTS(); 79 | rclcpp::shutdown(); 80 | return result; 81 | } 82 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_exponential_filter_parameters.yaml: -------------------------------------------------------------------------------- 1 | TestExponentialFilterComputation: 2 | ros__parameters: 3 | alpha: 0.7 4 | 5 | TestExponentialInvalidParameterValue: 6 | ros__parameters: 7 | alpha: "a" 8 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_filter_util.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_ 16 | #define CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_ 17 | 18 | #include 19 | #include "gmock/gmock.h" 20 | 21 | #include "rclcpp/logger.hpp" 22 | #include "rclcpp/node.hpp" 23 | 24 | class FilterTest : public ::testing::Test 25 | { 26 | public: 27 | void SetUp() override 28 | { 29 | auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); 30 | node_ = std::make_shared(testname); 31 | } 32 | 33 | FilterTest() {} 34 | 35 | void TearDown() override { node_.reset(); } 36 | 37 | protected: 38 | rclcpp::Node::SharedPtr node_; 39 | }; 40 | 41 | #endif // CONTROL_FILTERS__TEST_FILTER_UTIL_HPP_ 42 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_gravity_compensation.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "test_gravity_compensation.hpp" 16 | 17 | #include 18 | 19 | TEST_F(GravityCompensationTest, TestGravityCompensationFilterThrowsUnconfigured) 20 | { 21 | std::shared_ptr> filter_ = 22 | std::make_shared>(); 23 | geometry_msgs::msg::WrenchStamped in, out; 24 | ASSERT_THROW(filter_->update(in, out), std::runtime_error); 25 | } 26 | 27 | TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) 28 | { 29 | std::shared_ptr> filter_ = 30 | std::make_shared>(); 31 | 32 | // one mandatory param missing, should fail 33 | ASSERT_THROW( 34 | filter_->configure( 35 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 36 | node_->get_node_parameters_interface()), 37 | std::exception); 38 | /* NOTE: one cannot declare or set the missing param afterwards, to then test if configure works, 39 | * because the param is read only and cannot be set anymore. 40 | */ 41 | } 42 | 43 | TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter) 44 | { 45 | std::shared_ptr> filter_ = 46 | std::make_shared>(); 47 | 48 | // wrong vector size, should fail 49 | ASSERT_THROW( 50 | filter_->configure( 51 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 52 | node_->get_node_parameters_interface()), 53 | std::exception); 54 | 55 | // fixed wrong vector size 56 | node_->set_parameter(rclcpp::Parameter("tool.CoG", std::vector({0.0, 0.0, 0.0}))); 57 | // all parameters correctly set AND second call to yet unconfigured filter 58 | ASSERT_TRUE(filter_->configure( 59 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 60 | node_->get_node_parameters_interface())); 61 | 62 | // change a parameter 63 | node_->set_parameter(rclcpp::Parameter("tool.CoG", std::vector({0.0, 0.0, 0.2}))); 64 | // accept second call to configure with valid parameters to already configured filter 65 | ASSERT_TRUE(filter_->configure( 66 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 67 | node_->get_node_parameters_interface())); 68 | } 69 | 70 | TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform) 71 | { 72 | std::shared_ptr> filter_ = 73 | std::make_shared>(); 74 | 75 | // all parameters correctly set 76 | ASSERT_TRUE(filter_->configure( 77 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 78 | node_->get_node_parameters_interface())); 79 | 80 | geometry_msgs::msg::WrenchStamped in, out; 81 | in.header.frame_id = "world"; 82 | in.wrench.force.x = 1.0; 83 | in.wrench.torque.x = 10.0; 84 | 85 | // should fail due to missing datain frame to sensor frame transform 86 | ASSERT_FALSE(filter_->update(in, out)); 87 | } 88 | 89 | TEST_F(GravityCompensationTest, TestGravityCompensationComputation) 90 | { 91 | std::shared_ptr> filter_ = 92 | std::make_shared>(); 93 | 94 | double gravity_acc = 9.81; 95 | double mass = 5.0; 96 | 97 | ASSERT_TRUE(filter_->configure( 98 | "", "TestGravityCompensation", node_->get_node_logging_interface(), 99 | node_->get_node_parameters_interface())); 100 | 101 | geometry_msgs::msg::WrenchStamped in, out; 102 | in.header.frame_id = "world"; 103 | in.wrench.force.x = 1.0; 104 | in.wrench.torque.x = 10.0; 105 | 106 | // should pass (transform is identity) 107 | ASSERT_TRUE(filter_->update(in, out)); 108 | 109 | ASSERT_EQ(out.wrench.force.x, 1.0); 110 | ASSERT_EQ(out.wrench.force.y, 0.0); 111 | ASSERT_NEAR(out.wrench.force.z, gravity_acc * mass, 0.0001); 112 | 113 | ASSERT_EQ(out.wrench.torque.x, 10.0); 114 | ASSERT_EQ(out.wrench.torque.y, 0.0); 115 | ASSERT_EQ(out.wrench.torque.z, 0.0); 116 | 117 | out.header.frame_id = "base"; 118 | // should fail due to missing transform for desired output frame 119 | ASSERT_FALSE(filter_->update(in, out)); 120 | } 121 | 122 | int main(int argc, char ** argv) 123 | { 124 | ::testing::InitGoogleTest(&argc, argv); 125 | rclcpp::init(argc, argv); 126 | int result = RUN_ALL_TESTS(); 127 | rclcpp::shutdown(); 128 | return result; 129 | } 130 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_gravity_compensation.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ 16 | #define CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ 17 | 18 | #include 19 | #include 20 | #include "gmock/gmock.h" 21 | 22 | #include "control_filters/gravity_compensation.hpp" 23 | #include "geometry_msgs/msg/wrench_stamped.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | 26 | namespace 27 | { 28 | static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); 29 | } // namespace 30 | 31 | class GravityCompensationTest : public ::testing::Test 32 | { 33 | public: 34 | void SetUp() override 35 | { 36 | auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); 37 | node_ = std::make_shared(testname); 38 | executor_->add_node(node_); 39 | executor_thread_ = std::thread([this]() { executor_->spin(); }); 40 | } 41 | 42 | GravityCompensationTest() 43 | { 44 | executor_ = std::make_shared(); 45 | } 46 | 47 | void TearDown() override 48 | { 49 | executor_->cancel(); 50 | if (executor_thread_.joinable()) 51 | { 52 | executor_thread_.join(); 53 | } 54 | node_.reset(); 55 | } 56 | 57 | protected: 58 | rclcpp::Node::SharedPtr node_; 59 | rclcpp::Executor::SharedPtr executor_; 60 | std::thread executor_thread_; 61 | }; 62 | 63 | #endif // CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ 64 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_gravity_compensation_parameters.yaml: -------------------------------------------------------------------------------- 1 | TestGravityCompensationAllParameters: 2 | ros__parameters: 3 | world_frame: world 4 | sensor_frame: sensor 5 | tool: 6 | gravity_field: [0.0, 0.0, -9.81] 7 | mass: 5.0 8 | CoG: [0.0, 0.0, 0.0] 9 | 10 | TestGravityCompensationMissingParameters: 11 | ros__parameters: 12 | world_frame: world 13 | 14 | TestGravityCompensationInvalidThenFixedParameter: 15 | ros__parameters: 16 | world_frame: world 17 | sensor_frame: sensor 18 | tool: 19 | gravity_field: [0.0, 0.0, -9.81] 20 | mass: 5.0 21 | CoG: [0.0, 0.0] 22 | 23 | TestGravityCompensationMissingTransform: 24 | ros__parameters: 25 | world_frame: world 26 | sensor_frame: sensor 27 | tool: 28 | gravity_field: [0.0, 0.0, -9.81] 29 | mass: 5.0 30 | CoG: [0.0, 0.0, 0.0] 31 | 32 | TestGravityCompensationComputation: 33 | ros__parameters: 34 | world_frame: world 35 | sensor_frame: world 36 | tool: 37 | gravity_field: [0.0, 0.0, -9.81] 38 | mass: 5.0 39 | CoG: [0.0, 0.0, 0.0] 40 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_load_exponential_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, AIT Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include 16 | 17 | #include "gmock/gmock.h" 18 | #include "pluginlib/class_loader.hpp" 19 | #include "rclcpp/utilities.hpp" 20 | 21 | #include "control_filters/exponential_filter.hpp" 22 | 23 | TEST(TestLoadExponentialFilter, load_exponential_filter_double) 24 | { 25 | rclcpp::init(0, nullptr); 26 | 27 | pluginlib::ClassLoader> filter_loader( 28 | "filters", "filters::FilterBase"); 29 | std::shared_ptr> filter; 30 | auto available_classes = filter_loader.getDeclaredClasses(); 31 | std::stringstream sstr; 32 | sstr << "available filters:" << std::endl; 33 | for (const auto & available_class : available_classes) 34 | { 35 | sstr << " " << available_class << std::endl; 36 | } 37 | 38 | std::string filter_type = "control_filters/ExponentialFilterDouble"; 39 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 40 | EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)) << sstr.str(); 41 | 42 | rclcpp::shutdown(); 43 | } 44 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_load_gravity_compensation.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "control_filters/gravity_compensation.hpp" 20 | #include "geometry_msgs/msg/wrench_stamped.hpp" 21 | #include "pluginlib/class_loader.hpp" 22 | #include "rclcpp/utilities.hpp" 23 | 24 | TEST(TestLoadGravityCompensationFilter, load_gravity_compensation_filter_wrench) 25 | { 26 | rclcpp::init(0, nullptr); 27 | 28 | pluginlib::ClassLoader> filter_loader( 29 | "filters", "filters::FilterBase"); 30 | std::shared_ptr> filter; 31 | auto available_classes = filter_loader.getDeclaredClasses(); 32 | std::stringstream sstr; 33 | sstr << "available filters:" << std::endl; 34 | for (const auto & available_class : available_classes) 35 | { 36 | sstr << " " << available_class << std::endl; 37 | } 38 | 39 | std::string filter_type = "control_filters/GravityCompensationWrench"; 40 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 41 | ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); 42 | 43 | rclcpp::shutdown(); 44 | } 45 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_load_low_pass_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include 16 | 17 | #include "geometry_msgs/msg/wrench_stamped.hpp" 18 | #include "gmock/gmock.h" 19 | #include "pluginlib/class_loader.hpp" 20 | #include "rclcpp/utilities.hpp" 21 | 22 | #include "control_filters/low_pass_filter.hpp" 23 | 24 | TEST(TestLoadLowPassFilter, load_low_pass_filter_double) 25 | { 26 | rclcpp::init(0, nullptr); 27 | 28 | pluginlib::ClassLoader> filter_loader( 29 | "filters", "filters::FilterBase"); 30 | std::shared_ptr> filter; 31 | auto available_classes = filter_loader.getDeclaredClasses(); 32 | std::stringstream sstr; 33 | sstr << "available filters:" << std::endl; 34 | for (const auto & available_class : available_classes) 35 | { 36 | sstr << " " << available_class << std::endl; 37 | } 38 | 39 | std::string filter_type = "control_filters/LowPassFilterDouble"; 40 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 41 | EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); 42 | 43 | rclcpp::shutdown(); 44 | } 45 | 46 | TEST(TestLoadLowPassFilter, load_low_pass_filter_vector_double) 47 | { 48 | rclcpp::init(0, nullptr); 49 | 50 | pluginlib::ClassLoader>> filter_loader( 51 | "filters", "filters::FilterBase>"); 52 | std::shared_ptr>> filter; 53 | auto available_classes = filter_loader.getDeclaredClasses(); 54 | std::stringstream sstr; 55 | sstr << "available filters:" << std::endl; 56 | for (const auto & available_class : available_classes) 57 | { 58 | sstr << " " << available_class << std::endl; 59 | } 60 | 61 | std::string filter_type = "control_filters/LowPassFilterVectorDouble"; 62 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 63 | EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); 64 | 65 | rclcpp::shutdown(); 66 | } 67 | 68 | TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) 69 | { 70 | rclcpp::init(0, nullptr); 71 | 72 | pluginlib::ClassLoader> filter_loader( 73 | "filters", "filters::FilterBase"); 74 | std::shared_ptr> filter; 75 | auto available_classes = filter_loader.getDeclaredClasses(); 76 | std::stringstream sstr; 77 | sstr << "available filters:" << std::endl; 78 | for (const auto & available_class : available_classes) 79 | { 80 | sstr << " " << available_class << std::endl; 81 | } 82 | 83 | std::string filter_type = "control_filters/LowPassFilterWrench"; 84 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 85 | EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); 86 | 87 | rclcpp::shutdown(); 88 | } 89 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_load_rate_limiter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 AIT - Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #include 15 | #include 16 | 17 | #include "gmock/gmock.h" 18 | #include "pluginlib/class_loader.hpp" 19 | #include "rclcpp/utilities.hpp" 20 | 21 | #include "control_filters/rate_limiter.hpp" 22 | 23 | TEST(TestLoadRateLimtier, load_rate_limiter_double) 24 | { 25 | rclcpp::init(0, nullptr); 26 | 27 | pluginlib::ClassLoader> filter_loader( 28 | "filters", "filters::FilterBase"); 29 | std::shared_ptr> filter; 30 | auto available_classes = filter_loader.getDeclaredClasses(); 31 | std::stringstream sstr; 32 | sstr << "available filters:" << std::endl; 33 | for (const auto & available_class : available_classes) 34 | { 35 | sstr << " " << available_class << std::endl; 36 | } 37 | 38 | std::string filter_type = "control_filters/RateLimiterDouble"; 39 | ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); 40 | EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); 41 | 42 | rclcpp::shutdown(); 43 | } 44 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_low_pass_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "test_filter_util.hpp" 16 | 17 | #include 18 | #include "gmock/gmock.h" 19 | 20 | #include "geometry_msgs/msg/wrench_stamped.hpp" 21 | 22 | #include "control_filters/low_pass_filter.hpp" 23 | 24 | TEST_F(FilterTest, TestLowPassWrenchFilterAllParameters) 25 | { 26 | std::shared_ptr> filter_ = 27 | std::make_shared>(); 28 | 29 | // should allow configuration and find parameters in sensor_filter_chain param namespace 30 | ASSERT_TRUE(filter_->configure( 31 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 32 | node_->get_node_parameters_interface())); 33 | 34 | // change a parameter 35 | node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0)); 36 | // accept second call to configure with valid parameters to already configured filter 37 | // will give a warning "Filter %s already being reconfigured" 38 | ASSERT_TRUE(filter_->configure( 39 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 40 | node_->get_node_parameters_interface())); 41 | } 42 | 43 | TEST_F(FilterTest, TestLowPassWrenchFilterMissingParameter) 44 | { 45 | std::shared_ptr> filter_ = 46 | std::make_shared>(); 47 | 48 | // should deny configuration as sampling frequency is missing 49 | ASSERT_FALSE(filter_->configure( 50 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 51 | node_->get_node_parameters_interface())); 52 | } 53 | 54 | TEST_F(FilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) 55 | { 56 | std::shared_ptr> filter_ = 57 | std::make_shared>(); 58 | 59 | // should deny configuration as sampling frequency is invalid 60 | ASSERT_FALSE(filter_->configure( 61 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 62 | node_->get_node_parameters_interface())); 63 | 64 | // fix the param 65 | node_->set_parameter(rclcpp::Parameter("sampling_frequency", 1000.0)); 66 | // should allow configuration and pass second call to unconfigured filter 67 | ASSERT_TRUE(filter_->configure( 68 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 69 | node_->get_node_parameters_interface())); 70 | } 71 | 72 | TEST_F(FilterTest, TestLowPassFilterThrowsUnconfigured) 73 | { 74 | std::shared_ptr> filter_ = 75 | std::make_shared>(); 76 | double in, out; 77 | ASSERT_THROW(filter_->update(in, out), std::runtime_error); 78 | } 79 | 80 | TEST_F(FilterTest, TestLowPassWrenchFilterThrowsUnconfigured) 81 | { 82 | std::shared_ptr> filter_ = 83 | std::make_shared>(); 84 | geometry_msgs::msg::WrenchStamped in, out; 85 | ASSERT_THROW(filter_->update(in, out), std::runtime_error); 86 | } 87 | 88 | TEST_F(FilterTest, TestLowPassWrenchFilterComputation) 89 | { 90 | // parameters should match the test yaml file 91 | double sampling_freq = 1000.0; 92 | double damping_freq = 20.5; 93 | double damping_intensity = 1.25; 94 | 95 | double a1, b1; 96 | a1 = exp( 97 | -1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); 98 | b1 = 1.0 - a1; 99 | 100 | geometry_msgs::msg::WrenchStamped in, calculated, out; 101 | in.header.frame_id = "world"; 102 | in.wrench.force.x = 1.0; 103 | in.wrench.torque.x = 10.0; 104 | 105 | std::shared_ptr> filter_ = 106 | std::make_shared>(); 107 | 108 | // configure 109 | ASSERT_TRUE(filter_->configure( 110 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 111 | node_->get_node_parameters_interface())); 112 | 113 | // first filter pass, output should be equal to the input 114 | ASSERT_TRUE(filter_->update(in, out)); 115 | ASSERT_EQ(out.wrench.force.x, in.wrench.force.x); 116 | ASSERT_EQ(out.wrench.force.y, in.wrench.force.y); 117 | ASSERT_EQ(out.wrench.force.z, in.wrench.force.z); 118 | ASSERT_EQ(out.wrench.torque.x, in.wrench.torque.x); 119 | ASSERT_EQ(out.wrench.torque.y, in.wrench.torque.y); 120 | ASSERT_EQ(out.wrench.torque.z, in.wrench.torque.z); 121 | 122 | // second filter pass with a step in values (otherwise there is nothing to filter): 123 | in.wrench.force.x = 2.0; 124 | in.wrench.torque.x = 20.0; 125 | ASSERT_TRUE(filter_->update(in, out)); 126 | 127 | // update once again and check results 128 | // calculate wrench by hand 129 | calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * out.wrench.force.x; 130 | calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * out.wrench.force.y; 131 | calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * out.wrench.force.z; 132 | calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * out.wrench.torque.x; 133 | calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * out.wrench.torque.y; 134 | calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * out.wrench.torque.z; 135 | // check equality with low-pass-filter 136 | ASSERT_TRUE(filter_->update(in, out)); 137 | ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); 138 | ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); 139 | ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); 140 | ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); 141 | ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); 142 | ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); 143 | } 144 | 145 | TEST_F(FilterTest, TestLowPassVectorDoubleFilterComputation) 146 | { 147 | // parameters should match the test yaml file 148 | double sampling_freq = 1000.0; 149 | double damping_freq = 20.5; 150 | double damping_intensity = 1.25; 151 | 152 | double a1, b1; 153 | a1 = exp( 154 | -1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / (pow(10.0, damping_intensity / -10.0))); 155 | b1 = 1.0 - a1; 156 | 157 | std::vector in{1.0, 2.0, 3.0}; 158 | std::vector calculated, out; 159 | calculated.resize(in.size()); 160 | out.resize(in.size()); 161 | 162 | std::shared_ptr>> filter_ = 163 | std::make_shared>>(); 164 | 165 | node_->declare_parameter("sampling_frequency", rclcpp::ParameterValue(sampling_freq)); 166 | node_->declare_parameter("damping_frequency", rclcpp::ParameterValue(damping_freq)); 167 | node_->declare_parameter("damping_intensity", rclcpp::ParameterValue(damping_intensity)); 168 | ASSERT_TRUE(filter_->configure( 169 | "", "TestLowPassFilter", node_->get_node_logging_interface(), 170 | node_->get_node_parameters_interface())); 171 | 172 | ASSERT_TRUE(filter_->update(in, out)); 173 | ASSERT_TRUE(std::equal(in.begin(), in.end(), out.begin())); 174 | 175 | // second filter pass with a step in values (otherwise there is nothing to filter): 176 | in = {2.0, 4.0, 6.0}; 177 | ASSERT_TRUE(filter_->update(in, out)); 178 | 179 | // update once again and check results 180 | // calculate wrench by hand 181 | calculated[0] = b1 * in[0] + a1 * out[0]; 182 | calculated[1] = b1 * in[1] + a1 * out[1]; 183 | calculated[2] = b1 * in[2] + a1 * out[2]; 184 | // check equality with low-pass-filter 185 | ASSERT_TRUE(filter_->update(in, out)); 186 | ASSERT_TRUE(std::equal(out.begin(), out.end(), calculated.begin())); 187 | } 188 | 189 | int main(int argc, char ** argv) 190 | { 191 | ::testing::InitGoogleMock(&argc, argv); 192 | rclcpp::init(argc, argv); 193 | int result = RUN_ALL_TESTS(); 194 | rclcpp::shutdown(); 195 | return result; 196 | } 197 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_low_pass_filter_parameters.yaml: -------------------------------------------------------------------------------- 1 | TestLowPassWrenchFilterAllParameters: 2 | ros__parameters: 3 | sampling_frequency: 1000.0 4 | damping_frequency: 20.5 5 | damping_intensity: 1.25 6 | 7 | TestLowPassWrenchFilterMissingParameter: 8 | ros__parameters: 9 | damping_frequency: 20.5 10 | damping_intensity: 1.25 11 | 12 | TestLowPassWrenchFilterInvalidThenFixedParameter: 13 | ros__parameters: 14 | sampling_frequency: 0.0 15 | damping_frequency: 20.5 16 | damping_intensity: 1.25 17 | 18 | TestLowPassWrenchFilterComputation: 19 | ros__parameters: 20 | sampling_frequency: 1000.0 21 | damping_frequency: 20.5 22 | damping_intensity: 1.25 23 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_rate_limiter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 AIT - Austrian Institute of Technology GmbH 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "test_filter_util.hpp" 16 | 17 | #include 18 | #include 19 | #include "gmock/gmock.h" 20 | 21 | #include "control_filters/rate_limiter.hpp" 22 | 23 | TEST_F(FilterTest, TestRateLimiterAllParameters) 24 | { 25 | std::shared_ptr> filter_ = 26 | std::make_shared>(); 27 | 28 | // should allow configuration and find parameters in sensor_filter_chain param namespace 29 | ASSERT_TRUE(filter_->configure( 30 | "", "TestRateLimiter", node_->get_node_logging_interface(), 31 | node_->get_node_parameters_interface())); 32 | 33 | // change a parameter 34 | node_->set_parameter(rclcpp::Parameter("sampling_interval", 0.5)); 35 | // accept second call to configure with valid parameters to already configured filter 36 | // will give a warning "Filter %s already being reconfigured" 37 | ASSERT_TRUE(filter_->configure( 38 | "", "TestRateLimiter", node_->get_node_logging_interface(), 39 | node_->get_node_parameters_interface())); 40 | } 41 | 42 | TEST_F(FilterTest, TestRateLimiterMissingParameter) 43 | { 44 | std::shared_ptr> filter_ = 45 | std::make_shared>(); 46 | 47 | // should deny configuration as sampling_interval is missing 48 | ASSERT_FALSE(filter_->configure( 49 | "", "TestRateLimiter", node_->get_node_logging_interface(), 50 | node_->get_node_parameters_interface())); 51 | } 52 | 53 | TEST_F(FilterTest, TestRateLimiterInvalidThenFixedParameter) 54 | { 55 | std::shared_ptr> filter_ = 56 | std::make_shared>(); 57 | 58 | // should deny configuration as sampling_interval is invalid 59 | ASSERT_FALSE(filter_->configure( 60 | "", "TestRateLimiter", node_->get_node_logging_interface(), 61 | node_->get_node_parameters_interface())); 62 | 63 | // fix the param 64 | node_->set_parameter(rclcpp::Parameter("sampling_interval", 1.0)); 65 | // should allow configuration and pass second call to unconfigured filter 66 | ASSERT_TRUE(filter_->configure( 67 | "", "TestRateLimiter", node_->get_node_logging_interface(), 68 | node_->get_node_parameters_interface())); 69 | } 70 | 71 | TEST_F(FilterTest, TestRateLimiterThrowsUnconfigured) 72 | { 73 | std::shared_ptr> filter_ = 74 | std::make_shared>(); 75 | double in, out; 76 | ASSERT_THROW(filter_->update(in, out), std::runtime_error); 77 | } 78 | 79 | TEST_F(FilterTest, TestRateLimiterCompute) 80 | { 81 | std::shared_ptr> filter_ = 82 | std::make_shared>(); 83 | 84 | ASSERT_TRUE(filter_->configure( 85 | "", "TestRateLimiter", node_->get_node_logging_interface(), 86 | node_->get_node_parameters_interface())); 87 | 88 | double in = 10.0, out; 89 | for (int i = 0; i < 10; i++) 90 | { 91 | ASSERT_NO_THROW(filter_->update(in, out)); 92 | // no change 93 | EXPECT_THAT(out, ::testing::DoubleEq(in)); 94 | } 95 | in = 0.0; 96 | // takes 14 steps to reach 0 (first (10.0/1.0) plus second derivative limits) 97 | for (int i = 0; i < 14; i++) 98 | { 99 | ASSERT_NO_THROW(filter_->update(in, out)); 100 | EXPECT_THAT(out, ::testing::Not(::testing::DoubleNear(in, 1e-6))) << "i=" << i; 101 | } 102 | ASSERT_NO_THROW(filter_->update(in, out)); 103 | EXPECT_THAT(out, ::testing::DoubleNear(in, 1e-6)); 104 | } 105 | 106 | int main(int argc, char ** argv) 107 | { 108 | ::testing::InitGoogleMock(&argc, argv); 109 | rclcpp::init(argc, argv); 110 | int result = RUN_ALL_TESTS(); 111 | rclcpp::shutdown(); 112 | return result; 113 | } 114 | -------------------------------------------------------------------------------- /control_toolbox/test/control_filters/test_rate_limiter_parameters.yaml: -------------------------------------------------------------------------------- 1 | TestRateLimiterMissingParameter: 2 | ros__parameters: {} 3 | 4 | TestRateLimiterAllParameters: 5 | ros__parameters: 6 | sampling_interval: 1.0 7 | 8 | TestRateLimiterInvalidThenFixedParameter: 9 | ros__parameters: {} 10 | 11 | TestRateLimiterThrowsUnconfigured: 12 | ros__parameters: 13 | sampling_interval: 1.0 14 | 15 | TestRateLimiterCompute: 16 | ros__parameters: 17 | sampling_interval: 1.0 18 | max_value: .NAN 19 | min_value: .NAN 20 | max_first_derivative_pos: 1.0 21 | min_first_derivative_pos: -1.0 22 | max_first_derivative_neg: 1.0 23 | min_first_derivative_neg: -1.0 24 | max_second_derivative: 0.1 25 | min_second_derivative: -0.1 26 | -------------------------------------------------------------------------------- /control_toolbox/test/pid_ros_publisher_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 PAL Robotics SL 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "control_toolbox/pid_ros.hpp" 21 | 22 | #include "gmock/gmock.h" 23 | 24 | #include "rclcpp/duration.hpp" 25 | #include "rclcpp/executors.hpp" 26 | #include "rclcpp/node.hpp" 27 | #include "rclcpp/utilities.hpp" 28 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 29 | 30 | using PidStateMsg = control_msgs::msg::PidState; 31 | using rclcpp::executors::MultiThreadedExecutor; 32 | 33 | TEST(PidPublisherTest, PublishTest) 34 | { 35 | const size_t ATTEMPTS = 100; 36 | const std::chrono::milliseconds DELAY(250); 37 | 38 | auto node = std::make_shared("pid_publisher_test"); 39 | 40 | control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node); 41 | 42 | pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); 43 | 44 | bool callback_called = false; 45 | control_msgs::msg::PidState::SharedPtr last_state_msg; 46 | auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) 47 | { callback_called = true; }; 48 | 49 | auto state_sub = node->create_subscription( 50 | "/pid_state", rclcpp::SensorDataQoS(), state_callback); 51 | 52 | double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); 53 | EXPECT_EQ(-1.5, command); 54 | 55 | // wait for callback 56 | for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) 57 | { 58 | pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); 59 | rclcpp::spin_some(node); 60 | std::this_thread::sleep_for(DELAY); 61 | } 62 | 63 | ASSERT_TRUE(callback_called); 64 | } 65 | 66 | TEST(PidPublisherTest, PublishTestLifecycle) 67 | { 68 | const size_t ATTEMPTS = 100; 69 | const std::chrono::milliseconds DELAY(250); 70 | 71 | auto node = std::make_shared("pid_publisher_test"); 72 | 73 | control_toolbox::PidROS pid_ros(node); 74 | 75 | auto state_pub_lifecycle_ = 76 | std::dynamic_pointer_cast>( 77 | pid_ros.get_pid_state_publisher()); 78 | 79 | pid_ros.initialize_from_args(1.0, 1.0, 1.0, 5.0, -5.0, false, false); 80 | 81 | bool callback_called = false; 82 | control_msgs::msg::PidState::SharedPtr last_state_msg; 83 | auto state_callback = [&](const control_msgs::msg::PidState::SharedPtr) 84 | { callback_called = true; }; 85 | 86 | auto state_sub = node->create_subscription( 87 | "/pid_state", rclcpp::SensorDataQoS(), state_callback); 88 | 89 | double command = pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); 90 | EXPECT_EQ(-1.5, command); 91 | 92 | // wait for callback 93 | for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) 94 | { 95 | pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); 96 | rclcpp::spin_some(node->get_node_base_interface()); 97 | std::this_thread::sleep_for(DELAY); 98 | } 99 | ASSERT_TRUE(callback_called); 100 | 101 | node->shutdown(); // won't be called in destructor 102 | } 103 | 104 | int main(int argc, char ** argv) 105 | { 106 | ::testing::InitGoogleMock(&argc, argv); 107 | rclcpp::init(argc, argv); 108 | int result = RUN_ALL_TESTS(); 109 | rclcpp::shutdown(); 110 | return result; 111 | } 112 | -------------------------------------------------------------------------------- /doc/control_filters.md: -------------------------------------------------------------------------------- 1 | # Control filters 2 | 3 | Implement filter plugins for control purposes as [ros/filters](https://github.com/ros/filters) 4 | 5 | ## Available filters 6 | 7 | * Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). 8 | * Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench). 9 | * Exponential Filter: Exponential filter for double data type. 10 | 11 | 12 | ## Gravity compensation filter 13 | 14 | This filter implements an algorithm compensating for the gravity forces acting at the center of gravity (CoG) of a known mass, computed at a `sensor_frame` and applied to a `data_in` wrench. 15 | 16 | The filter relies on tf2, and might fail if transforms are missing. 17 | 18 | Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given. 19 | 20 | ### GC: Required parameters 21 | 22 | * `world_frame` (ℛw): frame in which the `CoG.force` is represented. 23 | * `sensor_frame` (ℛs): frame in which the `CoG.pos` is defined 24 | * `CoG.pos` (ps): position of the CoG of the mass the filter should compensate for 25 | * `CoG.force` (gw): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame` 26 | 27 | ### GC: Algorithm 28 | 29 | Given 30 | 31 | * above-required parameters, ℛw, ℛs, ps, gw 32 | * `data_in`, a wrench ℱi = {fi, τi} represented in the `data_in` frame ℛi 33 | * access to tf2 homogeneous transforms: 34 | * Tsi from ℛi to ℛs 35 | * Tsw from ℛw to ℛs 36 | * Tos from ℛs to ℛo 37 | 38 | Compute `data_out` compensated wrench ℱco = {fco, τco} represented in the `data_out` frame ℛo if given, or the `data_in` frame ℛi otherwise, with equations: 39 | 40 | ℱco = Tos.ℱcs, 41 | 42 | 43 | with ℱcs = {fcs, τcs} the compensated wrench in `sensor_frame` (common frame for computation) 44 | 45 | and, 46 | 47 | fcs = fs - Tswgw 48 | 49 | its force and, 50 | 51 | τcs = τs - ps x (Tswgw) 52 | 53 | its torque, and 54 | 55 | ℱs = Tsi.ℱi = {fs, τs} 56 | 57 | the full transform of the input wrench ℱi to sensor frame ℛs 58 | 59 | Remarks : 60 | * a full vector is used for gravity force, to not impose gravity to be only along z of `world_frame`. 61 | * `data_in` frame is usually equal to `sensor_frame`, but could be different since measurement of wrench might occur in another frame. E.g.: measurements are at the **FT sensor flange** = `data_in` frame, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thickness of the sensor) to be accounted for. 62 | * `data_out` frame is usually `data_in` frame, but for convenience, can be set to any other useful frame. E.g.: wrench expressed in a `control_frame` like the center of a gripper. 63 | * Tsw will only rotate the gw vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛw to ℛs). 64 | 65 | 66 | ## Low Pass filter 67 | 68 | This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench). 69 | The feedforward and feedback coefficients of the IIR filter are computed from the low-pass filter parameters. 70 | 71 | ### LPF: Required parameters 72 | 73 | * sampling frequency as `sf` 74 | * damping frequency as `df` 75 | * damping intensity as `di` 76 | 77 | ### LPF: Algorithm 78 | 79 | Given 80 | 81 | * above-required parameters, `sf`, `df`, `di` 82 | * `data_in`, a double or wrench `x` 83 | 84 | Compute `data_out`, the filtered output `y(n)` with equation: 85 | 86 | y(n) = b x(n-1) + a y(n-1) 87 | 88 | with 89 | 90 | * a the feedbackward coefficient such that a = exp( -1/sf (2 pi df) / (10^(di/-10)) ) 91 | * b the feedforward coefficient such that b = 1 - a 92 | 93 | 94 | ## Exponential filter 95 | 96 | ### EF: Required parameters 97 | * `alpha`: the exponential decay factor 98 | 99 | ### EF: Algorithm 100 | 101 | smoothed_value = alpha * current_value + (1 - alpha) * last_smoothed_value; 102 | -------------------------------------------------------------------------------- /doc/control_toolbox.md: -------------------------------------------------------------------------------- 1 | # Base classes 2 | 3 | Tbd. 4 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | :github_url: https://github.com/ros-controls/control_toolbox/blob/{REPOS_FILE_BRANCH}/doc/index.rst 2 | 3 | control_toolbox 4 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 5 | This package contains several C++ classes and filter plugins useful in writing controllers. 6 | 7 | .. include:: control_toolbox.md 8 | :parser: myst_parser.sphinx_ 9 | 10 | .. include:: control_filters.md 11 | :parser: myst_parser.sphinx_ 12 | -------------------------------------------------------------------------------- /doc/migration.rst: -------------------------------------------------------------------------------- 1 | :github_url: https://github.com/ros-controls/realtime_tools/blob/{REPOS_FILE_BRANCH}/doc/migration.rst 2 | 3 | Migration Guides: Jazzy to Kilted 4 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 5 | This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary. 6 | -------------------------------------------------------------------------------- /doc/release_notes.rst: -------------------------------------------------------------------------------- 1 | :github_url: https://github.com/ros-controls/realtime_tools/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst 2 | 3 | Release Notes: Jazzy to Kilted 4 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 5 | This list summarizes the changes between Jazzy (previous) and Kilted (current) releases. 6 | -------------------------------------------------------------------------------- /ros_controls.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/kinematics_interface: 3 | type: git 4 | url: https://github.com/ros-controls/kinematics_interface.git 5 | version: humble 6 | ros-controls/ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/ros2_control.git 9 | version: humble 10 | ros-controls/ros2_controllers: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_controllers.git 13 | version: humble 14 | -------------------------------------------------------------------------------- /ros_controls.jazzy.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/kinematics_interface: 3 | type: git 4 | url: https://github.com/ros-controls/kinematics_interface.git 5 | version: jazzy 6 | ros-controls/ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/ros2_control.git 9 | version: jazzy 10 | ros-controls/ros2_controllers: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_controllers.git 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /ros_controls.kilted.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/kinematics_interface: 3 | type: git 4 | url: https://github.com/ros-controls/kinematics_interface.git 5 | version: master 6 | ros-controls/ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/ros2_control.git 9 | version: master 10 | ros-controls/ros2_controllers: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_controllers.git 13 | version: master 14 | -------------------------------------------------------------------------------- /ros_controls.rolling.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros-controls/kinematics_interface: 3 | type: git 4 | url: https://github.com/ros-controls/kinematics_interface.git 5 | version: master 6 | ros-controls/ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/ros2_control.git 9 | version: master 10 | ros-controls/ros2_controllers: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_controllers.git 13 | version: master 14 | --------------------------------------------------------------------------------