diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 27dd8bae..5266e38f 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -17,7 +17,7 @@ jobs: matrix: os: [ubuntu-latest] steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-ci@v0.2 with: package-name: lbr_fri_ros2_stack diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 00000000..1bb1a149 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,88 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package LBR FRI ROS 2 Stack +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v1.4.0 (2023-12-08) +-------------------------- +* The general goal of this release is a tighter ``ros2_control`` integration. The ``lbr_bringup`` + will serve as single entry point in the future. For now, ``app_component`` and ``app.launch.py`` are kept +* Changes to ``lbr_fri_ros2``: + + * Removes logging / parameter interfaces from ``lbr_fri_ros2`` (so ``lbr_ros2_control`` serves as single interaction point) + * Updates legacy ``app_component`` in ``lbr_fri_ros2`` for changes. To be depracted in the future + * Adds force-torque estimator to ``lbr_fri_ros2`` +* Changes to ``lbr_ros2_control``: + + * Removes now redundant node from ``lbr_ros2_control`` + * Adds forward position and forward torque controllers to ``lbr_ros2_control`` + * Removes estimated force-torque broadcaster from ``lbr_ros2_control`` in favor of ``ros2_control`` default implementation + + * Force-torque now available under ``/lbr/force_torque_broadcaster/wrench`` + * Namespace issues since ``lbr_controllers.yaml`` includes namespace in ``frame_id`` parameter + * Adds ``lbr_fri_ros2`` force-torque estimator to ``lbr_ros2_control`` as sensor + * Adds configurations to ``lbr_system_interface.xacro`` + * Simplifies ``lbr_ros2_control`` class names +* ``/lbr/command/position`` topic now under ``/lbr/command/joint_position`` +* Adds this changelog with release notes +* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/142 + +Humble v1.3.1 (2023-11-21) +-------------------------- +* v1.3.0 Gazebo namespace fixes in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/123 +* Fix iiwa ee link in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/126 +* Humble v.1.3.1 in https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/130 +* Full log: https://github.com/lbr-stack/lbr_fri_ros2_stack/compare/humble-v1.3.0-beta...humble-v1.3.1 + +Humble v1.3.0 beta (2023-10-03) +------------------------------- +* Namespaced robot_description and joint_states +* De-coupled commands, user will interact through LBRPositionCommand, LBRTorqueCommand, LBRWrenchCommand +* Multi-robot support +* New command / state interfaces in lbr_fri_ros2 +* Topic free ros2_control support through command / state interfaces in lbr_fri_ros2 +* Intraprocess cpp admittance demo +* New app component based on command / state interfaces in lbr_fri_ros2 +* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/121 + +Humble v1.2.5 (2023-09-08) +-------------------------- +* Updated visualization (STL -> DAE files with materials, might occur dark in Gazebo, caused by lack of light) +* Fixes joint bug in Gazebo +* Improved logging in command guard + +Humble v1.2.4 (2023-08-09) +-------------------------- +* Remove robot name from configs and use frame_prefix from robot state publisher instead +* Removed robot name from joint names, e.g. lbr_A1 -> A1 +* Added PID for asynchronous control rate +* Simplified class names, e.g. LBRApp -> App +* Add utils.hpp for PID and exponential filter + +Humble v1.2.3 (2023-08-07) +-------------------------- +* Utilizes FRI through vendor package for common fri source in https://github.com/lbr-stack/ +* Addresses some of https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/85 +* Give command guard only logger interface +* Fix open loop bug +* Adds real-time priority via rt_prio parameter + +Humble v1.2.2 (2023-08-05) +-------------------------- +* Adds base frame parameter to URDF and launch +* Adds an open loop option to control the robot, which works extremely well +* Updates logo in readme +* Updates joint names to KUKA convention, i.e. A1,... + +Humble v1.2.1 (2023-08-04) +-------------------------- +* Stack's new home at: https://github.com/lbr-stack + +Humble v1.2.0 (2023-08-03) +-------------------------- +* Re-introduces MoveIt, refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/52 +* Moves demo prefix to front for improved package overview +* Single node for hardware interface +* Static executors where possible +* Adds plenty documentation +* Introduce /lbr, i.e. robot name, namespace to LBRClient for better multi-robot support. Commands / states now e.g. published to /lbr/command / /lbr/state +* Hardware interface exact limits (stand-alone use has safety-limits) +* Gives command guard a node handle diff --git a/CITATION.cff b/CITATION.cff index 2d2fbd47..e02fa4d5 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: 1.3.1 +version: 1.4.0 doi: 10.48550/arXiv.2311.12709 -date-released: 2023-11-22 +date-released: 2023-12-08 diff --git a/LICENSE b/LICENSE index e212c0b0..ba78d0d8 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2021 Martin Huber +Copyright (c) 2023 Martin Huber Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index 186d2e86..87a3bd4d 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -8,8 +8,9 @@ Panels: - /Status1 - /RobotModel1 - /RobotModel1/Description Topic1 - Splitter Ratio: 0.5 - Tree Height: 724 + - /Wrench1 + Splitter Ratio: 0.40760868787765503 + Tree Height: 717 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -123,6 +124,25 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.20000000298023224 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 0.10000000149011612 + Force Color: 0; 85; 255 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lbr/force_torque_broadcaster/wrench + Torque Arrow Scale: 0.30000001192092896 + Torque Color: 170; 85; 255 + Value: true Enabled: true Global Options: Background Color: 21; 21; 26 @@ -195,7 +215,7 @@ Window Geometry: Height: 1021 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015b0000035ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c40000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002120000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000035b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d00650100000000000004500000000000000000000005220000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 7099ea30..ff1b15f5 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -57,7 +57,7 @@ and select: Make sure that the ``update_rate`` in `lbr_controllers.yaml `_ is greater or equal ``100`` (``FRI send period``). -For using other ``FRI send period``, also change the ``sample_time`` in the `lbr.ros2_control.xacro `_ (automated in the future). +For using other ``FRI send period``, also change the ``sample_time`` in the `lbr_system_interface.xacro `_ (automated in the future). Standalone Launch ----------------- diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index b64d86b4..09e286a4 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -29,12 +29,12 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" ) + force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="force_torque_broadcaster" + ) lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - lbr_estimated_ft_broadcast = LBRROS2ControlMixin.node_controller_spawner( - controller="lbr_estimated_ft_broadcaster" - ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -44,8 +44,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: target_action=ros2_control_node, on_start=[ joint_state_broadcaster, + force_torque_broadcaster, lbr_state_broadcaster, - lbr_estimated_ft_broadcast, controller, ], ) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 5aa5f8b7..284fb1e8 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 1.3.1 + 1.4.0 LBR launch files. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt index be18d3ab..c92089b4 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt @@ -11,8 +11,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) find_package(kdl_parser REQUIRED) @@ -34,7 +32,6 @@ target_include_directories(admittance_control_component ament_target_dependencies( admittance_control_component - Eigen3 kdl_parser lbr_fri_ros2 lbr_fri_msgs diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml index 22596062..a3a275d9 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -2,17 +2,15 @@ lbr_demos_fri_ros2_advanced_cpp - 1.3.1 + 1.4.0 Advanced C++ demos for the lbr_fri_ros2. mhubii MIT ament_cmake - eigen3_cmake_module lbr_description - eigen fri_vendor kdl_parser lbr_fri_ros2 diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp index c98e2345..9dd60a7f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_control_node.cpp @@ -8,7 +8,6 @@ #include "lbr_fri_ros2/app.hpp" #include "admittance_controller.hpp" -#include "damped_least_squares.hpp" namespace lbr_fri_ros2 { class AdmittanceControlNode : public rclcpp::Node { @@ -31,7 +30,7 @@ class AdmittanceControlNode : public rclcpp::Node { this->get_parameter("dx_gains").as_double_array()); lbr_position_command_pub_ = - create_publisher("/lbr/command/position", 1); + create_publisher("/lbr/command/joint_position", 1); lbr_state_sub_ = create_subscription( "/lbr/state", 1, std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp index f14034cb..7d8a8c0f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp @@ -14,8 +14,7 @@ #include "lbr_fri_msgs/msg/lbr_position_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" - -#include "damped_least_squares.hpp" +#include "lbr_fri_ros2/pinv.hpp" namespace lbr_fri_ros2 { class AdmittanceController { @@ -51,7 +50,7 @@ class AdmittanceController { jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = damped_least_squares(jacobian_.data); + jacobian_inv_ = pinv(jacobian_.data); f_ext_ = jacobian_inv_.transpose() * tau_ext_; for (int i = 0; i < 6; i++) { diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp deleted file mode 100644 index c983f7cf..00000000 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ -#define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ - -#include -#include - -template -Eigen::Matrix -damped_least_squares(const MatT &mat, typename MatT::Scalar lambda = - typename MatT::Scalar{2e-1}) // choose appropriately -{ - typedef typename MatT::Scalar Scalar; - auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - const auto &singularValues = svd.singularValues(); - Eigen::Matrix dampedSingularValuesInv( - mat.cols(), mat.rows()); - dampedSingularValuesInv.setZero(); - for (unsigned int i = 0; i < singularValues.size(); ++i) { - dampedSingularValuesInv(i, i) = - singularValues(i) / (singularValues(i) * singularValues(i) + lambda * lambda); - } - return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); -} -#endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py index f1c10408..ef99295f 100755 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_control_node.py @@ -30,7 +30,9 @@ def __init__(self, node_name="admittance_control_node") -> None: LBRState, "/lbr/state", self.on_lbr_state_, 1 ) self.lbr_position_command_pub_ = self.create_publisher( - LBRPositionCommand, "/lbr/command/position", 1 + LBRPositionCommand, + "/lbr/command/joint_position", + 1, ) def on_lbr_state_(self, lbr_state: LBRState) -> None: diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml index 5ab6a0c4..dce7579e 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_advanced_python - 1.3.1 + 1.4.0 Advanced Python demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py index 9d50960a..8ba5bb9f 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.3.1", + version="1.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml index be003933..1088cc46 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_cpp - 1.3.1 + 1.4.0 C++ demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp index 61bc1865..dd2234e3 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp @@ -18,9 +18,9 @@ class JointSineOverlayNode : public rclcpp::Node { public: JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { - // create publisher to /lbr/command/position - lbr_position_command_pub_ = - this->create_publisher("/lbr/command/position", 1); + // create publisher to /lbr/command/joint_position + lbr_position_command_pub_ = this->create_publisher( + "/lbr/command/joint_position", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py index e74cb44b..3dc65d51 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/joint_sine_overlay_node.py @@ -16,9 +16,11 @@ def __init__(self, node_name: str) -> None: self.phase_ = 0.0 self.lbr_position_command_ = LBRPositionCommand() - # create publisher to /lbr/command/position + # create publisher to /lbr/command/joint_position self.lbr_position_command_pub_ = self.create_publisher( - LBRPositionCommand, "/lbr/command/position", 1 + LBRPositionCommand, + "/lbr/command/joint_position", + 1, ) # create subscription to /lbr_state diff --git a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py index dbf992bd..ae46d7c9 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/lbr_demos_fri_ros2_python/torque_sine_overlay_node.py @@ -4,7 +4,7 @@ from rclpy.node import Node # import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRTorqueCommand, LBRState +from lbr_fri_msgs.msg import LBRState, LBRTorqueCommand class TorqueSineOverlayNode(Node): diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_python/package.xml index 6b972b2f..450e29a7 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_python - 1.3.1 + 1.4.0 Python demos for the lbr_fri_ros2. mhubii MIT diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_python/setup.py index b7483cbc..ebe826cb 100644 --- a/lbr_demos/lbr_demos_fri_ros2_python/setup.py +++ b/lbr_demos/lbr_demos_fri_ros2_python/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="1.3.1", + version="1.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml index add6e9d1..dd7f5a6e 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_ros2_control_cpp - 1.3.1 + 1.4.0 C++ demos for the LBR ros2_control integration. mhubii MIT diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp index 444d5a47..fee75cd6 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp +++ b/lbr_demos/lbr_demos_ros2_control_cpp/src/joint_trajectory_executioner_node.cpp @@ -16,7 +16,7 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { joint_trajectory_action_client_ = rclcpp_action::create_client( - this, "/lbr/position_trajectory_controller/follow_joint_trajectory"); + this, "/lbr/joint_trajectory_controller/follow_joint_trajectory"); while (!joint_trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_INFO(this->get_logger(), "Waiting for action server to become available..."); diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py index bccef79f..203ace45 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py +++ b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/joint_trajectory_executioner_node.py @@ -14,7 +14,7 @@ def __init__( self.joint_trajectory_action_client_ = ActionClient( node=self, action_type=FollowJointTrajectory, - action_name="/lbr/position_trajectory_controller/follow_joint_trajectory", + action_name="/lbr/joint_trajectory_controller/follow_joint_trajectory", ) while not self.joint_trajectory_action_client_.wait_for_server(1): self.get_logger().info("Waiting for action server to become available...") diff --git a/lbr_demos/lbr_demos_ros2_control_python/package.xml b/lbr_demos/lbr_demos_ros2_control_python/package.xml index d5b3167a..37ea5649 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/package.xml +++ b/lbr_demos/lbr_demos_ros2_control_python/package.xml @@ -2,7 +2,7 @@ lbr_demos_ros2_control_python - 1.3.1 + 1.4.0 Python demos for the LBR ros2_control integration. mhubii MIT diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.py b/lbr_demos/lbr_demos_ros2_control_python/setup.py index 68ea2799..2180e5b7 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/setup.py +++ b/lbr_demos/lbr_demos_ros2_control_python/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="1.3.1", + version="1.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt index e0e2319d..664aab8e 100644 --- a/lbr_description/CMakeLists.txt +++ b/lbr_description/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(ament_cmake_python REQUIRED) # install install( - DIRECTORY config gazebo launch meshes ros2_control urdf + DIRECTORY config gazebo launch meshes urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index 813154c7..11f66a0a 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -10,94 +10,36 @@ - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - + + + + 0.2 + 0.2 + + + + + + true + true + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_description/package.xml b/lbr_description/package.xml index 7bfff445..5e4e5bbe 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 1.3.1 + 1.4.0 KUKA LBR description files mhubii MIT diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro deleted file mode 100644 index 184c2de8..00000000 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ /dev/null @@ -1,192 +0,0 @@ - - - - - - - - - - gazebo_ros2_control/GazeboSystem - - - - - lbr_ros2_control::LBRSystemInterface - ${port_id} - ${remote_host} - ${rt_prio} - ${open_loop} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -${joint_0_position_limit} - ${joint_0_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_1_position_limit} - ${joint_1_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_2_position_limit} - ${joint_2_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_3_position_limit} - ${joint_3_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_4_position_limit} - ${joint_4_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_5_position_limit} - ${joint_5_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_6_position_limit} - ${joint_6_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - \ No newline at end of file diff --git a/lbr_description/test/lbr_model_specifications.py b/lbr_description/test/lbr_model_specifications.py index e15cdc6d..37d3668d 100644 --- a/lbr_description/test/lbr_model_specifications.py +++ b/lbr_description/test/lbr_model_specifications.py @@ -1,6 +1,8 @@ +from dataclasses import dataclass from typing import Dict, List, Type +@dataclass class JointLimit: min_position: float max_position: float @@ -17,6 +19,7 @@ def __init__( self.max_velocity = max_velocity +@dataclass class LBRSpecification: dof: int name: str @@ -36,18 +39,6 @@ def __init__( self.joint_limits = joint_limits -# reference URDF joint names to KUKA joint names -URDF_TO_KUKA_JOINT_NAME_DICT: Dict[str, str] = { - "joint_0": "A1", - "joint_1": "A2", - "joint_2": "A3", - "joint_3": "A4", - "joint_4": "A5", - "joint_5": "A6", - "joint_6": "A7", -} - - # specifications as extracted from https://xpert.kuka.com/ LBR_SPECIFICATIONS_DICT: Dict[str, Type[LBRSpecification]] = { "AR7606": LBRSpecification( diff --git a/lbr_description/test/test_urdf.py b/lbr_description/test/test_urdf.py index bb72ab5a..e36372c9 100644 --- a/lbr_description/test/test_urdf.py +++ b/lbr_description/test/test_urdf.py @@ -8,11 +8,7 @@ from ament_index_python import get_package_share_directory from urdf_parser_py.urdf import URDF -from .lbr_model_specifications import ( - LBR_SPECIFICATIONS_DICT, - URDF_TO_KUKA_JOINT_NAME_DICT, - LBRSpecification, -) +from .lbr_model_specifications import LBR_SPECIFICATIONS_DICT, LBRSpecification @pytest.fixture @@ -71,25 +67,22 @@ def test_position_limits( for joint in urdf.joints: if joint.type == "revolute": - urdf_joint_name = "_".join(joint.name.split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - urdf_min_position = joint.limit.lower kuka_min_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].min_position + lbr_specification.joint_limits[joint.name].min_position ) if not math.isclose(urdf_min_position, kuka_min_position, abs_tol=abs_tol): raise ValueError( - f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and joint {joint.name}." ) urdf_max_position = joint.limit.upper kuka_max_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_position + lbr_specification.joint_limits[joint.name].max_position ) if not math.isclose(urdf_max_position, kuka_max_position, abs_tol=abs_tol): raise ValueError( - f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and joint {joint.name}." ) @@ -102,16 +95,13 @@ def test_velocity_limits( for joint in urdf.joints: if joint.type == "revolute": - urdf_joint_name = "_".join(joint.name.split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - urdf_max_velocity = joint.limit.velocity kuka_max_velcoity = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_velocity + lbr_specification.joint_limits[joint.name].max_velocity ) if not math.isclose(urdf_max_velocity, kuka_max_velcoity, abs_tol=abs_tol): raise ValueError( - f"Expected minimum joint position {kuka_max_velcoity} rad/s, found {urdf_max_velocity} rad/s for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_max_velcoity} rad/s, found {urdf_max_velocity} rad/s for model {lbr_specification.name} and joint {joint.name}." ) @@ -121,34 +111,35 @@ def test_position_limits_ros2_control( ) -> None: xml, lbr_specification = setup_xml_and_reference xml = ET.ElementTree(ET.fromstring(xml)) - for joint in xml.find("ros2_control").iter("joint"): - urdf_joint_name = "_".join(joint.get("name").split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - - for command_interface in joint.iter("command_interface"): + for joint_interface in xml.find("ros2_control").iter("joint_interface"): + for command_interface in joint_interface.iter("command_interface"): if command_interface.get("name") == "position": for param in command_interface.iter("param"): if param.get("name") == "min": urdf_min_position = float(param.text) kuka_min_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].min_position + lbr_specification.joint_limits[ + joint_interface.name + ].min_position ) if not math.isclose( urdf_min_position, kuka_min_position, abs_tol=abs_tol ): raise ValueError( - f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and position command interface joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and position command interface joint {joint_interface.name}." ) elif param.get("name") == "max": urdf_max_position = float(param.text) kuka_max_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_position + lbr_specification.joint_limits[ + joint_interface.name + ].max_position ) if not math.isclose( urdf_max_position, kuka_max_position, abs_tol=abs_tol ): raise ValueError( - f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and position command interface joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and position command interface joint {joint_interface.name}." ) else: raise ValueError("Couldn't find name.") diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index 719ca2df..e568050d 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -6,25 +6,25 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + @@ -88,9 +88,9 @@ - + @@ -120,9 +120,9 @@ - + @@ -152,9 +152,9 @@ - + @@ -184,9 +184,9 @@ - + @@ -216,9 +216,9 @@ - + @@ -248,9 +248,9 @@ - + @@ -286,14 +286,21 @@ diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index db3e5645..21cdbd49 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -6,25 +6,25 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + @@ -88,9 +88,9 @@ - + @@ -120,9 +120,9 @@ - + @@ -152,9 +152,9 @@ - + @@ -184,9 +184,9 @@ - + @@ -216,9 +216,9 @@ - + @@ -249,9 +249,9 @@ - + @@ -287,14 +287,21 @@ diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 996d30d4..717dea39 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -6,25 +6,25 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + @@ -88,9 +88,9 @@ - + @@ -120,9 +120,9 @@ - + @@ -152,9 +152,9 @@ - + @@ -184,9 +184,9 @@ - + @@ -216,9 +216,9 @@ - + @@ -248,9 +248,9 @@ - + @@ -286,14 +286,21 @@ diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index adf0d22a..cba06813 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -6,25 +6,25 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + @@ -88,9 +88,9 @@ - + @@ -120,9 +120,9 @@ - + @@ -152,9 +152,9 @@ - + @@ -184,9 +184,9 @@ - + @@ -216,9 +216,9 @@ - + @@ -248,9 +248,9 @@ - + @@ -286,14 +286,21 @@ diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml index 462af45f..dabe201b 100644 --- a/lbr_fri_msgs/package.xml +++ b/lbr_fri_msgs/package.xml @@ -2,7 +2,7 @@ lbr_fri_msgs - 1.3.1 + 1.4.0 ROS 2 message for the Fast Robot Interface (FRI) specific states. mhubii MIT diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 6a837d5f..189794d6 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -13,9 +13,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(control_toolbox REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) +find_package(kdl_parser REQUIRED) find_package(lbr_fri_msgs REQUIRED) +find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(realtime_tools REQUIRED) @@ -25,11 +29,12 @@ find_package(urdf REQUIRED) add_library(lbr_fri_ros2 SHARED src/app.cpp - src/client.cpp + src/async_client.cpp src/command_guard.cpp src/command_interface.cpp + src/filters.cpp + src/ft_estimator.cpp src/state_interface.cpp - src/utils.cpp ) target_include_directories(lbr_fri_ros2 @@ -40,10 +45,12 @@ target_include_directories(lbr_fri_ros2 ament_target_dependencies(lbr_fri_ros2 control_toolbox + Eigen3 + kdl_parser lbr_fri_msgs + orocos_kdl_vendor rclcpp realtime_tools - urdf ) target_link_libraries(lbr_fri_ros2 @@ -53,12 +60,15 @@ 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 fri_vendor FRIClient + kdl_parser lbr_fri_msgs + orocos_kdl_vendor rclcpp realtime_tools - urdf ) install( diff --git a/lbr_fri_ros2/config/app.yaml b/lbr_fri_ros2/config/app.yaml index 89df7fcc..7b34cbc6 100644 --- a/lbr_fri_ros2/config/app.yaml +++ b/lbr_fri_ros2/config/app.yaml @@ -3,6 +3,13 @@ app: port_id: 30200 # valid in range [30200, 30209], usefull for multi-robot setup remote_host: null # if null, any IP is accepted rt_prio: 80 # real-time priority of the FRI thread + pid.p: 1.0 # proportional gain on joint position, which will be scaled by sample time + pid.i: 0.0 # integral gain on joint position, which will be scaled by sample time + pid.d: 0.0 # derivative gain on joint position, which will be scaled by sample time + pid.i_max: 0.0 # maximum integral term + pid.i_min: 0.0 # minimum integral term + pid.antiwindup: false # if true, anti-windup is enabled command_guard_variant: "safe_stop" # ["default", "safe_stop"] - default uses exact position limits, safe_stop uses safe zone - external_torque.cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the external torque in Hz - measured_torque.cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the measured torque in Hz + external_torque_cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the external torque in Hz + measured_torque_cutoff_frequency: 10.0 # cutoff frequency of the low-pass filter for the measured torque in Hz + open_loop: true # if true, the robot is controlled in open loop mode diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 3a6745a5..708fd43e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -3,21 +3,24 @@ #include #include -#include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "realtime_tools/thread_priority.hpp" #include "friClientApplication.h" #include "friUdpConnection.h" -#include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/async_client.hpp" namespace lbr_fri_ros2 { class App { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; + public: - App(const rclcpp::Node::SharedPtr node_ptr, const std::shared_ptr client_ptr); + App(const std::shared_ptr async_client_ptr); ~App(); bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); @@ -28,13 +31,10 @@ class App { protected: bool valid_port_(const int &port_id); - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_; - std::atomic_bool should_stop_, running_; std::thread run_thread_; - std::shared_ptr client_ptr_; + std::shared_ptr async_client_ptr_; std::unique_ptr connection_ptr_; std::unique_ptr app_ptr_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp similarity index 52% rename from lbr_fri_ros2/include/lbr_fri_ros2/client.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 82f413e7..8c3f2bfb 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -1,23 +1,32 @@ -#ifndef LBR_FRI_ROS2__CLIENT_HPP_ -#define LBR_FRI_ROS2__CLIENT_HPP_ +#ifndef LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ +#define LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ #include #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" #include "lbr_fri_ros2/command_interface.hpp" #include "lbr_fri_ros2/enum_maps.hpp" +#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/state_interface.hpp" namespace lbr_fri_ros2 { -class Client : public KUKA::FRI::LBRClient { +class AsyncClient : public KUKA::FRI::LBRClient { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; + public: - Client() = delete; - Client(const rclcpp::Node::SharedPtr node_ptr); + AsyncClient() = delete; + AsyncClient(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant, + const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, + const bool &open_loop = true); inline CommandInterface &get_command_interface() { return command_interface_; } inline StateInterface &get_state_interface() { return state_interface_; } @@ -29,13 +38,10 @@ class Client : public KUKA::FRI::LBRClient { void command() override; protected: - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_; - CommandInterface command_interface_; StateInterface state_interface_; bool open_loop_; }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__CLIENT_HPP_ +#endif // LBR_FRI_ROS2__ASYNC_CLIENT_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 97d95b71..7ebc0f2e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -1,167 +1,77 @@ -#ifndef LBR_FRI_ROS2__COMMAND_GUARDS_HPP_ -#define LBR_FRI_ROS2__COMMAND_GUARDS_HPP_ +#ifndef LBR_FRI_ROS2__COMMAND_GUARD_HPP_ +#define LBR_FRI_ROS2__COMMAND_GUARD_HPP_ #include #include #include #include -#include "rclcpp/rclcpp.hpp" -#include "urdf/model.h" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" +#include "friLBRState.h" #include "lbr_fri_msgs/msg/lbr_command.hpp" namespace lbr_fri_ros2 { -/** - * @brief CommandGuard checks desired commands for limits. - * - */ +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_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/ + jnt_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/ + jnt_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/ + jnt_array_t max_torque{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ +}; + class CommandGuard { protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; + // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; - using joint_array_t = idl_command_t::_joint_position_type; - using const_joint_array_t_ref = const joint_array_t &; + using jnt_array_t = idl_command_t::_joint_position_type; + using const_jnt_array_t_ref = const jnt_array_t &; // FRI types using fri_state_t = KUKA::FRI::LBRState; using const_fri_state_t_ref = const fri_state_t &; public: - /** - * @brief Command guard default constructor. Limits zero by default. - * - */ CommandGuard() = default; - - /** - * @brief Construct a new CommandGuard object. - * - * @param[in] logging_interface_ptr Shared node logger interface - * @param[in] parameters_interface_ptr Shared node parameter interface - */ - CommandGuard( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr); - - /** - * @brief Checks for command validity given the current state. - * - * @param[in] lbr_command The desired command - * @param[in] lbr_state The current state - * - * @return true if lbr_command is valid - * @return false if lbr_command is invalid - */ + CommandGuard(const CommandGuardParameters &command_guard_parameters); virtual bool is_valid_command(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const; + void log_info() const; + protected: - /** - * @brief Checks for nans. - * - * @param[in] begin Pointer to begin - * @param[in] end Pointer to end - * - * @return true if any nans between begin to end - * @return false if no nans between begin to end - */ - bool is_nan_(const double *begin, const double *end) const; - - /** - * @brief Checks for joint position limits. - * - * @param[in] lbr_command The desired command - * @param[in] lbr_state The current state - * - * @return true if lbr_command in position limits - * @return false if lbr_command outside position limits - */ virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref /*lbr_state*/) const; - - /** - * @brief Checks for joint velocity limits. - * - * @param[in] lbr_command The desired command - * @param[in] lbr_state The current state - * - * @return true if lbr_command in velocity limits - * @return false if lbr_command outside velocity limits - */ virtual bool command_in_velocity_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const; - - /** - * @brief Checks for joint torque limits. - * - * @param[in] lbr_command The desired command - * @param[in] lbr_state The current state - * - * @return true if lbr_command in torque limits - * @return false if lbr_command outside torque limits - */ virtual bool command_in_torque_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - logging_interface_ptr_; /**< Shared node logger interface.*/ - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr - parameters_interface_ptr_; /**< Shared node parameter interface.*/ - - joint_array_t min_position_; /**< Minimum joint position [rad].*/ - joint_array_t max_position_; /**< Maximum joint position [rad].*/ - joint_array_t max_velocity_; /**< Maximum joint velocity [rad/s].*/ - joint_array_t max_torque_; /**< Maximum joint torque [Nm].*/ + CommandGuardParameters parameters_; }; -/** - * @brief Adds early stopping to CommandGuard. - * - */ class SafeStopCommandGuard : public CommandGuard { public: - /** - * @brief Construct a new SafeStopCommandGuard object. - * - * @param[in] logging_interface_ptr Shared node logger interface - * @param[in] parameters_interface_ptr Shared node parameter interface - */ - SafeStopCommandGuard( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr) - : CommandGuard(logging_interface_ptr, parameters_interface_ptr){}; + SafeStopCommandGuard(const CommandGuardParameters &command_guard_parameters) + : CommandGuard(command_guard_parameters){}; protected: - /** - * @brief Checks for joint position limits and guarantees an early stop given the maximum - * velocity. - * - * @param[in] lbr_command The desired command - * @param[in] lbr_state The current state - * - * @return true if lbr_command in position limits - * @return false if lbr_command outside position limits - */ virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const override; }; -/** - * @brief Creates an CommandGuard object. - * - * @param[in] logging_interface_ptr Shared node logger interface - * @param[in] parameters_interface_ptr Shared node parameter interface - * @param[in] variant Which variant of CommandGuard to create - * - * @return std::unique_ptr Pointer to CommandGuard object - */ -std::unique_ptr command_guard_factory( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr, - const std::string &variant); +std::unique_ptr +command_guard_factory(const CommandGuardParameters &command_guard_parameters, + const std::string &variant = "default"); } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__COMMAND_GUARDS_HPP_ +#endif // LBR_FRI_ROS2__COMMAND_GUARD_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp index 44b5b6dd..22bb47c6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -1,21 +1,25 @@ #ifndef LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ #define LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ +#include #include #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/utils.hpp" +#include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { class CommandInterface { protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; + // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; using const_idl_command_t_ref = const idl_command_t &; @@ -28,7 +32,9 @@ class CommandInterface { public: CommandInterface() = delete; - CommandInterface(const rclcpp::Node::SharedPtr node_ptr); + CommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant = "default"); void get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state); void get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state); @@ -39,14 +45,13 @@ class CommandInterface { inline const_idl_command_t_ref get_command() const { return command_; } inline const_idl_command_t_ref get_command_target() const { return command_target_; } -protected: - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_; + void log_info() const; +protected: std::unique_ptr command_guard_; - JointPIDArrayROS joint_position_pid_; + PIDParameters pid_parameters_; + JointPIDArray joint_position_pid_; idl_command_t command_, command_target_; - bool pid_init_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp new file mode 100644 index 00000000..b9929479 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -0,0 +1,145 @@ +#ifndef LBR_FRI_ROS2__FILTERS_HPP_ +#define LBR_FRI_ROS2__FILTERS_HPP_ + +#include +#include +#include +#include + +#include "control_toolbox/filters.hpp" +#include "control_toolbox/pid_ros.hpp" + +#include "friLBRClient.h" + +#include "lbr_fri_msgs/msg/lbr_state.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. + * + */ + 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. + * + * @param[in] cutoff_frequency Frequency in Hz. + * @param[in] sample_time Sample time in seconds. + */ + ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + + /** + * @brief Compute the exponential smoothing using the control_toolbox + * https://github.com/ros-controls/control_toolbox. + * + * @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_); + }; + + /** + * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * + * @param[in] cutoff_frequency Frequency in Hz. + * @param[in] sample_time Sample time in seconds. + */ + void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + + /** + * @brief Get #sample_time_. + * + * @return const double& + */ + inline const double &get_sample_time() const { return sample_time_; }; + + /** + * @brief Get #alpha_. + * + * @return const double& + */ + 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]. + * + * @param[in] alpha Alpha parameter for smoothing. + * @return true if in [0, 1]. + * @return false if outside [0, 1]. + */ + 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.*/ +}; + +class JointExponentialFilterArray { + using value_array_t = std::array; + +public: + JointExponentialFilterArray() = default; + + void compute(const double *const current, value_array_t &previous); + void initialize(const double &cutoff_frequency, 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 value_array_t = std::array; + using pid_array_t = std::array; + +public: + JointPIDArray() = default; + + 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 initialize(const PIDParameters &pid_parameters, const double &dt); + inline const bool &is_initialized() const { return initialized_; }; + + void log_info() const; + +protected: + bool initialized_{false}; /**< True if initialized.*/ + pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp new file mode 100644 index 00000000..f71c6eb9 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -0,0 +1,70 @@ +#ifndef LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#define LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ + +#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_msgs/msg/lbr_state.hpp" +#include "lbr_fri_ros2/pinv.hpp" + +namespace lbr_fri_ros2 { +class FTEstimator { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; + using jnt_pos_array_t = lbr_fri_msgs::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_msgs::msg::LBRState::_external_torque_type; + 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_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", + 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 reset(); + +protected: + // 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_; + + // force estimation + KDL::Jacobian jacobian_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix tau_ext_; + Eigen::Matrix f_ext_; +}; +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp new file mode 100644 index 00000000..1616c27e --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp @@ -0,0 +1,29 @@ +#ifndef LBR_FRI_ROS2__PINV_HPP_ +#define LBR_FRI_ROS2__PINV_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/SVD" + +namespace lbr_fri_ros2 { +template +Eigen::Matrix +pinv(const MatT &mat, + typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately +{ + typedef typename MatT::Scalar Scalar; + auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); + const auto &singularValues = svd.singularValues(); + Eigen::Matrix dampedSingularValuesInv( + mat.cols(), mat.rows()); + dampedSingularValuesInv.setZero(); + std::for_each(singularValues.data(), singularValues.data() + singularValues.size(), + [&, i = 0](const Scalar &s) mutable { + dampedSingularValuesInv(i, i) = s / (s * s + lambda * lambda); + ++i; + }); + return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); +} +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__PINV_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp index 8caa0eeb..f230587f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -3,16 +3,24 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_ros2/utils.hpp" +#include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { +struct StateInterfaceParameters { + double external_torque_cutoff_frequency; /*Hz*/ + double measured_torque_cutoff_frequency; /*Hz*/ +}; + class StateInterface { protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; + // ROS IDL types using idl_state_t = lbr_fri_msgs::msg::LBRState; using const_idl_state_t_ref = const idl_state_t &; @@ -26,11 +34,9 @@ class StateInterface { public: StateInterface() = delete; - StateInterface( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr); + StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); - inline const_idl_state_t_ref &get_state() const { return state_; }; + 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); @@ -38,17 +44,15 @@ class StateInterface { inline void uninitialize() { state_initialized_ = false; } inline bool is_initialized() const { return state_initialized_; }; + void log_info() const; + protected: void init_filters_(); - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_; - std::atomic_bool state_initialized_; idl_state_t state_; - JointExponentialFilterArrayROS external_torque_filter_; - JointExponentialFilterArrayROS measured_torque_filter_; - bool filters_init_; + StateInterfaceParameters parameters_; + JointExponentialFilterArray external_torque_filter_, measured_torque_filter_; }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__STATE_INTERFACE_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp deleted file mode 100644 index e71fd236..00000000 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ /dev/null @@ -1,202 +0,0 @@ -#ifndef LBR_FRI_ROS2__UTILS_HPP_ -#define LBR_FRI_ROS2__UTILS_HPP_ - -#include -#include -#include -#include -#include - -#include "control_toolbox/filters.hpp" -#include "control_toolbox/pid_ros.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "friLBRClient.h" - -#include "lbr_fri_msgs/msg/lbr_command.hpp" -#include "lbr_fri_msgs/msg/lbr_state.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. - * - */ - 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. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - */ - ExponentialFilter(const double &cutoff_frequency, const double &sample_time); - - /** - * @brief Compute the exponential smoothing using the control_toolbox - * https://github.com/ros-controls/control_toolbox. - * - * @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_); - }; - - /** - * @brief Set the cutoff frequency object. Internally computes the new #alpha_. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - */ - void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); - - /** - * @brief Get #sample_time_. - * - * @return const double& - */ - inline const double &get_sample_time() const { return sample_time_; }; - - /** - * @brief Get #alpha_. - * - * @return const double& - */ - 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]. - * - * @param[in] alpha Alpha parameter for smoothing. - * @return true if in [0, 1]. - * @return false if outside [0, 1]. - */ - 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.*/ -}; - -class JointExponentialFilterArrayROS { - using value_array_t = std::array; - -public: - JointExponentialFilterArrayROS() = delete; - - /** - * @brief Construct a new JointExponentialFilterArrayROS object. - * - * @param[in] logging_interface Logging interface. - * @param[in] parameter_interface Parameter interface. - * @param[in] param_prefix Parameter prefix is e.g. used as: param_prefix + "." + - * "cut_off_frequency". - */ - JointExponentialFilterArrayROS( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, - const std::string ¶m_prefix = ""); - - /** - * @brief Compute the exponential smoothing for each joints using #exponential_filter_. - * - * @param[in] current The current joint values. - * @param[in, out] previous The previous smoothed joint values. Will be updated. - */ - void compute(const double *const current, value_array_t &previous); - void init(const double &cutoff_frequency, const double &sample_time); - inline const std::string ¶m_prefix() const { return param_prefix_; } - -protected: - const std::string cutoff_frequency_param_name_ = "cutoff_frequency"; /**< Parameter name.*/ - ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr - logging_interface_; /**< Logging interface.*/ - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr - parameter_interface_; /** Parameter interface.*/ - std::string param_prefix_; /** Parameter prefix is used as "param_prefix" + "." + "param_name".*/ - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr - parameter_callback_handle_; /**< Parameter callback handle to update parameters.*/ -}; - -class JointPIDArrayROS { - using value_array_t = std::array; - using name_array_t = std::array; - using pid_array_t = std::array; - -public: - JointPIDArrayROS() = delete; - - /** - * @brief Construct a new JointPIDArrayROS object. Used to control the LBRs joints. The parameters - * / topics are prefixed as prefix + names[i]. - * - * @param[in] node Shared node. - * @param[in] names Names of the joints. - * @param[in] prefix Prefix for the parameters. - */ - JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const name_array_t &names, - const std::string &prefix = ""); - - /** - * @brief Compute the PID update. - * - * @param[in] command_target The target joint command. - * @param[in] state The current joint state. - * @param[in] dt The time step. - * @param[out] command The returned joint command. - */ - void compute(const value_array_t &command_target, const value_array_t &state, - const rclcpp::Duration &dt, value_array_t &command); - - /** - * @brief Compute the PID update. - * - * @param[in] command_target The target joint command. - * @param[in] state The current joint state. - * @param[in] dt The time step. - * @param[out] command The returned joint command. - */ - void compute(const value_array_t &command_target, const double *state, const rclcpp::Duration &dt, - value_array_t &command); - - /** - * @brief Initialize the PID controllers. Sets all #pid_controllers_ to the same parameters, but - * can be overriden individually. - * - * @param[in] p The proportional gain. - * @param[in] i The integral gain. - * @param[in] d The derivative gain. - * @param[in] i_max The maximum integral value. - * @param[in] i_min The minimum integral value. - * @param[in] antiwindup Antiwindup enabled or disabled. - */ - void init(const double &p, const double &i, const double &d, const double &i_max, - const double &i_min, const bool &antiwindup); - -protected: - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ -}; -} // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__UTILS_HPP_ diff --git a/lbr_fri_ros2/launch/app.launch.py b/lbr_fri_ros2/launch/app.launch.py index cc9d39c5..73464504 100644 --- a/lbr_fri_ros2/launch/app.launch.py +++ b/lbr_fri_ros2/launch/app.launch.py @@ -12,6 +12,7 @@ def generate_launch_description() -> LaunchDescription: robot_description = LBRDescriptionMixin.param_robot_description(sim=False) ld.add_action(LBRFRIROS2Mixin.arg_open_loop()) ld.add_action(LBRFRIROS2Mixin.arg_rt_prio()) + ld.add_action(LBRFRIROS2Mixin.arg_pid_p()) ld.add_action(LBRFRIROS2Mixin.arg_port_id()) ld.add_action( LBRFRIROS2Mixin.node_app( @@ -19,6 +20,7 @@ def generate_launch_description() -> LaunchDescription: robot_description, LBRFRIROS2Mixin.param_open_loop(), LBRFRIROS2Mixin.param_rt_prio(), + LBRFRIROS2Mixin.param_pid_p(), LBRFRIROS2Mixin.param_port_id(), ] ) diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py index 9608e2ff..1f873a69 100644 --- a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py +++ b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py @@ -33,6 +33,54 @@ def arg_rt_prio() -> DeclareLaunchArgument: "\t'user - rtprio 99', where user is your username.", ) + @staticmethod + def arg_pid_p() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.p", + default_value="1.0", + description="Joint position PID controller proportional gain.", + ) + + @staticmethod + def arg_pid_i() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.i", + default_value="0.0", + description="Joint position PID controller integral gain.", + ) + + @staticmethod + def arg_pid_d() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.d", + default_value="1.0", + description="Joint position PID controller derivative gain.", + ) + + @staticmethod + def arg_pid_i_max() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.i_max", + default_value="0.0", + description="Joint position PID controller maximum integral value.", + ) + + @staticmethod + def arg_pid_i_min() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.i_min", + default_value="0.0", + description="Joint position PID controller minimum integral value.", + ) + + @staticmethod + def arg_pid_antiwindup() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="pid.antiwindup", + default_value="0.0", + description="Joint position PID controller antiwindup.", + ) + @staticmethod def arg_port_id() -> DeclareLaunchArgument: return DeclareLaunchArgument( @@ -58,6 +106,30 @@ def param_open_loop() -> Dict[str, LaunchConfiguration]: def param_rt_prio() -> Dict[str, LaunchConfiguration]: return {"rt_prio": LaunchConfiguration("rt_prio", default="80")} + @staticmethod + def param_pid_p() -> Dict[str, LaunchConfiguration]: + return {"pid.p": LaunchConfiguration("pid.p", default="1.0")} + + @staticmethod + def param_pid_i() -> Dict[str, LaunchConfiguration]: + return {"pid.i": LaunchConfiguration("pid.i", default="0.0")} + + @staticmethod + def param_pid_d() -> Dict[str, LaunchConfiguration]: + return {"pid.d": LaunchConfiguration("pid.d", default="0.0")} + + @staticmethod + def param_pid_i_max() -> Dict[str, LaunchConfiguration]: + return {"pid.i_max": LaunchConfiguration("pid.i_max", default="0.0")} + + @staticmethod + def param_pid_i_min() -> Dict[str, LaunchConfiguration]: + return {"pid.i_min": LaunchConfiguration("pid.i_min", default="0.0")} + + @staticmethod + def param_pid_antiwindup() -> Dict[str, LaunchConfiguration]: + return {"pid.antiwindup": LaunchConfiguration("pid.antiwindup", default="0.0")} + @staticmethod def param_port_id() -> Dict[str, LaunchConfiguration]: return {"port_id": LaunchConfiguration("port_id", default="30200")} diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index df1637bf..f4d1767b 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 1.3.1 + 1.4.0 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii @@ -10,10 +10,15 @@ ament_cmake ament_cmake_python + eigen3_cmake_module + + eigen control_toolbox fri_vendor + kdl_parser lbr_fri_msgs + orocos_kdl_vendor rclcpp rclcpp_components realtime_tools @@ -21,6 +26,9 @@ lbr_description + eigen3_cmake_module + eigen + ament_lint_auto ament_lint_common diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 6ba76826..e77f39ab 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -1,13 +1,12 @@ #include "lbr_fri_ros2/app.hpp" namespace lbr_fri_ros2 { -App::App(const rclcpp::Node::SharedPtr node_ptr, const std::shared_ptr client_ptr) - : logging_interface_ptr_(node_ptr->get_node_logging_interface()), - parameters_interface_ptr_(node_ptr->get_node_parameters_interface()), should_stop_(true), - running_(false), client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { - client_ptr_ = client_ptr; +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_ = async_client_ptr; connection_ptr_ = std::make_unique(); - app_ptr_ = std::make_unique(*connection_ptr_, *client_ptr_); + app_ptr_ = std::make_unique(*connection_ptr_, *async_client_ptr_); } App::~App() { @@ -17,99 +16,98 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Opening UDP socket with port_id %d.", port_id); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Opening UDP socket with port_id %d.", port_id); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "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(logging_interface_ptr_->get_logger(), "Failed to open socket."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to open socket."); return false; } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Socket opened successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket opened successfully."); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Closing UDP socket."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Closing UDP socket."); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Socket already closed."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed."); return true; } connection_ptr_->close(); - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Socket closed successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket closed successfully."); return true; } void App::run(int rt_prio) { - if (!client_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Client not configured."); + if (!async_client_ptr_) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); return; } if (!connection_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Connection not open."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not open."); return; } if (!app_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "App not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "App not configured."); return; } if (running_) { - RCLCPP_WARN(logging_interface_ptr_->get_logger(), "App already running."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "App already running."); return; } run_thread_ = std::thread([&]() { if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN(logging_interface_ptr_->get_logger(), + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Failed to set FIFO realtime scheduling policy."); } } else { - RCLCPP_WARN(logging_interface_ptr_->get_logger(), - "Realtime kernel recommended but not required."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required."); } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Starting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread."); should_stop_ = false; running_ = true; bool success = true; while (rclcpp::ok() && success && !should_stop_) { success = app_ptr_->step(); // TODO: blocks until robot heartbeat, stuck if port id mismatches - if (client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "LBR in session state idle, exiting."); + if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting."); break; } } - client_ptr_->get_state_interface().uninitialize(); + async_client_ptr_->get_state_interface().uninitialize(); running_ = false; - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Exiting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread."); }); run_thread_.detach(); } void App::stop_run() { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Requesting run thread stop."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop."); should_stop_ = true; } bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), - "Expected port_id in [30200, 30209], got %d.", port_id); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", + port_id); return false; } return true; diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 82a73046..a42bd61c 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -7,9 +7,70 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) { app_node_ptr_->declare_parameter("port_id", 30200); app_node_ptr_->declare_parameter("remote_host", std::string("")); app_node_ptr_->declare_parameter("rt_prio", 80); + app_node_ptr_->declare_parameter("robot_description", std::string("")); + app_node_ptr_->declare_parameter("pid.p", 1.0); + app_node_ptr_->declare_parameter("pid.i", 0.0); + app_node_ptr_->declare_parameter("pid.d", 0.0); + app_node_ptr_->declare_parameter("pid.i_max", 0.0); + app_node_ptr_->declare_parameter("pid.i_min", 0.0); + app_node_ptr_->declare_parameter("pid.antiwindup", false); + app_node_ptr_->declare_parameter("command_guard_variant", std::string("safe_stop")); + app_node_ptr_->declare_parameter("external_torque_cutoff_frequency", 10.); + app_node_ptr_->declare_parameter("measured_torque_cutoff_frequency", 10.); + app_node_ptr_->declare_parameter("open_loop", true); - client_ptr_ = std::make_shared(app_node_ptr_); - app_ptr_ = std::make_unique(app_node_ptr_, client_ptr_); + // prepare parameters + PIDParameters pid_parameters; + pid_parameters.p = app_node_ptr_->get_parameter("pid.p").as_double(); + pid_parameters.i = app_node_ptr_->get_parameter("pid.i").as_double(); + pid_parameters.d = app_node_ptr_->get_parameter("pid.d").as_double(); + pid_parameters.i_max = app_node_ptr_->get_parameter("pid.i_max").as_double(); + pid_parameters.i_min = app_node_ptr_->get_parameter("pid.i_min").as_double(); + pid_parameters.antiwindup = app_node_ptr_->get_parameter("pid.antiwindup").as_bool(); + CommandGuardParameters command_guard_parameters; + std::string command_guard_variant = + app_node_ptr_->get_parameter("command_guard_variant").as_string(); + StateInterfaceParameters state_interface_parameters; + state_interface_parameters.external_torque_cutoff_frequency = + app_node_ptr_->get_parameter("external_torque_cutoff_frequency").as_double(); + state_interface_parameters.measured_torque_cutoff_frequency = + app_node_ptr_->get_parameter("measured_torque_cutoff_frequency").as_double(); + bool open_loop = app_node_ptr_->get_parameter("open_loop").as_bool(); + + // load robot description and parse limits + auto robot_description_param = app_node_ptr_->get_parameter("robot_description"); + urdf::Model model; + if (!model.initString(robot_description_param.as_string())) { + std::string err = "Failed to intialize urdf model from '" + robot_description_param.get_name() + + "' parameter."; + RCLCPP_ERROR(app_node_ptr_->get_logger(), err.c_str()); + throw std::runtime_error(err); + } + + std::size_t jnt_cnt = 0; + for (const auto &name_joint_pair : model.joints_) { + const auto joint = name_joint_pair.second; + if (joint->type == urdf::Joint::REVOLUTE) { + if (jnt_cnt >= command_guard_parameters.joint_names.size()) { + std::string errs = + "Found too many joints in '" + robot_description_param.get_name() + "' parameter."; + RCLCPP_ERROR(app_node_ptr_->get_logger(), errs.c_str()); + throw std::runtime_error(errs); + } + command_guard_parameters.joint_names[jnt_cnt] = name_joint_pair.first; + command_guard_parameters.min_position[jnt_cnt] = joint->limits->lower; + command_guard_parameters.max_position[jnt_cnt] = joint->limits->upper; + command_guard_parameters.max_velocity[jnt_cnt] = joint->limits->velocity; + command_guard_parameters.max_torque[jnt_cnt] = joint->limits->effort; + ++jnt_cnt; + } + } + + // configure client + async_client_ptr_ = + std::make_shared(pid_parameters, command_guard_parameters, command_guard_variant, + state_interface_parameters, open_loop); + app_ptr_ = std::make_unique(async_client_ptr_); // default connect connect_(app_node_ptr_->get_parameter("port_id").as_int(), @@ -39,7 +100,7 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, }; app_ptr_->run(rt_prio); uint8_t attempt = 0; - while (!client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { + while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", attempt + 1, max_attempts, port_id); std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -52,40 +113,41 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, RCLCPP_INFO(app_node_ptr_->get_logger(), "Robot connected."); RCLCPP_INFO( - app_node_ptr_->get_logger(), "Control mode: %s.", - EnumMaps::control_mode_map(client_ptr_->get_state_interface().get_state().control_mode) + app_node_ptr_->get_logger(), "Control mode: '%s'.", + EnumMaps::control_mode_map(async_client_ptr_->get_state_interface().get_state().control_mode) .c_str()); - RCLCPP_INFO(app_node_ptr_->get_logger(), "Sample time: %.3f s.", - client_ptr_->get_state_interface().get_state().sample_time); + RCLCPP_INFO(app_node_ptr_->get_logger(), "Sample time: %.3f s / %.1f Hz.", + async_client_ptr_->get_state_interface().get_state().sample_time, + 1. / async_client_ptr_->get_state_interface().get_state().sample_time); // publisher state_pub_ = app_node_ptr_->create_publisher("state", 1); state_pub_timer_ = app_node_ptr_->create_wall_timer( - std::chrono::milliseconds( - static_cast(client_ptr_->get_state_interface().get_state().sample_time * 1.e3)), + std::chrono::milliseconds(static_cast( + async_client_ptr_->get_state_interface().get_state().sample_time * 1.e3)), std::bind(&AppComponent::on_state_pub_timer_, this)); // await commanding active thread std::thread await_commanding_active_thread([this]() { - while (client_ptr_->get_state_interface().get_state().session_state != + while (async_client_ptr_->get_state_interface().get_state().session_state != KUKA::FRI::ESessionState::COMMANDING_ACTIVE && rclcpp::ok()) { - RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot to enter %s state.", + RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot to enter '%s' state.", EnumMaps::session_state_map(KUKA::FRI::ESessionState::COMMANDING_ACTIVE).c_str()); std::this_thread::sleep_for(std::chrono::seconds(2)); } - RCLCPP_INFO(app_node_ptr_->get_logger(), "Client command mode: %s.", + RCLCPP_INFO(app_node_ptr_->get_logger(), "AsyncClient command mode: '%s'.", EnumMaps::client_command_mode_map( - client_ptr_->get_state_interface().get_state().client_command_mode) + async_client_ptr_->get_state_interface().get_state().client_command_mode) .c_str()); // subscriptions - switch (client_ptr_->get_state_interface().get_state().client_command_mode) { + switch (async_client_ptr_->get_state_interface().get_state().client_command_mode) { case KUKA::FRI::EClientCommandMode::POSITION: position_command_sub_ = app_node_ptr_->create_subscription( - "command/position", 1, + "command/joint_position", 1, std::bind(&AppComponent::on_position_command_, this, std::placeholders::_1)); break; case KUKA::FRI::EClientCommandMode::TORQUE: @@ -111,16 +173,16 @@ void AppComponent::on_position_command_( return; } - if (client_ptr_->get_state_interface().get_state().session_state == + if (async_client_ptr_->get_state_interface().get_state().session_state == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { lbr_command_.joint_position = lbr_position_command->joint_position; - client_ptr_->get_command_interface().set_command_target(lbr_command_); + async_client_ptr_->get_command_interface().set_command_target(lbr_command_); return; } // if not commanding active, reset lbr_command_.joint_position = - client_ptr_->get_state_interface().get_state().measured_joint_position; + async_client_ptr_->get_state_interface().get_state().measured_joint_position; } void AppComponent::on_torque_command_( @@ -129,17 +191,17 @@ void AppComponent::on_torque_command_( return; } - if (client_ptr_->get_state_interface().get_state().session_state == + if (async_client_ptr_->get_state_interface().get_state().session_state == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { lbr_command_.joint_position = lbr_torque_command->joint_position; lbr_command_.torque = lbr_torque_command->torque; - client_ptr_->get_command_interface().set_command_target(lbr_command_); + async_client_ptr_->get_command_interface().set_command_target(lbr_command_); return; } // if not active, reset lbr_command_.joint_position = - client_ptr_->get_state_interface().get_state().measured_joint_position; + async_client_ptr_->get_state_interface().get_state().measured_joint_position; std::fill(lbr_command_.torque.begin(), lbr_command_.torque.end(), 0.0); } @@ -149,30 +211,31 @@ void AppComponent::on_wrench_command_( return; } - if (client_ptr_->get_state_interface().get_state().session_state == + if (async_client_ptr_->get_state_interface().get_state().session_state == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { lbr_command_.joint_position = lbr_wrench_command->joint_position; lbr_command_.wrench = lbr_wrench_command->wrench; - client_ptr_->get_command_interface().set_command_target(lbr_command_); + async_client_ptr_->get_command_interface().set_command_target(lbr_command_); return; } // if not active, reset lbr_command_.joint_position = - client_ptr_->get_state_interface().get_state().measured_joint_position; + async_client_ptr_->get_state_interface().get_state().measured_joint_position; std::fill(lbr_command_.wrench.begin(), lbr_command_.wrench.end(), 0.0); } bool AppComponent::on_command_checks_(const int &expected_command_mode) { - if (!client_ptr_) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Client not configured."); + if (!async_client_ptr_) { + RCLCPP_ERROR(app_node_ptr_->get_logger(), "AsyncClient not configured."); return false; } - if (client_ptr_->get_state_interface().get_state().client_command_mode == + if (async_client_ptr_->get_state_interface().get_state().client_command_mode == KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE) { return false; } - if (client_ptr_->get_state_interface().get_state().client_command_mode != expected_command_mode) { + if (async_client_ptr_->get_state_interface().get_state().client_command_mode != + expected_command_mode) { RCLCPP_ERROR(app_node_ptr_->get_logger(), "Wrench command only allowed in wrench command mode."); return false; @@ -181,7 +244,7 @@ bool AppComponent::on_command_checks_(const int &expected_command_mode) { } void AppComponent::on_state_pub_timer_() { - state_pub_->publish(client_ptr_->get_state_interface().get_state()); + state_pub_->publish(async_client_ptr_->get_state_interface().get_state()); } void AppComponent::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request::SharedPtr request, @@ -191,7 +254,7 @@ void AppComponent::on_app_connect_(const lbr_fri_msgs::srv::AppConnect::Request: request->remote_host.c_str()); connect_(request->port_id, request->remote_host.empty() ? NULL : request->remote_host.c_str(), request->rt_prio, request->max_attempts); - response->connected = client_ptr_->get_state_interface().is_initialized(); + response->connected = async_client_ptr_->get_state_interface().is_initialized(); response->message = response->connected ? "Robot connected." : "Failed."; } diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index 459fda22..e818ecae 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -6,6 +6,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "urdf/model.h" #include "lbr_fri_msgs/msg/lbr_position_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" @@ -14,8 +15,11 @@ #include "lbr_fri_msgs/srv/app_connect.hpp" #include "lbr_fri_msgs/srv/app_disconnect.hpp" #include "lbr_fri_ros2/app.hpp" -#include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/enum_maps.hpp" +#include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/state_interface.hpp" namespace lbr_fri_ros2 { /** @@ -81,7 +85,7 @@ class AppComponent { // app rclcpp::Node::SharedPtr app_node_ptr_; /** Node for communicating with ROS.<*/ - std::shared_ptr client_ptr_; + std::shared_ptr async_client_ptr_; std::unique_ptr app_ptr_; /** #lbr_fri_ros2::App for communicating with the hardware.<*/ }; } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/async_client.cpp similarity index 51% rename from lbr_fri_ros2/src/client.cpp rename to lbr_fri_ros2/src/async_client.cpp index cb10247c..923bf6db 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -1,30 +1,33 @@ -#include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/async_client.hpp" namespace lbr_fri_ros2 { -Client::Client(const rclcpp::Node::SharedPtr node_ptr) - : logging_interface_ptr_(node_ptr->get_node_logging_interface()), - parameters_interface_ptr_(node_ptr->get_node_parameters_interface()), - command_interface_(node_ptr), - state_interface_(logging_interface_ptr_, parameters_interface_ptr_), open_loop_(true) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Configuring client."); - if (!node_ptr->has_parameter("open_loop")) { - node_ptr->declare_parameter("open_loop", true); - } - node_ptr->get_parameter("open_loop", open_loop_); - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Configured client with open_loop '%s'.", - open_loop_ ? "true" : "false"); +AsyncClient::AsyncClient(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant, + const StateInterfaceParameters &state_interface_parameters, + const bool &open_loop) + : command_interface_(pid_parameters, command_guard_parameters, command_guard_variant), + state_interface_(state_interface_parameters), open_loop_(open_loop) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Configuring client."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: '%s'.", + command_guard_variant.c_str()); + command_interface_.log_info(); + state_interface_.log_info(); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: '%s'.", open_loop_ ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured."); } -void Client::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "LBR switched from %s to %s.", +void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, + KUKA::FRI::ESessionState new_state) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR switched from '%s' to '%s'.", EnumMaps::session_state_map(old_state).c_str(), EnumMaps::session_state_map(new_state).c_str()); command_interface_.init_command(robotState()); } -void Client::monitor() { state_interface_.set_state(robotState()); }; +void AsyncClient::monitor() { state_interface_.set_state(robotState()); }; -void Client::waitForCommand() { +void AsyncClient::waitForCommand() { KUKA::FRI::LBRClient::waitForCommand(); state_interface_.set_state(robotState()); @@ -37,7 +40,7 @@ void Client::waitForCommand() { } } -void Client::command() { +void AsyncClient::command() { if (open_loop_) { state_interface_.set_state_open_loop(robotState(), command_interface_.get_command().joint_position); @@ -58,7 +61,7 @@ void Client::command() { default: std::string err = "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + "."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } } diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 366be0fb..f663781d 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -1,57 +1,8 @@ #include "lbr_fri_ros2/command_guard.hpp" namespace lbr_fri_ros2 { -CommandGuard::CommandGuard( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr) - : logging_interface_ptr_(logging_interface_ptr), - parameters_interface_ptr_(parameters_interface_ptr) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Configuring command guard."); - rclcpp::Parameter robot_description_param; - if (!parameters_interface_ptr_->has_parameter("robot_description")) { - parameters_interface_ptr_->declare_parameter("robot_description", rclcpp::ParameterValue("")); - } - parameters_interface_ptr_->get_parameter("robot_description", robot_description_param); - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Reading joint limits from '%s' parameter.", - robot_description_param.get_name().c_str()); - urdf::Model model; - if (!model.initString(robot_description_param.as_string())) { - std::string err = "Failed to intialize urdf model from '" + robot_description_param.get_name() + - "' parameter."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); - throw std::runtime_error(err); - } - std::size_t jnt_cnt = 0; - for (const auto &name_joint_pair : model.joints_) { - const auto joint = name_joint_pair.second; - if (joint->type == urdf::Joint::REVOLUTE) { - if (jnt_cnt > std::tuple_size()) { - std::string errs = - "Found too many joints in '" + robot_description_param.get_name() + "' parameter."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), errs.c_str()); - throw std::runtime_error(errs); - } - min_position_[jnt_cnt] = joint->limits->lower; - max_position_[jnt_cnt] = joint->limits->upper; - max_velocity_[jnt_cnt] = joint->limits->velocity; - max_torque_[jnt_cnt] = joint->limits->effort; - RCLCPP_INFO( - logging_interface_ptr_->get_logger(), - "Joint %s limits: Position [%.1f, %.1f] deg, velocity %.1f deg/s, torque %.1f Nm.", - name_joint_pair.first.c_str(), min_position_[jnt_cnt] * (180. / M_PI), - max_position_[jnt_cnt] * (180. / M_PI), max_velocity_[jnt_cnt] * (180. / M_PI), - max_torque_[jnt_cnt]); - ++jnt_cnt; - } - } - if (jnt_cnt != std::tuple_size()) { - std::string err = "Didn't find expected number of joints in '" + - robot_description_param.get_name() + "' parameter."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); - throw std::runtime_error(err); - }; - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Configured command guard."); -}; +CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters) + : parameters_(command_guard_parameters){}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const { @@ -59,9 +10,6 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: return false; case KUKA::FRI::EClientCommandMode::POSITION: - if (is_nan_(lbr_command.joint_position.cbegin(), lbr_command.joint_position.cend())) { - return false; - } if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; } @@ -70,46 +18,42 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, } return true; case KUKA::FRI::EClientCommandMode::WRENCH: - if (is_nan_(lbr_command.joint_position.cbegin(), lbr_command.joint_position.cend())) { - return false; - } - if (is_nan_(lbr_command.wrench.cbegin(), lbr_command.wrench.cend())) { - return false; - } if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; } return true; case KUKA::FRI::EClientCommandMode::TORQUE: - if (is_nan_(lbr_command.joint_position.cbegin(), lbr_command.joint_position.cend())) { - return false; - } - if (is_nan_(lbr_command.torque.cbegin(), lbr_command.torque.cend())) { - return false; - } if (!command_in_position_limits_(lbr_command, lbr_state)) { return false; } return true; default: - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Invalid EClientCommandMode provided."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided."); return false; } - - return true; + return false; } -bool CommandGuard::is_nan_(const double *begin, const double *end) const { - return std::find_if(begin, end, [&](const auto &xi) { return std::isnan(xi); }) != end; +void CommandGuard::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + for (std::size_t i = 0; i < parameters_.joint_names.size(); ++i) { + RCLCPP_INFO( + rclcpp::get_logger(LOGGER_NAME), + "* Joint %s limits: Position: [%.1f, %.1f] deg, velocity: %.1f deg/s, torque: %.1f Nm", + parameters_.joint_names[i].c_str(), parameters_.min_position[i] * (180. / M_PI), + parameters_.max_position[i] * (180. / M_PI), parameters_.max_velocity[i] * (180. / M_PI), + parameters_.max_torque[i]); + } } bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref /*lbr_state*/) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { - if (lbr_command.joint_position[i] < min_position_[i] || - lbr_command.joint_position[i] > max_position_[i]) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), - "Position command not in limits for joint A%ld.", i + 1); + if (lbr_command.joint_position[i] < parameters_.min_position[i] || + lbr_command.joint_position[i] > parameters_.max_position[i]) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Position command not in limits for joint '%s'.", + parameters_.joint_names[i].c_str()); return false; } } @@ -121,9 +65,9 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma const double &dt = lbr_state.getSampleTime(); for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > - max_velocity_[i]) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Velocity not in limits for joint A%ld.", - i + 1); + parameters_.max_velocity[i]) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint '%s'.", + parameters_.joint_names[i].c_str()); return false; } } @@ -133,9 +77,10 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { - if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > max_torque_[i]) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), - "Torque command not in limits for joint A%ld.", i + 1); + if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > + parameters_.max_torque[i]) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint '%s'.", + parameters_.joint_names[i].c_str()); return false; } } @@ -146,29 +91,30 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < - min_position_[i] + max_velocity_[i] * lbr_state.getSampleTime() || + parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > - max_position_[i] - max_velocity_[i] * lbr_state.getSampleTime()) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), - "Position command not in limits for joint A%ld.", i + 1); + parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Position command not in limits for joint '%s'.", + parameters_.joint_names[i].c_str()); return false; } } return true; } -std::unique_ptr command_guard_factory( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr, - const std::string &variant) { +std::unique_ptr +command_guard_factory(const CommandGuardParameters &command_guard_parameters, + const std::string &variant) { + constexpr char LOGGER_NAME[] = "lbr_fri_ros2::command_guard_factory"; if (variant == "default") { - return std::make_unique(logging_interface_ptr, parameters_interface_ptr); + return std::make_unique(command_guard_parameters); } if (variant == "safe_stop") { - return std::make_unique(logging_interface_ptr, parameters_interface_ptr); + return std::make_unique(command_guard_parameters); } std::string err = "Invalid CommandGuard variant provided."; - RCLCPP_ERROR(logging_interface_ptr->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 1b58efeb..9ff16367 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -2,50 +2,39 @@ namespace lbr_fri_ros2 { -CommandInterface::CommandInterface(const rclcpp::Node::SharedPtr node_ptr) - : logging_interface_ptr_(node_ptr->get_node_logging_interface()), - parameters_interface_ptr_(node_ptr->get_node_parameters_interface()), - joint_position_pid_(node_ptr, {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}), pid_init_(false) { - rclcpp::Parameter command_guard_variant_param; - if (!parameters_interface_ptr_->has_parameter("command_guard_variant")) { - parameters_interface_ptr_->declare_parameter("command_guard_variant", - rclcpp::ParameterValue("safe_stop")); - } - parameters_interface_ptr_->get_parameter("command_guard_variant", command_guard_variant_param); - RCLCPP_INFO(logging_interface_ptr_->get_logger(), - "Configuring command interface with command guard '%s'.", - command_guard_variant_param.as_string().c_str()); - command_guard_ = command_guard_factory(logging_interface_ptr_, parameters_interface_ptr_, - command_guard_variant_param.as_string()); +CommandInterface::CommandInterface(const PIDParameters &pid_parameters, + const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) + : pid_parameters_(pid_parameters) { + command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; void CommandInterface::get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = "Set joint position only allowed in position command mode."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } // PID - if (!pid_init_) { - joint_position_pid_.init(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); - pid_init_ = true; + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); } - joint_position_pid_.compute(command_target_.joint_position, state.getMeasuredJointPosition(), - rclcpp::Duration(std::chrono::milliseconds( - static_cast(state.getSampleTime() * 1e3))), - command_.joint_position); + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -56,24 +45,23 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { std::string err = "Set torque only allowed in torque command mode."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } // PID - if (!pid_init_) { - joint_position_pid_.init(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); - pid_init_ = true; + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); } - joint_position_pid_.compute(command_target_.joint_position, state.getMeasuredJointPosition(), - rclcpp::Duration(std::chrono::milliseconds( - static_cast(state.getSampleTime() * 1e3))), - command_.joint_position); + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); command_.torque = command_target_.torque; // validate @@ -89,30 +77,29 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { std::string err = "Set wrench only allowed in wrench command mode."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } // PID - if (!pid_init_) { - joint_position_pid_.init(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); - pid_init_ = true; + if (!joint_position_pid_.is_initialized()) { + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); } - joint_position_pid_.compute(command_target_.joint_position, state.getMeasuredJointPosition(), - rclcpp::Duration(std::chrono::milliseconds( - static_cast(state.getSampleTime() * 1e3))), - command_.joint_position); + joint_position_pid_.compute( + command_target_.joint_position, state.getMeasuredJointPosition(), + std::chrono::nanoseconds(static_cast(state.getSampleTime() * 1.e9)), + command_.joint_position); command_.wrench = command_target_.wrench; // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -128,4 +115,16 @@ void CommandInterface::init_command(const_fri_state_t_ref state) { command_target_.wrench.fill(0.); command_ = command_target_; } -} // namespace lbr_fri_ros2 \ No newline at end of file + +void CommandInterface::log_info() const { + command_guard_->log_info(); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.p: %.1f", pid_parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %.1f", pid_parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %.1f", pid_parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %.1f", pid_parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %.1f", pid_parameters_.i_min); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.antiwindup: %s", + pid_parameters_.antiwindup ? "true" : "false"); +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp new file mode 100644 index 00000000..27d23034 --- /dev/null +++ b/lbr_fri_ros2/src/filters.cpp @@ -0,0 +1,69 @@ +#include "lbr_fri_ros2/filters.hpp" + +namespace lbr_fri_ros2 { +ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} + +ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { + set_cutoff_frequency(cutoff_frequency, 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); + } + sample_time_ = sample_time; + alpha_ = compute_alpha_(cutoff_frequency, sample_time); + 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); +} + +bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } + +void JointExponentialFilterArray::compute(const double *const current, value_array_t &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; + }); +} + +void JointExponentialFilterArray::initialize(const double &cutoff_frequency, + const double &sample_time) { + exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); + initialized_ = true; +} + +void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, + const std::chrono::nanoseconds &dt, value_array_t &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) { + 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::initialize(const PIDParameters &pid_parameters, const double &dt) { + std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { + pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt, + pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup); + }); + initialized_ = true; +} +} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp new file mode 100644 index 00000000..35929be8 --- /dev/null +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -0,0 +1,59 @@ +#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) { + 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); + + 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); + 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); + + 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); + } + }); +} + +void FTEstimator::reset() { + q_.data.setZero(); + tau_ext_.setZero(); + f_ext_.setZero(); +} +} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 881839e6..5cf5597b 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -1,14 +1,8 @@ #include "lbr_fri_ros2/state_interface.hpp" namespace lbr_fri_ros2 { -StateInterface::StateInterface( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_ptr, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr) - : logging_interface_ptr_(logging_interface_ptr), - parameters_interface_ptr_(parameters_interface_ptr), state_initialized_(false), - external_torque_filter_(logging_interface_ptr, parameters_interface_ptr, "external_torque"), - measured_torque_filter_(logging_interface_ptr, parameters_interface_ptr, "measured_torque"), - filters_init_(false) {} +StateInterface::StateInterface(const StateInterfaceParameters &state_interface_parameters) + : state_initialized_(false), parameters_(state_interface_parameters) {} void StateInterface::set_state(const_fri_state_t_ref state) { state_.client_command_mode = state.getClientCommandMode(); @@ -37,10 +31,9 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.time_stamp_sec = state.getTimestampSec(); state_.tracking_performance = state.getTrackingPerformance(); - if (!filters_init_) { - // init after state_ is available + if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { + // initialize once state_ is available init_filters_(); - filters_init_ = true; } state_initialized_ = true; }; @@ -73,16 +66,25 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.time_stamp_sec = state.getTimestampSec(); state_.tracking_performance = state.getTrackingPerformance(); - if (!filters_init_) { - // init after state_ is available + if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { + // initialize once state_ is available init_filters_(); - filters_init_ = true; } state_initialized_ = true; } void StateInterface::init_filters_() { - external_torque_filter_.init(10. /*Hz*/, state_.sample_time); - measured_torque_filter_.init(10. /*Hz*/, state_.sample_time); + external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, + state_.sample_time); + measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, + 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); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp deleted file mode 100644 index 4d278f14..00000000 --- a/lbr_fri_ros2/src/utils.cpp +++ /dev/null @@ -1,121 +0,0 @@ -#include "lbr_fri_ros2/utils.hpp" - -namespace lbr_fri_ros2 { -ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} - -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { - set_cutoff_frequency(cutoff_frequency, 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); - } - sample_time_ = sample_time; - alpha_ = compute_alpha_(cutoff_frequency, sample_time); - 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); -} - -bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } - -JointExponentialFilterArrayROS::JointExponentialFilterArrayROS( - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameter_interface, - const std::string ¶m_prefix) - : logging_interface_(logging_interface), parameter_interface_(parameter_interface), - param_prefix_(param_prefix) { - if (!param_prefix_.empty()) { - param_prefix_ += "."; - } -} - -void JointExponentialFilterArrayROS::compute(const double *const current, value_array_t &previous) { - int i = 0; - std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - [&](const auto ¤t_i) { - previous[i] = exponential_filter_.compute(current_i, previous[i]); - ++i; - }); -} - -void JointExponentialFilterArrayROS::init(const double &cutoff_frequency, - const double &sample_time) { - if (!parameter_interface_->has_parameter(param_prefix_ + cutoff_frequency_param_name_)) { - parameter_interface_->declare_parameter(param_prefix_ + cutoff_frequency_param_name_, - rclcpp::ParameterValue(cutoff_frequency)); - } - exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); - - parameter_callback_handle_ = parameter_interface_->add_on_set_parameters_callback( - [this](const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto ¶meter : parameters) { - try { - if (parameter.get_name() == param_prefix_ + cutoff_frequency_param_name_) { - exponential_filter_.set_cutoff_frequency(parameter.as_double(), - exponential_filter_.get_sample_time()); - RCLCPP_INFO(logging_interface_->get_logger(), - "Set %s to: %f, new smoothing factor: %f. 0: no smoothing, 1: maximal " - "smoothing.", - parameter.get_name().c_str(), parameter.as_double(), - 1. - exponential_filter_.get_alpha()); - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { - std::string info_msg = "Invalid parameter type: " + std::string(e.what()); - RCLCPP_INFO(logging_interface_->get_logger(), info_msg.c_str()); - result.reason = info_msg; - result.successful = false; - } - } - return result; - }); -} - -JointPIDArrayROS::JointPIDArrayROS(const rclcpp::Node::SharedPtr node, const name_array_t &names, - const std::string &prefix) - : pid_controllers_(pid_array_t{ - control_toolbox::PidROS{node, prefix + names[0]}, - control_toolbox::PidROS{node, prefix + names[1]}, - control_toolbox::PidROS{node, prefix + names[2]}, - control_toolbox::PidROS{node, prefix + names[3]}, - control_toolbox::PidROS{node, prefix + names[4]}, - control_toolbox::PidROS{node, prefix + names[5]}, - control_toolbox::PidROS{node, prefix + names[6]}, - }) {} - -void JointPIDArrayROS::compute(const value_array_t &command_target, const value_array_t &state, - const rclcpp::Duration &dt, value_array_t &command) { - int i = 0; - std::for_each(command.begin(), command.end(), [&](double &command_i) { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt); - ++i; - }); -} - -void JointPIDArrayROS::compute(const value_array_t &command_target, const double *state, - const rclcpp::Duration &dt, value_array_t &command) { - int i = 0; - std::for_each(command.begin(), command.end(), [&](double &command_i) { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt); - ++i; - }); -} - -void JointPIDArrayROS::init(const double &p, const double &i, const double &d, const double &i_max, - const double &i_min, const bool &antiwindup) { - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), - [&](auto &pid) { pid.initPid(p, i, d, i_max, i_min, antiwindup); }); -} -} // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 875c0765..b9d2ab8c 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 1.3.1 + 1.4.0 ROS 2 stack for KUKA LBRs. mhubii MIT diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 207d2a86..245aab41 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 `_ use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_ change the ``arm_controller`` to ``position_trajectory_controller``, as in `lbr_controllers.yaml `_ + #. In `moveit_controllers.yaml `_ change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_ Update MoveIt Configuration --------------------------- 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 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true 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 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true 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 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true 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 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 62ca0f77..7f202da5 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -17,13 +17,9 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(controller_interface REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) find_package(fri_vendor REQUIRED) find_package(FRIClient REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) -find_package(kinematics_interface_kdl REQUIRED) find_package(lbr_fri_msgs REQUIRED) find_package(lbr_fri_ros2 REQUIRED) find_package(kinematics_interface REQUIRED) @@ -35,9 +31,10 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED - src/lbr_estimated_ft_broadcaster.cpp + src/forward_lbr_position_command_controller.cpp + src/forward_lbr_torque_command_controller.cpp src/lbr_state_broadcaster.cpp - src/lbr_system_interface.cpp + src/system_interface.cpp ) # Add include directories @@ -54,11 +51,8 @@ target_include_directories( ament_target_dependencies( ${PROJECT_NAME} controller_interface - Eigen3 fri_vendor - geometry_msgs hardware_interface - kinematics_interface_kdl lbr_fri_msgs lbr_fri_ros2 pluginlib @@ -70,8 +64,9 @@ target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient ) -pluginlib_export_plugin_description_file(controller_interface lbr_broadcasters.xml) -pluginlib_export_plugin_description_file(hardware_interface lbr_system_interface.xml) +pluginlib_export_plugin_description_file(controller_interface plugin_description_files/broadcasters.xml) +pluginlib_export_plugin_description_file(controller_interface plugin_description_files/forward_command_controllers.xml) +pluginlib_export_plugin_description_file(hardware_interface plugin_description_files/system_interface.xml) # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html ament_export_targets( @@ -80,13 +75,9 @@ ament_export_targets( ament_export_dependencies( controller_interface - eigen3_cmake_module - Eigen3 fri_vendor FRIClient - geometry_msgs hardware_interface - kinematics_interface_kdl lbr_fri_msgs lbr_fri_ros2 pluginlib diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 3c8a7042..e3e52d30 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,30 +2,39 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 200 + update_rate: 100 - # Broadcasters + # ROS 2 control broadcasters joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + force_torque_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # LBR ROS 2 control broadcasters lbr_state_broadcaster: type: lbr_ros2_control/LBRStateBroadcaster - lbr_estimated_ft_broadcaster: - type: lbr_ros2_control/LBREstimatedFTBroadcaster - - # Controllers - position_trajectory_controller: + # ROS 2 control controllers + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: position_controllers/JointGroupPositionController -/**/lbr_estimated_ft_broadcaster: + # LBR ROS 2 control controllers + forward_lbr_position_command_controller: + type: lbr_ros2_control/ForwardLBRPositionCommandController + + forward_lbr_torque_command_controller: + type: lbr_ros2_control/ForwardLBRTorqueCommandController + +/**/force_torque_broadcaster: ros__parameters: - damping: 0.2 + frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 + sensor_name: estimated_ft_sensor -/**/position_trajectory_controller: +/**/joint_trajectory_controller: ros__parameters: joints: - A1 diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro new file mode 100644 index 00000000..3f389121 --- /dev/null +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -0,0 +1,160 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + lbr_ros2_control::SystemInterface + ${port_id} + ${remote_host} + ${rt_prio} + ${pid_p} + ${pid_i} + ${pid_d} + ${pid_i_max} + ${pid_i_min} + ${pid_antiwindup} + ${command_guard_variant} + + ${external_torque_cutoff_frequency} + + ${measured_torque_cutoff_frequency} + ${open_loop} + + + + + + + + + + + + + + + + + + + + + + + ${chain_root} + ${chain_tip} + ${damping} + ${force_x_th} + ${force_y_th} + ${force_z_th} + ${torque_x_th} + ${torque_y_th} + ${torque_z_th} + + + + + + + + + + + + + ${min_position} + ${max_position} + ${max_velocity} + ${max_torque} + + ${min_position} + ${max_position} + + + -${max_torque} + ${max_torque} + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp new file mode 100644 index 00000000..e769b459 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp @@ -0,0 +1,52 @@ +#ifndef LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "controller_interface/controller_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_msgs/msg/lbr_position_command.hpp" + +namespace lbr_ros2_control { +class ForwardLBRPositionCommandController : public controller_interface::ControllerInterface { +public: + ForwardLBRPositionCommandController(); + + 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: + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + + realtime_tools::RealtimeBuffer + rt_lbr_position_command_ptr_; + rclcpp::Subscription::SharedPtr + lbr_position_command_subscription_ptr_; +}; +} // end of namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp new file mode 100644 index 00000000..adad4304 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp @@ -0,0 +1,60 @@ +#ifndef LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_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_msgs/msg/lbr_torque_command.hpp" + +namespace lbr_ros2_control { +class ForwardLBRTorqueCommandController : public controller_interface::ControllerInterface { +public: + ForwardLBRTorqueCommandController(); + + 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_command_interfaces_(); + void clear_command_interfaces_(); + + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + + std::vector> + joint_position_command_interfaces_, torque_command_interfaces_; + + realtime_tools::RealtimeBuffer + rt_lbr_torque_command_ptr_; + rclcpp::Subscription::SharedPtr + lbr_torque_command_subscription_ptr_; +}; +} // end of namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp deleted file mode 100644 index 6b552575..00000000 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef LBR_ROS2_CONTROL__LBR_ESTIMATED_FT_BROADCASTER_HPP_ -#define LBR_ROS2_CONTROL__LBR_ESTIMATED_FT_BROADCASTER_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/SVD" -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "kinematics_interface_kdl/kinematics_interface_kdl.hpp" -#include "rclcpp/rclcpp.hpp" -#include "realtime_tools/realtime_publisher.h" - -#include "friClientIf.h" -#include "friLBRState.h" - -#include "lbr_ros2_control/lbr_system_interface_type_values.hpp" - -namespace lbr_ros2_control { -class LBREstimatedFTBroadcaster : public controller_interface::ControllerInterface { -public: - LBREstimatedFTBroadcaster(); - - 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: - void init_states_(); - bool reference_state_interfaces_(); - void clear_state_interfaces_(); - - template - Eigen::Matrix - damped_least_squares_(const MatT &mat, - typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}); - - kinematics_interface_kdl::KinematicsInterfaceKDL kinematics_interface_kdl_; - - Eigen::Matrix jacobian_; - Eigen::Matrix jacobian_pinv_; - Eigen::Matrix joint_positions_, - external_joint_torques_; - Eigen::Matrix virtual_ft_; - - double damping_; - - std::string end_effector_link_ = "link_ee"; - std::array joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - - std::vector> - joint_position_interfaces_, external_joint_torque_interfaces_; - - rclcpp::Publisher::SharedPtr wrench_stamped_publisher_ptr_; - std::shared_ptr> - rt_wrench_stamped_publisher_ptr_; -}; -} // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_ESTIMATED_FT_BROADCASTER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp index f054da1c..14ef402c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp @@ -1,11 +1,13 @@ #ifndef LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ #define LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ +#include #include #include #include #include #include +#include #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -16,7 +18,7 @@ #include "friLBRState.h" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_ros2_control/lbr_system_interface_type_values.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { class LBRStateBroadcaster : public controller_interface::ControllerInterface { @@ -45,7 +47,8 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { void init_state_interface_map_(); void init_state_msg_(); - std::array joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; std::unordered_map> state_interface_map_; rclcpp::Publisher::SharedPtr state_publisher_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp similarity index 53% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface.hpp rename to lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index ee69f3e1..e6295b7f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -1,9 +1,8 @@ -#ifndef LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_HPP_ -#define LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_HPP_ +#ifndef LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ +#define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ #include #include -#include #include #include #include @@ -21,14 +20,55 @@ #include "lbr_fri_msgs/msg/lbr_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" #include "lbr_fri_ros2/app.hpp" -#include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/async_client.hpp" +#include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/enum_maps.hpp" -#include "lbr_ros2_control/lbr_system_interface_type_values.hpp" +#include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/ft_estimator.hpp" +#include "lbr_fri_ros2/state_interface.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -class LBRSystemInterface : public hardware_interface::SystemInterface { +struct SystemInterfaceParameters { + int32_t port_id{30200}; + 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}; + std::string command_guard_variant{"default"}; + double external_torque_cutoff_frequency{10.0}; + double measured_torque_cutoff_frequency{10.0}; +}; + +struct EstimatedFTSensorParameters { + std::string chain_root{"link_0"}; + std::string chain_tip{"link_ee"}; + double damping{0.2}; + double force_x_th{2.0}; + double force_y_th{2.0}; + double force_z_th{2.0}; + double torque_x_th{0.5}; + double torque_y_th{0.5}; + double torque_z_th{0.5}; +}; + +class SystemInterface : public hardware_interface::SystemInterface { +protected: + static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; + + static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; + static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; + static constexpr uint8_t LBR_FRI_SENSORS = 2; + static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; + static constexpr uint8_t ESTIMATED_FT_SENSOR_SIZE = 6; + public: - LBRSystemInterface() = default; + SystemInterface() = default; // hardware interface controller_interface::CallbackReturn @@ -58,25 +98,27 @@ class LBRSystemInterface : public hardware_interface::SystemInterface { bool verify_joint_command_interfaces_(); bool verify_joint_state_interfaces_(); bool verify_sensors_(); + bool verify_auxiliary_sensor_(); + bool verify_estimated_ft_sensor_(); // monitor end of commanding active bool exit_commanding_active_(const KUKA::FRI::ESessionState &previous_session_state, const KUKA::FRI::ESessionState &session_state); - const uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; - const uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; - const uint8_t LBR_FRI_SENSOR_SIZE = 12; + // robot parameters + SystemInterfaceParameters parameters_; + EstimatedFTSensorParameters ft_parameters_; - // node for handling communication - rclcpp::Node::SharedPtr app_node_ptr_; - std::shared_ptr client_ptr_; + // robot driver + std::shared_ptr async_client_ptr_; std::unique_ptr app_ptr_; - lbr_fri_msgs::msg::LBRCommand lbr_command_; - lbr_fri_msgs::msg::LBRState lbr_state_; + // exposed state interfaces (ideally these are taken from async_client_ptr_ but + // ros2_control ReadOnlyHandle does not allow for const pointers, refer + // https://github.com/ros-controls/ros2_control/issues/1196) + lbr_fri_msgs::msg::LBRState hw_lbr_state_; - // exposed state interfaces - double hw_sample_time_; + // exposed state interfaces that require casting double hw_session_state_; double hw_connection_quality_; double hw_safety_state_; @@ -85,38 +127,27 @@ class LBRSystemInterface : public hardware_interface::SystemInterface { double hw_client_command_mode_; double hw_overlay_type_; double hw_control_mode_; - double hw_time_stamp_sec_; double hw_time_stamp_nano_sec_; - lbr_fri_msgs::msg::LBRState::_measured_joint_position_type hw_position_; - lbr_fri_msgs::msg::LBRState::_commanded_joint_position_type hw_commanded_joint_position_; - lbr_fri_msgs::msg::LBRState::_measured_torque_type hw_effort_; - lbr_fri_msgs::msg::LBRState::_commanded_torque_type hw_commanded_torque_; - lbr_fri_msgs::msg::LBRState::_external_torque_type hw_external_torque_; - lbr_fri_msgs::msg::LBRState::_ipo_joint_position_type hw_ipo_joint_position_; - double hw_tracking_performance_; + // additional velocity state interface + lbr_fri_msgs::msg::LBRState::_measured_joint_position_type last_hw_measured_joint_position_; + double last_hw_time_stamp_sec_; + double last_hw_time_stamp_nano_sec_; + lbr_fri_msgs::msg::LBRState::_measured_joint_position_type hw_velocity_; - // comput velocity for state interface + // compute velocity for state interface double time_stamps_to_sec_(const double &sec, const double &nano_sec) const; void nan_last_hw_states_(); void update_last_hw_states_(); void compute_hw_velocity_(); - lbr_fri_msgs::msg::LBRState::_measured_joint_position_type last_hw_position_; - double last_hw_time_stamp_sec_; - double last_hw_time_stamp_nano_sec_; - lbr_fri_msgs::msg::LBRState::_measured_joint_position_type hw_velocity_; + // additional force-torque state interface + lbr_fri_ros2::FTEstimator::cart_array_t hw_ft_; + std::unique_ptr ft_estimator_ptr_; // exposed command interfaces - lbr_fri_msgs::msg::LBRCommand::_joint_position_type hw_position_command_; - lbr_fri_msgs::msg::LBRCommand::_torque_type hw_effort_command_; - - // app connect call request - int32_t port_id_; - const char *remote_host_; - int32_t rt_prio_; - bool open_loop_; + lbr_fri_msgs::msg::LBRCommand hw_lbr_command_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_HPP_ +#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp similarity index 73% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface_type_values.hpp rename to lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp index 74d10ece..8e898f6c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_system_interface_type_values.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_TYPE_VALUES_HPP_ -#define LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_TYPE_VALUES_HPP_ +#ifndef LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ +#define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ // see // https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -26,7 +26,15 @@ constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; constexpr char HW_IF_IPO_JOINT_POSITION[] = "ipo_joint_position"; constexpr char HW_IF_TRACKING_PERFORMANCE[] = "tracking_performance"; +// additional estimated force-torque state interface +constexpr char HW_IF_FORCE_X[] = "force.x"; +constexpr char HW_IF_FORCE_Y[] = "force.y"; +constexpr char HW_IF_FORCE_Z[] = "force.z"; +constexpr char HW_IF_TORQUE_X[] = "torque.x"; +constexpr char HW_IF_TORQUE_Y[] = "torque.y"; +constexpr char HW_IF_TORQUE_Z[] = "torque.z"; + // additional LBR command interfaces, reference KUKA::FRI::LBRCommand constexpr char HW_IF_WRENCH[] = "wrench"; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_SYSTEM_INTERFACE_TYPE_VALUES_HPP_ +#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 90b4fe81..a8859041 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -28,12 +28,12 @@ def generate_launch_description() -> LaunchDescription: joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="joint_state_broadcaster" ) + force_torque_broadcaster = LBRSystemInterface.node_controller_spawner( + controller="force_torque_broadcaster" + ) lbr_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="lbr_state_broadcaster" ) - lbr_estimated_ft_broadcast = LBRSystemInterface.node_controller_spawner( - controller="lbr_estimated_ft_broadcaster" - ) controller = LBRSystemInterface.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -42,8 +42,8 @@ def generate_launch_description() -> LaunchDescription: target_action=ros2_control_node, on_start=[ joint_state_broadcaster, + force_torque_broadcaster, lbr_state_broadcaster, - lbr_estimated_ft_broadcast, controller, ], ) diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index 8f11ce92..c7fce4a3 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -27,9 +27,14 @@ def arg_ctrl_cfg() -> DeclareLaunchArgument: def arg_ctrl() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl", - default_value="position_trajectory_controller", + default_value="joint_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", - choices=["position_trajectory_controller", "forward_position_controller"], + choices=[ + "joint_trajectory_controller", + "forward_position_controller", + "forward_lbr_position_command_controller", + "forward_lbr_torque_command_controller", + ], ) @staticmethod diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index d1f28312..675b0eb5 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,19 +2,15 @@ lbr_ros2_control - 1.3.1 + 1.4.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii MIT ament_cmake ament_cmake_python - eigen3_cmake_module - eigen fri_vendor - geometry_msgs - kinematics_interface_kdl lbr_fri_msgs lbr_fri_ros2 pluginlib diff --git a/lbr_ros2_control/lbr_broadcasters.xml b/lbr_ros2_control/plugin_description_files/broadcasters.xml similarity index 50% rename from lbr_ros2_control/lbr_broadcasters.xml rename to lbr_ros2_control/plugin_description_files/broadcasters.xml index 45a1d97b..684fdfe3 100644 --- a/lbr_ros2_control/lbr_broadcasters.xml +++ b/lbr_ros2_control/plugin_description_files/broadcasters.xml @@ -5,12 +5,4 @@ base_class_type="controller_interface::ControllerInterface"> Broadcaster for LBRState messages, see lbr_fri_msgs/msg/LBRState.msg. - - - - Broadcaster for end-effector force-torque as estimated from external joint - torques. - \ No newline at end of file diff --git a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml new file mode 100644 index 00000000..dd26c929 --- /dev/null +++ b/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml @@ -0,0 +1,18 @@ + + + + + Forward command controller for LBRPositionCommand message, see + lbr_fri_msgs/msg/LBRPositionCommand.msg. + + + + + Forward command controller for LBRTorqueCommand message, see + lbr_fri_msgs/msg/LBRTorqueCommand.msg. + + \ No newline at end of file diff --git a/lbr_ros2_control/lbr_system_interface.xml b/lbr_ros2_control/plugin_description_files/system_interface.xml similarity index 75% rename from lbr_ros2_control/lbr_system_interface.xml rename to lbr_ros2_control/plugin_description_files/system_interface.xml index 6483e900..2246c2e4 100644 --- a/lbr_ros2_control/lbr_system_interface.xml +++ b/lbr_ros2_control/plugin_description_files/system_interface.xml @@ -1,6 +1,6 @@ - \ No newline at end of file diff --git a/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp new file mode 100644 index 00000000..bfe960fe --- /dev/null +++ b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp @@ -0,0 +1,74 @@ +#include "lbr_ros2_control/forward_lbr_position_command_controller.hpp" + +namespace lbr_ros2_control { +ForwardLBRPositionCommandController::ForwardLBRPositionCommandController() + : rt_lbr_position_command_ptr_(nullptr), lbr_position_command_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +ForwardLBRPositionCommandController::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 +ForwardLBRPositionCommandController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_init() { + try { + lbr_position_command_subscription_ptr_ = + this->get_node()->create_subscription( + "command/joint_position", 1, + [this](const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr msg) { + rt_lbr_position_command_ptr_.writeFromNonRT(msg); + }); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize forward LBR position command controller with: %s.", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +ForwardLBRPositionCommandController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + auto lbr_position_command = rt_lbr_position_command_ptr_.readFromRT(); + if (!lbr_position_command || !(*lbr_position_command)) { + return controller_interface::return_type::OK; + } + std::for_each(command_interfaces_.begin(), command_interfaces_.end(), + [lbr_position_command, idx = 0](auto &command_interface) mutable { + command_interface.set_value((*lbr_position_command)->joint_position[idx++]); + }); + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} +} // end of namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRPositionCommandController, + controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp new file mode 100644 index 00000000..58becd53 --- /dev/null +++ b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp @@ -0,0 +1,110 @@ +#include "lbr_ros2_control/forward_lbr_torque_command_controller.hpp" + +namespace lbr_ros2_control { +ForwardLBRTorqueCommandController::ForwardLBRTorqueCommandController() + : rt_lbr_torque_command_ptr_(nullptr), lbr_torque_command_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +ForwardLBRTorqueCommandController::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); + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_EFFORT); + } + return interface_configuration; +} + +controller_interface::InterfaceConfiguration +ForwardLBRTorqueCommandController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init() { + try { + lbr_torque_command_subscription_ptr_ = + this->get_node()->create_subscription( + "command/torque", 1, [this](const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr msg) { + rt_lbr_torque_command_ptr_.writeFromNonRT(msg); + }); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize forward LBR torque command controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +ForwardLBRTorqueCommandController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + auto lbr_torque_command = rt_lbr_torque_command_ptr_.readFromRT(); + 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) { + 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]); + } + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +ForwardLBRTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!reference_command_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + clear_command_interfaces_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool ForwardLBRTorqueCommandController::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)); + } + if (command_interface.get_interface_name() == hardware_interface::HW_IF_EFFORT) { + torque_command_interfaces_.emplace_back(std::ref(command_interface)); + } + } + if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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); + return false; + } + if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + 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); + return false; + } + return true; +} + +void ForwardLBRTorqueCommandController::clear_command_interfaces_() { + joint_position_command_interfaces_.clear(); + torque_command_interfaces_.clear(); +} +} // end of namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRTorqueCommandController, + controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp deleted file mode 100644 index 2f8a7ad7..00000000 --- a/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include "lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp" - -namespace lbr_ros2_control { -LBREstimatedFTBroadcaster::LBREstimatedFTBroadcaster() - : jacobian_(6, KUKA::FRI::LBRState::NUMBER_OF_JOINTS), - jacobian_pinv_(KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 6) {} - -controller_interface::InterfaceConfiguration -LBREstimatedFTBroadcaster::command_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; -} - -controller_interface::InterfaceConfiguration -LBREstimatedFTBroadcaster::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(joint_name + "/" + HW_IF_EXTERNAL_TORQUE); - } - return interface_configuration; -} - -controller_interface::CallbackReturn LBREstimatedFTBroadcaster::on_init() { - try { - wrench_stamped_publisher_ptr_ = - this->get_node()->create_publisher( - "estimated_force_torque", rclcpp::SensorDataQoS()); - rt_wrench_stamped_publisher_ptr_ = - std::make_shared>( - wrench_stamped_publisher_ptr_); - kinematics_interface_kdl_.initialize(this->get_node()->get_node_parameters_interface(), - end_effector_link_); - if (!this->get_node()->get_parameter_or("damping", damping_, 2e-1)) { - RCLCPP_WARN(this->get_node()->get_logger(), - "Failed to get damping parameter, using default value: %f.", damping_); - } - 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; - } - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize LBR virtual FT broadcaster with: %s.", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type -LBREstimatedFTBroadcaster::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - // check any for nan - if (std::isnan(joint_position_interfaces_[0].get().get_value())) { - return controller_interface::return_type::OK; - } - for (std::size_t i = 0; i < joint_names_.size(); ++i) { - joint_positions_(i) = joint_position_interfaces_[i].get().get_value(); - external_joint_torques_(i) = external_joint_torque_interfaces_[i].get().get_value(); - } - // compute virtual FT given Jacobian and external joint torques - kinematics_interface_kdl_.calculate_jacobian(joint_positions_, end_effector_link_, jacobian_); - jacobian_pinv_ = damped_least_squares_(jacobian_, damping_); - virtual_ft_ = jacobian_pinv_.transpose() * external_joint_torques_; - // publish - if (rt_wrench_stamped_publisher_ptr_->trylock()) { - rt_wrench_stamped_publisher_ptr_->msg_.header.stamp = this->get_node()->now(); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.x = virtual_ft_(0); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = virtual_ft_(1); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = virtual_ft_(2); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = virtual_ft_(3); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = virtual_ft_(4); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5); - rt_wrench_stamped_publisher_ptr_->unlockAndPublish(); - } - return controller_interface::return_type::OK; -} - -controller_interface::CallbackReturn -LBREstimatedFTBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -LBREstimatedFTBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - init_states_(); - if (!reference_state_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -LBREstimatedFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_state_interfaces_(); - return controller_interface::CallbackReturn::SUCCESS; -} - -void LBREstimatedFTBroadcaster::init_states_() { - jacobian_.setConstant(std::numeric_limits::quiet_NaN()); - jacobian_pinv_.setConstant(std::numeric_limits::quiet_NaN()); - joint_positions_.setConstant(std::numeric_limits::quiet_NaN()); - external_joint_torques_.setConstant(std::numeric_limits::quiet_NaN()); - virtual_ft_.setConstant(std::numeric_limits::quiet_NaN()); - rt_wrench_stamped_publisher_ptr_->msg_.header.frame_id = end_effector_link_; - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.x = virtual_ft_(0); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = virtual_ft_(1); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = virtual_ft_(2); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = virtual_ft_(3); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = virtual_ft_(4); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5); -} - -bool LBREstimatedFTBroadcaster::reference_state_interfaces_() { - for (auto &state_interface : state_interfaces_) { - if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_interfaces_.emplace_back(std::ref(state_interface)); - } - if (state_interface.get_interface_name() == HW_IF_EXTERNAL_TORQUE) { - external_joint_torque_interfaces_.emplace_back(std::ref(state_interface)); - } - } - if (joint_position_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(this->get_node()->get_logger(), - "Number of joint position interfaces (%ld) does not match the number of joints " - "in the robot (%d).", - joint_position_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return false; - } - if (external_joint_torque_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR( - this->get_node()->get_logger(), - "Number of external joint torque interfaces (%ld) does not match the number of joints " - "in the robot (%d).", - external_joint_torque_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return false; - } - return true; -} - -void LBREstimatedFTBroadcaster::clear_state_interfaces_() { - joint_position_interfaces_.clear(); - external_joint_torque_interfaces_.clear(); -} - -template -Eigen::Matrix -LBREstimatedFTBroadcaster::damped_least_squares_( - const MatT &mat, - typename MatT::Scalar lambda) // choose appropriately -{ - typedef typename MatT::Scalar Scalar; - auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - const auto &singularValues = svd.singularValues(); - Eigen::Matrix dampedSingularValuesInv( - mat.cols(), mat.rows()); - dampedSingularValuesInv.setZero(); - for (unsigned int i = 0; i < singularValues.size(); ++i) { - dampedSingularValuesInv(i, i) = - singularValues(i) / (singularValues(i) * singularValues(i) + lambda * lambda); - } - return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); -} -} // end of namespace lbr_ros2_control - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBREstimatedFTBroadcaster, - controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index ed69eb68..f6c988b3 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -15,8 +15,8 @@ LBRStateBroadcaster::state_interface_configuration() const { controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { try { - state_publisher_ptr_ = this->get_node()->create_publisher( - "state", rclcpp::SensorDataQoS()); + state_publisher_ptr_ = + this->get_node()->create_publisher("state", 1); rt_state_publisher_ptr_ = std::make_shared>( @@ -50,29 +50,29 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time if (rt_state_publisher_ptr_->trylock()) { // FRI related states rt_state_publisher_ptr_->msg_.client_command_mode = - static_cast(state_interface_map_["fri_sensor"][HW_IF_CLIENT_COMMAND_MODE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CLIENT_COMMAND_MODE]); rt_state_publisher_ptr_->msg_.connection_quality = - static_cast(state_interface_map_["fri_sensor"][HW_IF_CONNECTION_QUALITY]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CONNECTION_QUALITY]); rt_state_publisher_ptr_->msg_.control_mode = - static_cast(state_interface_map_["fri_sensor"][HW_IF_CONTROL_MODE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_CONTROL_MODE]); rt_state_publisher_ptr_->msg_.drive_state = - static_cast(state_interface_map_["fri_sensor"][HW_IF_DRIVE_STATE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_DRIVE_STATE]); rt_state_publisher_ptr_->msg_.operation_mode = - static_cast(state_interface_map_["fri_sensor"][HW_IF_OPERATION_MODE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_OPERATION_MODE]); rt_state_publisher_ptr_->msg_.overlay_type = - static_cast(state_interface_map_["fri_sensor"][HW_IF_OVERLAY_TYPE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_OVERLAY_TYPE]); rt_state_publisher_ptr_->msg_.safety_state = - static_cast(state_interface_map_["fri_sensor"][HW_IF_SAFETY_STATE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_SAFETY_STATE]); rt_state_publisher_ptr_->msg_.sample_time = - state_interface_map_["fri_sensor"][HW_IF_SAMPLE_TIME]; + state_interface_map_["auxiliary_sensor"][HW_IF_SAMPLE_TIME]; rt_state_publisher_ptr_->msg_.session_state = - static_cast(state_interface_map_["fri_sensor"][HW_IF_SESSION_STATE]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_SESSION_STATE]); rt_state_publisher_ptr_->msg_.time_stamp_nano_sec = - static_cast(state_interface_map_["fri_sensor"][HW_IF_TIME_STAMP_NANO_SEC]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_NANO_SEC]); rt_state_publisher_ptr_->msg_.time_stamp_sec = - static_cast(state_interface_map_["fri_sensor"][HW_IF_TIME_STAMP_SEC]); + static_cast(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_SEC]); rt_state_publisher_ptr_->msg_.tracking_performance = - state_interface_map_["fri_sensor"][HW_IF_TRACKING_PERFORMANCE]; + state_interface_map_["auxiliary_sensor"][HW_IF_TRACKING_PERFORMANCE]; // joint related states std::for_each(joint_names_.begin(), joint_names_.end(), diff --git a/lbr_ros2_control/src/lbr_system_interface.cpp b/lbr_ros2_control/src/lbr_system_interface.cpp deleted file mode 100644 index 399aee6f..00000000 --- a/lbr_ros2_control/src/lbr_system_interface.cpp +++ /dev/null @@ -1,413 +0,0 @@ -#include "lbr_ros2_control/lbr_system_interface.hpp" - -namespace lbr_ros2_control { -controller_interface::CallbackReturn -LBRSystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { - auto ret = hardware_interface::SystemInterface::on_init(system_info); - if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Failed to initialize LBRSystemInterface."); - return ret; - } - - // parameters from lbr.ros2_control.xacro - port_id_ = std::stoul(info_.hardware_parameters["port_id"]); - info_.hardware_parameters["remote_host"] == "INADDR_ANY" - ? remote_host_ = NULL - : remote_host_ = info_.hardware_parameters["remote_host"].c_str(); - rt_prio_ = std::stoul(info_.hardware_parameters["rt_prio"]); - - if (port_id_ < 30200 || port_id_ > 30209) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Expected port_id in [30200, 30209]. Found %d.", - port_id_); - return controller_interface::CallbackReturn::ERROR; - } - std::transform(info_.hardware_parameters["open_loop"].begin(), - info_.hardware_parameters["open_loop"].end(), - info_.hardware_parameters["open_loop"].begin(), ::tolower); - open_loop_ = info_.hardware_parameters["open_loop"] == "true"; - - // setup node - app_node_ptr_ = std::make_shared("app"); - - app_node_ptr_->declare_parameter("port_id", port_id_); - app_node_ptr_->declare_parameter("remote_host", remote_host_ ? remote_host_ : ""); - app_node_ptr_->declare_parameter("command_guard_variant", "default"); - app_node_ptr_->declare_parameter("open_loop", open_loop_); - - client_ptr_ = std::make_shared(app_node_ptr_); - app_ptr_ = std::make_unique(app_node_ptr_, client_ptr_); - - nan_command_interfaces_(); - nan_state_interfaces_(); - nan_last_hw_states_(); - - if (!verify_number_of_joints_()) { - return controller_interface::CallbackReturn::ERROR; - } - - if (!verify_joint_command_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } - - if (!verify_joint_state_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } - - if (!verify_sensors_()) { - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -std::vector LBRSystemInterface::export_state_interfaces() { - std::vector state_interfaces; - - const auto &lbr_fri_sensor = info_.sensors[0]; - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_SAMPLE_TIME, &hw_sample_time_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_SESSION_STATE, &hw_session_state_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_CONNECTION_QUALITY, - &hw_connection_quality_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_SAFETY_STATE, &hw_safety_state_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_OPERATION_MODE, &hw_operation_mode_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_DRIVE_STATE, &hw_drive_state_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_CLIENT_COMMAND_MODE, - &hw_client_command_mode_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_OVERLAY_TYPE, &hw_overlay_type_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_CONTROL_MODE, &hw_control_mode_); - - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_TIME_STAMP_SEC, &hw_time_stamp_sec_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_TIME_STAMP_NANO_SEC, - &hw_time_stamp_nano_sec_); - - for (std::size_t i = 0; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_position_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_JOINT_POSITION, - &hw_commanded_joint_position_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_effort_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_TORQUE, - &hw_commanded_torque_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EXTERNAL_TORQUE, - &hw_external_torque_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, HW_IF_IPO_JOINT_POSITION, - &hw_ipo_joint_position_[i]); - - state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &hw_velocity_[i]); - } - - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_TRACKING_PERFORMANCE, - &hw_tracking_performance_); - - return state_interfaces; -} - -std::vector LBRSystemInterface::export_command_interfaces() { - std::vector command_interfaces; - - for (std::size_t i = 0; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_position_command_[i]); - - command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_effort_command_[i]); - } - - return command_interfaces; -} - -hardware_interface::return_type LBRSystemInterface::prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) { - return hardware_interface::return_type::OK; -} - -controller_interface::CallbackReturn -LBRSystemInterface::on_activate(const rclcpp_lifecycle::State &) { - if (!client_ptr_) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Client not configured."); - return controller_interface::CallbackReturn::ERROR; - } - if (!app_ptr_->open_udp_socket(port_id_, remote_host_)) { - return controller_interface::CallbackReturn::ERROR; - } - app_ptr_->run(rt_prio_); - uint8_t attempt = 0; - uint8_t max_attempts = 10; - while (!client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { - RCLCPP_INFO(app_node_ptr_->get_logger(), "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", - attempt + 1, max_attempts, port_id_); - std::this_thread::sleep_for(std::chrono::seconds(1)); - if (++attempt >= max_attempts) { - app_ptr_->close_udp_socket(); // hard close as run gets stuck - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Failed to connect to robot on max attempts."); - return controller_interface::CallbackReturn::ERROR; - } - } - RCLCPP_INFO(app_node_ptr_->get_logger(), "Robot connected."); - RCLCPP_INFO(app_node_ptr_->get_logger(), "Control mode: %s.", - lbr_fri_ros2::EnumMaps::control_mode_map( - client_ptr_->get_state_interface().get_state().control_mode) - .c_str()); - RCLCPP_INFO(app_node_ptr_->get_logger(), "Sample time: %.3f s.", - client_ptr_->get_state_interface().get_state().sample_time); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -LBRSystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { - app_ptr_->stop_run(); - app_ptr_->close_udp_socket(); - return controller_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::return_type LBRSystemInterface::read(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - lbr_state_ = client_ptr_->get_state_interface().get_state(); - if (exit_commanding_active_(static_cast(hw_session_state_), - static_cast(lbr_state_.session_state))) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), - "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); - app_ptr_->stop_run(); - app_ptr_->close_udp_socket(); - return hardware_interface::return_type::ERROR; - } - - hw_sample_time_ = lbr_state_.sample_time; - hw_session_state_ = static_cast(lbr_state_.session_state); - hw_connection_quality_ = static_cast(lbr_state_.connection_quality); - hw_safety_state_ = static_cast(lbr_state_.safety_state); - hw_operation_mode_ = static_cast(lbr_state_.operation_mode); - hw_drive_state_ = static_cast(lbr_state_.drive_state); - hw_client_command_mode_ = static_cast(lbr_state_.client_command_mode); - hw_overlay_type_ = static_cast(lbr_state_.overlay_type); - hw_control_mode_ = static_cast(lbr_state_.control_mode); - - hw_time_stamp_sec_ = static_cast(lbr_state_.time_stamp_sec); - hw_time_stamp_nano_sec_ = static_cast(lbr_state_.time_stamp_nano_sec); - - std::memcpy(hw_position_.data(), lbr_state_.measured_joint_position.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(hw_commanded_joint_position_.data(), lbr_state_.commanded_joint_position.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(hw_effort_.data(), lbr_state_.measured_torque.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(hw_commanded_torque_.data(), lbr_state_.commanded_torque.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(hw_external_torque_.data(), lbr_state_.external_torque.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(hw_ipo_joint_position_.data(), lbr_state_.ipo_joint_position.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - hw_tracking_performance_ = lbr_state_.tracking_performance; - compute_hw_velocity_(); - update_last_hw_states_(); - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type LBRSystemInterface::write(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) { - return hardware_interface::return_type::OK; - } - if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) { - if (std::any_of(hw_position_command_.cbegin(), hw_position_command_.cend(), - [](const double &v) { return std::isnan(v); })) { - return hardware_interface::return_type::OK; - } - std::memcpy(lbr_command_.joint_position.data(), hw_position_command_.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - client_ptr_->get_command_interface().set_command_target(lbr_command_); - return hardware_interface::return_type::OK; - } - return hardware_interface::return_type::ERROR; -} - -void LBRSystemInterface::nan_command_interfaces_() { - hw_position_command_.fill(std::numeric_limits::quiet_NaN()); - hw_effort_command_.fill(std::numeric_limits::quiet_NaN()); -} - -void LBRSystemInterface::nan_state_interfaces_() { - hw_sample_time_ = std::numeric_limits::quiet_NaN(); - hw_session_state_ = std::numeric_limits::quiet_NaN(); - hw_connection_quality_ = std::numeric_limits::quiet_NaN(); - hw_safety_state_ = std::numeric_limits::quiet_NaN(); - hw_operation_mode_ = std::numeric_limits::quiet_NaN(); - hw_drive_state_ = std::numeric_limits::quiet_NaN(); - hw_client_command_mode_ = std::numeric_limits::quiet_NaN(); - hw_overlay_type_ = std::numeric_limits::quiet_NaN(); - hw_control_mode_ = std::numeric_limits::quiet_NaN(); - - hw_time_stamp_sec_ = std::numeric_limits::quiet_NaN(); - hw_time_stamp_nano_sec_ = std::numeric_limits::quiet_NaN(); - - hw_position_.fill(std::numeric_limits::quiet_NaN()); - hw_commanded_joint_position_.fill(std::numeric_limits::quiet_NaN()); - hw_effort_.fill(std::numeric_limits::quiet_NaN()); - hw_commanded_torque_.fill(std::numeric_limits::quiet_NaN()); - hw_external_torque_.fill(std::numeric_limits::quiet_NaN()); - hw_ipo_joint_position_.fill(std::numeric_limits::quiet_NaN()); - hw_tracking_performance_ = std::numeric_limits::quiet_NaN(); - - hw_velocity_.fill(std::numeric_limits::quiet_NaN()); -} - -bool LBRSystemInterface::verify_number_of_joints_() { - if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Expected %d joints in URDF. Found %ld.", - KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); - return false; - } - return true; -} - -bool LBRSystemInterface::verify_joint_command_interfaces_() { - // check command interfaces - for (auto &joint : info_.joints) { - if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { - RCLCPP_ERROR( - app_node_ptr_->get_logger(), - "Joint %s received invalid number of command interfaces. Received %ld, expected %d.", - joint.name.c_str(), joint.command_interfaces.size(), LBR_FRI_COMMAND_INTERFACE_SIZE); - return false; - } - for (auto &ci : joint.command_interfaces) { - if (ci.name != hardware_interface::HW_IF_POSITION && - ci.name != hardware_interface::HW_IF_EFFORT) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), - "Joint %s received invalid command interface: %s. Expected %s or %s.", - joint.name.c_str(), ci.name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_EFFORT); - return false; - } - } - } - return true; -} - -bool LBRSystemInterface::verify_joint_state_interfaces_() { - // check state interfaces - for (auto &joint : info_.joints) { - if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { - RCLCPP_ERROR( - app_node_ptr_->get_logger(), - "Joint %s received invalid number of state interfaces. Received %ld, expected %d.", - joint.name.c_str(), joint.state_interfaces.size(), LBR_FRI_STATE_INTERFACE_SIZE); - return false; - } - for (auto &si : joint.state_interfaces) { - if (si.name != hardware_interface::HW_IF_POSITION && - si.name != HW_IF_COMMANDED_JOINT_POSITION && - si.name != hardware_interface::HW_IF_EFFORT && si.name != HW_IF_COMMANDED_TORQUE && - si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && - si.name != hardware_interface::HW_IF_VELOCITY) { - RCLCPP_ERROR( - app_node_ptr_->get_logger(), - "Joint %s received invalid state interface: %s. Expected %s, %s, %s, %s, %s, %s or %s.", - joint.name.c_str(), si.name.c_str(), hardware_interface::HW_IF_POSITION, - HW_IF_COMMANDED_JOINT_POSITION, hardware_interface::HW_IF_EFFORT, - HW_IF_COMMANDED_TORQUE, HW_IF_EXTERNAL_TORQUE, HW_IF_IPO_JOINT_POSITION, - hardware_interface::HW_IF_VELOCITY); - return false; - } - } - } - return true; -} - -bool LBRSystemInterface::verify_sensors_() { - // check lbr specific state interfaces - if (info_.sensors.size() > 1) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Expected 1 sensor, got %ld", info_.sensors.size()); - return false; - } - - // check all interfaces are defined in lbr.ros2_control.xacro - const auto &lbr_fri_sensor = info_.sensors[0]; - if (lbr_fri_sensor.state_interfaces.size() != LBR_FRI_SENSOR_SIZE) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), - "Sensor %s received invalid state interface. Received %ld, expected %d. ", - lbr_fri_sensor.name.c_str(), lbr_fri_sensor.state_interfaces.size(), - LBR_FRI_SENSOR_SIZE); - return false; - } - - // check only valid interfaces are defined - for (const auto &si : lbr_fri_sensor.state_interfaces) { - if (si.name != HW_IF_SAMPLE_TIME && si.name != HW_IF_SESSION_STATE && - si.name != HW_IF_CONNECTION_QUALITY && si.name != HW_IF_SAFETY_STATE && - si.name != HW_IF_OPERATION_MODE && si.name != HW_IF_DRIVE_STATE && - si.name != HW_IF_CLIENT_COMMAND_MODE && si.name != HW_IF_OVERLAY_TYPE && - si.name != HW_IF_CONTROL_MODE && si.name != HW_IF_TIME_STAMP_SEC && - si.name != HW_IF_TIME_STAMP_NANO_SEC && si.name != HW_IF_COMMANDED_JOINT_POSITION && - si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && - si.name != HW_IF_IPO_JOINT_POSITION && si.name != HW_IF_TRACKING_PERFORMANCE) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Sensor %s received invalid state interface %s.", - lbr_fri_sensor.name.c_str(), si.name.c_str()); - - return false; - } - } - return true; -} - -bool LBRSystemInterface::exit_commanding_active_( - const KUKA::FRI::ESessionState &previous_session_state, - const KUKA::FRI::ESessionState &session_state) { - if (previous_session_state == KUKA::FRI::ESessionState::COMMANDING_ACTIVE && - previous_session_state != session_state) { - return true; - } - return false; -} - -double LBRSystemInterface::time_stamps_to_sec_(const double &sec, const double &nano_sec) const { - return sec + nano_sec / 1.e9; -} - -void LBRSystemInterface::nan_last_hw_states_() { - last_hw_position_.fill(std::numeric_limits::quiet_NaN()); - last_hw_time_stamp_sec_ = std::numeric_limits::quiet_NaN(); - last_hw_time_stamp_nano_sec_ = std::numeric_limits::quiet_NaN(); -} - -void LBRSystemInterface::update_last_hw_states_() { - last_hw_position_ = hw_position_; - last_hw_time_stamp_sec_ = hw_time_stamp_sec_; - last_hw_time_stamp_nano_sec_ = hw_time_stamp_nano_sec_; -} - -void LBRSystemInterface::compute_hw_velocity_() { - // state uninitialized - if (std::isnan(last_hw_time_stamp_nano_sec_) || std::isnan(last_hw_position_[0])) { - return; - } - - // state wasn't updated - if (last_hw_time_stamp_sec_ == hw_time_stamp_sec_ && - last_hw_time_stamp_nano_sec_ == hw_time_stamp_nano_sec_) { - return; - } - - double dt = time_stamps_to_sec_(hw_time_stamp_sec_, hw_time_stamp_nano_sec_) - - time_stamps_to_sec_(last_hw_time_stamp_sec_, last_hw_time_stamp_nano_sec_); - std::size_t i = 0; - std::for_each(hw_velocity_.begin(), hw_velocity_.end(), [&](double &v) { - v = (hw_position_[i] - last_hw_position_[i]) / dt; - ++i; - }); -} - -} // end of namespace lbr_ros2_control - -#include - -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRSystemInterface, hardware_interface::SystemInterface) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp new file mode 100644 index 00000000..4722e417 --- /dev/null +++ b/lbr_ros2_control/src/system_interface.cpp @@ -0,0 +1,516 @@ +#include "lbr_ros2_control/system_interface.hpp" + +namespace lbr_ros2_control { +controller_interface::CallbackReturn +SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { + auto ret = hardware_interface::SystemInterface::on_init(system_info); + if (ret != controller_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to initialize SystemInterface."); + return ret; + } + + // parameters_ from config/lbr_system_interface.xacro + parameters_.port_id = std::stoul(info_.hardware_parameters["port_id"]); + if (parameters_.port_id < 30200 || parameters_.port_id > 30209) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209]. Found %d.", + parameters_.port_id); + return controller_interface::CallbackReturn::ERROR; + } + info_.hardware_parameters["remote_host"] == "INADDR_ANY" + ? parameters_.remote_host = NULL + : parameters_.remote_host = info_.hardware_parameters["remote_host"].c_str(); + 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); + 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); + 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_.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"]); + + // 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_position[idx] = + std::stod(system_info.joints[idx].parameters.at("max_position")); + command_guard_parameters.min_position[idx] = + std::stod(system_info.joints[idx].parameters.at("min_position")); + command_guard_parameters.max_velocity[idx] = + std::stod(system_info.joints[idx].parameters.at("max_velocity")); + command_guard_parameters.max_torque[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; + + async_client_ptr_ = std::make_shared( + pid_parameters, command_guard_parameters, parameters_.command_guard_variant, + state_interface_parameters, parameters_.open_loop); + app_ptr_ = std::make_unique(async_client_ptr_); + + nan_command_interfaces_(); + nan_state_interfaces_(); + nan_last_hw_states_(); + + // setup force-torque estimator + 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")); + ft_parameters_.force_x_th = std::stod(info_.sensors[1].parameters.at("force_x_th")); + ft_parameters_.force_y_th = std::stod(info_.sensors[1].parameters.at("force_y_th")); + ft_parameters_.force_z_th = std::stod(info_.sensors[1].parameters.at("force_z_th")); + 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::FTEstimator::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 (!verify_number_of_joints_()) { + return controller_interface::CallbackReturn::ERROR; + } + + if (!verify_joint_command_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + + if (!verify_joint_state_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + + if (!verify_sensors_()) { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +std::vector SystemInterface::export_state_interfaces() { + std::vector state_interfaces; + // state interfaces of type double + for (std::size_t i = 0; i < info_.joints.size(); ++i) { + state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &hw_lbr_state_.measured_joint_position[i]); + + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_JOINT_POSITION, + &hw_lbr_state_.commanded_joint_position[i]); + + state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, + &hw_lbr_state_.measured_torque[i]); + + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_TORQUE, + &hw_lbr_state_.commanded_torque[i]); + + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EXTERNAL_TORQUE, + &hw_lbr_state_.external_torque[i]); + + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_IPO_JOINT_POSITION, + &hw_lbr_state_.ipo_joint_position[i]); + + // additional velocity state interface + state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &hw_velocity_[i]); + } + + const auto &auxiliary_sensor = info_.sensors[0]; + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_SAMPLE_TIME, + &hw_lbr_state_.sample_time); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_TRACKING_PERFORMANCE, + &hw_lbr_state_.tracking_performance); + + // state interfaces that require cast + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_SESSION_STATE, &hw_session_state_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_CONNECTION_QUALITY, + &hw_connection_quality_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_SAFETY_STATE, &hw_safety_state_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_OPERATION_MODE, &hw_operation_mode_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_DRIVE_STATE, &hw_drive_state_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_CLIENT_COMMAND_MODE, + &hw_client_command_mode_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_OVERLAY_TYPE, &hw_overlay_type_); + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_CONTROL_MODE, &hw_control_mode_); + + state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_TIME_STAMP_SEC, &hw_time_stamp_sec_); + 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]); + + return state_interfaces; +} + +std::vector SystemInterface::export_command_interfaces() { + std::vector command_interfaces; + for (std::size_t i = 0; i < info_.joints.size(); ++i) { + command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &hw_lbr_command_.joint_position[i]); + + command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, + &hw_lbr_command_.torque[i]); + } + return command_interfaces; +} + +hardware_interface::return_type +SystemInterface::prepare_command_mode_switch(const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) { + return hardware_interface::return_type::OK; +} + +controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { + if (!async_client_ptr_) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); + return controller_interface::CallbackReturn::ERROR; + } + if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) { + return controller_interface::CallbackReturn::ERROR; + } + app_ptr_->run(parameters_.rt_prio); + uint8_t attempt = 0; + uint8_t max_attempts = 10; + while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", attempt + 1, max_attempts, + parameters_.port_id); + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (++attempt >= max_attempts) { + app_ptr_->close_udp_socket(); // hard close as run gets stuck + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to connect to robot on max attempts."); + return controller_interface::CallbackReturn::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Robot connected."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Control mode: '%s'.", + lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time: %.3f s / %.1f Hz.", + async_client_ptr_->get_state_interface().get_state().sample_time, + 1. / async_client_ptr_->get_state_interface().get_state().sample_time); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { + app_ptr_->stop_run(); + app_ptr_->close_udp_socket(); + return controller_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + hw_lbr_state_ = async_client_ptr_->get_state_interface().get_state(); + + // exit once robot exits COMMANDING_ACTIVE (for safety) + if (exit_commanding_active_(static_cast(hw_session_state_), + static_cast(hw_lbr_state_.session_state))) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); + app_ptr_->stop_run(); + app_ptr_->close_udp_socket(); + return hardware_interface::return_type::ERROR; + } + + // state interfaces that require cast + hw_session_state_ = static_cast(hw_lbr_state_.session_state); + hw_connection_quality_ = static_cast(hw_lbr_state_.connection_quality); + hw_safety_state_ = static_cast(hw_lbr_state_.safety_state); + hw_operation_mode_ = static_cast(hw_lbr_state_.operation_mode); + hw_drive_state_ = static_cast(hw_lbr_state_.drive_state); + hw_client_command_mode_ = static_cast(hw_lbr_state_.client_command_mode); + hw_overlay_type_ = static_cast(hw_lbr_state_.overlay_type); + hw_control_mode_ = static_cast(hw_lbr_state_.control_mode); + hw_time_stamp_sec_ = static_cast(hw_lbr_state_.time_stamp_sec); + hw_time_stamp_nano_sec_ = static_cast(hw_lbr_state_.time_stamp_nano_sec); + + // additional velocity state interface + compute_hw_velocity_(); + 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); + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) { + return hardware_interface::return_type::OK; + } + if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) { + if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(), + [](const double &v) { return std::isnan(v); })) { + return hardware_interface::return_type::OK; + } + async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); + return hardware_interface::return_type::OK; + } + if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) { + if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(), + [](const double &v) { return std::isnan(v); }) || + std::any_of(hw_lbr_command_.torque.cbegin(), hw_lbr_command_.torque.cend(), + [](const double &v) { return std::isnan(v); })) { + return hardware_interface::return_type::OK; + } + async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); + return hardware_interface::return_type::OK; + } + if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) { + throw std::runtime_error("Wrench command mode currently not implemented."); + } + return hardware_interface::return_type::ERROR; +} + +void SystemInterface::nan_command_interfaces_() { + hw_lbr_command_.joint_position.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_command_.torque.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_command_.wrench.fill(std::numeric_limits::quiet_NaN()); +} + +void SystemInterface::nan_state_interfaces_() { + // state interfaces of type double + hw_lbr_state_.measured_joint_position.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.commanded_joint_position.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.measured_torque.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.commanded_torque.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.external_torque.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.ipo_joint_position.fill(std::numeric_limits::quiet_NaN()); + hw_lbr_state_.sample_time = std::numeric_limits::quiet_NaN(); + hw_lbr_state_.tracking_performance = std::numeric_limits::quiet_NaN(); + + // state interfaces that require cast + hw_session_state_ = std::numeric_limits::quiet_NaN(); + hw_connection_quality_ = std::numeric_limits::quiet_NaN(); + hw_safety_state_ = std::numeric_limits::quiet_NaN(); + hw_operation_mode_ = std::numeric_limits::quiet_NaN(); + hw_drive_state_ = std::numeric_limits::quiet_NaN(); + hw_client_command_mode_ = std::numeric_limits::quiet_NaN(); + hw_overlay_type_ = std::numeric_limits::quiet_NaN(); + hw_control_mode_ = std::numeric_limits::quiet_NaN(); + hw_time_stamp_sec_ = std::numeric_limits::quiet_NaN(); + hw_time_stamp_nano_sec_ = std::numeric_limits::quiet_NaN(); + + // additional velocity state interface + hw_velocity_.fill(std::numeric_limits::quiet_NaN()); + + // additional force-torque state interface + hw_ft_.fill(std::numeric_limits::quiet_NaN()); +} + +bool SystemInterface::verify_number_of_joints_() { + if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected %d joints in URDF. Found %ld.", + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); + return false; + } + return true; +} + +bool SystemInterface::verify_joint_command_interfaces_() { + // check command interfaces + for (auto &joint : info_.joints) { + if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { + RCLCPP_ERROR( + rclcpp::get_logger(LOGGER_NAME), + "Joint %s received invalid number of command interfaces. Received %ld, expected %d.", + joint.name.c_str(), joint.command_interfaces.size(), LBR_FRI_COMMAND_INTERFACE_SIZE); + return false; + } + for (auto &ci : joint.command_interfaces) { + if (ci.name != hardware_interface::HW_IF_POSITION && + ci.name != hardware_interface::HW_IF_EFFORT) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Joint %s received invalid command interface: %s. Expected %s or %s.", + joint.name.c_str(), ci.name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_EFFORT); + return false; + } + } + } + return true; +} + +bool SystemInterface::verify_joint_state_interfaces_() { + // check state interfaces + for (auto &joint : info_.joints) { + if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { + RCLCPP_ERROR( + rclcpp::get_logger(LOGGER_NAME), + "Joint %s received invalid number of state interfaces. Received %ld, expected %d.", + joint.name.c_str(), joint.state_interfaces.size(), LBR_FRI_STATE_INTERFACE_SIZE); + return false; + } + for (auto &si : joint.state_interfaces) { + if (si.name != hardware_interface::HW_IF_POSITION && + si.name != HW_IF_COMMANDED_JOINT_POSITION && + si.name != hardware_interface::HW_IF_EFFORT && si.name != HW_IF_COMMANDED_TORQUE && + si.name != HW_IF_EXTERNAL_TORQUE && si.name != HW_IF_IPO_JOINT_POSITION && + si.name != hardware_interface::HW_IF_VELOCITY) { + RCLCPP_ERROR( + rclcpp::get_logger(LOGGER_NAME), + "Joint %s received invalid state interface: %s. Expected %s, %s, %s, %s, %s, %s or %s.", + joint.name.c_str(), si.name.c_str(), hardware_interface::HW_IF_POSITION, + HW_IF_COMMANDED_JOINT_POSITION, hardware_interface::HW_IF_EFFORT, + HW_IF_COMMANDED_TORQUE, HW_IF_EXTERNAL_TORQUE, HW_IF_IPO_JOINT_POSITION, + hardware_interface::HW_IF_VELOCITY); + return false; + } + } + } + return true; +} + +bool SystemInterface::verify_sensors_() { + // check lbr specific state interfaces + if (info_.sensors.size() != LBR_FRI_SENSORS) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected %d sensor, got %ld", LBR_FRI_SENSORS, + info_.sensors.size()); + return false; + } + if (!verify_auxiliary_sensor_()) { + return false; + } + if (!verify_estimated_ft_sensor_()) { + return false; + } + return true; +} + +bool SystemInterface::verify_auxiliary_sensor_() { + // check all interfaces are defined in config/lbr_system_interface.xacro + const auto &auxiliary_sensor = info_.sensors[0]; + if (auxiliary_sensor.state_interfaces.size() != AUXILIARY_SENSOR_SIZE) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Sensor %s received invalid state interface. Received %ld, expected %d. ", + auxiliary_sensor.name.c_str(), auxiliary_sensor.state_interfaces.size(), + AUXILIARY_SENSOR_SIZE); + return false; + } + // check only valid interfaces are defined + for (const auto &si : auxiliary_sensor.state_interfaces) { + if (si.name != HW_IF_SAMPLE_TIME && si.name != HW_IF_SESSION_STATE && + si.name != HW_IF_CONNECTION_QUALITY && si.name != HW_IF_SAFETY_STATE && + si.name != HW_IF_OPERATION_MODE && si.name != HW_IF_DRIVE_STATE && + si.name != HW_IF_CLIENT_COMMAND_MODE && si.name != HW_IF_OVERLAY_TYPE && + si.name != HW_IF_CONTROL_MODE && si.name != HW_IF_TIME_STAMP_SEC && + si.name != HW_IF_TIME_STAMP_NANO_SEC && si.name != HW_IF_COMMANDED_JOINT_POSITION && + si.name != HW_IF_COMMANDED_TORQUE && si.name != HW_IF_EXTERNAL_TORQUE && + si.name != HW_IF_IPO_JOINT_POSITION && si.name != HW_IF_TRACKING_PERFORMANCE) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Sensor %s received invalid state interface %s.", auxiliary_sensor.name.c_str(), + si.name.c_str()); + return false; + } + } + return true; +} + +bool SystemInterface::verify_estimated_ft_sensor_() { + const auto &estimated_ft_sensor = info_.sensors[1]; + if (estimated_ft_sensor.state_interfaces.size() != ESTIMATED_FT_SENSOR_SIZE) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Sensor %s received invalid state interface. Received %ld, expected %d. ", + estimated_ft_sensor.name.c_str(), estimated_ft_sensor.state_interfaces.size(), + ESTIMATED_FT_SENSOR_SIZE); + return false; + } + // check only valid interfaces are defined + for (const auto &si : estimated_ft_sensor.state_interfaces) { + if (si.name != HW_IF_FORCE_X && si.name != HW_IF_FORCE_Y && si.name != HW_IF_FORCE_Z && + si.name != HW_IF_TORQUE_X && si.name != HW_IF_TORQUE_Y && si.name != HW_IF_TORQUE_Z) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Sensor %s received invalid state interface %s.", + estimated_ft_sensor.name.c_str(), si.name.c_str()); + return false; + } + } + return true; +} + +bool SystemInterface::exit_commanding_active_( + const KUKA::FRI::ESessionState &previous_session_state, + const KUKA::FRI::ESessionState &session_state) { + if (previous_session_state == KUKA::FRI::ESessionState::COMMANDING_ACTIVE && + previous_session_state != session_state) { + return true; + } + return false; +} + +double SystemInterface::time_stamps_to_sec_(const double &sec, const double &nano_sec) const { + return sec + nano_sec / 1.e9; +} + +void SystemInterface::nan_last_hw_states_() { + last_hw_measured_joint_position_.fill(std::numeric_limits::quiet_NaN()); + last_hw_time_stamp_sec_ = std::numeric_limits::quiet_NaN(); + last_hw_time_stamp_nano_sec_ = std::numeric_limits::quiet_NaN(); +} + +void SystemInterface::update_last_hw_states_() { + last_hw_measured_joint_position_ = hw_lbr_state_.measured_joint_position; + last_hw_time_stamp_sec_ = hw_time_stamp_sec_; + last_hw_time_stamp_nano_sec_ = hw_time_stamp_nano_sec_; +} + +void SystemInterface::compute_hw_velocity_() { + // state uninitialized + if (std::isnan(last_hw_time_stamp_nano_sec_) || std::isnan(last_hw_measured_joint_position_[0])) { + return; + } + + // state wasn't updated + if (last_hw_time_stamp_sec_ == hw_time_stamp_sec_ && + last_hw_time_stamp_nano_sec_ == hw_time_stamp_nano_sec_) { + return; + } + + double dt = time_stamps_to_sec_(hw_time_stamp_sec_, hw_time_stamp_nano_sec_) - + time_stamps_to_sec_(last_hw_time_stamp_sec_, last_hw_time_stamp_nano_sec_); + std::size_t i = 0; + std::for_each(hw_velocity_.begin(), hw_velocity_.end(), [&](double &v) { + v = (hw_lbr_state_.measured_joint_position[i] - last_hw_measured_joint_position_[i]) / dt; + ++i; + }); +} + +} // end of namespace lbr_ros2_control + +#include + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::SystemInterface, hardware_interface::SystemInterface)