diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml index daacbb31..db51081a 100644 --- a/.github/workflows/backport.yaml +++ b/.github/workflows/backport.yaml @@ -30,5 +30,5 @@ jobs: uses: korthout/backport-action@v3 with: label_pattern: "" - target_branches: humble + target_branches: jazzy merge_commits: skip diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.11.yml b/.github/workflows/build-ubuntu-24.04-fri-1.11.yml index 3de0799f..a1c30aa1 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.11.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.11.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-1.11 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.14.yml b/.github/workflows/build-ubuntu-24.04-fri-1.14.yml index c85da043..09148606 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.14.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.14.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-1.14 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.15.yml b/.github/workflows/build-ubuntu-24.04-fri-1.15.yml index 5ec898dc..a2a70913 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.15.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.15.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-1.15 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.16.yml b/.github/workflows/build-ubuntu-24.04-fri-1.16.yml index 855eb38c..164e8dbf 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.16.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.16.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-1.16 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build-ubuntu-24.04-fri-2.5.yml b/.github/workflows/build-ubuntu-24.04-fri-2.5.yml index 8068003e..afb0f0fa 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-2.5.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-2.5.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-2.5 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build-ubuntu-24.04-fri-2.7.yml b/.github/workflows/build-ubuntu-24.04-fri-2.7.yml index 284fd789..d7fc4aeb 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-2.7.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-2.7.yml @@ -2,7 +2,7 @@ name: ubuntu-24.04-fri-2.7 on: pull_request: branches: - - rolling + - jazzy workflow_dispatch: schedule: - cron: "0 0 * * 0" diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cb223e18..ddffaa48 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -17,8 +17,8 @@ jobs: runs-on: ${{ inputs.os }} steps: - uses: ros-tooling/setup-ros@v0.7 - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@v0.4 with: package-name: lbr_fri_ros2_stack - target-ros2-distro: rolling - vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/rolling/lbr_fri_ros2_stack/repos-fri-${{ inputs.fri_version }}.yaml + target-ros2-distro: jazzy + vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/jazzy/lbr_fri_ros2_stack/repos-fri-${{ inputs.fri_version }}.yaml diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fb1bce54..d990dab5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,20 +1,78 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Jazzy v2.3.0 (2025-11-17) +Jazzy v2.4.3 (2025-12-09) +-------------------------- +* ``lbr_bringup``: Switched to declarative launch files and removed mixins from launch files: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/338 + +Jazzy v2.4.2 (2025-12-08) +-------------------------- +* ``lbr_description``: Added ``joint_limits_path`` argument to ``xacro`` files (defaults to KUKA values and user will currently need a custom launch file to configure): https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/333 + +Jazzy v2.4.1 (2025-12-08) +-------------------------- +* ``lbr_bringup``: Run a single ``controller_manager`` ``spawner`` for all controllers: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/329 + +Jazzy v2.4.0 (2025-12-06) +-------------------------- +This release removes the asynchronous force-torque estimation from the system interface (introduced in https://github.com/lbr-stack/lbr_fri_ros2_stack/releases/tag/humble-v2.2.0) +and instead provides a synchronous estimation in the form of a chainable ROS 2 controller. It further adds some controller updates, API fixes, and safety improvements. + +* ``lbr_bringup``: Added new chainable wrench interface controller to ``hardware.launch.py`` and added launch event handler for preceeding controllers +* ``lbr_demos``: Updated topics ``/lbr/state`` -> ``/lbr/lbr_state`` +* ``lbr_description``: + + * Removed now redundant ``estimated_ft_sensor`` from ``lbr_system_config.yaml`` and removed thus unused ``hardware`` prefix + * Removed all ``hardware`` specifiers from ``lbr_system_interface.xacro`` +* ``lbr_fri_ros2``: + + * Removed now redundant asynchronous ``lbr_fri_ros2::FTEstimator`` worker (also removed from ``lbr_ros2_control::SystemInterface``) + * Renamed ``lbr_fri_ros::FTEstimatorImpl`` -> ``lbr_fri_ros2::WrenchEstimator`` + * Fixed twist clamping bug: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/313 +* ``lbr_ros2_control``: + + * Changes to ``lbr_ros2_control::SystemInterface``: + + * Removed force-torque estimation from ``lbr_ros2_control::SystemInterface`` + * Exit ``on_activate`` in ``lbr_ros2_control::SystemInterface`` with error on ``IDLE``: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/321 + * Changes to controllers: + + * ``lbr_ros2_control::EstimatedWrenchInterface``: Added new chainable controller for synchronous force-torque estimation and state interface + * ``lbr_ros2_control::TwistController``: Stop twist controller on any joint limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/314 + * ``lbr_ros2_control::LBRStateBroadcaster``: + + * ``/lbr/state`` -> ``/lbr/lbr_state`` consistent with https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/271 + * Removed deprecated ``trylock()`` from ``lbr_ros2_control::LBRStateBroadcaster``: https://github.com/ros-controls/realtime_tools/pull/323 + * ``lbr_ros2_control::AdmittanceController``: + + * Fixed missing integration step in the admittance controller: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/320 + * Added an adjustable load data safety tolerance: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/325 + * Replaced veloctiy command filtering with force state filtering: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/327 +* Related pull requests: + + * Twist clamping and ``lbr_ros2_control::TwistController`` controller updates: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/315 + * ``lbr_ros2_control::AdmittanceController`` integration step and ``lbr_ros2_control::SystemInterface::on_activate`` exit: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/322 + * ``lbr_ros2_control::SystemInterface`` force-torque estimation removal (including new chainable controller): https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/324 + +Jazzy v2.3.0 (2025-11-21) -------------------------- * ``lbr_fri_ros2``: - * Interfaces now default to return by value for simplicity. - * Added a new ``StateGuard`` that tests for load data calibration on activation in compliant control modes and shuts the connection otherwise. + * Interfaces now default to return by value for simplicity. + * Added a new ``StateGuard`` that tests for load data calibration on activation in compliant control modes and shuts the connection otherwise + * Instead of disconnecting on ``CommandGuard`` limits, ``CommandInterfaces`` now execute a neutral command +* ``lbr_description``: + + * Updated joint limits (upper / lower) to be 1 degree stricter to avoid hardware limits. + * Added ``safety_controller`` tag to URDF files. Note, in Jazzy this is only utilised when ``enforce_command_limits:=true`` for the controller manager (default configured in ``lbr_controllers.yaml`` here). * ``lbr_ros2_control``: - * Migrated to Jazzy following guidelines at https://control.ros.org/jazzy/doc/ros2_control/doc/migration.html#migration-of-command-stateinterfaces. + * Migrated to Jazzy following guidelines at https://control.ros.org/jazzy/doc/ros2_control/doc/migration.html#migration-of-command-stateinterfaces * Removed the default error from ``AdmittanceController`` with the introduction of load data checks. Also now supports the default ``lbr_system_config.yaml``. - * WARN: KUKA's Cartesian impedance controller seems quite prone to singularities, and should thus be used with caution and only in ``T1`` mode. + * WARN: KUKA's Cartesian impedance controller seems quite prone to singularities, and should thus be used with caution and only in ``T1`` mode * ``LBRWrenchCommandController`` (uses Cartesian impedance) is now a ``ChainableControllerInterface`` to support separate wrench and joint position commands: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/250 - * Future releases will chain the ``LBRTorqueCommandController``, which uses KUKA's joint impedance controller without singularity issues. + * Future releases will chain the ``LBRTorqueCommandController``, which uses KUKA's joint impedance controller without singularity issues * Topics were updated to reflect the chained controller structure: * ``/lbr/wrench`` -> ``/lbr/lbr_wrench_command`` @@ -66,10 +124,10 @@ Humble v2.1.2 (2024-10-18) Humble v2.1.1 (2024-09-27) -------------------------- -* Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563). +* Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563) * ``lbr_bringup``: Updated launch files and dependencies. - * ``lbr_description``: Updated ```` tag to include Gazebo plugin (see https://github.com/ros-controls/gz_ros2_control/tree/humble). + * ``lbr_description``: Updated ```` tag to include Gazebo plugin (see https://github.com/ros-controls/gz_ros2_control/tree/humble) * ``lbr_ros2_control``: Changed ``gazebo_ros2_control/GazeboSystem`` -> ``ign_ros2_control/IgnitionSystem``` Humble v2.1.0 (2024-09-10) diff --git a/CITATION.cff b/CITATION.cff index f1508132..f25e743c 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -19,6 +19,6 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" -version: 2.3.0 +version: 2.4.3 doi: 10.21105/joss.06138 -date-released: 2025-03-26 +date-released: 2025-12-09 diff --git a/README.md b/README.md index 3254fd94..8760490d 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # lbr_fri_ros2_stack -[![License](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack)](https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/rolling?tab=Apache-2.0-1-ov-file#readme) +[![License](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack)](https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/jazzy?tab=Apache-2.0-1-ov-file#readme) [![Documentation Status](https://readthedocs.org/projects/lbr-stack/badge/?version=latest)](https://lbr-stack.readthedocs.io/en/latest/?badge=latest) [![JOSS](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef) [![Code Style: Black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) @@ -15,10 +15,18 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t LBR Med 14 R820 - LBR IIWA 7 R800 - LBR IIWA 14 R820 - LBR Med 7 R800 - LBR Med 14 R820 + + LBR IIWA 7 R800 + + + LBR IIWA 14 R820 + + + LBR Med 7 R800 + + + LBR Med 14 R820 + @@ -26,13 +34,13 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t ## Status | OS | ROS Distribution | FRI Version | Build Status | | :------------- | :--------------- | :---------- | :----------- | -| `Ubuntu-24.04` | `rolling` | `1.11` | [![ubuntu-24.04-fri-1.11](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.11.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.11.yml) | -| `Ubuntu-24.04` | `rolling` | `1.14` | [![ubuntu-24.04-fri-1.14](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.14.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.14.yml) | -| `Ubuntu-24.04` | `rolling` | `1.15` | [![ubuntu-24.04-fri-1.15](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.15.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.15.yml) | -| `Ubuntu-24.04` | `rolling` | `1.16` | [![ubuntu-24.04-fri-1.16](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.16.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.16.yml) | -| `Ubuntu-24.04` | `rolling` | `2.5` | [![ubuntu-24.04-fri-2.5](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml) | -| `Ubuntu-24.04` | `rolling` | `2.6` | [![ubuntu-24.04-fri-2.6](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml) | -| `Ubuntu-24.04` | `rolling` | `2.7` | [![ubuntu-24.04-fri-2.7](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.7.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.7.yml) | +| `Ubuntu-24.04` | `jazzy` | `1.11` | [![ubuntu-24.04-fri-1.11](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.11.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.11.yml) | +| `Ubuntu-24.04` | `jazzy` | `1.14` | [![ubuntu-24.04-fri-1.14](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.14.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.14.yml) | +| `Ubuntu-24.04` | `jazzy` | `1.15` | [![ubuntu-24.04-fri-1.15](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.15.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.15.yml) | +| `Ubuntu-24.04` | `jazzy` | `1.16` | [![ubuntu-24.04-fri-1.16](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.16.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-1.16.yml) | +| `Ubuntu-24.04` | `jazzy` | `2.5` | [![ubuntu-24.04-fri-2.5](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml) | +| `Ubuntu-24.04` | `jazzy` | `2.6` | [![ubuntu-24.04-fri-2.6](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.5.yml) | +| `Ubuntu-24.04` | `jazzy` | `2.7` | [![ubuntu-24.04-fri-2.7](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.7.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-24.04-fri-2.7.yml) | ## Documentation Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io/en/latest). @@ -47,10 +55,10 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io 2. Create a workspace, clone, and install dependencies ```shell - source /opt/ros/rolling/setup.bash + source /opt/ros/jazzy/setup.bash export FRI_CLIENT_VERSION=1.15 mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/rolling/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/jazzy/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml rosdep install --from-paths src -i -r -y ``` @@ -86,15 +94,28 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html). ## Repositories Using This Project -- [KUKA ROS2 controllers](https://github.com/idra-lab/kuka_lbr_control) — A repository for controlling KUKA LBR iiwa and med robots using various control algorithms. +- [KUKA ROS 2 Controllers](https://github.com/idra-lab/kuka_lbr_control): A repository for controlling KUKA LBR IIWA and Med robots using various control algorithms. -
- -| Kinematics Control | Gravity compensation | Impedance Control | -| :----------------- | :--------------------| :---------------- | -| | | | - -
+ + + + + + + + + + + + +
Kinematics ControlGravity CompensationImpedance Control
+ Kinematics Control + + Gravity Compensation + + Impedance Control +
+ ## Citation If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could leave a ⭐ and / or cite it, as it helps us to continue offering support. diff --git a/docker/Dockerfile b/docker/Dockerfile index 2d0c3e42..36f0eba7 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:rolling-ros-base-jammy +FROM ros:jazzy-ros-base # change default shell to bash SHELL ["/bin/bash", "-c"] diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 2d32e9b8..1a7b20d1 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -14,7 +14,7 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions export FRI_CLIENT_VERSION=1.15 # replace by your FRI client version mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/rolling/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml + vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/jazzy/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml #. Install `Docker `_:octicon:`link-external`. diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 4fab2142..fddddb52 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -6,24 +6,18 @@ The ``lbr_bringup`` package hosts some launch files, which can be included via s from launch import LaunchDescription from launch.actions import IncludeLaunchDescription - from launch.launch_description_sources import PythonLaunchDescriptionSource + from launch.substitutions import PathSubstitution + from launch_ros.substitutions import FindPackageShare + def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action( + return LaunchDescription( IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("lbr_bringup"), - "launch", - "mock.launch.py", - ] - ) - ), + PathSubstitution( + FindPackageShare("lbr_bringup") / "launch" / "mock.launch.py" + ) ) ) - return ld The launch files can also be run via the command line, as further described below. @@ -36,7 +30,7 @@ Launch Files ------------ Mock Setup ~~~~~~~~~~ -Useful for running a physics-free simulation of the system. This launch file will (see `mock.launch.py `_:octicon:`link-external`): +Useful for running a physics-free simulation of the system. This launch file will (see `mock.launch.py `_:octicon:`link-external`): #. Run the ``robot_state_publisher`` #. Run the ``ros2_control_node`` with mock components as loaded from ``robot_description`` @@ -52,7 +46,7 @@ Useful for running a physics-free simulation of the system. This launch file wil Gazebo Simulation ~~~~~~~~~~~~~~~~~ -Useful for running a physics simulation the the system. This launch file will will (see `gazebo.launch.py `_:octicon:`link-external`): +Useful for running a physics simulation the the system. This launch file will will (see `gazebo.launch.py `_:octicon:`link-external`): #. Start the ``robot_state_publisher`` #. Start the ``Gazebo`` simulation @@ -90,7 +84,7 @@ Hardware #. Launch file: - This launch file will (see `hardware.launch.py `_:octicon:`link-external`): + This launch file will (see `hardware.launch.py `_:octicon:`link-external`): #. Run the ``robot_state_publisher`` #. Run the ``ros2_control_node`` with the ``lbr_fri_ros2::SystemInterface`` plugin from :doc:`lbr_ros2_control <../../lbr_ros2_control/doc/lbr_ros2_control>` as loaded from ``robot_description`` (which will attempt to establish a connection to the real robot). @@ -106,7 +100,7 @@ Hardware RViz ~~~~ -This launch file will spin up ``RViz`` for visualization. It will (see `rviz.launch.py `_:octicon:`link-external`): +This launch file will spin up ``RViz`` for visualization. It will (see `rviz.launch.py `_:octicon:`link-external`): #. Read ``RViz`` configurations. #. Run ``RViz``. @@ -144,7 +138,7 @@ Mixins ------ The ``lbr_bringup`` package makes heavy use of mixins. Mixins are simply state-free classes with static methods. They are a convenient way of writing launch files. -The below shows an example of the `rviz.launch.py `_:octicon:`link-external` file: +The below shows an example of the `rviz.launch.py `_:octicon:`link-external` file: .. code:: python @@ -153,15 +147,9 @@ The below shows an example of the `rviz.launch.py LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action(RVizMixin.arg_rviz_cfg()) - ld.add_action(RVizMixin.arg_rviz_cfg_pkg()) - - # rviz - ld.add_action(RVizMixin.node_rviz()) - return ld + return LaunchDescription( + [RVizMixin.arg_rviz_cfg(), RVizMixin.arg_rviz_cfg_pkg(), RVizMixin.node_rviz()] + ) Which is quite compact and easy to read. @@ -183,5 +171,5 @@ Troubleshooting --------------- Noisy Execution ~~~~~~~~~~~~~~~ -- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_:octicon:`link-external`. +- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `hardware_controllers.yaml `_:octicon:`link-external`. - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username. diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index ef84ccf9..54f69cdd 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -1,47 +1,136 @@ from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration -from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.gazebo import GazeboMixin -from lbr_bringup.ros2_control import LBRROS2ControlMixin +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRROS2ControlMixin.arg_init_jnt_pos()) - ld.add_action( - LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml - - # robot description - robot_description = LBRDescriptionMixin.param_robot_description(mode="gazebo") - - # robot state publisher - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=True - ) - ld.add_action( - robot_state_publisher - ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description - - # Gazebo - ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - ld.add_action(GazeboMixin.node_clock_bridge()) - ld.add_action( - GazeboMixin.node_create() - ) # spawns robot in Gazebo through robot_description topic of robot_state_publisher - - # controllers - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="joint_state_broadcaster" - ) - ld.add_action(joint_state_broadcaster) - ld.add_action( - LBRROS2ControlMixin.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) + return LaunchDescription( + [ + DeclareLaunchArgument( + name="model", + default_value="iiwa7", + description="The LBR model in use.", + choices=["iiwa7", "iiwa14", "med7", "med14"], + ), + DeclareLaunchArgument( + name="robot_name", + default_value="lbr", + description="The robot's name.", + ), + DeclareLaunchArgument( + name="init_jnt_pos_pkg", + default_value="lbr_description", + description="Package containing the initial_joint_positions.yaml file.", + ), + DeclareLaunchArgument( + name="init_jnt_pos", + default_value="ros2_control/initial_joint_positions.yaml", + description="The relative path from sys_cfg_pkg to the initial_joint_positions.yaml file.", + ), + DeclareLaunchArgument( + name="ctrl", + default_value="joint_trajectory_controller", + description="Desired default controller. Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/gazebo_controllers.yaml.", + choices=[ + "forward_position_controller", + "joint_trajectory_controller", + ], + ), + DeclareLaunchArgument( + name="namespace", + defaultValue="lbr", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathSubstitution(FindPackageShare("lbr_description")) + / "urdf" + / LaunchConfiguration("model") + / LaunchConfiguration("model"), + ".xacro", + " robot_name:=", + LaunchConfiguration("robot_name"), + " mode:=gazebo", + " initial_joint_positions_path:=", + PathSubstitution( + FindPackageShare( + LaunchConfiguration("init_jnt_pos_pkg") + ) + ) + / LaunchConfiguration( + "init_jnt_pos", + ), + ] + ) + }, + {"use_sim_time": True}, + ], + namespace=LaunchConfiguration("namespace"), + ), + IncludeLaunchDescription( + PathSubstitution( + FindPackageShare("ros_gz_sim"), + ) + / "launch" + / "gz_sim.launch.py", + launch_arguments={"gz_args": "-r empty.sdf"}.items(), + ), # Gazebo has its own controller manager + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ), + Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", + "robot_description", + "-name", + LaunchConfiguration("robot_name"), + "-allow_renaming", + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.0", + "-R", + "0.0", + "-P", + "0.0", + "-Y", + "0.0", + ], + output="screen", + namespace=LaunchConfiguration("namespace"), + ), # spawns robot in Gazebo through robot_description topic of robot_state_publisher + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + LaunchConfiguration("ctrl"), + ], + namespace=LaunchConfiguration("namespace"), + ), + ] ) - return ld diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index 2ace1ec7..4e95d7cd 100644 --- a/lbr_bringup/launch/hardware.launch.py +++ b/lbr_bringup/launch/hardware.launch.py @@ -1,60 +1,126 @@ from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration -from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.ros2_control import LBRROS2ControlMixin +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRROS2ControlMixin.arg_sys_cfg_pkg()) - ld.add_action(LBRROS2ControlMixin.arg_sys_cfg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - - # robot description - robot_description = LBRDescriptionMixin.param_robot_description(mode="hardware") - - # robot state publisher - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=False - ) - ld.add_action(robot_state_publisher) - - # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) - ld.add_action(ros2_control_node) - - # joint state broad caster and controller on ros2 control node start - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="joint_state_broadcaster" - ) - force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="force_torque_broadcaster" - ) - lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="lbr_state_broadcaster" - ) - controller = LBRROS2ControlMixin.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) - - controller_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[ - joint_state_broadcaster, - force_torque_broadcaster, - lbr_state_broadcaster, - controller, - ], - ) + return LaunchDescription( + [ + DeclareLaunchArgument( + name="model", + default_value="iiwa7", + description="The LBR model in use.", + choices=["iiwa7", "iiwa14", "med7", "med14"], + ), + DeclareLaunchArgument( + name="robot_name", + default_value="lbr", + description="The robot's name.", + ), + DeclareLaunchArgument( + name="sys_cfg_pkg", + default_value="lbr_description", + description="Package containing the lbr_system_config.yaml file for FRI configurations.", + ), + DeclareLaunchArgument( + name="sys_cfg", + default_value="ros2_control/lbr_system_config.yaml", + description="The relative path from sys_cfg_pkg to the lbr_system_config.yaml file.", + ), + DeclareLaunchArgument( + name="ctrl_cfg_pkg", + default_value="lbr_description", + description="Controller configuration package. The package containing the ctrl_cfg.", + ), + DeclareLaunchArgument( + name="ctrl_cfg", + default_value="ros2_control/hardware_controllers.yaml", + description="Relative path from ctrl_cfg_pkg to the controllers.", + ), + DeclareLaunchArgument( + name="ctrl", + default_value="joint_trajectory_controller", + description="Desired default controller. One of specified in ctrl_cfg.", + choices=[ + "admittance_controller", + "forward_position_controller", + "joint_trajectory_controller", + "lbr_joint_position_command_controller", + "lbr_torque_command_controller", + "lbr_wrench_command_controller", + "twist_controller", + ], + ), + DeclareLaunchArgument( + name="namespace", + defaultValue="lbr", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathSubstitution(FindPackageShare("lbr_description")) + / "urdf" + / LaunchConfiguration("model") + / LaunchConfiguration("model"), + ".xacro", + " robot_name:=", + LaunchConfiguration("robot_name"), + " mode:=hardware", + " system_config_path:=", + PathSubstitution( + FindPackageShare(LaunchConfiguration("sys_cfg_pkg")) + ) + / LaunchConfiguration("sys_cfg"), + ] + ) + }, + {"use_sim_time": False}, + ], + namespace=LaunchConfiguration("namespaace"), + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution( + FindPackageShare(LaunchConfiguration("ctrl_cfg_pkg")) + ) + / LaunchConfiguration("ctrl_cfg"), + ], + namespace=LaunchConfiguration("namespace"), + remappings=[ + ("~/robot_description", "robot_description"), + ], + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + "estimated_wrench_interface", + "lbr_state_broadcaster", + "force_torque_broadcaster", + LaunchConfiguration("ctrl"), + ], + namespace=LaunchConfiguration("namespace"), + ), + ] ) - ld.add_action(controller_event_handler) - return ld diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py index 5170e705..b4797c7f 100644 --- a/lbr_bringup/launch/mock.launch.py +++ b/lbr_bringup/launch/mock.launch.py @@ -1,51 +1,122 @@ from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration -from lbr_bringup.description import LBRDescriptionMixin -from lbr_bringup.ros2_control import LBRROS2ControlMixin +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - ld.add_action(LBRROS2ControlMixin.arg_init_jnt_pos()) - - # robot description - robot_description = LBRDescriptionMixin.param_robot_description(mode="mock") - - # robot state publisher - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=False - ) - ld.add_action(robot_state_publisher) - - # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) - ld.add_action(ros2_control_node) - - # joint state broad caster and controller on ros2 control node start - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="joint_state_broadcaster" - ) - controller = LBRROS2ControlMixin.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) - - controller_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[ - joint_state_broadcaster, - controller, - ], - ) + return LaunchDescription( + [ + DeclareLaunchArgument( + name="model", + default_value="iiwa7", + description="The LBR model in use.", + choices=["iiwa7", "iiwa14", "med7", "med14"], + ), + DeclareLaunchArgument( + name="robot_name", + default_value="lbr", + description="The robot's name.", + ), + DeclareLaunchArgument( + name="ctrl_cfg_pkg", + default_value="lbr_description", + description="Controller configuration package. The package containing the ctrl_cfg.", + ), + DeclareLaunchArgument( + name="ctrl_cfg", + default_value="ros2_control/mock_controllers.yaml", + description="Relative path from ctrl_cfg_pkg to the controllers.", + ), + DeclareLaunchArgument( + name="ctrl", + default_value="joint_trajectory_controller", + description="Desired default controller. One of specified in ctrl_cfg.", + choices=[ + "forward_position_controller", + "joint_trajectory_controller", + ], + ), + DeclareLaunchArgument( + name="init_jnt_pos_pkg", + default_value="lbr_description", + description="Package containing the initial_joint_positions.yaml file.", + ), + DeclareLaunchArgument( + name="namespace", + default="lbr", + ), + DeclareLaunchArgument( + name="init_jnt_pos", + default_value="ros2_control/initial_joint_positions.yaml", + description="The relative path from sys_cfg_pkg to the initial_joint_positions.yaml file.", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + FindExecutable(name="xacro"), + " ", + PathSubstitution(FindPackageShare("lbr_description")) + / "urdf" + / LaunchConfiguration("model") + / LaunchConfiguration("model"), + ".xacro", + " robot_name:=", + LaunchConfiguration("robot_name"), + " mode:=mock", + " initial_joint_positions_path:=", + PathSubstitution( + FindPackageShare( + LaunchConfiguration("init_jnt_pos_pkg") + ) + ) + / LaunchConfiguration( + "init_jnt_pos", + ), + ] + ) + }, + {"use_sim_time": False}, + ], + namespace=LaunchConfiguration("namespace"), + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + {"use_sim_time": False}, + PathSubstitution( + FindPackageShare(LaunchConfiguration("ctrl_cfg_pkg")) + ) + / LaunchConfiguration("ctrl_cfg"), + ], + namespace=LaunchConfiguration("namespace"), + remappings=[ + ("~/robot_description", "robot_description"), + ], + ), + Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "--controller-manager", + "controller_manager", + "joint_state_broadcaster", + LaunchConfiguration("ctrl"), + ], + namespace=LaunchConfiguration("namespace"), + ), + ] ) - ld.add_action(controller_event_handler) - return ld diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index fcfb19aa..169e131d 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -3,7 +3,7 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.moveit import LBRMoveGroupMixin from lbr_bringup.rviz import RVizMixin @@ -55,29 +55,26 @@ def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: remappings=[ ( "display_planned_path", - PathJoinSubstitution([robot_name, "display_planned_path"]), + PathSubstitution(robot_name) / "display_planned_path", ), - ("joint_states", PathJoinSubstitution([robot_name, "joint_states"])), + ("joint_states", PathSubstitution(robot_name) / "joint_states"), ( "monitored_planning_scene", - PathJoinSubstitution([robot_name, "monitored_planning_scene"]), + PathSubstitution(robot_name) / "monitored_planning_scene", ), - ("planning_scene", PathJoinSubstitution([robot_name, "planning_scene"])), + ("planning_scene", PathSubstitution(robot_name) / "planning_scene"), ( "planning_scene_world", - PathJoinSubstitution([robot_name, "planning_scene_world"]), - ), - ( - "robot_description", - PathJoinSubstitution([robot_name, "robot_description"]), + PathSubstitution(robot_name) / "planning_scene_world", ), + ("robot_description", PathSubstitution(robot_name) / "robot_description"), ( "robot_description_semantic", - PathJoinSubstitution([robot_name, "robot_description_semantic"]), + PathSubstitution(robot_name) / "robot_description_semantic", ), ( "recognized_object_array", - PathJoinSubstitution([robot_name, "recognized_object_array"]), + PathSubstitution(robot_name) / "recognized_object_array", ), ], condition=IfCondition(LaunchConfiguration("rviz")), diff --git a/lbr_bringup/launch/moveit_servo.launch.py b/lbr_bringup/launch/moveit_servo.launch.py index 400a7258..b5a580b5 100644 --- a/lbr_bringup/launch/moveit_servo.launch.py +++ b/lbr_bringup/launch/moveit_servo.launch.py @@ -4,7 +4,7 @@ from launch.actions import OpaqueFunction, RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.substitutions import FindPackageShare from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.moveit import LBRMoveGroupMixin, LBRMoveItServoMixin @@ -13,8 +13,8 @@ def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() - moveit_servo_config = PathJoinSubstitution( - [FindPackageShare("lbr_bringup"), "config/moveit_servo.yaml"] + moveit_servo_config = ( + PathSubstitution(FindPackageShare("lbr_bringup")) / "config/moveit_servo.yaml" ) model = LaunchConfiguration("model").perform(context) moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py index 51857d14..afc01195 100644 --- a/lbr_bringup/launch/rviz.launch.py +++ b/lbr_bringup/launch/rviz.launch.py @@ -1,14 +1,34 @@ from launch import LaunchDescription -from lbr_bringup.rviz import RVizMixin +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action(RVizMixin.arg_rviz_cfg()) - ld.add_action(RVizMixin.arg_rviz_cfg_pkg()) - - # rviz - ld.add_action(RVizMixin.node_rviz()) - return ld + return LaunchDescription( + [ + DeclareLaunchArgument( + name="rviz_cfg", + default_value="config/mock.rviz", + description="The RViz configuration file relative to rviz_cfg_pkg.", + ), + DeclareLaunchArgument( + name="rviz_cfg_pkg", + default_value="lbr_bringup", + description="The package containing the RViz configuration file.", + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=[ + "-d", + PathSubstitution( + FindPackageShare(LaunchConfiguration("rviz_cfg_pkg")) + ) + / LaunchConfiguration("rviz_cfg"), + ], + ), + ] + ) diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 098a60fa..3293d756 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -5,7 +5,7 @@ Command, FindExecutable, LaunchConfiguration, - PathJoinSubstitution, + PathSubstitution, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -25,27 +25,21 @@ def param_robot_description( ), system_config_path: Optional[ Union[LaunchConfiguration, str] - ] = PathJoinSubstitution( - [ - FindPackageShare( - LaunchConfiguration("sys_cfg_pkg", default="lbr_description") - ), - LaunchConfiguration( - "sys_cfg", default="ros2_control/lbr_system_config.yaml" - ), - ] - ), + ] = PathSubstitution( + FindPackageShare( + LaunchConfiguration("sys_cfg_pkg", default="lbr_description") + ) + ) + / LaunchConfiguration("sys_cfg", default="ros2_control/lbr_system_config.yaml"), initial_joint_positions_path: Optional[ Union[LaunchConfiguration, str] - ] = PathJoinSubstitution( - [ - FindPackageShare( - LaunchConfiguration("sys_cfg_pkg", default="lbr_description") - ), - LaunchConfiguration( - "init_jnt_pos", default="ros2_control/initial_joint_positions.yaml" - ), - ] + ] = PathSubstitution( + FindPackageShare( + LaunchConfiguration("sys_cfg_pkg", default="lbr_description") + ) + ) + / LaunchConfiguration( + "init_jnt_pos", default="ros2_control/initial_joint_positions.yaml" ), ) -> Dict[str, str]: robot_description = { @@ -53,14 +47,10 @@ def param_robot_description( [ FindExecutable(name="xacro"), " ", - PathJoinSubstitution( - [ - FindPackageShare("lbr_description"), - "urdf", - model, - model, - ] - ), + PathSubstitution(FindPackageShare("lbr_description")) + / "urdf" + / model + / model, ".xacro", " robot_name:=", robot_name, diff --git a/lbr_bringup/lbr_bringup/gazebo.py b/lbr_bringup/lbr_bringup/gazebo.py index 07e74c6c..edc9c104 100644 --- a/lbr_bringup/lbr_bringup/gazebo.py +++ b/lbr_bringup/lbr_bringup/gazebo.py @@ -1,8 +1,7 @@ from typing import List, Optional, Union from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -11,15 +10,11 @@ class GazeboMixin: @staticmethod def include_gazebo(**kwargs) -> IncludeLaunchDescription: return IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ), - ), + PathSubstitution( + FindPackageShare("ros_gz_sim"), + ) + / "launch" + / "gz_sim.launch.py", launch_arguments={"gz_args": "-r empty.sdf"}.items(), **kwargs, ) diff --git a/lbr_bringup/lbr_bringup/moveit.py b/lbr_bringup/lbr_bringup/moveit.py index 5cc120ae..d146df1f 100644 --- a/lbr_bringup/lbr_bringup/moveit.py +++ b/lbr_bringup/lbr_bringup/moveit.py @@ -6,7 +6,7 @@ from launch.substitutions import ( FindExecutable, LaunchConfiguration, - PathJoinSubstitution, + PathSubstitution, ) from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -159,7 +159,7 @@ def call_start_servo_service( FindExecutable(name="ros2"), "service", "call", - PathJoinSubstitution([robot_name, "servo_node/start_servo"]), + PathSubstitution(robot_name) / "servo_node" / "start_servo", "std_srvs/srv/Trigger", ], **kwargs, diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index c1dd1560..e4efb260 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -1,7 +1,7 @@ -from typing import Dict, Optional, Union +from typing import Dict, List, Optional, Union from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -19,7 +19,7 @@ def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: def arg_ctrl_cfg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg", - default_value="ros2_control/lbr_controllers.yaml", + default_value="ros2_control/hardware_controllers.yaml", description="Relative path from ctrl_cfg_pkg to the controllers.", ) @@ -31,8 +31,8 @@ def arg_ctrl() -> DeclareLaunchArgument: description="Desired default controller. One of specified in ctrl_cfg.", choices=[ "admittance_controller", - "joint_trajectory_controller", "forward_position_controller", + "joint_trajectory_controller", "lbr_joint_position_command_controller", "lbr_torque_command_controller", "lbr_wrench_command_controller", @@ -87,17 +87,13 @@ def node_ros2_control( executable="ros2_control_node", parameters=[ {"use_sim_time": use_sim_time}, - PathJoinSubstitution( - [ - FindPackageShare( - LaunchConfiguration( - "ctrl_cfg_pkg", default="lbr_description" - ) - ), - LaunchConfiguration( - "ctrl_cfg", default="ros2_control/lbr_controllers.yaml" - ), - ] + PathSubstitution( + FindPackageShare( + LaunchConfiguration("ctrl_cfg_pkg", default="lbr_description") + ) + ) + / LaunchConfiguration( + "ctrl_cfg", default="ros2_control/hardware_controllers.yaml" ), ], namespace=robot_name, @@ -112,9 +108,9 @@ def node_controller_spawner( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), - controller: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "ctrl" - ), + controllers: Optional[List[Union[LaunchConfiguration, str]]] = [ + LaunchConfiguration("ctrl") + ], **kwargs, ) -> Node: return Node( @@ -122,10 +118,10 @@ def node_controller_spawner( executable="spawner", output="screen", arguments=[ - controller, "--controller-manager", "controller_manager", - ], + ] + + controllers, namespace=robot_name, **kwargs, ) diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py index b85e0ffe..ecd8bcca 100644 --- a/lbr_bringup/lbr_bringup/rviz.py +++ b/lbr_bringup/lbr_bringup/rviz.py @@ -1,7 +1,7 @@ from typing import Optional, Union from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -51,12 +51,7 @@ def node_rviz( name="rviz2", arguments=[ "-d", - PathJoinSubstitution( - [ - FindPackageShare(rviz_cfg_pkg), - rviz_cfg, - ] - ), + PathSubstitution(FindPackageShare(rviz_cfg_pkg)) / rviz_cfg, ], **kwargs, ) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 951c6f70..238c6e7f 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 2.3.0 + 2.4.3 LBR launch files. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt index b4a883db..d5059673 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt @@ -76,9 +76,4 @@ install( RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - ament_package() diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml deleted file mode 100644 index 165055de..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml +++ /dev/null @@ -1,37 +0,0 @@ -# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface -hardware: - fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro - major_version: 1 - minor_version: 15 - client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] - port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups - remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection - rt_prio: 80 # real-time priority for the control loop - # exponential moving average time constant for joint position commands [s]: - # Set tau > robot sample time for more smoothing on the commands - # Set tau << robot sample time for no smoothing on the commands - joint_position_tau: 0.2 - command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - # exponential moving average time constant for external joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the external torque measurements - # Set tau << robot sample time for more smoothing on the external torque measurements - external_torque_tau: 0.4 - # exponential moving average time constant for joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the raw torque measurements - # Set tau << robot sample time for more smoothing on the raw torque measurements - measured_torque_tau: 0.4 - open_loop: true # KUKA works the best in open_loop control mode - -estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - enabled: true # whether to enable the force-torque estimation - update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) - rt_prio: 30 # real-time priority for the force-torque estimation - chain_root: lbr_link_0 - chain_tip: lbr_link_ee - damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian - force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered - force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered - force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered - torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 91d58d27..5783a489 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -62,14 +62,12 @@ This demo uses the admittance controller. - ``FRI control mode``: ``POSITION_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Launch the robot driver (please note that a different system configuration file is used with heavier smoothing!): +#. Launch the robot driver: .. code-block:: bash ros2 launch lbr_bringup hardware.launch.py \ ctrl:=admittance_controller \ - sys_cfg_pkg:=lbr_demos_advanced_cpp \ - sys_cfg:=config/lbr_system_config.yaml \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Now gently move the robot at the end-effector. @@ -81,8 +79,8 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index ecdefd2c..d96f6772 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_cpp - 2.3.0 + 2.4.3 Advanced C++ demos for the lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index e477a43a..f1dd1fd8 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -25,7 +25,7 @@ class LBRBasePositionCommandNode : public rclcpp::Node { "command/lbr_joint_position_command", 1); lbr_state_sub_ = create_subscription( - "state", 1, + "lbr_state", 1, std::bind(&LBRBasePositionCommandNode::on_lbr_state_, this, std::placeholders::_1)); } diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 2284202f..38a61ddc 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -14,8 +14,8 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -38,7 +38,7 @@ This demo implements a simple admittance controller. ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `admittance_control `_:octicon:`link-external` with remapping and parameter file: +#. Run the `admittance_control `_:octicon:`link-external` with remapping and parameter file: .. code-block:: bash @@ -54,8 +54,8 @@ This demo implements an admittance controller with a remote center of motion (RC #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -78,7 +78,7 @@ This demo implements an admittance controller with a remote center of motion (RC ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `admittance_rcm_control `_:octicon:`link-external` with remapping and parameter file: +#. Run the `admittance_rcm_control `_:octicon:`link-external` with remapping and parameter file: .. code-block:: bash diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py index 663a80d5..a642025d 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py @@ -30,7 +30,7 @@ def __init__(self, node_name: str) -> None: # publishers and subscribers self._lbr_state_sub = self.create_subscription( - LBRState, "state", self._on_lbr_state, 1 + LBRState, "lbr_state", self._on_lbr_state, 1 ) self._lbr_joint_position_command_pub = self.create_publisher( LBRJointPositionCommand, diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index c6879941..517c32bb 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_py - 2.3.0 + 2.4.3 Advanced Python demos for the lbr_ros2_control. mhubii cmower diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index 6542daec..9ab9fc32 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="2.3.0", + version="2.4.3", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 8f7b834d..321b0b6a 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,8 +14,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -38,7 +38,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `joint_sine_overlay `_:octicon:`link-external` node: +#. Run the `joint_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash @@ -60,7 +60,7 @@ Simulation ctrl:=joint_trajectory_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `joint_trajectory_client `_:octicon:`link-external`: +#. Run the `joint_trajectory_client `_:octicon:`link-external`: .. code-block:: bash @@ -72,8 +72,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,8 +96,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -120,7 +120,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays ctrl:=lbr_torque_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `torque_sine_overlay `_:octicon:`link-external` node: +#. Run the `torque_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash @@ -134,8 +134,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -158,7 +158,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays ctrl:=lbr_wrench_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `wrench_sine_overlay `_:octicon:`link-external` node: +#. Run the `wrench_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 2d69fdf8..0c5e436b 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_cpp - 2.3.0 + 2.4.3 C++ demos for lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp index 948f6595..a511db87 100644 --- a/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp @@ -26,7 +26,7 @@ class JointSineOverlay { // create subscription to state lbr_state_sub_ = node_->create_subscription( - "state", 1, std::bind(&JointSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + "lbr_state", 1, std::bind(&JointSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager auto update_rate = diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index ea83485d..012cdd2c 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -25,7 +25,7 @@ class TorqueSineOverlay { // create subscription to state lbr_state_sub_ = node_->create_subscription( - "state", 1, std::bind(&TorqueSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + "lbr_state", 1, std::bind(&TorqueSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager auto update_rate = diff --git a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp index 89c63520..4f0442c3 100644 --- a/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/wrench_sine_overlay.cpp @@ -27,7 +27,7 @@ class WrenchSineOverlay { // create subscription to state lbr_state_sub_ = node_->create_subscription( - "state", 1, std::bind(&WrenchSineOverlay::on_lbr_state_, this, std::placeholders::_1)); + "lbr_state", 1, std::bind(&WrenchSineOverlay::on_lbr_state_, this, std::placeholders::_1)); // get control rate from controller_manager auto update_rate = diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index 4d1258d4..97a9e5e3 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,8 +14,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -38,7 +38,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `joint_sine_overlay `_:octicon:`link-external` node: +#. Run the `joint_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash @@ -60,7 +60,7 @@ Simulation ctrl:=joint_trajectory_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `joint_trajectory_client `_:octicon:`link-external`: +#. Run the `joint_trajectory_client `_:octicon:`link-external`: .. code-block:: bash @@ -72,8 +72,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,8 +96,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -120,7 +120,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays ctrl:=lbr_torque_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `torque_sine_overlay `_:octicon:`link-external` node: +#. Run the `torque_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash @@ -134,8 +134,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -158,7 +158,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays ctrl:=lbr_wrench_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `wrench_sine_overlay `_:octicon:`link-external` node: +#. Run the `wrench_sine_overlay `_:octicon:`link-external` node: .. code-block:: bash diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py index e3b34ef0..bbb9bf7c 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py @@ -28,7 +28,7 @@ def __init__(self, node_name: str) -> None: # create subscription to state self._lbr_state = None self._lbr_state_sub_ = self.create_subscription( - LBRState, "state", self._on_lbr_state, 1 + LBRState, "lbr_state", self._on_lbr_state, 1 ) # get control rate from controller_manager diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py index 7e63438f..9f70b8f7 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/torque_sine_overlay.py @@ -26,7 +26,7 @@ def __init__(self, node_name: str) -> None: # create subscription to state self._lbr_state = None self._lbr_state_sub = self.create_subscription( - LBRState, "state", self._on_lbr_state, 1 + LBRState, "lbr_state", self._on_lbr_state, 1 ) # get control rate from controller_manager diff --git a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py index 477c7a28..fc9c8682 100644 --- a/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py +++ b/lbr_demos/lbr_demos_py/lbr_demos_py/wrench_sine_overlay.py @@ -26,7 +26,7 @@ def __init__(self, node_name: str) -> None: # create subscription to state self._lbr_state = None self._lbr_state_sub = self.create_subscription( - LBRState, "state", self._on_lbr_state, 1 + LBRState, "lbr_state", self._on_lbr_state, 1 ) # get control rate from controller_manager diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index ed2707ea..0625ffcc 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_py - 2.3.0 + 2.4.3 Python demos for lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index 50ddbb1c..f2489c26 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.3.0", + version="2.4.3", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index cdfdbb49..98eb6449 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -54,16 +54,16 @@ MoveIt Servo - Simulation You can now experiment with -- Modifying the MoveIt Servo parameters in `moveit_servo.yaml `_:octicon:`link-external`. E.g. the ``robot_link_command_frame`` to change the commanding frame. +- Modifying the MoveIt Servo parameters in `moveit_servo.yaml `_:octicon:`link-external`. E.g. the ``robot_link_command_frame`` to change the commanding frame. - Connect a joystick or game controller. -- Or changing the veloctiy scales for this keyboard driver in `forward_keyboard.yaml `_:octicon:`link-external`. +- Or changing the veloctiy scales for this keyboard driver in `forward_keyboard.yaml `_:octicon:`link-external`. MoveIt Servo - Hardware ~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -122,8 +122,8 @@ MoveIt via RViz - Hardware ~~~~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py index c2368065..0c6f8e45 100644 --- a/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py +++ b/lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py @@ -1,47 +1,36 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from lbr_bringup.description import LBRDescriptionMixin def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - # launch arguments - ld.add_action( - DeclareLaunchArgument( - name="keyboard_config_pkg", - default_value="lbr_moveit", - description="The package containing the keyboard configurations.", - ) - ) - ld.add_action( - DeclareLaunchArgument( - name="keyboard_config", - default_value="config/forward_keyboard.yaml", - description="Location of the keyboard configuration file relative to keyboard_config_pkg.", - ) - ) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - - # forward keyboard node - keyboard_driver_config = PathJoinSubstitution( + return LaunchDescription( [ - FindPackageShare(LaunchConfiguration("keyboard_config_pkg")), - LaunchConfiguration("keyboard_config"), + DeclareLaunchArgument( + name="keyboard_config_pkg", + default_value="lbr_moveit", + description="The package containing the keyboard configurations.", + ), + DeclareLaunchArgument( + name="keyboard_config", + default_value="config/forward_keyboard.yaml", + description="Location of the keyboard configuration file relative to keyboard_config_pkg.", + ), + LBRDescriptionMixin.arg_robot_name(), + Node( + package="lbr_moveit", + executable="forward_keyboard", + output="screen", + parameters=[ + PathSubstitution( + FindPackageShare(LaunchConfiguration("keyboard_config_pkg")) + ) + / LaunchConfiguration("keyboard_config"), + ], + namespace=LaunchConfiguration("robot_name"), + ), ] ) - ld.add_action( - Node( - package="lbr_moveit", - executable="forward_keyboard", - output="screen", - parameters=[ - keyboard_driver_config, - ], - namespace=LaunchConfiguration("robot_name"), - ) - ) - return ld diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/package.xml index 8833f0e2..9797cde1 100644 --- a/lbr_demos/lbr_moveit/package.xml +++ b/lbr_demos/lbr_moveit/package.xml @@ -2,7 +2,7 @@ lbr_moveit - 2.3.0 + 2.4.3 MoveIt demos for the LBRs mhubii Apache-2.0 diff --git a/lbr_demos/lbr_moveit/setup.py b/lbr_demos/lbr_moveit/setup.py index de7601f9..a3ec02b0 100644 --- a/lbr_demos/lbr_moveit/setup.py +++ b/lbr_demos/lbr_moveit/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.3.0", + version="2.4.3", packages=find_packages(exclude=["test"]), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index 28df60a0..79e4e4a7 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -28,7 +28,7 @@ Simulation mode:=mock \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Run the `hello_moveit `_:octicon:`link-external` node: +#. Run the `hello_moveit `_:octicon:`link-external` node: .. code-block:: bash @@ -40,8 +40,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `hardware_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -60,7 +60,7 @@ Hardware Examining the Code ~~~~~~~~~~~~~~~~~~ -The source code for this demo is available on `GitHub `_:octicon:`link-external`. The demo vastly follows the official `MoveIt `_:octicon:`link-external` demo. +The source code for this demo is available on `GitHub `_:octicon:`link-external`. The demo vastly follows the official `MoveIt `_:octicon:`link-external` demo. Differently, this repository puts the ``MoveGroup`` under a namespace. The ``MoveGroup`` is thus created as follows: diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index 526b7c80..39253907 100644 --- a/lbr_demos/lbr_moveit_cpp/package.xml +++ b/lbr_demos/lbr_moveit_cpp/package.xml @@ -2,7 +2,7 @@ lbr_moveit_cpp - 2.3.0 + 2.4.3 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 3c9fa1ab..e26df5ea 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_description)/ros2_control/lbr_controllers.yaml + $(find lbr_description)/ros2_control/gazebo_controllers.yaml /${robot_name} diff --git a/lbr_description/package.xml b/lbr_description/package.xml index b12d2dcc..79846ecc 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 2.3.0 + 2.4.3 KUKA LBR description files mhubii Apache-2.0 diff --git a/lbr_description/ros2_control/gazebo_controllers.yaml b/lbr_description/ros2_control/gazebo_controllers.yaml new file mode 100644 index 00000000..76264c3e --- /dev/null +++ b/lbr_description/ros2_control/gazebo_controllers.yaml @@ -0,0 +1,48 @@ +# Use of /** so that the configurations hold for controller +# managers regardless of their namespace. Usefull in multi-robot setups. +/**/controller_manager: + ros__parameters: + update_rate: 100 + enforce_command_limits: true # can be removed from Kilted onward + + # ROS 2 control broadcasters + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + +# ROS 2 control controllers +/**/joint_trajectory_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + +/**/forward_position_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + interface_name: position diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/hardware_controllers.yaml similarity index 70% rename from lbr_description/ros2_control/lbr_controllers.yaml rename to lbr_description/ros2_control/hardware_controllers.yaml index ad879894..19cd33a1 100644 --- a/lbr_description/ros2_control/lbr_controllers.yaml +++ b/lbr_description/ros2_control/hardware_controllers.yaml @@ -3,6 +3,7 @@ /**/controller_manager: ros__parameters: update_rate: 100 + enforce_command_limits: true # can be removed from Kilted onward # ROS 2 control broadcasters joint_state_broadcaster: @@ -12,6 +13,9 @@ type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster # LBR ROS 2 control broadcasters + estimated_wrench_interface: + type: lbr_ros2_control/EstimatedWrenchInterface + lbr_state_broadcaster: type: lbr_ros2_control/LBRStateBroadcaster @@ -41,10 +45,24 @@ # ROS 2 control broadcasters /**/force_torque_broadcaster: ros__parameters: - frame_id: lbr_link_ee # namespace: https://github.com/ros2/rviz/issues/1103 - sensor_name: estimated_ft_sensor + frame_id: lbr_link_ee # note: frame_id must equal estimated_wrench_interface.wrench_estimator.chain_tip; namespace: https://github.com/ros2/rviz/issues/1103 + sensor_name: estimated_wrench_interface # note: name must equal estimated_wrench_interface node name (chained) # LBR ROS 2 control broadcasters +/**/estimated_wrench_interface: + ros__parameters: + robot_name: lbr + wrench_estimator: + chain_root: lbr_link_0 + chain_tip: lbr_link_ee + damping: 0.2 + force_x_th: 2.0 + force_y_th: 2.0 + force_z_th: 2.0 + torque_x_th: 0.5 + torque_y_th: 0.5 + torque_z_th: 0.5 + /**/lbr_state_broadcaster: ros__parameters: robot_name: lbr @@ -98,20 +116,24 @@ /**/admittance_controller: ros__parameters: robot_name: lbr + ft_sensor_name: estimated_wrench_interface # should equal the estimated_wrench_interface node name above admittance: - mass: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - damping: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + mass: [0.05, 0.05, 0.05, 0.005, 0.005, 0.005] + damping: [1., 1., 1., 0.1, 0.1, 0.1] stiffness: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] inv_jac_ctrl: chain_root: lbr_link_0 chain_tip: lbr_link_ee damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian max_linear_velocity: 2.0 # maximum linear velocity - max_angular_velocity: 2.0 # maximum linear acceleration + max_angular_velocity: 2.0 # maximum angular velocity joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains filter: - joint_velocity_tau: 0.4 # exponential moving average time constant for joint velocity commands [s] + f_ext_tau: 0.2 # exponential moving average time constant for external forces [s] + on_activate: + max_external_force: 0.0 + max_external_torque: 0.0 /**/twist_controller: ros__parameters: @@ -120,9 +142,9 @@ chain_root: lbr_link_0 chain_tip: lbr_link_ee twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame - damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian - max_linear_velocity: 0.1 # maximum linear velocity - max_angular_velocity: 0.1 # maximum linear acceleration + damping: 0.02 # damping factor for the pseudo-inverse of the Jacobian + max_linear_velocity: 0.2 # maximum linear velocity + max_angular_velocity: 0.2 # maximum angular velocity joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains timeout: 0.2 # zeros the twist command when no command is received within timeout [s] diff --git a/lbr_description/ros2_control/initial_joint_positions.yaml b/lbr_description/ros2_control/initial_joint_positions.yaml index 66572c33..a4846b46 100644 --- a/lbr_description/ros2_control/initial_joint_positions.yaml +++ b/lbr_description/ros2_control/initial_joint_positions.yaml @@ -1,8 +1,8 @@ # initial joint positions [degrees] -A1 : 0.0 -A2 : 25.0 -A3 : 0.0 -A4 : 90.0 -A5 : 0.0 -A6 : 0.0 -A7 : 0.0 +A1: 0.0 +A2: 25.0 +A3: 0.0 +A4: 90.0 +A5: 0.0 +A6: 0.0 +A7: 0.0 diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index dec430fd..522829eb 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -1,44 +1,32 @@ # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface -hardware: - fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro - major_version: 1 - minor_version: 15 - client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] - port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups - remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection - rt_prio: 80 # real-time priority for the control loop - # exponential moving average time constant for joint position commands [s]: - # set tau > robot sample time for more smoothing on the commands - # set tau << robot sample time for no smoothing on the commands - joint_position_tau: 0.04 - command_guard: - variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - state_guard: - # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE] - # in POSITION control mode, safety check is not performed - external_torque_safety_check: true - external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm] - # exponential moving average time constant for external joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the external torque measurements - # set tau << robot sample time for no smoothing on the external torque measurements - external_torque_tau: 0.1 - # exponential moving average time constant for joint torque measurements [s]: - # set tau > robot sample time for more smoothing on the raw torque measurements - # set tau << robot sample time for no smoothing on the raw torque measurements - measured_torque_tau: 0.1 - # for position control, LBR works best in open_loop control mode - # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode - open_loop: true -estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - enabled: true # whether to enable the force-torque estimation - update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) - rt_prio: 30 # real-time priority for the force-torque estimation - chain_root: lbr_link_0 - chain_tip: lbr_link_ee - damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian - force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered - force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered - force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered - torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered - torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered +fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 +client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] +port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups +remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection +rt_prio: 80 # real-time priority for the control loop +# exponential moving average time constant for joint position commands [s]: +# set tau > robot sample time for more smoothing on the commands +# set tau << robot sample time for no smoothing on the commands +joint_position_tau: 0.04 +command_guard: + # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead + # available: [default, safe_stop] + variant: default +state_guard: + # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE] + # in POSITION control mode, safety check is not performed + external_torque_safety_check: true + external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm] +# exponential moving average time constant for external joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the external torque measurements +# set tau << robot sample time for no smoothing on the external torque measurements +external_torque_tau: 0.01 +# exponential moving average time constant for joint torque measurements [s]: +# set tau > robot sample time for more smoothing on the raw torque measurements +# set tau << robot sample time for no smoothing on the raw torque measurements +measured_torque_tau: 0.01 +# for position control, LBR works best in open_loop control mode +# note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode +open_loop: true diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 72ef8ca6..4384950c 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -25,19 +25,19 @@ lbr_ros2_control::SystemInterface - ${system_config['hardware']['fri_client_sdk']['major_version']} - ${system_config['hardware']['fri_client_sdk']['minor_version']} - ${system_config['hardware']['client_command_mode']} - ${system_config['hardware']['port_id']} - ${system_config['hardware']['remote_host']} - ${system_config['hardware']['rt_prio']} - ${system_config['hardware']['joint_position_tau']} - ${system_config['hardware']['command_guard']['variant']} - ${system_config['hardware']['state_guard']['external_torque_safety_check']} - ${system_config['hardware']['state_guard']['external_torque_limit']} - ${system_config['hardware']['external_torque_tau']} - ${system_config['hardware']['measured_torque_tau']} - ${system_config['hardware']['open_loop']} + ${system_config['fri_client_sdk']['major_version']} + ${system_config['fri_client_sdk']['minor_version']} + ${system_config['client_command_mode']} + ${system_config['port_id']} + ${system_config['remote_host']} + ${system_config['rt_prio']} + ${system_config['joint_position_tau']} + ${system_config['command_guard']['variant']} + ${system_config['state_guard']['external_torque_safety_check']} + ${system_config['state_guard']['external_torque_limit']} + ${system_config['external_torque_tau']} + ${system_config['measured_torque_tau']} + ${system_config['open_loop']} @@ -60,30 +60,6 @@ - - ${system_config['estimated_ft_sensor']['enabled']} - ${system_config['estimated_ft_sensor']['update_rate']} - ${system_config['estimated_ft_sensor']['rt_prio']} - ${system_config['estimated_ft_sensor']['chain_root']} - ${system_config['estimated_ft_sensor']['chain_tip']} - ${system_config['estimated_ft_sensor']['damping']} - ${system_config['estimated_ft_sensor']['force_x_th']} - ${system_config['estimated_ft_sensor']['force_y_th']} - ${system_config['estimated_ft_sensor']['force_z_th']} - ${system_config['estimated_ft_sensor']['torque_x_th']} - ${system_config['estimated_ft_sensor']['torque_y_th']} - ${system_config['estimated_ft_sensor']['torque_z_th']} - - - - - - - - - - @@ -123,7 +99,7 @@ ${max_velocity} ${max_torque} + value="${system_config['fri_client_sdk']['major_version'] == 1}"> diff --git a/lbr_description/ros2_control/mock_controllers.yaml b/lbr_description/ros2_control/mock_controllers.yaml new file mode 100644 index 00000000..76264c3e --- /dev/null +++ b/lbr_description/ros2_control/mock_controllers.yaml @@ -0,0 +1,48 @@ +# Use of /** so that the configurations hold for controller +# managers regardless of their namespace. Usefull in multi-robot setups. +/**/controller_manager: + ros__parameters: + update_rate: 100 + enforce_command_limits: true # can be removed from Kilted onward + + # ROS 2 control broadcasters + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + +# ROS 2 control controllers +/**/joint_trajectory_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + +/**/forward_position_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + interface_name: position diff --git a/lbr_description/test/test_urdf.py b/lbr_description/test/test_urdf.py index ffe18443..072e5ae4 100644 --- a/lbr_description/test/test_urdf.py +++ b/lbr_description/test/test_urdf.py @@ -60,7 +60,9 @@ def test_mass( @pytest.mark.parametrize("kuka_id", LBR_SPECIFICATIONS_DICT) def test_position_limits( - setup_xml_and_reference: Tuple[str, LBRSpecification], abs_tol: float = 1.0e-5 + setup_xml_and_reference: Tuple[str, LBRSpecification], + position_limit_restriction: float = 1.0, + abs_tol: float = 1.0e-5, ) -> None: xml, lbr_specification = setup_xml_and_reference urdf = URDF.from_xml_string(xml) @@ -71,7 +73,12 @@ def test_position_limits( kuka_min_position = math.radians( lbr_specification.joint_limits[joint.name].min_position ) - if not math.isclose(urdf_min_position, kuka_min_position, abs_tol=abs_tol): + if not math.isclose( + urdf_min_position + + position_limit_restriction, # note that we enforce 1 degree stricter limits to avoid hardware limits + kuka_min_position, + abs_tol=abs_tol, + ): raise ValueError( f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and joint {joint.name}." ) @@ -80,7 +87,12 @@ def test_position_limits( kuka_max_position = math.radians( lbr_specification.joint_limits[joint.name].max_position ) - if not math.isclose(urdf_max_position, kuka_max_position, abs_tol=abs_tol): + if not math.isclose( + urdf_max_position + - position_limit_restriction, # note that we enforce 1 degree stricter limits to avoid hardware limits + kuka_max_position, + abs_tol=abs_tol, + ): raise ValueError( f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and joint {joint.name}." ) diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro index f5ef3e64..3d251ed4 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -11,6 +11,9 @@ + @@ -31,5 +34,6 @@ robot_name="$(arg robot_name)" mode="$(arg mode)" system_config_path="$(arg system_config_path)" + joint_limits_path="$(arg joint_limits_path)" initial_joint_positions_path="$(arg initial_joint_positions_path)" /> - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index aa6acb9e..3d79b17a 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -3,15 +3,13 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' joint_limits_path:=^|'$(find lbr_description)/urdf/iiwa14/joint_limits.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'"> - @@ -59,6 +57,11 @@ lower="${joint_limits['A1']['lower'] * PI / 180}" upper="${joint_limits['A1']['upper'] * PI / 180}" velocity="${joint_limits['A1']['velocity'] * PI / 180}" /> + @@ -93,6 +96,11 @@ lower="${joint_limits['A2']['lower'] * PI / 180}" upper="${joint_limits['A2']['upper'] * PI / 180}" velocity="${joint_limits['A2']['velocity'] * PI / 180}" /> + @@ -127,6 +135,11 @@ lower="${joint_limits['A3']['lower'] * PI / 180}" upper="${joint_limits['A3']['upper'] * PI / 180}" velocity="${joint_limits['A3']['velocity'] * PI / 180}" /> + @@ -161,6 +174,11 @@ lower="${joint_limits['A4']['lower'] * PI / 180}" upper="${joint_limits['A4']['upper'] * PI / 180}" velocity="${joint_limits['A4']['velocity'] * PI / 180}" /> + @@ -195,6 +213,11 @@ lower="${joint_limits['A5']['lower'] * PI / 180}" upper="${joint_limits['A5']['upper'] * PI / 180}" velocity="${joint_limits['A5']['velocity'] * PI / 180}" /> + @@ -229,6 +252,11 @@ lower="${joint_limits['A6']['lower'] * PI / 180}" upper="${joint_limits['A6']['upper'] * PI / 180}" velocity="${joint_limits['A6']['velocity'] * PI / 180}" /> + @@ -263,6 +291,11 @@ lower="${joint_limits['A7']['lower'] * PI / 180}" upper="${joint_limits['A7']['upper'] * PI / 180}" velocity="${joint_limits['A7']['velocity'] * PI / 180}" /> + @@ -295,4 +328,4 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/joint_limits.yaml b/lbr_description/urdf/iiwa14/joint_limits.yaml index c101a209..e1734bd7 100644 --- a/lbr_description/urdf/iiwa14/joint_limits.yaml +++ b/lbr_description/urdf/iiwa14/joint_limits.yaml @@ -1,35 +1,49 @@ A1: - lower: -170 - upper: 170 + lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits + upper: 169 velocity: 85 effort: 200 + safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin + k_velocity: 0 A2: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 85 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A3: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 100 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A4: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 75 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A5: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 130 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A6: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 135 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A7: - lower: -175 - upper: 175 + lower: -174 + upper: 174 velocity: 135 effort: 200 + safety_position_margin: 5 + k_velocity: 0 diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index 6f63c6b0..f22d307d 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -11,6 +11,9 @@ + @@ -31,5 +34,6 @@ robot_name="$(arg robot_name)" mode="$(arg mode)" system_config_path="$(arg system_config_path)" + joint_limits_path="$(arg joint_limits_path)" initial_joint_positions_path="$(arg initial_joint_positions_path)" /> - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index 9144a786..29b86e65 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -3,15 +3,13 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' joint_limits_path:=^|'$(find lbr_description)/urdf/iiwa7/joint_limits.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'"> - @@ -59,6 +57,11 @@ lower="${joint_limits['A1']['lower'] * PI / 180}" upper="${joint_limits['A1']['upper'] * PI / 180}" velocity="${joint_limits['A1']['velocity'] * PI / 180}" /> + @@ -93,6 +96,11 @@ lower="${joint_limits['A2']['lower'] * PI / 180}" upper="${joint_limits['A2']['upper'] * PI / 180}" velocity="${joint_limits['A2']['velocity'] * PI / 180}" /> + @@ -127,6 +135,11 @@ lower="${joint_limits['A3']['lower'] * PI / 180}" upper="${joint_limits['A3']['upper'] * PI / 180}" velocity="${joint_limits['A3']['velocity'] * PI / 180}" /> + @@ -161,6 +174,11 @@ lower="${joint_limits['A4']['lower'] * PI / 180}" upper="${joint_limits['A4']['upper'] * PI / 180}" velocity="${joint_limits['A4']['velocity'] * PI / 180}" /> + @@ -195,6 +213,11 @@ lower="${joint_limits['A5']['lower'] * PI / 180}" upper="${joint_limits['A5']['upper'] * PI / 180}" velocity="${joint_limits['A5']['velocity'] * PI / 180}" /> + @@ -229,6 +252,11 @@ lower="${joint_limits['A6']['lower'] * PI / 180}" upper="${joint_limits['A6']['upper'] * PI / 180}" velocity="${joint_limits['A6']['velocity'] * PI / 180}" /> + @@ -264,6 +292,11 @@ lower="${joint_limits['A7']['lower'] * PI / 180}" upper="${joint_limits['A7']['upper'] * PI / 180}" velocity="${joint_limits['A7']['velocity'] * PI / 180}" /> + @@ -296,4 +329,4 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/joint_limits.yaml b/lbr_description/urdf/iiwa7/joint_limits.yaml index d0d1bcae..a1012e21 100644 --- a/lbr_description/urdf/iiwa7/joint_limits.yaml +++ b/lbr_description/urdf/iiwa7/joint_limits.yaml @@ -1,35 +1,49 @@ A1: - lower: -170 - upper: 170 + lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits + upper: 169 velocity: 98 effort: 200 + safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin + k_velocity: 0 A2: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 98 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A3: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 100 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A4: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 130 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A5: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 140 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A6: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 180 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A7: - lower: -175 - upper: 175 + lower: -174 + upper: 174 velocity: 180 effort: 200 + safety_position_margin: 5 + k_velocity: 0 diff --git a/lbr_description/urdf/med14/joint_limits.yaml b/lbr_description/urdf/med14/joint_limits.yaml index c101a209..e1734bd7 100644 --- a/lbr_description/urdf/med14/joint_limits.yaml +++ b/lbr_description/urdf/med14/joint_limits.yaml @@ -1,35 +1,49 @@ A1: - lower: -170 - upper: 170 + lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits + upper: 169 velocity: 85 effort: 200 + safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin + k_velocity: 0 A2: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 85 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A3: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 100 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A4: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 75 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A5: - lower: -170 - upper: 170 + lower: -169 + upper: 169 velocity: 130 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A6: - lower: -120 - upper: 120 + lower: -119 + upper: 119 velocity: 135 effort: 200 + safety_position_margin: 5 + k_velocity: 0 A7: - lower: -175 - upper: 175 + lower: -174 + upper: 174 velocity: 135 effort: 200 + safety_position_margin: 5 + k_velocity: 0 diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index c311a7c6..ed9dec7e 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -11,6 +11,9 @@ + @@ -31,5 +34,6 @@ robot_name="$(arg robot_name)" mode="$(arg mode)" system_config_path="$(arg system_config_path)" + joint_limits_path="$(arg joint_limits_path)" initial_joint_positions_path="$(arg initial_joint_positions_path)" /> - + \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index 0a2cec60..c8e95e21 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -3,15 +3,13 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' joint_limits_path:=^|'$(find lbr_description)/urdf/med14/joint_limits.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'"> - @@ -60,6 +58,11 @@ lower="${joint_limits['A1']['lower'] * PI / 180}" upper="${joint_limits['A1']['upper'] * PI / 180}" velocity="${joint_limits['A1']['velocity'] * PI / 180}" /> + @@ -94,6 +97,11 @@ lower="${joint_limits['A2']['lower'] * PI / 180}" upper="${joint_limits['A2']['upper'] * PI / 180}" velocity="${joint_limits['A2']['velocity'] * PI / 180}" /> + @@ -128,6 +136,11 @@ lower="${joint_limits['A3']['lower'] * PI / 180}" upper="${joint_limits['A3']['upper'] * PI / 180}" velocity="${joint_limits['A3']['velocity'] * PI / 180}" /> + @@ -162,6 +175,11 @@ lower="${joint_limits['A4']['lower'] * PI / 180}" upper="${joint_limits['A4']['upper'] * PI / 180}" velocity="${joint_limits['A4']['velocity'] * PI / 180}" /> + @@ -196,6 +214,11 @@ lower="${joint_limits['A5']['lower'] * PI / 180}" upper="${joint_limits['A5']['upper'] * PI / 180}" velocity="${joint_limits['A5']['velocity'] * PI / 180}" /> + @@ -230,6 +253,11 @@ lower="${joint_limits['A6']['lower'] * PI / 180}" upper="${joint_limits['A6']['upper'] * PI / 180}" velocity="${joint_limits['A6']['velocity'] * PI / 180}" /> + @@ -264,6 +292,11 @@ lower="${joint_limits['A7']['lower'] * PI / 180}" upper="${joint_limits['A7']['upper'] * PI / 180}" velocity="${joint_limits['A7']['velocity'] * PI / 180}" /> + @@ -296,4 +329,4 @@ - + \ No newline at end of file diff --git a/lbr_description/urdf/med7/joint_limits.yaml b/lbr_description/urdf/med7/joint_limits.yaml index 43687ac0..a1012e21 100644 --- a/lbr_description/urdf/med7/joint_limits.yaml +++ b/lbr_description/urdf/med7/joint_limits.yaml @@ -1,35 +1,49 @@ A1: - lower: -170 - upper: 170 - velocity: 98 - effort: 200 + lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits + upper: 169 + velocity: 98 + effort: 200 + safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin + k_velocity: 0 A2: - lower: -120 - upper: 120 - velocity: 98 - effort: 200 + lower: -119 + upper: 119 + velocity: 98 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 A3: - lower: -170 - upper: 170 - velocity: 100 - effort: 200 + lower: -169 + upper: 169 + velocity: 100 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 A4: - lower: -120 - upper: 120 - velocity: 130 - effort: 200 + lower: -119 + upper: 119 + velocity: 130 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 A5: - lower: -170 - upper: 170 - velocity: 140 - effort: 200 + lower: -169 + upper: 169 + velocity: 140 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 A6: - lower: -120 - upper: 120 - velocity: 180 - effort: 200 + lower: -119 + upper: 119 + velocity: 180 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 A7: - lower: -175 - upper: 175 - velocity: 180 - effort: 200 + lower: -174 + upper: 174 + velocity: 180 + effort: 200 + safety_position_margin: 5 + k_velocity: 0 diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index df73e656..0903c58b 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -11,6 +11,9 @@ + @@ -31,5 +34,6 @@ robot_name="$(arg robot_name)" mode="$(arg mode)" system_config_path="$(arg system_config_path)" + joint_limits_path="$(arg joint_limits_path)" initial_joint_positions_path="$(arg initial_joint_positions_path)" /> - + \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 06f6d9d5..caae9f06 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -3,15 +3,13 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' joint_limits_path:=^|'$(find lbr_description)/urdf/med7/joint_limits.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'"> - @@ -60,6 +58,11 @@ lower="${joint_limits['A1']['lower'] * PI / 180}" upper="${joint_limits['A1']['upper'] * PI / 180}" velocity="${joint_limits['A1']['velocity'] * PI / 180}" /> + @@ -94,6 +97,11 @@ lower="${joint_limits['A2']['lower'] * PI / 180}" upper="${joint_limits['A2']['upper'] * PI / 180}" velocity="${joint_limits['A2']['velocity'] * PI / 180}" /> + @@ -128,6 +136,11 @@ lower="${joint_limits['A3']['lower'] * PI / 180}" upper="${joint_limits['A3']['upper'] * PI / 180}" velocity="${joint_limits['A3']['velocity'] * PI / 180}" /> + @@ -162,6 +175,11 @@ lower="${joint_limits['A4']['lower'] * PI / 180}" upper="${joint_limits['A4']['upper'] * PI / 180}" velocity="${joint_limits['A4']['velocity'] * PI / 180}" /> + @@ -196,6 +214,11 @@ lower="${joint_limits['A5']['lower'] * PI / 180}" upper="${joint_limits['A5']['upper'] * PI / 180}" velocity="${joint_limits['A5']['velocity'] * PI / 180}" /> + @@ -230,6 +253,11 @@ lower="${joint_limits['A6']['lower'] * PI / 180}" upper="${joint_limits['A6']['upper'] * PI / 180}" velocity="${joint_limits['A6']['velocity'] * PI / 180}" /> + @@ -264,6 +292,11 @@ lower="${joint_limits['A7']['lower'] * PI / 180}" upper="${joint_limits['A7']['upper'] * PI / 180}" velocity="${joint_limits['A7']['velocity'] * PI / 180}" /> + @@ -296,4 +329,4 @@ - + \ No newline at end of file diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 617b1f9e..45465a6b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -36,10 +36,10 @@ add_library(lbr_fri_ros2 src/async_client.cpp src/control.cpp src/filters.cpp - src/ft_estimator.cpp src/kinematics.cpp src/math.cpp src/worker.cpp + src/wrench_estimator.cpp ) target_include_directories(lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index 0a196e2b..42052770 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -43,6 +43,9 @@ class BaseCommandInterface { void log_info() const; +protected: + void neutralize_command_(const_idl_state_t_ref state, idl_command_t_ref command); + protected: mutable std::mutex command_mutex_; bool command_initialized_; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/math.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/math.hpp index 6752b2fb..74f48a6d 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/math.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/math.hpp @@ -3,8 +3,12 @@ #include +#include "lbr_fri_ros2/types.hpp" + namespace lbr_fri_ros2 { bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const double &max); bool norm_in_bounds(const std::array &vec, const double &max); +bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower, + const_jnt_array_t_ref upper); } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__MATH_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/wrench_estimator.hpp similarity index 53% rename from lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/wrench_estimator.hpp index 8243a85b..8ec41bf4 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/wrench_estimator.hpp @@ -1,43 +1,53 @@ -#ifndef LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ -#define LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#ifndef LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ +#define LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ #include #include #include #include #include -#include #include "eigen3/Eigen/Core" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/utilities.hpp" -#include "friLBRState.h" - -#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" #include "lbr_fri_ros2/types.hpp" -#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class FTEstimatorImpl { +struct WrenchEstimatorParameters { + std::string chain_root{"lbr_link_0"}; + std::string chain_tip{"lbr_link_ee"}; + double damping{0.2}; + double force_x_th{2.0}; + double force_y_th{2.0}; + double force_z_th{2.0}; + double torque_x_th{0.5}; + double torque_y_th{0.5}; + double torque_z_th{0.5}; + + bool valid() const { + return !(chain_root.empty() || chain_tip.empty() || damping < 0.0 || force_x_th < 0.0 || + force_y_th < 0.0 || force_z_th < 0.0 || torque_x_th < 0.0 || torque_y_th < 0.0 || + torque_z_th < 0.0); + } +}; + +class WrenchEstimator { /** - * @brief A class to estimate force-torques from external joint torque readings. Note that only + * @brief A class to estimate wrenches from external joint torque readings. Note that only * forces beyond a specified threshold are returned. The specified threshold is removed from the * estimated force-torque. * */ protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimatorImpl"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::WrenchEstimator"; public: - FTEstimatorImpl(const std::string &robot_description, - const std::string &chain_root = "lbr_link_0", - const std::string &chain_tip = "lbr_link_ee", - const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, - const double &damping = 0.2); + WrenchEstimator() = delete; + WrenchEstimator(const std::string &robot_description, + const WrenchEstimatorParameters ¶meters = WrenchEstimatorParameters()); void compute(); void reset(); @@ -52,6 +62,8 @@ class FTEstimatorImpl { } inline void set_q(const_jnt_array_t_ref q) { q_ = q; } + void log_info() const; + protected: // force threshold cart_array_t f_ext_th_; @@ -70,23 +82,5 @@ class FTEstimatorImpl { Eigen::Matrix tau_ext_; Eigen::Matrix f_ext_raw_, f_ext_, f_ext_tf_; }; - -class FTEstimator : public Worker { - /** - * @brief A simple class to run the FTEstimatorImpl asynchronously at a specified update rate. - * - */ -public: - FTEstimator(const std::shared_ptr ft_estimator, - const std::uint16_t &update_rate = 100); - - inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::FTEstimator"; }; - -protected: - void perform_work_() override; - - std::shared_ptr ft_estimator_impl_ptr_; - std::uint16_t update_rate_; -}; } // namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#endif // LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index a3921538..2cdfce1b 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 2.3.0 + 2.4.3 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp index 30e61cbb..297cabb1 100644 --- a/lbr_fri_ros2/src/control.cpp +++ b/lbr_fri_ros2/src/control.cpp @@ -101,10 +101,10 @@ void AdmittanceImpl::log_info() const { void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { // clip velocity - twist_target_.head(3).unaryExpr([&](double v) { + twist_target_.head(3) = twist_target_.head(3).unaryExpr([&](double v) { return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity); }); - twist_target_.tail(3).unaryExpr([&](double v) { + twist_target_.tail(3) = twist_target_.tail(3).unaryExpr([&](double v) { return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); }); diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp deleted file mode 100644 index 6377a9f3..00000000 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "lbr_fri_ros2/ft_estimator.hpp" - -namespace lbr_fri_ros2 { -FTEstimatorImpl::FTEstimatorImpl(const std::string &robot_description, - const std::string &chain_root, const std::string &chain_tip, - const_cart_array_t_ref f_ext_th, const double &damping) - : f_ext_th_(f_ext_th), damping_(damping) { - kinematics_ptr_ = std::make_unique(robot_description, chain_root, chain_tip); - reset(); -} - -void FTEstimatorImpl::compute() { - auto jacobian = kinematics_ptr_->compute_jacobian(q_); - jacobian_inv_ = pinv(jacobian.data, damping_); - f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; - int i = -1; - f_ext_ = f_ext_raw_.unaryExpr([&](double v) { - ++i; - if (std::abs(v) < f_ext_th_[i]) { - return 0.; - } else { - return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); - } - }); - - // rotate into chain tip frame - auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); - f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); - f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); -} - -void FTEstimatorImpl::reset() { - std::fill(q_.begin(), q_.end(), 0.0); - tau_ext_.setZero(); - f_ext_raw_.setZero(); - f_ext_.setZero(); - f_ext_tf_.setZero(); - jacobian_inv_.setZero(); -} - -FTEstimator::FTEstimator(const std::shared_ptr ft_estimator_impl_ptr, - const std::uint16_t &update_rate) - : ft_estimator_impl_ptr_(ft_estimator_impl_ptr), update_rate_(update_rate) {} - -void FTEstimator::perform_work_() { - running_ = true; - while (rclcpp::ok() && !should_stop_) { - auto start = std::chrono::high_resolution_clock::now(); - ft_estimator_impl_ptr_->compute(); - std::this_thread::sleep_until(start + std::chrono::nanoseconds(static_cast( - 1.e9 / static_cast(update_rate_)))); - } -}; -} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index 15186f78..266b4772 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -10,9 +10,7 @@ BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, }; void BaseCommandInterface::init_command(const_idl_state_t_ref state) { - command_target_.joint_position = state.measured_joint_position; - command_target_.torque.fill(0.); - command_target_.wrench.fill(0.); + neutralize_command_(state, command_target_); command_ = command_target_; command_initialized_ = true; } @@ -21,4 +19,11 @@ void BaseCommandInterface::log_info() const { command_guard_->log_info(); joint_position_filter_.log_info(); } + +void BaseCommandInterface::neutralize_command_(const_idl_state_t_ref state, + idl_command_t_ref command) { + command.joint_position = state.measured_joint_position; + command.torque.fill(0.); + command.wrench.fill(0.); +} } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 38a93e59..d476da48 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -12,7 +12,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command #if FRI_CLIENT_VERSION_MAJOR == 1 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = - "Client side (configured via hardware.client_command_mode in lbr_system_config.yaml) " + "Client side (configured via client_command_mode in lbr_system_config.yaml) " "expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + "' command mode, but robot was in '" + @@ -28,7 +28,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command #if FRI_CLIENT_VERSION_MAJOR >= 2 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { std::string err = - "Client side (configured via hardware.client_command_mode in lbr_system_config.yaml) " + "Client side (configured via client_command_mode in lbr_system_config.yaml) " "expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) + " command mode, but robot was in '" + @@ -68,10 +68,10 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command // validate if (!command_guard_->is_valid_command(command_, state)) { - std::string err = "Invalid command."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); + std::string warn = "Overriding invalid command to neutral command."; + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::WARNING << warn.c_str() << ColorScheme::ENDC); + neutralize_command_(state, command_); } // write joint position to output diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b6c97363..c6afcd4a 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -11,7 +11,7 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, std::lock_guard lock(command_mutex_); if (state.client_command_mode != KUKA::FRI::EClientCommandMode::TORQUE) { std::string err = - "Client side (configured via hardware.client_command_mode in lbr_system_config.yaml) " + "Client side (configured via client_command_mode in lbr_system_config.yaml) " "expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::TORQUE) + "' command mode, but robot was in '" + @@ -54,10 +54,10 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, // validate if (!command_guard_->is_valid_command(command_, state)) { - std::string err = "Invalid command."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); + std::string warn = "Overriding invalid command to neutral command."; + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::WARNING << warn.c_str() << ColorScheme::ENDC); + neutralize_command_(state, command_); } // write joint position and torque to output command.setJointPosition(command_.joint_position.data()); diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 43d3bd92..35bd4f94 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -11,7 +11,7 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, std::lock_guard lock(command_mutex_); if (state.client_command_mode != KUKA::FRI::EClientCommandMode::WRENCH) { std::string err = - "Client side (configured via hardware.client_command_mode in lbr_system_config.yaml) " + "Client side (configured via client_command_mode in lbr_system_config.yaml) " "expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::WRENCH) + "' command mode, but robot was in '" + @@ -54,10 +54,10 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, // validate if (!command_guard_->is_valid_command(command_, state)) { - std::string err = "Invalid command."; - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), - ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); - throw std::runtime_error(err); + std::string warn = "Overriding invalid command to neutral command."; + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::WARNING << warn.c_str() << ColorScheme::ENDC); + neutralize_command_(state, command_); } // write joint position and wrench to output diff --git a/lbr_fri_ros2/src/math.cpp b/lbr_fri_ros2/src/math.cpp index 500823e7..ccef6ba2 100644 --- a/lbr_fri_ros2/src/math.cpp +++ b/lbr_fri_ros2/src/math.cpp @@ -8,4 +8,13 @@ bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const bool norm_in_bounds(const std::array &vec, const double &max) { return norm_in_bounds(vec[0], vec[1], vec[2], max); } +bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower, + const_jnt_array_t_ref upper) { + for (std::size_t i = 0; i < vals.size(); ++i) { + if (vals[i] < lower[i] || upper[i] < vals[i]) { + return false; + } + } + return true; +} } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/wrench_estimator.cpp b/lbr_fri_ros2/src/wrench_estimator.cpp new file mode 100644 index 00000000..589c3190 --- /dev/null +++ b/lbr_fri_ros2/src/wrench_estimator.cpp @@ -0,0 +1,58 @@ +#include "lbr_fri_ros2/wrench_estimator.hpp" + +namespace lbr_fri_ros2 { +WrenchEstimator::WrenchEstimator(const std::string &robot_description, + const WrenchEstimatorParameters ¶meters) { + if (!parameters.valid()) { + throw std::invalid_argument("Invalid wrench estimator parameters."); + } + f_ext_th_[0] = parameters.force_x_th; + f_ext_th_[1] = parameters.force_y_th; + f_ext_th_[2] = parameters.force_z_th; + f_ext_th_[3] = parameters.torque_x_th; + f_ext_th_[4] = parameters.torque_y_th; + f_ext_th_[5] = parameters.torque_z_th; + damping_ = parameters.damping; + kinematics_ptr_ = + std::make_unique(robot_description, parameters.chain_root, parameters.chain_tip); + reset(); +} + +void WrenchEstimator::compute() { + auto jacobian = kinematics_ptr_->compute_jacobian(q_); + jacobian_inv_ = pinv(jacobian.data, damping_); + f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; + int i = -1; + f_ext_ = f_ext_raw_.unaryExpr([&](double v) { + ++i; + if (std::abs(v) < f_ext_th_[i]) { + return 0.; + } else { + return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); + } + }); + + // rotate into chain tip frame + auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); + f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); + f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); +} + +void WrenchEstimator::reset() { + std::fill(q_.begin(), q_.end(), 0.0); + tau_ext_.setZero(); + f_ext_raw_.setZero(); + f_ext_.setZero(); + f_ext_tf_.setZero(); + jacobian_inv_.setZero(); +} + +void WrenchEstimator::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", damping_); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Force thresholds: [%.3f, %.3f, %.3f]", + f_ext_th_[0], f_ext_th_[1], f_ext_th_[2]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Torque thresholds: [%.3f, %.3f, %.3f]", + f_ext_th_[3], f_ext_th_[4], f_ext_th_[5]); +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 486d1877..31c2cc94 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 2.3.0 + 2.4.3 ROS 2 stack for KUKA LBRs. mhubii Apache-2.0 diff --git a/lbr_fri_ros2_stack/repos-fri-1.11.yaml b/lbr_fri_ros2_stack/repos-fri-1.11.yaml index 2527d70f..b6e53a1a 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.11.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.11.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-1.14.yaml b/lbr_fri_ros2_stack/repos-fri-1.14.yaml index 74fda90e..b41fa3db 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.14.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.14.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-1.15.yaml b/lbr_fri_ros2_stack/repos-fri-1.15.yaml index a31b52b4..346a77a1 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.15.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.15.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-1.16.yaml b/lbr_fri_ros2_stack/repos-fri-1.16.yaml index cc20cdd6..a37fafe3 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.16.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.16.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-2.5.yaml b/lbr_fri_ros2_stack/repos-fri-2.5.yaml index 1280b6f6..dc254066 100644 --- a/lbr_fri_ros2_stack/repos-fri-2.5.yaml +++ b/lbr_fri_ros2_stack/repos-fri-2.5.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-2.6.yaml b/lbr_fri_ros2_stack/repos-fri-2.6.yaml index 927216ff..48e2bb0c 100644 --- a/lbr_fri_ros2_stack/repos-fri-2.6.yaml +++ b/lbr_fri_ros2_stack/repos-fri-2.6.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml index e04b513e..8f05e148 100644 --- a/lbr_fri_ros2_stack/repos-fri-2.7.yaml +++ b/lbr_fri_ros2_stack/repos-fri-2.7.yaml @@ -10,4 +10,4 @@ repositories: lbr_fri_ros2_stack: type: git url: https://github.com/lbr-stack/lbr_fri_ros2_stack - version: rolling + version: jazzy diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 50d95fb3..6f565e3b 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -86,11 +86,11 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. Manual changes: - #. Manually add acceleration limits in `joint_limits.yaml `_:octicon:`link-external` (not supported in ``URDF``) + #. Manually add acceleration limits in `joint_limits.yaml `_:octicon:`link-external` (not supported in ``URDF``) - #. In the `move_group.launch.py `_:octicon:`link-external` use the robot descriotion from ``lbr_description`` + #. In the `move_group.launch.py `_:octicon:`link-external` use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` + #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `hardware_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 837382d2..4967f7c8 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(controller_interface REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(kinematics_interface REQUIRED) find_package(lbr_fri_idl REQUIRED) @@ -32,6 +33,7 @@ add_library( ${PROJECT_NAME} SHARED src/controllers/admittance_controller.cpp + src/controllers/estimated_wrench_interface.cpp src/controllers/lbr_joint_position_command_controller.cpp src/controllers/lbr_torque_command_controller.cpp src/controllers/lbr_wrench_command_controller.cpp @@ -54,6 +56,7 @@ target_include_directories( set(AMENT_DEPENDENCIES controller_interface Eigen3 + geometry_msgs hardware_interface kinematics_interface lbr_fri_idl diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 781be29e..4fd09c24 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -3,7 +3,7 @@ lbr_ros2_control .. note:: - Refer to :ref:`lbr_demos` for exemplary usage - - Also refer to the official `ros2_control `_:octicon:`link-external` documentation + - Also refer to the official `ros2_control `_:octicon:`link-external` documentation .. contents:: Table of Contents :depth: 2 @@ -23,14 +23,14 @@ A simplified overview of ``lbr_ros2_control`` is shown :ref:`below `_:octicon:`link-external`. +New starters are often confused by ``ros2_control``. Everything in ``ros2_control`` is a plugin, refer `pluginlib `_:octicon:`link-external`. Hardware components and controllers are loaded as plugins (components) by the ``controller_manager::ControllerManager`` during runtime. Therefore, hardware and controllers communicate over shared memory in the same process, **not** topics! The ``controller_manager::ControllerManager`` has a node, the `controller_manager `_:octicon:`link-external`. - Hardware plugins are read from the ``robot_descritption`` parameter of the ``robot_state_publisher`` node and loaded at runtime. -- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. +- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `hardware_controllers.yaml `_:octicon:`link-external`. Hardware Plugin --------------- @@ -42,8 +42,8 @@ The ``lbr_ros2_control::SystemInterface`` plugin implements a ``hardware_interfa .. thumbnail:: img/lbr_ros2_control_detailed_v2.0.0.svg :alt: lbr_ros2_control_detailed -- The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external` -- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_config.yaml `_:octicon:`link-external` +- The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external` +- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_config.yaml `_:octicon:`link-external` The ``lbr_ros2_control::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time. diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp index 82498f4a..4627a525 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -49,40 +49,48 @@ class AdmittanceController : public controller_interface::ControllerInterface { on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_state_interfaces_(); - void clear_state_interfaces_(); + bool assign_state_interfaces_(); + void release_state_interfaces_(); void configure_joint_names_(); void configure_admittance_impl_(); void configure_inv_jac_ctrl_impl_(); void configure_filters_(); + void configure_safety_checks_(); void zero_all_values_(); void init_filters_with_update_rate_(); bool any_external_force_torques_on_horizon_( + const double &max_external_force = 0., const double &max_external_torque = 0., const std::chrono::milliseconds &horizon = std::chrono::milliseconds(200)) const; void log_info_() const; + // safety checks + double max_external_force_on_activate_{0.}; + double max_external_torque_on_activate_{0.}; + // admittance bool initialized_ = false; std::unique_ptr admittance_impl_ptr_; Eigen::Matrix t_init_, t_, t_prev_; // translation Eigen::Quaterniond r_init_, r_, r_prev_; // rotation - Eigen::Matrix f_ext_, delta_x_, dx_, ddx_; + Eigen::Matrix f_ext_, f_ext_filtered_, delta_x_, dx_, + ddx_; + + // external force smoothing + std::unique_ptr> + f_ext_filter_ptr_; // joint veloctiy computation std::unique_ptr inv_jac_ctrl_impl_ptr_; lbr_fri_ros2::jnt_array_t q_, dq_; Eigen::Matrix twist_command_; - // velocity command smoothing - lbr_fri_ros2::jnt_array_t dq_filtered_; - std::unique_ptr> dq_filter_ptr_; - // interfaces lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_state_interfaces_; std::unique_ptr> session_state_interface_ptr_; + std::string ft_sensor_name_; std::unique_ptr estimated_ft_sensor_ptr_; }; } // namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/estimated_wrench_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/estimated_wrench_interface.hpp new file mode 100644 index 00000000..d58c599f --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/estimated_wrench_interface.hpp @@ -0,0 +1,80 @@ +#ifndef LBR_ROS2_CONTROL__ESTIMATED_WRENCH_INTERFACE_HPP_ +#define LBR_ROS2_CONTROL__ESTIMATED_WRENCH_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/types.hpp" +#include "lbr_fri_ros2/wrench_estimator.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +class EstimatedWrenchInterface : public controller_interface::ChainableControllerInterface { +public: + EstimatedWrenchInterface(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + +protected: + std::vector on_export_state_interfaces() override; + std::vector on_export_reference_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; + + controller_interface::return_type + update_reference_from_subscribers(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + controller_interface::return_type + update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + +protected: + bool assign_state_interfaces_(); + void release_state_interfaces_(); + void configure_joint_names_(); + void configure_parameters_(); + bool read_state_interfaces_(); + bool valid_states_() const; + void nan_wrench_(); + void nan_referenced_states_(); + void estimate_wrench_(); + void log_info_() const; + + // force-torque estimation + lbr_fri_ros2::WrenchEstimatorParameters wrench_estimator_parameters_; + std::unique_ptr wrench_estimator_ptr_; + lbr_fri_ros2::cart_array_t wrench_; + + // joint names + lbr_fri_ros2::jnt_name_array_t joint_names_; + + // referenced by state interfaces + lbr_fri_ros2::jnt_array_t joint_positions_, external_torques_; + + // state interfaces (used by this controller to estimate wrenches) + std::vector> + joint_position_state_interfaces_, external_torque_state_interfaces_; +}; +} // namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__ESTIMATED_WRENCH_INTERFACE_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp index 4c2e8847..88ba9fbe 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp @@ -54,6 +54,7 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { lbr_fri_ros2::jnt_name_array_t joint_names_; std::unordered_map> state_interface_map_; + lbr_fri_idl::msg::LBRState lbr_state_; rclcpp::Publisher::SharedPtr state_publisher_ptr_; std::shared_ptr> rt_state_publisher_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp index 6310311d..7a7251a5 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp @@ -44,8 +44,8 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_command_interfaces_(); - void clear_command_interfaces_(); + bool assign_command_interfaces_(); + void release_command_interfaces_(); void configure_joint_names_(); lbr_fri_ros2::jnt_name_array_t joint_names_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp index 3b4e38af..a56fc66d 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp @@ -10,7 +10,6 @@ #include #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" @@ -47,12 +46,12 @@ class LBRWrenchCommandController : public controller_interface::ChainableControl std::vector on_export_reference_interfaces() override; bool on_set_chained_mode(bool chained_mode) override; - // expect full lbr_wrench command in this mode.... + // expect full lbr_wrench command in this mode controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time &time, const rclcpp::Duration &period) override; - // expect just wrench command in this mode.... + // expect just wrench command in this mode controller_interface::return_type update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override; @@ -66,10 +65,10 @@ class LBRWrenchCommandController : public controller_interface::ChainableControl on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_state_interfaces_(); - bool reference_command_interfaces_(); - void clear_state_interfaces_(); - void clear_command_interfaces_(); + bool assign_state_interfaces_(); + bool assign_command_interfaces_(); + void release_state_interfaces_(); + void release_command_interfaces_(); void configure_joint_names_(); void configure_parameters_(); bool zero_wrench_commands_(); @@ -91,7 +90,7 @@ class LBRWrenchCommandController : public controller_interface::ChainableControl lbr_fri_ros2::jnt_array_t joint_position_states_; lbr_fri_ros2::jnt_array_t joint_velocity_states_; - // state interfaces, consider access to external force interface for safety checking.... + // state interfaces std::vector> joint_position_state_interfaces_, joint_velocity_state_interfaces_; @@ -101,12 +100,12 @@ class LBRWrenchCommandController : public controller_interface::ChainableControl // in external mode, wrench and joint position are commanded realtime_tools::RealtimeBuffer - rt_lbr_wrench_command_ptr_; + lbr_wrench_command_rt_buffer_; rclcpp::Subscription::SharedPtr lbr_wrench_command_subscription_ptr_; // in chained mode, only wrench is commanded - realtime_tools::RealtimeBuffer rt_wrench_command_ptr_; + realtime_tools::RealtimeBuffer wrench_command_rt_buffer_; rclcpp::Subscription::SharedPtr wrench_command_subscription_ptr_; }; } // namespace lbr_ros2_control diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp index 46c5cbc3..e025de65 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -21,6 +21,7 @@ #include "lbr_fri_ros2/control.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/math.hpp" #include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" @@ -48,11 +49,12 @@ class TwistController : public controller_interface::ControllerInterface { on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_state_interfaces_(); - void clear_state_interfaces_(); + bool assign_state_interfaces_(); + void release_state_interfaces_(); void reset_command_buffer_(); void zero_joint_velocity_command_(); void configure_joint_names_(); + void configure_joint_limits_(); void configure_inv_jac_ctrl_impl_(); void log_info_() const; @@ -62,10 +64,11 @@ class TwistController : public controller_interface::ControllerInterface { // joint veloctiy computation std::unique_ptr inv_jac_ctrl_impl_ptr_; - lbr_fri_ros2::jnt_array_t q_, dq_; + lbr_fri_ros2::jnt_array_t q_, q_target_, dq_; // interfaces lbr_fri_ros2::jnt_name_array_t joint_names_; + lbr_fri_ros2::jnt_array_t lower_joint_limits_, upper_joint_limits_; std::vector> joint_position_state_interfaces_; std::unique_ptr> diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index a758ab12..54e98da1 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -24,7 +24,6 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/formatting.hpp" -#include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/guards/command_guard.hpp" #include "lbr_fri_ros2/guards/state_guard.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" @@ -34,7 +33,7 @@ namespace lbr_ros2_control { class SystemInterface : public hardware_interface::SystemInterface { protected: - struct SystemInterfaceParameters { + struct Parameters { uint8_t fri_client_sdk_major_version{1}; uint8_t fri_client_sdk_minor_version{15}; #if FRI_CLIENT_VERSION_MAJOR == 1 @@ -56,21 +55,6 @@ class SystemInterface : public hardware_interface::SystemInterface { bool open_loop{true}; }; - struct EstimatedFTSensorParameters { - bool enabled{true}; - std::uint16_t update_rate{100}; - int32_t rt_prio{30}; - std::string chain_root{"lbr_link_0"}; - std::string chain_tip{"lbr_link_ee"}; - double damping{0.2}; - double force_x_th{2.0}; - double force_y_th{2.0}; - double force_z_th{2.0}; - double torque_x_th{0.5}; - double torque_y_th{0.5}; - double torque_z_th{0.5}; - }; - struct CommandKeys { lbr_fri_ros2::jnt_name_array_t joint_position, torque; lbr_fri_ros2::cart_name_array_t wrench; @@ -99,7 +83,6 @@ class SystemInterface : public hardware_interface::SystemInterface { std::string sample_time, session_state, connection_quality, safety_state, operation_mode, drive_state, client_command_mode, overlay_type, control_mode, time_stamp_sec, time_stamp_nano_sec, tracking_performance; - lbr_fri_ros2::cart_name_array_t estimated_ft; void populate_keys(const hardware_interface::HardwareInfo &info) { for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { @@ -127,13 +110,6 @@ class SystemInterface : public hardware_interface::SystemInterface { time_stamp_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_SEC; time_stamp_nano_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_NANO_SEC; tracking_performance = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TRACKING_PERFORMANCE; - - estimated_ft[0] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_X; - estimated_ft[1] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Y; - estimated_ft[2] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Z; - estimated_ft[3] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_X; - estimated_ft[4] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Y; - estimated_ft[5] = std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Z; } }; @@ -145,9 +121,8 @@ class SystemInterface : public hardware_interface::SystemInterface { static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6; #endif static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; - static constexpr uint8_t LBR_FRI_SENSORS = 2; + static constexpr uint8_t LBR_FRI_SENSORS = 1; static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; - static constexpr uint8_t ESTIMATED_FT_SENSOR_SIZE = 6; static constexpr uint8_t GPIO_SIZE = 1; public: @@ -178,7 +153,6 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: // setup bool parse_parameters_(); - bool parse_ft_parameters_(); void nan_command_interfaces_(); void nan_state_interfaces_(); bool verify_number_of_joints_(); @@ -186,7 +160,6 @@ class SystemInterface : public hardware_interface::SystemInterface { bool verify_joint_state_interfaces_(); bool verify_sensors_(); bool verify_auxiliary_sensor_(); - bool verify_estimated_ft_sensor_(); bool verify_gpios_(); // monitor end of commanding active @@ -194,8 +167,7 @@ class SystemInterface : public hardware_interface::SystemInterface { const KUKA::FRI::ESessionState &session_state); // robot parameters - SystemInterfaceParameters parameters_; - EstimatedFTSensorParameters ft_parameters_; + Parameters parameters_; // robot driver std::shared_ptr async_client_ptr_; @@ -213,11 +185,6 @@ class SystemInterface : public hardware_interface::SystemInterface { void update_last_states_(); void compute_velocity_(); - // additional force-torque state interface - lbr_fri_ros2::cart_array_t ft_; - std::shared_ptr ft_estimator_impl_ptr_; - std::unique_ptr ft_estimator_ptr_; - // command and state buffers lbr_fri_idl::msg::LBRCommand lbr_command_; lbr_fri_idl::msg::LBRState lbr_state_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp index bb750426..23b109bf 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp @@ -37,6 +37,5 @@ constexpr char HW_IF_TORQUE_Z[] = "torque.z"; // additional LBR command interfaces, reference KUKA::FRI::LBRCommand constexpr char HW_IF_WRENCH_PREFIX[] = "wrench"; constexpr char HW_IF_AUXILIARY_PREFIX[] = "auxiliary_sensor"; -constexpr char HW_IF_ESTIMATED_FT_PREFIX[] = "estimated_ft_sensor"; } // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 3e1cc8af..7c7e3ce0 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control - 2.3.0 + 2.4.3 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii Apache-2.0 @@ -12,8 +12,11 @@ eigen + controller_interface fri_client_sdk geometry_msgs + hardware_interface + kinematics_interface lbr_fri_idl lbr_fri_ros2 pluginlib diff --git a/lbr_ros2_control/plugin_description_files/controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml index 4b498489..2e4f2fe3 100644 --- a/lbr_ros2_control/plugin_description_files/controllers.xml +++ b/lbr_ros2_control/plugin_description_files/controllers.xml @@ -7,6 +7,14 @@ A simple admittance controller. + + + Estimates external wrenches using the external joint torques and exposes them + as interface. + + get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("ft_sensor_name", "estimated_wrench_interface"); this->get_node()->declare_parameter("admittance.mass", std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); this->get_node()->declare_parameter("admittance.damping", @@ -52,11 +53,14 @@ controller_interface::CallbackReturn AdmittanceController::on_init() { std::vector(lbr_fri_ros2::N_JNTS, 0.0)); this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); - this->get_node()->declare_parameter("filter.joint_velocity_tau", 0.4); + this->get_node()->declare_parameter("filter.f_ext_tau", 0.4); + this->get_node()->declare_parameter("on_activate.max_external_force", 0.0); + this->get_node()->declare_parameter("on_activate.max_external_torque", 0.0); configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); configure_filters_(); + configure_safety_checks_(); log_info_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), @@ -127,11 +131,15 @@ AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Durati f_ext_.head(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.head(3); f_ext_.tail(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.tail(3); + // filter the external forces + f_ext_filter_ptr_->compute(f_ext_.data(), f_ext_filtered_.data()); + // compute admittance - admittance_impl_ptr_->compute(f_ext_, delta_x_, dx_, ddx_); + admittance_impl_ptr_->compute(f_ext_filtered_, delta_x_, dx_, ddx_); // integrate ddx_ to command velocity - twist_command_ = ddx_ * dt; + dx_ += ddx_ * dt; + twist_command_ = dx_; if (!inv_jac_ctrl_impl_ptr_) { RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); @@ -153,12 +161,9 @@ AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Durati // compute the joint velocity from the twist command target inv_jac_ctrl_impl_ptr_->compute(twist_command_, q_, dq_); - // filter the joint velocities - dq_filter_ptr_->compute(dq_.data(), dq_filtered_.data()); - // pass joint positions to hardware for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - if (!this->command_interfaces_[i].set_value(q_[i] + dq_filtered_[i] * dt)) { + if (!this->command_interfaces_[i].set_value(q_[i] + dq_[i] * dt)) { RCLCPP_ERROR_STREAM(this->get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR << "Failed to set joint position for joint '" << joint_names_[i] @@ -171,31 +176,33 @@ AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Durati controller_interface::CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + ft_sensor_name_ = this->get_node()->get_parameter("ft_sensor_name").as_string(); estimated_ft_sensor_ptr_ = std::make_unique( - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_X, - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Y, - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Z, - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_X, - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Y, - std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Z); + ft_sensor_name_ + "/" + HW_IF_FORCE_X, ft_sensor_name_ + "/" + HW_IF_FORCE_Y, + ft_sensor_name_ + "/" + HW_IF_FORCE_Z, ft_sensor_name_ + "/" + HW_IF_TORQUE_X, + ft_sensor_name_ + "/" + HW_IF_TORQUE_Y, ft_sensor_name_ + "/" + HW_IF_TORQUE_Z); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - if (!reference_state_interfaces_()) { + initialized_ = false; + if (!assign_state_interfaces_()) { + release_state_interfaces_(); return controller_interface::CallbackReturn::ERROR; } init_filters_with_update_rate_(); zero_all_values_(); try { - if (any_external_force_torques_on_horizon_()) { + if (any_external_force_torques_on_horizon_(max_external_force_on_activate_, + max_external_torque_on_activate_)) { RCLCPP_ERROR_STREAM( this->get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR << "External force-torques detected during admittance controller activation. " "Please make sure load data was calibrated." << lbr_fri_ros2::ColorScheme::ENDC); + release_state_interfaces_(); return controller_interface::CallbackReturn::ERROR; } } catch (const std::exception &e) { @@ -205,6 +212,7 @@ AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_sta << "Failed to check external force-torques during admittance controller activation " "with: " << e.what() << lbr_fri_ros2::ColorScheme::ENDC); + release_state_interfaces_(); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -212,14 +220,14 @@ AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_sta controller_interface::CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_state_interfaces_(); + release_state_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool AdmittanceController::reference_state_interfaces_() { +bool AdmittanceController::assign_state_interfaces_() { for (auto &state_interface : state_interfaces_) { if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + joint_position_state_interfaces_.push_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { session_state_interface_ptr_ = @@ -243,8 +251,9 @@ bool AdmittanceController::reference_state_interfaces_() { return true; } -void AdmittanceController::clear_state_interfaces_() { +void AdmittanceController::release_state_interfaces_() { joint_position_state_interfaces_.clear(); + session_state_interface_ptr_.reset(); estimated_ft_sensor_ptr_->release_interfaces(); } @@ -252,12 +261,12 @@ void AdmittanceController::configure_joint_names_() { if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + "Number of joint names '%ld' does not match the number of joints in the robot '%d'.", joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } @@ -266,8 +275,8 @@ void AdmittanceController::configure_admittance_impl_() { if (this->get_node()->get_parameter("admittance.mass").as_double_array().size() != lbr_fri_ros2::CARTESIAN_DOF) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Number of mass values (%ld) does not match the number of cartesian degrees of " - "freedom (%d).", + "Number of mass values '%ld' does not match the number of cartesian degrees of " + "freedom '%d'.", this->get_node()->get_parameter("admittance.mass").as_double_array().size(), lbr_fri_ros2::CARTESIAN_DOF); throw std::runtime_error("Failed to configure admittance parameters."); @@ -276,8 +285,8 @@ void AdmittanceController::configure_admittance_impl_() { lbr_fri_ros2::CARTESIAN_DOF) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of damping values (%ld) does not match the number of cartesian degrees of freedom " - "(%d).", + "Number of damping values '%ld' does not match the number of cartesian degrees of freedom " + "'%d'.", this->get_node()->get_parameter("admittance.damping").as_double_array().size(), lbr_fri_ros2::CARTESIAN_DOF); throw std::runtime_error("Failed to configure admittance parameters."); @@ -285,23 +294,23 @@ void AdmittanceController::configure_admittance_impl_() { if (this->get_node()->get_parameter("admittance.stiffness").as_double_array().size() != lbr_fri_ros2::CARTESIAN_DOF) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Number of stiffness values (%ld) does not match the number of cartesian degrees " + "Number of stiffness values '%ld' does not match the number of cartesian degrees " "of freedom " - "(%d).", + "'%d'.", this->get_node()->get_parameter("admittance.stiffness").as_double_array().size(), lbr_fri_ros2::CARTESIAN_DOF); throw std::runtime_error("Failed to configure admittance parameters."); } lbr_fri_ros2::cart_array_t mass_array; - for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { mass_array[i] = this->get_node()->get_parameter("admittance.mass").as_double_array()[i]; } lbr_fri_ros2::cart_array_t damping_array; - for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { damping_array[i] = this->get_node()->get_parameter("admittance.damping").as_double_array()[i]; } lbr_fri_ros2::cart_array_t stiffness_array; - for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { stiffness_array[i] = this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i]; } @@ -314,7 +323,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + "Number of joint gains '%ld' does not match the number of joints in the robot '%d'.", this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint gains."); @@ -323,19 +332,19 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { lbr_fri_ros2::CARTESIAN_DOF) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " - "(%d).", + "Number of cartesian gains '%ld' does not match the number of cartesian degrees of freedom " + "'%d'.", this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), lbr_fri_ros2::CARTESIAN_DOF); throw std::runtime_error("Failed to configure cartesian gains."); } lbr_fri_ros2::jnt_array_t joint_gains_array; - for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_gains_array[i] = this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; } lbr_fri_ros2::cart_array_t cartesian_gains_array; - for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { cartesian_gains_array[i] = this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; } @@ -352,37 +361,67 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { } void AdmittanceController::configure_filters_() { - auto joint_velocity_tau = - this->get_node()->get_parameter("filter.joint_velocity_tau").as_double(); - if (joint_velocity_tau < 0.4) { + auto f_ext_tau = this->get_node()->get_parameter("filter.f_ext_tau").as_double(); + if (f_ext_tau < 0.2) { RCLCPP_ERROR_STREAM(this->get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR - << "Joint velocity filter time constant too small (" - << joint_velocity_tau - << "s). Currently enforced to be at least 0.4s for proper smoothing." + << "External force filter time constant too small (" << f_ext_tau + << "s). Currently enforced to be at least 0.2s for proper smoothing." << lbr_fri_ros2::ColorScheme::ENDC); - throw std::runtime_error("Invalid joint velocity filter time constant."); + throw std::runtime_error("Invalid external force filter time constant."); + } + f_ext_filter_ptr_ = + std::make_unique>( + f_ext_tau); +} + +void AdmittanceController::configure_safety_checks_() { + max_external_force_on_activate_ = + this->get_node()->get_parameter("on_activate.max_external_force").as_double(); + max_external_torque_on_activate_ = + this->get_node()->get_parameter("on_activate.max_external_torque").as_double(); + if (max_external_force_on_activate_ < 0.0) { + RCLCPP_WARN_STREAM( + this->get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::WARNING + << "Parameter 'on_activate.max_external_force' is negative, overriding to 0.0." + << lbr_fri_ros2::ColorScheme::ENDC); + max_external_force_on_activate_ = 0.0; + } + if (max_external_torque_on_activate_ < 0.0) { + RCLCPP_WARN_STREAM( + this->get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::WARNING + << "Parameter 'on_activate.max_external_torque' is negative, overriding to 0.0." + << lbr_fri_ros2::ColorScheme::ENDC); + max_external_torque_on_activate_ = 0.0; } - dq_filter_ptr_ = std::make_unique>( - joint_velocity_tau); } void AdmittanceController::init_filters_with_update_rate_() { - dq_filter_ptr_->initialize(1. / static_cast(this->get_update_rate())); + f_ext_filter_ptr_->initialize(1. / static_cast(this->get_update_rate())); } void AdmittanceController::zero_all_values_() { f_ext_.setZero(); + f_ext_filtered_.setZero(); delta_x_.setZero(); dx_.setZero(); ddx_.setZero(); std::fill(dq_.begin(), dq_.end(), 0.0); - std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0.0); twist_command_.setZero(); } bool AdmittanceController::any_external_force_torques_on_horizon_( + const double &max_external_force, const double &max_external_torque, const std::chrono::milliseconds &horizon) const { + if (max_external_force < 0.0 || max_external_torque < 0.0) { + RCLCPP_ERROR_STREAM(this->get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::ERROR + << "Maximum external force-torque limits must be non-negative." + << lbr_fri_ros2::ColorScheme::ENDC); + throw std::runtime_error("Invalid maximum external force-torque limits."); + } if (!estimated_ft_sensor_ptr_) { RCLCPP_ERROR(this->get_node()->get_logger(), "Estimated force-torque sensor not initialized for external force-torque check."); @@ -398,7 +437,8 @@ bool AdmittanceController::any_external_force_torques_on_horizon_( auto torques = this->estimated_ft_sensor_ptr_->get_torques(); auto start_time = std::chrono::steady_clock::now(); while (std::chrono::steady_clock::now() - start_time < horizon) { - if (!(lbr_fri_ros2::norm_in_bounds(forces, 0.) && lbr_fri_ros2::norm_in_bounds(torques, 0.))) { + if (!(lbr_fri_ros2::norm_in_bounds(forces, max_external_force) && + lbr_fri_ros2::norm_in_bounds(torques, max_external_torque))) { RCLCPP_INFO_STREAM(this->get_node()->get_logger(), "External force-torques detected: forces = [" << forces[0] << ", " << forces[1] << ", " << forces[2] @@ -415,7 +455,7 @@ bool AdmittanceController::any_external_force_torques_on_horizon_( void AdmittanceController::log_info_() const { admittance_impl_ptr_->log_info(); inv_jac_ctrl_impl_ptr_->log_info(); - dq_filter_ptr_->log_info(); + f_ext_filter_ptr_->log_info(); } } // namespace lbr_ros2_control diff --git a/lbr_ros2_control/src/controllers/estimated_wrench_interface.cpp b/lbr_ros2_control/src/controllers/estimated_wrench_interface.cpp new file mode 100644 index 00000000..0395fe29 --- /dev/null +++ b/lbr_ros2_control/src/controllers/estimated_wrench_interface.cpp @@ -0,0 +1,256 @@ +#include "lbr_ros2_control/controllers/estimated_wrench_interface.hpp" + +namespace lbr_ros2_control { +EstimatedWrenchInterface::EstimatedWrenchInterface() {} + +controller_interface::InterfaceConfiguration +EstimatedWrenchInterface::command_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration +EstimatedWrenchInterface::state_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // joint position and external torque interfaces + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + interface_configuration.names.push_back(joint_name + "/" + HW_IF_EXTERNAL_TORQUE); + } + return interface_configuration; +} + +controller_interface::CallbackReturn EstimatedWrenchInterface::on_init() { + try { + get_node()->declare_parameter("robot_name", "lbr"); + get_node()->declare_parameter("wrench_estimator.chain_root", "lbr_link_0"); + get_node()->declare_parameter("wrench_estimator.chain_tip", "lbr_link_ee"); + get_node()->declare_parameter("wrench_estimator.damping", 0.2); + get_node()->declare_parameter("wrench_estimator.force_x_th", 2.0); + get_node()->declare_parameter("wrench_estimator.force_y_th", 2.0); + get_node()->declare_parameter("wrench_estimator.force_z_th", 2.0); + get_node()->declare_parameter("wrench_estimator.torque_x_th", 0.5); + get_node()->declare_parameter("wrench_estimator.torque_y_th", 0.5); + get_node()->declare_parameter("wrench_estimator.torque_z_th", 0.5); + configure_joint_names_(); + configure_parameters_(); + wrench_estimator_ptr_ = std::make_unique( + get_robot_description(), wrench_estimator_parameters_); + log_info_(); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::ERROR + << "Failed to initialize estimated wrench interface with: " << e.what() + << "." << lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +std::vector +EstimatedWrenchInterface::on_export_state_interfaces() { + std::vector state_interfaces; + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_FORCE_X, &wrench_[0]); + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_FORCE_Y, &wrench_[1]); + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_FORCE_Z, &wrench_[2]); + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_TORQUE_X, &wrench_[3]); + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_TORQUE_Y, &wrench_[4]); + state_interfaces.emplace_back(std::string(get_node()->get_name()), HW_IF_TORQUE_Z, &wrench_[5]); + return state_interfaces; +} + +std::vector +EstimatedWrenchInterface::on_export_reference_interfaces() { + return {}; +} + +bool EstimatedWrenchInterface::on_set_chained_mode(bool /*chained_mode*/) { + // broadcasters don't support chaining: + // https://github.com/ros-controls/ros2_controllers/issues/2052#issuecomment-3616731834 + return true; +} + +controller_interface::return_type +EstimatedWrenchInterface::update_reference_from_subscribers(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + return controller_interface::return_type::OK; +} + +controller_interface::return_type +EstimatedWrenchInterface::update_and_write_commands(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + // get joint positions and external torques + if (!read_state_interfaces_()) { + return controller_interface::return_type::OK; + } + + // validate read states + if (!valid_states_()) { + return controller_interface::return_type::OK; + } + + // estimate wrench given states + estimate_wrench_(); + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +EstimatedWrenchInterface::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +EstimatedWrenchInterface::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!assign_state_interfaces_()) { + release_state_interfaces_(); + return controller_interface::CallbackReturn::ERROR; + } + nan_wrench_(); + nan_referenced_states_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +EstimatedWrenchInterface::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + release_state_interfaces_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool EstimatedWrenchInterface::assign_state_interfaces_() { + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_state_interfaces_.push_back(std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_EXTERNAL_TORQUE) { + external_torque_state_interfaces_.push_back(std::ref(state_interface)); + } + } + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::ERROR + << "Number of joint position state interfaces '" + << joint_position_state_interfaces_.size() + << "' does not match the number of joints " + "in the robot '" + << lbr_fri_ros2::N_JNTS << "'." << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + if (external_torque_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR + << "Number of external torque interfaces '" + << external_torque_state_interfaces_.size() + << "' does not match the number of joints " + "in the robot '" + << lbr_fri_ros2::N_JNTS << "'." + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + return true; +} + +void EstimatedWrenchInterface::release_state_interfaces_() { + joint_position_state_interfaces_.clear(); + external_torque_state_interfaces_.clear(); +} + +void EstimatedWrenchInterface::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::ERROR + << "Number of joint names '" << joint_names_.size() + << "' does not match the number of joints in the robot '" + << lbr_fri_ros2::N_JNTS << "'." << lbr_fri_ros2::ColorScheme::ENDC); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} + +void EstimatedWrenchInterface::configure_parameters_() { + wrench_estimator_parameters_.chain_root = + get_node()->get_parameter("wrench_estimator.chain_root").as_string(); + wrench_estimator_parameters_.chain_tip = + get_node()->get_parameter("wrench_estimator.chain_tip").as_string(); + wrench_estimator_parameters_.damping = + get_node()->get_parameter("wrench_estimator.damping").as_double(); + wrench_estimator_parameters_.force_x_th = + get_node()->get_parameter("wrench_estimator.force_x_th").as_double(); + wrench_estimator_parameters_.force_y_th = + get_node()->get_parameter("wrench_estimator.force_y_th").as_double(); + wrench_estimator_parameters_.force_z_th = + get_node()->get_parameter("wrench_estimator.force_z_th").as_double(); + wrench_estimator_parameters_.torque_x_th = + get_node()->get_parameter("wrench_estimator.torque_x_th").as_double(); + wrench_estimator_parameters_.torque_y_th = + get_node()->get_parameter("wrench_estimator.torque_y_th").as_double(); + wrench_estimator_parameters_.torque_z_th = + get_node()->get_parameter("wrench_estimator.torque_z_th").as_double(); + if (!wrench_estimator_parameters_.valid()) { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR + << "Invalid wrench estimator parameters." + << lbr_fri_ros2::ColorScheme::ENDC); + throw std::runtime_error("Invalid wrench estimator parameters."); + } +} + +bool EstimatedWrenchInterface::read_state_interfaces_() { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + auto joint_position = joint_position_state_interfaces_[i].get().get_optional(); + if (!joint_position.has_value()) { + RCLCPP_WARN_STREAM(get_node()->get_logger(), lbr_fri_ros2::ColorScheme::WARNING + << "Failed to get joint position for joint '" + << joint_names_[i] << "'." + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + joint_positions_[i] = *joint_position; + + auto external_torque = external_torque_state_interfaces_[i].get().get_optional(); + if (!external_torque.has_value()) { + RCLCPP_WARN_STREAM(get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::WARNING + << "Failed to get external torque for joint '" << joint_names_[i] + << "'." << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } + external_torques_[i] = *external_torque; + } + return true; +} + +bool EstimatedWrenchInterface::valid_states_() const { + for (size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + if (std::isnan(joint_positions_[i]) || std::isnan(external_torques_[i])) { + return false; + } + } + return true; +} + +void EstimatedWrenchInterface::nan_wrench_() { + wrench_.fill(std::numeric_limits::quiet_NaN()); +} + +void EstimatedWrenchInterface::nan_referenced_states_() { + joint_positions_.fill(std::numeric_limits::quiet_NaN()); + external_torques_.fill(std::numeric_limits::quiet_NaN()); +} + +void EstimatedWrenchInterface::estimate_wrench_() { + wrench_estimator_ptr_->set_q(joint_positions_); + wrench_estimator_ptr_->set_tau_ext(external_torques_); + wrench_estimator_ptr_->compute(); + wrench_estimator_ptr_->get_f_ext_tf(wrench_); +} + +void EstimatedWrenchInterface::log_info_() const { wrench_estimator_ptr_->log_info(); } +} // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::EstimatedWrenchInterface, + controller_interface::ChainableControllerInterface) diff --git a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index 35bc4b79..b17b1745 100644 --- a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp @@ -86,7 +86,7 @@ void LBRJointPositionCommandController::configure_joint_names_() { throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index b2ce3773..5bdf9a93 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -16,7 +16,7 @@ LBRStateBroadcaster::state_interface_configuration() const { controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { try { state_publisher_ptr_ = - this->get_node()->create_publisher("state", 1); + this->get_node()->create_publisher("lbr_state", 1); rt_state_publisher_ptr_ = std::make_shared>( @@ -53,63 +53,57 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time if (std::isnan(state_interface_map_[joint_names_[0]][hardware_interface::HW_IF_POSITION])) { return controller_interface::return_type::OK; } - if (rt_state_publisher_ptr_->trylock()) { - // FRI related states - rt_state_publisher_ptr_->msg_.client_command_mode = static_cast( - state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CLIENT_COMMAND_MODE]); - rt_state_publisher_ptr_->msg_.connection_quality = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONNECTION_QUALITY]); - rt_state_publisher_ptr_->msg_.control_mode = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONTROL_MODE]); - rt_state_publisher_ptr_->msg_.drive_state = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_DRIVE_STATE]); - rt_state_publisher_ptr_->msg_.operation_mode = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OPERATION_MODE]); - rt_state_publisher_ptr_->msg_.overlay_type = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OVERLAY_TYPE]); - rt_state_publisher_ptr_->msg_.safety_state = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAFETY_STATE]); - rt_state_publisher_ptr_->msg_.sample_time = - state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAMPLE_TIME]; - rt_state_publisher_ptr_->msg_.session_state = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SESSION_STATE]); - rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = static_cast( - state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_NANO_SEC]); - rt_state_publisher_ptr_->msg_.time_stamp_sec = - static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_SEC]); - rt_state_publisher_ptr_->msg_.tracking_performance = - state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TRACKING_PERFORMANCE]; - - // joint related states - std::for_each(joint_names_.begin(), joint_names_.end(), - [&, idx = 0](const std::string &joint_name) mutable { + + // FRI related states + lbr_state_.client_command_mode = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CLIENT_COMMAND_MODE]); + lbr_state_.connection_quality = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONNECTION_QUALITY]); + lbr_state_.control_mode = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_CONTROL_MODE]); + lbr_state_.drive_state = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_DRIVE_STATE]); + lbr_state_.operation_mode = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OPERATION_MODE]); + lbr_state_.overlay_type = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_OVERLAY_TYPE]); + lbr_state_.safety_state = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAFETY_STATE]); + lbr_state_.sample_time = state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SAMPLE_TIME]; + lbr_state_.session_state = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_SESSION_STATE]); + lbr_state_.time_stamp_nano_sec = static_cast( + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_NANO_SEC]); + lbr_state_.time_stamp_sec = + static_cast(state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TIME_STAMP_SEC]); + lbr_state_.tracking_performance = + state_interface_map_[HW_IF_AUXILIARY_PREFIX][HW_IF_TRACKING_PERFORMANCE]; + + // joint related states + std::for_each(joint_names_.begin(), joint_names_.end(), + [&, idx = 0](const std::string &joint_name) mutable { #if FRI_CLIENT_VERSION_MAJOR == 1 - rt_state_publisher_ptr_->msg_.commanded_joint_position[idx] = - state_interface_map_[joint_name][HW_IF_COMMANDED_JOINT_POSITION]; + lbr_state_.commanded_joint_position[idx] = + state_interface_map_[joint_name][HW_IF_COMMANDED_JOINT_POSITION]; #endif - rt_state_publisher_ptr_->msg_.commanded_torque[idx] = - state_interface_map_[joint_name][HW_IF_COMMANDED_TORQUE]; - rt_state_publisher_ptr_->msg_.external_torque[idx] = - state_interface_map_[joint_name][HW_IF_EXTERNAL_TORQUE]; - if (rt_state_publisher_ptr_->msg_.session_state == KUKA::FRI::COMMANDING_WAIT || - rt_state_publisher_ptr_->msg_.session_state == - KUKA::FRI::COMMANDING_ACTIVE) { - rt_state_publisher_ptr_->msg_.ipo_joint_position[idx] = - state_interface_map_[joint_name][HW_IF_IPO_JOINT_POSITION]; - } else { - rt_state_publisher_ptr_->msg_.ipo_joint_position[idx] = - std::numeric_limits::quiet_NaN(); - } - rt_state_publisher_ptr_->msg_.measured_joint_position[idx] = - state_interface_map_[joint_name][hardware_interface::HW_IF_POSITION]; - rt_state_publisher_ptr_->msg_.measured_torque[idx] = - state_interface_map_[joint_name][hardware_interface::HW_IF_EFFORT]; - ++idx; - }); - - rt_state_publisher_ptr_->unlockAndPublish(); - } - + lbr_state_.commanded_torque[idx] = + state_interface_map_[joint_name][HW_IF_COMMANDED_TORQUE]; + lbr_state_.external_torque[idx] = + state_interface_map_[joint_name][HW_IF_EXTERNAL_TORQUE]; + if (lbr_state_.session_state == KUKA::FRI::COMMANDING_WAIT || + lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) { + lbr_state_.ipo_joint_position[idx] = + state_interface_map_[joint_name][HW_IF_IPO_JOINT_POSITION]; + } else { + lbr_state_.ipo_joint_position[idx] = std::numeric_limits::quiet_NaN(); + } + lbr_state_.measured_joint_position[idx] = + state_interface_map_[joint_name][hardware_interface::HW_IF_POSITION]; + lbr_state_.measured_torque[idx] = + state_interface_map_[joint_name][hardware_interface::HW_IF_EFFORT]; + ++idx; + }); + rt_state_publisher_ptr_->try_publish(lbr_state_); return controller_interface::return_type::OK; } @@ -138,27 +132,25 @@ void LBRStateBroadcaster::init_state_interface_map_() { } void LBRStateBroadcaster::init_state_msg_() { - rt_state_publisher_ptr_->msg_.client_command_mode = std::numeric_limits::quiet_NaN(); + lbr_state_.client_command_mode = std::numeric_limits::quiet_NaN(); #if FRI_CLIENT_VERSION_MAJOR == 1 - rt_state_publisher_ptr_->msg_.commanded_joint_position.fill( - std::numeric_limits::quiet_NaN()); + lbr_state_.commanded_joint_position.fill(std::numeric_limits::quiet_NaN()); #endif - rt_state_publisher_ptr_->msg_.commanded_torque.fill(std::numeric_limits::quiet_NaN()); - rt_state_publisher_ptr_->msg_.connection_quality = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.control_mode = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.drive_state = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.external_torque.fill(std::numeric_limits::quiet_NaN()); - rt_state_publisher_ptr_->msg_.ipo_joint_position.fill(std::numeric_limits::quiet_NaN()); - rt_state_publisher_ptr_->msg_.measured_joint_position.fill( - std::numeric_limits::quiet_NaN()); - rt_state_publisher_ptr_->msg_.measured_torque.fill(std::numeric_limits::quiet_NaN()); - rt_state_publisher_ptr_->msg_.overlay_type = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.safety_state = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.sample_time = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.session_state = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits::quiet_NaN(); - rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits::quiet_NaN(); + lbr_state_.commanded_torque.fill(std::numeric_limits::quiet_NaN()); + lbr_state_.connection_quality = std::numeric_limits::quiet_NaN(); + lbr_state_.control_mode = std::numeric_limits::quiet_NaN(); + lbr_state_.drive_state = std::numeric_limits::quiet_NaN(); + lbr_state_.external_torque.fill(std::numeric_limits::quiet_NaN()); + lbr_state_.ipo_joint_position.fill(std::numeric_limits::quiet_NaN()); + lbr_state_.measured_joint_position.fill(std::numeric_limits::quiet_NaN()); + lbr_state_.measured_torque.fill(std::numeric_limits::quiet_NaN()); + lbr_state_.overlay_type = std::numeric_limits::quiet_NaN(); + lbr_state_.safety_state = std::numeric_limits::quiet_NaN(); + lbr_state_.sample_time = std::numeric_limits::quiet_NaN(); + lbr_state_.session_state = std::numeric_limits::quiet_NaN(); + lbr_state_.time_stamp_nano_sec = std::numeric_limits::quiet_NaN(); + lbr_state_.time_stamp_sec = std::numeric_limits::quiet_NaN(); + lbr_state_.tracking_performance = std::numeric_limits::quiet_NaN(); } void LBRStateBroadcaster::configure_joint_names_() { @@ -171,7 +163,7 @@ void LBRStateBroadcaster::configure_joint_names_() { throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp index b04dc775..e22f5766 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -76,7 +76,8 @@ LBRTorqueCommandController::on_configure(const rclcpp_lifecycle::State & /*previ controller_interface::CallbackReturn LBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - if (!reference_command_interfaces_()) { + if (!assign_command_interfaces_()) { + release_command_interfaces_(); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -84,17 +85,17 @@ LBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previo controller_interface::CallbackReturn LBRTorqueCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_command_interfaces_(); + release_command_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool LBRTorqueCommandController::reference_command_interfaces_() { +bool LBRTorqueCommandController::assign_command_interfaces_() { for (auto &command_interface : command_interfaces_) { if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); + joint_position_command_interfaces_.push_back(std::ref(command_interface)); } if (command_interface.get_interface_name() == hardware_interface::HW_IF_EFFORT) { - torque_command_interfaces_.emplace_back(std::ref(command_interface)); + torque_command_interfaces_.push_back(std::ref(command_interface)); } } if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { @@ -120,7 +121,7 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { return true; } -void LBRTorqueCommandController::clear_command_interfaces_() { +void LBRTorqueCommandController::release_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } diff --git a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 91fdc779..983b8a30 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -2,8 +2,8 @@ namespace lbr_ros2_control { LBRWrenchCommandController::LBRWrenchCommandController() - : rt_lbr_wrench_command_ptr_(nullptr), lbr_wrench_command_subscription_ptr_(nullptr), - rt_wrench_command_ptr_(nullptr), wrench_command_subscription_ptr_(nullptr) {} + : lbr_wrench_command_rt_buffer_(nullptr), lbr_wrench_command_subscription_ptr_(nullptr), + wrench_command_rt_buffer_(nullptr), wrench_command_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration LBRWrenchCommandController::command_interface_configuration() const { @@ -98,7 +98,7 @@ bool LBRWrenchCommandController::on_set_chained_mode(bool chained_mode) { controller_interface::return_type LBRWrenchCommandController::update_reference_from_subscribers(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto lbr_wrench_command = rt_lbr_wrench_command_ptr_.readFromRT(); + auto lbr_wrench_command = lbr_wrench_command_rt_buffer_.readFromRT(); if (!lbr_wrench_command || !(*lbr_wrench_command)) { return controller_interface::return_type::OK; } @@ -170,7 +170,7 @@ LBRWrenchCommandController::update_and_write_commands(const rclcpp::Time & /*tim } // read wrench command in chained mode - auto wrench_command = rt_wrench_command_ptr_.readFromRT(); + auto wrench_command = wrench_command_rt_buffer_.readFromRT(); if (!wrench_command || !(*wrench_command)) { if (!zero_wrench_commands_()) { return controller_interface::return_type::ERROR; @@ -238,10 +238,13 @@ LBRWrenchCommandController::on_configure(const rclcpp_lifecycle::State & /*previ controller_interface::CallbackReturn LBRWrenchCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { reference_interfaces_.assign(lbr_fri_ros2::N_JNTS, std::numeric_limits::quiet_NaN()); - if (!reference_state_interfaces_()) { + if (!assign_state_interfaces_()) { + release_state_interfaces_(); return controller_interface::CallbackReturn::ERROR; } - if (!reference_command_interfaces_()) { + if (!assign_command_interfaces_()) { + release_state_interfaces_(); + release_command_interfaces_(); return controller_interface::CallbackReturn::ERROR; } return controller_interface::CallbackReturn::SUCCESS; @@ -249,18 +252,18 @@ LBRWrenchCommandController::on_activate(const rclcpp_lifecycle::State & /*previo controller_interface::CallbackReturn LBRWrenchCommandController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_state_interfaces_(); - clear_command_interfaces_(); + release_state_interfaces_(); + release_command_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool LBRWrenchCommandController::reference_state_interfaces_() { +bool LBRWrenchCommandController::assign_state_interfaces_() { for (auto &state_interface : state_interfaces_) { if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + joint_position_state_interfaces_.push_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY) { - joint_velocity_state_interfaces_.emplace_back(std::ref(state_interface)); + joint_velocity_state_interfaces_.push_back(std::ref(state_interface)); } } if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { @@ -286,13 +289,13 @@ bool LBRWrenchCommandController::reference_state_interfaces_() { return true; } -bool LBRWrenchCommandController::reference_command_interfaces_() { +bool LBRWrenchCommandController::assign_command_interfaces_() { for (auto &command_interface : command_interfaces_) { if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); + joint_position_command_interfaces_.push_back(std::ref(command_interface)); } if (command_interface.get_prefix_name() == HW_IF_WRENCH_PREFIX) { - wrench_command_interfaces_.emplace_back(std::ref(command_interface)); + wrench_command_interfaces_.push_back(std::ref(command_interface)); } } if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { @@ -317,12 +320,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() { return true; } -void LBRWrenchCommandController::clear_state_interfaces_() { +void LBRWrenchCommandController::release_state_interfaces_() { joint_position_state_interfaces_.clear(); joint_velocity_state_interfaces_.clear(); } -void LBRWrenchCommandController::clear_command_interfaces_() { +void LBRWrenchCommandController::release_command_interfaces_() { joint_position_command_interfaces_.clear(); wrench_command_interfaces_.clear(); } @@ -379,7 +382,7 @@ void LBRWrenchCommandController::init_lbr_wrench_command_subscription_() { this->get_node()->create_subscription( "command/lbr_wrench_command", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { - rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); + lbr_wrench_command_rt_buffer_.writeFromNonRT(msg); }); } @@ -387,7 +390,7 @@ void LBRWrenchCommandController::init_wrench_command_subscription_() { wrench_command_subscription_ptr_ = this->get_node()->create_subscription( "command/wrench", 1, [this](const geometry_msgs::msg::Wrench::SharedPtr msg) { - rt_wrench_command_ptr_.writeFromNonRT(msg); + wrench_command_rt_buffer_.writeFromNonRT(msg); }); } diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index f13679be..cab4747f 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -20,8 +20,6 @@ TwistController::state_interface_configuration() const { for (const auto &joint_name : joint_names_) { interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); } - interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + - HW_IF_SAMPLE_TIME); interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SESSION_STATE); return interface_configuration; @@ -47,6 +45,7 @@ controller_interface::CallbackReturn TwistController::on_init() { std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); + configure_joint_limits_(); configure_inv_jac_ctrl_impl_(); log_info_(); timeout_ = this->get_node()->get_parameter("timeout").as_double(); @@ -110,8 +109,25 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / return controller_interface::return_type::ERROR; } auto dt = 1. / update_rate; + + // compute new target + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + q_target_[i] = q_[i] + dq_[i] * dt; + } + + // check target validity and override otherwise + if (!lbr_fri_ros2::all_jnts_in_bounds(q_target_, lower_joint_limits_, upper_joint_limits_)) { + RCLCPP_WARN_STREAM_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 500 /*ms*/, + lbr_fri_ros2::ColorScheme::WARNING + << "Overriding command target to current state since one target beyond joint limits." + << lbr_fri_ros2::ColorScheme::ENDC); + q_target_ = q_; + } + + // set values for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - if (!this->command_interfaces_[i].set_value(q_[i] + dq_[i] * dt)) { + if (!this->command_interfaces_[i].set_value(q_target_[i])) { RCLCPP_ERROR_STREAM(this->get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR << "Failed to set joint position for joint '" << joint_names_[i] @@ -132,7 +148,8 @@ TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/ controller_interface::CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - if (!reference_state_interfaces_()) { + if (!assign_state_interfaces_()) { + release_state_interfaces_(); return controller_interface::CallbackReturn::ERROR; } reset_command_buffer_(); @@ -142,16 +159,16 @@ TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) controller_interface::CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_state_interfaces_(); + release_state_interfaces_(); reset_command_buffer_(); zero_joint_velocity_command_(); return controller_interface::CallbackReturn::SUCCESS; } -bool TwistController::reference_state_interfaces_() { +bool TwistController::assign_state_interfaces_() { for (auto &state_interface : state_interfaces_) { if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + joint_position_state_interfaces_.push_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { session_state_interface_ptr_ = @@ -170,7 +187,10 @@ bool TwistController::reference_state_interfaces_() { return true; } -void TwistController::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); } +void TwistController::release_state_interfaces_() { + joint_position_state_interfaces_.clear(); + session_state_interface_ptr_.reset(); +} void TwistController::reset_command_buffer_() { rt_twist_ptr_ = @@ -183,22 +203,34 @@ void TwistController::configure_joint_names_() { if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + "Number of joint names '%ld' does not match the number of joints in the robot '%d'.", joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } +void TwistController::configure_joint_limits_() { + auto hard_joint_limits = get_hard_joint_limits(); + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + auto it = hard_joint_limits.find(joint_names_[i]); + if (it == hard_joint_limits.end()) { + throw std::runtime_error("Could not find joint limits for '" + joint_names_[i] + "'."); + } + lower_joint_limits_[i] = it->second.min_position; + upper_joint_limits_[i] = it->second.max_position; + } +} + void TwistController::configure_inv_jac_ctrl_impl_() { if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + "Number of joint gains '%ld' does not match the number of joints in the robot '%d'.", this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint gains."); @@ -207,19 +239,19 @@ void TwistController::configure_inv_jac_ctrl_impl_() { lbr_fri_ros2::CARTESIAN_DOF) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " - "(%d).", + "Number of cartesian gains '%ld' does not match the number of cartesian degrees of freedom " + "'%d'.", this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), lbr_fri_ros2::CARTESIAN_DOF); throw std::runtime_error("Failed to configure cartesian gains."); } lbr_fri_ros2::jnt_array_t joint_gains_array; - for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_gains_array[i] = this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; } lbr_fri_ros2::cart_array_t cartesian_gains_array; - for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { cartesian_gains_array[i] = this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 90eb13fc..c1bd5f94 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -56,25 +56,6 @@ SystemInterface::on_init(const hardware_interface::HardwareComponentInterfacePar return controller_interface::CallbackReturn::ERROR; } - // setup force-torque estimator - if (!parse_ft_parameters_()) { - return controller_interface::CallbackReturn::ERROR; - } - if (ft_parameters_.enabled) { - ft_estimator_impl_ptr_ = std::make_shared( - info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, - lbr_fri_ros2::cart_array_t{ - ft_parameters_.force_x_th, - ft_parameters_.force_y_th, - ft_parameters_.force_z_th, - ft_parameters_.torque_x_th, - ft_parameters_.torque_y_th, - ft_parameters_.torque_z_th, - }); - ft_estimator_ptr_ = std::make_unique(ft_estimator_impl_ptr_, - ft_parameters_.update_rate); - } - // populate the keys command_keys_.populate_keys(info_); state_keys_.populate_keys(info_); @@ -165,32 +146,27 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l << lbr_fri_ros2::ColorScheme::BOLD << lbr_fri_ros2::ColorScheme::OKBLUE << lbr_fri_ros2::EnumMaps::session_state_map(state.session_state) << lbr_fri_ros2::ColorScheme::ENDC << "'."); + if (state.session_state == KUKA::FRI::ESessionState::IDLE) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + lbr_fri_ros2::ColorScheme::ERROR + << "Robot in '" + << lbr_fri_ros2::EnumMaps::session_state_map(KUKA::FRI::ESessionState::IDLE) + << "' state. Please restart the FRI server." << lbr_fri_ros2::ColorScheme::ENDC); + app_ptr_->close_udp_socket(); // no need to request stop since already stopped when IDLE + return controller_interface::CallbackReturn::ERROR; + } if (!rclcpp::ok()) { return controller_interface::CallbackReturn::ERROR; } std::this_thread::sleep_for(std::chrono::seconds(1)); } - - // ft sensor - if (!ft_estimator_ptr_ && ft_parameters_.enabled) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::ERROR - << "Failed to instantiate FTEstimator despite user request." - << lbr_fri_ros2::ColorScheme::ENDC); - return controller_interface::CallbackReturn::ERROR; - } - if (ft_estimator_ptr_) { - ft_estimator_ptr_->run_async(ft_parameters_.rt_prio); - } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { app_ptr_->request_stop(); - if (ft_estimator_ptr_) { - ft_estimator_ptr_->request_stop(); - } return controller_interface::CallbackReturn::SUCCESS; } @@ -208,11 +184,11 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim lbr_state_ = async_client_ptr_->get_state_interface()->get_state(); if (period.seconds() - lbr_state_.sample_time * 0.2 > lbr_state_.sample_time) { - RCLCPP_WARN_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::WARNING - << "Increase update_rate parameter for controller_manager to " - << std::to_string(static_cast(1. / lbr_state_.sample_time)) - << " Hz or more" << lbr_fri_ros2::ColorScheme::ENDC); + RCLCPP_WARN_STREAM_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 500 /*ms*/, + lbr_fri_ros2::ColorScheme::WARNING + << "Increase update_rate parameter for controller_manager to " + << std::to_string(static_cast(1. / lbr_state_.sample_time)) + << " Hz or more" << lbr_fri_ros2::ColorScheme::ENDC); } // exit once robot exits COMMANDING_ACTIVE (for safety) @@ -225,7 +201,6 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim << lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->request_stop(); app_ptr_->close_udp_socket(); - ft_estimator_ptr_->request_stop(); return hardware_interface::return_type::ERROR; } @@ -261,18 +236,6 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim // additional velocity state interface compute_velocity_(); update_last_states_(); - - // additional force-torque state interface - if (ft_parameters_.enabled) { - // note that (if enabled) the computation is performed asynchronously to not block the main - // thread - ft_estimator_impl_ptr_->set_q(lbr_state_.measured_joint_position); - ft_estimator_impl_ptr_->set_tau_ext(lbr_state_.external_torque); - ft_estimator_impl_ptr_->get_f_ext_tf(ft_); - for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { - set_state(state_keys_.estimated_ft[i], ft_[i]); - } - } return hardware_interface::return_type::OK; } @@ -375,35 +338,6 @@ bool SystemInterface::parse_parameters_() { return true; } -bool SystemInterface::parse_ft_parameters_() { - try { - std::transform(info_.sensors[1].parameters.at("enabled").begin(), - info_.sensors[1].parameters.at("enabled").end(), - info_.sensors[1].parameters.at("enabled").begin(), - ::tolower); // convert to lower case - const auto &estimated_ft_sensor = info_.sensors[1]; - ft_parameters_.enabled = estimated_ft_sensor.parameters.at("enabled") == "true"; - ft_parameters_.update_rate = std::stoul(estimated_ft_sensor.parameters.at("update_rate")); - ft_parameters_.rt_prio = std::stoi(estimated_ft_sensor.parameters.at("rt_prio")); - ft_parameters_.chain_root = estimated_ft_sensor.parameters.at("chain_root"); - ft_parameters_.chain_tip = estimated_ft_sensor.parameters.at("chain_tip"); - ft_parameters_.damping = std::stod(estimated_ft_sensor.parameters.at("damping")); - ft_parameters_.force_x_th = std::stod(estimated_ft_sensor.parameters.at("force_x_th")); - ft_parameters_.force_y_th = std::stod(estimated_ft_sensor.parameters.at("force_y_th")); - ft_parameters_.force_z_th = std::stod(estimated_ft_sensor.parameters.at("force_z_th")); - ft_parameters_.torque_x_th = std::stod(estimated_ft_sensor.parameters.at("torque_x_th")); - ft_parameters_.torque_y_th = std::stod(estimated_ft_sensor.parameters.at("torque_y_th")); - ft_parameters_.torque_z_th = std::stod(estimated_ft_sensor.parameters.at("torque_z_th")); - } catch (const std::out_of_range &e) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::ERROR - << "Failed to parse force-torque sensor parameters with: " << e.what() - << lbr_fri_ros2::ColorScheme::ENDC); - return false; - } - return true; -} - void SystemInterface::nan_command_interfaces_() { for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { set_command(command_keys_.joint_position[i], std::numeric_limits::quiet_NaN()); @@ -446,9 +380,6 @@ void SystemInterface::nan_state_interfaces_() { // additional velocity state interface velocity_.fill(std::numeric_limits::quiet_NaN()); - - // additional force-torque state interface - ft_.fill(std::numeric_limits::quiet_NaN()); } bool SystemInterface::verify_number_of_joints_() { @@ -540,11 +471,6 @@ bool SystemInterface::verify_sensors_() { if (!verify_auxiliary_sensor_()) { return false; } - if (ft_parameters_.enabled) { - if (!verify_estimated_ft_sensor_()) { - return false; - } - } return true; } @@ -591,41 +517,6 @@ bool SystemInterface::verify_auxiliary_sensor_() { return true; } -bool SystemInterface::verify_estimated_ft_sensor_() { - const auto &estimated_ft_sensor = info_.sensors[1]; - if (estimated_ft_sensor.name != HW_IF_ESTIMATED_FT_PREFIX) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::ERROR - << "Sensor '" << estimated_ft_sensor.name.c_str() - << "' received invalid name. Expected '" << HW_IF_ESTIMATED_FT_PREFIX - << "'" << lbr_fri_ros2::ColorScheme::ENDC); - return false; - } - if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::ERROR - << "Sensor '" << estimated_ft_sensor.name.c_str() - << "' received invalid number of state interfaces. Received '" - << estimated_ft_sensor.state_interfaces.size() << "', expected '" - << static_cast(ESTIMATED_FT_SENSOR_SIZE) << "'" - << lbr_fri_ros2::ColorScheme::ENDC); - return false; - } - // check only valid interfaces are defined - for (const auto &si : estimated_ft_sensor.state_interfaces) { - if (si.name != HW_IF_FORCE_X && si.name != HW_IF_FORCE_Y && si.name != HW_IF_FORCE_Z && - si.name != HW_IF_TORQUE_X && si.name != HW_IF_TORQUE_Y && si.name != HW_IF_TORQUE_Z) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - lbr_fri_ros2::ColorScheme::ERROR - << "Sensor '" << estimated_ft_sensor.name.c_str() - << "' received invalid state interface '" << si.name.c_str() << "'" - << lbr_fri_ros2::ColorScheme::ENDC); - return false; - } - } - return true; -} - bool SystemInterface::verify_gpios_() { if (info_.gpios.size() != GPIO_SIZE) { RCLCPP_ERROR_STREAM(get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR