From baa8987cad8117b047299149dad8e04403d2f9d6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 21 Nov 2023 10:36:47 +0000 Subject: [PATCH 01/82] added placeholder files --- .../include/lbr_ros2_control/lbr_forward_position_command.hpp | 0 .../include/lbr_ros2_control/lbr_forward_torque_command.hpp | 0 .../include/lbr_ros2_control/lbr_forward_wrench_command.hpp | 0 lbr_ros2_control/src/lbr_forward_position_command.cpp | 0 lbr_ros2_control/src/lbr_forward_torque_command.cpp | 0 lbr_ros2_control/src/lbr_forward_wrench_command.cpp | 0 6 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command.hpp create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command.hpp create mode 100644 lbr_ros2_control/src/lbr_forward_position_command.cpp create mode 100644 lbr_ros2_control/src/lbr_forward_torque_command.cpp create mode 100644 lbr_ros2_control/src/lbr_forward_wrench_command.cpp diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp new file mode 100644 index 00000000..e69de29b diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command.hpp new file mode 100644 index 00000000..e69de29b diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command.hpp new file mode 100644 index 00000000..e69de29b diff --git a/lbr_ros2_control/src/lbr_forward_position_command.cpp b/lbr_ros2_control/src/lbr_forward_position_command.cpp new file mode 100644 index 00000000..e69de29b diff --git a/lbr_ros2_control/src/lbr_forward_torque_command.cpp b/lbr_ros2_control/src/lbr_forward_torque_command.cpp new file mode 100644 index 00000000..e69de29b diff --git a/lbr_ros2_control/src/lbr_forward_wrench_command.cpp b/lbr_ros2_control/src/lbr_forward_wrench_command.cpp new file mode 100644 index 00000000..e69de29b From d9be7b51e28f6e502e39a8be2fc9cafee61a2e5b Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 10:30:41 +0000 Subject: [PATCH 02/82] added the forward lbr position command controler --- lbr_ros2_control/CMakeLists.txt | 2 + lbr_ros2_control/config/lbr_controllers.yaml | 9 ++- .../lbr_forward_position_command.hpp | 0 ...br_forward_position_command_controller.hpp | 54 ++++++++++++++ .../lbr_forward_command_controllers.xml | 10 +++ .../src/lbr_forward_position_command.cpp | 0 ...br_forward_position_command_controller.cpp | 73 +++++++++++++++++++ 7 files changed, 146 insertions(+), 2 deletions(-) delete mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp create mode 100644 lbr_ros2_control/lbr_forward_command_controllers.xml delete mode 100644 lbr_ros2_control/src/lbr_forward_position_command.cpp create mode 100644 lbr_ros2_control/src/lbr_forward_position_command_controller.cpp diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 62ca0f77..2c8a60a6 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -36,6 +36,7 @@ add_library( ${PROJECT_NAME} SHARED src/lbr_estimated_ft_broadcaster.cpp + src/lbr_forward_position_command_controller.cpp src/lbr_state_broadcaster.cpp src/lbr_system_interface.cpp ) @@ -71,6 +72,7 @@ target_link_libraries(${PROJECT_NAME} ) pluginlib_export_plugin_description_file(controller_interface lbr_broadcasters.xml) +pluginlib_export_plugin_description_file(controller_interface lbr_forward_command_controllers.xml) pluginlib_export_plugin_description_file(hardware_interface lbr_system_interface.xml) # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 3c8a7042..f6232f28 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -4,23 +4,28 @@ ros__parameters: update_rate: 200 - # Broadcasters + # ROS 2 control broadcasters joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + # LBR ROS 2 control broadcasters lbr_state_broadcaster: type: lbr_ros2_control/LBRStateBroadcaster lbr_estimated_ft_broadcaster: type: lbr_ros2_control/LBREstimatedFTBroadcaster - # Controllers + # ROS 2 control controllers position_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: position_controllers/JointGroupPositionController + # LBR ROS 2 control controllers + forward_lbr_position_command_controller: + type: lbr_ros2_control/LBRForwardPositionCommandController + /**/lbr_estimated_ft_broadcaster: ros__parameters: damping: 0.2 diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command.hpp deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp new file mode 100644 index 00000000..21dddd84 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp @@ -0,0 +1,54 @@ +#ifndef LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ + +#include +#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 "friClientIf.h" +#include "friLBRState.h" + +#include "lbr_fri_msgs/msg/lbr_position_command.hpp" + +namespace lbr_ros2_control { +class LBRForwardPositionCommandController : public controller_interface::ControllerInterface { +public: + LBRForwardPositionCommandController(); + + 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__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/lbr_forward_command_controllers.xml b/lbr_ros2_control/lbr_forward_command_controllers.xml new file mode 100644 index 00000000..26e1c620 --- /dev/null +++ b/lbr_ros2_control/lbr_forward_command_controllers.xml @@ -0,0 +1,10 @@ + + + + + Forward command controller for LBRPositionCommand message, see + lbr_fri_msgs/msg/LBRPositionCommand.msg. + + \ No newline at end of file diff --git a/lbr_ros2_control/src/lbr_forward_position_command.cpp b/lbr_ros2_control/src/lbr_forward_position_command.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp b/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp new file mode 100644 index 00000000..3cbf19c3 --- /dev/null +++ b/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp @@ -0,0 +1,73 @@ +#include "lbr_ros2_control/lbr_forward_position_command_controller.hpp" + +namespace lbr_ros2_control { +LBRForwardPositionCommandController::LBRForwardPositionCommandController() + : rt_lbr_position_command_ptr_(nullptr), lbr_position_command_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +LBRForwardPositionCommandController::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 +LBRForwardPositionCommandController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn LBRForwardPositionCommandController::on_init() { + try { + lbr_position_command_subscription_ptr_ = + this->get_node()->create_subscription( + "command/position", rclcpp::SystemDefaultsQoS(), + [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 LBR forward position command with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +LBRForwardPositionCommandController::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 LBRForwardPositionCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn LBRForwardPositionCommandController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn LBRForwardPositionCommandController::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::LBRForwardPositionCommandController, + controller_interface::ControllerInterface) From a527c4b3c5e18aa2e747d229d893dcc201a8f25e Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 10:30:54 +0000 Subject: [PATCH 03/82] added missing includes --- .../lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp | 5 ++++- .../include/lbr_ros2_control/lbr_state_broadcaster.hpp | 4 +++- 2 files changed, 7 insertions(+), 2 deletions(-) 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 index e179f61f..8d57f263 100644 --- 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 @@ -1,11 +1,13 @@ #ifndef LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ #define LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ +#include #include #include #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" @@ -67,7 +69,8 @@ class LBREstimatedFTBroadcaster : public controller_interface::ControllerInterfa double damping_; std::string end_effector_link_ = "link_ee"; - std::array joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; std::vector> joint_position_interfaces_, external_joint_torque_interfaces_; 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..93043c07 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" @@ -45,7 +47,7 @@ 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_; From 31d121cda79e8ddf067eb8c7fd3c86dc153a8dc3 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 10:31:08 +0000 Subject: [PATCH 04/82] re-named placeholder files --- ...rque_command.hpp => lbr_forward_torque_command_controller.hpp} | 0 ...ench_command.hpp => lbr_forward_wrench_command_controller.hpp} | 0 ...rque_command.cpp => lbr_forward_torque_command_controller.cpp} | 0 ...ench_command.cpp => lbr_forward_wrench_command_controller.cpp} | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_forward_torque_command.hpp => lbr_forward_torque_command_controller.hpp} (100%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_forward_wrench_command.hpp => lbr_forward_wrench_command_controller.hpp} (100%) rename lbr_ros2_control/src/{lbr_forward_torque_command.cpp => lbr_forward_torque_command_controller.cpp} (100%) rename lbr_ros2_control/src/{lbr_forward_wrench_command.cpp => lbr_forward_wrench_command_controller.cpp} (100%) diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp similarity index 100% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command.hpp rename to lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command_controller.hpp similarity index 100% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command.hpp rename to lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command_controller.hpp diff --git a/lbr_ros2_control/src/lbr_forward_torque_command.cpp b/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp similarity index 100% rename from lbr_ros2_control/src/lbr_forward_torque_command.cpp rename to lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp diff --git a/lbr_ros2_control/src/lbr_forward_wrench_command.cpp b/lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp similarity index 100% rename from lbr_ros2_control/src/lbr_forward_wrench_command.cpp rename to lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp From df39851f1e67f0a41816978c11e85bb4f9a20abe Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 10:41:38 +0000 Subject: [PATCH 05/82] added a changelog file --- CHANGELOG.rst | 63 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 CHANGELOG.rst diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 00000000..9115541a --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package LBR FRI ROS 2 Stack +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 + +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 From 2160972070cb5c3127d1c35173e3c8c319ed084b Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 11:01:22 +0000 Subject: [PATCH 06/82] simplified links --- CHANGELOG.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9115541a..5a712423 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -3,9 +3,9 @@ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 +* v1.3.0 Gazebo namespace fixes in #123 +* Fix iiwa ee link in #126 +* Humble v.1.3.1 in #130 Humble v1.3.0 beta (2023-10-03) ------------------------------- @@ -16,7 +16,7 @@ Humble v1.3.0 beta (2023-10-03) * 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 +* Refers to #121 Humble v1.2.5 (2023-09-08) -------------------------- @@ -35,7 +35,7 @@ Humble v1.2.4 (2023-08-09) 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 +* Addresses some of #85 * Give command guard only logger interface * Fix open loop bug * Adds real-time priority via rt_prio parameter @@ -53,7 +53,7 @@ Humble v1.2.1 (2023-08-04) Humble v1.2.0 (2023-08-03) -------------------------- -* Re-introduces MoveIt, refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/52 +* Re-introduces MoveIt, refer to #52 * Moves demo prefix to front for improved package overview * Single node for hardware interface * Static executors where possible From fd4de43f67efb02ef58b05d27d89395cadbc3733 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 11:02:31 +0000 Subject: [PATCH 07/82] reverted to links --- CHANGELOG.rst | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5a712423..9115541a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -3,9 +3,9 @@ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Humble v1.3.1 (2023-11-21) -------------------------- -* v1.3.0 Gazebo namespace fixes in #123 -* Fix iiwa ee link in #126 -* Humble v.1.3.1 in #130 +* 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 Humble v1.3.0 beta (2023-10-03) ------------------------------- @@ -16,7 +16,7 @@ Humble v1.3.0 beta (2023-10-03) * 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 #121 +* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/121 Humble v1.2.5 (2023-09-08) -------------------------- @@ -35,7 +35,7 @@ Humble v1.2.4 (2023-08-09) 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 #85 +* 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 @@ -53,7 +53,7 @@ Humble v1.2.1 (2023-08-04) Humble v1.2.0 (2023-08-03) -------------------------- -* Re-introduces MoveIt, refer to #52 +* 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 From 884120af21704fb0b5748b58f9c1cee14cb3064f Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 11:03:27 +0000 Subject: [PATCH 08/82] added full log hint --- CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9115541a..4c1574e6 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,6 +6,7 @@ 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) ------------------------------- From 1286d8d90ffe8815b80f2aac43d5b6b01628d3c2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 12:25:22 +0000 Subject: [PATCH 09/82] added a forward torque command controller --- lbr_ros2_control/CMakeLists.txt | 1 + .../lbr_forward_torque_command_controller.hpp | 60 ++++++++++ .../lbr_forward_torque_command_controller.cpp | 111 ++++++++++++++++++ 3 files changed, 172 insertions(+) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 2c8a60a6..199ef964 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -37,6 +37,7 @@ add_library( SHARED src/lbr_estimated_ft_broadcaster.cpp src/lbr_forward_position_command_controller.cpp + src/lbr_forward_torque_command_controller.cpp src/lbr_state_broadcaster.cpp src/lbr_system_interface.cpp ) diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp index e69de29b..0635dcff 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp @@ -0,0 +1,60 @@ +#ifndef LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__LBR_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 LBRForwardTorqueCommandController : public controller_interface::ControllerInterface { +public: + LBRForwardTorqueCommandController(); + + 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__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp index e69de29b..9d221afd 100644 --- a/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp @@ -0,0 +1,111 @@ +#include "lbr_ros2_control/lbr_forward_torque_command_controller.hpp" + +namespace lbr_ros2_control { +LBRForwardTorqueCommandController::LBRForwardTorqueCommandController() + : rt_lbr_torque_command_ptr_(nullptr), lbr_torque_command_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +LBRForwardTorqueCommandController::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 +LBRForwardTorqueCommandController::state_interface_configuration() const { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_init() { + try { + lbr_torque_command_subscription_ptr_ = + this->get_node()->create_subscription( + "command/torque", rclcpp::SystemDefaultsQoS(), + [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 LBR forward torque command controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +LBRForwardTorqueCommandController::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 LBRForwardTorqueCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +LBRForwardTorqueCommandController::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 LBRForwardTorqueCommandController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + clear_command_interfaces_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool LBRForwardTorqueCommandController::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 LBRForwardTorqueCommandController::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::LBRForwardTorqueCommandController, + controller_interface::ControllerInterface) From 5a0954802b640add14192c1f006e33a067b38f7f Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 12:27:47 +0000 Subject: [PATCH 10/82] cleaned up --- .../src/admittance_control_node.cpp | 2 +- .../admittance_control_node.py | 2 +- .../src/joint_sine_overlay_node.cpp | 6 ++-- .../joint_sine_overlay_node.py | 4 +-- lbr_fri_ros2/src/app_component.cpp | 2 +- .../lbr_estimated_ft_broadcaster.hpp | 2 +- ...br_forward_position_command_controller.hpp | 4 +-- .../src/lbr_estimated_ft_broadcaster.cpp | 36 ++++++++++--------- ...br_forward_position_command_controller.cpp | 5 +-- 9 files changed, 32 insertions(+), 31 deletions(-) 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..19435615 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 @@ -31,7 +31,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_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..70bb108d 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,7 @@ 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_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..74c43d9d 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,9 @@ 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_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 82a73046..8cfbce05 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -85,7 +85,7 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, 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: 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 index 8d57f263..c962c486 100644 --- 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 @@ -73,7 +73,7 @@ class LBREstimatedFTBroadcaster : public controller_interface::ControllerInterfa "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; std::vector> - joint_position_interfaces_, external_joint_torque_interfaces_; + joint_position_state_interfaces_, external_joint_torque_state_interfaces_; rclcpp::Publisher::SharedPtr wrench_stamped_publisher_ptr_; std::shared_ptr> diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp index 21dddd84..7c60020f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp @@ -1,19 +1,17 @@ #ifndef LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ #define LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#include #include -#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 "friClientIf.h" #include "friLBRState.h" #include "lbr_fri_msgs/msg/lbr_position_command.hpp" diff --git a/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp index 2f8a7ad7..148df897 100644 --- a/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp @@ -55,12 +55,12 @@ 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())) { + if (std::isnan(joint_position_state_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(); + joint_positions_(i) = joint_position_state_interfaces_[i].get().get_value(); + external_joint_torques_(i) = external_joint_torque_state_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_); @@ -118,33 +118,35 @@ void LBREstimatedFTBroadcaster::init_states_() { 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)); + joint_position_state_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)); + external_joint_torque_state_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) { + if (joint_position_state_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 " + "Number of joint position state interfaces (%ld) does not match the number of joints " "in the robot (%d).", - external_joint_torque_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return false; + } + if (external_joint_torque_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of external joint torque state interfaces (%ld) does not match the number " + "of joints " + "in the robot (%d).", + external_joint_torque_state_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(); + joint_position_state_interfaces_.clear(); + external_joint_torque_state_interfaces_.clear(); } template diff --git a/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp b/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp index 3cbf19c3..e375a603 100644 --- a/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp @@ -24,13 +24,14 @@ controller_interface::CallbackReturn LBRForwardPositionCommandController::on_ini try { lbr_position_command_subscription_ptr_ = this->get_node()->create_subscription( - "command/position", rclcpp::SystemDefaultsQoS(), + "command/joint_position", rclcpp::SystemDefaultsQoS(), [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 LBR forward position command with: %s.", e.what()); + "Failed to initialize LBR forward position command controller with: %s.", + e.what()); return controller_interface::CallbackReturn::ERROR; } From 44e7e20a0d343aa8f7b852f9b02f37d28505c31e Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 12:39:37 +0000 Subject: [PATCH 11/82] added forward torque controller to config --- lbr_ros2_control/config/lbr_controllers.yaml | 3 +++ lbr_ros2_control/lbr_forward_command_controllers.xml | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index f6232f28..0021978b 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -26,6 +26,9 @@ forward_lbr_position_command_controller: type: lbr_ros2_control/LBRForwardPositionCommandController + forward_lbr_torque_command_controller: + type: lbr_ros2_control/LBRForwardTorqueCommandController + /**/lbr_estimated_ft_broadcaster: ros__parameters: damping: 0.2 diff --git a/lbr_ros2_control/lbr_forward_command_controllers.xml b/lbr_ros2_control/lbr_forward_command_controllers.xml index 26e1c620..2420041e 100644 --- a/lbr_ros2_control/lbr_forward_command_controllers.xml +++ b/lbr_ros2_control/lbr_forward_command_controllers.xml @@ -7,4 +7,12 @@ 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 From ba8e5772beb809addc403c9e3c9fb7f759f29c52 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 14:22:58 +0000 Subject: [PATCH 12/82] removed log --- lbr_ros2_control/src/lbr_state_broadcaster.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index 5a220649..ed69eb68 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -45,8 +45,6 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time } // check any for nan if (std::isnan(state_interface_map_[joint_names_[0]][hardware_interface::HW_IF_POSITION])) { - RCLCPP_INFO(this->get_node()->get_logger(), - "LBRStateBroadcaster: joint position is nan, skipping publish."); return controller_interface::return_type::OK; } if (rt_state_publisher_ptr_->trylock()) { From 79925512f63d08f98c181f515efb68cfa48c6617 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 19:14:49 +0000 Subject: [PATCH 13/82] removed wrench command controller for now --- .../lbr_ros2_control/lbr_forward_wrench_command_controller.hpp | 0 lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command_controller.hpp delete mode 100644 lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_wrench_command_controller.hpp deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp b/lbr_ros2_control/src/lbr_forward_wrench_command_controller.cpp deleted file mode 100644 index e69de29b..00000000 From a51678b1df59773123dcb2bed4ceee10af2c03c2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 19:15:36 +0000 Subject: [PATCH 14/82] added torque command handling --- lbr_ros2_control/src/lbr_system_interface.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/lbr_ros2_control/src/lbr_system_interface.cpp b/lbr_ros2_control/src/lbr_system_interface.cpp index 399aee6f..87d5572c 100644 --- a/lbr_ros2_control/src/lbr_system_interface.cpp +++ b/lbr_ros2_control/src/lbr_system_interface.cpp @@ -227,6 +227,23 @@ hardware_interface::return_type LBRSystemInterface::write(const rclcpp::Time & / client_ptr_->get_command_interface().set_command_target(lbr_command_); return hardware_interface::return_type::OK; } + if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) { + if (std::any_of(hw_position_command_.cbegin(), hw_position_command_.cend(), + [](const double &v) { return std::isnan(v); }) || + std::any_of(hw_effort_command_.cbegin(), hw_effort_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); + std::memcpy(lbr_command_.torque.data(), hw_effort_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; + } + if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) { + throw std::runtime_error("Wrench command mode not implemented."); + } return hardware_interface::return_type::ERROR; } From 44a341b9a434dc1837d4451759e8a5b229f1bc7f Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 19:37:21 +0000 Subject: [PATCH 15/82] removed redundant lbr prefix --- lbr_bringup/launch/real.launch.py | 6 +-- .../ros2_control/lbr.ros2_control.xacro | 2 +- lbr_ros2_control/CMakeLists.txt | 14 ++--- ...{lbr_broadcasters.xml => broadcasters.xml} | 4 +- lbr_ros2_control/config/lbr_controllers.yaml | 10 ++-- ...rs.xml => forward_command_controllers.xml} | 8 +-- ...aster.hpp => estimated_ft_broadcaster.hpp} | 12 ++--- ...rward_lbr_position_command_controller.hpp} | 10 ++-- ...forward_lbr_torque_command_controller.hpp} | 10 ++-- .../lbr_state_broadcaster.hpp | 5 +- ...tem_interface.hpp => system_interface.hpp} | 12 ++--- ...s.hpp => system_interface_type_values.hpp} | 6 +-- ...aster.cpp => estimated_ft_broadcaster.cpp} | 34 ++++++------ ...rward_lbr_position_command_controller.cpp} | 22 ++++---- ...forward_lbr_torque_command_controller.cpp} | 26 ++++----- ...tem_interface.cpp => system_interface.cpp} | 53 +++++++++---------- ...tem_interface.xml => system_interface.xml} | 2 +- 17 files changed, 117 insertions(+), 119 deletions(-) rename lbr_ros2_control/{lbr_broadcasters.xml => broadcasters.xml} (84%) rename lbr_ros2_control/{lbr_forward_command_controllers.xml => forward_command_controllers.xml} (72%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_estimated_ft_broadcaster.hpp => estimated_ft_broadcaster.hpp} (88%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_forward_position_command_controller.hpp => forward_lbr_position_command_controller.hpp} (83%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_forward_torque_command_controller.hpp => forward_lbr_torque_command_controller.hpp} (85%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_system_interface.hpp => system_interface.hpp} (92%) rename lbr_ros2_control/include/lbr_ros2_control/{lbr_system_interface_type_values.hpp => system_interface_type_values.hpp} (87%) rename lbr_ros2_control/src/{lbr_estimated_ft_broadcaster.cpp => estimated_ft_broadcaster.cpp} (85%) rename lbr_ros2_control/src/{lbr_forward_position_command_controller.cpp => forward_lbr_position_command_controller.cpp} (78%) rename lbr_ros2_control/src/{lbr_forward_torque_command_controller.cpp => forward_lbr_torque_command_controller.cpp} (83%) rename lbr_ros2_control/src/{lbr_system_interface.cpp => system_interface.cpp} (90%) rename lbr_ros2_control/{lbr_system_interface.xml => system_interface.xml} (75%) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index b64d86b4..3d749f40 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -32,8 +32,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - lbr_estimated_ft_broadcast = LBRROS2ControlMixin.node_controller_spawner( - controller="lbr_estimated_ft_broadcaster" + estimated_ft_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="estimated_ft_broadcaster" ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") @@ -45,7 +45,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: on_start=[ joint_state_broadcaster, lbr_state_broadcaster, - lbr_estimated_ft_broadcast, + estimated_ft_broadcaster, controller, ], ) diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index 184c2de8..d237b592 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -22,7 +22,7 @@ - lbr_ros2_control::LBRSystemInterface + lbr_ros2_control::SystemInterface ${port_id} ${remote_host} ${rt_prio} diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 199ef964..61f10e64 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -35,11 +35,11 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED - src/lbr_estimated_ft_broadcaster.cpp - src/lbr_forward_position_command_controller.cpp - src/lbr_forward_torque_command_controller.cpp + src/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 @@ -72,9 +72,9 @@ target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient ) -pluginlib_export_plugin_description_file(controller_interface lbr_broadcasters.xml) -pluginlib_export_plugin_description_file(controller_interface lbr_forward_command_controllers.xml) -pluginlib_export_plugin_description_file(hardware_interface lbr_system_interface.xml) +pluginlib_export_plugin_description_file(controller_interface broadcasters.xml) +pluginlib_export_plugin_description_file(controller_interface forward_command_controllers.xml) +pluginlib_export_plugin_description_file(hardware_interface system_interface.xml) # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html ament_export_targets( diff --git a/lbr_ros2_control/lbr_broadcasters.xml b/lbr_ros2_control/broadcasters.xml similarity index 84% rename from lbr_ros2_control/lbr_broadcasters.xml rename to lbr_ros2_control/broadcasters.xml index 45a1d97b..8cbb5ffc 100644 --- a/lbr_ros2_control/lbr_broadcasters.xml +++ b/lbr_ros2_control/broadcasters.xml @@ -7,8 +7,8 @@ - Broadcaster for end-effector force-torque as estimated from external joint torques. diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 0021978b..6af537f5 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -12,8 +12,8 @@ lbr_state_broadcaster: type: lbr_ros2_control/LBRStateBroadcaster - lbr_estimated_ft_broadcaster: - type: lbr_ros2_control/LBREstimatedFTBroadcaster + estimated_ft_broadcaster: + type: lbr_ros2_control/EstimatedFTBroadcaster # ROS 2 control controllers position_trajectory_controller: @@ -24,12 +24,12 @@ # LBR ROS 2 control controllers forward_lbr_position_command_controller: - type: lbr_ros2_control/LBRForwardPositionCommandController + type: lbr_ros2_control/ForwardLBRPositionCommandController forward_lbr_torque_command_controller: - type: lbr_ros2_control/LBRForwardTorqueCommandController + type: lbr_ros2_control/ForwardLBRTorqueCommandController -/**/lbr_estimated_ft_broadcaster: +/**/estimated_ft_broadcaster: ros__parameters: damping: 0.2 diff --git a/lbr_ros2_control/lbr_forward_command_controllers.xml b/lbr_ros2_control/forward_command_controllers.xml similarity index 72% rename from lbr_ros2_control/lbr_forward_command_controllers.xml rename to lbr_ros2_control/forward_command_controllers.xml index 2420041e..dd26c929 100644 --- a/lbr_ros2_control/lbr_forward_command_controllers.xml +++ b/lbr_ros2_control/forward_command_controllers.xml @@ -1,16 +1,16 @@ - 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. diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp similarity index 88% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp rename to lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp index c962c486..84d12d20 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ -#define LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ +#ifndef LBR_ROS2_CONTROL__ESTIMATED_FT_BROADCASTER_HPP_ +#define LBR_ROS2_CONTROL__ESTIMATED_FT_BROADCASTER_HPP_ #include #include @@ -23,12 +23,12 @@ #include "friClientIf.h" #include "friLBRState.h" -#include "lbr_ros2_control/lbr_system_interface_type_values.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -class LBREstimatedFTBroadcaster : public controller_interface::ControllerInterface { +class EstimatedFTBroadcaster : public controller_interface::ControllerInterface { public: - LBREstimatedFTBroadcaster(); + EstimatedFTBroadcaster(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -80,4 +80,4 @@ class LBREstimatedFTBroadcaster : public controller_interface::ControllerInterfa rt_wrench_stamped_publisher_ptr_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ +#endif // LBR_ROS2_CONTROL__ESTIMATED_FT_BROADCASTER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp similarity index 83% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp index 7c60020f..e769b459 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_position_command_controller.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ -#define LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#ifndef LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ #include #include @@ -17,9 +17,9 @@ #include "lbr_fri_msgs/msg/lbr_position_command.hpp" namespace lbr_ros2_control { -class LBRForwardPositionCommandController : public controller_interface::ControllerInterface { +class ForwardLBRPositionCommandController : public controller_interface::ControllerInterface { public: - LBRForwardPositionCommandController(); + ForwardLBRPositionCommandController(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -49,4 +49,4 @@ class LBRForwardPositionCommandController : public controller_interface::Control lbr_position_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ +#endif // LBR_ROS2_CONTROL__FORWARD_POSITION_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp similarity index 85% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp index 0635dcff..adad4304 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/forward_lbr_torque_command_controller.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ -#define LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#ifndef LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ #include #include @@ -19,9 +19,9 @@ #include "lbr_fri_msgs/msg/lbr_torque_command.hpp" namespace lbr_ros2_control { -class LBRForwardTorqueCommandController : public controller_interface::ControllerInterface { +class ForwardLBRTorqueCommandController : public controller_interface::ControllerInterface { public: - LBRForwardTorqueCommandController(); + ForwardLBRTorqueCommandController(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -57,4 +57,4 @@ class LBRForwardTorqueCommandController : public controller_interface::Controlle lbr_torque_command_subscription_ptr_; }; } // end of namespace lbr_ros2_control -#endif // LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_ +#endif // LBR_ROS2_CONTROL__FORWARD_TORQUE_COMMAND_CONTROLLER_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 93043c07..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 @@ -18,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 { @@ -47,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 92% 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..ebbc1521 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,5 +1,5 @@ -#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 @@ -23,12 +23,12 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/client.hpp" #include "lbr_fri_ros2/enum_maps.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 LBRSystemInterface : public hardware_interface::SystemInterface { +class SystemInterface : public hardware_interface::SystemInterface { public: - LBRSystemInterface() = default; + SystemInterface() = default; // hardware interface controller_interface::CallbackReturn @@ -119,4 +119,4 @@ class LBRSystemInterface : public hardware_interface::SystemInterface { bool open_loop_; }; } // 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 87% 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..86e4ef63 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 @@ -29,4 +29,4 @@ constexpr char HW_IF_TRACKING_PERFORMANCE[] = "tracking_performance"; // 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/src/lbr_estimated_ft_broadcaster.cpp b/lbr_ros2_control/src/estimated_ft_broadcaster.cpp similarity index 85% rename from lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp rename to lbr_ros2_control/src/estimated_ft_broadcaster.cpp index 148df897..fc095687 100644 --- a/lbr_ros2_control/src/lbr_estimated_ft_broadcaster.cpp +++ b/lbr_ros2_control/src/estimated_ft_broadcaster.cpp @@ -1,18 +1,18 @@ -#include "lbr_ros2_control/lbr_estimated_ft_broadcaster.hpp" +#include "lbr_ros2_control/estimated_ft_broadcaster.hpp" namespace lbr_ros2_control { -LBREstimatedFTBroadcaster::LBREstimatedFTBroadcaster() +EstimatedFTBroadcaster::EstimatedFTBroadcaster() : 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 { +EstimatedFTBroadcaster::command_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } controller_interface::InterfaceConfiguration -LBREstimatedFTBroadcaster::state_interface_configuration() const { +EstimatedFTBroadcaster::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_) { @@ -22,7 +22,7 @@ LBREstimatedFTBroadcaster::state_interface_configuration() const { return interface_configuration; } -controller_interface::CallbackReturn LBREstimatedFTBroadcaster::on_init() { +controller_interface::CallbackReturn EstimatedFTBroadcaster::on_init() { try { wrench_stamped_publisher_ptr_ = this->get_node()->create_publisher( @@ -45,15 +45,14 @@ controller_interface::CallbackReturn LBREstimatedFTBroadcaster::on_init() { } } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize LBR virtual FT broadcaster with: %s.", e.what()); + "Failed to initialize estimated 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*/) { +EstimatedFTBroadcaster::update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // check any for nan if (std::isnan(joint_position_state_interfaces_[0].get().get_value())) { return controller_interface::return_type::OK; @@ -81,12 +80,12 @@ LBREstimatedFTBroadcaster::update(const rclcpp::Time & /*time*/, } controller_interface::CallbackReturn -LBREstimatedFTBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { +EstimatedFTBroadcaster::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*/) { +EstimatedFTBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { init_states_(); if (!reference_state_interfaces_()) { return controller_interface::CallbackReturn::ERROR; @@ -95,12 +94,12 @@ LBREstimatedFTBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previou } controller_interface::CallbackReturn -LBREstimatedFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { +EstimatedFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { clear_state_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -void LBREstimatedFTBroadcaster::init_states_() { +void EstimatedFTBroadcaster::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()); @@ -115,7 +114,7 @@ void LBREstimatedFTBroadcaster::init_states_() { rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5); } -bool LBREstimatedFTBroadcaster::reference_state_interfaces_() { +bool EstimatedFTBroadcaster::reference_state_interfaces_() { for (auto &state_interface : state_interfaces_) { if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); @@ -144,16 +143,15 @@ bool LBREstimatedFTBroadcaster::reference_state_interfaces_() { return true; } -void LBREstimatedFTBroadcaster::clear_state_interfaces_() { +void EstimatedFTBroadcaster::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); external_joint_torque_state_interfaces_.clear(); } template Eigen::Matrix -LBREstimatedFTBroadcaster::damped_least_squares_( - const MatT &mat, - typename MatT::Scalar lambda) // choose appropriately +EstimatedFTBroadcaster::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); @@ -171,5 +169,5 @@ LBREstimatedFTBroadcaster::damped_least_squares_( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBREstimatedFTBroadcaster, +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::EstimatedFTBroadcaster, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp similarity index 78% rename from lbr_ros2_control/src/lbr_forward_position_command_controller.cpp rename to lbr_ros2_control/src/forward_lbr_position_command_controller.cpp index e375a603..df869106 100644 --- a/lbr_ros2_control/src/lbr_forward_position_command_controller.cpp +++ b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp @@ -1,11 +1,11 @@ -#include "lbr_ros2_control/lbr_forward_position_command_controller.hpp" +#include "lbr_ros2_control/forward_lbr_position_command_controller.hpp" namespace lbr_ros2_control { -LBRForwardPositionCommandController::LBRForwardPositionCommandController() +ForwardLBRPositionCommandController::ForwardLBRPositionCommandController() : rt_lbr_position_command_ptr_(nullptr), lbr_position_command_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration -LBRForwardPositionCommandController::command_interface_configuration() const { +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_) { @@ -15,12 +15,12 @@ LBRForwardPositionCommandController::command_interface_configuration() const { } controller_interface::InterfaceConfiguration -LBRForwardPositionCommandController::state_interface_configuration() const { +ForwardLBRPositionCommandController::state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn LBRForwardPositionCommandController::on_init() { +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_init() { try { lbr_position_command_subscription_ptr_ = this->get_node()->create_subscription( @@ -30,7 +30,7 @@ controller_interface::CallbackReturn LBRForwardPositionCommandController::on_ini }); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize LBR forward position command controller with: %s.", + "Failed to initialize forward LBR position command controller with: %s.", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -39,7 +39,7 @@ controller_interface::CallbackReturn LBRForwardPositionCommandController::on_ini } controller_interface::return_type -LBRForwardPositionCommandController::update(const rclcpp::Time & /*time*/, +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)) { @@ -52,17 +52,17 @@ LBRForwardPositionCommandController::update(const rclcpp::Time & /*time*/, return controller_interface::return_type::OK; } -controller_interface::CallbackReturn LBRForwardPositionCommandController::on_configure( +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn LBRForwardPositionCommandController::on_activate( +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn LBRForwardPositionCommandController::on_deactivate( +controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } @@ -70,5 +70,5 @@ controller_interface::CallbackReturn LBRForwardPositionCommandController::on_dea #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRForwardPositionCommandController, +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRPositionCommandController, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp similarity index 83% rename from lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp rename to lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp index 9d221afd..135d7545 100644 --- a/lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp +++ b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp @@ -1,11 +1,11 @@ -#include "lbr_ros2_control/lbr_forward_torque_command_controller.hpp" +#include "lbr_ros2_control/forward_lbr_torque_command_controller.hpp" namespace lbr_ros2_control { -LBRForwardTorqueCommandController::LBRForwardTorqueCommandController() +ForwardLBRTorqueCommandController::ForwardLBRTorqueCommandController() : rt_lbr_torque_command_ptr_(nullptr), lbr_torque_command_subscription_ptr_(nullptr) {} controller_interface::InterfaceConfiguration -LBRForwardTorqueCommandController::command_interface_configuration() const { +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_) { @@ -16,12 +16,12 @@ LBRForwardTorqueCommandController::command_interface_configuration() const { } controller_interface::InterfaceConfiguration -LBRForwardTorqueCommandController::state_interface_configuration() const { +ForwardLBRTorqueCommandController::state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_init() { +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init() { try { lbr_torque_command_subscription_ptr_ = this->get_node()->create_subscription( @@ -31,7 +31,7 @@ controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_init( }); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), - "Failed to initialize LBR forward torque command controller with: %s.", e.what()); + "Failed to initialize forward LBR torque command controller with: %s.", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -39,7 +39,7 @@ controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_init( } controller_interface::return_type -LBRForwardTorqueCommandController::update(const rclcpp::Time & /*time*/, +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)) { @@ -53,26 +53,26 @@ LBRForwardTorqueCommandController::update(const rclcpp::Time & /*time*/, return controller_interface::return_type::OK; } -controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_configure( +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -LBRForwardTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { +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 LBRForwardTorqueCommandController::on_deactivate( +controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { clear_command_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool LBRForwardTorqueCommandController::reference_command_interfaces_() { +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)); @@ -99,7 +99,7 @@ bool LBRForwardTorqueCommandController::reference_command_interfaces_() { return true; } -void LBRForwardTorqueCommandController::clear_command_interfaces_() { +void ForwardLBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } @@ -107,5 +107,5 @@ void LBRForwardTorqueCommandController::clear_command_interfaces_() { #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRForwardTorqueCommandController, +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::ForwardLBRTorqueCommandController, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/lbr_system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp similarity index 90% rename from lbr_ros2_control/src/lbr_system_interface.cpp rename to lbr_ros2_control/src/system_interface.cpp index 87d5572c..3595feb5 100644 --- a/lbr_ros2_control/src/lbr_system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -1,11 +1,11 @@ -#include "lbr_ros2_control/lbr_system_interface.hpp" +#include "lbr_ros2_control/system_interface.hpp" namespace lbr_ros2_control { controller_interface::CallbackReturn -LBRSystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { +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(app_node_ptr_->get_logger(), "Failed to initialize LBRSystemInterface."); + RCLCPP_ERROR(app_node_ptr_->get_logger(), "Failed to initialize SystemInterface."); return ret; } @@ -60,7 +60,7 @@ LBRSystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) return controller_interface::CallbackReturn::SUCCESS; } -std::vector LBRSystemInterface::export_state_interfaces() { +std::vector SystemInterface::export_state_interfaces() { std::vector state_interfaces; const auto &lbr_fri_sensor = info_.sensors[0]; @@ -109,7 +109,7 @@ std::vector LBRSystemInterface::export_state return state_interfaces; } -std::vector LBRSystemInterface::export_command_interfaces() { +std::vector SystemInterface::export_command_interfaces() { std::vector command_interfaces; for (std::size_t i = 0; i < info_.joints.size(); ++i) { @@ -123,14 +123,13 @@ std::vector LBRSystemInterface::export_com return command_interfaces; } -hardware_interface::return_type LBRSystemInterface::prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_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 -LBRSystemInterface::on_activate(const rclcpp_lifecycle::State &) { +controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { if (!client_ptr_) { RCLCPP_ERROR(app_node_ptr_->get_logger(), "Client not configured."); return controller_interface::CallbackReturn::ERROR; @@ -162,14 +161,14 @@ LBRSystemInterface::on_activate(const rclcpp_lifecycle::State &) { } controller_interface::CallbackReturn -LBRSystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { +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 LBRSystemInterface::read(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +hardware_interface::return_type SystemInterface::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))) { @@ -212,8 +211,8 @@ hardware_interface::return_type LBRSystemInterface::read(const rclcpp::Time & /* return hardware_interface::return_type::OK; } -hardware_interface::return_type LBRSystemInterface::write(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +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; } @@ -247,12 +246,12 @@ hardware_interface::return_type LBRSystemInterface::write(const rclcpp::Time & / return hardware_interface::return_type::ERROR; } -void LBRSystemInterface::nan_command_interfaces_() { +void SystemInterface::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_() { +void SystemInterface::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(); @@ -277,7 +276,7 @@ void LBRSystemInterface::nan_state_interfaces_() { hw_velocity_.fill(std::numeric_limits::quiet_NaN()); } -bool LBRSystemInterface::verify_number_of_joints_() { +bool SystemInterface::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()); @@ -286,7 +285,7 @@ bool LBRSystemInterface::verify_number_of_joints_() { return true; } -bool LBRSystemInterface::verify_joint_command_interfaces_() { +bool SystemInterface::verify_joint_command_interfaces_() { // check command interfaces for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { @@ -310,7 +309,7 @@ bool LBRSystemInterface::verify_joint_command_interfaces_() { return true; } -bool LBRSystemInterface::verify_joint_state_interfaces_() { +bool SystemInterface::verify_joint_state_interfaces_() { // check state interfaces for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { @@ -340,7 +339,7 @@ bool LBRSystemInterface::verify_joint_state_interfaces_() { return true; } -bool LBRSystemInterface::verify_sensors_() { +bool SystemInterface::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()); @@ -376,7 +375,7 @@ bool LBRSystemInterface::verify_sensors_() { return true; } -bool LBRSystemInterface::exit_commanding_active_( +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 && @@ -386,23 +385,23 @@ bool LBRSystemInterface::exit_commanding_active_( return false; } -double LBRSystemInterface::time_stamps_to_sec_(const double &sec, const double &nano_sec) const { +double SystemInterface::time_stamps_to_sec_(const double &sec, const double &nano_sec) const { return sec + nano_sec / 1.e9; } -void LBRSystemInterface::nan_last_hw_states_() { +void SystemInterface::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_() { +void SystemInterface::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_() { +void SystemInterface::compute_hw_velocity_() { // state uninitialized if (std::isnan(last_hw_time_stamp_nano_sec_) || std::isnan(last_hw_position_[0])) { return; @@ -427,4 +426,4 @@ void LBRSystemInterface::compute_hw_velocity_() { #include -PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRSystemInterface, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::SystemInterface, hardware_interface::SystemInterface) diff --git a/lbr_ros2_control/lbr_system_interface.xml b/lbr_ros2_control/system_interface.xml similarity index 75% rename from lbr_ros2_control/lbr_system_interface.xml rename to lbr_ros2_control/system_interface.xml index 6483e900..2246c2e4 100644 --- a/lbr_ros2_control/lbr_system_interface.xml +++ b/lbr_ros2_control/system_interface.xml @@ -1,6 +1,6 @@ - \ No newline at end of file From 549461676e53b43552dd9eef3ec1a3dda3d78305 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 19:51:55 +0000 Subject: [PATCH 16/82] removed ros2_control configs from lbr_description --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_description/CMakeLists.txt | 2 +- lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro | 2 +- lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro | 2 +- lbr_description/urdf/med14/med14_description.urdf.xacro | 2 +- lbr_description/urdf/med7/med7_description.urdf.xacro | 2 +- lbr_ros2_control/CMakeLists.txt | 6 +++--- .../config/lbr_system_interface.xacro | 0 .../{ => plugin_description_files}/broadcasters.xml | 0 .../forward_command_controllers.xml | 0 .../{ => plugin_description_files}/system_interface.xml | 0 lbr_ros2_control/src/system_interface.cpp | 4 ++-- 12 files changed, 11 insertions(+), 11 deletions(-) rename lbr_description/ros2_control/lbr.ros2_control.xacro => lbr_ros2_control/config/lbr_system_interface.xacro (100%) rename lbr_ros2_control/{ => plugin_description_files}/broadcasters.xml (100%) rename lbr_ros2_control/{ => plugin_description_files}/forward_command_controllers.xml (100%) rename lbr_ros2_control/{ => plugin_description_files}/system_interface.xml (100%) 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_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/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index 719ca2df..b66514ac 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -24,7 +24,7 @@ - + - + - + - + get_logger(), From 4a3017aa31fe1a33051764d3f20fcaafe25e4179 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 20:02:35 +0000 Subject: [PATCH 17/82] updated license --- LICENSE | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From e7f0ddb77d952c9dd694d4cf2837f5a6931df916 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 20:13:12 +0000 Subject: [PATCH 18/82] bumped version 1.3.1 -> 1.4.0 --- lbr_bringup/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py | 2 +- lbr_demos/lbr_demos_fri_ros2_cpp/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_python/package.xml | 2 +- lbr_demos/lbr_demos_fri_ros2_python/setup.py | 2 +- lbr_demos/lbr_demos_ros2_control_cpp/package.xml | 2 +- lbr_demos/lbr_demos_ros2_control_python/package.xml | 2 +- lbr_demos/lbr_demos_ros2_control_python/setup.py | 2 +- lbr_description/package.xml | 2 +- lbr_fri_msgs/package.xml | 2 +- lbr_fri_ros2/package.xml | 2 +- lbr_fri_ros2_stack/package.xml | 2 +- lbr_ros2_control/package.xml | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) 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/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml index 22596062..078be467 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_fri_ros2_advanced_cpp - 1.3.1 + 1.4.0 Advanced C++ demos for the lbr_fri_ros2. mhubii MIT 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_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_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/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_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/package.xml b/lbr_fri_ros2/package.xml index df1637bf..ea568f40 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 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_ros2_control/package.xml b/lbr_ros2_control/package.xml index d1f28312..ae426cc2 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control - 1.3.1 + 1.4.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii MIT From 231859fc33e1bf98ef6caf52d6c1665d121884cc Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 20:18:19 +0000 Subject: [PATCH 19/82] added choices to launch --- lbr_ros2_control/lbr_ros2_control/launch_mixin.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index 8f11ce92..e1fd3c30 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -29,7 +29,12 @@ def arg_ctrl() -> DeclareLaunchArgument: name="ctrl", default_value="position_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", - choices=["position_trajectory_controller", "forward_position_controller"], + choices=[ + "position_trajectory_controller", + "forward_position_controller", + "forward_lbr_position_command_controller", + "forward_lbr_torque_command_controller", + ], ) @staticmethod From 5d86725db5cb5e7669ae4f889f8788353c9dbd47 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 5 Dec 2023 20:22:19 +0000 Subject: [PATCH 20/82] updated qos profiles --- .../src/admittance_control_node.cpp | 6 +++--- .../admittance_control_node.py | 7 +++++-- .../lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp | 4 ++-- .../src/torque_sine_overlay_node.cpp | 6 +++--- .../lbr_demos_fri_ros2_python/joint_sine_overlay_node.py | 7 +++++-- .../lbr_demos_fri_ros2_python/torque_sine_overlay_node.py | 7 ++++--- 6 files changed, 22 insertions(+), 15 deletions(-) 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 19435615..7b502306 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 @@ -30,10 +30,10 @@ class AdmittanceControlNode : public rclcpp::Node { this->get_parameter("dq_gains").as_double_array(), this->get_parameter("dx_gains").as_double_array()); - lbr_position_command_pub_ = - create_publisher("/lbr/command/joint_position", 1); + lbr_position_command_pub_ = create_publisher( + "/lbr/command/joint_position", rclcpp::SystemDefaultsQoS()); lbr_state_sub_ = create_subscription( - "/lbr/state", 1, + "/lbr/state", rclcpp::SensorDataQoS(), std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); } 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 70bb108d..777a3da0 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 @@ -1,6 +1,7 @@ import numpy as np import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default from lbr_fri_msgs.msg import LBRPositionCommand, LBRState @@ -27,10 +28,12 @@ def __init__(self, node_name="admittance_control_node") -> None: # publishers and subscribers self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data ) self.lbr_position_command_pub_ = self.create_publisher( - LBRPositionCommand, "/lbr/command/joint_position", 1 + LBRPositionCommand, + "/lbr/command/joint_position", + qos_profile_system_default, ) def on_lbr_state_(self, lbr_state: LBRState) -> None: 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 dd2234e3..532f5118 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 @@ -20,11 +20,11 @@ class JointSineOverlayNode : public rclcpp::Node { JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr/command/joint_position lbr_position_command_pub_ = this->create_publisher( - "/lbr/command/joint_position", 1); + "/lbr/command/joint_position", rclcpp::SystemDefaultsQoS()); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/lbr/state", 1, + "/lbr/state", rclcpp::SensorDataQoS(), std::bind(&JointSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); }; diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp index cc7b6b04..493d4c68 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp @@ -19,12 +19,12 @@ class TorqueSineOverlayNode : public rclcpp::Node { public: TorqueSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr/command/torque - lbr_torque_command_pub_ = - this->create_publisher("/lbr/command/torque", 1); + lbr_torque_command_pub_ = this->create_publisher( + "/lbr/command/torque", rclcpp::SystemDefaultsQoS()); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/lbr/state", 1, + "/lbr/state", rclcpp::SensorDataQoS(), std::bind(&TorqueSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); }; 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 74c43d9d..1a23f54f 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 @@ -2,6 +2,7 @@ import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default # import lbr_fri_msgs from lbr_fri_msgs.msg import LBRPositionCommand, LBRState @@ -18,12 +19,14 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr/command/joint_position self.lbr_position_command_pub_ = self.create_publisher( - LBRPositionCommand, "/lbr/command/joint_position", 1 + LBRPositionCommand, + "/lbr/command/joint_position", + qos_profile_system_default, ) # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data ) def on_lbr_state_(self, lbr_state: LBRState) -> None: 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..4875f3c0 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 @@ -2,9 +2,10 @@ import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default # import lbr_fri_msgs -from lbr_fri_msgs.msg import LBRTorqueCommand, LBRState +from lbr_fri_msgs.msg import LBRState, LBRTorqueCommand class TorqueSineOverlayNode(Node): @@ -18,12 +19,12 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr/command/torque self.lbr_torque_command_pub_ = self.create_publisher( - LBRTorqueCommand, "/lbr/command/torque", 1 + LBRTorqueCommand, "/lbr/command/torque", qos_profile_system_default ) # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, 1 + LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data ) def on_lbr_state_(self, lbr_state: LBRState) -> None: From 4bc53b1930b016c8df11b20be5406f60184234cb Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 09:49:15 +0000 Subject: [PATCH 21/82] cleaned xacro files --- lbr_description/gazebo/lbr.gazebo.xacro | 120 +++++------------- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 56 ++++---- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 56 ++++---- .../urdf/med14/med14_description.urdf.xacro | 56 ++++---- .../urdf/med7/med7_description.urdf.xacro | 56 ++++---- 5 files changed, 143 insertions(+), 201 deletions(-) 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/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index b66514ac..cad4fcb9 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -6,13 +6,13 @@ - - - - - - - + + + + + + + @@ -56,8 +56,8 @@ - @@ -88,8 +88,8 @@ - @@ -120,8 +120,8 @@ - @@ -152,8 +152,8 @@ - @@ -184,8 +184,8 @@ - @@ -216,8 +216,8 @@ - @@ -248,8 +248,8 @@ - @@ -286,13 +286,13 @@ diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 4fc5af89..85055dbe 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -6,13 +6,13 @@ - - - - - - - + + + + + + + @@ -56,8 +56,8 @@ - @@ -88,8 +88,8 @@ - @@ -120,8 +120,8 @@ - @@ -152,8 +152,8 @@ - @@ -184,8 +184,8 @@ - @@ -216,8 +216,8 @@ - @@ -249,8 +249,8 @@ - @@ -287,13 +287,13 @@ diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index ade55874..9b16af2e 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -6,13 +6,13 @@ - - - - - - - + + + + + + + @@ -56,8 +56,8 @@ - @@ -88,8 +88,8 @@ - @@ -120,8 +120,8 @@ - @@ -152,8 +152,8 @@ - @@ -184,8 +184,8 @@ - @@ -216,8 +216,8 @@ - @@ -248,8 +248,8 @@ - @@ -286,13 +286,13 @@ diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index c0ee3fa0..a04fd08d 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -6,13 +6,13 @@ - - - - - - - + + + + + + + @@ -56,8 +56,8 @@ - @@ -88,8 +88,8 @@ - @@ -120,8 +120,8 @@ - @@ -152,8 +152,8 @@ - @@ -184,8 +184,8 @@ - @@ -216,8 +216,8 @@ - @@ -248,8 +248,8 @@ - @@ -286,13 +286,13 @@ From 043d2d607b6ed56342625e9e06cfef137916484a Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 09:52:24 +0000 Subject: [PATCH 22/82] cleaned tests --- .../test/lbr_model_specifications.py | 12 ------- lbr_description/test/test_urdf.py | 35 ++++++------------- 2 files changed, 11 insertions(+), 36 deletions(-) diff --git a/lbr_description/test/lbr_model_specifications.py b/lbr_description/test/lbr_model_specifications.py index e15cdc6d..090fa100 100644 --- a/lbr_description/test/lbr_model_specifications.py +++ b/lbr_description/test/lbr_model_specifications.py @@ -36,18 +36,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..df81be06 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}." ) @@ -122,33 +112,30 @@ def test_position_limits_ros2_control( 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"): 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.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.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.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.name}." ) else: raise ValueError("Couldn't find name.") From a0ba50da15eb895eaf2279cc4ea8058a40efbef6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 10:01:55 +0000 Subject: [PATCH 23/82] removed redundancy --- lbr_description/test/test_urdf.py | 16 +- .../config/lbr_system_interface.xacro | 185 +++++------------- 2 files changed, 54 insertions(+), 147 deletions(-) diff --git a/lbr_description/test/test_urdf.py b/lbr_description/test/test_urdf.py index df81be06..e36372c9 100644 --- a/lbr_description/test/test_urdf.py +++ b/lbr_description/test/test_urdf.py @@ -111,31 +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"): - 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[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 {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[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 {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_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index d237b592..6dd14dc5 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -2,13 +2,13 @@ @@ -53,140 +53,43 @@ - - - - -${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} - - - - - - - - - - - + + + + ${min_position} + ${max_position} + + + -${effort_limit} + ${effort_limit} + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 5012ff2f83c49daa9a35c8776ceba64740c65988 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 10:05:12 +0000 Subject: [PATCH 24/82] position_* -> joint_trajectory_controller --- .../src/joint_trajectory_executioner_node.cpp | 2 +- .../joint_trajectory_executioner_node.py | 2 +- .../iiwa14_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../iiwa7_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../med14_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../med7_moveit_config/config/moveit_controllers.yaml | 4 ++-- lbr_ros2_control/config/lbr_controllers.yaml | 4 ++-- lbr_ros2_control/lbr_ros2_control/launch_mixin.py | 4 ++-- 8 files changed, 14 insertions(+), 14 deletions(-) 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_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/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 6af537f5..8064de61 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -16,7 +16,7 @@ type: lbr_ros2_control/EstimatedFTBroadcaster # ROS 2 control controllers - position_trajectory_controller: + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: @@ -33,7 +33,7 @@ ros__parameters: damping: 0.2 -/**/position_trajectory_controller: +/**/joint_trajectory_controller: ros__parameters: joints: - A1 diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index e1fd3c30..c7fce4a3 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -27,10 +27,10 @@ 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", + "joint_trajectory_controller", "forward_position_controller", "forward_lbr_position_command_controller", "forward_lbr_torque_command_controller", From 64d1bc9d931819e49659f01721519ff030224972 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 11:08:46 +0000 Subject: [PATCH 25/82] removed redundant members --- .../lbr_ros2_control/system_interface.hpp | 51 +++--- lbr_ros2_control/src/system_interface.cpp | 157 ++++++++---------- 2 files changed, 94 insertions(+), 114 deletions(-) diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index ebbc1521..3cdf6c5a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -27,6 +27,10 @@ namespace lbr_ros2_control { class SystemInterface : public hardware_interface::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_SENSOR_SIZE = 12; + public: SystemInterface() = default; @@ -63,20 +67,22 @@ class SystemInterface : public hardware_interface::SystemInterface { 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 + int32_t port_id_; + const char *remote_host_; + int32_t rt_prio_; + bool open_loop_; - // node for handling communication + // robot driver rclcpp::Node::SharedPtr app_node_ptr_; std::shared_ptr client_ptr_; std::unique_ptr app_ptr_; - lbr_fri_msgs::msg::LBRCommand lbr_command_; - lbr_fri_msgs::msg::LBRState lbr_state_; - // exposed state interfaces - double hw_sample_time_; + lbr_fri_msgs::msg::LBRState hw_lbr_state_; + + // state interfaces that require cast, this could be mitigated by defining LBRState exclusively + // with doubles double hw_session_state_; double hw_connection_quality_; double hw_safety_state_; @@ -85,38 +91,23 @@ class SystemInterface : 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_; + // added 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_; - // 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__SYSTEM_INTERFACE_HPP_ diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 6ba5a221..f3df1e76 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -62,49 +62,50 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { std::vector SystemInterface::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_); - + // 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_position_[i]); + &hw_lbr_state_.measured_joint_position[i]); state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_JOINT_POSITION, - &hw_commanded_joint_position_[i]); + &hw_lbr_state_.commanded_joint_position[i]); state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_effort_[i]); + &hw_lbr_state_.measured_torque[i]); state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_TORQUE, - &hw_commanded_torque_[i]); + &hw_lbr_state_.commanded_torque[i]); state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EXTERNAL_TORQUE, - &hw_external_torque_[i]); + &hw_lbr_state_.external_torque[i]); state_interfaces.emplace_back(info_.joints[i].name, HW_IF_IPO_JOINT_POSITION, - &hw_ipo_joint_position_[i]); + &hw_lbr_state_.ipo_joint_position[i]); + // added velocity state interface state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_[i]); } - + const auto &lbr_fri_sensor = info_.sensors[0]; + state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_SAMPLE_TIME, &hw_lbr_state_.sample_time); state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_TRACKING_PERFORMANCE, - &hw_tracking_performance_); + &hw_lbr_state_.tracking_performance); + + // state interfaces that require cast + 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_); return state_interfaces; } @@ -114,12 +115,15 @@ std::vector SystemInterface::export_comman 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]); + &hw_lbr_command_.joint_position[i]); command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, - &hw_effort_command_[i]); + &hw_lbr_command_.torque[i]); } + // prefix? + // /link_ee/ + return command_interfaces; } @@ -169,9 +173,10 @@ SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - lbr_state_ = client_ptr_->get_state_interface().get_state(); + // read state + hw_lbr_state_ = client_ptr_->get_state_interface().get_state(); if (exit_commanding_active_(static_cast(hw_session_state_), - static_cast(lbr_state_.session_state))) { + static_cast(hw_lbr_state_.session_state))) { RCLCPP_ERROR(app_node_ptr_->get_logger(), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); app_ptr_->stop_run(); @@ -179,32 +184,19 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim 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; + // 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); + + // added velocity state interface compute_hw_velocity_(); update_last_hw_states_(); @@ -217,42 +209,47 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti 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(), + 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; } - 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_); + 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_position_command_.cbegin(), hw_position_command_.cend(), + 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_effort_command_.cbegin(), hw_effort_command_.cend(), + 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; } - std::memcpy(lbr_command_.joint_position.data(), hw_position_command_.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(lbr_command_.torque.data(), hw_effort_command_.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - client_ptr_->get_command_interface().set_command_target(lbr_command_); + 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 not implemented."); + throw std::runtime_error("Wrench command mode currently not implemented."); } return hardware_interface::return_type::ERROR; } void SystemInterface::nan_command_interfaces_() { - hw_position_command_.fill(std::numeric_limits::quiet_NaN()); - hw_effort_command_.fill(std::numeric_limits::quiet_NaN()); + 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_() { - hw_sample_time_ = std::numeric_limits::quiet_NaN(); + // 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(); @@ -261,18 +258,10 @@ void SystemInterface::nan_state_interfaces_() { 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(); - + // added velocity state interface hw_velocity_.fill(std::numeric_limits::quiet_NaN()); } @@ -390,20 +379,20 @@ double SystemInterface::time_stamps_to_sec_(const double &sec, const double &nan } void SystemInterface::nan_last_hw_states_() { - last_hw_position_.fill(std::numeric_limits::quiet_NaN()); + 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_position_ = hw_position_; + 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_position_[0])) { + if (std::isnan(last_hw_time_stamp_nano_sec_) || std::isnan(last_hw_measured_joint_position_[0])) { return; } @@ -417,7 +406,7 @@ void SystemInterface::compute_hw_velocity_() { 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; + v = (hw_lbr_state_.measured_joint_position[i] - last_hw_measured_joint_position_[i]) / dt; ++i; }); } From e86ca0216d60e099479ca03ce649fd9ae503b1b2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 11:08:54 +0000 Subject: [PATCH 26/82] added dataclass tag --- lbr_description/test/lbr_model_specifications.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lbr_description/test/lbr_model_specifications.py b/lbr_description/test/lbr_model_specifications.py index 090fa100..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 From c2bd43ee1de5cbdfa47739ace6e90b09df91cf75 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 11:09:01 +0000 Subject: [PATCH 27/82] updated doc --- lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 --------------------------- From f78826b6dde08d7c6326824f6bc2ae536ed85164 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 11:27:01 +0000 Subject: [PATCH 28/82] setup-ros update https://github.com/marketplace/actions/setup-ros-environment --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From c67182da20d8dcc7a6237ed9ea2825af039934d1 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 11:55:50 +0000 Subject: [PATCH 29/82] virtual -> estimated --- .../estimated_ft_broadcaster.hpp | 2 +- .../src/estimated_ft_broadcaster.cpp | 28 +++++++++---------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp index 84d12d20..49df6021 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp @@ -64,7 +64,7 @@ class EstimatedFTBroadcaster : public controller_interface::ControllerInterface Eigen::Matrix jacobian_pinv_; Eigen::Matrix joint_positions_, external_joint_torques_; - Eigen::Matrix virtual_ft_; + Eigen::Matrix estimated_ft_; double damping_; diff --git a/lbr_ros2_control/src/estimated_ft_broadcaster.cpp b/lbr_ros2_control/src/estimated_ft_broadcaster.cpp index fc095687..69187c4b 100644 --- a/lbr_ros2_control/src/estimated_ft_broadcaster.cpp +++ b/lbr_ros2_control/src/estimated_ft_broadcaster.cpp @@ -64,16 +64,16 @@ EstimatedFTBroadcaster::update(const rclcpp::Time & /*time*/, const rclcpp::Dura // 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_; + estimated_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_->msg_.wrench.force.x = estimated_ft_(0); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = estimated_ft_(1); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = estimated_ft_(2); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = estimated_ft_(3); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = estimated_ft_(4); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = estimated_ft_(5); rt_wrench_stamped_publisher_ptr_->unlockAndPublish(); } return controller_interface::return_type::OK; @@ -104,14 +104,14 @@ void EstimatedFTBroadcaster::init_states_() { 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()); + estimated_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); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.x = estimated_ft_(0); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = estimated_ft_(1); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = estimated_ft_(2); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = estimated_ft_(3); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = estimated_ft_(4); + rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = estimated_ft_(5); } bool EstimatedFTBroadcaster::reference_state_interfaces_() { From 30a3ef2e90e9f99902cd2c2c9608139b154ed5e4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 14:16:35 +0000 Subject: [PATCH 30/82] moved damped least squares to lbr_fri_ros2 --- lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt | 3 --- lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml | 2 -- .../src/admittance_control_node.cpp | 1 - .../src/admittance_controller.hpp | 3 +-- lbr_fri_ros2/CMakeLists.txt | 5 +++++ .../include/lbr_fri_ros2}/damped_least_squares.hpp | 8 +++++--- lbr_fri_ros2/package.xml | 6 ++++++ 7 files changed, 17 insertions(+), 11 deletions(-) rename {lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src => lbr_fri_ros2/include/lbr_fri_ros2}/damped_least_squares.hpp (81%) 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 078be467..a3a275d9 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml @@ -8,11 +8,9 @@ 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 7b502306..544bacd6 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 { 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..d6d85f8a 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/damped_least_squares.hpp" namespace lbr_fri_ros2 { class AdmittanceController { diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 6a837d5f..dd2211d5 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -13,6 +13,8 @@ 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(lbr_fri_msgs REQUIRED) @@ -40,6 +42,7 @@ target_include_directories(lbr_fri_ros2 ament_target_dependencies(lbr_fri_ros2 control_toolbox + Eigen3 lbr_fri_msgs rclcpp realtime_tools @@ -53,6 +56,8 @@ 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 lbr_fri_msgs diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp similarity index 81% rename from lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp index c983f7cf..880e9a79 100644 --- a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/damped_least_squares.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp @@ -1,9 +1,10 @@ -#ifndef LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ -#define LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ +#ifndef LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ +#define LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ #include #include +namespace lbr_fri_ros2 { template Eigen::Matrix damped_least_squares(const MatT &mat, typename MatT::Scalar lambda = @@ -21,4 +22,5 @@ damped_least_squares(const MatT &mat, typename MatT::Scalar lambda = } return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); } -#endif // LBR_DEMOS_FRI_ROS2_ADVANCED_CPP__DAMPED_LEAST_SQUARES_HPP_ +} // end of namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index ea568f40..b480f9d3 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -10,6 +10,9 @@ ament_cmake ament_cmake_python + eigen3_cmake_module + + eigen control_toolbox fri_vendor @@ -21,6 +24,9 @@ lbr_description + eigen3_cmake_module + eigen + ament_lint_auto ament_lint_common From 5f94f1aabec98a99f2755dc054cf5339bf6b5070 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 16:08:28 +0000 Subject: [PATCH 31/82] removed the ft broadcaster --- lbr_bringup/launch/real.launch.py | 4 - lbr_ros2_control/CMakeLists.txt | 1 - lbr_ros2_control/config/lbr_controllers.yaml | 7 - .../estimated_ft_broadcaster.hpp | 83 --------- .../launch/system_interface.launch.py | 4 - .../plugin_description_files/broadcasters.xml | 8 - .../src/estimated_ft_broadcaster.cpp | 173 ------------------ 7 files changed, 280 deletions(-) delete mode 100644 lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp delete mode 100644 lbr_ros2_control/src/estimated_ft_broadcaster.cpp diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 3d749f40..a82da17c 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -32,9 +32,6 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - estimated_ft_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="estimated_ft_broadcaster" - ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -45,7 +42,6 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: on_start=[ joint_state_broadcaster, lbr_state_broadcaster, - estimated_ft_broadcaster, controller, ], ) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 51ff75bf..f2052c6f 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -35,7 +35,6 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED - src/estimated_ft_broadcaster.cpp src/forward_lbr_position_command_controller.cpp src/forward_lbr_torque_command_controller.cpp src/lbr_state_broadcaster.cpp diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 8064de61..073dde1a 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -12,9 +12,6 @@ lbr_state_broadcaster: type: lbr_ros2_control/LBRStateBroadcaster - estimated_ft_broadcaster: - type: lbr_ros2_control/EstimatedFTBroadcaster - # ROS 2 control controllers joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -29,10 +26,6 @@ forward_lbr_torque_command_controller: type: lbr_ros2_control/ForwardLBRTorqueCommandController -/**/estimated_ft_broadcaster: - ros__parameters: - damping: 0.2 - /**/joint_trajectory_controller: ros__parameters: joints: diff --git a/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp deleted file mode 100644 index 49df6021..00000000 --- a/lbr_ros2_control/include/lbr_ros2_control/estimated_ft_broadcaster.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef LBR_ROS2_CONTROL__ESTIMATED_FT_BROADCASTER_HPP_ -#define LBR_ROS2_CONTROL__ESTIMATED_FT_BROADCASTER_HPP_ - -#include -#include -#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/system_interface_type_values.hpp" - -namespace lbr_ros2_control { -class EstimatedFTBroadcaster : public controller_interface::ControllerInterface { -public: - EstimatedFTBroadcaster(); - - 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 estimated_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_state_interfaces_, external_joint_torque_state_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__ESTIMATED_FT_BROADCASTER_HPP_ diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 90b4fe81..74664008 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -31,9 +31,6 @@ def generate_launch_description() -> LaunchDescription: 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") ) @@ -43,7 +40,6 @@ def generate_launch_description() -> LaunchDescription: on_start=[ joint_state_broadcaster, lbr_state_broadcaster, - lbr_estimated_ft_broadcast, controller, ], ) diff --git a/lbr_ros2_control/plugin_description_files/broadcasters.xml b/lbr_ros2_control/plugin_description_files/broadcasters.xml index 8cbb5ffc..684fdfe3 100644 --- a/lbr_ros2_control/plugin_description_files/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/src/estimated_ft_broadcaster.cpp b/lbr_ros2_control/src/estimated_ft_broadcaster.cpp deleted file mode 100644 index 69187c4b..00000000 --- a/lbr_ros2_control/src/estimated_ft_broadcaster.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include "lbr_ros2_control/estimated_ft_broadcaster.hpp" - -namespace lbr_ros2_control { -EstimatedFTBroadcaster::EstimatedFTBroadcaster() - : jacobian_(6, KUKA::FRI::LBRState::NUMBER_OF_JOINTS), - jacobian_pinv_(KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 6) {} - -controller_interface::InterfaceConfiguration -EstimatedFTBroadcaster::command_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; -} - -controller_interface::InterfaceConfiguration -EstimatedFTBroadcaster::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 EstimatedFTBroadcaster::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 estimated FT broadcaster with: %s.", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type -EstimatedFTBroadcaster::update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // check any for nan - if (std::isnan(joint_position_state_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_state_interfaces_[i].get().get_value(); - external_joint_torques_(i) = external_joint_torque_state_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_); - estimated_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 = estimated_ft_(0); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = estimated_ft_(1); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = estimated_ft_(2); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = estimated_ft_(3); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = estimated_ft_(4); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = estimated_ft_(5); - rt_wrench_stamped_publisher_ptr_->unlockAndPublish(); - } - return controller_interface::return_type::OK; -} - -controller_interface::CallbackReturn -EstimatedFTBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn -EstimatedFTBroadcaster::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 -EstimatedFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - clear_state_interfaces_(); - return controller_interface::CallbackReturn::SUCCESS; -} - -void EstimatedFTBroadcaster::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()); - estimated_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 = estimated_ft_(0); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = estimated_ft_(1); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = estimated_ft_(2); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = estimated_ft_(3); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = estimated_ft_(4); - rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = estimated_ft_(5); -} - -bool EstimatedFTBroadcaster::reference_state_interfaces_() { - for (auto &state_interface : state_interfaces_) { - if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); - } - if (state_interface.get_interface_name() == HW_IF_EXTERNAL_TORQUE) { - external_joint_torque_state_interfaces_.emplace_back(std::ref(state_interface)); - } - } - if (joint_position_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR( - this->get_node()->get_logger(), - "Number of joint position state interfaces (%ld) does not match the number of joints " - "in the robot (%d).", - joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return false; - } - if (external_joint_torque_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR(this->get_node()->get_logger(), - "Number of external joint torque state interfaces (%ld) does not match the number " - "of joints " - "in the robot (%d).", - external_joint_torque_state_interfaces_.size(), - KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return false; - } - return true; -} - -void EstimatedFTBroadcaster::clear_state_interfaces_() { - joint_position_state_interfaces_.clear(); - external_joint_torque_state_interfaces_.clear(); -} - -template -Eigen::Matrix -EstimatedFTBroadcaster::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::EstimatedFTBroadcaster, - controller_interface::ControllerInterface) From bed3678f1302f47acb044381b8611190022e2713 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 6 Dec 2023 16:15:33 +0000 Subject: [PATCH 32/82] updated includes --- lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp index 880e9a79..1f307608 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp @@ -1,8 +1,8 @@ #ifndef LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ #define LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ -#include -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/SVD" namespace lbr_fri_ros2 { template From f209973a597f2eba29b4570184988b20dbcfaa46 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 12:13:57 +0000 Subject: [PATCH 33/82] updated lstsq --- .../src/admittance_controller.hpp | 4 ++-- .../{damped_least_squares.hpp => lstsq.hpp} | 21 +++++++++++-------- 2 files changed, 14 insertions(+), 11 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{damped_least_squares.hpp => lstsq.hpp} (56%) 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 d6d85f8a..70490451 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,7 +14,7 @@ #include "lbr_fri_msgs/msg/lbr_position_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_ros2/damped_least_squares.hpp" +#include "lbr_fri_ros2/lstsq.hpp" namespace lbr_fri_ros2 { class AdmittanceController { @@ -50,7 +50,7 @@ class AdmittanceController { jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = damped_least_squares(jacobian_.data); + jacobian_inv_ = lstsq(jacobian_.data); f_ext_ = jacobian_inv_.transpose() * tau_ext_; for (int i = 0; i < 6; i++) { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp similarity index 56% rename from lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp index 1f307608..16019a04 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/damped_least_squares.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp @@ -1,5 +1,7 @@ -#ifndef LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ -#define LBR_FRI_ROS2__DAMPED_LEAST_SQUARES_HPP_ +#ifndef LBR_FRI_ROS2__LSTSQ_HPP_ +#define LBR_FRI_ROS2__LSTSQ_HPP_ + +#include #include "eigen3/Eigen/Core" #include "eigen3/Eigen/SVD" @@ -7,8 +9,8 @@ namespace lbr_fri_ros2 { template Eigen::Matrix -damped_least_squares(const MatT &mat, typename MatT::Scalar lambda = - typename MatT::Scalar{2e-1}) // choose appropriately +lstsq(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); @@ -16,11 +18,12 @@ damped_least_squares(const MatT &mat, typename MatT::Scalar lambda = 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); - } + std::for_each(singularValues.data(), singularValues.data() + singularValues.size(), + [&, i = 0](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__DAMPED_LEAST_SQUARES_HPP_ +#endif // LBR_FRI_ROS2__LSTSQ_HPP_ From 2a9807649b6a6ae2248394f63e0e58ad78b93e5e Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 12:24:04 +0000 Subject: [PATCH 34/82] lstsq -> pinv --- .../src/admittance_controller.hpp | 4 ++-- .../include/lbr_fri_ros2/{lstsq.hpp => pinv.hpp} | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{lstsq.hpp => pinv.hpp} (81%) 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 70490451..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,7 +14,7 @@ #include "lbr_fri_msgs/msg/lbr_position_command.hpp" #include "lbr_fri_msgs/msg/lbr_state.hpp" -#include "lbr_fri_ros2/lstsq.hpp" +#include "lbr_fri_ros2/pinv.hpp" namespace lbr_fri_ros2 { class AdmittanceController { @@ -50,7 +50,7 @@ class AdmittanceController { jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = lstsq(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_fri_ros2/include/lbr_fri_ros2/lstsq.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp similarity index 81% rename from lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp index 16019a04..d7e57aad 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/lstsq.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__LSTSQ_HPP_ -#define LBR_FRI_ROS2__LSTSQ_HPP_ +#ifndef LBR_FRI_ROS2__PINV_HPP_ +#define LBR_FRI_ROS2__PINV_HPP_ #include @@ -9,8 +9,8 @@ namespace lbr_fri_ros2 { template Eigen::Matrix -lstsq(const MatT &mat, - typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately +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); @@ -26,4 +26,4 @@ lstsq(const MatT &mat, return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); } } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__LSTSQ_HPP_ +#endif // LBR_FRI_ROS2__PINV_HPP_ From fdc3b887723ec38339b7d09b747e67a11b114e20 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 13:12:50 +0000 Subject: [PATCH 35/82] removed ROS exposure from filters --- .../lbr_fri_ros2/command_interface.hpp | 4 +- .../include/lbr_fri_ros2/state_interface.hpp | 5 +- lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp | 101 +++--------------- lbr_fri_ros2/src/command_interface.cpp | 42 ++++---- lbr_fri_ros2/src/state_interface.cpp | 19 ++-- lbr_fri_ros2/src/utils.cpp | 85 +++------------ 6 files changed, 62 insertions(+), 194 deletions(-) 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..0137bbba 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -1,6 +1,7 @@ #ifndef LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ #define LBR_FRI_ROS2__COMMAND_INTERFACE_HPP_ +#include #include #include #include @@ -44,9 +45,8 @@ class CommandInterface { rclcpp::node_interfaces::NodeParametersInterface::SharedPtr parameters_interface_ptr_; std::unique_ptr command_guard_; - JointPIDArrayROS joint_position_pid_; + 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/state_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp index 8caa0eeb..66e43a66 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -46,9 +46,8 @@ class StateInterface { std::atomic_bool state_initialized_; idl_state_t state_; - JointExponentialFilterArrayROS external_torque_filter_; - JointExponentialFilterArrayROS measured_torque_filter_; - bool filters_init_; + JointExponentialFilterArray external_torque_filter_; + JointExponentialFilterArray 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 index e71fd236..14806aa8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp @@ -3,18 +3,14 @@ #include #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 { @@ -99,103 +95,38 @@ class ExponentialFilter { https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ }; -class JointExponentialFilterArrayROS { +class JointExponentialFilterArray { using value_array_t = std::array; public: - JointExponentialFilterArrayROS() = delete; + JointExponentialFilterArray() = default; - /** - * @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_; } + void initialize(const double &cutoff_frequency, const double &sample_time); + inline const bool &is_initialized() const { return initialized_; }; protected: - const std::string cutoff_frequency_param_name_ = "cutoff_frequency"; /**< Parameter name.*/ + bool initialized_{false}; /**< True if initialized.*/ 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 { +class JointPIDArray { using value_array_t = std::array; - using name_array_t = std::array; - using pid_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 = ""); + JointPIDArray() = default; - /** - * @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); + 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 double &p, const double &i, const double &d, const double &i_max, + const double &i_min, const bool &antiwindup); + inline const bool &is_initialized() const { return initialized_; }; protected: + bool initialized_{false}; /**< True if initialized.*/ pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // 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..28088fea 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -4,8 +4,7 @@ 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) { + parameters_interface_ptr_(node_ptr->get_node_parameters_interface()) { rclcpp::Parameter command_guard_variant_param; if (!parameters_interface_ptr_->has_parameter("command_guard_variant")) { parameters_interface_ptr_->declare_parameter("command_guard_variant", @@ -33,14 +32,13 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, } // 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(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); } - 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)) { @@ -66,14 +64,13 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s } // 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(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); } - 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 @@ -99,14 +96,13 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s } // 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(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); } - 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 diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 881839e6..b12f7d19 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -5,10 +5,7 @@ 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) {} + parameters_interface_ptr_(parameters_interface_ptr), state_initialized_(false) {} void StateInterface::set_state(const_fri_state_t_ref state) { state_.client_command_mode = state.getClientCommandMode(); @@ -37,10 +34,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 +69,15 @@ 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(10. /*Hz*/, state_.sample_time); + measured_torque_filter_.initialize(10. /*Hz*/, state_.sample_time); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp index 4d278f14..a29ee1fe 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/utils.cpp @@ -29,93 +29,40 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, 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; +void JointExponentialFilterArray::compute(const double *const current, value_array_t &previous) { std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - [&](const auto ¤t_i) { + [&, i = 0](const auto ¤t_i) mutable { 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)); - } +void JointExponentialFilterArray::initialize(const double &cutoff_frequency, + const double &sample_time) { 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; - }); + initialized_ = true; } -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); +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 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); +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 JointPIDArrayROS::init(const double &p, const double &i, const double &d, const double &i_max, - const double &i_min, const bool &antiwindup) { +void JointPIDArray::initialize(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); }); + initialized_ = true; } } // end of namespace lbr_fri_ros2 From 1fd3dce3bba38e7a2b3dc98b11cd43261525393a Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 13:14:41 +0000 Subject: [PATCH 36/82] utils -> filters --- lbr_fri_ros2/CMakeLists.txt | 2 +- lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp | 2 +- .../include/lbr_fri_ros2/{utils.hpp => filters.hpp} | 6 +++--- lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp | 2 +- lbr_fri_ros2/src/{utils.cpp => filters.cpp} | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{utils.hpp => filters.hpp} (97%) rename lbr_fri_ros2/src/{utils.cpp => filters.cpp} (98%) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index dd2211d5..cddd0a7a 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -30,8 +30,8 @@ add_library(lbr_fri_ros2 src/client.cpp src/command_guard.cpp src/command_interface.cpp + src/filters.cpp src/state_interface.cpp - src/utils.cpp ) target_include_directories(lbr_fri_ros2 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 0137bbba..64d7c09c 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -12,7 +12,7 @@ #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 { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp similarity index 97% rename from lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 14806aa8..ea15392b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -1,5 +1,5 @@ -#ifndef LBR_FRI_ROS2__UTILS_HPP_ -#define LBR_FRI_ROS2__UTILS_HPP_ +#ifndef LBR_FRI_ROS2__FILTERS_HPP_ +#define LBR_FRI_ROS2__FILTERS_HPP_ #include #include @@ -130,4 +130,4 @@ class JointPIDArray { pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // end of namespace lbr_fri_ros2 -#endif // LBR_FRI_ROS2__UTILS_HPP_ +#endif // LBR_FRI_ROS2__FILTERS_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 66e43a66..42374c25 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -8,7 +8,7 @@ #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 { class StateInterface { diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/filters.cpp similarity index 98% rename from lbr_fri_ros2/src/utils.cpp rename to lbr_fri_ros2/src/filters.cpp index a29ee1fe..017e37a1 100644 --- a/lbr_fri_ros2/src/utils.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -1,4 +1,4 @@ -#include "lbr_fri_ros2/utils.hpp" +#include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} From 5077bd3db2a38018c428a5d06f98b7debe033400 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 13:59:39 +0000 Subject: [PATCH 37/82] fixed typo --- lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp index d7e57aad..165ad9cf 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp @@ -19,7 +19,7 @@ pinv(const MatT &mat, mat.cols(), mat.rows()); dampedSingularValuesInv.setZero(); std::for_each(singularValues.data(), singularValues.data() + singularValues.size(), - [&, i = 0](Scalar &s)[mutable] { + [&, i = 0](Scalar &s) mutable { dampedSingularValuesInv(i, i) = s / (s * s + lambda * lambda); ++i; }); From 1ec897260a853b6f597690a0b73ce75dfcae9269 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 14:34:41 +0000 Subject: [PATCH 38/82] removed rclcpp param from state if --- lbr_fri_ros2/include/lbr_fri_ros2/client.hpp | 3 ++- .../include/lbr_fri_ros2/state_interface.hpp | 18 ++++++++---------- lbr_fri_ros2/src/client.cpp | 6 +++--- lbr_fri_ros2/src/state_interface.cpp | 13 ++++++------- 4 files changed, 19 insertions(+), 21 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp index 82f413e7..0bfb7e95 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp @@ -17,7 +17,8 @@ namespace lbr_fri_ros2 { class Client : public KUKA::FRI::LBRClient { public: Client() = delete; - Client(const rclcpp::Node::SharedPtr node_ptr); + Client(const rclcpp::Node::SharedPtr node_ptr, + const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); inline CommandInterface &get_command_interface() { return command_interface_; } inline StateInterface &get_state_interface() { return state_interface_; } 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 42374c25..bd307aa6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -3,14 +3,17 @@ #include #include -#include "rclcpp/rclcpp.hpp" - #include "friLBRClient.h" #include "lbr_fri_msgs/msg/lbr_state.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: // ROS IDL types @@ -26,9 +29,7 @@ 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_; }; @@ -41,13 +42,10 @@ class StateInterface { 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_; - JointExponentialFilterArray external_torque_filter_; - JointExponentialFilterArray measured_torque_filter_; + StateInterfaceParameters state_interface_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/src/client.cpp b/lbr_fri_ros2/src/client.cpp index cb10247c..b39de7d5 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -1,11 +1,11 @@ #include "lbr_fri_ros2/client.hpp" namespace lbr_fri_ros2 { -Client::Client(const rclcpp::Node::SharedPtr node_ptr) +Client::Client(const rclcpp::Node::SharedPtr node_ptr, + const StateInterfaceParameters &state_interface_parameters) : 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) { + command_interface_(node_ptr), state_interface_(state_interface_parameters), 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); diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index b12f7d19..aecf603a 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -1,11 +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) {} +StateInterface::StateInterface(const StateInterfaceParameters &state_interface_parameters) + : state_initialized_(false), state_interface_parameters_(state_interface_parameters) {} void StateInterface::set_state(const_fri_state_t_ref state) { state_.client_command_mode = state.getClientCommandMode(); @@ -77,7 +74,9 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, } void StateInterface::init_filters_() { - external_torque_filter_.initialize(10. /*Hz*/, state_.sample_time); - measured_torque_filter_.initialize(10. /*Hz*/, state_.sample_time); + external_torque_filter_.initialize(state_interface_parameters_.external_torque_cutoff_frequency, + state_.sample_time); + measured_torque_filter_.initialize(state_interface_parameters_.measured_torque_cutoff_frequency, + state_.sample_time); } } // namespace lbr_fri_ros2 From 889dfeb3bfc82548c9ad9e2052d7d19753b597c4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 15:50:12 +0000 Subject: [PATCH 39/82] removed logging and parameter interfaces --- lbr_fri_ros2/include/lbr_fri_ros2/client.hpp | 15 +- .../include/lbr_fri_ros2/command_guard.hpp | 148 ++++-------------- .../lbr_fri_ros2/command_interface.hpp | 13 +- .../include/lbr_fri_ros2/state_interface.hpp | 9 +- lbr_fri_ros2/src/client.cpp | 27 ++-- lbr_fri_ros2/src/command_guard.cpp | 133 +++++----------- lbr_fri_ros2/src/command_interface.cpp | 38 ++--- lbr_fri_ros2/src/state_interface.cpp | 16 +- 8 files changed, 138 insertions(+), 261 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp index 0bfb7e95..6c37f4c5 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp @@ -5,7 +5,8 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" @@ -15,10 +16,15 @@ namespace lbr_fri_ros2 { class Client : public KUKA::FRI::LBRClient { +protected: + static constexpr char CLIENT_LOGGER_NAME[] = "lbr_fri_ros2::Client"; + public: Client() = delete; - Client(const rclcpp::Node::SharedPtr node_ptr, - const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); + Client(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_; } @@ -30,9 +36,6 @@ 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_; 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..8e5da499 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -1,25 +1,36 @@ -#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 joint_array_t = std::array; + using joint_name_array_t = std::array; + + joint_name_array_t joint_names_; /**< Joint names.*/ + 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].*/ +}; + class CommandGuard { protected: + static constexpr char COMMAND_GUARD_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 &; @@ -31,137 +42,36 @@ class CommandGuard { 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 64d7c09c..566a1d88 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -6,7 +6,8 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" @@ -17,6 +18,8 @@ namespace lbr_fri_ros2 { class CommandInterface { protected: + static constexpr char COMMAND_INTERFACE_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 &; @@ -29,7 +32,8 @@ class CommandInterface { public: CommandInterface() = delete; - CommandInterface(const rclcpp::Node::SharedPtr node_ptr); + CommandInterface(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); @@ -40,10 +44,9 @@ 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_; JointPIDArray joint_position_pid_; idl_command_t command_, command_target_; 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 bd307aa6..555c512f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -3,6 +3,9 @@ #include #include +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + #include "friLBRClient.h" #include "lbr_fri_msgs/msg/lbr_state.hpp" @@ -16,6 +19,8 @@ struct StateInterfaceParameters { class StateInterface { protected: + static constexpr char STATE_INTERFACE_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 &; @@ -39,12 +44,14 @@ class StateInterface { inline void uninitialize() { state_initialized_ = false; } inline bool is_initialized() const { return state_initialized_; }; + void log_info() const; + protected: void init_filters_(); std::atomic_bool state_initialized_; idl_state_t state_; - StateInterfaceParameters state_interface_parameters_; + StateInterfaceParameters parameters_; JointExponentialFilterArray external_torque_filter_, measured_torque_filter_; }; } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/client.cpp b/lbr_fri_ros2/src/client.cpp index b39de7d5..f742dd59 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/client.cpp @@ -1,22 +1,23 @@ #include "lbr_fri_ros2/client.hpp" namespace lbr_fri_ros2 { -Client::Client(const rclcpp::Node::SharedPtr node_ptr, - const StateInterfaceParameters &state_interface_parameters) - : logging_interface_ptr_(node_ptr->get_node_logging_interface()), - parameters_interface_ptr_(node_ptr->get_node_parameters_interface()), - command_interface_(node_ptr), state_interface_(state_interface_parameters), 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'.", +Client::Client(const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant, + const StateInterfaceParameters &state_interface_parameters, const bool &open_loop) + : command_interface_(command_guard_parameters, command_guard_variant), + state_interface_(state_interface_parameters), open_loop_(open_loop) { + RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Configuring client."); + RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Command guard variant: %s.", + command_guard_variant.c_str()); + command_interface_.log_info(); + state_interface_.log_info(); + RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(CLIENT_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.", + RCLCPP_INFO(rclcpp::get_logger(CLIENT_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()); @@ -58,7 +59,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(CLIENT_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..2693ebbf 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(COMMAND_GUARD_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(COMMAND_GUARD_LOGGER_NAME), "Parameters:"); + for (std::size_t i = 0; i < parameters_.joint_names_.size(); ++i) { + RCLCPP_INFO( + rclcpp::get_logger(COMMAND_GUARD_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], + parameters_.max_position_[i], parameters_.max_velocity_[i], 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(COMMAND_GUARD_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(COMMAND_GUARD_LOGGER_NAME), + "Velocity not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } } @@ -133,9 +77,11 @@ 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(COMMAND_GUARD_LOGGER_NAME), + "Torque command not in limits for joint %s.", + parameters_.joint_names_[i].c_str()); return false; } } @@ -146,29 +92,32 @@ 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(COMMAND_GUARD_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 COMMANG_GUARD_FACTORY_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(COMMANG_GUARD_FACTORY_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 28088fea..e95379ef 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -2,32 +2,24 @@ 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()) { - 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(), +CommandInterface::CommandInterface(const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant) { + RCLCPP_INFO(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), "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()); + command_guard_variant.c_str()); + 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(COMMAND_INTERFACE_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(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -43,7 +35,7 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // 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(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -54,12 +46,12 @@ 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(COMMAND_INTERFACE_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(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -86,12 +78,12 @@ 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(COMMAND_INTERFACE_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(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -108,7 +100,7 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // 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(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -124,4 +116,6 @@ 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(); } +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index aecf603a..3eb97933 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { StateInterface::StateInterface(const StateInterfaceParameters &state_interface_parameters) - : state_initialized_(false), state_interface_parameters_(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(); @@ -74,9 +74,19 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, } void StateInterface::init_filters_() { - external_torque_filter_.initialize(state_interface_parameters_.external_torque_cutoff_frequency, + external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, state_.sample_time); - measured_torque_filter_.initialize(state_interface_parameters_.measured_torque_cutoff_frequency, + measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, state_.sample_time); } + +void StateInterface::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), "Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), + " external_torque_cutoff_frequency: %f", + parameters_.external_torque_cutoff_frequency); + RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), + " measured_torque_cutoff_frequency: %f", + parameters_.measured_torque_cutoff_frequency); +} } // namespace lbr_fri_ros2 From 520139fe788f350cf6fcf20c8147b3fd8fd1429a Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 16:24:51 +0000 Subject: [PATCH 40/82] removed logging and parameter interfaces --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 12 +++--- lbr_fri_ros2/src/app.cpp | 51 +++++++++++------------ 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 3a6745a5..cbe2e6a0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -3,10 +3,10 @@ #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" @@ -16,8 +16,11 @@ namespace lbr_fri_ros2 { class App { +protected: + static constexpr char APP_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 client_ptr); ~App(); bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); @@ -28,9 +31,6 @@ 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_; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 6ba76826..6236cb54 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -1,10 +1,9 @@ #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) { +App::App(const std::shared_ptr client_ptr) + : should_stop_(true), running_(false), client_ptr_(nullptr), connection_ptr_(nullptr), + app_ptr_(nullptr) { client_ptr_ = client_ptr; connection_ptr_ = std::make_unique(); app_ptr_ = std::make_unique(*connection_ptr_, *client_ptr_); @@ -17,99 +16,99 @@ 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(APP_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(APP_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(APP_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(APP_LOGGER_NAME), "Failed to open socket."); return false; } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Socket opened successfully."); + RCLCPP_INFO(rclcpp::get_logger(APP_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(APP_LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Closing UDP socket."); + RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Closing UDP socket."); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Socket already closed."); + RCLCPP_INFO(rclcpp::get_logger(APP_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(APP_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."); + RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Client not configured."); return; } if (!connection_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(APP_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(APP_LOGGER_NAME), "Connection not open."); return; } if (!app_ptr_) { - RCLCPP_ERROR(logging_interface_ptr_->get_logger(), "App not configured."); + RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "App not configured."); return; } if (running_) { - RCLCPP_WARN(logging_interface_ptr_->get_logger(), "App already running."); + RCLCPP_WARN(rclcpp::get_logger(APP_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(APP_LOGGER_NAME), "Failed to set FIFO realtime scheduling policy."); } } else { - RCLCPP_WARN(logging_interface_ptr_->get_logger(), + RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), "Realtime kernel recommended but not required."); } - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Starting run thread."); + RCLCPP_INFO(rclcpp::get_logger(APP_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."); + RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "LBR in session state idle, exiting."); break; } } client_ptr_->get_state_interface().uninitialize(); running_ = false; - RCLCPP_INFO(logging_interface_ptr_->get_logger(), "Exiting run thread."); + RCLCPP_INFO(rclcpp::get_logger(APP_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(APP_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(APP_LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", + port_id); return false; } return true; From f337b4e6403b27eddd68101ce34640d494be2463 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 16:30:32 +0000 Subject: [PATCH 41/82] Client -> AsyncClient --- lbr_fri_ros2/CMakeLists.txt | 62 +++++++++---------- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 6 +- .../{client.hpp => async_client.hpp} | 20 +++--- lbr_fri_ros2/src/app.cpp | 4 +- lbr_fri_ros2/src/app_component.cpp | 6 +- lbr_fri_ros2/src/app_component.hpp | 4 +- .../src/{client.cpp => async_client.cpp} | 30 ++++----- 7 files changed, 67 insertions(+), 65 deletions(-) rename lbr_fri_ros2/include/lbr_fri_ros2/{client.hpp => async_client.hpp} (60%) rename lbr_fri_ros2/src/{client.cpp => async_client.cpp} (62%) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index cddd0a7a..7a70c3f2 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -27,7 +27,7 @@ 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 @@ -82,36 +82,36 @@ install( DESTINATION share/lbr_fri_ros2 ) -# app_component -add_library(app_component - SHARED - src/app_component.cpp -) - -target_include_directories(app_component - PRIVATE src -) - -ament_target_dependencies(app_component - rclcpp - urdf - rclcpp_components -) - -target_link_libraries(app_component - lbr_fri_ros2 -) - -rclcpp_components_register_node(app_component - PLUGIN lbr_fri_ros2::AppComponent - EXECUTABLE app -) - -install( - TARGETS app_component - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/lbr_fri_ros2 -) +# # app_component +# add_library(app_component +# SHARED +# src/app_component.cpp +# ) + +# target_include_directories(app_component +# PRIVATE src +# ) + +# ament_target_dependencies(app_component +# rclcpp +# urdf +# rclcpp_components +# ) + +# target_link_libraries(app_component +# lbr_fri_ros2 +# ) + +# rclcpp_components_register_node(app_component +# PLUGIN lbr_fri_ros2::AppComponent +# EXECUTABLE app +# ) + +# install( +# TARGETS app_component +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION lib/lbr_fri_ros2 +# ) install( DIRECTORY config launch diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index cbe2e6a0..f8bb7aab 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -12,7 +12,7 @@ #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 { @@ -20,7 +20,7 @@ class App { static constexpr char APP_LOGGER_NAME[] = "lbr_fri_ros2::App"; public: - App(const std::shared_ptr client_ptr); + App(const std::shared_ptr client_ptr); ~App(); bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); @@ -34,7 +34,7 @@ class App { std::atomic_bool should_stop_, running_; std::thread run_thread_; - std::shared_ptr client_ptr_; + std::shared_ptr 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 60% rename from lbr_fri_ros2/include/lbr_fri_ros2/client.hpp rename to lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 6c37f4c5..f75a3d84 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -1,5 +1,5 @@ -#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 @@ -15,16 +15,16 @@ #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 CLIENT_LOGGER_NAME[] = "lbr_fri_ros2::Client"; + static constexpr char ASYNC_CLIENT_LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; public: - Client() = delete; - Client(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); + AsyncClient() = delete; + AsyncClient(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_; } @@ -42,4 +42,4 @@ class Client : public KUKA::FRI::LBRClient { 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/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 6236cb54..7635b67a 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -1,7 +1,7 @@ #include "lbr_fri_ros2/app.hpp" namespace lbr_fri_ros2 { -App::App(const std::shared_ptr client_ptr) +App::App(const std::shared_ptr client_ptr) : should_stop_(true), running_(false), client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { client_ptr_ = client_ptr; @@ -52,7 +52,7 @@ bool App::close_udp_socket() { void App::run(int rt_prio) { if (!client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Client not configured."); + RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "AsyncClient not configured."); return; } if (!connection_ptr_) { diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 8cfbce05..b07a6152 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -8,7 +8,7 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) { app_node_ptr_->declare_parameter("remote_host", std::string("")); app_node_ptr_->declare_parameter("rt_prio", 80); - client_ptr_ = std::make_shared(app_node_ptr_); + client_ptr_ = std::make_shared(app_node_ptr_); app_ptr_ = std::make_unique(app_node_ptr_, client_ptr_); // default connect @@ -75,7 +75,7 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, 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) .c_str()); @@ -165,7 +165,7 @@ void AppComponent::on_wrench_command_( bool AppComponent::on_command_checks_(const int &expected_command_mode) { if (!client_ptr_) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Client not configured."); + RCLCPP_ERROR(app_node_ptr_->get_logger(), "AsyncClient not configured."); return false; } if (client_ptr_->get_state_interface().get_state().client_command_mode == diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index 459fda22..edeb72de 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -14,7 +14,7 @@ #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/enum_maps.hpp" namespace lbr_fri_ros2 { @@ -81,7 +81,7 @@ class AppComponent { // app rclcpp::Node::SharedPtr app_node_ptr_; /** Node for communicating with ROS.<*/ - std::shared_ptr client_ptr_; + std::shared_ptr 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 62% rename from lbr_fri_ros2/src/client.cpp rename to lbr_fri_ros2/src/async_client.cpp index f742dd59..8d994c7a 100644 --- a/lbr_fri_ros2/src/client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -1,31 +1,33 @@ -#include "lbr_fri_ros2/client.hpp" +#include "lbr_fri_ros2/async_client.hpp" namespace lbr_fri_ros2 { -Client::Client(const CommandGuardParameters &command_guard_parameters, - const std::string &command_guard_variant, - const StateInterfaceParameters &state_interface_parameters, const bool &open_loop) +AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, + const std::string &command_guard_variant, + const StateInterfaceParameters &state_interface_parameters, + const bool &open_loop) : command_interface_(command_guard_parameters, command_guard_variant), state_interface_(state_interface_parameters), open_loop_(open_loop) { - RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Configuring client."); - RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Command guard variant: %s.", + RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Configuring client."); + RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Command guard variant: %s.", command_guard_variant.c_str()); command_interface_.log_info(); state_interface_.log_info(); - RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Open loop: %s.", + RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "Client configured."); + RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "AsyncClient configured."); } -void Client::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(rclcpp::get_logger(CLIENT_LOGGER_NAME), "LBR switched from %s to %s.", +void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, + KUKA::FRI::ESessionState new_state) { + RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_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()); @@ -38,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); @@ -59,7 +61,7 @@ void Client::command() { default: std::string err = "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + "."; - RCLCPP_ERROR(rclcpp::get_logger(CLIENT_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } } From 4bc01739dd91ef2fcb17c049f00864e1005142e9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 16:33:46 +0000 Subject: [PATCH 42/82] simplified logger names --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 2 +- .../include/lbr_fri_ros2/async_client.hpp | 2 +- .../include/lbr_fri_ros2/command_guard.hpp | 2 +- .../lbr_fri_ros2/command_interface.hpp | 2 +- .../include/lbr_fri_ros2/state_interface.hpp | 2 +- lbr_fri_ros2/src/app.cpp | 43 +++++++++---------- lbr_fri_ros2/src/async_client.cpp | 13 +++--- lbr_fri_ros2/src/command_guard.cpp | 24 +++++------ lbr_fri_ros2/src/command_interface.cpp | 18 ++++---- lbr_fri_ros2/src/state_interface.cpp | 8 ++-- 10 files changed, 54 insertions(+), 62 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index f8bb7aab..35cc1457 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -17,7 +17,7 @@ namespace lbr_fri_ros2 { class App { protected: - static constexpr char APP_LOGGER_NAME[] = "lbr_fri_ros2::App"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; public: App(const std::shared_ptr client_ptr); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index f75a3d84..568b5f9c 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -17,7 +17,7 @@ namespace lbr_fri_ros2 { class AsyncClient : public KUKA::FRI::LBRClient { protected: - static constexpr char ASYNC_CLIENT_LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; public: AsyncClient() = delete; 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 8e5da499..79f39f65 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -29,7 +29,7 @@ struct CommandGuardParameters { class CommandGuard { protected: - static constexpr char COMMAND_GUARD_LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; 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 566a1d88..002129c8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -18,7 +18,7 @@ namespace lbr_fri_ros2 { class CommandInterface { protected: - static constexpr char COMMAND_INTERFACE_LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; 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 555c512f..04211c95 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -19,7 +19,7 @@ struct StateInterfaceParameters { class StateInterface { protected: - static constexpr char STATE_INTERFACE_LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; // ROS IDL types using idl_state_t = lbr_fri_msgs::msg::LBRState; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 7635b67a..f13b22c4 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -16,98 +16,97 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), "Socket already open."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open."); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Failed to open socket."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to open socket."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Closing UDP socket."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Closing UDP socket."); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Socket already closed."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed."); return true; } connection_ptr_->close(); - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), "AsyncClient not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); return; } if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not open."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not open."); return; } if (!app_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "App not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "App not configured."); return; } if (running_) { - RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Failed to set FIFO realtime scheduling policy."); } } else { - RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), - "Realtime kernel recommended but not required."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required."); } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), "LBR in session state idle, exiting."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting."); break; } } client_ptr_->get_state_interface().uninitialize(); running_ = false; - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Exiting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread."); }); run_thread_.detach(); } void App::stop_run() { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "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(rclcpp::get_logger(APP_LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", port_id); return false; } diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 8d994c7a..66a5fff2 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -7,19 +7,18 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, const bool &open_loop) : command_interface_(command_guard_parameters, command_guard_variant), state_interface_(state_interface_parameters), open_loop_(open_loop) { - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Configuring client."); - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Command guard variant: %s.", + 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(ASYNC_CLIENT_LOGGER_NAME), "Open loop: %s.", - open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "AsyncClient configured."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "AsyncClient configured."); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "LBR switched from %s to %s.", + 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()); @@ -61,7 +60,7 @@ void AsyncClient::command() { default: std::string err = "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + "."; - RCLCPP_ERROR(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), 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 2693ebbf..a98e22ae 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -28,18 +28,17 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, } return true; default: - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Invalid EClientCommandMode provided."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided."); return false; } return false; } void CommandGuard::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), "Parameters:"); + 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(COMMAND_GUARD_LOGGER_NAME), + 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], parameters_.max_position_[i], parameters_.max_velocity_[i], parameters_.max_torque_[i]); @@ -51,8 +50,7 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < parameters_.min_position_[i] || lbr_command.joint_position[i] > parameters_.max_position_[i]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Position command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -66,8 +64,8 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma 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 > parameters_.max_velocity_[i]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Velocity not in limits for joint %s.", parameters_.joint_names_[i].c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.", + parameters_.joint_names_[i].c_str()); return false; } } @@ -79,8 +77,7 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > parameters_.max_torque_[i]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Torque command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -97,8 +94,7 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l lbr_command.joint_position[i] > parameters_.max_position_[i] - parameters_.max_velocity_[i] * lbr_state.getSampleTime()) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Position command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -109,7 +105,7 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l std::unique_ptr command_guard_factory(const CommandGuardParameters &command_guard_parameters, const std::string &variant) { - constexpr char COMMANG_GUARD_FACTORY_LOGGER_NAME[] = "lbr_fri_ros2::command_guard_factory"; + constexpr char LOGGER_NAME[] = "lbr_fri_ros2::command_guard_factory"; if (variant == "default") { return std::make_unique(command_guard_parameters); } @@ -117,7 +113,7 @@ command_guard_factory(const CommandGuardParameters &command_guard_parameters, return std::make_unique(command_guard_parameters); } std::string err = "Invalid CommandGuard variant provided."; - RCLCPP_ERROR(rclcpp::get_logger(COMMANG_GUARD_FACTORY_LOGGER_NAME), 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 e95379ef..25677bc1 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -4,7 +4,7 @@ namespace lbr_fri_ros2 { CommandInterface::CommandInterface(const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) { - RCLCPP_INFO(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Configuring command interface with command guard '%s'.", command_guard_variant.c_str()); command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); @@ -14,12 +14,12 @@ 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -35,7 +35,7 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -46,12 +46,12 @@ 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -78,12 +78,12 @@ 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), 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(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -100,7 +100,7 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), 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/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 3eb97933..06f25a73 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -81,12 +81,10 @@ void StateInterface::init_filters_() { } void StateInterface::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), "Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), - " external_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " external_torque_cutoff_frequency: %f", parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), - " measured_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " measured_torque_cutoff_frequency: %f", parameters_.measured_torque_cutoff_frequency); } } // namespace lbr_fri_ros2 From 7064415e069615a7f2d96a42a98e286d1d740d3f Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 16:35:59 +0000 Subject: [PATCH 43/82] client -> async_client --- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 4 +- lbr_fri_ros2/src/app.cpp | 14 +++---- lbr_fri_ros2/src/app_component.cpp | 49 ++++++++++++----------- lbr_fri_ros2/src/app_component.hpp | 2 +- 4 files changed, 35 insertions(+), 34 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 35cc1457..708fd43e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -20,7 +20,7 @@ class App { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; public: - App(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); @@ -34,7 +34,7 @@ class App { 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/src/app.cpp b/lbr_fri_ros2/src/app.cpp index f13b22c4..e77f39ab 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -1,12 +1,12 @@ #include "lbr_fri_ros2/app.hpp" namespace lbr_fri_ros2 { -App::App(const std::shared_ptr client_ptr) - : should_stop_(true), running_(false), client_ptr_(nullptr), connection_ptr_(nullptr), +App::App(const std::shared_ptr async_client_ptr) + : should_stop_(true), running_(false), async_client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { - client_ptr_ = client_ptr; + 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() { @@ -51,7 +51,7 @@ bool App::close_udp_socket() { } void App::run(int rt_prio) { - if (!client_ptr_) { + if (!async_client_ptr_) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); return; } @@ -87,12 +87,12 @@ void App::run(int rt_prio) { 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) { + 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(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread."); }); diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index b07a6152..608c42fb 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -8,8 +8,8 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) { app_node_ptr_->declare_parameter("remote_host", std::string("")); app_node_ptr_->declare_parameter("rt_prio", 80); - client_ptr_ = std::make_shared(app_node_ptr_); - app_ptr_ = std::make_unique(app_node_ptr_, client_ptr_); + async_client_ptr_ = std::make_shared(app_node_ptr_); + app_ptr_ = std::make_unique(app_node_ptr_, async_client_ptr_); // default connect connect_(app_node_ptr_->get_parameter("port_id").as_int(), @@ -39,7 +39,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)); @@ -53,21 +53,21 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, RCLCPP_INFO( app_node_ptr_->get_logger(), "Control mode: %s.", - EnumMaps::control_mode_map(client_ptr_->get_state_interface().get_state().control_mode) + 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); + 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.", @@ -77,11 +77,11 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, 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( @@ -111,16 +111,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 +129,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 +149,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_) { + 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 +182,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 +192,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 edeb72de..dc8d73f6 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -81,7 +81,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 From 24c89d1a9d9dd923f3d2e4dbd7f99f670dac8351 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 16:47:48 +0000 Subject: [PATCH 44/82] updated log --- lbr_fri_ros2/src/command_guard.cpp | 4 ++-- lbr_fri_ros2/src/state_interface.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index a98e22ae..88ac4ae7 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -35,11 +35,11 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, } void CommandGuard::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Parameters:"); + 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", + "* Joint %s limits: Position: [%.1f, %.1f] deg, velocity: %.1f deg/s, torque: %.1f Nm", parameters_.joint_names_[i].c_str(), parameters_.min_position_[i], parameters_.max_position_[i], parameters_.max_velocity_[i], parameters_.max_torque_[i]); } diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 06f25a73..2d0216cb 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -81,10 +81,10 @@ void StateInterface::init_filters_() { } void StateInterface::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " external_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %f", parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " measured_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %f", parameters_.measured_torque_cutoff_frequency); } } // namespace lbr_fri_ros2 From 5075b7da8f461b1abc9c90021d4cf2428a5d1497 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:04:48 +0000 Subject: [PATCH 45/82] updated log --- .../include/lbr_fri_ros2/command_guard.hpp | 10 +++---- lbr_fri_ros2/src/async_client.cpp | 2 +- lbr_fri_ros2/src/command_guard.cpp | 29 +++++++++---------- 3 files changed, 20 insertions(+), 21 deletions(-) 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 79f39f65..37b4f1fd 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -20,11 +20,11 @@ struct CommandGuardParameters { using joint_array_t = std::array; using joint_name_array_t = std::array; - joint_name_array_t joint_names_; /**< Joint names.*/ - 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].*/ + joint_name_array_t joint_names; /**< Joint names.*/ + 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].*/ }; class CommandGuard { diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 66a5fff2..68a16492 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -13,7 +13,7 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, 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), "AsyncClient configured."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured."); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 88ac4ae7..d9cd835c 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -36,22 +36,23 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, 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) { + 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], - parameters_.max_position_[i], parameters_.max_velocity_[i], parameters_.max_torque_[i]); + 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] < parameters_.min_position_[i] || - lbr_command.joint_position[i] > parameters_.max_position_[i]) { + 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()); + parameters_.joint_names[i].c_str()); return false; } } @@ -63,9 +64,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 > - parameters_.max_velocity_[i]) { + parameters_.max_velocity[i]) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.", - parameters_.joint_names_[i].c_str()); + parameters_.joint_names[i].c_str()); return false; } } @@ -76,9 +77,9 @@ 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]) > - parameters_.max_torque_[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()); + parameters_.joint_names[i].c_str()); return false; } } @@ -89,13 +90,11 @@ 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] < - parameters_.min_position_[i] + - parameters_.max_velocity_[i] * lbr_state.getSampleTime() || + parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > - parameters_.max_position_[i] - - parameters_.max_velocity_[i] * lbr_state.getSampleTime()) { + 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()); + parameters_.joint_names[i].c_str()); return false; } } From fa82185d004431d2fbeabde3abc60f1aff9915da Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:05:29 +0000 Subject: [PATCH 46/82] updated app --- lbr_fri_ros2/CMakeLists.txt | 62 +++++++++++++++--------------- lbr_fri_ros2/src/app_component.cpp | 51 +++++++++++++++++++++++- lbr_fri_ros2/src/app_component.hpp | 3 ++ 3 files changed, 82 insertions(+), 34 deletions(-) diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 7a70c3f2..ad531301 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -46,7 +46,6 @@ ament_target_dependencies(lbr_fri_ros2 lbr_fri_msgs rclcpp realtime_tools - urdf ) target_link_libraries(lbr_fri_ros2 @@ -63,7 +62,6 @@ ament_export_dependencies( lbr_fri_msgs rclcpp realtime_tools - urdf ) install( @@ -82,36 +80,36 @@ install( DESTINATION share/lbr_fri_ros2 ) -# # app_component -# add_library(app_component -# SHARED -# src/app_component.cpp -# ) - -# target_include_directories(app_component -# PRIVATE src -# ) - -# ament_target_dependencies(app_component -# rclcpp -# urdf -# rclcpp_components -# ) - -# target_link_libraries(app_component -# lbr_fri_ros2 -# ) - -# rclcpp_components_register_node(app_component -# PLUGIN lbr_fri_ros2::AppComponent -# EXECUTABLE app -# ) - -# install( -# TARGETS app_component -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION lib/lbr_fri_ros2 -# ) +# app_component +add_library(app_component + SHARED + src/app_component.cpp +) + +target_include_directories(app_component + PRIVATE src +) + +ament_target_dependencies(app_component + rclcpp + urdf + rclcpp_components +) + +target_link_libraries(app_component + lbr_fri_ros2 +) + +rclcpp_components_register_node(app_component + PLUGIN lbr_fri_ros2::AppComponent + EXECUTABLE app +) + +install( + TARGETS app_component + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/lbr_fri_ros2 +) install( DIRECTORY config launch diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 608c42fb..9a56908c 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -7,9 +7,56 @@ 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("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); - async_client_ptr_ = std::make_shared(app_node_ptr_); - app_ptr_ = std::make_unique(app_node_ptr_, async_client_ptr_); + // prepare parameters + 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(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(), diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index dc8d73f6..b23e05d5 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" @@ -15,7 +16,9 @@ #include "lbr_fri_msgs/srv/app_disconnect.hpp" #include "lbr_fri_ros2/app.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/state_interface.hpp" namespace lbr_fri_ros2 { /** From 9ff07e548e74bb27e3c14c6a4666329edb4a7690 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:06:47 +0000 Subject: [PATCH 47/82] updated log --- lbr_fri_ros2/src/async_client.cpp | 2 +- lbr_fri_ros2/src/command_interface.cpp | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 68a16492..b56abf33 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -8,7 +8,7 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, : command_interface_(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.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: '%s'.", command_guard_variant.c_str()); command_interface_.log_info(); state_interface_.log_info(); diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 25677bc1..79bccf31 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -4,9 +4,6 @@ namespace lbr_fri_ros2 { CommandInterface::CommandInterface(const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), - "Configuring command interface with command guard '%s'.", - command_guard_variant.c_str()); command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; From 56932f75edeebe05cc66cead414c7dec29fb025c Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:12:50 +0000 Subject: [PATCH 48/82] updated log --- lbr_fri_ros2/src/app_component.cpp | 6 +++--- lbr_fri_ros2/src/async_client.cpp | 4 ++-- lbr_fri_ros2/src/command_guard.cpp | 10 ++++++---- lbr_fri_ros2/src/state_interface.cpp | 4 ++-- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 9a56908c..c2d767ed 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -99,7 +99,7 @@ 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.", + 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.", @@ -117,12 +117,12 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, 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(), "AsyncClient command mode: %s.", + RCLCPP_INFO(app_node_ptr_->get_logger(), "AsyncClient command mode: '%s'.", EnumMaps::client_command_mode_map( async_client_ptr_->get_state_interface().get_state().client_command_mode) .c_str()); diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index b56abf33..e3d53b44 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -12,13 +12,13 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, 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), "Open loop: '%s'.", open_loop_ ? "true" : "false"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured."); } 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.", + 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()); diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index d9cd835c..f663781d 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -51,7 +51,8 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { 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.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Position command not in limits for joint '%s'.", parameters_.joint_names[i].c_str()); return false; } @@ -65,7 +66,7 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma 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 > parameters_.max_velocity[i]) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint '%s'.", parameters_.joint_names[i].c_str()); return false; } @@ -78,7 +79,7 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { 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.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint '%s'.", parameters_.joint_names[i].c_str()); return false; } @@ -93,7 +94,8 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > 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.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), + "Position command not in limits for joint '%s'.", parameters_.joint_names[i].c_str()); return false; } diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 2d0216cb..e504a411 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -82,9 +82,9 @@ void StateInterface::init_filters_() { void StateInterface::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %.1f", parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f", parameters_.measured_torque_cutoff_frequency); } } // namespace lbr_fri_ros2 From b24ee8d51c420ae762dfa34b904da75febd9dfd2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:16:57 +0000 Subject: [PATCH 49/82] updated config --- lbr_fri_ros2/config/app.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/config/app.yaml b/lbr_fri_ros2/config/app.yaml index 89df7fcc..bd1ae50a 100644 --- a/lbr_fri_ros2/config/app.yaml +++ b/lbr_fri_ros2/config/app.yaml @@ -4,5 +4,6 @@ app: remote_host: null # if null, any IP is accepted rt_prio: 80 # real-time priority of the FRI thread 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 From 083e43f3f607bdf267f5cf35d0771aca1b6bf138 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:34:04 +0000 Subject: [PATCH 50/82] improved log --- lbr_fri_ros2/src/app_component.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index c2d767ed..0fe06447 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -102,8 +102,9 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host, 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.", - async_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); From 75e26f7a6cee545ef4bad30f6eb930b8bbb9b32e Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 18:07:21 +0000 Subject: [PATCH 51/82] added explicit zero init --- lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 37b4f1fd..d6bc0c21 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -20,11 +20,11 @@ struct CommandGuardParameters { using joint_array_t = std::array; using joint_name_array_t = std::array; - joint_name_array_t joint_names; /**< Joint names.*/ - 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].*/ + joint_name_array_t joint_names; /**< Joint names.*/ + joint_array_t min_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/ + joint_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/ + joint_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/ + joint_array_t max_torque{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ }; class CommandGuard { From d819e525e0e53d65a7e11c43a97dfd7d3a6472f6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 18:14:04 +0000 Subject: [PATCH 52/82] re-named velocity limits --- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 28 +++++++++---------- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 28 +++++++++---------- .../urdf/med14/med14_description.urdf.xacro | 28 +++++++++---------- .../urdf/med7/med7_description.urdf.xacro | 28 +++++++++---------- 4 files changed, 56 insertions(+), 56 deletions(-) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index cad4fcb9..d3d40b68 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -13,13 +13,13 @@ - - - - - - - + + + + + + + @@ -58,7 +58,7 @@ + velocity="${A1_velocity_limit * PI / 180}" /> @@ -90,7 +90,7 @@ + velocity="${A2_velocity_limit * PI / 180}" /> @@ -122,7 +122,7 @@ + velocity="${A3_velocity_limit * PI / 180}" /> @@ -154,7 +154,7 @@ + velocity="${A4_velocity_limit * PI / 180}" /> @@ -186,7 +186,7 @@ + velocity="${A5_velocity_limit * PI / 180}" /> @@ -218,7 +218,7 @@ + velocity="${A6_velocity_limit * PI / 180}" /> @@ -250,7 +250,7 @@ + velocity="${A7_velocity_limit * PI / 180}" /> diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 85055dbe..77987aa1 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -13,13 +13,13 @@ - - - - - - - + + + + + + + @@ -58,7 +58,7 @@ + velocity="${A1_velocity_limit * PI / 180}" /> @@ -90,7 +90,7 @@ + velocity="${A2_velocity_limit * PI / 180}" /> @@ -122,7 +122,7 @@ + velocity="${A3_velocity_limit * PI / 180}" /> @@ -154,7 +154,7 @@ + velocity="${A4_velocity_limit * PI / 180}" /> @@ -186,7 +186,7 @@ + velocity="${A5_velocity_limit * PI / 180}" /> @@ -218,7 +218,7 @@ + velocity="${A6_velocity_limit * PI / 180}" /> @@ -251,7 +251,7 @@ + velocity="${A7_velocity_limit * PI / 180}" /> diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 9b16af2e..2b82fe65 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -13,13 +13,13 @@ - - - - - - - + + + + + + + @@ -58,7 +58,7 @@ + velocity="${A1_velocity_limit * PI / 180}" /> @@ -90,7 +90,7 @@ + velocity="${A2_velocity_limit * PI / 180}" /> @@ -122,7 +122,7 @@ + velocity="${A3_velocity_limit * PI / 180}" /> @@ -154,7 +154,7 @@ + velocity="${A4_velocity_limit * PI / 180}" /> @@ -186,7 +186,7 @@ + velocity="${A5_velocity_limit * PI / 180}" /> @@ -218,7 +218,7 @@ + velocity="${A6_velocity_limit * PI / 180}" /> @@ -250,7 +250,7 @@ + velocity="${A7_velocity_limit * PI / 180}" /> diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index a04fd08d..4c3d8363 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -13,13 +13,13 @@ - - - - - - - + + + + + + + @@ -58,7 +58,7 @@ + velocity="${A1_velocity_limit * PI / 180}" /> @@ -90,7 +90,7 @@ + velocity="${A2_velocity_limit * PI / 180}" /> @@ -122,7 +122,7 @@ + velocity="${A3_velocity_limit * PI / 180}" /> @@ -154,7 +154,7 @@ + velocity="${A4_velocity_limit * PI / 180}" /> @@ -186,7 +186,7 @@ + velocity="${A5_velocity_limit * PI / 180}" /> @@ -218,7 +218,7 @@ + velocity="${A6_velocity_limit * PI / 180}" /> @@ -250,7 +250,7 @@ + velocity="${A7_velocity_limit * PI / 180}" /> From b6770cf2d484488a6ff61647aec3d92f09c18e52 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 18:40:37 +0000 Subject: [PATCH 53/82] added params for ros2 control --- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 9 ++- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 9 ++- .../urdf/med14/med14_description.urdf.xacro | 9 ++- .../urdf/med7/med7_description.urdf.xacro | 9 ++- .../config/lbr_system_interface.xacro | 56 ++++++++++++++----- 5 files changed, 74 insertions(+), 18 deletions(-) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index d3d40b68..e568050d 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -293,7 +293,14 @@ A5_position_limit="${A5_position_limit * PI / 180}" A6_position_limit="${A6_position_limit * PI / 180}" A7_position_limit="${A7_position_limit * PI / 180}" - effort_limit="${effort_limit}" + A1_velocity_limit="${A1_velocity_limit * PI / 180}" + A2_velocity_limit="${A2_velocity_limit * PI / 180}" + A3_velocity_limit="${A3_velocity_limit * PI / 180}" + A4_velocity_limit="${A4_velocity_limit * PI / 180}" + A5_velocity_limit="${A5_velocity_limit * PI / 180}" + A6_velocity_limit="${A6_velocity_limit * PI / 180}" + A7_velocity_limit="${A7_velocity_limit * PI / 180}" + max_torque="${effort_limit}" sim="${sim}" remote_host="INADDR_ANY" port_id="${port_id}" /> diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 77987aa1..21cdbd49 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -294,7 +294,14 @@ A5_position_limit="${A5_position_limit * PI / 180}" A6_position_limit="${A6_position_limit * PI / 180}" A7_position_limit="${A7_position_limit * PI / 180}" - effort_limit="${effort_limit}" + A1_velocity_limit="${A1_velocity_limit * PI / 180}" + A2_velocity_limit="${A2_velocity_limit * PI / 180}" + A3_velocity_limit="${A3_velocity_limit * PI / 180}" + A4_velocity_limit="${A4_velocity_limit * PI / 180}" + A5_velocity_limit="${A5_velocity_limit * PI / 180}" + A6_velocity_limit="${A6_velocity_limit * PI / 180}" + A7_velocity_limit="${A7_velocity_limit * PI / 180}" + max_torque="${effort_limit}" sim="${sim}" remote_host="INADDR_ANY" port_id="${port_id}" /> diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 2b82fe65..717dea39 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -293,7 +293,14 @@ A5_position_limit="${A5_position_limit * PI / 180}" A6_position_limit="${A6_position_limit * PI / 180}" A7_position_limit="${A7_position_limit * PI / 180}" - effort_limit="${effort_limit}" + A1_velocity_limit="${A1_velocity_limit * PI / 180}" + A2_velocity_limit="${A2_velocity_limit * PI / 180}" + A3_velocity_limit="${A3_velocity_limit * PI / 180}" + A4_velocity_limit="${A4_velocity_limit * PI / 180}" + A5_velocity_limit="${A5_velocity_limit * PI / 180}" + A6_velocity_limit="${A6_velocity_limit * PI / 180}" + A7_velocity_limit="${A7_velocity_limit * PI / 180}" + max_torque="${effort_limit}" sim="${sim}" remote_host="INADDR_ANY" port_id="${port_id}" /> diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index 4c3d8363..cba06813 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -293,7 +293,14 @@ A5_position_limit="${A5_position_limit * PI / 180}" A6_position_limit="${A6_position_limit * PI / 180}" A7_position_limit="${A7_position_limit * PI / 180}" - effort_limit="${effort_limit}" + A1_velocity_limit="${A1_velocity_limit * PI / 180}" + A2_velocity_limit="${A2_velocity_limit * PI / 180}" + A3_velocity_limit="${A3_velocity_limit * PI / 180}" + A4_velocity_limit="${A4_velocity_limit * PI / 180}" + A5_velocity_limit="${A5_velocity_limit * PI / 180}" + A6_velocity_limit="${A6_velocity_limit * PI / 180}" + A7_velocity_limit="${A7_velocity_limit * PI / 180}" + max_torque="${effort_limit}" sim="${sim}" remote_host="INADDR_ANY" port_id="${port_id}" /> diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 6dd14dc5..f65c13d9 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -9,8 +9,22 @@ A5_position_limit A6_position_limit A7_position_limit - effort_limit - sim:=^|true port_id:=^|30200 remote_host:=^|INADDR_ANY rt_prio:=^|80 open_loop:=^|true"> + A1_velocity_limit + A2_velocity_limit + A3_velocity_limit + A4_velocity_limit + A5_velocity_limit + A6_velocity_limit + A7_velocity_limit + max_torque + sim:=^|true + port_id:=^|30200 + remote_host:=^|INADDR_ANY + rt_prio:=^|80 + command_guard_variant:=^|default + external_torque_cutoff_frequency:=^|10 + measured_torque_cutoff_frequency:=^|10 + open_loop:=^|true"> @@ -26,6 +40,11 @@ ${port_id} ${remote_host} ${rt_prio} + ${command_guard_variant} + + ${external_torque_cutoff_frequency} + + ${measured_torque_cutoff_frequency} ${open_loop} @@ -44,25 +63,27 @@ - - + params="name min_position max_position max_velocity max_torque sim"> + ${min_position} + ${max_position} + ${max_velocity} + ${max_torque} ${min_position} ${max_position} - -${effort_limit} - ${effort_limit} + -${max_torque} + ${max_torque} @@ -77,19 +98,26 @@ + max_position="${A1_position_limit}" max_velocity="${A1_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A2_position_limit}" max_velocity="${A2_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A3_position_limit}" max_velocity="${A3_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A4_position_limit}" max_velocity="${A4_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A5_position_limit}" max_velocity="${A5_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A6_position_limit}" max_velocity="${A6_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> + max_position="${A7_position_limit}" max_velocity="${A7_velocity_limit}" + max_torque="${max_torque}" sim="${sim}" /> \ No newline at end of file From 72a1f3fd8bba3728364940f949eb1d8d797988e9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 18:45:08 +0000 Subject: [PATCH 54/82] removed node from system interface --- .../lbr_ros2_control/system_interface.hpp | 29 +++-- lbr_ros2_control/src/system_interface.cpp | 122 ++++++++++-------- 2 files changed, 87 insertions(+), 64 deletions(-) diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 3cdf6c5a..a64a1de0 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -3,7 +3,6 @@ #include #include -#include #include #include #include @@ -21,12 +20,27 @@ #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_fri_ros2/state_interface.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { +struct SystemInterfaceParameters { + int32_t port_id{30200}; + const char *remote_host{nullptr}; + int32_t rt_prio{80}; + bool open_loop{true}; + std::string command_guard_variant{"default"}; + double external_torque_cutoff_frequency{10.0}; + double measured_torque_cutoff_frequency{10.0}; +}; + class SystemInterface : public hardware_interface::SystemInterface { +protected: + static constexpr char LOGGER[] = "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_SENSOR_SIZE = 12; @@ -68,17 +82,14 @@ class SystemInterface : public hardware_interface::SystemInterface { const KUKA::FRI::ESessionState &session_state); // robot parameters - int32_t port_id_; - const char *remote_host_; - int32_t rt_prio_; - bool open_loop_; + SystemInterfaceParameters parameters_; // robot driver - rclcpp::Node::SharedPtr app_node_ptr_; - std::shared_ptr client_ptr_; + std::shared_ptr async_client_ptr_; std::unique_ptr app_ptr_; - // exposed state interfaces + // exposed state interfaces (ideally these are taken from async_client_ptr_ but + // ros2_control ReadOnlyHandle does not allow for const pointers) lbr_fri_msgs::msg::LBRState hw_lbr_state_; // state interfaces that require cast, this could be mitigated by defining LBRState exclusively diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index f3df1e76..fb713813 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -5,37 +5,54 @@ 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(app_node_ptr_->get_logger(), "Failed to initialize SystemInterface."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "Failed to initialize SystemInterface."); return ret; } - // parameters from config/lbr_system_interface.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_); + // 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), "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"]); + 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"]); 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_); + parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; + + // setup driver + lbr_fri_ros2::CommandGuardParameters command_guard_parameters; + lbr_fri_ros2::StateInterfaceParameters state_interface_parameters; + 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; - client_ptr_ = std::make_shared(app_node_ptr_); - app_ptr_ = std::make_unique(app_node_ptr_, client_ptr_); + async_client_ptr_ = std::make_shared( + 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_(); @@ -112,7 +129,6 @@ std::vector SystemInterface::export_state_in 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]); @@ -120,10 +136,6 @@ std::vector SystemInterface::export_comman command_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_lbr_command_.torque[i]); } - - // prefix? - // /link_ee/ - return command_interfaces; } @@ -134,33 +146,32 @@ SystemInterface::prepare_command_mode_switch(const std::vector & /* } controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { - if (!client_ptr_) { - RCLCPP_ERROR(app_node_ptr_->get_logger(), "Client not configured."); + if (!async_client_ptr_) { + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "AsyncClient not configured."); return controller_interface::CallbackReturn::ERROR; } - if (!app_ptr_->open_udp_socket(port_id_, remote_host_)) { + if (!app_ptr_->open_udp_socket(parameters_.port_id, parameters_.remote_host)) { return controller_interface::CallbackReturn::ERROR; } - app_ptr_->run(rt_prio_); + app_ptr_->run(parameters_.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_); + while (!async_client_ptr_->get_state_interface().is_initialized() && rclcpp::ok()) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER), "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(app_node_ptr_->get_logger(), "Failed to connect to robot on max attempts."); + RCLCPP_ERROR(rclcpp::get_logger(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); + RCLCPP_INFO(rclcpp::get_logger(LOGGER), "Robot connected."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER), "Control mode: '%s'.", + lbr_fri_ros2::EnumMaps::control_mode_map(hw_lbr_state_.control_mode).c_str()); + RCLCPP_INFO(rclcpp::get_logger(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); return controller_interface::CallbackReturn::SUCCESS; } @@ -173,11 +184,12 @@ SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // read state - hw_lbr_state_ = client_ptr_->get_state_interface().get_state(); + 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(app_node_ptr_->get_logger(), + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup."); app_ptr_->stop_run(); app_ptr_->close_udp_socket(); @@ -213,7 +225,7 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti [](const double &v) { return std::isnan(v); })) { return hardware_interface::return_type::OK; } - client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); + 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) { @@ -223,7 +235,7 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti [](const double &v) { return std::isnan(v); })) { return hardware_interface::return_type::OK; } - client_ptr_->get_command_interface().set_command_target(hw_lbr_command_); + 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) { @@ -267,7 +279,7 @@ void SystemInterface::nan_state_interfaces_() { bool SystemInterface::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.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "Expected %d joints in URDF. Found %ld.", KUKA::FRI::LBRState::NUMBER_OF_JOINTS, info_.joints.size()); return false; } @@ -279,7 +291,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR( - app_node_ptr_->get_logger(), + rclcpp::get_logger(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; @@ -287,7 +299,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { 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(), + RCLCPP_ERROR(rclcpp::get_logger(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); @@ -303,7 +315,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR( - app_node_ptr_->get_logger(), + rclcpp::get_logger(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; @@ -315,7 +327,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { 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(), + rclcpp::get_logger(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, @@ -331,14 +343,14 @@ bool SystemInterface::verify_joint_state_interfaces_() { bool SystemInterface::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()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "Expected 1 sensor, got %ld", info_.sensors.size()); return false; } // check all interfaces are defined in config/lbr_system_interface.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(), + RCLCPP_ERROR(rclcpp::get_logger(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); @@ -355,7 +367,7 @@ bool SystemInterface::verify_sensors_() { 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.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "Sensor %s received invalid state interface %s.", lbr_fri_sensor.name.c_str(), si.name.c_str()); return false; From 7b486b72310918b5e0a5fe2bccf5a61b48e0055b Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 19:05:38 +0000 Subject: [PATCH 55/82] removed unused ref --- lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 04211c95..f230587f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -36,7 +36,7 @@ class StateInterface { StateInterface() = delete; 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); From 44d941c86d1d4ac26aac6898f2506d0f69044072 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 19:17:46 +0000 Subject: [PATCH 56/82] added doc --- .../include/lbr_ros2_control/system_interface.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index a64a1de0..5e8017ba 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -89,11 +89,11 @@ class SystemInterface : public hardware_interface::SystemInterface { std::unique_ptr app_ptr_; // exposed state interfaces (ideally these are taken from async_client_ptr_ but - // ros2_control ReadOnlyHandle does not allow for const pointers) + // 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_; - // state interfaces that require cast, this could be mitigated by defining LBRState exclusively - // with doubles + // exposed state interfaces that require casting double hw_session_state_; double hw_connection_quality_; double hw_safety_state_; From 5150746ddf7129c2c16a63f59ec233b668002cb4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 20:37:51 +0000 Subject: [PATCH 57/82] added pid params --- lbr_fri_ros2/config/app.yaml | 6 ++ .../include/lbr_fri_ros2/async_client.hpp | 4 +- .../lbr_fri_ros2/command_interface.hpp | 4 +- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 19 ++++- lbr_fri_ros2/launch/app.launch.py | 2 + lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py | 72 +++++++++++++++++++ lbr_fri_ros2/src/app_component.cpp | 18 ++++- lbr_fri_ros2/src/app_component.hpp | 1 + lbr_fri_ros2/src/async_client.cpp | 5 +- lbr_fri_ros2/src/command_interface.cpp | 15 ++-- lbr_fri_ros2/src/filters.cpp | 21 ++++-- 11 files changed, 150 insertions(+), 17 deletions(-) diff --git a/lbr_fri_ros2/config/app.yaml b/lbr_fri_ros2/config/app.yaml index bd1ae50a..7b34cbc6 100644 --- a/lbr_fri_ros2/config/app.yaml +++ b/lbr_fri_ros2/config/app.yaml @@ -3,6 +3,12 @@ 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 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 568b5f9c..8c3f2bfb 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -12,6 +12,7 @@ #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 { @@ -21,7 +22,8 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; - AsyncClient(const CommandGuardParameters &command_guard_parameters, + 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); 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 002129c8..22bb47c6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -32,7 +32,8 @@ class CommandInterface { public: CommandInterface() = delete; - CommandInterface(const CommandGuardParameters &command_guard_parameters, + 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); @@ -48,6 +49,7 @@ class CommandInterface { protected: std::unique_ptr command_guard_; + PIDParameters pid_parameters_; JointPIDArray joint_position_pid_; idl_command_t command_, command_target_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index ea15392b..4a6ae23a 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -8,6 +8,8 @@ #include "control_toolbox/filters.hpp" #include "control_toolbox/pid_ros.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "friLBRClient.h" @@ -110,7 +112,18 @@ class JointExponentialFilterArray { 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; @@ -121,12 +134,14 @@ class JointPIDArray { 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 double &p, const double &i, const double &d, const double &i_max, - const double &i_min, const bool &antiwindup); + 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.*/ + PIDParameters parameters_; /**< PID parameters.*/ pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // end of namespace lbr_fri_ros2 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/src/app_component.cpp b/lbr_fri_ros2/src/app_component.cpp index 0fe06447..a42bd61c 100644 --- a/lbr_fri_ros2/src/app_component.cpp +++ b/lbr_fri_ros2/src/app_component.cpp @@ -8,12 +8,25 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) { 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); // 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(); @@ -54,8 +67,9 @@ AppComponent::AppComponent(const rclcpp::NodeOptions &options) { } // configure client - async_client_ptr_ = std::make_shared(command_guard_parameters, command_guard_variant, - state_interface_parameters, open_loop); + 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 diff --git a/lbr_fri_ros2/src/app_component.hpp b/lbr_fri_ros2/src/app_component.hpp index b23e05d5..e818ecae 100644 --- a/lbr_fri_ros2/src/app_component.hpp +++ b/lbr_fri_ros2/src/app_component.hpp @@ -18,6 +18,7 @@ #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 { diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index e3d53b44..923bf6db 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -1,11 +1,12 @@ #include "lbr_fri_ros2/async_client.hpp" namespace lbr_fri_ros2 { -AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, +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_(command_guard_parameters, command_guard_variant), + : 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'.", diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 79bccf31..79cb6312 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -2,8 +2,10 @@ namespace lbr_fri_ros2 { -CommandInterface::CommandInterface(const CommandGuardParameters &command_guard_parameters, - const std::string &command_guard_variant) { +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); }; @@ -22,7 +24,8 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -54,7 +57,8 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -86,7 +90,8 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(state.getSampleTime() * 1.0, 0.0, 0.0, 0.0, 0.0, false); + joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 017e37a1..2086971f 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -59,10 +59,23 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s }); } -void JointPIDArray::initialize(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); }); +void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) { + parameters_ = pid_parameters; + std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { + pid.initPid(parameters_.p * dt, parameters_.i * dt, parameters_.d * dt, parameters_.i_max * dt, + parameters_.i_min * dt, parameters_.antiwindup); + }); initialized_ = true; } + +void JointPIDArray::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.p: %f", parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %f", parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %f", parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %f", parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %f", parameters_.i_min); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.antiwindup: %s", + parameters_.antiwindup ? "true" : "false"); +} } // end of namespace lbr_fri_ros2 From 3cb651a2418f67bbdc40d0eae2af3a5b7add73dc Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 20:49:54 +0000 Subject: [PATCH 58/82] added pid parameters --- .../config/lbr_system_interface.xacro | 12 ++++++++ .../lbr_ros2_control/system_interface.hpp | 7 +++++ lbr_ros2_control/src/system_interface.cpp | 28 +++++++++++++++---- 3 files changed, 41 insertions(+), 6 deletions(-) diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index f65c13d9..375af462 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -21,6 +21,12 @@ port_id:=^|30200 remote_host:=^|INADDR_ANY rt_prio:=^|80 + pid_p:=^|1.0 + pid_i:=^|0.0 + pid_d:=^|0.0 + pid_i_max:=^|0.0 + pid_i_min:=^|0.0 + pid_antiwindup:=^|false command_guard_variant:=^|default external_torque_cutoff_frequency:=^|10 measured_torque_cutoff_frequency:=^|10 @@ -40,6 +46,12 @@ ${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} diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 5e8017ba..e440f89f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -23,6 +23,7 @@ #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" #include "lbr_ros2_control/system_interface_type_values.hpp" @@ -32,6 +33,12 @@ struct SystemInterfaceParameters { const char *remote_host{nullptr}; int32_t rt_prio{80}; bool open_loop{true}; + double pid_p{0.0}; + double pid_i{0.0}; + double pid_d{0.0}; + double pid_i_max{0.0}; + double pid_i_min{0.0}; + double pid_antiwindup{0.0}; std::string command_guard_variant{"default"}; double external_torque_cutoff_frequency{10.0}; double measured_torque_cutoff_frequency{10.0}; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index fb713813..863e82e7 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -20,19 +20,35 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ? 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"]); - 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"; // 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] = @@ -50,8 +66,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { parameters_.measured_torque_cutoff_frequency; async_client_ptr_ = std::make_shared( - command_guard_parameters, parameters_.command_guard_variant, state_interface_parameters, - parameters_.open_loop); + 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_(); From 39d5858ca6a6f25c80b454fbcb6d77f24915f6a3 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 20:58:00 +0000 Subject: [PATCH 59/82] moved log --- lbr_fri_ros2/src/command_interface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 79cb6312..109794c2 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -25,7 +25,6 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -58,7 +57,6 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -91,7 +89,6 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -119,5 +116,8 @@ void CommandInterface::init_command(const_fri_state_t_ref state) { command_ = command_target_; } -void CommandInterface::log_info() const { command_guard_->log_info(); } +void CommandInterface::log_info() const { + command_guard_->log_info(); + joint_position_pid_.log_info(); +} } // namespace lbr_fri_ros2 From 7a20d39c95570ae74131d02dc2f54846b350ffe6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 21:00:11 +0000 Subject: [PATCH 60/82] moved log --- lbr_fri_ros2/src/command_interface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index 109794c2..79cb6312 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -25,6 +25,7 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -57,6 +58,7 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -89,6 +91,7 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); + joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -116,8 +119,5 @@ void CommandInterface::init_command(const_fri_state_t_ref state) { command_ = command_target_; } -void CommandInterface::log_info() const { - command_guard_->log_info(); - joint_position_pid_.log_info(); -} +void CommandInterface::log_info() const { command_guard_->log_info(); } } // namespace lbr_fri_ros2 From 5e5c75abac812740ccd6390efba19d37446f679a Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 21:03:27 +0000 Subject: [PATCH 61/82] moved logging --- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 3 --- lbr_fri_ros2/src/command_interface.cpp | 15 +++++++++++---- lbr_fri_ros2/src/filters.cpp | 16 ++-------------- 3 files changed, 13 insertions(+), 21 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 4a6ae23a..b9929479 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -8,8 +8,6 @@ #include "control_toolbox/filters.hpp" #include "control_toolbox/pid_ros.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/logging.hpp" #include "friLBRClient.h" @@ -141,7 +139,6 @@ class JointPIDArray { protected: bool initialized_{false}; /**< True if initialized.*/ - PIDParameters parameters_; /**< PID parameters.*/ pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // 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 79cb6312..df82f164 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -25,7 +25,6 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -58,7 +57,6 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -91,7 +89,6 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // PID if (!joint_position_pid_.is_initialized()) { joint_position_pid_.initialize(pid_parameters_, state.getSampleTime()); - joint_position_pid_.log_info(); } joint_position_pid_.compute( command_target_.joint_position, state.getMeasuredJointPosition(), @@ -119,5 +116,15 @@ void CommandInterface::init_command(const_fri_state_t_ref state) { command_ = command_target_; } -void CommandInterface::log_info() const { command_guard_->log_info(); } +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: %f", pid_parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %f", pid_parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %f", pid_parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %f", pid_parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %f", 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 index 2086971f..27d23034 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -60,22 +60,10 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s } void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) { - parameters_ = pid_parameters; std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(parameters_.p * dt, parameters_.i * dt, parameters_.d * dt, parameters_.i_max * dt, - parameters_.i_min * dt, parameters_.antiwindup); + 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; } - -void JointPIDArray::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.p: %f", parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %f", parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %f", parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %f", parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %f", parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.antiwindup: %s", - parameters_.antiwindup ? "true" : "false"); -} } // end of namespace lbr_fri_ros2 From 08358cddac81c44a11826459fae37eef698ebacf Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 21:04:29 +0000 Subject: [PATCH 62/82] added unit --- lbr_fri_ros2/src/state_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index e504a411..5cf5597b 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -82,9 +82,9 @@ void StateInterface::init_filters_() { 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", + 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", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f Hz", parameters_.measured_torque_cutoff_frequency); } } // namespace lbr_fri_ros2 From 4d0304d92bd2efd055bfacec08a3d6c2c4c4ef30 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 21:43:40 +0000 Subject: [PATCH 63/82] fixed const --- lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp index 165ad9cf..1616c27e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp @@ -19,7 +19,7 @@ pinv(const MatT &mat, mat.cols(), mat.rows()); dampedSingularValuesInv.setZero(); std::for_each(singularValues.data(), singularValues.data() + singularValues.size(), - [&, i = 0](Scalar &s) mutable { + [&, i = 0](const Scalar &s) mutable { dampedSingularValuesInv(i, i) = s / (s * s + lambda * lambda); ++i; }); From 787f46d4c61f1ab25f11337acbb0da3fccc44e00 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 21:59:46 +0000 Subject: [PATCH 64/82] added a ft estimator implementation --- lbr_fri_ros2/CMakeLists.txt | 7 +++ .../include/lbr_fri_ros2/ft_estimator.hpp | 46 +++++++++++++++++++ lbr_fri_ros2/package.xml | 2 + lbr_fri_ros2/src/ft_estimator.cpp | 34 ++++++++++++++ 4 files changed, 89 insertions(+) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp create mode 100644 lbr_fri_ros2/src/ft_estimator.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index ad531301..189794d6 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -17,7 +17,9 @@ 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) @@ -31,6 +33,7 @@ add_library(lbr_fri_ros2 src/command_guard.cpp src/command_interface.cpp src/filters.cpp + src/ft_estimator.cpp src/state_interface.cpp ) @@ -43,7 +46,9 @@ 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 ) @@ -59,7 +64,9 @@ ament_export_dependencies( Eigen3 fri_vendor FRIClient + kdl_parser lbr_fri_msgs + orocos_kdl_vendor rclcpp realtime_tools ) 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..14d405e1 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -0,0 +1,46 @@ +#ifndef LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#define LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ + +#include +#include + +#include "eigen3/Eigen/Core" +#include "kdl/chain.hpp" +#include "kdl/chainjnttojacsolver.hpp" +#include "kdl/tree.hpp" +#include "kdl_parser/kdl_parser.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +#include "lbr_fri_msgs/msg/lbr_state.hpp" +#include "lbr_fri_ros2/pinv.hpp" + +#include "friLBRState.h" + +namespace lbr_fri_ros2 { +class FTEstimator { +protected: + static constexpr uint8_t CARTESIAN_DOF = 6; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; + +public: + FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", + const std::string &chain_tip = "link_ee"); + + void + compute(const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position, + const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque, + std::array &f_ext); + +protected: + KDL::Tree tree_; + KDL::Chain chain_; + std::unique_ptr jacobian_solver_; + KDL::Jacobian jacobian_; + Eigen::Matrix jacobian_inv_; + KDL::JntArray q_; + 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/package.xml b/lbr_fri_ros2/package.xml index b480f9d3..f4d1767b 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -16,7 +16,9 @@ control_toolbox fri_vendor + kdl_parser lbr_fri_msgs + orocos_kdl_vendor rclcpp rclcpp_components realtime_tools diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp new file mode 100644 index 00000000..b02e4231 --- /dev/null +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -0,0 +1,34 @@ +#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) { + 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_); + jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); +} + +void FTEstimator::compute( + const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position, + const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque, + std::array &f_ext) { + 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); + f_ext_ = jacobian_inv_.transpose() * tau_ext_; + Eigen::Map>(f_ext.data()) = f_ext_; +} +} // end of namespace lbr_fri_ros2 From 40db67625d2e47d2c274c0caba5ea3bf5016e80a Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 09:48:51 +0000 Subject: [PATCH 65/82] added the estimated ft sensor and broadcaster --- lbr_bringup/launch/real.launch.py | 4 + .../include/lbr_fri_ros2/command_guard.hpp | 20 +-- .../include/lbr_fri_ros2/ft_estimator.hpp | 19 ++- lbr_fri_ros2/src/ft_estimator.cpp | 6 +- lbr_ros2_control/config/lbr_controllers.yaml | 8 ++ .../config/lbr_system_interface.xacro | 11 +- .../lbr_ros2_control/system_interface.hpp | 15 +- .../system_interface_type_values.hpp | 8 ++ .../launch/system_interface.launch.py | 4 + .../src/lbr_state_broadcaster.cpp | 24 ++-- lbr_ros2_control/src/system_interface.cpp | 135 ++++++++++++------ 11 files changed, 175 insertions(+), 79 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index a82da17c..09e286a4 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -29,6 +29,9 @@ 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" ) @@ -41,6 +44,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: target_action=ros2_control_node, on_start=[ joint_state_broadcaster, + force_torque_broadcaster, lbr_state_broadcaster, controller, ], 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 d6bc0c21..7ebc0f2e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -17,14 +17,14 @@ namespace lbr_fri_ros2 { struct CommandGuardParameters { // ROS IDL types - using joint_array_t = std::array; - using joint_name_array_t = std::array; - - joint_name_array_t joint_names; /**< Joint names.*/ - joint_array_t min_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/ - joint_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/ - joint_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/ - joint_array_t max_torque{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ + 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 { @@ -34,8 +34,8 @@ class 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; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 14d405e1..0691a358 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -12,25 +12,30 @@ #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" -#include "friLBRState.h" - namespace lbr_fri_ros2 { class FTEstimator { protected: - static constexpr uint8_t CARTESIAN_DOF = 6; 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 &; + FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", const std::string &chain_tip = "link_ee"); - void - compute(const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position, - const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque, - std::array &f_ext); + 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); protected: KDL::Tree tree_; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index b02e4231..147ac2e6 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -18,10 +18,8 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); } -void FTEstimator::compute( - const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position, - const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque, - std::array &f_ext) { +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) { q_.data = Eigen::Map>( measured_joint_position.data()); tau_ext_ = Eigen::Map>( diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 073dde1a..e0842b44 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -8,6 +8,9 @@ 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 @@ -26,6 +29,11 @@ forward_lbr_torque_command_controller: type: lbr_ros2_control/ForwardLBRTorqueCommandController +/**/force_torque_broadcaster: + ros__parameters: + frame_id: lbr/link_ee + sensor_name: estimated_ft_sensor + /**/joint_trajectory_controller: ros__parameters: joints: diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 375af462..a052eb8c 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -64,7 +64,7 @@ - + @@ -79,6 +79,15 @@ + + + + + + + + + diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index e440f89f..cafe759a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -24,6 +24,7 @@ #include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/enum_maps.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" @@ -46,11 +47,13 @@ struct SystemInterfaceParameters { class SystemInterface : public hardware_interface::SystemInterface { protected: - static constexpr char LOGGER[] = "lbr_ros2_control::SystemInterface"; + 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_SENSOR_SIZE = 12; + 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: SystemInterface() = default; @@ -83,6 +86,8 @@ class SystemInterface : 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, @@ -112,7 +117,7 @@ class SystemInterface : public hardware_interface::SystemInterface { double hw_time_stamp_sec_; double hw_time_stamp_nano_sec_; - // added velocity state interface + // 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_; @@ -124,6 +129,10 @@ class SystemInterface : public hardware_interface::SystemInterface { void update_last_hw_states_(); void compute_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 hw_lbr_command_; }; diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp index 86e4ef63..8e898f6c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp @@ -26,6 +26,14 @@ 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 diff --git a/lbr_ros2_control/launch/system_interface.launch.py b/lbr_ros2_control/launch/system_interface.launch.py index 74664008..a8859041 100644 --- a/lbr_ros2_control/launch/system_interface.launch.py +++ b/lbr_ros2_control/launch/system_interface.launch.py @@ -28,6 +28,9 @@ 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" ) @@ -39,6 +42,7 @@ def generate_launch_description() -> LaunchDescription: target_action=ros2_control_node, on_start=[ joint_state_broadcaster, + force_torque_broadcaster, lbr_state_broadcaster, controller, ], diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index ed69eb68..39d4e0e1 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -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/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 863e82e7..590bb492 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -5,14 +5,14 @@ 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), "Failed to initialize SystemInterface."); + 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), "Expected port_id in [30200, 30209]. Found %d.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209]. Found %d.", parameters_.port_id); return controller_interface::CallbackReturn::ERROR; } @@ -74,6 +74,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { nan_state_interfaces_(); nan_last_hw_states_(); + ft_estimator_ptr_ = std::make_unique(info_.original_xml); + if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; } @@ -115,31 +117,42 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(info_.joints[i].name, HW_IF_IPO_JOINT_POSITION, &hw_lbr_state_.ipo_joint_position[i]); - // added velocity state interface + // additional velocity state interface state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_[i]); } - const auto &lbr_fri_sensor = info_.sensors[0]; - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_SAMPLE_TIME, &hw_lbr_state_.sample_time); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_TRACKING_PERFORMANCE, + + 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(lbr_fri_sensor.name, HW_IF_SESSION_STATE, &hw_session_state_); - state_interfaces.emplace_back(lbr_fri_sensor.name, HW_IF_CONNECTION_QUALITY, + 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(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, + 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(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(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(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, + 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; } @@ -163,7 +176,7 @@ SystemInterface::prepare_command_mode_switch(const std::vector & /* controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { if (!async_client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "AsyncClient not configured."); + 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)) { @@ -173,19 +186,20 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l 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), "Waiting for robot heartbeat [%d/%d]. Port ID: %d.", - attempt + 1, max_attempts, parameters_.port_id); + 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), "Failed to connect to robot on max attempts."); + 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), "Robot connected."); - RCLCPP_INFO(rclcpp::get_logger(LOGGER), "Control mode: '%s'.", + 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), "Sample time: %.3f s / %.1f Hz.", + 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; @@ -205,7 +219,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim // 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), + 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(); @@ -224,10 +238,13 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim 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); - // added velocity state interface + // 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_); return hardware_interface::return_type::OK; } @@ -289,13 +306,16 @@ void SystemInterface::nan_state_interfaces_() { hw_time_stamp_sec_ = std::numeric_limits::quiet_NaN(); hw_time_stamp_nano_sec_ = std::numeric_limits::quiet_NaN(); - // added velocity state interface + // 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), "Expected %d joints in URDF. Found %ld.", + 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; } @@ -307,7 +327,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { for (auto &joint : info_.joints) { if (joint.command_interfaces.size() != LBR_FRI_COMMAND_INTERFACE_SIZE) { RCLCPP_ERROR( - rclcpp::get_logger(LOGGER), + 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; @@ -315,7 +335,7 @@ bool SystemInterface::verify_joint_command_interfaces_() { 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), + 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); @@ -331,7 +351,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { for (auto &joint : info_.joints) { if (joint.state_interfaces.size() != LBR_FRI_STATE_INTERFACE_SIZE) { RCLCPP_ERROR( - rclcpp::get_logger(LOGGER), + 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; @@ -343,7 +363,7 @@ bool SystemInterface::verify_joint_state_interfaces_() { 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), + 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, @@ -358,23 +378,32 @@ bool SystemInterface::verify_joint_state_interfaces_() { bool SystemInterface::verify_sensors_() { // check lbr specific state interfaces - if (info_.sensors.size() > 1) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER), "Expected 1 sensor, got %ld", info_.sensors.size()); + 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 &lbr_fri_sensor = info_.sensors[0]; - if (lbr_fri_sensor.state_interfaces.size() != LBR_FRI_SENSOR_SIZE) { - RCLCPP_ERROR(rclcpp::get_logger(LOGGER), + 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. ", - lbr_fri_sensor.name.c_str(), lbr_fri_sensor.state_interfaces.size(), - LBR_FRI_SENSOR_SIZE); + 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 : lbr_fri_sensor.state_interfaces) { + 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 && @@ -383,9 +412,31 @@ bool SystemInterface::verify_sensors_() { 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), "Sensor %s received invalid state interface %s.", - lbr_fri_sensor.name.c_str(), si.name.c_str()); + 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; } } From beb4630eac58628d0db1b840061cfb39ef858eec Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:16:53 +0000 Subject: [PATCH 66/82] added transform --- lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp | 13 ++++++++++++- lbr_fri_ros2/src/ft_estimator.cpp | 7 +++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 0691a358..68f433c0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -6,6 +6,7 @@ #include "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" @@ -40,10 +41,20 @@ class FTEstimator { protected: KDL::Tree tree_; KDL::Chain chain_; + + // solvers std::unique_ptr jacobian_solver_; + std::unique_ptr fk_solver_; + + // robot state + KDL::JntArray q_; + + // forward kinematics + KDL::Frame chain_tip_frame_; + + // force estimation KDL::Jacobian jacobian_; Eigen::Matrix jacobian_inv_; - KDL::JntArray q_; Eigen::Matrix tau_ext_; Eigen::Matrix f_ext_; }; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 147ac2e6..52706f0c 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -14,6 +14,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string 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); } @@ -27,6 +28,12 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, jacobian_solver_->JntToJac(q_, jacobian_); jacobian_inv_ = pinv(jacobian_.data); 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_; } } // end of namespace lbr_fri_ros2 From 5b78f9e9db06565ad048ff1eaad494ac054df87d Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:17:10 +0000 Subject: [PATCH 67/82] added hint --- lbr_ros2_control/config/lbr_controllers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index e0842b44..e3e52d30 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 200 + update_rate: 100 # ROS 2 control broadcasters joint_state_broadcaster: @@ -31,7 +31,7 @@ /**/force_torque_broadcaster: ros__parameters: - frame_id: lbr/link_ee + frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 sensor_name: estimated_ft_sensor /**/joint_trajectory_controller: From 126c75347b0d9d51b0d5475c983f9ac42bedbb0e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:38:08 +0000 Subject: [PATCH 68/82] updated rviz config --- lbr_bringup/config/config.rviz | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index 186d2e86..2720300e 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.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 0.20000000298023224 + 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.4000000059604645 + 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: 000000ff00000000fd0000000400000000000002120000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000035b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f0000035b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d006501000000000000045000000000000000000000040d0000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 1f81ddecded6af70cf165dd7575c8284c81538ac Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:40:40 +0000 Subject: [PATCH 69/82] improved config --- lbr_bringup/config/config.rviz | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index 2720300e..2d9adb4d 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -126,10 +126,10 @@ Visualization Manager: Visual Enabled: true - Accept NaN Values: false Alpha: 1 - Arrow Width: 0.5 + Arrow Width: 0.20000000298023224 Class: rviz_default_plugins/Wrench Enabled: true - Force Arrow Scale: 0.20000000298023224 + Force Arrow Scale: 0.10000000149011612 Force Color: 0; 85; 255 History Length: 1 Name: Wrench @@ -140,7 +140,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /lbr/force_torque_broadcaster/wrench - Torque Arrow Scale: 0.4000000059604645 + Torque Arrow Scale: 0.30000001192092896 Torque Color: 170; 85; 255 Value: true Enabled: true From 5af8e518418ab03baa09dec0bc57b4c1f966a94b Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:43:30 +0000 Subject: [PATCH 70/82] zero members --- lbr_fri_ros2/src/ft_estimator.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 52706f0c..a175d881 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -17,6 +17,10 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string fk_solver_ = std::make_unique(chain_); jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + + q_.data.setZero(); + tau_ext_.setZero(); + f_ext_.setZero(); } void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, From a1154d89ba82a81a7bf49466e16198af383e0a14 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:44:51 +0000 Subject: [PATCH 71/82] added a reset --- lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp | 2 +- lbr_fri_ros2/src/ft_estimator.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 68f433c0..ce171ab6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -34,9 +34,9 @@ class FTEstimator { FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", const std::string &chain_tip = "link_ee"); - 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); + void reset(); protected: KDL::Tree tree_; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index a175d881..e36e5ccd 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -18,9 +18,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.data.setZero(); - tau_ext_.setZero(); - f_ext_.setZero(); + reset(); } void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, @@ -40,4 +38,10 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, Eigen::Map>(f_ext.data()) = f_ext_; } + +void FTEstimator::reset() { + q_.data.setZero(); + tau_ext_.setZero(); + f_ext_.setZero(); +} } // end of namespace lbr_fri_ros2 From 82423d12452e75e3787498b76b842cf9130e36ec Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:58:44 +0000 Subject: [PATCH 72/82] added parameters for ft sensor --- lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp | 3 ++- lbr_fri_ros2/src/ft_estimator.cpp | 5 +++-- lbr_ros2_control/config/lbr_system_interface.xacro | 10 +++++++--- .../include/lbr_ros2_control/system_interface.hpp | 7 +++++++ lbr_ros2_control/src/system_interface.cpp | 9 +++++++-- 5 files changed, 26 insertions(+), 8 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index ce171ab6..98d64f76 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -35,7 +35,8 @@ class FTEstimator { FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", const std::string &chain_tip = "link_ee"); 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_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, + const double &damping = 0.2); void reset(); protected: diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index e36e5ccd..3828ee6d 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -22,13 +22,14 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string } 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_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); + jacobian_inv_ = pinv(jacobian_.data, damping); f_ext_ = jacobian_inv_.transpose() * tau_ext_; // rotate into chain tip frame diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index a052eb8c..6703d0b0 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -30,9 +30,10 @@ command_guard_variant:=^|default external_torque_cutoff_frequency:=^|10 measured_torque_cutoff_frequency:=^|10 - open_loop:=^|true"> - - + open_loop:=^|true + damping:=^|0.2 + chain_root:=^|link_0 + chain_tip:=^|link_ee"> @@ -81,6 +82,9 @@ + ${chain_root} + ${chain_tip} + ${damping} diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index cafe759a..2d63a003 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -45,6 +45,12 @@ struct SystemInterfaceParameters { 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}; +}; + class SystemInterface : public hardware_interface::SystemInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; @@ -95,6 +101,7 @@ class SystemInterface : public hardware_interface::SystemInterface { // robot parameters SystemInterfaceParameters parameters_; + EstimatedFTSensorParameters ft_parameters_; // robot driver std::shared_ptr async_client_ptr_; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 590bb492..fb20bc66 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -74,7 +74,12 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { nan_state_interfaces_(); nan_last_hw_states_(); - ft_estimator_ptr_ = std::make_unique(info_.original_xml); + // 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_estimator_ptr_ = std::make_unique( + info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip); if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; @@ -244,7 +249,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim // additional force-torque state interface ft_estimator_ptr_->compute(hw_lbr_state_.measured_joint_position, hw_lbr_state_.external_torque, - hw_ft_); + hw_ft_, ft_parameters_.damping); return hardware_interface::return_type::OK; } From 006e4d6b4510f316010a781e911df8bddb91bade Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 11:00:33 +0000 Subject: [PATCH 73/82] cleaned dependencies --- lbr_ros2_control/CMakeLists.txt | 11 ----------- lbr_ros2_control/package.xml | 4 ---- 2 files changed, 15 deletions(-) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index f2052c6f..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) @@ -55,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 @@ -82,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/package.xml b/lbr_ros2_control/package.xml index ae426cc2..675b0eb5 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -9,12 +9,8 @@ ament_cmake ament_cmake_python - eigen3_cmake_module - eigen fri_vendor - geometry_msgs - kinematics_interface_kdl lbr_fri_msgs lbr_fri_ros2 pluginlib From a4f310a76821d821f69b495ce53f2741dfbffe6e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 11:36:32 +0000 Subject: [PATCH 74/82] improved log --- lbr_fri_ros2/src/command_interface.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index df82f164..9ff16367 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -119,11 +119,11 @@ void CommandInterface::init_command(const_fri_state_t_ref state) { 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: %f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i: %f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.d: %f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_max: %f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* pid.i_min: %f", pid_parameters_.i_min); + 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"); } From f317f27c7d8f26cfea54f2c7abdbe9e051dd863c Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 11:51:37 +0000 Subject: [PATCH 75/82] added ft threshold --- .../include/lbr_fri_ros2/ft_estimator.hpp | 9 ++++++++- lbr_fri_ros2/src/ft_estimator.cpp | 13 ++++++++++++- .../config/lbr_system_interface.xacro | 16 ++++++++++++++-- .../lbr_ros2_control/system_interface.hpp | 6 ++++++ lbr_ros2_control/src/system_interface.cpp | 16 +++++++++++++++- 5 files changed, 55 insertions(+), 5 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 98d64f76..f71c6eb9 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -1,7 +1,9 @@ #ifndef LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ #define LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#include #include +#include #include #include "eigen3/Eigen/Core" @@ -31,15 +33,20 @@ class FTEstimator { 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 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_; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 3828ee6d..35929be8 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -2,7 +2,8 @@ namespace lbr_fri_ros2 { FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, - const std::string &chain_tip) { + 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()); @@ -38,6 +39,16 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, 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() { diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 6703d0b0..3f389121 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -31,9 +31,15 @@ external_torque_cutoff_frequency:=^|10 measured_torque_cutoff_frequency:=^|10 open_loop:=^|true - damping:=^|0.2 chain_root:=^|link_0 - chain_tip:=^|link_ee"> + chain_tip:=^|link_ee + damping:=^|0.2 + force_x_th:=^|2.0 + force_y_th:=^|2.0 + force_z_th:=^|2.0 + torque_x_th:=^|0.5 + torque_y_th:=^|0.5 + torque_z_th:=^|0.5"> @@ -85,6 +91,12 @@ ${chain_root} ${chain_tip} ${damping} + ${force_x_th} + ${force_y_th} + ${force_z_th} + ${torque_x_th} + ${torque_y_th} + ${torque_z_th} diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 2d63a003..e6295b7f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -49,6 +49,12 @@ 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 { diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index fb20bc66..4722e417 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -78,8 +78,22 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { 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); + 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; From 43c0ba6bc1e5e2ec417137712fb4f32595276a4e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 11:56:51 +0000 Subject: [PATCH 76/82] improved config --- lbr_bringup/config/config.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index 2d9adb4d..87a3bd4d 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -215,7 +215,7 @@ Window Geometry: Height: 1021 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002120000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000035b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f0000035b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d006501000000000000045000000000000000000000040d0000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002120000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000035b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d00650100000000000004500000000000000000000005220000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From 847b2ee4d777a5b9889c30961d2b0ea6f56239d1 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:07:08 +0000 Subject: [PATCH 77/82] udpate logs --- CHANGELOG.rst | 22 +++++++++++++++++++++- CITATION.cff | 4 ++-- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4c1574e6..eeb88995 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,26 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package LBR FRI ROS 2 Stack +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 + * 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` +* `/lbr/command/...` now uses system default quality of service (QoS) +* `/lbr/state` now uses sensor data quality of service (QoS) +* Adds this changelog with release notes + 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 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 From 54a7e1a79a4a3dfce1cf3da1e22731a72628a181 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:08:31 +0000 Subject: [PATCH 78/82] update logs --- CHANGELOG.rst | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index eeb88995..74b59753 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -3,22 +3,24 @@ 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 - * 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` -* `/lbr/command/...` now uses system default quality of service (QoS) -* `/lbr/state` now uses sensor data quality of service (QoS) +* 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 + * 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`` +* ``/lbr/command/...`` now uses system default quality of service (QoS) +* ``/lbr/state`` now uses sensor data quality of service (QoS) * Adds this changelog with release notes Humble v1.3.1 (2023-11-21) From 74895d0c68ab454b9994907354351d92ab398707 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:09:07 +0000 Subject: [PATCH 79/82] added PR link --- CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 74b59753..dfa064ef 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -22,6 +22,7 @@ Humble v1.4.0 (2023-12-08) * ``/lbr/command/...`` now uses system default quality of service (QoS) * ``/lbr/state`` now uses sensor data quality of service (QoS) * 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) -------------------------- From 668969105ea798e31018fc7a7e71509a68848182 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:19:30 +0000 Subject: [PATCH 80/82] reverted QoS for now --- CHANGELOG.rst | 2 -- .../src/admittance_control_node.cpp | 4 ++-- .../admittance_control_node.py | 5 ++--- .../lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp | 4 ++-- .../lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp | 6 +++--- .../lbr_demos_fri_ros2_python/joint_sine_overlay_node.py | 5 ++--- .../lbr_demos_fri_ros2_python/torque_sine_overlay_node.py | 5 ++--- .../src/forward_lbr_position_command_controller.cpp | 2 +- .../src/forward_lbr_torque_command_controller.cpp | 3 +-- lbr_ros2_control/src/lbr_state_broadcaster.cpp | 4 ++-- 10 files changed, 17 insertions(+), 23 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index dfa064ef..c4ce8b1c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -19,8 +19,6 @@ Humble v1.4.0 (2023-12-08) * Adds configurations to ``lbr_system_interface.xacro`` * Simplifies ``lbr_ros2_control`` class names * ``/lbr/command/position`` topic now under ``/lbr/command/joint_position`` -* ``/lbr/command/...`` now uses system default quality of service (QoS) -* ``/lbr/state`` now uses sensor data quality of service (QoS) * Adds this changelog with release notes * Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/142 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 544bacd6..7c47f92d 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 @@ -30,9 +30,9 @@ class AdmittanceControlNode : public rclcpp::Node { this->get_parameter("dx_gains").as_double_array()); lbr_position_command_pub_ = create_publisher( - "/lbr/command/joint_position", rclcpp::SystemDefaultsQoS()); + "/lbr/command/joint_position", 1); lbr_state_sub_ = create_subscription( - "/lbr/state", rclcpp::SensorDataQoS(), + "/lbr/state", 1, std::bind(&AdmittanceControlNode::on_lbr_state, this, std::placeholders::_1)); } 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 777a3da0..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 @@ -1,7 +1,6 @@ import numpy as np import rclpy from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default from lbr_fri_msgs.msg import LBRPositionCommand, LBRState @@ -28,12 +27,12 @@ def __init__(self, node_name="admittance_control_node") -> None: # publishers and subscribers self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data + LBRState, "/lbr/state", self.on_lbr_state_, 1 ) self.lbr_position_command_pub_ = self.create_publisher( LBRPositionCommand, "/lbr/command/joint_position", - qos_profile_system_default, + 1, ) def on_lbr_state_(self, lbr_state: LBRState) -> None: 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 532f5118..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 @@ -20,11 +20,11 @@ class JointSineOverlayNode : public rclcpp::Node { JointSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr/command/joint_position lbr_position_command_pub_ = this->create_publisher( - "/lbr/command/joint_position", rclcpp::SystemDefaultsQoS()); + "/lbr/command/joint_position", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/lbr/state", rclcpp::SensorDataQoS(), + "/lbr/state", 1, std::bind(&JointSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); }; diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp index 493d4c68..cc7b6b04 100644 --- a/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp +++ b/lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp @@ -19,12 +19,12 @@ class TorqueSineOverlayNode : public rclcpp::Node { public: TorqueSineOverlayNode(const std::string &node_name) : Node(node_name), phase_(0.) { // create publisher to /lbr/command/torque - lbr_torque_command_pub_ = this->create_publisher( - "/lbr/command/torque", rclcpp::SystemDefaultsQoS()); + lbr_torque_command_pub_ = + this->create_publisher("/lbr/command/torque", 1); // create subscription to /lbr/state lbr_state_sub_ = this->create_subscription( - "/lbr/state", rclcpp::SensorDataQoS(), + "/lbr/state", 1, std::bind(&TorqueSineOverlayNode::on_lbr_state_, this, std::placeholders::_1)); }; 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 1a23f54f..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 @@ -2,7 +2,6 @@ import rclpy from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default # import lbr_fri_msgs from lbr_fri_msgs.msg import LBRPositionCommand, LBRState @@ -21,12 +20,12 @@ def __init__(self, node_name: str) -> None: self.lbr_position_command_pub_ = self.create_publisher( LBRPositionCommand, "/lbr/command/joint_position", - qos_profile_system_default, + 1, ) # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data + LBRState, "/lbr/state", self.on_lbr_state_, 1 ) def on_lbr_state_(self, lbr_state: LBRState) -> None: 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 4875f3c0..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 @@ -2,7 +2,6 @@ import rclpy from rclpy.node import Node -from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default # import lbr_fri_msgs from lbr_fri_msgs.msg import LBRState, LBRTorqueCommand @@ -19,12 +18,12 @@ def __init__(self, node_name: str) -> None: # create publisher to /lbr/command/torque self.lbr_torque_command_pub_ = self.create_publisher( - LBRTorqueCommand, "/lbr/command/torque", qos_profile_system_default + LBRTorqueCommand, "/lbr/command/torque", 1 ) # create subscription to /lbr_state self.lbr_state_sub_ = self.create_subscription( - LBRState, "/lbr/state", self.on_lbr_state_, qos_profile_sensor_data + LBRState, "/lbr/state", self.on_lbr_state_, 1 ) def on_lbr_state_(self, lbr_state: LBRState) -> None: diff --git a/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp index df869106..bfe960fe 100644 --- a/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp +++ b/lbr_ros2_control/src/forward_lbr_position_command_controller.cpp @@ -24,7 +24,7 @@ controller_interface::CallbackReturn ForwardLBRPositionCommandController::on_ini try { lbr_position_command_subscription_ptr_ = this->get_node()->create_subscription( - "command/joint_position", rclcpp::SystemDefaultsQoS(), + "command/joint_position", 1, [this](const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr msg) { rt_lbr_position_command_ptr_.writeFromNonRT(msg); }); diff --git a/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp index 135d7545..58becd53 100644 --- a/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/forward_lbr_torque_command_controller.cpp @@ -25,8 +25,7 @@ controller_interface::CallbackReturn ForwardLBRTorqueCommandController::on_init( try { lbr_torque_command_subscription_ptr_ = this->get_node()->create_subscription( - "command/torque", rclcpp::SystemDefaultsQoS(), - [this](const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr msg) { + "command/torque", 1, [this](const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); } catch (const std::exception &e) { diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index 39d4e0e1..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>( From 1dd098d461504f66f245b94a198f0de8d37c7ccd Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:21:57 +0000 Subject: [PATCH 81/82] updated log --- CHANGELOG.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c4ce8b1c..1bb1a149 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -15,6 +15,9 @@ Humble v1.4.0 (2023-12-08) * 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 From e39b85a01911f8c5e82e2f0d2f301c388e28702d Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 12:23:52 +0000 Subject: [PATCH 82/82] formatted --- .../src/admittance_control_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 7c47f92d..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 @@ -29,8 +29,8 @@ class AdmittanceControlNode : public rclcpp::Node { this->get_parameter("dq_gains").as_double_array(), this->get_parameter("dx_gains").as_double_array()); - lbr_position_command_pub_ = create_publisher( - "/lbr/command/joint_position", 1); + lbr_position_command_pub_ = + 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));