From 03cd7539081856be242b883cc750fce6c9d9bea4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 10:55:31 +0100 Subject: [PATCH 01/42] humble -> rolling, ubuntu 22.04 -> 24.04 (#180) --- .github/CONTRIBUTING.md | 2 +- ...11.yml => build-ubuntu-24.04-fri-1.11.yml} | 6 +-- ...14.yml => build-ubuntu-24.04-fri-1.14.yml} | 6 +-- ...15.yml => build-ubuntu-24.04-fri-1.15.yml} | 6 +-- ...16.yml => build-ubuntu-24.04-fri-1.16.yml} | 6 +-- ...2.5.yml => build-ubuntu-24.04-fri-2.5.yml} | 6 +-- ...2.7.yml => build-ubuntu-24.04-fri-2.7.yml} | 6 +-- .github/workflows/build.yml | 4 +- README.md | 26 ++++++------ docker/Dockerfile | 2 +- docker/doc/docker.rst | 2 +- lbr_bringup/config/moveit_servo.yaml | 41 +++++++------------ lbr_bringup/doc/lbr_bringup.rst | 12 +++--- .../doc/lbr_demos_advanced_cpp.rst | 10 ++--- .../doc/lbr_demos_advanced_py.rst | 12 +++--- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 24 +++++------ lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 24 +++++------ lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 12 +++--- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 10 ++--- lbr_fri_ros2_stack/repos-fri-1.11.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.14.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.15.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.16.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-2.5.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-2.7.yaml | 2 +- lbr_moveit_config/doc/lbr_moveit_config.rst | 6 +-- lbr_ros2_control/doc/lbr_ros2_control.rst | 12 +++--- 27 files changed, 118 insertions(+), 129 deletions(-) rename .github/workflows/{build-ubuntu-22.04-fri-1.11.yml => build-ubuntu-24.04-fri-1.11.yml} (71%) rename .github/workflows/{build-ubuntu-22.04-fri-1.14.yml => build-ubuntu-24.04-fri-1.14.yml} (71%) rename .github/workflows/{build-ubuntu-22.04-fri-1.15.yml => build-ubuntu-24.04-fri-1.15.yml} (71%) rename .github/workflows/{build-ubuntu-22.04-fri-1.16.yml => build-ubuntu-24.04-fri-1.16.yml} (71%) rename .github/workflows/{build-ubuntu-22.04-fri-2.5.yml => build-ubuntu-24.04-fri-2.5.yml} (71%) rename .github/workflows/{build-ubuntu-22.04-fri-2.7.yml => build-ubuntu-24.04-fri-2.7.yml} (71%) diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md index 5310de88..e9e31d2c 100644 --- a/.github/CONTRIBUTING.md +++ b/.github/CONTRIBUTING.md @@ -13,7 +13,7 @@ Contributions are vital to this project. If you want to contribute a feature / f 1. Open an issue: - Explain the issue and your solution - - Request a new branch: `dev--`, e.g. `dev-humble-my-new-demo` + - Request a new branch: `dev--`, e.g. `dev-rolling-my-new-demo` 2. Fork this repository 3. Create a [pull request](https://github.com/lbr-stack/lbr_fri_ros2_stack/pulls) against `dev--` diff --git a/.github/workflows/build-ubuntu-22.04-fri-1.11.yml b/.github/workflows/build-ubuntu-24.04-fri-1.11.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-1.11.yml rename to .github/workflows/build-ubuntu-24.04-fri-1.11.yml index 1f105d60..3de0799f 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-1.11.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.11.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-1.11 +name: ubuntu-24.04-fri-1.11 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 1.11 diff --git a/.github/workflows/build-ubuntu-22.04-fri-1.14.yml b/.github/workflows/build-ubuntu-24.04-fri-1.14.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-1.14.yml rename to .github/workflows/build-ubuntu-24.04-fri-1.14.yml index 0dbb74c9..c85da043 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-1.14.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.14.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-1.14 +name: ubuntu-24.04-fri-1.14 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 1.14 diff --git a/.github/workflows/build-ubuntu-22.04-fri-1.15.yml b/.github/workflows/build-ubuntu-24.04-fri-1.15.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-1.15.yml rename to .github/workflows/build-ubuntu-24.04-fri-1.15.yml index 7842296c..5ec898dc 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-1.15.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.15.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-1.15 +name: ubuntu-24.04-fri-1.15 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 1.15 diff --git a/.github/workflows/build-ubuntu-22.04-fri-1.16.yml b/.github/workflows/build-ubuntu-24.04-fri-1.16.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-1.16.yml rename to .github/workflows/build-ubuntu-24.04-fri-1.16.yml index 2d45ee57..855eb38c 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-1.16.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-1.16.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-1.16 +name: ubuntu-24.04-fri-1.16 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 1.16 diff --git a/.github/workflows/build-ubuntu-22.04-fri-2.5.yml b/.github/workflows/build-ubuntu-24.04-fri-2.5.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-2.5.yml rename to .github/workflows/build-ubuntu-24.04-fri-2.5.yml index de3e3a82..8068003e 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-2.5.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-2.5.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-2.5 +name: ubuntu-24.04-fri-2.5 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 2.5 diff --git a/.github/workflows/build-ubuntu-22.04-fri-2.7.yml b/.github/workflows/build-ubuntu-24.04-fri-2.7.yml similarity index 71% rename from .github/workflows/build-ubuntu-22.04-fri-2.7.yml rename to .github/workflows/build-ubuntu-24.04-fri-2.7.yml index 71c65ae1..284fd789 100644 --- a/.github/workflows/build-ubuntu-22.04-fri-2.7.yml +++ b/.github/workflows/build-ubuntu-24.04-fri-2.7.yml @@ -1,8 +1,8 @@ -name: build-ubuntu-22.04-fri-2.7 +name: ubuntu-24.04-fri-2.7 on: pull_request: branches: - - humble + - rolling workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-22.04 + os: ubuntu-24.04 fri_version: 2.7 diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ea5e4e11..cb223e18 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -20,5 +20,5 @@ jobs: - uses: ros-tooling/action-ros-ci@v0.2 with: package-name: lbr_fri_ros2_stack - target-ros2-distro: humble - vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${{ inputs.fri_version }}.yaml + 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 diff --git a/README.md b/README.md index 612c46f1..6a8dd0d3 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/humble?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/rolling?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,10 @@ 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,12 +26,12 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t ## Status | OS | ROS Distribution | FRI Version | Build Status | | :------------- | :--------------- | :---------- | :----------- | -| `Ubuntu-22.04` | `humble` | `1.11` | [![build-ubuntu-22.04-fri-1.11](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.11.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.11.yml) | -| `Ubuntu-22.04` | `humble` | `1.14` | [![build-ubuntu-22.04-fri-1.14](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.14.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.14.yml) | -| `Ubuntu-22.04` | `humble` | `1.15` | [![build-ubuntu-22.04-fri-1.15](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.15.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.15.yml) | -| `Ubuntu-22.04` | `humble` | `1.16` | [![build-ubuntu-22.04-fri-1.16](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.16.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.16.yml) | -| `Ubuntu-22.04` | `humble` | `2.5` | [![build-ubuntu-22.04-fri-2.5](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.5.yml) | -| `Ubuntu-22.04` | `humble` | `2.7` | [![build-ubuntu-22.04-fri-2.7](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.7.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.7.yml) | +| `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.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). @@ -46,10 +46,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/humble/setup.bash + source /opt/ros/rolling/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/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml + 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 rosdep install --from-paths src -i -r -y ``` diff --git a/docker/Dockerfile b/docker/Dockerfile index 7c660dbe..2d0c3e42 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:humble-ros-base-jammy +FROM ros:rolling-ros-base-jammy # change default shell to bash SHELL ["/bin/bash", "-c"] diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 53803077..2d32e9b8 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/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml + 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 #. Install `Docker `_:octicon:`link-external`. diff --git a/lbr_bringup/config/moveit_servo.yaml b/lbr_bringup/config/moveit_servo.yaml index 9d292fba..f78fb770 100644 --- a/lbr_bringup/config/moveit_servo.yaml +++ b/lbr_bringup/config/moveit_servo.yaml @@ -1,25 +1,21 @@ # Please do e.g. refer to -# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml -# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +# - https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/test_config_panda.yaml +# - https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/panda_simulated_config.yaml /**/servo_node: ros__parameters: moveit_servo: - ## Properties of incoming commands - command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s + ## Properties of outgoing commands + publish_period: 0.005 # 1/controller manager update rate [seconds] + + incoming_command_timeout: 0.5 # seconds + command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.2 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 - # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) - # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] - - ## Properties of outgoing commands - publish_period: 0.005 # #1/controller manager update rate [seconds] - low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) - # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory command_out_type: std_msgs/Float64MultiArray @@ -30,6 +26,7 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands + use_smoothing: true smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, @@ -40,23 +37,15 @@ ## MoveIt properties move_group_name: arm # Often 'manipulator' or 'arm' - planning_frame: link_0 # The MoveIt planning frame. Often 'base_link' or 'world' - - ## Other frames - ee_frame_name: link_ee # The name of the end effector link, used to return the EE pose - robot_link_command_frame: link_0 # commands must be given in the frame of a robot link. Usually either the base or end effector - - ## Stopping behaviour - incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command - # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. - # Important because ROS may drop some messages and we need the robot to halt reliably. - num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this - joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. - leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) + # Added as a buffer to joint variable position bounds [in that joint variable's respective units]. + # Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. + # If moving quickly, make these values larger. + joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index da2c79d5..7493620c 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -36,7 +36,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 +52,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 +90,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 +106,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 +144,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 @@ -183,5 +183,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 `lbr_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_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 2cd26566..eb08980a 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 @@ -14,8 +14,8 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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] -#. Launch the `admittance_control `_:octicon:`link-external`: +#. Launch the `admittance_control `_:octicon:`link-external`: .. code-block:: bash @@ -55,8 +55,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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 fb37b371..532ccd98 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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index dafaa698..edf2a975 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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_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/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index c15ed340..aca3edce 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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_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_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 9b7d394a..42c17ea9 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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_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_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 5d907950..8442254b 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -2,7 +2,7 @@ lbr_moveit_cpp ============== .. note:: - Also refer to the official `MoveIt `_:octicon:`link-external` documentation. + Also refer to the official `MoveIt `_:octicon:`link-external` documentation. .. contents:: Table of Contents :depth: 2 @@ -29,7 +29,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 @@ -41,8 +41,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -61,7 +61,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_fri_ros2_stack/repos-fri-1.11.yaml b/lbr_fri_ros2_stack/repos-fri-1.11.yaml index a0e658db..2527d70f 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: humble + version: rolling diff --git a/lbr_fri_ros2_stack/repos-fri-1.14.yaml b/lbr_fri_ros2_stack/repos-fri-1.14.yaml index 3bde0b56..74fda90e 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: humble + version: rolling diff --git a/lbr_fri_ros2_stack/repos-fri-1.15.yaml b/lbr_fri_ros2_stack/repos-fri-1.15.yaml index 5e7f40fe..a31b52b4 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: humble + version: rolling diff --git a/lbr_fri_ros2_stack/repos-fri-1.16.yaml b/lbr_fri_ros2_stack/repos-fri-1.16.yaml index 84df1702..cc20cdd6 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: humble + version: rolling diff --git a/lbr_fri_ros2_stack/repos-fri-2.5.yaml b/lbr_fri_ros2_stack/repos-fri-2.5.yaml index 637e81a0..1280b6f6 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: humble + version: rolling diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml index 90dcd423..e04b513e 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: humble + version: rolling diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 3484e3a1..52e70858 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 `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 36f2f7ba..0354f68a 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`. +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. `lbr_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_paramters.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_paramters.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. From a5071d9ffd393cafc0aa269a45b0e7fc48578fa8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 11:09:56 +0100 Subject: [PATCH 02/42] ign -> gz --- lbr_bringup/package.xml | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 4 ++-- lbr_description/package.xml | 2 +- lbr_ros2_control/config/lbr_system_interface.xacro | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 5b351434..d6905e26 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -11,7 +11,7 @@ ament_cmake_python controller_manager - ign_ros2_control + gz_ros2_control joint_state_broadcaster joint_trajectory_controller lbr_description diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index a828c0d5..174dc306 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -4,8 +4,8 @@ - + $(find lbr_ros2_control)/config/lbr_controllers.yaml /${robot_name} diff --git a/lbr_description/package.xml b/lbr_description/package.xml index a3b807df..df572b03 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -10,7 +10,7 @@ ament_cmake ament_cmake_pytest - ign_ros2_control + gz_ros2_control joint_state_publisher_gui robot_state_publisher rviz2 diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 23738b9a..833af578 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -15,7 +15,7 @@ - ign_ros2_control/IgnitionSystem + gz_ros2_control/GazeboSimSystem From 9f7d243cdf7fc3b0a58c563916b84833e2b9d263 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 11:45:18 +0100 Subject: [PATCH 03/42] moved configuration files to lbr_description for stand-alone urdf use --- .../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst | 4 ++-- .../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst | 4 ++-- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 ++++---- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 ++++---- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 ++-- lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- .../ros2_control}/lbr_system_interface.xacro | 0 .../ros2_control}/lbr_system_parameters.yaml | 0 lbr_description/urdf/iiwa14/iiwa14.xacro | 2 +- lbr_description/urdf/iiwa14/iiwa14_description.xacro | 4 ++-- lbr_description/urdf/iiwa7/iiwa7.xacro | 2 +- lbr_description/urdf/iiwa7/iiwa7_description.xacro | 4 ++-- lbr_description/urdf/med14/med14.xacro | 2 +- lbr_description/urdf/med14/med14_description.xacro | 4 ++-- lbr_description/urdf/med7/med7.xacro | 2 +- lbr_description/urdf/med7/med7_description.xacro | 4 ++-- lbr_ros2_control/doc/lbr_ros2_control.rst | 4 ++-- lbr_ros2_control/src/system_interface.cpp | 6 ++++-- 18 files changed, 33 insertions(+), 31 deletions(-) rename {lbr_ros2_control/config => lbr_description/ros2_control}/lbr_system_interface.xacro (100%) rename {lbr_ros2_control/config => lbr_description/ros2_control}/lbr_system_parameters.yaml (100%) 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 eb08980a..323b231b 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 @@ -14,7 +14,7 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ 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_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 532ccd98..a33a6088 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,7 +14,7 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -54,7 +54,7 @@ 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_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 edf2a975..5e29f63e 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,7 +14,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,7 +96,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,7 +134,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 aca3edce..f71c7d2c 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,7 +14,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,7 +96,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,7 +134,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 42c17ea9..6848a6df 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -62,7 +62,7 @@ MoveIt Servo - Hardware ~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -122,7 +122,7 @@ MoveIt via RViz - Hardware ~~~~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 8442254b..089c9fb5 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -41,7 +41,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro similarity index 100% rename from lbr_ros2_control/config/lbr_system_interface.xacro rename to lbr_description/ros2_control/lbr_system_interface.xacro diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_description/ros2_control/lbr_system_parameters.yaml similarity index 100% rename from lbr_ros2_control/config/lbr_system_parameters.yaml rename to lbr_description/ros2_control/lbr_system_parameters.yaml diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro index 7746f30b..b52b1578 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -10,7 +10,7 @@ + default="$(find lbr_description)/ros2_control/lbr_system_parameters.yaml" /> diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 712fc099..e53bf380 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_description)/ros2_control/lbr_system_parameters.yaml'"> - + + default="$(find lbr_description)/ros2_control/lbr_system_parameters.yaml" /> diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index d177bb80..28e99965 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_description)/ros2_control/lbr_system_parameters.yaml'"> - + + default="$(find lbr_description)/ros2_control/lbr_system_parameters.yaml" /> diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index 0eac46fe..f36889d7 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_description)/ros2_control/lbr_system_parameters.yaml'"> - + + default="$(find lbr_description)/ros2_control/lbr_system_parameters.yaml" /> diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 58c885c4..00144acf 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_description)/ros2_control/lbr_system_parameters.yaml'"> - + `` 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_paramters.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_paramters.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/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 30753044..b98d6d5b 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -11,7 +11,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { return ret; } - // parameters_ from config/lbr_system_interface.xacro + // parameters_ from lbr_system_interface.xacro (located in + // lbr_description/ros2_control/lbr_system_interface.xacro) if (!parse_parameters_(system_info)) { return controller_interface::CallbackReturn::ERROR; } @@ -531,7 +532,8 @@ bool SystemInterface::verify_sensors_() { } bool SystemInterface::verify_auxiliary_sensor_() { - // check all interfaces are defined in config/lbr_system_interface.xacro + // check all interfaces are defined in lbr_system_interface.xacro (located in + // lbr_description/ros2_control/lbr_system_interface.xacro) const auto &auxiliary_sensor = info_.sensors[0]; if (auxiliary_sensor.name != HW_IF_AUXILIARY_PREFIX) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), From 8832fc0d90c152d0f6f154f341e140c7b2aac0a2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 12:32:50 +0100 Subject: [PATCH 04/42] moved configuration files to lbr_description for stand-alone urdf use --- lbr_description/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt index 6debb078..e54c291e 100644 --- a/lbr_description/CMakeLists.txt +++ b/lbr_description/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake_pytest REQUIRED) # install install( - DIRECTORY gazebo meshes urdf + DIRECTORY gazebo meshes ros2_control urdf DESTINATION share/${PROJECT_NAME} ) From 4c02befd93d61569e8cb58678cccb301a3784583 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 13:34:22 +0100 Subject: [PATCH 05/42] updated moveit --- lbr_bringup/lbr_bringup/moveit.py | 5 ++++- lbr_bringup/package.xml | 3 +++ lbr_moveit_config/iiwa14_moveit_config/.setup_assistant | 2 +- .../iiwa14_moveit_config/config/moveit_controllers.yaml | 6 +++--- lbr_moveit_config/iiwa14_moveit_config/package.xml | 6 +++--- lbr_moveit_config/iiwa7_moveit_config/.setup_assistant | 2 +- .../iiwa7_moveit_config/config/moveit_controllers.yaml | 6 +++--- lbr_moveit_config/iiwa7_moveit_config/package.xml | 6 +++--- lbr_moveit_config/med14_moveit_config/.setup_assistant | 2 +- .../med14_moveit_config/config/moveit_controllers.yaml | 6 +++--- lbr_moveit_config/med14_moveit_config/package.xml | 6 +++--- lbr_moveit_config/med7_moveit_config/.setup_assistant | 2 +- .../med7_moveit_config/config/moveit_controllers.yaml | 6 +++--- lbr_moveit_config/med7_moveit_config/package.xml | 6 +++--- 14 files changed, 35 insertions(+), 29 deletions(-) diff --git a/lbr_bringup/lbr_bringup/moveit.py b/lbr_bringup/lbr_bringup/moveit.py index f2a69fbf..5cc120ae 100644 --- a/lbr_bringup/lbr_bringup/moveit.py +++ b/lbr_bringup/lbr_bringup/moveit.py @@ -67,7 +67,10 @@ def moveit_configs_builder( f"urdf/{robot_name}/{robot_name}.xacro", ), ) - .planning_pipelines(default_planning_pipeline="ompl") + .planning_pipelines( + default_planning_pipeline="ompl", + pipelines=["chomp", "pilz_industrial_motion_planner", "stomp", "ompl"], + ) ) @staticmethod diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index d6905e26..4cf72c0e 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -17,6 +17,9 @@ lbr_description lbr_fri_ros2 lbr_ros2_control + moveit_planners_chomp + moveit_planners_ompl + moveit_planners_stomp moveit_ros_move_group moveit_servo rclpy diff --git a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant index 55161cd3..ab4fdddb 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640386 + generated_timestamp: 1729340960 control_xacro: command: - position diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml index 57d65c17..15869edc 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -8,8 +8,6 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - A1 - A2 @@ -17,4 +15,6 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 \ No newline at end of file + - A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/iiwa14_moveit_config/package.xml b/lbr_moveit_config/iiwa14_moveit_config/package.xml index 5f3d20c0..30458809 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa14_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant index 1b331588..70b59fa7 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690638243 + generated_timestamp: 1729340852 control_xacro: command: - position diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml index 57d65c17..15869edc 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -8,8 +8,6 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - A1 - A2 @@ -17,4 +15,6 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 \ No newline at end of file + - A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/iiwa7_moveit_config/package.xml b/lbr_moveit_config/iiwa7_moveit_config/package.xml index e42c1af2..c009235d 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa7_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/med14_moveit_config/.setup_assistant b/lbr_moveit_config/med14_moveit_config/.setup_assistant index a9a44cf4..1d225ccc 100644 --- a/lbr_moveit_config/med14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med14_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640936 + generated_timestamp: 1729341043 control_xacro: command: - position diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml index 57d65c17..15869edc 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -8,8 +8,6 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - A1 - A2 @@ -17,4 +15,6 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 \ No newline at end of file + - A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/med14_moveit_config/package.xml b/lbr_moveit_config/med14_moveit_config/package.xml index a6a6a8ce..ce1e9a0e 100644 --- a/lbr_moveit_config/med14_moveit_config/package.xml +++ b/lbr_moveit_config/med14_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index d788c22c..48ab6236 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640668 + generated_timestamp: 1729340711 control_xacro: command: - position diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index 57d65c17..15869edc 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -8,8 +8,6 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - A1 - A2 @@ -17,4 +15,6 @@ moveit_simple_controller_manager: - A4 - A5 - A6 - - A7 \ No newline at end of file + - A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/med7_moveit_config/package.xml b/lbr_moveit_config/med7_moveit_config/package.xml index dd007600..a391df42 100644 --- a/lbr_moveit_config/med7_moveit_config/package.xml +++ b/lbr_moveit_config/med7_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii From 9b65978581a84d19cb82c0b99b06a89da42a372b Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 14:46:01 +0100 Subject: [PATCH 06/42] robot name prefix in config files (#189) --- lbr_bringup/config/gazebo.rviz | 20 ++--- lbr_bringup/config/hardware.rviz | 20 ++--- lbr_bringup/config/mock.rviz | 20 ++--- lbr_bringup/lbr_bringup/ros2_control.py | 4 - lbr_description/gazebo/lbr_gazebo.xacro | 30 +++---- .../ros2_control/lbr_system_interface.xacro | 14 ++-- lbr_description/urdf/iiwa14/iiwa14.xacro | 2 +- .../urdf/iiwa14/iiwa14_description.xacro | 64 +++++++-------- lbr_description/urdf/iiwa7/iiwa7.xacro | 2 +- .../urdf/iiwa7/iiwa7_description.xacro | 64 +++++++-------- lbr_description/urdf/med14/med14.xacro | 2 +- .../urdf/med14/med14_description.xacro | 64 +++++++-------- lbr_description/urdf/med7/med7.xacro | 2 +- .../urdf/med7/med7_description.xacro | 64 +++++++-------- .../iiwa14_moveit_config/config/iiwa14.srdf | 80 +++++++++---------- .../config/joint_limits.yaml | 14 ++-- .../iiwa14_moveit_config/config/moveit.rviz | 36 ++++----- .../config/moveit_controllers.yaml | 14 ++-- .../iiwa7_moveit_config/config/iiwa7.srdf | 80 +++++++++---------- .../config/joint_limits.yaml | 14 ++-- .../iiwa7_moveit_config/config/moveit.rviz | 36 ++++----- .../config/moveit_controllers.yaml | 14 ++-- .../config/joint_limits.yaml | 14 ++-- .../med14_moveit_config/config/med14.srdf | 80 +++++++++---------- .../med14_moveit_config/config/moveit.rviz | 36 ++++----- .../config/moveit_controllers.yaml | 14 ++-- .../config/joint_limits.yaml | 14 ++-- .../med7_moveit_config/config/med7.srdf | 80 +++++++++---------- .../med7_moveit_config/config/moveit.rviz | 36 ++++----- .../config/moveit_controllers.yaml | 14 ++-- lbr_ros2_control/config/lbr_controllers.yaml | 30 +++---- 31 files changed, 487 insertions(+), 491 deletions(-) diff --git a/lbr_bringup/config/gazebo.rviz b/lbr_bringup/config/gazebo.rviz index f4486f40..c45723d4 100644 --- a/lbr_bringup/config/gazebo.rviz +++ b/lbr_bringup/config/gazebo.rviz @@ -67,47 +67,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -119,7 +119,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/config/hardware.rviz b/lbr_bringup/config/hardware.rviz index b3a3df75..86a38438 100644 --- a/lbr_bringup/config/hardware.rviz +++ b/lbr_bringup/config/hardware.rviz @@ -68,47 +68,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -120,7 +120,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz index f4486f40..c45723d4 100644 --- a/lbr_bringup/config/mock.rviz +++ b/lbr_bringup/config/mock.rviz @@ -67,47 +67,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -119,7 +119,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index a5a58627..24f01bcf 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -122,10 +122,6 @@ def node_robot_state_publisher( parameters=[ robot_description, {"use_sim_time": use_sim_time}, - # use robot name as frame prefix - { - "frame_prefix": PathJoinSubstitution([robot_name, ""]) - }, # neat hack to add trailing slash, which is required by frame_prefix ], namespace=robot_name, **kwargs, diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 174dc306..78a833fc 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -29,20 +29,20 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 833af578..9b578f7f 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -124,43 +124,43 @@ - - - - - - - - + diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index e53bf380..00eba77b 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -26,7 +26,7 @@ joint_limits="${joint_limits}" system_parameters_path="${system_parameters_path}" /> - + @@ -47,10 +47,10 @@ - + - - + + - + @@ -81,10 +81,10 @@ - + - - + + - + @@ -115,10 +115,10 @@ - + - - + + - + @@ -149,10 +149,10 @@ - + - - + + - + @@ -183,10 +183,10 @@ - + - - + + - + @@ -217,10 +217,10 @@ - + - - + + - + @@ -251,10 +251,10 @@ - + - - + + - + @@ -285,12 +285,12 @@ - - + + - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index 7f89fac7..5f0712ce 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -19,7 +19,7 @@ between world and link_0--> - + diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index 28e99965..c9234c68 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -26,7 +26,7 @@ joint_limits="${joint_limits}" system_parameters_path="${system_parameters_path}" /> - + @@ -47,10 +47,10 @@ - + - - + + - + @@ -81,10 +81,10 @@ - + - - + + - + @@ -115,10 +115,10 @@ - + - - + + - + @@ -149,10 +149,10 @@ - + - - + + - + @@ -183,10 +183,10 @@ - + - - + + - + @@ -217,10 +217,10 @@ - + - - + + - + @@ -252,10 +252,10 @@ - + - - + + - + @@ -286,12 +286,12 @@ - - + + - + \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index 9276bf37..df863a1a 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -19,7 +19,7 @@ between world and link_0--> - + diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index f36889d7..fd78c8da 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -27,7 +27,7 @@ system_parameters_path="${system_parameters_path}" /> - + @@ -48,10 +48,10 @@ - + - - + + - + @@ -82,10 +82,10 @@ - + - - + + - + @@ -116,10 +116,10 @@ - + - - + + - + @@ -150,10 +150,10 @@ - + - - + + - + @@ -184,10 +184,10 @@ - + - - + + - + @@ -218,10 +218,10 @@ - + - - + + - + @@ -252,10 +252,10 @@ - + - - + + - + @@ -286,12 +286,12 @@ - - + + - + \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index 15209761..01df31af 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -19,7 +19,7 @@ between world and link_0--> - + diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 00144acf..d7b6abfa 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -27,7 +27,7 @@ system_parameters_path="${system_parameters_path}" /> - + @@ -48,10 +48,10 @@ - + - - + + - + @@ -82,10 +82,10 @@ - + - - + + - + @@ -116,10 +116,10 @@ - + - - + + - + @@ -150,10 +150,10 @@ - + - - + + - + @@ -184,10 +184,10 @@ - + - - + + - + @@ -218,10 +218,10 @@ - + - - + + - + @@ -252,10 +252,10 @@ - + - - + + - + @@ -286,12 +286,12 @@ - - + + - + \ No newline at end of file diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf index f1b96afe..934159c3 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf +++ b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml index ae4149e9..0ab19715 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz index c2cac1a0..9fea5cff 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz @@ -66,47 +66,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,47 +160,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml index 15869edc..ce72820a 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -9,12 +9,12 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 action_ns: follow_joint_trajectory default: true \ No newline at end of file diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf index 37a9aa08..5ef397ba 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf +++ b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml index e97eee4e..2a2bc84e 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz index c2cac1a0..9fea5cff 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz @@ -66,47 +66,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,47 +160,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml index 15869edc..ce72820a 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -9,12 +9,12 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 action_ns: follow_joint_trajectory default: true \ No newline at end of file diff --git a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml index ae4149e9..0ab19715 100644 --- a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.srdf b/lbr_moveit_config/med14_moveit_config/config/med14.srdf index f5ef369c..1925c3e5 100644 --- a/lbr_moveit_config/med14_moveit_config/config/med14.srdf +++ b/lbr_moveit_config/med14_moveit_config/config/med14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz index c2cac1a0..9fea5cff 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz @@ -66,47 +66,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,47 +160,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml index 15869edc..ce72820a 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -9,12 +9,12 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 action_ns: follow_joint_trajectory default: true \ No newline at end of file diff --git a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml index e97eee4e..2a2bc84e 100644 --- a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/med7_moveit_config/config/med7.srdf b/lbr_moveit_config/med7_moveit_config/config/med7.srdf index 1b4932a5..63e98c43 100644 --- a/lbr_moveit_config/med7_moveit_config/config/med7.srdf +++ b/lbr_moveit_config/med7_moveit_config/config/med7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz index c2cac1a0..9fea5cff 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz @@ -66,47 +66,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,47 +160,47 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_1: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index 15869edc..ce72820a 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -9,12 +9,12 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 action_ns: follow_joint_trajectory default: true \ No newline at end of file diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index c7939d25..f8fc7aad 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -38,19 +38,19 @@ /**/force_torque_broadcaster: ros__parameters: - frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 + frame_id: lbr_link_ee # namespace: https://github.com/ros2/rviz/issues/1103 sensor_name: estimated_ft_sensor /**/joint_trajectory_controller: ros__parameters: joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 command_interfaces: - position state_interfaces: @@ -62,11 +62,11 @@ /**/forward_position_controller: ros__parameters: joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 interface_name: position From f76be5846c1e2dc3d6421f54a833fbb412ef6822 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 16:11:41 +0100 Subject: [PATCH 07/42] controller modifications for name prefix (#189) --- .../config/admittance_control.yaml | 4 ++-- .../src/pose_control_node.cpp | 4 ++-- .../config/admittance_control.yaml | 4 ++-- .../config/admittance_rcm_control.yaml | 4 ++-- .../admittance_control_node.py | 4 ++-- .../admittance_controller.py | 4 ++-- .../admittance_rcm_control_node.py | 4 ++-- .../admittance_rcm_controller.py | 4 ++-- .../ros2_control/lbr_system_interface.xacro | 2 +- .../ros2_control/lbr_system_parameters.yaml | 4 ++-- .../urdf/iiwa14/iiwa14_description.xacro | 1 + .../urdf/iiwa7/iiwa7_description.xacro | 1 + .../urdf/med14/med14_description.xacro | 1 + .../urdf/med7/med7_description.xacro | 1 + .../include/lbr_fri_ros2/ft_estimator.hpp | 4 ++-- lbr_fri_ros2/test/test_position_command.cpp | 3 ++- lbr_fri_ros2/test/test_torque_command.cpp | 3 ++- lbr_fri_ros2/test/test_wrench_command.cpp | 3 ++- lbr_ros2_control/config/lbr_controllers.yaml | 21 ++++++++++++++++- .../controllers/admittance_controller.hpp | 4 ++-- .../lbr_joint_position_command_controller.hpp | 5 ++-- .../controllers/lbr_state_broadcaster.hpp | 3 ++- .../lbr_torque_command_controller.hpp | 4 ++-- .../lbr_wrench_command_controller.hpp | 4 ++-- .../lbr_ros2_control/system_interface.hpp | 4 ++-- .../src/controllers/admittance_controller.cpp | 14 +++++++++++ .../lbr_joint_position_command_controller.cpp | 16 +++++++++++++ .../src/controllers/lbr_state_broadcaster.cpp | 23 +++++++++++++------ .../lbr_torque_command_controller.cpp | 16 +++++++++++++ .../lbr_wrench_command_controller.cpp | 16 +++++++++++++ 30 files changed, 142 insertions(+), 43 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml index 7df54a6d..b356195d 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml @@ -1,7 +1,7 @@ /**/admittance_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index 8d38376a..47602410 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -32,8 +32,8 @@ class PoseControlNode : public LBRBasePositionCommandNode { return; } - this->declare_parameter("base_link", "link_0"); - this->declare_parameter("end_effector_link", "link_ee"); + this->declare_parameter("base_link", "lbr_link_0"); + this->declare_parameter("end_effector_link", "lbr_link_ee"); std::string root_link = this->get_parameter("base_link").as_string(); std::string tip_link = this->get_parameter("end_effector_link").as_string(); diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml index 4146d02f..1dbf503e 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml @@ -1,7 +1,7 @@ /**/admittance_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml index 901d1d87..0f95f796 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml @@ -1,7 +1,7 @@ /**/admittance_rcm_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py index eaccee81..d1aae155 100755 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_control") -> None: super().__init__(node_name=node_name) # parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py index 39aece33..d6875744 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py @@ -8,8 +8,8 @@ class AdmittanceController(object): def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py index 55eb5bac..cc99dfbd 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None: super().__init__(node_name) # declare and get parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py index 95fd3b18..4c9b5486 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py @@ -6,8 +6,8 @@ class AdmittanceRCMController: def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", ): self._robot = optas.RobotModel( urdf_string=robot_description, time_derivs=[0, 1] diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 9b578f7f..9a61a792 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -1,7 +1,7 @@ + params="robot_name mode joint_limits system_parameters_path"> diff --git a/lbr_description/ros2_control/lbr_system_parameters.yaml b/lbr_description/ros2_control/lbr_system_parameters.yaml index 4ae4cbf2..b55b0aef 100644 --- a/lbr_description/ros2_control/lbr_system_parameters.yaml +++ b/lbr_description/ros2_control/lbr_system_parameters.yaml @@ -19,8 +19,8 @@ hardware: 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 - chain_root: link_0 - chain_tip: link_ee + 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 diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 00eba77b..b4eb7212 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index c9234c68..089ffe65 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index fd78c8da..e76b72f9 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index d7b6abfa..675b1a2c 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -22,6 +22,7 @@ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index b4257a9e..8ef59405 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -35,8 +35,8 @@ class FTEstimator { using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; - FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", - const std::string &chain_tip = "link_ee", + FTEstimator(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}); void compute(const_jnt_pos_array_t_ref measured_joint_position, const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index 22cd7348..e2f7830d 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 1.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 2af7f93a..9c580828 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 10.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index de88e0e6..9ad8b315 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -24,7 +24,8 @@ int main() { pid_params.p = 10.0; - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index f8fc7aad..e6936732 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -32,15 +32,21 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController - # Admittance controller admittance_controller: type: lbr_ros2_control/AdmittanceController +# 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 +# LBR ROS 2 control broadcasters +/**/lbr_state_broadcaster: + ros__parameters: + robot_name: lbr + +# ROS 2 control controllers /**/joint_trajectory_controller: ros__parameters: joints: @@ -70,3 +76,16 @@ - lbr_A6 - lbr_A7 interface_name: position + +# LBR ROS 2 control controllers +/**/lbr_joint_position_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_torque_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_wrench_command_controller: + ros__parameters: + robot_name: lbr 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 6d28e3a1..1a75be92 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 @@ -50,9 +50,9 @@ class AdmittanceController : public controller_interface::ControllerInterface { bool reference_state_interfaces_(); void clear_command_interfaces_(); void clear_state_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp index 46eebecd..929dcf63 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp @@ -40,8 +40,9 @@ class LBRJointPositionCommandController : public controller_interface::Controlle on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + void configure_joint_names_(); + + std::array joint_names_; realtime_tools::RealtimeBuffer rt_lbr_joint_position_command_ptr_; 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 877f71e6..70efcdb3 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 @@ -47,9 +47,10 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { protected: void init_state_interface_map_(); void init_state_msg_(); + void configure_joint_names_(); std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + "lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; std::unordered_map> state_interface_map_; rclcpp::Publisher::SharedPtr 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 7a4c5f7b..65d6f0a8 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,9 +44,9 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_, torque_command_interfaces_; 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 52b76f35..c145f043 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 @@ -47,9 +47,9 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_; std::vector> joint_position_command_interfaces_, wrench_command_interfaces_; 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 ceb5157c..42a4e77a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -55,8 +55,8 @@ struct SystemInterfaceParameters { }; struct EstimatedFTSensorParameters { - std::string chain_root{"link_0"}; - std::string chain_tip{"link_ee"}; + 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}; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 147ad742..8c1a94f4 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -74,4 +74,18 @@ void AdmittanceController::clear_command_interfaces_() { void AdmittanceController::clear_state_interfaces_() { estimated_ft_sensor_state_interface_.clear(); } + +void AdmittanceController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control 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 f5217b82..380827ec 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 @@ -29,6 +29,8 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init( [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) { rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR position command controller with: %s.", e.what()); @@ -66,6 +68,20 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } + +void LBRJointPositionCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index d858d220..b8d9f733 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -21,13 +21,8 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { rt_state_publisher_ptr_ = std::make_shared>( state_publisher_ptr_); - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR( - this->get_node()->get_logger(), - "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return controller_interface::CallbackReturn::ERROR; - } + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR state broadcaster with: %s.", e.what()); @@ -154,6 +149,20 @@ void LBRStateBroadcaster::init_state_msg_() { 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(); } + +void LBRStateBroadcaster::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" 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 a4f674f6..22a341d2 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -28,6 +28,8 @@ controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { "command/torque", 1, [this](const lbr_fri_idl::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR torque command controller with: %s.", e.what()); @@ -102,6 +104,20 @@ void LBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } + +void LBRTorqueCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" 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 53445f1f..59763270 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -33,6 +33,8 @@ controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { "command/wrench", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); }); + this->get_node()->declare_parameter("robot_name", "lbr"); + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR wrench command controller with: %s.", e.what()); @@ -108,6 +110,20 @@ void LBRWrenchCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); wrench_command_interfaces_.clear(); } + +void LBRWrenchCommandController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" From b33aec26ca1862308e01c447cddcdb3212dfe7cf Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 17:17:47 +0100 Subject: [PATCH 08/42] fixed lambda capture for rt prio --- lbr_fri_ros2/src/app.cpp | 20 ++++++++++++-------- lbr_ros2_control/config/lbr_controllers.yaml | 2 +- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 7642a814..4d277d35 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -88,15 +88,19 @@ void App::run_async(int rt_prio) { ColorScheme::WARNING << "App already running" << ColorScheme::ENDC); return; } - run_thread_ = std::thread([&]() { - if (realtime_tools::has_realtime_kernel()) { - if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING << "Failed to set FIFO realtime scheduling policy" - << ColorScheme::ENDC); - } + run_thread_ = std::thread([this, rt_prio]() { + if (!realtime_tools::configure_sched_fifo(rt_prio)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING + << "Failed to set FIFO realtime scheduling policy. Refer to " + "[https://control.ros.org/master/doc/ros2_control/" + "controller_manager/doc/userdoc.html]." + << ColorScheme::ENDC); } else { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required"); + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN + << "Realtime scheduling policy set to FIFO with priority '" << rt_prio + << "'" << ColorScheme::ENDC); } RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index e6936732..35878f5d 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 200 + update_rate: 100 # ROS 2 control broadcasters joint_state_broadcaster: From 88bd6309b5ec42502fdbec50fcb0218b8735c67b Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 19 Oct 2024 17:28:01 +0100 Subject: [PATCH 09/42] fixed licenses --- lbr_demos/lbr_demos_advanced_py/setup.py | 2 +- lbr_demos/lbr_demos_py/setup.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index c465713a..c037589c 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -18,7 +18,7 @@ maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", description="Advanced Python demos for the lbr_ros2_control.", - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index 76198e83..3fe2a962 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -15,7 +15,7 @@ maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", description="Python demos for lbr_ros2_control.", - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ From 5e2103203c293b95045884cfd8da8fe8feb79921 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 11:28:58 +0100 Subject: [PATCH 10/42] added system config to launch arguments --- lbr_bringup/launch/hardware.launch.py | 3 +- lbr_bringup/lbr_bringup/description.py | 25 +++----- lbr_bringup/lbr_bringup/ros2_control.py | 16 +++++ .../doc/lbr_demos_advanced_cpp.rst | 4 +- .../doc/lbr_demos_advanced_py.rst | 4 +- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 +-- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 +-- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 +- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- ...parameters.yaml => lbr_system_config.yaml} | 0 .../ros2_control/lbr_system_interface.xacro | 58 +++++++++---------- lbr_description/urdf/iiwa14/iiwa14.xacro | 6 +- .../urdf/iiwa14/iiwa14_description.xacro | 4 +- lbr_description/urdf/iiwa7/iiwa7.xacro | 6 +- .../urdf/iiwa7/iiwa7_description.xacro | 4 +- lbr_description/urdf/med14/med14.xacro | 6 +- .../urdf/med14/med14_description.xacro | 4 +- lbr_description/urdf/med7/med7.xacro | 6 +- .../urdf/med7/med7_description.xacro | 4 +- .../img/lbr_ros2_control_detailed_v2.0.0.svg | 2 +- .../doc/img/lbr_ros2_control_v2.0.0.svg | 2 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 +- 22 files changed, 92 insertions(+), 86 deletions(-) rename lbr_description/ros2_control/{lbr_system_parameters.yaml => lbr_system_config.yaml} (100%) diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index 5240557f..a0f41313 100644 --- a/lbr_bringup/launch/hardware.launch.py +++ b/lbr_bringup/launch/hardware.launch.py @@ -12,7 +12,8 @@ def generate_launch_description() -> LaunchDescription: # launch arguments ld.add_action(LBRDescriptionMixin.arg_model()) ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_port_id()) + 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()) diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 8d612d5a..c38d456f 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -20,12 +20,14 @@ def param_robot_description( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), - port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "port_id", default="30200" - ), mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( "mode", default="mock" ), + system_config_path: Optional[ + Union[LaunchConfiguration, str] + ] = PathJoinSubstitution( + [LaunchConfiguration("sys_cfg_pkg"), LaunchConfiguration("sys_cfg")] + ), ) -> Dict[str, str]: robot_description = { "robot_description": Command( @@ -43,10 +45,10 @@ def param_robot_description( ".xacro", " robot_name:=", robot_name, - " port_id:=", - port_id, " mode:=", mode, + "system_config_path:=", + system_config_path, ] ) } @@ -69,15 +71,6 @@ def arg_robot_name(default_value: str = "lbr") -> DeclareLaunchArgument: description="The robot's name.", ) - @staticmethod - def arg_port_id(default_value: str = "30200") -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="port_id", - default_value=default_value, - description="Port ID of the FRI communication. Valid in range [30200, 30209].\n" - "\tUsefull in multi-robot setups.", - ) - @staticmethod def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument: return DeclareLaunchArgument( @@ -95,10 +88,6 @@ def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument: def param_robot_name() -> Dict[str, LaunchConfiguration]: return {"robot_name": LaunchConfiguration("robot_name", default="lbr")} - @staticmethod - def param_port_id() -> Dict[str, LaunchConfiguration]: - return {"port_id": LaunchConfiguration("port_id", default="30200")} - @staticmethod def param_mode() -> Dict[str, LaunchConfiguration]: return {"mode": LaunchConfiguration("mode", default="mock")} diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index 24f01bcf..b8d6bffb 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -38,6 +38,22 @@ def arg_ctrl() -> DeclareLaunchArgument: ], ) + @staticmethod + def arg_sys_cfg_pkg() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="sys_cfg_pkg", + default_value="lbr_description", + description="Package containing the lbr_system_config.yaml file for FRI configurations.", + ) + + @staticmethod + def arg_sys_cfg() -> DeclareLaunchArgument: + return 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.", + ) + @staticmethod def arg_use_sim_time() -> DeclareLaunchArgument: return DeclareLaunchArgument( 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 323b231b..f8426012 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 @@ -14,7 +14,7 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ 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_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 a33a6088..616751d0 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,7 +14,7 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -54,7 +54,7 @@ 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_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 5e29f63e..652f5aef 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,7 +14,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,7 +96,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,7 +134,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 f71c7d2c..2eeb8dd6 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,7 +14,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,7 +96,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,7 +134,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 6848a6df..ccab9103 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -62,7 +62,7 @@ MoveIt Servo - Hardware ~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -122,7 +122,7 @@ MoveIt via RViz - Hardware ~~~~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 089c9fb5..0816829a 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -41,7 +41,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_description/ros2_control/lbr_system_parameters.yaml b/lbr_description/ros2_control/lbr_system_config.yaml similarity index 100% rename from lbr_description/ros2_control/lbr_system_parameters.yaml rename to lbr_description/ros2_control/lbr_system_config.yaml diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 9a61a792..d078c1a9 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -1,10 +1,10 @@ + params="robot_name mode joint_limits system_config_path"> - + @@ -21,22 +21,22 @@ lbr_ros2_control::SystemInterface - ${system_parameters['hardware']['fri_client_sdk']['major_version']} - ${system_parameters['hardware']['fri_client_sdk']['minor_version']} - ${system_parameters['hardware']['client_command_mode']} - ${system_parameters['hardware']['port_id']} - ${system_parameters['hardware']['remote_host']} - ${system_parameters['hardware']['rt_prio']} - ${system_parameters['hardware']['pid_p']} - ${system_parameters['hardware']['pid_i']} - ${system_parameters['hardware']['pid_d']} - ${system_parameters['hardware']['pid_i_max']} - ${system_parameters['hardware']['pid_i_min']} - ${system_parameters['hardware']['pid_antiwindup']} - ${system_parameters['hardware']['command_guard_variant']} - ${system_parameters['hardware']['external_torque_cutoff_frequency']} - ${system_parameters['hardware']['measured_torque_cutoff_frequency']} - ${system_parameters['hardware']['open_loop']} + ${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']['pid_p']} + ${system_config['hardware']['pid_i']} + ${system_config['hardware']['pid_d']} + ${system_config['hardware']['pid_i_max']} + ${system_config['hardware']['pid_i_min']} + ${system_config['hardware']['pid_antiwindup']} + ${system_config['hardware']['command_guard_variant']} + ${system_config['hardware']['external_torque_cutoff_frequency']} + ${system_config['hardware']['measured_torque_cutoff_frequency']} + ${system_config['hardware']['open_loop']} @@ -61,15 +61,15 @@ - ${system_parameters['estimated_ft_sensor']['chain_root']} - ${system_parameters['estimated_ft_sensor']['chain_tip']} - ${system_parameters['estimated_ft_sensor']['damping']} - ${system_parameters['estimated_ft_sensor']['force_x_th']} - ${system_parameters['estimated_ft_sensor']['force_y_th']} - ${system_parameters['estimated_ft_sensor']['force_z_th']} - ${system_parameters['estimated_ft_sensor']['torque_x_th']} - ${system_parameters['estimated_ft_sensor']['torque_y_th']} - ${system_parameters['estimated_ft_sensor']['torque_z_th']} + ${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']} @@ -114,7 +114,7 @@ ${max_position} ${max_velocity} ${max_torque} - + diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro index 1ff8bcc8..2b5b5f8b 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -9,8 +9,8 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> @@ -26,5 +26,5 @@ + system_config_path="$(arg system_config_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 b4eb7212..fde51896 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> @@ -25,7 +25,7 @@ robot_name="${robot_name}" mode="${mode}" joint_limits="${joint_limits}" - system_parameters_path="${system_parameters_path}" /> + system_config_path="${system_config_path}" /> diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index 5f0712ce..f06f1c64 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -9,8 +9,8 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> @@ -26,5 +26,5 @@ + system_config_path="$(arg system_config_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 089ffe65..e9bfaa1e 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> @@ -25,7 +25,7 @@ robot_name="${robot_name}" mode="${mode}" joint_limits="${joint_limits}" - system_parameters_path="${system_parameters_path}" /> + system_config_path="${system_config_path}" /> diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index df863a1a..2388c3ac 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -9,8 +9,8 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> @@ -26,5 +26,5 @@ + system_config_path="$(arg system_config_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 e76b72f9..6a5e5a27 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> @@ -25,7 +25,7 @@ robot_name="${robot_name}" mode="${mode}" joint_limits="${joint_limits}" - system_parameters_path="${system_parameters_path}" /> + system_config_path="${system_config_path}" /> diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index 01df31af..be27c65d 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -9,8 +9,8 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> @@ -26,5 +26,5 @@ + system_config_path="$(arg system_config_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 675b1a2c..1b9998f8 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> @@ -25,7 +25,7 @@ robot_name="${robot_name}" mode="${mode}" joint_limits="${joint_limits}" - system_parameters_path="${system_parameters_path}" /> + system_config_path="${system_config_path}" /> diff --git a/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg b/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg index 1ea801a2..939e9932 100644 --- a/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg +++ b/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg @@ -1,4 +1,4 @@ -
lbr_fri_ros2
lbr_fri_ros2::App
Runs the lbr_fri_ros2::AsyncClient at the robot's sample time. This sample time can be different from the controller_manager's update_rate!
lbr_fri_ros2::AsyncClient
The client with different interfaces, depending on the robot's command mode.
1
lbr_ros2_control
lbr_ros2_control::SystemInterface
The lbr_ros2_control::SystemInterface has an lbr_fri_ros2::App for communicating with the robot. It further implements the hardware_interface::SystemInterface interfaces for ROS 2 control integration.
1
1
1
1
controller_manager::ControllerManager
The controller_manager::ControllerManager loads the lbr_ros2_control::SystemInterface at runtime. It has a shared instance of the controller_manager node.
controller_manager

This node is run at update_rate.
hardware_interface::SystemInterface
Base class for ROS 2 control system plugins.
parent
child
lbr_system_paramters yaml
Robot configurations such as control mode, port_id, and others
Loads
1
Requests
1
lbr_system_interface.xacro
This xacro file tells the controller_manager::ControllerManager to load the lbr_ros2_control::SystemInterface.

It loads system_parameters.yaml and forwards the configurations to lbr_ros2_control::SystemInterface.
robot_description
 \ No newline at end of file +
lbr_fri_ros2
lbr_fri_ros2::App
Runs the lbr_fri_ros2::AsyncClient at the robot's sample time. This sample time can be different from the controller_manager's update_rate!
lbr_fri_ros2::AsyncClient
The client with different interfaces, depending on the robot's command mode.
1
lbr_ros2_control
lbr_ros2_control::SystemInterface
The lbr_ros2_control::SystemInterface has an lbr_fri_ros2::App for communicating with the robot. It further implements the hardware_interface::SystemInterface interfaces for ROS 2 control integration.
1
1
1
1
controller_manager::ControllerManager
The controller_manager::ControllerManager loads the lbr_ros2_control::SystemInterface at runtime. It has a shared instance of the controller_manager node.
controller_manager

This node is run at update_rate.
hardware_interface::SystemInterface
Base class for ROS 2 control system plugins.
parent
child
lbr_system_paramters yaml
Robot configurations such as control mode, port_id, and others
Loads
1
Requests
1
lbr_system_interface.xacro
This xacro file tells the controller_manager::ControllerManager to load the lbr_ros2_control::SystemInterface.

It loads lbr_system_config.yaml and forwards the configurations to lbr_ros2_control::SystemInterface.
robot_description
 \ No newline at end of file diff --git a/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg b/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg index 86cfb20d..4406d270 100644 --- a/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg +++ b/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg @@ -1,4 +1,4 @@ -
lbr_system_parameters.yaml
controller_manager::ControllerManager
Robot
UDP Connection
lbr_ros2_control::SystemInterface
lbr_fri_ros2::App
Runs asynchronously
at robot's sample time
  • Owns:
    • Controllers
    • Hardware plugins, i.e.
      lbr_ros2_control::SystemInterface
  • Runs at update_rate
Controller A
Controller B
 \ No newline at end of file +
lbr_system_config.yaml
controller_manager::ControllerManager
Robot
UDP Connection
lbr_ros2_control::SystemInterface
lbr_fri_ros2::App
Runs asynchronously
at robot's sample time
  • Owns:
    • Controllers
    • Hardware plugins, i.e.
      lbr_ros2_control::SystemInterface
  • Runs at update_rate
Controller A
Controller B
 \ No newline at end of file diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index bd94ef42..07bc8c7c 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -43,7 +43,7 @@ The ``lbr_ros2_control::SystemInterface`` plugin implements a ``hardware_interfa :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_paramters.yaml `_: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. From 62a5cfaaf336cbad6cc36171a7a84fc1b3bdfb37 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 11:41:01 +0100 Subject: [PATCH 11/42] fixed the default path to system config --- lbr_bringup/lbr_bringup/description.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index c38d456f..90b92836 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -26,7 +26,10 @@ def param_robot_description( system_config_path: Optional[ Union[LaunchConfiguration, str] ] = PathJoinSubstitution( - [LaunchConfiguration("sys_cfg_pkg"), LaunchConfiguration("sys_cfg")] + [ + FindPackageShare(LaunchConfiguration("sys_cfg_pkg")), + LaunchConfiguration("sys_cfg"), + ] ), ) -> Dict[str, str]: robot_description = { @@ -47,7 +50,7 @@ def param_robot_description( robot_name, " mode:=", mode, - "system_config_path:=", + " system_config_path:=", system_config_path, ] ) From 0115ba2567a2a7e939c58994c0d13a8541cc1014 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 14:23:44 +0100 Subject: [PATCH 12/42] rviz_config -> rviz_cfg --- README.md | 4 ++-- lbr_bringup/doc/lbr_bringup.rst | 8 ++++---- lbr_bringup/launch/move_group.launch.py | 4 ++-- lbr_bringup/launch/rviz.launch.py | 4 ++-- lbr_bringup/lbr_bringup/rviz.py | 26 ++++++++++++------------- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 6a8dd0d3..08c5da0f 100644 --- a/README.md +++ b/README.md @@ -78,8 +78,8 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io ```shell source install/setup.bash ros2 launch lbr_bringup rviz.launch.py \ - rviz_config_pkg:=lbr_bringup \ - rviz_config:=config/mock.rviz + rviz_cfg_pkg:=lbr_bringup \ + rviz_cfg:=config/mock.rviz ``` 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). diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 7493620c..b0395b29 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -114,8 +114,8 @@ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.lau .. code:: bash ros2 launch lbr_bringup rviz.launch.py \ - rviz_config_pkg:=lbr_bringup \ - rviz_config:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz] + rviz_cfg_pkg:=lbr_bringup \ + rviz_cfg:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz] .. note:: List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``. @@ -156,8 +156,8 @@ The below shows an example of the `rviz.launch.py List[LaunchDescriptionEntity]: # RViz if desired rviz = RVizMixin.node_rviz( - rviz_config_pkg=f"{model}_moveit_config", - rviz_config="config/moveit.rviz", + rviz_cfg_pkg=f"{model}_moveit_config", + rviz_cfg="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( moveit_configs=moveit_configs_builder.to_moveit_configs() ) diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py index cacc2faf..51857d14 100644 --- a/lbr_bringup/launch/rviz.launch.py +++ b/lbr_bringup/launch/rviz.launch.py @@ -6,8 +6,8 @@ def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() # launch arguments - ld.add_action(RVizMixin.arg_rviz_config()) - ld.add_action(RVizMixin.arg_rviz_config_pkg()) + ld.add_action(RVizMixin.arg_rviz_cfg()) + ld.add_action(RVizMixin.arg_rviz_cfg_pkg()) # rviz ld.add_action(RVizMixin.node_rviz()) diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py index 824bee74..b85e0ffe 100644 --- a/lbr_bringup/lbr_bringup/rviz.py +++ b/lbr_bringup/lbr_bringup/rviz.py @@ -16,32 +16,32 @@ def arg_rviz(default_value: str = "false") -> DeclareLaunchArgument: ) @staticmethod - def arg_rviz_config_pkg( + def arg_rviz_cfg_pkg( default_value: str = "lbr_bringup", ) -> DeclareLaunchArgument: return DeclareLaunchArgument( - name="rviz_config_pkg", + name="rviz_cfg_pkg", default_value=default_value, - description="The RViz configuration file.", + description="The package containing the RViz configuration file.", ) @staticmethod - def arg_rviz_config( + def arg_rviz_cfg( default_value: str = "config/mock.rviz", ) -> DeclareLaunchArgument: return DeclareLaunchArgument( - name="rviz_config", + name="rviz_cfg", default_value=default_value, - description="The RViz configuration file.", + description="The RViz configuration file relative to rviz_cfg_pkg.", ) @staticmethod def node_rviz( - rviz_config_pkg: Optional[ - Union[LaunchConfiguration, str] - ] = LaunchConfiguration("rviz_config_pkg", default="lbr_bringup"), - rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "rviz_config", default="config/mock.rviz" + rviz_cfg_pkg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_cfg_pkg", default="lbr_bringup" + ), + rviz_cfg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_cfg", default="config/mock.rviz" ), **kwargs, ) -> Node: @@ -53,8 +53,8 @@ def node_rviz( "-d", PathJoinSubstitution( [ - FindPackageShare(rviz_config_pkg), - rviz_config, + FindPackageShare(rviz_cfg_pkg), + rviz_cfg, ] ), ], From dae64a8001a9b4c0fe5dfb239c2d80fe8775d435 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 15:10:25 +0100 Subject: [PATCH 13/42] added a kinematics class --- lbr_fri_ros2/CMakeLists.txt | 1 + .../include/lbr_fri_ros2/ft_estimator.hpp | 30 +++------ .../include/lbr_fri_ros2/kinematics.hpp | 58 +++++++++++++++++ lbr_fri_ros2/src/ft_estimator.cpp | 31 ++------- lbr_fri_ros2/src/kinematics.cpp | 65 +++++++++++++++++++ 5 files changed, 139 insertions(+), 46 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp create mode 100644 lbr_fri_ros2/src/kinematics.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 47a55f94..e28e2132 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -35,6 +35,7 @@ add_library(lbr_fri_ros2 src/command_guard.cpp src/filters.cpp src/ft_estimator.cpp + src/kinematics.cpp ) target_include_directories(lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 8ef59405..48f1b04e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -4,20 +4,17 @@ #include #include #include +#include #include #include "eigen3/Eigen/Core" -#include "kdl/chain.hpp" -#include "kdl/chainfksolverpos_recursive.hpp" -#include "kdl/chainjnttojacsolver.hpp" -#include "kdl/tree.hpp" -#include "kdl_parser/kdl_parser.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" namespace lbr_fri_ros2 { @@ -30,8 +27,7 @@ class FTEstimator { using const_ext_tau_array_t_ref = const ext_tau_array_t &; public: - static constexpr uint8_t CARTESIAN_DOF = 6; - using cart_array_t = std::array; + using cart_array_t = std::array; using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; @@ -47,24 +43,14 @@ class FTEstimator { // force threshold cart_array_t f_ext_th_; - KDL::Tree tree_; - KDL::Chain chain_; - - // solvers - std::unique_ptr jacobian_solver_; - std::unique_ptr fk_solver_; - - // robot state - KDL::JntArray q_; - - // forward kinematics - KDL::Frame chain_tip_frame_; + // kinematics + std::unique_ptr kinematics_; // force estimation - KDL::Jacobian jacobian_; - Eigen::Matrix jacobian_inv_; + Eigen::Matrix + jacobian_inv_; Eigen::Matrix tau_ext_; - Eigen::Matrix f_ext_; + Eigen::Matrix f_ext_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp new file mode 100644 index 00000000..aa9f040b --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp @@ -0,0 +1,58 @@ +#ifndef LBR_FRI_ROS2__KINEMATICS_HPP_ +#define LBR_FRI_ROS2__KINEMATICS_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "kdl/chain.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainjnttojacsolver.hpp" +#include "kdl/tree.hpp" +#include "kdl_parser/kdl_parser.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +#include "friLBRState.h" + +#include "lbr_fri_idl/msg/lbr_state.hpp" + +namespace lbr_fri_ros2 { +class Kinematics { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics"; + +public: + static constexpr uint8_t CARTESIAN_DOF = 6; + using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type; + using const_jnt_pos_array_t_ref = const jnt_pos_array_t &; + + Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", + const std::string &chain_tip = "lbr_link_ee"); + + // internally computes the jacobian and return a reference to it + const KDL::Jacobian &compute_jacobian(const_jnt_pos_array_t_ref q); + const KDL::Jacobian &compute_jacobian(const std::vector &q); + + // forward kinematics + const KDL::Frame &compute_fk(const_jnt_pos_array_t_ref q); + const KDL::Frame &compute_fk(const std::vector &q); + +protected: + KDL::Tree tree_; + KDL::Chain chain_; + + // solvers + std::unique_ptr jacobian_solver_; + std::unique_ptr fk_solver_; + + // robot state + KDL::JntArray q_; + + // forward kinematics + KDL::Frame chain_tip_frame_; + + // jacobian + KDL::Jacobian jacobian_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__KINEMATICS_HPP_ diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 8ac0981c..c8378892 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -4,41 +4,25 @@ namespace lbr_fri_ros2 { FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, const std::string &chain_tip, const_cart_array_t_ref f_ext_th) : f_ext_th_(f_ext_th) { - if (!kdl_parser::treeFromString(robot_description, tree_)) { - std::string err = "Failed to construct kdl tree from robot description."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - if (!tree_.getChain(chain_root, chain_tip, chain_)) { - std::string err = "Failed to construct kdl chain from robot description."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - jacobian_solver_ = std::make_unique(chain_); - fk_solver_ = std::make_unique(chain_); - jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - + kinematics_ = std::make_unique(robot_description, chain_root, chain_tip); reset(); } void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, const double &damping) { - q_.data = Eigen::Map>( - measured_joint_position.data()); tau_ext_ = Eigen::Map>( external_torque.data()); - jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = pinv(jacobian_.data, damping); + auto jacobian = kinematics_->compute_jacobian(measured_joint_position); + jacobian_inv_ = pinv(jacobian.data, damping); f_ext_ = jacobian_inv_.transpose() * tau_ext_; // rotate into chain tip frame - fk_solver_->JntToCart(q_, chain_tip_frame_); - f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.topRows(3); - f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.bottomRows(3); + auto chain_tip_frame = kinematics_->compute_fk(measured_joint_position); + f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); + f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); - Eigen::Map>(f_ext.data()) = f_ext_; + Eigen::Map>(f_ext.data()) = f_ext_; // threshold force-torque std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), @@ -52,7 +36,6 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, } void FTEstimator::reset() { - q_.data.setZero(); tau_ext_.setZero(); f_ext_.setZero(); } diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp new file mode 100644 index 00000000..8ee6a11a --- /dev/null +++ b/lbr_fri_ros2/src/kinematics.cpp @@ -0,0 +1,65 @@ +#include "lbr_fri_ros2/kinematics.hpp" + +namespace lbr_fri_ros2 { +Kinematics::Kinematics(const std::string &robot_description, const std::string &chain_root, + const std::string &chain_tip) { + if (!kdl_parser::treeFromString(robot_description, tree_)) { + std::string err = "Failed to construct kdl tree from robot description."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + if (!tree_.getChain(chain_root, chain_tip, chain_)) { + std::string err = "Failed to construct kdl chain from robot description."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + std::string err = "Invalid number of joints in chain."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + jacobian_solver_ = std::make_unique(chain_); + fk_solver_ = std::make_unique(chain_); + jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + q_.data.setZero(); +} + +const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_pos_array_t_ref q) { + q_.data = + Eigen::Map>(q.data()); + jacobian_solver_->JntToJac(q_, jacobian_); + return jacobian_; +} + +const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) { + if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + std::string err = "Invalid number of joint positions."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + q_.data = + Eigen::Map>(q.data()); + jacobian_solver_->JntToJac(q_, jacobian_); + return jacobian_; +} + +const KDL::Frame &Kinematics::compute_fk(const_jnt_pos_array_t_ref q) { + q_.data = + Eigen::Map>(q.data()); + fk_solver_->JntToCart(q_, chain_tip_frame_); + return chain_tip_frame_; +} + +const KDL::Frame &Kinematics::compute_fk(const std::vector &q) { + if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + std::string err = "Invalid number of joint positions."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + q_.data = + Eigen::Map>(q.data()); + fk_solver_->JntToCart(q_, chain_tip_frame_); + return chain_tip_frame_; +} +} // namespace lbr_fri_ros2 From ffe3fac3dfd39a63830823d4d0488a38391e83a6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 17:09:48 +0100 Subject: [PATCH 14/42] added a twist controller --- lbr_bringup/lbr_bringup/ros2_control.py | 1 + .../include/lbr_fri_ros2/ft_estimator.hpp | 2 +- lbr_fri_ros2/src/ft_estimator.cpp | 6 +- lbr_ros2_control/CMakeLists.txt | 25 ++- lbr_ros2_control/config/lbr_controllers.yaml | 16 +- .../controllers/twist_controller.hpp | 101 +++++++++ lbr_ros2_control/package.xml | 9 +- .../plugin_description_files/controllers.xml | 21 +- .../src/controllers/twist_controller.cpp | 203 ++++++++++++++++++ 9 files changed, 359 insertions(+), 25 deletions(-) create mode 100644 lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp create mode 100644 lbr_ros2_control/src/controllers/twist_controller.cpp diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index b8d6bffb..e45a5fb9 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -35,6 +35,7 @@ def arg_ctrl() -> DeclareLaunchArgument: "lbr_joint_position_command_controller", "lbr_torque_command_controller", "lbr_wrench_command_controller", + "twist_controller", ], ) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 48f1b04e..816bdd77 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -44,7 +44,7 @@ class FTEstimator { cart_array_t f_ext_th_; // kinematics - std::unique_ptr kinematics_; + std::unique_ptr kinematics_ptr_; // force estimation Eigen::Matrix diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index c8378892..93ec4037 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -4,7 +4,7 @@ namespace lbr_fri_ros2 { FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, const std::string &chain_tip, const_cart_array_t_ref f_ext_th) : f_ext_th_(f_ext_th) { - kinematics_ = std::make_unique(robot_description, chain_root, chain_tip); + kinematics_ptr_ = std::make_unique(robot_description, chain_root, chain_tip); reset(); } @@ -13,12 +13,12 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, const double &damping) { tau_ext_ = Eigen::Map>( external_torque.data()); - auto jacobian = kinematics_->compute_jacobian(measured_joint_position); + auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); jacobian_inv_ = pinv(jacobian.data, damping); f_ext_ = jacobian_inv_.transpose() * tau_ext_; // rotate into chain tip frame - auto chain_tip_frame = kinematics_->compute_fk(measured_joint_position); + auto chain_tip_frame = kinematics_ptr_->compute_fk(measured_joint_position); f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 6dc55918..cf019396 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -16,11 +16,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) find_package(FRIClient REQUIRED) find_package(hardware_interface REQUIRED) +find_package(kinematics_interface REQUIRED) find_package(lbr_fri_idl REQUIRED) find_package(lbr_fri_ros2 REQUIRED) -find_package(kinematics_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(realtime_tools REQUIRED) @@ -33,6 +35,7 @@ add_library( src/controllers/lbr_torque_command_controller.cpp src/controllers/lbr_wrench_command_controller.cpp src/controllers/lbr_state_broadcaster.cpp + src/controllers/twist_controller.cpp src/system_interface.cpp ) @@ -47,16 +50,21 @@ target_include_directories( ) # Link against dependencies -ament_target_dependencies( - ${PROJECT_NAME} +set(AMENT_DEPENDENCIES controller_interface + Eigen3 hardware_interface + kinematics_interface lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp realtime_tools ) +ament_target_dependencies( + ${PROJECT_NAME} + ${AMENT_DEPENDENCIES} +) target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient @@ -71,16 +79,11 @@ ament_export_targets( ) ament_export_dependencies( - controller_interface FRIClient - hardware_interface - lbr_fri_idl - lbr_fri_ros2 - pluginlib - rclcpp - realtime_tools + ${AMENT_DEPENDENCIES} + eigen3_cmake_module ) - + install( DIRECTORY include/ DESTINATION include diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 35878f5d..07052fbe 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -23,6 +23,9 @@ type: position_controllers/JointGroupPositionController # LBR ROS 2 control controllers + admittance_controller: + type: lbr_ros2_control/AdmittanceController + lbr_joint_position_command_controller: type: lbr_ros2_control/LBRJointPositionCommandController @@ -32,8 +35,8 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController - admittance_controller: - type: lbr_ros2_control/AdmittanceController + twist_controller: + type: lbr_ros2_control/TwistController # ROS 2 control broadcasters /**/force_torque_broadcaster: @@ -89,3 +92,12 @@ /**/lbr_wrench_command_controller: ros__parameters: robot_name: lbr + +/**/twist_controller: + ros__parameters: + robot_name: lbr + 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: 0.1 # maximum linear velocity + max_angular_velocity: 0.1 # maximum linear acceleration 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 new file mode 100644 index 00000000..ed364a25 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -0,0 +1,101 @@ +#ifndef LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "eigen3/Eigen/Core" +#include "geometry_msgs/msg/twist.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_buffer.h" + +#include "friLBRState.h" + +#include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/pinv.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +struct TwistParameters { + std::string chain_root; + std::string chain_tip; + double damping; + double max_linear_velocity; + double max_angular_velocity; +}; + +class TwistImpl { +public: + TwistImpl(const std::string &robot_description, const TwistParameters ¶meters); + + void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + lbr_fri_ros2::Kinematics::const_jnt_pos_array_t_ref q, + lbr_fri_ros2::Kinematics::jnt_pos_array_t &dq); + +protected: + TwistParameters parameters_; + + lbr_fri_ros2::Kinematics::jnt_pos_array_t q_; + std::unique_ptr kinematics_ptr_; + Eigen::Matrix + jacobian_inv_; + Eigen::Matrix twist_target_; +}; + +class TwistController : public controller_interface::ControllerInterface { +public: + TwistController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::return_type update(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 reference_state_interfaces_(); + void clear_state_interfaces_(); + void reset_command_buffer_(); + void configure_joint_names_(); + void configure_twist_impl_(); + + // joint veloctiy computation + std::unique_ptr twist_impl_ptr_; + lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_; + + // interfaces + std::array joint_names_; + std::vector> + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_; + std::unique_ptr> + session_state_interface_; + + // real-time twist command topic + realtime_tools::RealtimeBuffer rt_twist_ptr_; + rclcpp::Subscription::SharedPtr twist_subscription_ptr_; +}; +} // namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index f6f5b81a..932c667c 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -8,18 +8,25 @@ Apache-2.0 ament_cmake + eigen3_cmake_module + + eigen fri_client_sdk + geometry_msgs lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp realtime_tools ros2_control - + lbr_description ros2_controllers + eigen3_cmake_module + eigen + ament_cmake diff --git a/lbr_ros2_control/plugin_description_files/controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml index 82c4ae8e..10694639 100644 --- a/lbr_ros2_control/plugin_description_files/controllers.xml +++ b/lbr_ros2_control/plugin_description_files/controllers.xml @@ -1,9 +1,10 @@ - - + - Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + A simple admittance controller. @@ -14,6 +15,12 @@ lbr_fri_idl/msg/LBRJointPositionCommand.msg. + + + Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + + - - + - A simple admittance controller. + A simple twist controller. \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp new file mode 100644 index 00000000..e01d03e7 --- /dev/null +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -0,0 +1,203 @@ +#include "lbr_ros2_control/controllers/twist_controller.hpp" + +namespace lbr_ros2_control { +TwistImpl::TwistImpl(const std::string &robot_description, const TwistParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique( + robot_description, parameters_.chain_root, parameters_.chain_tip); +} + +void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + lbr_fri_ros2::Kinematics::const_jnt_pos_array_t_ref q, + lbr_fri_ros2::Kinematics::jnt_pos_array_t &dq) { + // twist to Eigen + twist_target_[0] = twist_target->linear.x; + twist_target_[1] = twist_target->linear.y; + twist_target_[2] = twist_target->linear.z; + twist_target_[3] = twist_target->angular.x; + twist_target_[4] = twist_target->angular.y; + twist_target_[5] = twist_target->angular.z; + + // clip velocity + 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) { + return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); + }); + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = + jacobian_inv_ * twist_target_; +} + +TwistController::TwistController() : rt_twist_ptr_(nullptr), twist_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +TwistController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + return interface_configuration; +} + +controller_interface::InterfaceConfiguration +TwistController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + 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; +} + +controller_interface::CallbackReturn TwistController::on_init() { + try { + twist_subscription_ptr_ = this->get_node()->create_subscription( + "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { + rt_twist_ptr_.writeFromNonRT(msg); + }); + this->get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("damping", 0.2); + this->get_node()->declare_parameter("max_linear_velocity", 0.1); + this->get_node()->declare_parameter("max_angular_velocity", 0.1); + configure_joint_names_(); + configure_twist_impl_(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + auto twist_command = rt_twist_ptr_.readFromRT(); + if (!twist_command || !(*twist_command)) { + return controller_interface::return_type::OK; + } + if (!twist_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Twist controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + + // pass joint positions to q_ + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute the joint velocity from the twist command target + twist_impl_ptr_->compute(*twist_command, q_, dq_); + + // pass joint positions to hardware + std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { + this->command_interfaces_[i].set_value( + q_i + dq_[i] * sample_time_state_interface_->get().get_value()); + ++i; + }); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!reference_state_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + clear_state_interfaces_(); + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TwistController::reference_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)); + } + if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { + sample_time_state_interface_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ = + std::make_unique>( + std::ref(state_interface)); + } + } + if (joint_position_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint position state interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", + joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return false; + } + return true; +} + +void TwistController::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); } + +void TwistController::reset_command_buffer_() { + rt_twist_ptr_ = + realtime_tools::RealtimeBuffer>(nullptr); +}; + +void TwistController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} + +void TwistController::configure_twist_impl_() { + twist_impl_ptr_ = std::make_unique( + this->get_robot_description(), + TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), + this->get_node()->get_parameter("chain_tip").as_string(), + this->get_node()->get_parameter("damping").as_double(), + this->get_node()->get_parameter("max_linear_velocity").as_double(), + this->get_node()->get_parameter("max_angular_velocity").as_double()}); +} +} // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::TwistController, controller_interface::ControllerInterface) From b9c1c43cf6fe84930f881dab90092c3ce2a7ed28 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 17:21:39 +0100 Subject: [PATCH 15/42] turn off controller in case to command are received --- .../controllers/twist_controller.hpp | 5 +++++ .../src/controllers/twist_controller.cpp | 12 +++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) 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 ed364a25..b1503a2d 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 @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -80,6 +81,10 @@ class TwistController : public controller_interface::ControllerInterface { void configure_joint_names_(); void configure_twist_impl_(); + // some safety checks + std::atomic updates_since_last_command_ = 0; + double max_time_without_command_ = 0.2; + // joint veloctiy computation std::unique_ptr twist_impl_ptr_; lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_; diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index e01d03e7..f93f0981 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -66,6 +66,7 @@ controller_interface::CallbackReturn TwistController::on_init() { twist_subscription_ptr_ = this->get_node()->create_subscription( "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { rt_twist_ptr_.writeFromNonRT(msg); + updates_since_last_command_ = 0; }); this->get_node()->declare_parameter("robot_name", "lbr"); this->get_node()->declare_parameter("chain_root", "lbr_link_0"); @@ -85,7 +86,7 @@ controller_interface::CallbackReturn TwistController::on_init() { } controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { + const rclcpp::Duration &period) { auto twist_command = rt_twist_ptr_.readFromRT(); if (!twist_command || !(*twist_command)) { return controller_interface::return_type::OK; @@ -98,6 +99,13 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } + if (updates_since_last_command_ > + static_cast(max_time_without_command_ / period.seconds())) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "No twist command received within time %f. Stopping the controller.", + max_time_without_command_); + return controller_interface::return_type::ERROR; + } // pass joint positions to q_ std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { @@ -115,6 +123,8 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / ++i; }); + ++updates_since_last_command_; + return controller_interface::return_type::OK; } From 27fcbd49b2b86df4254db901d529dc939a72c6ef Mon Sep 17 00:00:00 2001 From: mhubii Date: Sun, 20 Oct 2024 17:38:35 +0100 Subject: [PATCH 16/42] added default values for sys cfg --- lbr_bringup/lbr_bringup/description.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 90b92836..161bf12e 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -27,8 +27,12 @@ def param_robot_description( Union[LaunchConfiguration, str] ] = PathJoinSubstitution( [ - FindPackageShare(LaunchConfiguration("sys_cfg_pkg")), - LaunchConfiguration("sys_cfg"), + FindPackageShare( + LaunchConfiguration("sys_cfg_pkg", default="lbr_description") + ), + LaunchConfiguration( + "sys_cfg", default="ros2_control/lbr_system_config.yaml" + ), ] ), ) -> Dict[str, str]: From bb31bcc7f128d9e30bc5e8192ffbb45b781d1582 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 21 Oct 2024 10:28:34 +0100 Subject: [PATCH 17/42] added a floating link with name prefix (#189) --- lbr_bringup/config/gazebo.rviz | 8 ++++---- lbr_bringup/config/hardware.rviz | 8 ++++---- lbr_bringup/config/mock.rviz | 8 ++++---- lbr_bringup/launch/gazebo.launch.py | 8 +++++--- lbr_bringup/launch/hardware.launch.py | 8 +++++--- lbr_bringup/launch/mock.launch.py | 8 +++++--- lbr_description/urdf/iiwa14/iiwa14.xacro | 10 +++++----- lbr_description/urdf/iiwa7/iiwa7.xacro | 10 +++++----- lbr_description/urdf/med14/med14.xacro | 10 +++++----- lbr_description/urdf/med7/med7.xacro | 10 +++++----- .../iiwa14_moveit_config/config/moveit.rviz | 16 ++++++++-------- .../iiwa7_moveit_config/config/moveit.rviz | 16 ++++++++-------- .../med14_moveit_config/config/moveit.rviz | 16 ++++++++-------- .../med7_moveit_config/config/moveit.rviz | 16 ++++++++-------- 14 files changed, 79 insertions(+), 73 deletions(-) diff --git a/lbr_bringup/config/gazebo.rviz b/lbr_bringup/config/gazebo.rviz index c45723d4..cbcb143b 100644 --- a/lbr_bringup/config/gazebo.rviz +++ b/lbr_bringup/config/gazebo.rviz @@ -67,6 +67,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -111,10 +115,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Mass Properties: Inertia: false Mass: false diff --git a/lbr_bringup/config/hardware.rviz b/lbr_bringup/config/hardware.rviz index 86a38438..597820ee 100644 --- a/lbr_bringup/config/hardware.rviz +++ b/lbr_bringup/config/hardware.rviz @@ -68,6 +68,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -112,10 +116,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Mass Properties: Inertia: false Mass: false diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz index c45723d4..cbcb143b 100644 --- a/lbr_bringup/config/mock.rviz +++ b/lbr_bringup/config/mock.rviz @@ -67,6 +67,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -111,10 +115,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Mass Properties: Inertia: false Mass: false diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index bc1ff586..34da3615 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -1,5 +1,5 @@ from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PythonExpression from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.gazebo import GazeboMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -15,13 +15,15 @@ def generate_launch_description() -> LaunchDescription: LBRROS2ControlMixin.arg_ctrl() ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml - # static transform world -> robot_name/world + # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero ld.add_action( LBRDescriptionMixin.node_static_tf( tf=world_robot_tf, parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index a0f41313..d6ec6b9b 100644 --- a/lbr_bringup/launch/hardware.launch.py +++ b/lbr_bringup/launch/hardware.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PythonExpression from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -18,12 +18,14 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - # static transform world -> robot_name/world + # static transform world -> _floating_link ld.add_action( LBRDescriptionMixin.node_static_tf( tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py index 3443431f..a87be2d4 100644 --- a/lbr_bringup/launch/mock.launch.py +++ b/lbr_bringup/launch/mock.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PythonExpression from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -16,12 +16,14 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - # static transform world -> robot_name/world + # static transform world -> _floating_link ld.add_action( LBRDescriptionMixin.node_static_tf( tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro index 2b5b5f8b..860438bf 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -12,13 +12,13 @@ name="system_config_path" default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - + between _floating_link and robot_name_link_0--> + + diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index f06f1c64..fb9aca5e 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -12,13 +12,13 @@ name="system_config_path" default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - + between _floating_link and robot_name_link_0--> + + diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index 2388c3ac..8cfa9566 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -12,13 +12,13 @@ name="system_config_path" default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - + between _floating_link and robot_name_link_0--> + + diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index be27c65d..81e2a72d 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -12,13 +12,13 @@ name="system_config_path" default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - + between _floating_link and robot_name_link_0--> + + diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz index 9fea5cff..948e32b5 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz @@ -66,6 +66,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -110,10 +114,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -160,6 +160,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -204,10 +208,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz index 9fea5cff..948e32b5 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz @@ -66,6 +66,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -110,10 +114,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -160,6 +160,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -204,10 +208,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz index 9fea5cff..948e32b5 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz @@ -66,6 +66,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -110,10 +114,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -160,6 +160,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -204,10 +208,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz index 9fea5cff..948e32b5 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz @@ -66,6 +66,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -110,10 +114,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -160,6 +160,10 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + lbr_floating_link: + Alpha: 1 + Show Axes: false + Show Trail: false lbr_link_0: Alpha: 1 Show Axes: false @@ -204,10 +208,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true From 079232b93ea8f179380400172ad9874d6a9afbaf Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 10:41:54 +0100 Subject: [PATCH 18/42] bundling types in shared header --- .../include/lbr_fri_ros2/command_guard.hpp | 13 +----- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 14 +++---- .../include/lbr_fri_ros2/ft_estimator.hpp | 19 +++------ .../include/lbr_fri_ros2/kinematics.hpp | 9 ++--- lbr_fri_ros2/include/lbr_fri_ros2/types.hpp | 40 +++++++++++++++++++ lbr_fri_ros2/src/app.cpp | 3 +- lbr_fri_ros2/src/command_guard.cpp | 2 +- lbr_fri_ros2/src/filters.cpp | 10 ++--- lbr_fri_ros2/src/ft_estimator.cpp | 6 +-- lbr_fri_ros2/src/kinematics.cpp | 4 +- .../controllers/admittance_controller.hpp | 7 ++-- .../lbr_joint_position_command_controller.hpp | 3 +- .../controllers/lbr_state_broadcaster.hpp | 4 +- .../lbr_torque_command_controller.hpp | 3 +- .../lbr_wrench_command_controller.hpp | 3 +- .../controllers/twist_controller.hpp | 21 +++++----- .../lbr_ros2_control/system_interface.hpp | 3 +- .../src/controllers/admittance_controller.cpp | 5 ++- .../src/controllers/twist_controller.cpp | 3 +- lbr_ros2_control/src/system_interface.cpp | 10 +++-- 20 files changed, 102 insertions(+), 80 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/types.hpp diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 9b963266..c4a86b05 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -17,13 +17,10 @@ #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { struct CommandGuardParameters { - // ROS IDL types - using jnt_array_t = std::array; - using jnt_name_array_t = std::array; - jnt_name_array_t joint_names; /**< Joint names.*/ jnt_array_t min_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint positions [rad].*/ jnt_array_t max_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint positions [rad].*/ @@ -35,14 +32,6 @@ class CommandGuard { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; - // ROS IDL types - using idl_command_t = lbr_fri_idl::msg::LBRCommand; - using const_idl_command_t_ref = const idl_command_t &; - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - using jnt_array_t = idl_command_t::_joint_position_type; - using const_jnt_array_t_ref = const jnt_array_t &; - public: CommandGuard() = default; CommandGuard(const CommandGuardParameters &command_guard_parameters); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index f14d5915..07dcff6d 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -12,6 +12,7 @@ #include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { @@ -96,12 +97,10 @@ class ExponentialFilter { }; class JointExponentialFilterArray { - using value_array_t = std::array; - public: JointExponentialFilterArray() = default; - void compute(const double *const current, value_array_t &previous); + void compute(const double *const current, jnt_array_t_ref previous); void initialize(const double &cutoff_frequency, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; @@ -122,17 +121,16 @@ struct PIDParameters { class JointPIDArray { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using value_array_t = std::array; using pid_array_t = std::array; public: JointPIDArray() = delete; JointPIDArray(const PIDParameters &pid_parameters); - void compute(const value_array_t &command_target, const value_array_t &state, - const std::chrono::nanoseconds &dt, value_array_t &command); - void compute(const value_array_t &command_target, const double *state, - const std::chrono::nanoseconds &dt, value_array_t &command); + void compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command); + void compute(const_jnt_array_t_ref command_target, const double *state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command); void log_info() const; protected: diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 816bdd77..abd13f94 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -16,27 +16,19 @@ #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" namespace lbr_fri_ros2 { class FTEstimator { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; - using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type; - using const_jnt_pos_array_t_ref = const jnt_pos_array_t &; - using ext_tau_array_t = lbr_fri_idl::msg::LBRState::_external_torque_type; - using const_ext_tau_array_t_ref = const ext_tau_array_t &; public: - using cart_array_t = std::array; - using cart_array_t_ref = cart_array_t &; - using const_cart_array_t_ref = const cart_array_t &; - FTEstimator(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}); - void compute(const_jnt_pos_array_t_ref measured_joint_position, - const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, - const double &damping = 0.2); + void compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque, + cart_array_t_ref f_ext, const double &damping = 0.2); void reset(); protected: @@ -47,10 +39,9 @@ class FTEstimator { std::unique_ptr kinematics_ptr_; // force estimation - Eigen::Matrix - jacobian_inv_; + Eigen::Matrix jacobian_inv_; Eigen::Matrix tau_ext_; - Eigen::Matrix f_ext_; + Eigen::Matrix f_ext_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp index aa9f040b..4192d400 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp @@ -15,6 +15,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class Kinematics { @@ -22,19 +23,15 @@ class Kinematics { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics"; public: - static constexpr uint8_t CARTESIAN_DOF = 6; - using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type; - using const_jnt_pos_array_t_ref = const jnt_pos_array_t &; - Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", const std::string &chain_tip = "lbr_link_ee"); // internally computes the jacobian and return a reference to it - const KDL::Jacobian &compute_jacobian(const_jnt_pos_array_t_ref q); + const KDL::Jacobian &compute_jacobian(const_jnt_array_t_ref q); const KDL::Jacobian &compute_jacobian(const std::vector &q); // forward kinematics - const KDL::Frame &compute_fk(const_jnt_pos_array_t_ref q); + const KDL::Frame &compute_fk(const_jnt_array_t_ref q); const KDL::Frame &compute_fk(const std::vector &q); protected: diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp new file mode 100644 index 00000000..d039fcae --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp @@ -0,0 +1,40 @@ +#ifndef LBR_FRI_ROS2__TYPES_HPP_ +#define LBR_FRI_ROS2__TYPES_HPP_ + +#include +#include +#include + +#include "friLBRState.h" + +#include "lbr_fri_idl/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" + +namespace lbr_fri_ros2 { +// joint positions, velocities, accelerations, torques etc. +using jnt_array_t = std::array; +using jnt_array_t_ref = jnt_array_t &; +using const_jnt_array_t_ref = const jnt_array_t &; + +// joint names +using jnt_name_array_t = std::array; +using jnt_name_array_t_ref = jnt_name_array_t &; +using const_jnt_name_array_t_ref = const jnt_name_array_t &; + +// cartesian dof +constexpr std::uint8_t CARTESIAN_DOF = 6; + +// cartesian positions, velocities, accelerations, torques etc. +using cart_array_t = std::array; +using cart_array_t_ref = cart_array_t &; +using const_cart_array_t_ref = const cart_array_t &; + +// idl types +using idl_command_t = lbr_fri_idl::msg::LBRCommand; +using idl_command_t_ref = idl_command_t &; +using const_idl_command_t_ref = const idl_command_t &; +using idl_state_t = lbr_fri_idl::msg::LBRState; +using idl_state_t_ref = idl_state_t &; +using const_idl_state_t_ref = const idl_state_t &; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__TYPES_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 4d277d35..d8860834 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -107,7 +107,8 @@ void App::run_async(int rt_prio) { should_stop_ = false; bool success = true; while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); // stuck if never connected + success = app_ptr_->step(); // stuck if never connected, we thus detach the thread as join may + // never return running_ = true; if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 92d06e5b..c7bff823 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters) - : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false){}; + : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false) {}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, const_idl_state_t_ref lbr_state) { diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 4047e59a..983835d2 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -29,7 +29,7 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } -void JointExponentialFilterArray::compute(const double *const current, value_array_t &previous) { +void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, [&, i = 0](const auto ¤t_i) mutable { previous[i] = exponential_filter_.compute(current_i, previous[i]); @@ -54,16 +54,16 @@ JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) }); } -void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, - const std::chrono::nanoseconds &dt, value_array_t &command) { +void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); ++i; }); } -void JointPIDArray::compute(const value_array_t &command_target, const double *state, - const std::chrono::nanoseconds &dt, value_array_t &command) { +void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); ++i; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 93ec4037..05ba96aa 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -8,8 +8,8 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string reset(); } -void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, - const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, +void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, + const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, const double &damping) { tau_ext_ = Eigen::Map>( external_torque.data()); @@ -22,7 +22,7 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); - Eigen::Map>(f_ext.data()) = f_ext_; + Eigen::Map>(f_ext.data()) = f_ext_; // threshold force-torque std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp index 8ee6a11a..b7da0ebe 100644 --- a/lbr_fri_ros2/src/kinematics.cpp +++ b/lbr_fri_ros2/src/kinematics.cpp @@ -25,7 +25,7 @@ Kinematics::Kinematics(const std::string &robot_description, const std::string & q_.data.setZero(); } -const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_pos_array_t_ref q) { +const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) { q_.data = Eigen::Map>(q.data()); jacobian_solver_->JntToJac(q_, jacobian_); @@ -44,7 +44,7 @@ const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) return jacobian_; } -const KDL::Frame &Kinematics::compute_fk(const_jnt_pos_array_t_ref q) { +const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) { q_.data = Eigen::Map>(q.data()); fk_solver_->JntToCart(q_, chain_tip_frame_); 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 1a75be92..f6909e11 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 @@ -14,12 +14,13 @@ #include "rclcpp/rclcpp.hpp" #include "friLBRState.h" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { class Admittance { - -} + // implement an addmittance... +}; class AdmittanceController : public controller_interface::ControllerInterface { static constexpr uint8_t CARTESIAN_DOF = 6; @@ -52,7 +53,7 @@ class AdmittanceController : public controller_interface::ControllerInterface { void clear_state_interfaces_(); void configure_joint_names_(); - std::array joint_names_; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp index 929dcf63..12bd9d00 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp @@ -15,6 +15,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_joint_position_command.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_ros2_control { class LBRJointPositionCommandController : public controller_interface::ControllerInterface { @@ -42,7 +43,7 @@ class LBRJointPositionCommandController : public controller_interface::Controlle protected: void configure_joint_names_(); - std::array joint_names_; + lbr_fri_ros2::jnt_name_array_t joint_names_; realtime_tools::RealtimeBuffer rt_lbr_joint_position_command_ptr_; 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 70efcdb3..c89d564b 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 @@ -19,6 +19,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -49,8 +50,7 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { void init_state_msg_(); void configure_joint_names_(); - std::array joint_names_ = { - "lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::unordered_map> state_interface_map_; rclcpp::Publisher::SharedPtr 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 65d6f0a8..8094e065 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 @@ -17,6 +17,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_torque_command.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_ros2_control { class LBRTorqueCommandController : public controller_interface::ControllerInterface { @@ -46,7 +47,7 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf void clear_command_interfaces_(); void configure_joint_names_(); - std::array joint_names_; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_command_interfaces_, torque_command_interfaces_; 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 c145f043..8f43904f 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 @@ -17,6 +17,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_wrench_command.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -49,7 +50,7 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf void clear_command_interfaces_(); void configure_joint_names_(); - std::array joint_names_; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_command_interfaces_, wrench_command_interfaces_; 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 b1503a2d..ddbbc9aa 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 @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ -#define LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ +#ifndef LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ #include #include @@ -22,6 +22,7 @@ #include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -38,18 +39,16 @@ class TwistImpl { TwistImpl(const std::string &robot_description, const TwistParameters ¶meters); void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, - lbr_fri_ros2::Kinematics::const_jnt_pos_array_t_ref q, - lbr_fri_ros2::Kinematics::jnt_pos_array_t &dq); + lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq); protected: TwistParameters parameters_; - lbr_fri_ros2::Kinematics::jnt_pos_array_t q_; + lbr_fri_ros2::jnt_array_t q_; std::unique_ptr kinematics_ptr_; - Eigen::Matrix + Eigen::Matrix jacobian_inv_; - Eigen::Matrix twist_target_; + Eigen::Matrix twist_target_; }; class TwistController : public controller_interface::ControllerInterface { @@ -87,10 +86,10 @@ class TwistController : public controller_interface::ControllerInterface { // joint veloctiy computation std::unique_ptr twist_impl_ptr_; - lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_; + lbr_fri_ros2::jnt_array_t q_, dq_; // interfaces - std::array joint_names_; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_state_interfaces_; std::unique_ptr> @@ -103,4 +102,4 @@ class TwistController : public controller_interface::ControllerInterface { rclcpp::Subscription::SharedPtr twist_subscription_ptr_; }; } // namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_ +#endif // LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ 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 42a4e77a..ffa14fd7 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -27,6 +27,7 @@ #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -160,7 +161,7 @@ class SystemInterface : public hardware_interface::SystemInterface { void compute_hw_velocity_(); // additional force-torque state interface - lbr_fri_ros2::FTEstimator::cart_array_t hw_ft_; + lbr_fri_ros2::cart_array_t hw_ft_; std::unique_ptr ft_estimator_ptr_; // exposed command interfaces diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 8c1a94f4..1d4eb80a 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -4,11 +4,12 @@ namespace lbr_ros2_control { AdmittanceController::AdmittanceController() {} controller_interface::InterfaceConfiguration -AdmittanceController::command_interface_configuration() { +AdmittanceController::command_interface_configuration() const { // reference joint position command interface } -controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() { +controller_interface::InterfaceConfiguration +AdmittanceController::state_interface_configuration() const { // retrieve estimated ft state interface } diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index f93f0981..c259ac2e 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -8,8 +8,7 @@ TwistImpl::TwistImpl(const std::string &robot_description, const TwistParameters } void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, - lbr_fri_ros2::Kinematics::const_jnt_pos_array_t_ref q, - lbr_fri_ros2::Kinematics::jnt_pos_array_t &dq) { + lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq) { // twist to Eigen twist_target_[0] = twist_target->linear.x; twist_target_[1] = twist_target->linear.y; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index b98d6d5b..e4fdbf6c 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -11,7 +11,7 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { return ret; } - // parameters_ from lbr_system_interface.xacro (located in + // parameters_ from lbr_system_interface.xacro (default configurations located in // lbr_description/ros2_control/lbr_system_interface.xacro) if (!parse_parameters_(system_info)) { return controller_interface::CallbackReturn::ERROR; @@ -72,7 +72,7 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th")); ft_estimator_ptr_ = std::make_unique( info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, - lbr_fri_ros2::FTEstimator::cart_array_t{ + lbr_fri_ros2::cart_array_t{ ft_parameters_.force_x_th, ft_parameters_.force_y_th, ft_parameters_.force_z_th, @@ -371,11 +371,13 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & parameters_.rt_prio = std::stoul(info_.hardware_parameters["rt_prio"]); std::transform(info_.hardware_parameters["open_loop"].begin(), info_.hardware_parameters["open_loop"].end(), - info_.hardware_parameters["open_loop"].begin(), ::tolower); + info_.hardware_parameters["open_loop"].begin(), + ::tolower); // convert to lower case parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), info_.hardware_parameters["pid_antiwindup"].end(), - info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); + info_.hardware_parameters["pid_antiwindup"].begin(), + ::tolower); // convert to lower case parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); From ea11f1ecc69333b675ea70876a72ad1432714498 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 10:57:41 +0100 Subject: [PATCH 19/42] replaced types among interfaces --- .../lbr_fri_ros2/interfaces/base_command.hpp | 12 +----------- .../include/lbr_fri_ros2/interfaces/state.hpp | 15 ++------------- lbr_fri_ros2/include/lbr_fri_ros2/types.hpp | 16 ++++++++++++---- lbr_fri_ros2/src/interfaces/state.cpp | 10 +++++----- 4 files changed, 20 insertions(+), 33 deletions(-) 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 eab3b4a3..bb46d0ae 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 @@ -10,28 +10,18 @@ #include "rclcpp/logging.hpp" #include "friClientVersion.h" -#include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class BaseCommandInterface { protected: virtual std::string LOGGER_NAME() const = 0; - // ROS IDL types - using idl_command_t = lbr_fri_idl::msg::LBRCommand; - using const_idl_command_t_ref = const idl_command_t &; - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - - // FRI types - using fri_command_t = KUKA::FRI::LBRCommand; - using fri_command_t_ref = fri_command_t &; - public: BaseCommandInterface() = delete; BaseCommandInterface(const PIDParameters &pid_parameters, diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 1a943bd7..23457637 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -7,10 +7,10 @@ #include "rclcpp/logging.hpp" #include "friClientVersion.h" -#include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { struct StateInterfaceParameters { @@ -22,17 +22,6 @@ class StateInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; - // ROS IDL types - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - using idl_joint_pos_t = idl_state_t::_measured_joint_position_type; - using const_idl_joint_pos_t_ref = const idl_joint_pos_t &; - - // FRI types - using fri_state_t = KUKA::FRI::LBRState; - using const_fri_state_t_ref = const fri_state_t &; - using fri_session_state_t = KUKA::FRI::ESessionState; - public: StateInterface() = delete; StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); @@ -40,7 +29,7 @@ class StateInterface { inline const_idl_state_t_ref get_state() const { return state_; }; void set_state(const_fri_state_t_ref state); - void set_state_open_loop(const_fri_state_t_ref state, const_idl_joint_pos_t_ref joint_position); + void set_state_open_loop(const_fri_state_t_ref state, const_jnt_array_t_ref joint_position); inline void uninitialize() { state_initialized_ = false; } inline bool is_initialized() const { return state_initialized_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp index d039fcae..fbd2a610 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp @@ -5,7 +5,7 @@ #include #include -#include "friLBRState.h" +#include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_idl/msg/lbr_state.hpp" @@ -21,15 +21,23 @@ using jnt_name_array_t = std::array; using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; -// idl types +// FRI types +using fri_command_t = KUKA::FRI::LBRCommand; +using fri_command_t_ref = fri_command_t &; +using const_fri_command_t_ref = const fri_command_t &; +using fri_state_t = KUKA::FRI::LBRState; +using fri_state_t_ref = fri_state_t &; +using const_fri_state_t_ref = const fri_state_t &; + +// ROS IDL types using idl_command_t = lbr_fri_idl::msg::LBRCommand; using idl_command_t_ref = idl_command_t &; using const_idl_command_t_ref = const idl_command_t &; diff --git a/lbr_fri_ros2/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 7e49c9bd..3054bf55 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -16,8 +16,8 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); - if (state.getSessionState() == fri_session_state_t::COMMANDING_WAIT || - state.getSessionState() == fri_session_state_t::COMMANDING_ACTIVE) { + if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || + state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); } @@ -41,7 +41,7 @@ void StateInterface::set_state(const_fri_state_t_ref state) { }; void StateInterface::set_state_open_loop(const_fri_state_t_ref state, - const_idl_joint_pos_t_ref joint_position) { + const_jnt_array_t_ref joint_position) { state_.client_command_mode = state.getClientCommandMode(); #if FRI_CLIENT_VERSION_MAJOR == 1 std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(), @@ -53,8 +53,8 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); - if (state.getSessionState() == fri_session_state_t::COMMANDING_WAIT || - state.getSessionState() == fri_session_state_t::COMMANDING_ACTIVE) { + if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || + state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); } From 2171bd85ca36dba786a6580e570f2d000a7d3f06 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 11:14:30 +0100 Subject: [PATCH 20/42] added a N_JNTS alias to types --- .../src/pose_control_node.cpp | 8 +++---- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 2 +- .../include/lbr_fri_ros2/ft_estimator.hpp | 4 ++-- lbr_fri_ros2/include/lbr_fri_ros2/types.hpp | 11 ++++++---- lbr_fri_ros2/src/filters.cpp | 9 ++++---- lbr_fri_ros2/src/ft_estimator.cpp | 3 +-- lbr_fri_ros2/src/kinematics.cpp | 22 ++++++++----------- .../controllers/twist_controller.hpp | 3 +-- .../src/controllers/admittance_controller.cpp | 10 ++++----- .../lbr_joint_position_command_controller.cpp | 8 +++---- .../src/controllers/lbr_state_broadcaster.cpp | 6 ++--- .../lbr_torque_command_controller.cpp | 16 +++++++------- .../lbr_wrench_command_controller.cpp | 12 +++++----- .../src/controllers/twist_controller.cpp | 12 +++++----- lbr_ros2_control/src/system_interface.cpp | 7 +++--- 15 files changed, 64 insertions(+), 69 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index 47602410..d3d7c048 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -54,7 +54,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { current_lbr_state_ = *lbr_state; double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_position[i] = current_lbr_state_.measured_joint_position[i]; } current_pose_ = compute_fk_(joint_position); @@ -65,7 +65,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { if (current_lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) { KDL::JntArray current_joint_positions(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { current_joint_positions(i) = current_lbr_state_.measured_joint_position[i]; } @@ -79,7 +79,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_); KDL::JntArray joint_positions = KDL::JntArray(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_positions(i) = position_array_ptr[i]; } @@ -131,7 +131,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { { RCLCPP_ERROR(this->get_logger(), "Inverse kinematics failed."); } else { - for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) { + for (unsigned int i = 0; i < result_joint_positions.data.size(); ++i) { joint_position_command.joint_position[i] = result_joint_positions(i); } } diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 07dcff6d..6f62bdd6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -121,7 +121,7 @@ struct PIDParameters { class JointPIDArray { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using pid_array_t = std::array; + using pid_array_t = std::array; public: JointPIDArray() = delete; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index abd13f94..aa53d109 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -39,8 +39,8 @@ class FTEstimator { std::unique_ptr kinematics_ptr_; // force estimation - Eigen::Matrix jacobian_inv_; - Eigen::Matrix tau_ext_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix tau_ext_; Eigen::Matrix f_ext_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp index fbd2a610..faddc9eb 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp @@ -11,20 +11,23 @@ #include "lbr_fri_idl/msg/lbr_state.hpp" namespace lbr_fri_ros2 { +// joint DoF alias +constexpr std::uint8_t N_JNTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS; + // joint positions, velocities, accelerations, torques etc. -using jnt_array_t = std::array; +using jnt_array_t = std::array; using jnt_array_t_ref = jnt_array_t &; using const_jnt_array_t_ref = const jnt_array_t &; // joint names -using jnt_name_array_t = std::array; +using jnt_name_array_t = std::array; using jnt_name_array_t_ref = jnt_name_array_t &; using const_jnt_name_array_t_ref = const jnt_name_array_t &; -// Cartesian dof +// Cartesian DoF constexpr std::uint8_t CARTESIAN_DOF = 6; -// Cartesian positions, velocities, accelerations, torques etc. +// Cartesian positions, velocities, accelerations, wrenches etc. using cart_array_t = std::array; using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 983835d2..eb842ccc 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -30,11 +30,10 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { - std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - [&, i = 0](const auto ¤t_i) mutable { - previous[i] = exponential_filter_.compute(current_i, previous[i]); - ++i; - }); + std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { + previous[i] = exponential_filter_.compute(current_i, previous[i]); + ++i; + }); } void JointExponentialFilterArray::initialize(const double &cutoff_frequency, diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 05ba96aa..8f6a9d73 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -11,8 +11,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, const double &damping) { - tau_ext_ = Eigen::Map>( - external_torque.data()); + tau_ext_ = Eigen::Map>(external_torque.data()); auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); jacobian_inv_ = pinv(jacobian.data, damping); f_ext_ = jacobian_inv_.transpose() * tau_ext_; diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp index b7da0ebe..6cc56929 100644 --- a/lbr_fri_ros2/src/kinematics.cpp +++ b/lbr_fri_ros2/src/kinematics.cpp @@ -13,52 +13,48 @@ Kinematics::Kinematics(const std::string &robot_description, const std::string & RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (chain_.getNrOfJoints() != N_JNTS) { std::string err = "Invalid number of joints in chain."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } jacobian_solver_ = std::make_unique(chain_); fk_solver_ = std::make_unique(chain_); - jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + jacobian_.resize(N_JNTS); + q_.resize(N_JNTS); q_.data.setZero(); } const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) { - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); jacobian_solver_->JntToJac(q_, jacobian_); return jacobian_; } const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) { - if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (q.size() != N_JNTS) { std::string err = "Invalid number of joint positions."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); jacobian_solver_->JntToJac(q_, jacobian_); return jacobian_; } const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) { - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); fk_solver_->JntToCart(q_, chain_tip_frame_); return chain_tip_frame_; } const KDL::Frame &Kinematics::compute_fk(const std::vector &q) { - if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (q.size() != N_JNTS) { std::string err = "Invalid number of joint positions."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); fk_solver_->JntToCart(q_, chain_tip_frame_); return chain_tip_frame_; } 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 ddbbc9aa..bd8b157c 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 @@ -46,8 +46,7 @@ class TwistImpl { lbr_fri_ros2::jnt_array_t q_; std::unique_ptr kinematics_ptr_; - Eigen::Matrix - jacobian_inv_; + Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; }; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 1d4eb80a..f7b61c29 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -50,12 +50,12 @@ bool AdmittanceController::reference_command_interfaces_() { joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } } @@ -77,15 +77,15 @@ void AdmittanceController::clear_state_interfaces_() { } void AdmittanceController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index 380827ec..232acf93 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 @@ -49,7 +49,7 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/, } std::for_each(command_interfaces_.begin(), command_interfaces_.end(), [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { - command_interface.set_value((*lbr_joint_position_command)->joint_position[idx++]); + command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]); }); return controller_interface::return_type::OK; } @@ -70,15 +70,15 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact } void LBRJointPositionCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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 b8d9f733..765bdd5b 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -151,15 +151,15 @@ void LBRStateBroadcaster::init_state_msg_() { } void LBRStateBroadcaster::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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 22a341d2..b56bd062 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -46,7 +46,7 @@ LBRTorqueCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_torque_command || !(*lbr_torque_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_torque_command)->joint_position[idx]); torque_command_interfaces_[idx].get().set_value((*lbr_torque_command)->torque[idx]); @@ -82,19 +82,19 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { torque_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } - if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (torque_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR(this->get_node()->get_logger(), "Number of torque command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + torque_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } return true; @@ -106,15 +106,15 @@ void LBRTorqueCommandController::clear_command_interfaces_() { } void LBRTorqueCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 59763270..37a278d9 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -51,7 +51,7 @@ LBRWrenchCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_wrench_command || !(*lbr_wrench_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_wrench_command)->joint_position[idx]); } @@ -89,12 +89,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() { wrench_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } if (wrench_command_interfaces_.size() != CARTESIAN_DOF) { @@ -112,15 +112,15 @@ void LBRWrenchCommandController::clear_command_interfaces_() { } void LBRWrenchCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index c259ac2e..fa827b8e 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -30,7 +30,7 @@ void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); // compute target joint veloctiy and map it to dq - Eigen::Map>(dq.data()) = + Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; } @@ -164,12 +164,12 @@ bool TwistController::reference_state_interfaces_() { std::ref(state_interface)); } } - if (joint_position_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position state interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } return true; @@ -183,15 +183,15 @@ void TwistController::reset_command_buffer_() { }; void TwistController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + 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 < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int 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/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index e4fdbf6c..743f0061 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -438,12 +438,11 @@ void SystemInterface::nan_state_interfaces_() { } bool SystemInterface::verify_number_of_joints_() { - if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (info_.joints.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR - << "Expected '" << KUKA::FRI::LBRState::NUMBER_OF_JOINTS - << "' joints in URDF, got '" << info_.joints.size() << "'" - << lbr_fri_ros2::ColorScheme::ENDC); + << "Expected '" << lbr_fri_ros2::N_JNTS << "' joints in URDF, got '" + << info_.joints.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } return true; From ac36e53902f6520b2d70d06a9e24ecac19014993 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 15:07:50 +0100 Subject: [PATCH 21/42] added a worker class --- lbr_fri_ros2/CMakeLists.txt | 1 + lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 14 +++-- lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp | 32 ++++++++++++ lbr_fri_ros2/src/app.cpp | 55 ++++++-------------- lbr_fri_ros2/src/worker.cpp | 53 +++++++++++++++++++ 5 files changed, 112 insertions(+), 43 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp create mode 100644 lbr_fri_ros2/src/worker.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index e28e2132..04dff3fd 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(lbr_fri_ros2 src/filters.cpp src/ft_estimator.cpp src/kinematics.cpp + src/worker.cpp ) target_include_directories(lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index f66a8af0..e1fa36a9 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -14,9 +14,17 @@ #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class App { +class App : public Worker { + /** + * @brief This clas provides utilities to run the KUKA::FRI::ClientApplication asynchronously. + * Note that the rate at which the application runs is determined by the robot. This is because + * the run_thread_ uses blocking function calls from the FRI client SDK, i.e. + * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). + * + */ protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; @@ -26,10 +34,10 @@ class App { bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); bool close_udp_socket(); - void run_async(int rt_prio = 80); - void request_stop(); + void run_async(int rt_prio = 80) override; protected: + void perform_work_() override; bool valid_port_(const int &port_id); std::atomic_bool should_stop_, running_; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp new file mode 100644 index 00000000..d1ff65ac --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -0,0 +1,32 @@ +#ifndef LBR_FRI_ROS2__WORKER_HPP_ +#define LBR_FRI_ROS2__WORKER_HPP_ + +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "realtime_tools/thread_priority.hpp" + +#include "lbr_fri_ros2/formatting.hpp" + +namespace lbr_fri_ros2 { +class Worker { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Worker"; + +public: + Worker(); + ~Worker(); + + virtual void run_async(int rt_prio = 80); + void request_stop(); + +protected: + virtual void perform_work_() = 0; + + std::atomic_bool should_stop_, running_; + std::thread run_thread_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__WORKER_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index d8860834..45559466 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -2,8 +2,7 @@ namespace lbr_fri_ros2 { App::App(const std::shared_ptr async_client_ptr) - : should_stop_(true), running_(false), async_client_ptr_(nullptr), connection_ptr_(nullptr), - app_ptr_(nullptr) { + : async_client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { async_client_ptr_ = async_client_ptr; connection_ptr_ = std::make_unique(); app_ptr_ = std::make_unique(*connection_ptr_, *async_client_ptr_); @@ -83,48 +82,24 @@ void App::run_async(int rt_prio) { ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } - if (running_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING << "App already running" << ColorScheme::ENDC); - return; - } - run_thread_ = std::thread([this, rt_prio]() { - if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING - << "Failed to set FIFO realtime scheduling policy. Refer to " - "[https://control.ros.org/master/doc/ros2_control/" - "controller_manager/doc/userdoc.html]." - << ColorScheme::ENDC); - } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::OKGREEN - << "Realtime scheduling policy set to FIFO with priority '" << rt_prio - << "'" << ColorScheme::ENDC); - } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); - should_stop_ = false; - bool success = true; - while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); // stuck if never connected, we thus detach the thread as join may - // never return - running_ = true; - if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); - break; - } - } - async_client_ptr_->get_state_interface()->uninitialize(); - running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); - }); + // call base class run_async post checks + Worker::run_async(rt_prio); run_thread_.detach(); } -void App::request_stop() { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); - should_stop_ = true; +void App::perform_work_() { + bool success = true; + while (rclcpp::ok() && success && !should_stop_) { + success = app_ptr_->step(); // stuck if never connected, we thus detach the run_thread_ as join + // may never return + running_ = true; + if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); + break; + } + } + async_client_ptr_->get_state_interface()->uninitialize(); } bool App::valid_port_(const int &port_id) { diff --git a/lbr_fri_ros2/src/worker.cpp b/lbr_fri_ros2/src/worker.cpp new file mode 100644 index 00000000..ed8d0495 --- /dev/null +++ b/lbr_fri_ros2/src/worker.cpp @@ -0,0 +1,53 @@ +#include "lbr_fri_ros2/worker.hpp" + +namespace lbr_fri_ros2 { +Worker::Worker() : should_stop_(true), running_(false) {} + +Worker::~Worker() { + this->request_stop(); + if (run_thread_.joinable()) { + run_thread_.join(); + } +} + +void Worker::run_async(int rt_prio) { + if (running_) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); + return; + } + run_thread_ = std::thread([this, rt_prio]() { + if (!realtime_tools::configure_sched_fifo(rt_prio)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::WARNING + << "Failed to set FIFO realtime scheduling policy. Refer to " + "[https://control.ros.org/master/doc/ros2_control/" + "controller_manager/doc/userdoc.html]." + << ColorScheme::ENDC); + } else { + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + ColorScheme::OKGREEN + << "Realtime scheduling policy set to FIFO with priority '" << rt_prio + << "'" << ColorScheme::ENDC); + } + + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); + should_stop_ = false; + + // perform work in child-class + this->perform_work_(); + // perform work end + + running_ = false; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); + }); +} + +void Worker::request_stop() { + if (!running_) { + return; + } + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); + should_stop_ = true; +} +} // namespace lbr_fri_ros2 From 74afbb89877c57392d79302dcc2cb3fc3ef68a4c Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 15:20:41 +0100 Subject: [PATCH 22/42] removed redundant members --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index e1fa36a9..6e2f3f52 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -1,13 +1,10 @@ #ifndef LBR_FRI_ROS2__APP_HPP_ #define LBR_FRI_ROS2__APP_HPP_ -#include #include -#include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -#include "realtime_tools/thread_priority.hpp" #include "friClientApplication.h" #include "friUdpConnection.h" @@ -40,9 +37,6 @@ class App : public Worker { void perform_work_() override; bool valid_port_(const int &port_id); - std::atomic_bool should_stop_, running_; - std::thread run_thread_; - std::shared_ptr async_client_ptr_; std::unique_ptr connection_ptr_; std::unique_ptr app_ptr_; From 48f92c14858fb68078d608c2fd1c84ed8adceb02 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 18:59:39 +0100 Subject: [PATCH 23/42] moved the FT estimation into an asynchronous thread (#213) --- .../ros2_control/lbr_system_config.yaml | 3 + .../ros2_control/lbr_system_interface.xacro | 20 +++-- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 6 +- .../include/lbr_fri_ros2/ft_estimator.hpp | 60 ++++++++++++-- lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp | 5 +- lbr_fri_ros2/src/app.cpp | 32 ++++---- lbr_fri_ros2/src/ft_estimator.cpp | 65 +++++++++------ lbr_fri_ros2/src/worker.cpp | 12 +-- lbr_ros2_control/config/lbr_controllers.yaml | 1 + .../controllers/twist_controller.hpp | 2 +- .../lbr_ros2_control/system_interface.hpp | 4 + .../src/controllers/twist_controller.cpp | 8 +- lbr_ros2_control/src/system_interface.cpp | 80 ++++++++++++++----- 13 files changed, 202 insertions(+), 96 deletions(-) diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index b55b0aef..6e110244 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -19,6 +19,9 @@ hardware: 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 diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index d078c1a9..5a31f0af 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -61,6 +61,9 @@ + ${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']} @@ -70,12 +73,14 @@ ${system_config['estimated_ft_sensor']['torque_x_th']} ${system_config['estimated_ft_sensor']['torque_y_th']} ${system_config['estimated_ft_sensor']['torque_z_th']} - - - - - - + + + + + + + + @@ -114,7 +119,8 @@ ${max_position} ${max_velocity} ${max_torque} - + diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 6e2f3f52..aea97da5 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -2,6 +2,7 @@ #define LBR_FRI_ROS2__APP_HPP_ #include +#include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" @@ -22,9 +23,6 @@ class App : public Worker { * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). * */ -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; - public: App(const std::shared_ptr async_client_ptr); ~App(); @@ -33,6 +31,8 @@ class App : public Worker { bool close_udp_socket(); void run_async(int rt_prio = 80) override; + inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::App"; }; + protected: void perform_work_() override; bool valid_port_(const int &port_id); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index aa53d109..d7b93a0b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "eigen3/Eigen/Core" #include "rclcpp/logger.hpp" @@ -17,31 +18,74 @@ #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 FTEstimator { +class FTEstimatorImpl { + /** + * @brief A class to estimate force-torques 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::FTEstimator"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimatorImpl"; public: - FTEstimator(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}); - void compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque, - cart_array_t_ref f_ext, const double &damping = 0.2); + 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); + void compute(); void reset(); + inline void get_f_ext(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_; + } + inline void get_f_ext_tf(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_tf_; + } + inline void set_tau_ext(const_jnt_array_t_ref tau_ext) { + tau_ext_ = Eigen::Map>(tau_ext.data()); + } + inline void set_q(const_jnt_array_t_ref q) { q_ = q; } + protected: // force threshold cart_array_t f_ext_th_; + // damping for pseudo-inverse of Jacobian + double damping_; + + // joint positions and external joint torques + jnt_array_t q_; + // kinematics std::unique_ptr kinematics_ptr_; // force estimation Eigen::Matrix jacobian_inv_; Eigen::Matrix tau_ext_; - Eigen::Matrix f_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_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp index d1ff65ac..1eb42b8f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -2,6 +2,7 @@ #define LBR_FRI_ROS2__WORKER_HPP_ #include +#include #include #include "rclcpp/logger.hpp" @@ -12,15 +13,13 @@ namespace lbr_fri_ros2 { class Worker { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Worker"; - public: Worker(); ~Worker(); virtual void run_async(int rt_prio = 80); void request_stop(); + inline virtual std::string LOGGER_NAME() const = 0; protected: virtual void perform_work_() = 0; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 45559466..9be75fe8 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -15,70 +15,70 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Opening UDP socket with port_id '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already open"); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Failed to open socket" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket opened successfully" << ColorScheme::ENDC); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } while (running_) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Waiting for run thread termination"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Waiting for run thread termination"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already closed"); return true; } connection_ptr_->close(); - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket closed successfully" << ColorScheme::ENDC); return true; } void App::run_async(int rt_prio) { if (!async_client_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not open" << ColorScheme::ENDC); return; } if (!app_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } @@ -95,7 +95,7 @@ void App::perform_work_() { // may never return running_ = true; if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "LBR in session state idle, exiting"); break; } } @@ -104,7 +104,7 @@ void App::perform_work_() { bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 8f6a9d73..e846ba6f 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -1,41 +1,54 @@ #include "lbr_fri_ros2/ft_estimator.hpp" namespace lbr_fri_ros2 { -FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, - const std::string &chain_tip, const_cart_array_t_ref f_ext_th) - : f_ext_th_(f_ext_th) { +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 FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, - const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, - const double &damping) { - tau_ext_ = Eigen::Map>(external_torque.data()); - auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); - jacobian_inv_ = pinv(jacobian.data, damping); - f_ext_ = jacobian_inv_.transpose() * tau_ext_; +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(measured_joint_position); - f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); - f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); - - Eigen::Map>(f_ext.data()) = f_ext_; - - // threshold force-torque - std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), - [](const double &f_ext_i, const double &f_ext_th_i) { - if (std::abs(f_ext_i) < f_ext_th_i) { - return 0.; - } else { - return std::copysign(1., f_ext_i) * (std::abs(f_ext_i) - f_ext_th_i); - } - }); + 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 FTEstimator::reset() { +void FTEstimatorImpl::reset() { + std::for_each(q_.begin(), q_.end(), [](double &q_i) { q_i = 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/worker.cpp b/lbr_fri_ros2/src/worker.cpp index ed8d0495..b42f55a0 100644 --- a/lbr_fri_ros2/src/worker.cpp +++ b/lbr_fri_ros2/src/worker.cpp @@ -12,26 +12,26 @@ Worker::~Worker() { void Worker::run_async(int rt_prio) { if (running_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); return; } run_thread_ = std::thread([this, rt_prio]() { if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::WARNING << "Failed to set FIFO realtime scheduling policy. Refer to " "[https://control.ros.org/master/doc/ros2_control/" "controller_manager/doc/userdoc.html]." << ColorScheme::ENDC); } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Realtime scheduling policy set to FIFO with priority '" << rt_prio << "'" << ColorScheme::ENDC); } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Starting run thread"); should_stop_ = false; // perform work in child-class @@ -39,7 +39,7 @@ void Worker::run_async(int rt_prio) { // perform work end running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Exiting run thread"); }); } @@ -47,7 +47,7 @@ void Worker::request_stop() { if (!running_) { return; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Requesting run thread stop"); should_stop_ = true; } } // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 07052fbe..64c91616 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -101,3 +101,4 @@ 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 + timeout: 0.2 # stop controller if no command is received within this time [s] 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 bd8b157c..a10a2ba0 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 @@ -81,7 +81,7 @@ class TwistController : public controller_interface::ControllerInterface { // some safety checks std::atomic updates_since_last_command_ = 0; - double max_time_without_command_ = 0.2; + double timeout_ = 0.2; // joint veloctiy computation std::unique_ptr twist_impl_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 ffa14fd7..9a43948b 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -56,6 +56,9 @@ struct SystemInterfaceParameters { }; 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}; @@ -162,6 +165,7 @@ class SystemInterface : public hardware_interface::SystemInterface { // additional force-torque state interface lbr_fri_ros2::cart_array_t hw_ft_; + std::shared_ptr ft_estimator_impl_ptr_; std::unique_ptr ft_estimator_ptr_; // exposed command interfaces diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index fa827b8e..0483180c 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -73,8 +73,10 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("damping", 0.2); this->get_node()->declare_parameter("max_linear_velocity", 0.1); this->get_node()->declare_parameter("max_angular_velocity", 0.1); + this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); configure_twist_impl_(); + timeout_ = this->get_node()->get_parameter("timeout").as_double(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", e.what()); @@ -98,11 +100,9 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } - if (updates_since_last_command_ > - static_cast(max_time_without_command_ / period.seconds())) { + if (updates_since_last_command_ > static_cast(timeout_ / period.seconds())) { RCLCPP_ERROR(this->get_node()->get_logger(), - "No twist command received within time %f. Stopping the controller.", - max_time_without_command_); + "No twist command received within %.3f s. Stopping the controller.", timeout_); return controller_interface::return_type::ERROR; } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 743f0061..91cbf710 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -61,6 +61,13 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { nan_last_hw_states_(); // setup force-torque estimator + 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 + ft_parameters_.enabled = info_.sensors[1].parameters.at("enabled") == "true"; + ft_parameters_.update_rate = std::stoul(info_.sensors[1].parameters.at("update_rate")); + ft_parameters_.rt_prio = std::stoi(info_.sensors[1].parameters.at("rt_prio")); ft_parameters_.chain_root = info_.sensors[1].parameters.at("chain_root"); ft_parameters_.chain_tip = info_.sensors[1].parameters.at("chain_tip"); ft_parameters_.damping = std::stod(info_.sensors[1].parameters.at("damping")); @@ -70,16 +77,20 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ft_parameters_.torque_x_th = std::stod(info_.sensors[1].parameters.at("torque_x_th")); ft_parameters_.torque_y_th = std::stod(info_.sensors[1].parameters.at("torque_y_th")); ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th")); - ft_estimator_ptr_ = std::make_unique( - 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, - }); + 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); + } if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; @@ -155,14 +166,16 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_TIME_STAMP_NANO_SEC, &hw_time_stamp_nano_sec_); - // additional force-torque state interface - const auto &estimated_ft_sensor = info_.sensors[1]; - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + // additional force-torque state interface (if enabled) + if (ft_parameters_.enabled) { + const auto &estimated_ft_sensor = info_.sensors[1]; + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + } return state_interfaces; } @@ -250,6 +263,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } std::this_thread::sleep_for(std::chrono::seconds(1)); } + + // ft sensor + if (!ft_estimator_ptr_ && ft_parameters_.enabled) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + 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; } @@ -257,6 +282,9 @@ controller_interface::CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { app_ptr_->request_stop(); app_ptr_->close_udp_socket(); + if (ft_estimator_ptr_) { + ft_estimator_ptr_->request_stop(); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -285,6 +313,7 @@ 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; } @@ -305,8 +334,13 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim update_last_hw_states_(); // additional force-torque state interface - ft_estimator_ptr_->compute(hw_lbr_state_.measured_joint_position, hw_lbr_state_.external_torque, - hw_ft_, ft_parameters_.damping); + 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(hw_lbr_state_.measured_joint_position); + ft_estimator_impl_ptr_->set_tau_ext(hw_lbr_state_.external_torque); + ft_estimator_impl_ptr_->get_f_ext_tf(hw_ft_); + } return hardware_interface::return_type::OK; } @@ -526,8 +560,10 @@ bool SystemInterface::verify_sensors_() { if (!verify_auxiliary_sensor_()) { return false; } - if (!verify_estimated_ft_sensor_()) { - return false; + if (ft_parameters_.enabled) { + if (!verify_estimated_ft_sensor_()) { + return false; + } } return true; } From 88945b674488151d35ef6d59a93b7329abd71d1c Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 23 Oct 2024 11:06:32 +0100 Subject: [PATCH 24/42] added prefix (#189) --- lbr_description/urdf/iiwa14/iiwa14_description.xacro | 2 +- lbr_description/urdf/iiwa7/iiwa7_description.xacro | 2 +- lbr_description/urdf/med14/med14_description.xacro | 2 +- lbr_description/urdf/med7/med7_description.xacro | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index fde51896..cc0fbc64 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -285,7 +285,7 @@ - + diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index e9bfaa1e..65374c44 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -286,7 +286,7 @@ - + diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index 6a5e5a27..b5f55d5f 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -286,7 +286,7 @@ - + diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 1b9998f8..344fc4f7 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -286,7 +286,7 @@ - + From 218d723f817c61314988833611effddc5333ca6c Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 23 Oct 2024 11:27:27 +0100 Subject: [PATCH 25/42] added unique name to system interface (#189) --- lbr_description/ros2_control/lbr_system_interface.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index 5a31f0af..fbe30cc6 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -6,7 +6,7 @@ - + From cb113b1a6d8482ba95ea612c7123cdfbdc2ecd3f Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 16 Nov 2024 13:10:28 +0000 Subject: [PATCH 26/42] updating doi (https://github.com/openjournals/joss-reviews/issues/6138) --- CITATION.cff | 2 +- README.md | 18 +++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/CITATION.cff b/CITATION.cff index 203549cd..d97faf1a 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -20,5 +20,5 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" version: 2.1.2 -doi: 10.48550/arXiv.2311.12709 +doi: 10.21105/joss.06138 date-released: 2024-10-18 diff --git a/README.md b/README.md index 08c5da0f..4a04a440 100644 --- a/README.md +++ b/README.md @@ -88,13 +88,17 @@ Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_sta 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. ``` -@misc{huber2023lbrstack, - title={LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots}, - author={Martin Huber and Christopher E. Mower and Sebastien Ourselin and Tom Vercauteren and Christos Bergeles}, - year={2023}, - eprint={2311.12709}, - archivePrefix={arXiv}, - primaryClass={cs.RO} +@article{Huber2024, + doi = {10.21105/joss.06138}, + url = {https://doi.org/10.21105/joss.06138}, + year = {2024}, + publisher = {The Open Journal}, + volume = {9}, + number = {103}, + pages = {6138}, + author = {Martin Huber and Christopher E. Mower and Sebastien Ourselin and Tom Vercauteren and Christos Bergeles}, + title = {LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots}, + journal = {Journal of Open Source Software} } ``` From 5fdaed9630d89ca82cbecaf472595f28433b87bf Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 18 Nov 2024 13:18:37 +0000 Subject: [PATCH 27/42] tip frame twist command support (#163) --- lbr_ros2_control/config/lbr_controllers.yaml | 1 + .../lbr_ros2_control/controllers/twist_controller.hpp | 1 + lbr_ros2_control/src/controllers/twist_controller.cpp | 11 +++++++++++ 3 files changed, 13 insertions(+) diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 64c91616..4d858951 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -98,6 +98,7 @@ robot_name: lbr 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 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 a10a2ba0..477ba98f 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 @@ -29,6 +29,7 @@ namespace lbr_ros2_control { struct TwistParameters { std::string chain_root; std::string chain_tip; + bool twist_in_tip_frame; double damping; double max_linear_velocity; double max_angular_velocity; diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 0483180c..5cb7d7b2 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -25,6 +25,15 @@ void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); }); + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + // compute jacobian auto jacobian = kinematics_ptr_->compute_jacobian(q); jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); @@ -70,6 +79,7 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("robot_name", "lbr"); this->get_node()->declare_parameter("chain_root", "lbr_link_0"); this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("twist_in_tip_frame", true); this->get_node()->declare_parameter("damping", 0.2); this->get_node()->declare_parameter("max_linear_velocity", 0.1); this->get_node()->declare_parameter("max_angular_velocity", 0.1); @@ -201,6 +211,7 @@ void TwistController::configure_twist_impl_() { this->get_robot_description(), TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), this->get_node()->get_parameter("chain_tip").as_string(), + this->get_node()->get_parameter("twist_in_tip_frame").as_bool(), this->get_node()->get_parameter("damping").as_double(), this->get_node()->get_parameter("max_linear_velocity").as_double(), this->get_node()->get_parameter("max_angular_velocity").as_double()}); From 836f30cb7b6aadb37b1d3bb70a84610bcf301766 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 18 Nov 2024 18:03:53 +0000 Subject: [PATCH 28/42] initial admittance controller impl (#163) --- lbr_fri_ros2/CMakeLists.txt | 4 + lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 48 ++++ lbr_fri_ros2/package.xml | 1 + lbr_fri_ros2/src/control.cpp | 72 ++++++ lbr_ros2_control/CMakeLists.txt | 1 + lbr_ros2_control/config/lbr_controllers.yaml | 27 ++- .../controllers/admittance_controller.hpp | 57 ++++- .../controllers/twist_controller.hpp | 37 +-- .../src/controllers/admittance_controller.cpp | 211 +++++++++++++++--- .../src/controllers/twist_controller.cpp | 87 ++------ 10 files changed, 399 insertions(+), 146 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/control.hpp create mode 100644 lbr_fri_ros2/src/control.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 04dff3fd..1dc8a943 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(lbr_fri_idl REQUIRED) find_package(orocos_kdl_vendor REQUIRED) @@ -33,6 +34,7 @@ add_library(lbr_fri_ros2 src/app.cpp src/async_client.cpp src/command_guard.cpp + src/control.cpp src/filters.cpp src/ft_estimator.cpp src/kinematics.cpp @@ -48,6 +50,7 @@ target_include_directories(lbr_fri_ros2 ament_target_dependencies(lbr_fri_ros2 control_toolbox Eigen3 + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor @@ -65,6 +68,7 @@ ament_export_dependencies( eigen3_cmake_module Eigen3 FRIClient + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp new file mode 100644 index 00000000..88297ae5 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -0,0 +1,48 @@ +#ifndef LBR_FRI_ROS2__CONTROL_HPP_ +#define LBR_FRI_ROS2__CONTROL_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/Core" +#include "geometry_msgs/msg/twist.hpp" + +#include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" + +namespace lbr_fri_ros2 { +struct InvJacCtrlParameters { + std::string chain_root; + std::string chain_tip; + bool twist_in_tip_frame; + double damping; + double max_linear_velocity; + double max_angular_velocity; +}; + +class InvJacCtrlImpl { +public: + InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters ¶meters); + + void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + void compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, jnt_array_t_ref dq); + void compute(const Eigen::Matrix &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + + inline const std::unique_ptr &get_kinematics_ptr() const { return kinematics_ptr_; }; + +protected: + void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); + + InvJacCtrlParameters parameters_; + + jnt_array_t q_; + std::unique_ptr kinematics_ptr_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix twist_target_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 62379bc1..d9cc4bf1 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -15,6 +15,7 @@ control_toolbox fri_client_sdk + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp new file mode 100644 index 00000000..7016c974 --- /dev/null +++ b/lbr_fri_ros2/src/control.cpp @@ -0,0 +1,72 @@ +#include "lbr_fri_ros2/control.hpp" + +namespace lbr_fri_ros2 { +InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description, + const InvJacCtrlParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique(robot_description, parameters_.chain_root, + parameters_.chain_tip); +} + +void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref dq) { + // twist to Eigen + twist_target_[0] = twist_target->linear.x; + twist_target_[1] = twist_target->linear.y; + twist_target_[2] = twist_target->linear.z; + twist_target_[3] = twist_target->angular.x; + twist_target_[4] = twist_target->angular.y; + twist_target_[5] = twist_target->angular.z; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq) { + // twist to Eigen + twist_target_[0] = twist_target[0]; + twist_target_[1] = twist_target[1]; + twist_target_[2] = twist_target[2]; + twist_target_[3] = twist_target[3]; + twist_target_[4] = twist_target[4]; + twist_target_[5] = twist_target[5]; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const Eigen::Matrix &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref dq) { + twist_target_ = twist_target; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { + // clip velocity + 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) { + return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); + }); + + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; +} +} // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index cf019396..8621fa41 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED + src/controllers/admittance_controller.cpp src/controllers/lbr_joint_position_command_controller.cpp src/controllers/lbr_torque_command_controller.cpp src/controllers/lbr_wrench_command_controller.cpp diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 4d858951..aae3b6ea 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -93,13 +93,28 @@ ros__parameters: robot_name: lbr +/**/admittance_controller: + ros__parameters: + robot_name: lbr + admittance: + mass: 0.2 + damping: 0.1 + stiffness: 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: 0.1 # maximum linear velocity + max_angular_velocity: 0.1 # maximum linear acceleration + /**/twist_controller: ros__parameters: robot_name: lbr - 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 + inv_jac_ctrl: + 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 timeout: 0.2 # stop controller if no command is received within this time [s] 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 f6909e11..d110d2ba 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 @@ -9,22 +9,40 @@ #include #include "controller_interface/controller_interface.hpp" -#include "hardware_interface/loaned_command_interface.hpp" +#include "eigen3/Eigen/Core" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "semantic_components/force_torque_sensor.hpp" #include "friLBRState.h" +#include "lbr_fri_ros2/control.hpp" #include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -class Admittance { - // implement an addmittance... +struct AdmittanceParameters { + double m = 1.0; + double b = 0.1; + double k = 0.0; }; -class AdmittanceController : public controller_interface::ControllerInterface { - static constexpr uint8_t CARTESIAN_DOF = 6; +class AdmittanceImpl { +public: + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + + void compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; + } +protected: + AdmittanceParameters parameters_; +}; + +class AdmittanceController : public controller_interface::ControllerInterface { public: AdmittanceController(); @@ -47,18 +65,33 @@ class AdmittanceController : public controller_interface::ControllerInterface { on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_command_interfaces_(); bool reference_state_interfaces_(); - void clear_command_interfaces_(); void clear_state_interfaces_(); void configure_joint_names_(); - + void configure_admittance_impl_(); + void configure_inv_jac_ctrl_impl_(); + void zero_all_values_(); + + // admittance + bool initialized_ = false; + std::unique_ptr admittance_impl_ptr_; + Eigen::Matrix x_init_, x_prev_; + Eigen::Matrix f_ext_, x_, dx_, ddx_; + + // joint veloctiy computation + std::unique_ptr inv_jac_ctrl_impl_ptr_; + lbr_fri_ros2::jnt_array_t q_, dq_; + Eigen::Matrix twist_command_; + + // interfaces lbr_fri_ros2::jnt_name_array_t joint_names_; - - std::vector> - joint_position_command_interfaces_; std::vector> - estimated_ft_sensor_state_interface_; + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_ptr_; + std::unique_ptr> + session_state_interface_ptr_; + std::unique_ptr estimated_ft_sensor_ptr_; }; } // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ 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 477ba98f..b4d27780 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 @@ -4,14 +4,12 @@ #include #include #include -#include #include #include #include #include #include "controller_interface/controller_interface.hpp" -#include "eigen3/Eigen/Core" #include "geometry_msgs/msg/twist.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -20,37 +18,12 @@ #include "friLBRState.h" +#include "lbr_fri_ros2/control.hpp" #include "lbr_fri_ros2/kinematics.hpp" -#include "lbr_fri_ros2/pinv.hpp" #include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -struct TwistParameters { - std::string chain_root; - std::string chain_tip; - bool twist_in_tip_frame; - double damping; - double max_linear_velocity; - double max_angular_velocity; -}; - -class TwistImpl { -public: - TwistImpl(const std::string &robot_description, const TwistParameters ¶meters); - - void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, - lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq); - -protected: - TwistParameters parameters_; - - lbr_fri_ros2::jnt_array_t q_; - std::unique_ptr kinematics_ptr_; - Eigen::Matrix jacobian_inv_; - Eigen::Matrix twist_target_; -}; - class TwistController : public controller_interface::ControllerInterface { public: TwistController(); @@ -78,14 +51,14 @@ class TwistController : public controller_interface::ControllerInterface { void clear_state_interfaces_(); void reset_command_buffer_(); void configure_joint_names_(); - void configure_twist_impl_(); + void configure_inv_jac_ctrl_impl_(); // some safety checks std::atomic updates_since_last_command_ = 0; double timeout_ = 0.2; // joint veloctiy computation - std::unique_ptr twist_impl_ptr_; + std::unique_ptr inv_jac_ctrl_impl_ptr_; lbr_fri_ros2::jnt_array_t q_, dq_; // interfaces @@ -93,9 +66,9 @@ class TwistController : public controller_interface::ControllerInterface { std::vector> joint_position_state_interfaces_; std::unique_ptr> - sample_time_state_interface_; + sample_time_state_interface_ptr_; std::unique_ptr> - session_state_interface_; + session_state_interface_ptr_; // real-time twist command topic realtime_tools::RealtimeBuffer rt_twist_ptr_; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index f7b61c29..fe06c703 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -5,75 +5,182 @@ AdmittanceController::AdmittanceController() {} controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { - // reference joint position command interface + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + return interface_configuration; } controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { - // retrieve estimated ft state interface + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // joint position interface + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + + // estimated force-torque sensor interface + for (const auto &interface_name : estimated_ft_sensor_ptr_->get_state_interface_names()) { + interface_configuration.names.push_back(interface_name); + } + + // additional state interfaces + 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; } -controller_interface::CallbackReturn AdmittanceController::on_init() {} +controller_interface::CallbackReturn AdmittanceController::on_init() { + try { + this->get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("admittance.mass", 1.0); + this->get_node()->declare_parameter("admittance.damping", 0.1); + this->get_node()->declare_parameter("admittance.stiffness", 0.0); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + configure_joint_names_(); + configure_admittance_impl_(); + configure_inv_jac_ctrl_impl_(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize admittance controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} -controller_interface::return_type AdmittanceController::update(const rclcpp::Time &time, +controller_interface::return_type AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) { + // get estimated force-torque sensor values + f_ext_.head(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_forces().data()); + f_ext_.tail(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_torques().data()); + + // get joint positions + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute forward kinematics + auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_); + x_.head(3) = Eigen::Map>(chain_tip_frame.p.data); + chain_tip_frame.M.GetRPY(x_init_[3], x_init_[4], x_init_[5]); + + // compute steady state position and orientation + if (!initialized_) { + x_init_ = x_; + x_prev_ = x_; + initialized_ = true; + } + + // compute velocity + dx_ = (x_ - x_prev_) / period.seconds(); + // compute admittance - // add warning for high force-torques and refer to load data calibration + admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_); + + // update previous position + x_prev_ = x_; + + // integrate ddx_ to command velocity + twist_command_ = ddx_ * period.seconds(); + + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_ptr_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + + // compute the joint velocity from the twist command target + inv_jac_ctrl_impl_ptr_->compute(twist_command_, q_, dq_); + + // pass joint positions to hardware + std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { + this->command_interfaces_[i].set_value( + q_i + dq_[i] * sample_time_state_interface_ptr_->get().get_value()); + ++i; + }); + + return controller_interface::return_type::OK; } controller_interface::CallbackReturn -AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { +AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + 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); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - if (!reference_command_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } +AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!reference_state_interfaces_()) { return controller_interface::CallbackReturn::ERROR; } + zero_all_values_(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - clear_command_interfaces_(); +AdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { clear_state_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool AdmittanceController::reference_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)); +bool AdmittanceController::reference_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)); + } + if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { + sample_time_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); } } - if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { + if (!estimated_ft_sensor_ptr_->assign_loaned_state_interfaces(state_interfaces_)) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to assign estimated force torque state interfaces."); + return false; + } + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint position command interfaces '%ld' does not match the number of joints " + "Number of joint position state interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } -} - -bool AdmittanceController::reference_state_interfaces_() { - for (auto &state_interface : state_interfaces_) { - if (state_interface.get_interface_name() == HW_IF_ESTIMATED_FT_PREFIX) { - estimated_ft_sensor_state_interface_.emplace_back(std::ref(state_interface)); - } - } -} - -void AdmittanceController::clear_command_interfaces_() { - joint_position_command_interfaces_.clear(); + return true; } void AdmittanceController::clear_state_interfaces_() { - estimated_ft_sensor_state_interface_.clear(); + joint_position_state_interfaces_.clear(); + estimated_ft_sensor_ptr_->release_interfaces(); } void AdmittanceController::configure_joint_names_() { @@ -89,4 +196,44 @@ void AdmittanceController::configure_joint_names_() { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } + +void AdmittanceController::configure_admittance_impl_() { + admittance_impl_ptr_ = std::make_unique( + AdmittanceParameters{this->get_node()->get_parameter("admittance.mass").as_double(), + this->get_node()->get_parameter("admittance.damping").as_double(), + this->get_node()->get_parameter("admittance.stiffness").as_double()}); + RCLCPP_INFO(this->get_node()->get_logger(), "Admittance controller initialized."); + RCLCPP_INFO(this->get_node()->get_logger(), "Mass: %f", + this->get_node()->get_parameter("admittance.mass").as_double()); + RCLCPP_INFO(this->get_node()->get_logger(), "Damping: %f", + this->get_node()->get_parameter("admittance.damping").as_double()); + RCLCPP_INFO(this->get_node()->get_logger(), "Stiffness: %f", + this->get_node()->get_parameter("admittance.stiffness").as_double()); +} + +void AdmittanceController::configure_inv_jac_ctrl_impl_() { + inv_jac_ctrl_impl_ptr_ = std::make_unique( + this->get_robot_description(), + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + true, // always assume twist in tip frame, since force-torque is estimated in tip frame + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); +} + +void AdmittanceController::zero_all_values_() { + f_ext_.setZero(); + x_.setZero(); + dx_.setZero(); + ddx_.setZero(); + std::fill(dq_.begin(), dq_.end(), 0.0); + twist_command_.setZero(); +} } // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::AdmittanceController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 5cb7d7b2..9994f556 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -1,48 +1,6 @@ #include "lbr_ros2_control/controllers/twist_controller.hpp" namespace lbr_ros2_control { -TwistImpl::TwistImpl(const std::string &robot_description, const TwistParameters ¶meters) - : parameters_(parameters) { - kinematics_ptr_ = std::make_unique( - robot_description, parameters_.chain_root, parameters_.chain_tip); -} - -void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, - lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq) { - // twist to Eigen - twist_target_[0] = twist_target->linear.x; - twist_target_[1] = twist_target->linear.y; - twist_target_[2] = twist_target->linear.z; - twist_target_[3] = twist_target->angular.x; - twist_target_[4] = twist_target->angular.y; - twist_target_[5] = twist_target->angular.z; - - // clip velocity - 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) { - return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); - }); - - // if desired, transform to tip frame - if (parameters_.twist_in_tip_frame) { - auto chain_tip_frame = kinematics_ptr_->compute_fk(q); - twist_target_.topRows(3) = - Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); - twist_target_.bottomRows(3) = - Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); - } - - // compute jacobian - auto jacobian = kinematics_ptr_->compute_jacobian(q); - jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); - - // compute target joint veloctiy and map it to dq - Eigen::Map>(dq.data()) = - jacobian_inv_ * twist_target_; -} - TwistController::TwistController() : rt_twist_ptr_(nullptr), twist_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration @@ -77,15 +35,15 @@ controller_interface::CallbackReturn TwistController::on_init() { updates_since_last_command_ = 0; }); this->get_node()->declare_parameter("robot_name", "lbr"); - this->get_node()->declare_parameter("chain_root", "lbr_link_0"); - this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); - this->get_node()->declare_parameter("twist_in_tip_frame", true); - this->get_node()->declare_parameter("damping", 0.2); - this->get_node()->declare_parameter("max_linear_velocity", 0.1); - this->get_node()->declare_parameter("max_angular_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true); + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); - configure_twist_impl_(); + configure_inv_jac_ctrl_impl_(); timeout_ = this->get_node()->get_parameter("timeout").as_double(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", @@ -102,11 +60,11 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / if (!twist_command || !(*twist_command)) { return controller_interface::return_type::OK; } - if (!twist_impl_ptr_) { - RCLCPP_ERROR(this->get_node()->get_logger(), "Twist controller not initialized."); + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); return controller_interface::return_type::ERROR; } - if (static_cast(session_state_interface_->get().get_value()) != + if (static_cast(session_state_interface_ptr_->get().get_value()) != KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } @@ -123,12 +81,12 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / }); // compute the joint velocity from the twist command target - twist_impl_ptr_->compute(*twist_command, q_, dq_); + inv_jac_ctrl_impl_ptr_->compute(*twist_command, q_, dq_); // pass joint positions to hardware std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { this->command_interfaces_[i].set_value( - q_i + dq_[i] * sample_time_state_interface_->get().get_value()); + q_i + dq_[i] * sample_time_state_interface_ptr_->get().get_value()); ++i; }); @@ -164,12 +122,12 @@ bool TwistController::reference_state_interfaces_() { joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { - sample_time_state_interface_ = + sample_time_state_interface_ptr_ = std::make_unique>( std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { - session_state_interface_ = + session_state_interface_ptr_ = std::make_unique>( std::ref(state_interface)); } @@ -206,15 +164,16 @@ void TwistController::configure_joint_names_() { } } -void TwistController::configure_twist_impl_() { - twist_impl_ptr_ = std::make_unique( +void TwistController::configure_inv_jac_ctrl_impl_() { + inv_jac_ctrl_impl_ptr_ = std::make_unique( this->get_robot_description(), - TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), - this->get_node()->get_parameter("chain_tip").as_string(), - this->get_node()->get_parameter("twist_in_tip_frame").as_bool(), - this->get_node()->get_parameter("damping").as_double(), - this->get_node()->get_parameter("max_linear_velocity").as_double(), - this->get_node()->get_parameter("max_angular_velocity").as_double()}); + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.twist_in_tip_frame").as_bool(), + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); } } // namespace lbr_ros2_control From ccab2db2806d0c305132c0f90b26d0f82bc7c4d7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 16:24:42 +0000 Subject: [PATCH 29/42] pid -> exp smooth + exp smooth with time constant (#163) --- .../ros2_control/lbr_system_config.yaml | 20 ++-- .../ros2_control/lbr_system_interface.xacro | 11 +- lbr_fri_ros2/CMakeLists.txt | 3 - .../include/lbr_fri_ros2/async_client.hpp | 5 +- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 113 ++++++++---------- .../lbr_fri_ros2/interfaces/base_command.hpp | 4 +- .../interfaces/position_command.hpp | 2 +- .../include/lbr_fri_ros2/interfaces/state.hpp | 8 +- .../interfaces/torque_command.hpp | 2 +- .../interfaces/wrench_command.hpp | 2 +- lbr_fri_ros2/package.xml | 1 - lbr_fri_ros2/src/async_client.cpp | 8 +- lbr_fri_ros2/src/filters.cpp | 90 ++++++-------- lbr_fri_ros2/src/interfaces/base_command.cpp | 6 +- .../src/interfaces/position_command.cpp | 14 +-- lbr_fri_ros2/src/interfaces/state.cpp | 32 ++--- .../src/interfaces/torque_command.cpp | 15 ++- .../src/interfaces/wrench_command.cpp | 15 ++- lbr_fri_ros2/test/test_command_interfaces.cpp | 15 ++- lbr_fri_ros2/test/test_position_command.cpp | 11 +- lbr_fri_ros2/test/test_torque_command.cpp | 11 +- lbr_fri_ros2/test/test_wrench_command.cpp | 11 +- .../lbr_ros2_control/system_interface.hpp | 12 +- lbr_ros2_control/src/system_interface.cpp | 28 +---- 24 files changed, 184 insertions(+), 255 deletions(-) diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 6e110244..8f96cf0d 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -7,15 +7,19 @@ hardware: 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 - pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) - pid_i: 0.0 # I gain for the joint position command - pid_d: 0.0 # D gain for the joint position command - pid_i_max: 0.0 # max integral value for the joint position command - pid_i_min: 0.0 # min integral value for the joint position command - pid_antiwindup: false # enable antiwindup for the joint position command + # 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] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] - measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + # 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.04 + # 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.04 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 diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index fbe30cc6..f4a4452f 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -27,15 +27,10 @@ ${system_config['hardware']['port_id']} ${system_config['hardware']['remote_host']} ${system_config['hardware']['rt_prio']} - ${system_config['hardware']['pid_p']} - ${system_config['hardware']['pid_i']} - ${system_config['hardware']['pid_d']} - ${system_config['hardware']['pid_i_max']} - ${system_config['hardware']['pid_i_min']} - ${system_config['hardware']['pid_antiwindup']} + ${system_config['hardware']['joint_position_tau']} ${system_config['hardware']['command_guard_variant']} - ${system_config['hardware']['external_torque_cutoff_frequency']} - ${system_config['hardware']['measured_torque_cutoff_frequency']} + ${system_config['hardware']['external_torque_tau']} + ${system_config['hardware']['measured_torque_tau']} ${system_config['hardware']['open_loop']} diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 1dc8a943..a26f445b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -11,7 +11,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) @@ -48,7 +47,6 @@ target_include_directories(lbr_fri_ros2 ) ament_target_dependencies(lbr_fri_ros2 - control_toolbox Eigen3 geometry_msgs kdl_parser @@ -64,7 +62,6 @@ target_link_libraries(lbr_fri_ros2 ament_export_targets(lbr_fri_ros2_export HAS_LIBRARY_TARGET) ament_export_dependencies( - control_toolbox eigen3_cmake_module Eigen3 FRIClient diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index db194729..f44f33a8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,7 +11,6 @@ #include "friClientVersion.h" #include "friLBRClient.h" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" @@ -27,10 +26,10 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, - const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, + const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}, const bool &open_loop = true); inline std::shared_ptr get_command_interface() { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 6f62bdd6..3773be57 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -3,57 +3,71 @@ #include #include -#include #include +#include -#include "control_toolbox/filters.hpp" -#include "control_toolbox/pid_ros.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" -#include "friLBRClient.h" - -#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { public: /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. * */ ExponentialFilter(); /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. + * + * @param[in] tau Time constant in seconds. + */ + ExponentialFilter(const double &tau); + + /** + * @brief Initialize the filter from members. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) * - * @param[in] cutoff_frequency Frequency in Hz. * @param[in] sample_time Sample time in seconds. */ - ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + void initialize(const double &sample_time); /** - * @brief Compute the exponential smoothing using the control_toolbox - * https://github.com/ros-controls/control_toolbox. + * @brief Initialize the filter. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) + * + * @param[in] tau Time constant in seconds. + * @param[in] sample_time Sample time in seconds. + */ + void initialize(const double &tau, const double &sample_time); + + /** + * @brief Compute the exponential smoothing. Internally computes the new smoothed value following + * smoothed = alpha * current + (1 - alpha) * previous. * * @param[in] current The current value. * @param[in] previous The previous smoothed value. * @return double The returned smoothed value. */ inline double compute(const double ¤t, const double &previous) { - return filters::exponentialSmoothing(current, previous, alpha_); + return alpha_ * current + (1.0 - alpha_) * previous; }; /** - * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * @brief Get the time constant #tau_. * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. + * @return const double& */ - void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + inline const double &get_tau() const { return tau_; }; /** * @brief Get #sample_time_. @@ -70,16 +84,6 @@ class ExponentialFilter { inline const double &get_alpha() const { return alpha_; }; protected: - /** - * @brief Compute alpha given the cutoff frequency and the sample time. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - * @return double Alpha based on - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. - */ - double compute_alpha_(const double &cutoff_frequency, const double &sample_time); - /** * @brief Validate alpha in [0, 1]. * @@ -89,53 +93,30 @@ class ExponentialFilter { */ bool validate_alpha_(const double &alpha); - double cutoff_frequency_; /**< Frequency in Hz.*/ - double sample_time_; /**< Sample time in seconds.*/ - double - alpha_; /**< Alpha parameter based on - https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ + double tau_; /**< Time constant in seconds.*/ + double sample_time_; /**< Sample time in seconds.*/ + double alpha_; /**< Smoothing parameter in [0, 1].*/ }; class JointExponentialFilterArray { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointExponentialFilterArray"; + public: JointExponentialFilterArray() = default; + JointExponentialFilterArray(const double &tau); void compute(const double *const current, jnt_array_t_ref previous); - void initialize(const double &cutoff_frequency, const double &sample_time); + void compute(const_jnt_array_t_ref current, jnt_array_t_ref previous); + void initialize(const double &sample_time); + void initialize(const double &tau, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; -protected: - bool initialized_{false}; /**< True if initialized.*/ - ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ -}; - -struct PIDParameters { - double p{0.0}; /**< Proportional gain.*/ - double i{0.0}; /**< Integral gain.*/ - double d{0.0}; /**< Derivative gain.*/ - double i_max{0.0}; /**< Maximum integral value.*/ - double i_min{0.0}; /**< Minimum integral value.*/ - bool antiwindup{false}; /**< Antiwindup enabled.*/ -}; - -class JointPIDArray { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using pid_array_t = std::array; - -public: - JointPIDArray() = delete; - JointPIDArray(const PIDParameters &pid_parameters); - - void compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); - void compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); void log_info() const; protected: - PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + bool initialized_{false}; /**< True if initialized.*/ + ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ 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 bb46d0ae..4aa3c582 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 @@ -24,7 +24,7 @@ class BaseCommandInterface { public: BaseCommandInterface() = delete; - BaseCommandInterface(const PIDParameters &pid_parameters, + BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); @@ -39,7 +39,7 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - JointPIDArray joint_position_pid_; + JointExponentialFilterArray joint_position_filter_; idl_command_t command_, command_target_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index 24e8a324..399beed7 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -12,7 +12,7 @@ class PositionCommandInterface : public BaseCommandInterface { public: PositionCommandInterface() = delete; - PositionCommandInterface(const PIDParameters &pid_parameters, + PositionCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 23457637..ca0a6068 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ #define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ + #include +#include #include #include "rclcpp/logger.hpp" @@ -14,8 +16,8 @@ namespace lbr_fri_ros2 { struct StateInterfaceParameters { - double external_torque_cutoff_frequency; /*Hz*/ - double measured_torque_cutoff_frequency; /*Hz*/ + double external_torque_tau; /*seconds*/ + double measured_torque_tau; /*seconds*/ }; class StateInterface { @@ -24,7 +26,7 @@ class StateInterface { public: StateInterface() = delete; - StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); + StateInterface(const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}); inline const_idl_state_t_ref get_state() const { return state_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index e4ec7874..70536bee 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -12,7 +12,7 @@ class TorqueCommandInterface : public BaseCommandInterface { public: TorqueCommandInterface() = delete; - TorqueCommandInterface(const PIDParameters &pid_parameters, + TorqueCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index 0c846ee4..4e68fae0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -12,7 +12,7 @@ class WrenchCommandInterface : public BaseCommandInterface { public: WrenchCommandInterface() = delete; - WrenchCommandInterface(const PIDParameters &pid_parameters, + WrenchCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index d9cc4bf1..a13fe491 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -13,7 +13,6 @@ eigen - control_toolbox fri_client_sdk geometry_msgs kdl_parser diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 14ad96bf..7d11414c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters, @@ -26,16 +26,16 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #endif { command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; } case KUKA::FRI::EClientCommandMode::TORQUE: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; case KUKA::FRI::EClientCommandMode::WRENCH: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; default: std::string err = "Unsupported client command mode."; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index eb842ccc..2bf0ed2c 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -1,34 +1,41 @@ #include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { -ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} +ExponentialFilter::ExponentialFilter() + : tau_(std::numeric_limits::quiet_NaN()), + sample_time_(std::numeric_limits::quiet_NaN()), + alpha_(std::numeric_limits::quiet_NaN()) {} -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { - set_cutoff_frequency(cutoff_frequency, sample_time); +ExponentialFilter::ExponentialFilter(const double &tau) : tau_(tau) {} + +void ExponentialFilter::initialize(const double &sample_time) { + if (std::isnan(tau_)) { + throw std::runtime_error("Time constant must be set before initializing."); + } + return initialize(tau_, sample_time); } -void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, - const double &sample_time) { - cutoff_frequency_ = cutoff_frequency; - if (cutoff_frequency_ > (1. / sample_time)) { - cutoff_frequency_ = (1. / sample_time); +void ExponentialFilter::initialize(const double &tau, const double &sample_time) { + if (tau <= 0.0) { + throw std::runtime_error("Time constant must be positive and greater zero."); } - sample_time_ = sample_time; - alpha_ = compute_alpha_(cutoff_frequency, sample_time); - if (!validate_alpha_(alpha_)) { + if (sample_time < 0.0) { + throw std::runtime_error("Sample time must be positive."); + } + double alpha = 1.0 - std::exp(-sample_time / tau); + if (!validate_alpha_(alpha)) { throw std::runtime_error("Alpha is not within [0, 1]"); } -} - -double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, - const double &sample_time) { - double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; - return std::cos(omega_3db) - 1 + - std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); + tau_ = tau; + sample_time_ = sample_time; + alpha_ = alpha; } bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } +JointExponentialFilterArray::JointExponentialFilterArray(const double &tau) + : exponential_filter_(tau) {} + void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { previous[i] = exponential_filter_.compute(current_i, previous[i]); @@ -36,47 +43,22 @@ void JointExponentialFilterArray::compute(const double *const current, jnt_array }); } -void JointExponentialFilterArray::initialize(const double &cutoff_frequency, - const double &sample_time) { - exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); - initialized_ = true; +void JointExponentialFilterArray::compute(const_jnt_array_t_ref current, jnt_array_t_ref previous) { + compute(current.data(), previous); } -JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) - : pid_parameters_(pid_parameters) // keep local copy of parameters since - // controller_toolbox::Pid::getGains is not const correct - // (i.e. can't be called in this->log_info) -{ - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, - pid_parameters_.i_min, pid_parameters_.antiwindup); - }); -} - -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &sample_time) { + exponential_filter_.initialize(sample_time); + initialized_ = true; } -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &tau, const double &sample_time) { + exponential_filter_.initialize(tau, sample_time); + initialized_ = true; } -void JointPIDArray::log_info() const { +void JointExponentialFilterArray::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); -}; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* tau: %.5f s", exponential_filter_.get_tau()); +} } // 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 f53bddeb..e43111ea 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -2,10 +2,10 @@ namespace lbr_fri_ros2 { -BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, +BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : joint_position_pid_(pid_parameters) { + : joint_position_filter_(joint_position_tau) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,6 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - joint_position_pid_.log_info(); + joint_position_filter_.log_info(); } } // 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 b80530f3..95533981 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { PositionCommandInterface::PositionCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -40,11 +40,11 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 3054bf55..d34b457e 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -15,7 +15,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -23,7 +22,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { } std::memcpy(state_.measured_joint_position.data(), state.getMeasuredJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -34,9 +32,14 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; }; @@ -52,7 +55,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -60,7 +62,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, } std::memcpy(state_.measured_joint_position.data(), joint_position.data(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -71,24 +72,27 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; } void StateInterface::init_filters_() { - external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, - state_.sample_time); - measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, - state_.sample_time); + external_torque_filter_.initialize(parameters_.external_torque_tau, state_.sample_time); + measured_torque_filter_.initialize(parameters_.measured_torque_tau, state_.sample_time); } void StateInterface::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %.1f Hz", - parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f Hz", - parameters_.measured_torque_cutoff_frequency); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_tau: %.5f s", + parameters_.external_torque_tau); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_tau: %.5f s", + parameters_.measured_torque_tau); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b658b266..54f53a2b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { TorqueCommandInterface::TorqueCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -30,12 +30,11 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.torque = command_target_.torque; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9dd9618a..1a1c0ab7 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { WrenchCommandInterface::WrenchCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -28,12 +28,11 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.wrench = command_target_.wrench; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 127fd2ee..d662643a 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -18,7 +18,6 @@ class TestCommandInterfaces : public ::testing::Test { public: TestCommandInterfaces() : random_engine_(std::random_device{}()) { - pid_params_ = lbr_fri_ros2::PIDParameters(); cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); state_interface_params_ = lbr_fri_ros2::StateInterfaceParameters(); @@ -40,17 +39,17 @@ class TestCommandInterfaces : public ::testing::Test { case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; } case KUKA::FRI::EClientCommandMode::TORQUE: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; case KUKA::FRI::EClientCommandMode::WRENCH: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; default: throw std::runtime_error("Unsupported client command mode."); @@ -129,7 +128,7 @@ class TestCommandInterfaces : public ::testing::Test { std::default_random_engine random_engine_; std::uniform_real_distribution uniform_real_dist_{-1.0, 1.0}; - lbr_fri_ros2::PIDParameters pid_params_; + double joint_position_tau_{0.04}; lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; lbr_fri_ros2::StateInterfaceParameters state_interface_params_; diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index e2f7830d..7da064aa 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 1.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::POSITION, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::POSITION, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 9c580828..45472578 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::TORQUE, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::TORQUE, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index 9ad8b315..e353476b 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::WRENCH, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::WRENCH, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); 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 9a43948b..a7448a3c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -23,7 +23,6 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" @@ -44,15 +43,10 @@ struct SystemInterfaceParameters { const char *remote_host{nullptr}; int32_t rt_prio{80}; bool open_loop{true}; - double pid_p{0.0}; - double pid_i{0.0}; - double pid_d{0.0}; - double pid_i_max{0.0}; - double pid_i_min{0.0}; - double pid_antiwindup{0.0}; + double joint_position_tau{0.04}; std::string command_guard_variant{"default"}; - double external_torque_cutoff_frequency{10.0}; - double measured_torque_cutoff_frequency{10.0}; + double external_torque_tau{0.04}; + double measured_torque_tau{0.04}; }; struct EstimatedFTSensorParameters { diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 91cbf710..ebab71e4 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -18,15 +18,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { } // setup driver - lbr_fri_ros2::PIDParameters pid_parameters; lbr_fri_ros2::CommandGuardParameters command_guard_parameters; lbr_fri_ros2::StateInterfaceParameters state_interface_parameters; - pid_parameters.p = parameters_.pid_p; - pid_parameters.i = parameters_.pid_i; - pid_parameters.d = parameters_.pid_d; - pid_parameters.i_max = parameters_.pid_i_max; - pid_parameters.i_min = parameters_.pid_i_min; - pid_parameters.antiwindup = parameters_.pid_antiwindup; for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) { command_guard_parameters.joint_names[idx] = system_info.joints[idx].name; command_guard_parameters.max_positions[idx] = @@ -38,14 +31,12 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { command_guard_parameters.max_torques[idx] = std::stod(system_info.joints[idx].parameters.at("max_torque")); } - state_interface_parameters.external_torque_cutoff_frequency = - parameters_.external_torque_cutoff_frequency; - state_interface_parameters.measured_torque_cutoff_frequency = - parameters_.measured_torque_cutoff_frequency; + state_interface_parameters.external_torque_tau = parameters_.external_torque_tau; + state_interface_parameters.measured_torque_tau = parameters_.measured_torque_tau; try { async_client_ptr_ = std::make_shared( - parameters_.client_command_mode, pid_parameters, command_guard_parameters, + parameters_.client_command_mode, parameters_.joint_position_tau, command_guard_parameters, parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { @@ -412,17 +403,10 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & info_.hardware_parameters["pid_antiwindup"].end(), info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); // convert to lower case - parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); - parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); - parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); - parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); - parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); - parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; + parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); - parameters_.external_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); - parameters_.measured_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); + parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]); + parameters_.measured_torque_tau = std::stod(info_.hardware_parameters["measured_torque_tau"]); } catch (const std::out_of_range &e) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR From 78462d4833d8237ee537b0a460258eb001e7aa44 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 18:48:32 +0000 Subject: [PATCH 30/42] addressed review (https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/220#pullrequestreview-2443348238) --- lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 46 +++++++++++++++++++ lbr_fri_ros2/src/control.cpp | 33 +++++++++++++ .../controllers/admittance_controller.hpp | 24 +--------- .../controllers/twist_controller.hpp | 1 + .../src/controllers/admittance_controller.cpp | 36 ++++++++------- .../src/controllers/twist_controller.cpp | 3 ++ 6 files changed, 104 insertions(+), 39 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp index 88297ae5..bab95773 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -3,10 +3,13 @@ #include #include +#include #include #include "eigen3/Eigen/Core" #include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" @@ -23,6 +26,9 @@ struct InvJacCtrlParameters { }; class InvJacCtrlImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::InvJacCtrlImpl"; + public: InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters ¶meters); @@ -34,6 +40,8 @@ class InvJacCtrlImpl { inline const std::unique_ptr &get_kinematics_ptr() const { return kinematics_ptr_; }; + void log_info() const; + protected: void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); @@ -44,5 +52,43 @@ class InvJacCtrlImpl { Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; }; + +struct AdmittanceParameters { + AdmittanceParameters() = delete; + AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0) + : m(m), b(b), k(k) { + if (m <= 0.0) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + if (b < 0.0) { + throw std::runtime_error("Damping must be positive."); + } + if (k < 0.0) { + throw std::runtime_error("Stiffness must be positive."); + } + } + + double m = 1.0; + double b = 0.1; + double k = 0.0; +}; + +class AdmittanceImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl"; + +public: + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + + void compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx); + + void log_info() const; + +protected: + AdmittanceParameters parameters_; +}; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp index 7016c974..66ba5284 100644 --- a/lbr_fri_ros2/src/control.cpp +++ b/lbr_fri_ros2/src/control.cpp @@ -44,6 +44,20 @@ void InvJacCtrlImpl::compute(const Eigen::Matrix &twis compute_impl_(q, dq); } +void InvJacCtrlImpl::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain root: %s", + parameters_.chain_root.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain tip: %s", parameters_.chain_tip.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Twist in tip frame: %s", + parameters_.twist_in_tip_frame ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.damping); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max linear velocity: %.3f", + parameters_.max_linear_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f", + parameters_.max_angular_velocity); +} + void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { // clip velocity twist_target_.head(3).unaryExpr([&](double v) { @@ -69,4 +83,23 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) // compute target joint veloctiy and map it to dq Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; } + +void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + if (parameters_.m <= 0.0) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; +} + +void AdmittanceImpl::log_info() const { + { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: %.3f", parameters_.m); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.b); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Stiffness: %.3f", parameters_.k); + } +} } // namespace lbr_fri_ros2 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 d110d2ba..17143903 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 @@ -21,27 +21,6 @@ #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -struct AdmittanceParameters { - double m = 1.0; - double b = 0.1; - double k = 0.0; -}; - -class AdmittanceImpl { -public: - AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} - - void compute(const Eigen::Matrix &f_ext, - const Eigen::Matrix &x, - const Eigen::Matrix &dx, - Eigen::Matrix &ddx) { - ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; - } - -protected: - AdmittanceParameters parameters_; -}; - class AdmittanceController : public controller_interface::ControllerInterface { public: AdmittanceController(); @@ -71,10 +50,11 @@ class AdmittanceController : public controller_interface::ControllerInterface { void configure_admittance_impl_(); void configure_inv_jac_ctrl_impl_(); void zero_all_values_(); + void log_info_() const; // admittance bool initialized_ = false; - std::unique_ptr admittance_impl_ptr_; + std::unique_ptr admittance_impl_ptr_; Eigen::Matrix x_init_, x_prev_; Eigen::Matrix f_ext_, x_, dx_, ddx_; 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 b4d27780..f0a1fea4 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 @@ -52,6 +52,7 @@ class TwistController : public controller_interface::ControllerInterface { void reset_command_buffer_(); void configure_joint_names_(); void configure_inv_jac_ctrl_impl_(); + void log_info_() const; // some safety checks std::atomic updates_since_last_command_ = 0; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index fe06c703..0178285c 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -50,6 +50,7 @@ controller_interface::CallbackReturn AdmittanceController::on_init() { configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); + log_info_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize admittance controller with: %s.", e.what()); @@ -76,7 +77,7 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim // compute forward kinematics auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_); x_.head(3) = Eigen::Map>(chain_tip_frame.p.data); - chain_tip_frame.M.GetRPY(x_init_[3], x_init_[4], x_init_[5]); + x_.tail(3) = Eigen::Map>(chain_tip_frame.M.GetRot().data); // compute steady state position and orientation if (!initialized_) { @@ -85,15 +86,17 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim initialized_ = true; } - // compute velocity + // compute velocity & update previous position dx_ = (x_ - x_prev_) / period.seconds(); + x_prev_ = x_; + + // convert f_ext_ back to root frame + 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); // compute admittance admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_); - // update previous position - x_prev_ = x_; - // integrate ddx_ to command velocity twist_command_ = ddx_ * period.seconds(); @@ -198,17 +201,11 @@ void AdmittanceController::configure_joint_names_() { } void AdmittanceController::configure_admittance_impl_() { - admittance_impl_ptr_ = std::make_unique( - AdmittanceParameters{this->get_node()->get_parameter("admittance.mass").as_double(), - this->get_node()->get_parameter("admittance.damping").as_double(), - this->get_node()->get_parameter("admittance.stiffness").as_double()}); - RCLCPP_INFO(this->get_node()->get_logger(), "Admittance controller initialized."); - RCLCPP_INFO(this->get_node()->get_logger(), "Mass: %f", - this->get_node()->get_parameter("admittance.mass").as_double()); - RCLCPP_INFO(this->get_node()->get_logger(), "Damping: %f", - this->get_node()->get_parameter("admittance.damping").as_double()); - RCLCPP_INFO(this->get_node()->get_logger(), "Stiffness: %f", - this->get_node()->get_parameter("admittance.stiffness").as_double()); + admittance_impl_ptr_ = + std::make_unique(lbr_fri_ros2::AdmittanceParameters{ + this->get_node()->get_parameter("admittance.mass").as_double(), + this->get_node()->get_parameter("admittance.damping").as_double(), + this->get_node()->get_parameter("admittance.stiffness").as_double()}); } void AdmittanceController::configure_inv_jac_ctrl_impl_() { @@ -217,7 +214,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { lbr_fri_ros2::InvJacCtrlParameters{ this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), - true, // always assume twist in tip frame, since force-torque is estimated in tip frame + false, // always assume twist in root frame this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); @@ -231,6 +228,11 @@ void AdmittanceController::zero_all_values_() { std::fill(dq_.begin(), dq_.end(), 0.0); twist_command_.setZero(); } + +void AdmittanceController::log_info_() const { + admittance_impl_ptr_->log_info(); + inv_jac_ctrl_impl_ptr_->log_info(); +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 9994f556..3b3befdf 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -44,6 +44,7 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); configure_inv_jac_ctrl_impl_(); + log_info_(); timeout_ = this->get_node()->get_parameter("timeout").as_double(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", @@ -175,6 +176,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() { this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); } + +void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); } } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" From 412922e1f08cdff8ae620f725f808783fa6ce16f Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 19:03:24 +0000 Subject: [PATCH 31/42] moved controller config for standalone URDF use --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_bringup/launch/gazebo.launch.py | 2 +- lbr_bringup/lbr_bringup/ros2_control.py | 2 +- .../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst | 8 ++++++-- .../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst | 4 ++-- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 ++++---- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 ++++---- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 ++-- lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 2 +- .../ros2_control}/lbr_controllers.yaml | 0 lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 +- 13 files changed, 25 insertions(+), 21 deletions(-) rename {lbr_ros2_control/config => lbr_description/ros2_control}/lbr_controllers.yaml (100%) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index b0395b29..c55bcb01 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -183,5 +183,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 `lbr_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 34da3615..b6505ab4 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/config/lbr_controllers.yaml # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index e45a5fb9..3083ba10 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -11,7 +11,7 @@ class LBRROS2ControlMixin: def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg_pkg", - default_value="lbr_ros2_control", + default_value="lbr_description", description="Controller configuration package. The package containing the ctrl_cfg.", ) 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 f8426012..22e46da4 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 @@ -8,6 +8,10 @@ lbr_demos_advanced_cpp :local: :backlinks: none +Twist Controller +---------------- +TODO + Admittance Controller --------------------- This demo implements a simple admittance controller. @@ -15,7 +19,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -56,7 +60,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 616751d0..8e23245f 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 @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 652f5aef..530e82f4 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 2eeb8dd6..df04a2e6 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index ccab9103..02a35fbe 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -63,7 +63,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -123,7 +123,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 0816829a..8856e511 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -42,7 +42,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 78a833fc..637fc8bb 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_ros2_control)/config/lbr_controllers.yaml + $(find lbr_description)/config/lbr_controllers.yaml /${robot_name} diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml similarity index 100% rename from lbr_ros2_control/config/lbr_controllers.yaml rename to lbr_description/ros2_control/lbr_controllers.yaml diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 52e70858..f4fba85c 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -90,7 +90,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. 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 `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 07bc8c7c..986895ae 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -30,7 +30,7 @@ Hardware components and controllers are loaded as plugins (components) by the `` 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. `lbr_controllers.yaml `_:octicon:`link-external`. Hardware Plugin --------------- From afda9c9a34ef750bd8d9231e57e23b6ae754b2bb Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 19:04:12 +0000 Subject: [PATCH 32/42] moved controller config for standalone URDF use --- lbr_ros2_control/CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 8621fa41..837382d2 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -98,9 +98,4 @@ install( INCLUDES DESTINATION include ) -install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - ament_package() From 26f502233aa10b2986b4c2851fa4722bb38f2083 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 20:22:54 +0000 Subject: [PATCH 33/42] updated documentation (#163) --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_bringup/launch/gazebo.launch.py | 2 +- lbr_bringup/lbr_bringup/ros2_control.py | 7 ++- .../config/admittance_control.yaml | 8 --- .../config/lbr_system_config.yaml | 37 ++++++++++++ .../doc/lbr_demos_advanced_cpp.rst | 58 +++++++++++++------ .../doc/lbr_demos_advanced_py.rst | 4 +- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 +-- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 +-- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 +- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 2 +- .../ros2_control/lbr_controllers.yaml | 8 +-- lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 +- 15 files changed, 103 insertions(+), 51 deletions(-) delete mode 100644 lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml create mode 100644 lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index c55bcb01..4fab2142 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -183,5 +183,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 `lbr_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 b6505ab4..d9f68126 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/config/lbr_controllers.yaml + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index 3083ba10..f98ef127 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -19,7 +19,7 @@ def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: def arg_ctrl_cfg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg", - default_value="config/lbr_controllers.yaml", + default_value="ros2_control/lbr_controllers.yaml", description="Relative path from ctrl_cfg_pkg to the controllers.", ) @@ -30,6 +30,7 @@ def arg_ctrl() -> DeclareLaunchArgument: default_value="joint_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", choices=[ + "admittance_controller", "joint_trajectory_controller", "forward_position_controller", "lbr_joint_position_command_controller", @@ -82,11 +83,11 @@ def node_ros2_control( [ FindPackageShare( LaunchConfiguration( - "ctrl_cfg_pkg", default="lbr_ros2_control" + "ctrl_cfg_pkg", default="lbr_description" ) ), LaunchConfiguration( - "ctrl_cfg", default="config/lbr_controllers.yaml" + "ctrl_cfg", default="ros2_control/lbr_controllers.yaml" ), ] ), diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml deleted file mode 100644 index b356195d..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**/admittance_control: - ros__parameters: - base_link: "lbr_link_0" - end_effector_link: "lbr_link_ee" - f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] - dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - exp_smooth: 0.95 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 new file mode 100644 index 00000000..165055de --- /dev/null +++ b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml @@ -0,0 +1,37 @@ +# 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 22e46da4..0b6fea60 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 @@ -10,16 +10,7 @@ lbr_demos_advanced_cpp Twist Controller ---------------- -TODO - -Admittance Controller ---------------------- -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` +This demo uses the twist controller. #. Remote side configurations: @@ -39,16 +30,47 @@ This demo implements a simple admittance controller. .. code-block:: bash ros2 launch lbr_bringup hardware.launch.py \ - ctrl:=lbr_joint_position_command_controller \ + ctrl:=twist_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Launch the `admittance_control `_:octicon:`link-external`: +#. Next, publish to the ``/lbr/command/twist`` topic: - .. code-block:: bash - - ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml + .. code-block:: bash + + ros2 topic pub \ + --rate 100 \ + /lbr/command/twist \ + geometry_msgs/msg/Twist \ + "{linear: {x: 0.0, y: 0.0, z: 0.05}, angular: {x: 0.0, y: 0.0, z: 0.0}}" + +#. If you ``Ctrl+C`` the publisher, the ``twist_controller`` throws an error as it expects a continuous stream of twist commands. + +Admittance Controller +--------------------- +This demo uses the admittance controller. + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``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!): + + .. 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. @@ -60,7 +82,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 8e23245f..2284202f 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 @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 530e82f4..8f7b834d 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 df04a2e6..4d1258d4 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -15,7 +15,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ 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` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 02a35fbe..cdfdbb49 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -63,7 +63,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -123,7 +123,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 8856e511..f65da254 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -42,7 +42,7 @@ 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` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 637fc8bb..3c9fa1ab 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_description)/config/lbr_controllers.yaml + $(find lbr_description)/ros2_control/lbr_controllers.yaml /${robot_name} diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml index aae3b6ea..207113be 100644 --- a/lbr_description/ros2_control/lbr_controllers.yaml +++ b/lbr_description/ros2_control/lbr_controllers.yaml @@ -97,15 +97,15 @@ ros__parameters: robot_name: lbr admittance: - mass: 0.2 - damping: 0.1 + mass: 0.01 + damping: 1.0 stiffness: 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: 0.1 # maximum linear velocity - max_angular_velocity: 0.1 # maximum linear acceleration + max_linear_velocity: 2.0 # maximum linear velocity + max_angular_velocity: 2.0 # maximum linear acceleration /**/twist_controller: ros__parameters: diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index f4fba85c..50d95fb3 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -90,7 +90,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. 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 `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 986895ae..7e74b5b3 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -30,7 +30,7 @@ Hardware components and controllers are loaded as plugins (components) by the `` 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. `lbr_controllers.yaml `_:octicon:`link-external`. Hardware Plugin --------------- From 5c84eb17b19cbd663c279958dbce8d1bee074cfa Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 21:19:29 +0000 Subject: [PATCH 34/42] added backport workflow --- .github/workflows/backport.yaml | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 .github/workflows/backport.yaml diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml new file mode 100644 index 00000000..912d637a --- /dev/null +++ b/.github/workflows/backport.yaml @@ -0,0 +1,33 @@ +name: Backport merged pull request +on: + issue_comment: + types: [created] +permissions: + contents: write # so it can comment + pull-requests: write # so it can create pull requests +jobs: + backport: + name: Backport pull request + runs-on: ubuntu-latest + + # Only run when pull request is merged + # or when a comment starting with `/backport` is created by someone other than the + # https://github.com/backport-action bot user (user id: 97796249). Note that if you use your + # own PAT as `github_token`, that you should replace this id with yours. + if: > + ( + github.event_name == 'pull_request_target' && + github.event.pull_request.merged + ) || ( + github.event_name == 'issue_comment' && + github.event.issue.pull_request && + github.event.comment.user.id != 97796249 && + startsWith(github.event.comment.body, '/backport') + ) + steps: + - uses: actions/checkout@v4 + - name: Create backport pull requests + uses: korthout/backport-action@v3 + with: + label_pattern: "" + target_branches: humble From eff5a63d5eae1d25d2e66298f96270d8bfa8b3b1 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 21:55:12 +0000 Subject: [PATCH 35/42] added merge commit skip --- .github/workflows/backport.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml index 912d637a..daacbb31 100644 --- a/.github/workflows/backport.yaml +++ b/.github/workflows/backport.yaml @@ -31,3 +31,4 @@ jobs: with: label_pattern: "" target_branches: humble + merge_commits: skip From 7c00ac916eb80fa356e885bed9631506a1601077 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 13:22:30 +0000 Subject: [PATCH 36/42] added multi-dof gains (#163) --- .../ros2_control/lbr_controllers.yaml | 10 +- lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 42 ++++++--- lbr_fri_ros2/src/control.cpp | 69 ++++++++++---- .../src/controllers/admittance_controller.cpp | 94 +++++++++++++++++-- .../src/controllers/twist_controller.cpp | 36 ++++++- 5 files changed, 206 insertions(+), 45 deletions(-) diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml index 207113be..9a2a1a3d 100644 --- a/lbr_description/ros2_control/lbr_controllers.yaml +++ b/lbr_description/ros2_control/lbr_controllers.yaml @@ -97,15 +97,17 @@ ros__parameters: robot_name: lbr admittance: - mass: 0.01 - damping: 1.0 - stiffness: 0.0 + 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] + 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 + 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 /**/twist_controller: ros__parameters: @@ -117,4 +119,6 @@ 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 + 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 # stop controller if no command is received within this time [s] diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp index bab95773..128f9b85 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -17,12 +17,14 @@ namespace lbr_fri_ros2 { struct InvJacCtrlParameters { - std::string chain_root; - std::string chain_tip; - bool twist_in_tip_frame; - double damping; - double max_linear_velocity; - double max_angular_velocity; + std::string chain_root = "lbr_link_0"; + std::string chain_tip = "lbr_link_ee"; + bool twist_in_tip_frame = true; + double damping = 0.2; + double max_linear_velocity = 0.1; + double max_angular_velocity = 0.1; + jnt_array_t joint_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + cart_array_t cartesian_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; }; class InvJacCtrlImpl { @@ -44,6 +46,7 @@ class InvJacCtrlImpl { protected: void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); + void set_all_zero_(); InvJacCtrlParameters parameters_; @@ -51,26 +54,30 @@ class InvJacCtrlImpl { std::unique_ptr kinematics_ptr_; Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; + Eigen::Matrix q_gains_; + Eigen::Matrix x_gains_; }; struct AdmittanceParameters { AdmittanceParameters() = delete; - AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0) + AdmittanceParameters(const_cart_array_t_ref m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}, + const_cart_array_t_ref b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + const_cart_array_t_ref k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) : m(m), b(b), k(k) { - if (m <= 0.0) { + if (std::any_of(m.cbegin(), m.cend(), [](const double &m_i) { return m_i <= 0.0; })) { throw std::runtime_error("Mass must be positive and greater zero."); } - if (b < 0.0) { + if (std::any_of(b.cbegin(), b.cend(), [](const double &b_i) { return b_i < 0.0; })) { throw std::runtime_error("Damping must be positive."); } - if (k < 0.0) { + if (std::any_of(k.cbegin(), k.cend(), [](const double &k_i) { return k_i < 0.0; })) { throw std::runtime_error("Stiffness must be positive."); } } - double m = 1.0; - double b = 0.1; - double k = 0.0; + cart_array_t m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; + cart_array_t b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; + cart_array_t k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; }; class AdmittanceImpl { @@ -78,7 +85,12 @@ class AdmittanceImpl { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl"; public: - AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + AdmittanceImpl() = delete; + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) { + m_ = Eigen::Map>(parameters_.m.data()); + b_ = Eigen::Map>(parameters_.b.data()); + k_ = Eigen::Map>(parameters_.k.data()); + } void compute(const Eigen::Matrix &f_ext, const Eigen::Matrix &x, @@ -89,6 +101,8 @@ class AdmittanceImpl { protected: AdmittanceParameters parameters_; + + Eigen::Matrix m_, b_, k_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp index 66ba5284..30e61cbb 100644 --- a/lbr_fri_ros2/src/control.cpp +++ b/lbr_fri_ros2/src/control.cpp @@ -6,6 +6,10 @@ InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description, : parameters_(parameters) { kinematics_ptr_ = std::make_unique(robot_description, parameters_.chain_root, parameters_.chain_tip); + set_all_zero_(); + q_gains_ = Eigen::Map>(parameters_.joint_gains.data()); + x_gains_ = + Eigen::Map>(parameters_.cartesian_gains.data()); } void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, @@ -56,6 +60,43 @@ void InvJacCtrlImpl::log_info() const { parameters_.max_linear_velocity); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f", parameters_.max_angular_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Joint gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.joint_gains[0], parameters_.joint_gains[1], parameters_.joint_gains[2], + parameters_.joint_gains[3], parameters_.joint_gains[4], parameters_.joint_gains[5], + parameters_.joint_gains[6]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Cartesian gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.cartesian_gains[0], parameters_.cartesian_gains[1], + parameters_.cartesian_gains[2], parameters_.cartesian_gains[3], + parameters_.cartesian_gains[4], parameters_.cartesian_gains[5]); +} + +void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + if ((m_.array() < 0.).any()) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + ddx = (f_ext - b_.asDiagonal() * dx - k_.asDiagonal() * x).array() / m_.array(); +} + +void AdmittanceImpl::log_info() const { + { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.m[0], parameters_.m[1], parameters_.m[2], parameters_.m[3], + parameters_.m[4], parameters_.m[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Damping: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.b[0], + parameters_.b[1], parameters_.b[2], parameters_.b[3], parameters_.b[4], + parameters_.b[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Stiffness: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.k[0], + parameters_.k[1], parameters_.k[2], parameters_.k[3], parameters_.k[4], + parameters_.k[5]); + } } void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { @@ -76,30 +117,22 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); } + twist_target_ = x_gains_.asDiagonal() * twist_target_; + // compute jacobian auto jacobian = kinematics_ptr_->compute_jacobian(q); jacobian_inv_ = pinv(jacobian.data, parameters_.damping); // compute target joint veloctiy and map it to dq - Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; + Eigen::Map>(dq.data()) = + q_gains_.asDiagonal() * jacobian_inv_ * twist_target_; } -void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, - const Eigen::Matrix &x, - const Eigen::Matrix &dx, - Eigen::Matrix &ddx) { - if (parameters_.m <= 0.0) { - throw std::runtime_error("Mass must be positive and greater zero."); - } - ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; -} - -void AdmittanceImpl::log_info() const { - { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: %.3f", parameters_.m); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.b); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Stiffness: %.3f", parameters_.k); - } +void InvJacCtrlImpl::set_all_zero_() { + std::fill(q_.begin(), q_.end(), 0.0); + jacobian_inv_.setZero(); + twist_target_.setZero(); + q_gains_.setZero(); + x_gains_.setZero(); } } // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 0178285c..bbe950b6 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -39,14 +39,21 @@ AdmittanceController::state_interface_configuration() const { controller_interface::CallbackReturn AdmittanceController::on_init() { try { this->get_node()->declare_parameter("robot_name", "lbr"); - this->get_node()->declare_parameter("admittance.mass", 1.0); - this->get_node()->declare_parameter("admittance.damping", 0.1); - this->get_node()->declare_parameter("admittance.stiffness", 0.0); + this->get_node()->declare_parameter("admittance.mass", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); + this->get_node()->declare_parameter("admittance.damping", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + this->get_node()->declare_parameter("admittance.stiffness", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + 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)); configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); @@ -201,14 +208,82 @@ void AdmittanceController::configure_joint_names_() { } void AdmittanceController::configure_admittance_impl_() { - admittance_impl_ptr_ = - std::make_unique(lbr_fri_ros2::AdmittanceParameters{ - this->get_node()->get_parameter("admittance.mass").as_double(), - this->get_node()->get_parameter("admittance.damping").as_double(), - this->get_node()->get_parameter("admittance.stiffness").as_double()}); + 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).", + 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."); + } + if (this->get_node()->get_parameter("admittance.damping").as_double_array().size() != + 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).", + 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."); + } + 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 " + "of freedom " + "(%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) { + 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) { + 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) { + stiffness_array[i] = + this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i]; + } + admittance_impl_ptr_ = std::make_unique( + lbr_fri_ros2::AdmittanceParameters{mass_array, damping_array, stiffness_array}); } void AdmittanceController::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).", + 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."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + 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).", + 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) { + 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) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } inv_jac_ctrl_impl_ptr_ = std::make_unique( this->get_robot_description(), lbr_fri_ros2::InvJacCtrlParameters{ @@ -217,7 +292,8 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { false, // always assume twist in root frame this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), - this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); } void AdmittanceController::zero_all_values_() { diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 3b3befdf..d851f165 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -41,6 +41,10 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + 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("timeout", 0.2); configure_joint_names_(); configure_inv_jac_ctrl_impl_(); @@ -166,6 +170,35 @@ void TwistController::configure_joint_names_() { } 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).", + 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."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + 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).", + 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) { + 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) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } inv_jac_ctrl_impl_ptr_ = std::make_unique( this->get_robot_description(), lbr_fri_ros2::InvJacCtrlParameters{ @@ -174,7 +207,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() { this->get_node()->get_parameter("inv_jac_ctrl.twist_in_tip_frame").as_bool(), this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), - this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); } void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); } From 5bc524cec95395e806c40ccd9abea7a3a09808a6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 17:03:08 +0000 Subject: [PATCH 37/42] Revert "humble -> rolling, ubuntu 22.04 -> 24.04 (#180)" This reverts commit 03cd7539081856be242b883cc750fce6c9d9bea4. --- .github/CONTRIBUTING.md | 2 +- ...11.yml => build-ubuntu-22.04-fri-1.11.yml} | 6 +-- ...14.yml => build-ubuntu-22.04-fri-1.14.yml} | 6 +-- ...15.yml => build-ubuntu-22.04-fri-1.15.yml} | 6 +-- ...16.yml => build-ubuntu-22.04-fri-1.16.yml} | 6 +-- ...2.5.yml => build-ubuntu-22.04-fri-2.5.yml} | 6 +-- ...2.7.yml => build-ubuntu-22.04-fri-2.7.yml} | 6 +-- .github/workflows/build.yml | 4 +- README.md | 26 ++++++------ docker/Dockerfile | 2 +- docker/doc/docker.rst | 2 +- lbr_bringup/config/moveit_servo.yaml | 41 ++++++++++++------- lbr_bringup/doc/lbr_bringup.rst | 12 +++--- .../doc/lbr_demos_advanced_cpp.rst | 4 +- .../doc/lbr_demos_advanced_py.rst | 12 +++--- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 24 +++++------ lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 24 +++++------ lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 12 +++--- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 10 ++--- lbr_fri_ros2_stack/repos-fri-1.11.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.14.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.15.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-1.16.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-2.5.yaml | 2 +- lbr_fri_ros2_stack/repos-fri-2.7.yaml | 2 +- lbr_moveit_config/doc/lbr_moveit_config.rst | 6 +-- lbr_ros2_control/doc/lbr_ros2_control.rst | 12 +++--- 27 files changed, 126 insertions(+), 115 deletions(-) rename .github/workflows/{build-ubuntu-24.04-fri-1.11.yml => build-ubuntu-22.04-fri-1.11.yml} (71%) rename .github/workflows/{build-ubuntu-24.04-fri-1.14.yml => build-ubuntu-22.04-fri-1.14.yml} (71%) rename .github/workflows/{build-ubuntu-24.04-fri-1.15.yml => build-ubuntu-22.04-fri-1.15.yml} (71%) rename .github/workflows/{build-ubuntu-24.04-fri-1.16.yml => build-ubuntu-22.04-fri-1.16.yml} (71%) rename .github/workflows/{build-ubuntu-24.04-fri-2.5.yml => build-ubuntu-22.04-fri-2.5.yml} (71%) rename .github/workflows/{build-ubuntu-24.04-fri-2.7.yml => build-ubuntu-22.04-fri-2.7.yml} (71%) diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md index e9e31d2c..5310de88 100644 --- a/.github/CONTRIBUTING.md +++ b/.github/CONTRIBUTING.md @@ -13,7 +13,7 @@ Contributions are vital to this project. If you want to contribute a feature / f 1. Open an issue: - Explain the issue and your solution - - Request a new branch: `dev--`, e.g. `dev-rolling-my-new-demo` + - Request a new branch: `dev--`, e.g. `dev-humble-my-new-demo` 2. Fork this repository 3. Create a [pull request](https://github.com/lbr-stack/lbr_fri_ros2_stack/pulls) against `dev--` diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.11.yml b/.github/workflows/build-ubuntu-22.04-fri-1.11.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-1.11.yml rename to .github/workflows/build-ubuntu-22.04-fri-1.11.yml index 3de0799f..1f105d60 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.11.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-1.11.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-1.11 +name: build-ubuntu-22.04-fri-1.11 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 1.11 diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.14.yml b/.github/workflows/build-ubuntu-22.04-fri-1.14.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-1.14.yml rename to .github/workflows/build-ubuntu-22.04-fri-1.14.yml index c85da043..0dbb74c9 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.14.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-1.14.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-1.14 +name: build-ubuntu-22.04-fri-1.14 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 1.14 diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.15.yml b/.github/workflows/build-ubuntu-22.04-fri-1.15.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-1.15.yml rename to .github/workflows/build-ubuntu-22.04-fri-1.15.yml index 5ec898dc..7842296c 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.15.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-1.15.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-1.15 +name: build-ubuntu-22.04-fri-1.15 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 1.15 diff --git a/.github/workflows/build-ubuntu-24.04-fri-1.16.yml b/.github/workflows/build-ubuntu-22.04-fri-1.16.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-1.16.yml rename to .github/workflows/build-ubuntu-22.04-fri-1.16.yml index 855eb38c..2d45ee57 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-1.16.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-1.16.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-1.16 +name: build-ubuntu-22.04-fri-1.16 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 1.16 diff --git a/.github/workflows/build-ubuntu-24.04-fri-2.5.yml b/.github/workflows/build-ubuntu-22.04-fri-2.5.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-2.5.yml rename to .github/workflows/build-ubuntu-22.04-fri-2.5.yml index 8068003e..de3e3a82 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-2.5.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-2.5.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-2.5 +name: build-ubuntu-22.04-fri-2.5 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 2.5 diff --git a/.github/workflows/build-ubuntu-24.04-fri-2.7.yml b/.github/workflows/build-ubuntu-22.04-fri-2.7.yml similarity index 71% rename from .github/workflows/build-ubuntu-24.04-fri-2.7.yml rename to .github/workflows/build-ubuntu-22.04-fri-2.7.yml index 284fd789..71c65ae1 100644 --- a/.github/workflows/build-ubuntu-24.04-fri-2.7.yml +++ b/.github/workflows/build-ubuntu-22.04-fri-2.7.yml @@ -1,8 +1,8 @@ -name: ubuntu-24.04-fri-2.7 +name: build-ubuntu-22.04-fri-2.7 on: pull_request: branches: - - rolling + - humble workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -11,5 +11,5 @@ jobs: build: uses: ./.github/workflows/build.yml with: - os: ubuntu-24.04 + os: ubuntu-22.04 fri_version: 2.7 diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cb223e18..ea5e4e11 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -20,5 +20,5 @@ jobs: - uses: ros-tooling/action-ros-ci@v0.2 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: humble + vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${{ inputs.fri_version }}.yaml diff --git a/README.md b/README.md index 4a04a440..8a04632e 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/humble?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,10 @@ 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,12 +26,12 @@ 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.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-22.04` | `humble` | `1.11` | [![build-ubuntu-22.04-fri-1.11](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.11.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.11.yml) | +| `Ubuntu-22.04` | `humble` | `1.14` | [![build-ubuntu-22.04-fri-1.14](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.14.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.14.yml) | +| `Ubuntu-22.04` | `humble` | `1.15` | [![build-ubuntu-22.04-fri-1.15](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.15.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.15.yml) | +| `Ubuntu-22.04` | `humble` | `1.16` | [![build-ubuntu-22.04-fri-1.16](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.16.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-1.16.yml) | +| `Ubuntu-22.04` | `humble` | `2.5` | [![build-ubuntu-22.04-fri-2.5](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.5.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.5.yml) | +| `Ubuntu-22.04` | `humble` | `2.7` | [![build-ubuntu-22.04-fri-2.7](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.7.yml/badge.svg)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build-ubuntu-22.04-fri-2.7.yml) | ## Documentation Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io/en/latest). @@ -46,10 +46,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/humble/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/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml rosdep install --from-paths src -i -r -y ``` diff --git a/docker/Dockerfile b/docker/Dockerfile index 2d0c3e42..7c660dbe 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:rolling-ros-base-jammy +FROM ros:humble-ros-base-jammy # change default shell to bash SHELL ["/bin/bash", "-c"] diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 2d32e9b8..53803077 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/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml #. Install `Docker `_:octicon:`link-external`. diff --git a/lbr_bringup/config/moveit_servo.yaml b/lbr_bringup/config/moveit_servo.yaml index f78fb770..9d292fba 100644 --- a/lbr_bringup/config/moveit_servo.yaml +++ b/lbr_bringup/config/moveit_servo.yaml @@ -1,21 +1,25 @@ # Please do e.g. refer to -# - https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/test_config_panda.yaml -# - https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +# - https://github.com/moveit/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml /**/servo_node: ros__parameters: moveit_servo: - ## Properties of outgoing commands - publish_period: 0.005 # 1/controller manager update rate [seconds] - - incoming_command_timeout: 0.5 # seconds - command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s + ## Properties of incoming commands + command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.2 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 + # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) + # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + + ## Properties of outgoing commands + publish_period: 0.005 # #1/controller manager update rate [seconds] + low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory command_out_type: std_msgs/Float64MultiArray @@ -26,7 +30,6 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands - use_smoothing: true smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, @@ -37,15 +40,23 @@ ## MoveIt properties move_group_name: arm # Often 'manipulator' or 'arm' + planning_frame: link_0 # The MoveIt planning frame. Often 'base_link' or 'world' + + ## Other frames + ee_frame_name: link_ee # The name of the end effector link, used to return the EE pose + robot_link_command_frame: link_0 # commands must be given in the frame of a robot link. Usually either the base or end effector + + ## Stopping behaviour + incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command + # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. + # Important because ROS may drop some messages and we need the robot to halt reliably. + num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this - leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) - # Added as a buffer to joint variable position bounds [in that joint variable's respective units]. - # Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. - # If moving quickly, make these values larger. - joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 4fab2142..779dc199 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -36,7 +36,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 +52,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 +90,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 +106,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 +144,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 @@ -183,5 +183,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 `lbr_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_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 0b6fea60..9fa5c943 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 @@ -81,8 +81,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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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..a87c5776 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 `lbr_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 `lbr_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_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 8f7b834d..d11bf3f2 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 `lbr_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 `lbr_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 `lbr_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 `lbr_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/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index 4d1258d4..901a6f15 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 `lbr_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 `lbr_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 `lbr_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 `lbr_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_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index cdfdbb49..ac21da03 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 `lbr_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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: 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 f65da254..46e27a17 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -2,7 +2,7 @@ lbr_moveit_cpp ============== .. note:: - Also refer to the official `MoveIt `_:octicon:`link-external` documentation. + Also refer to the official `MoveIt `_:octicon:`link-external` documentation. .. contents:: Table of Contents :depth: 2 @@ -29,7 +29,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 @@ -41,8 +41,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 `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -61,7 +61,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_fri_ros2_stack/repos-fri-1.11.yaml b/lbr_fri_ros2_stack/repos-fri-1.11.yaml index 2527d70f..a0e658db 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: humble diff --git a/lbr_fri_ros2_stack/repos-fri-1.14.yaml b/lbr_fri_ros2_stack/repos-fri-1.14.yaml index 74fda90e..3bde0b56 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: humble diff --git a/lbr_fri_ros2_stack/repos-fri-1.15.yaml b/lbr_fri_ros2_stack/repos-fri-1.15.yaml index a31b52b4..5e7f40fe 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: humble diff --git a/lbr_fri_ros2_stack/repos-fri-1.16.yaml b/lbr_fri_ros2_stack/repos-fri-1.16.yaml index cc20cdd6..84df1702 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: humble diff --git a/lbr_fri_ros2_stack/repos-fri-2.5.yaml b/lbr_fri_ros2_stack/repos-fri-2.5.yaml index 1280b6f6..637e81a0 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: humble diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml index e04b513e..90dcd423 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: humble diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 50d95fb3..a4960eae 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 `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 7e74b5b3..d3fe9d84 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`. +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. `lbr_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. From cf4030ef72cf6b4a906d7018faf8b1b3822f8ff5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 17:05:01 +0000 Subject: [PATCH 38/42] Revert "ign -> gz" This reverts commit a5071d9ffd393cafc0aa269a45b0e7fc48578fa8. --- lbr_bringup/package.xml | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 4 ++-- lbr_description/package.xml | 2 +- lbr_description/ros2_control/lbr_system_interface.xacro | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 4cf72c0e..3029627a 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -11,7 +11,7 @@ ament_cmake_python controller_manager - gz_ros2_control + ign_ros2_control joint_state_broadcaster joint_trajectory_controller lbr_description diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 3c9fa1ab..c9539f90 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -4,8 +4,8 @@ - + $(find lbr_description)/ros2_control/lbr_controllers.yaml /${robot_name} diff --git a/lbr_description/package.xml b/lbr_description/package.xml index df572b03..a3b807df 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -10,7 +10,7 @@ ament_cmake ament_cmake_pytest - gz_ros2_control + ign_ros2_control joint_state_publisher_gui robot_state_publisher rviz2 diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index f4a4452f..eb9c1480 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -15,7 +15,7 @@ - gz_ros2_control/GazeboSimSystem + ign_ros2_control/IgnitionSystem From 7da8f2a630f9ab1db4672128b1a1601a1f0f2d6c Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 18:01:30 +0000 Subject: [PATCH 39/42] addressed humble compatability (https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/225#discussion_r1850702805) --- .github/workflows/backport.yaml | 34 ---------- lbr_bringup/launch/hardware.launch.py | 4 +- lbr_bringup/launch/mock.launch.py | 4 +- lbr_bringup/lbr_bringup/ros2_control.py | 4 ++ .../controllers/admittance_controller.hpp | 3 + .../controllers/twist_controller.hpp | 3 + .../src/controllers/admittance_controller.cpp | 63 ++++++++++++++----- .../lbr_joint_position_command_controller.cpp | 4 +- .../src/controllers/lbr_state_broadcaster.cpp | 4 +- .../lbr_torque_command_controller.cpp | 4 +- .../lbr_wrench_command_controller.cpp | 4 +- .../src/controllers/twist_controller.cpp | 53 ++++++++++++---- 12 files changed, 114 insertions(+), 70 deletions(-) delete mode 100644 .github/workflows/backport.yaml diff --git a/.github/workflows/backport.yaml b/.github/workflows/backport.yaml deleted file mode 100644 index daacbb31..00000000 --- a/.github/workflows/backport.yaml +++ /dev/null @@ -1,34 +0,0 @@ -name: Backport merged pull request -on: - issue_comment: - types: [created] -permissions: - contents: write # so it can comment - pull-requests: write # so it can create pull requests -jobs: - backport: - name: Backport pull request - runs-on: ubuntu-latest - - # Only run when pull request is merged - # or when a comment starting with `/backport` is created by someone other than the - # https://github.com/backport-action bot user (user id: 97796249). Note that if you use your - # own PAT as `github_token`, that you should replace this id with yours. - if: > - ( - github.event_name == 'pull_request_target' && - github.event.pull_request.merged - ) || ( - github.event_name == 'issue_comment' && - github.event.issue.pull_request && - github.event.comment.user.id != 97796249 && - startsWith(github.event.comment.body, '/backport') - ) - steps: - - uses: actions/checkout@v4 - - name: Create backport pull requests - uses: korthout/backport-action@v3 - with: - label_pattern: "" - target_branches: humble - merge_commits: skip diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index d6ec6b9b..cffdf59b 100644 --- a/lbr_bringup/launch/hardware.launch.py +++ b/lbr_bringup/launch/hardware.launch.py @@ -39,7 +39,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(robot_state_publisher) # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ros2_control_node = LBRROS2ControlMixin.node_ros2_control( + use_sim_time=False, robot_description=robot_description + ) ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py index a87be2d4..fc7c24f7 100644 --- a/lbr_bringup/launch/mock.launch.py +++ b/lbr_bringup/launch/mock.launch.py @@ -37,7 +37,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(robot_state_publisher) # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ros2_control_node = LBRROS2ControlMixin.node_ros2_control( + use_sim_time=False, robot_description=robot_description + ) ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index f98ef127..00f3a30e 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -72,6 +72,9 @@ def node_ros2_control( use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( "use_sim_time", default="false" ), + robot_description: Optional[ + Dict[str, str] + ] = {}, # required for certain ROS 2 controllers in Humble **kwargs, ) -> Node: return Node( @@ -91,6 +94,7 @@ def node_ros2_control( ), ] ), + robot_description, ], namespace=robot_name, remappings=[ 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 17143903..7488d6b2 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 @@ -52,6 +52,9 @@ class AdmittanceController : public controller_interface::ControllerInterface { void zero_all_values_(); void log_info_() const; + // robot description + std::string robot_description_; + // admittance bool initialized_ = false; std::unique_ptr admittance_impl_ptr_; 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 f0a1fea4..6f202520 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 @@ -54,6 +54,9 @@ class TwistController : public controller_interface::ControllerInterface { void configure_inv_jac_ctrl_impl_(); void log_info_() const; + // robot description + std::string robot_description_; + // some safety checks std::atomic updates_since_last_command_ = 0; double timeout_ = 0.2; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index bbe950b6..2b6c0b45 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -38,22 +38,51 @@ AdmittanceController::state_interface_configuration() const { controller_interface::CallbackReturn AdmittanceController::on_init() { try { - this->get_node()->declare_parameter("robot_name", "lbr"); - this->get_node()->declare_parameter("admittance.mass", - std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); - this->get_node()->declare_parameter("admittance.damping", - std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); - this->get_node()->declare_parameter("admittance.stiffness", - std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); - this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); - this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); - this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); - this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); - this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); - this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", - 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)); + if (!this->get_node()->has_parameter("robot_description")) { + this->get_node()->declare_parameter("robot_description", ""); + } + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + if (!this->get_node()->has_parameter("admittance.mass")) { + this->get_node()->declare_parameter("admittance.mass", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); + } + if (!this->get_node()->has_parameter("admittance.damping")) { + this->get_node()->declare_parameter("admittance.damping", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("admittance.stiffness")) { + this->get_node()->declare_parameter("admittance.stiffness", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) { + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + robot_description_ = this->get_node()->get_parameter("robot_description").as_string(); + if (robot_description_.empty()) { + throw std::runtime_error("No robot description provided"); + } configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); @@ -285,7 +314,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; } inv_jac_ctrl_impl_ptr_ = std::make_unique( - this->get_robot_description(), + robot_description_, lbr_fri_ros2::InvJacCtrlParameters{ this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), 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 232acf93..5556159f 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 @@ -29,7 +29,9 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init( [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) { rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg); }); - this->get_node()->declare_parameter("robot_name", "lbr"); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index 765bdd5b..cafd0a63 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -21,7 +21,9 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { rt_state_publisher_ptr_ = std::make_shared>( state_publisher_ptr_); - this->get_node()->declare_parameter("robot_name", "lbr"); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), 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 b56bd062..62d027e4 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -28,7 +28,9 @@ controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { "command/torque", 1, [this](const lbr_fri_idl::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); - this->get_node()->declare_parameter("robot_name", "lbr"); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), 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 37a278d9..87e669e4 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -33,7 +33,9 @@ controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { "command/wrench", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); }); - this->get_node()->declare_parameter("robot_name", "lbr"); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index d851f165..379e89f6 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -34,18 +34,45 @@ controller_interface::CallbackReturn TwistController::on_init() { rt_twist_ptr_.writeFromNonRT(msg); updates_since_last_command_ = 0; }); - this->get_node()->declare_parameter("robot_name", "lbr"); - this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); - this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); - this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true); - this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); - this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); - this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); - this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", - 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("timeout", 0.2); + if (!this->get_node()->has_parameter("robot_description")) { + this->get_node()->declare_parameter("robot_description", ""); + } + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.twist_in_tip_frame")) { + this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) { + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("timeout")) { + this->get_node()->declare_parameter("timeout", 0.2); + } + robot_description_ = this->get_node()->get_parameter("robot_description").as_string(); + if (robot_description_.empty()) { + throw std::runtime_error("No robot description provided"); + } configure_joint_names_(); configure_inv_jac_ctrl_impl_(); log_info_(); @@ -200,7 +227,7 @@ void TwistController::configure_inv_jac_ctrl_impl_() { this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; } inv_jac_ctrl_impl_ptr_ = std::make_unique( - this->get_robot_description(), + robot_description_, lbr_fri_ros2::InvJacCtrlParameters{ this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), From deb2b556081bd73189f2064cd4cbc7d995607e27 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 18:37:36 +0000 Subject: [PATCH 40/42] revert moveit configs for humble --- lbr_bringup/lbr_bringup/moveit.py | 2 +- lbr_bringup/package.xml | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/lbr_bringup/lbr_bringup/moveit.py b/lbr_bringup/lbr_bringup/moveit.py index 5cc120ae..05685a3f 100644 --- a/lbr_bringup/lbr_bringup/moveit.py +++ b/lbr_bringup/lbr_bringup/moveit.py @@ -69,7 +69,7 @@ def moveit_configs_builder( ) .planning_pipelines( default_planning_pipeline="ompl", - pipelines=["chomp", "pilz_industrial_motion_planner", "stomp", "ompl"], + pipelines=["ompl"], ) ) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 3029627a..01c7e5f5 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -19,7 +19,6 @@ lbr_ros2_control moveit_planners_chomp moveit_planners_ompl - moveit_planners_stomp moveit_ros_move_group moveit_servo rclpy From 7afb59fce4ce4f205c0255726cd1de34ae722176 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 18:49:10 +0000 Subject: [PATCH 41/42] release notes and version bump 2.1.2 -> 2.2.0 --- CHANGELOG.rst | 20 ++++++++++++++++++++ CITATION.cff | 4 ++-- lbr_bringup/package.xml | 2 +- lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/setup.py | 2 +- lbr_demos/lbr_demos_cpp/package.xml | 2 +- lbr_demos/lbr_demos_py/package.xml | 2 +- lbr_demos/lbr_demos_py/setup.py | 2 +- lbr_demos/lbr_moveit/package.xml | 2 +- lbr_demos/lbr_moveit/setup.py | 2 +- lbr_demos/lbr_moveit_cpp/package.xml | 2 +- lbr_description/package.xml | 2 +- lbr_fri_ros2/package.xml | 2 +- lbr_fri_ros2_stack/package.xml | 2 +- lbr_ros2_control/package.xml | 2 +- 16 files changed, 36 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f3cc4e47..b14ac3e3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,26 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.2.0 (2024-11-20) +-------------------------- +This release backports new ``rolling`` features to ``humble``. Following has changed: + + * Related PRs: + + * https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/213 and https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/214 + + * Joints and links are now prefixed with ``lbr_`` (i.e. the robot name) + * Robot state publisher has no ``lbr/`` prefix anymore + * async + deactivateable FT estimation + * Issue with setting real-time priority fixed + * Modifiable source for ``lbr_system_config.yaml`` in launch files + + * https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/220 + + * PID on joint position commands replaced by simpler exponential filter (please test robot in T1 mode as this will affect your control) + * Introduction of twist and admittance controllers + * Configurations from ``lbr_ros2_control`` now in ``lbr_description`` (for stand alone URDF use) + Humble v2.1.2 (2024-10-18) -------------------------- * Adds MoveIt Servo demo, related to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/50 and https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/211 diff --git a/CITATION.cff b/CITATION.cff index d97faf1a..35882061 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.1.2 +version: 2.2.0 doi: 10.21105/joss.06138 -date-released: 2024-10-18 +date-released: 2024-11-20 diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 01c7e5f5..9639935a 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 2.1.2 + 2.2.0 LBR launch files. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index a1bcf8b9..8f955b23 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.1.2 + 2.2.0 Advanced C++ demos for the lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index c485e4ac..f7098ccc 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.1.2 + 2.2.0 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 c037589c..e49a6d12 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.1.2", + version="2.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 6ed61d1a..7376b480 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.1.2 + 2.2.0 C++ demos for lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 3d0ca085..d0e55a3a 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.1.2 + 2.2.0 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 3fe2a962..b16641b8 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.1.2", + version="2.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/package.xml index f2116fe8..44f3cb60 100644 --- a/lbr_demos/lbr_moveit/package.xml +++ b/lbr_demos/lbr_moveit/package.xml @@ -2,7 +2,7 @@ lbr_moveit - 2.1.2 + 2.2.0 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 a4d342ce..65adda1c 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.1.2", + version="2.2.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index 85d80d66..245ca34e 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.1.2 + 2.2.0 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_description/package.xml b/lbr_description/package.xml index a3b807df..4e14f30b 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 2.1.2 + 2.2.0 KUKA LBR description files mhubii Apache-2.0 diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index a13fe491..879bbbea 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 2.1.2 + 2.2.0 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_stack/package.xml b/lbr_fri_ros2_stack/package.xml index bb361f9b..3b5099c2 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 2.1.2 + 2.2.0 ROS 2 stack for KUKA LBRs. mhubii Apache-2.0 diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 932c667c..d7cfe74f 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control - 2.1.2 + 2.2.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii Apache-2.0 From 8639214e19dddf90d7934bf580c56becd416c52a Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 19:49:09 +0000 Subject: [PATCH 42/42] fixed increment --- .../lbr_joint_position_command_controller.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) 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 5556159f..d78d825b 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 @@ -49,10 +49,13 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_joint_position_command || !(*lbr_joint_position_command)) { return controller_interface::return_type::OK; } - std::for_each(command_interfaces_.begin(), command_interfaces_.end(), - [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { - command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]); - }); + std::for_each( + command_interfaces_.begin(), command_interfaces_.end(), + [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { + command_interface.set_value( + (*lbr_joint_position_command) + ->joint_position[idx++]); // important to post-increment idx, don't use ++idx + }); return controller_interface::return_type::OK; }