From 46e5f8b31df0ceab02919d950cea393c6aebc4f0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 26 Nov 2024 15:49:39 +0000 Subject: [PATCH 1/7] fixed typo (#227) (cherry picked from commit 70724d20c18224580277e8c0da680f9e953a5b32) --- lbr_description/ros2_control/lbr_system_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 8f96cf0d..604d1239 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -14,11 +14,11 @@ hardware: command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] # exponential moving average time constant for external joint torque measurements [s]: # Set tau > robot sample time for more smoothing on the external torque measurements - # Set tau << robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for no smoothing on the external torque measurements external_torque_tau: 0.04 # exponential moving average time constant for joint torque measurements [s]: # Set tau > robot sample time for more smoothing on the raw torque measurements - # Set tau << robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for no smoothing on the raw torque measurements measured_torque_tau: 0.04 open_loop: true # KUKA works the best in open_loop control mode From 4577580a3d55ccddd080f5ce1fbe571a5005e161 Mon Sep 17 00:00:00 2001 From: Matteo Dalle Vedove Date: Wed, 4 Dec 2024 09:19:37 +0100 Subject: [PATCH 2/7] Update TorqueCommandInterface - #232 (cherry picked from commit 2cc7ae8929979f75af2a5d0a1558cae53422149d) --- lbr_fri_ros2/src/interfaces/torque_command.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 54f53a2b..cad5e2b2 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -36,6 +36,9 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); + command_.torque = command_target_.torque; + command_.joint_position = command_target_.joint_position; + // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; From d6aee144ac3fd71464686999272b29f4647bb23c Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 4 Dec 2024 17:49:27 +0000 Subject: [PATCH 3/7] adds wrench comand (https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/233#discussion_r1869192458) (cherry picked from commit be0f0a3bf677608ca36ff17634802236d52ee004) --- lbr_fri_ros2/src/interfaces/torque_command.cpp | 2 -- lbr_fri_ros2/src/interfaces/wrench_command.cpp | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index cad5e2b2..31dcb558 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -35,9 +35,7 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, joint_position_filter_.initialize(state.sample_time); } joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); - command_.torque = command_target_.torque; - command_.joint_position = command_target_.joint_position; // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 1a1c0ab7..9bdc3254 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -33,6 +33,7 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, joint_position_filter_.initialize(state.sample_time); } joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); + command_.wrench = command_target_.wrench; // validate if (!command_guard_->is_valid_command(command_, state)) { From d63939c005f038604ceda9e9dbdeb95d9894dddb Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 11:35:15 +0000 Subject: [PATCH 4/7] thread_priority.hpp deprecated (https://github.com/ros-controls/realtime_tools/pull/178) (cherry picked from commit 1832b443e37ead2f951bf19ad7e7ef41d605793a) --- lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp index 1eb42b8f..a23c39cd 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -7,7 +7,7 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -#include "realtime_tools/thread_priority.hpp" +#include "realtime_tools/realtime_helpers.hpp" #include "lbr_fri_ros2/formatting.hpp" From 9e091e1dfd3db8579aa880bf5381569d871d65c0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 15:05:05 +0000 Subject: [PATCH 5/7] remove permanent command re-initialization (#236) (cherry picked from commit 779817b728f2ee0110ed27b69cff18e6781ca186) --- .../lbr_fri_ros2/interfaces/base_command.hpp | 1 + lbr_fri_ros2/src/interfaces/base_command.cpp | 3 +- .../src/interfaces/position_command.cpp | 26 +++++++++----- .../src/interfaces/torque_command.cpp | 33 ++++++++++------- .../src/interfaces/wrench_command.cpp | 36 ++++++++++++------- 5 files changed, 64 insertions(+), 35 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index 4aa3c582..14cd471c 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -38,6 +38,7 @@ class BaseCommandInterface { void log_info() const; protected: + bool command_initialized_; std::unique_ptr command_guard_; JointExponentialFilterArray joint_position_filter_; idl_command_t command_, command_target_; diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index e43111ea..15186f78 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -5,7 +5,7 @@ namespace lbr_fri_ros2 { BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : joint_position_filter_(joint_position_tau) { + : command_initialized_(false), joint_position_filter_(joint_position_tau) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -14,6 +14,7 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { command_target_.torque.fill(0.); command_target_.wrench.fill(0.); command_ = command_target_; + command_initialized_ = true; } void BaseCommandInterface::log_info() const { diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 95533981..407dae01 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -29,22 +29,30 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } #endif - if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), - [](const double &v) { return std::isnan(v); })) { - this->init_command(state); + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); } - if (!command_guard_) { - std::string err = "Uninitialized command guard."; + + if (!command_initialized_) { + std::string err = "Uninitialized command."; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } - // exponential smooth - if (!joint_position_filter_.is_initialized()) { - joint_position_filter_.initialize(state.sample_time); + if (!std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); })) { + // write command_target_ to command_ (with exponential smooth on joint positions), else use + // internal command_ + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); + } + + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); } - joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 31dcb558..7d1648d8 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -17,25 +17,34 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } - if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), - [](const double &v) { return std::isnan(v); }) || - std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(), - [](const double &v) { return std::isnan(v); })) { - this->init_command(state); + + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); } - if (!command_guard_) { - std::string err = "Uninitialized command guard."; + + if (!command_initialized_) { + std::string err = "Uninitialized command."; RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } - // exponential smooth - if (!joint_position_filter_.is_initialized()) { - joint_position_filter_.initialize(state.sample_time); + if (!std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); }) && + !std::any_of(command_target_.torque.cbegin(), command_target_.torque.cend(), + [](const double &v) { return std::isnan(v); })) { + // write command_target_ to command_ (with exponential smooth on joint positions), else use + // internal command_ + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); + command_.torque = command_target_.torque; + } + + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); } - joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); - command_.torque = command_target_.torque; // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9bdc3254..0560c5b8 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -16,24 +16,34 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); throw std::runtime_error(err); } - if (std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), - [](const double &v) { return std::isnan(v); }) || - std::any_of(command_target_.wrench.cbegin(), command_target_.wrench.cend(), - [](const double &v) { return std::isnan(v); })) { - this->init_command(state); + + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); } - if (!command_guard_) { - std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME()), err.c_str()); + + if (!command_initialized_) { + std::string err = "Uninitialized command."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } - // exponential smooth - if (!joint_position_filter_.is_initialized()) { - joint_position_filter_.initialize(state.sample_time); + if (!std::any_of(command_target_.joint_position.cbegin(), command_target_.joint_position.cend(), + [](const double &v) { return std::isnan(v); }) && + !std::any_of(command_target_.wrench.cbegin(), command_target_.wrench.cend(), + [](const double &v) { return std::isnan(v); })) { + // write command_target_ to command_ (with exponential smooth on joint positions), else use + // internal command_ + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); + command_.wrench = command_target_.wrench; + } + + if (!command_guard_) { + std::string err = "Uninitialized command guard."; + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); + throw std::runtime_error(err); } - joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); - command_.wrench = command_target_.wrench; // validate if (!command_guard_->is_valid_command(command_, state)) { From 15a3b05d3b5d1b591da107b9b109f34172eff601 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 15:47:57 +0000 Subject: [PATCH 6/7] override open_loop to false for compliant control modes (#226) (cherry picked from commit 250611cc7c2c05314e0fae30f98f62a418df66d8) --- .../ros2_control/lbr_system_config.yaml | 17 +++++++++-------- lbr_fri_ros2/src/async_client.cpp | 17 +++++++++++++++++ lbr_ros2_control/src/system_interface.cpp | 4 ---- 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 604d1239..98880ad5 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -8,20 +8,21 @@ hardware: remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop # exponential moving average time constant for joint position commands [s]: - # Set tau > robot sample time for more smoothing on the commands - # Set tau << robot sample time for no smoothing on the commands + # set tau > robot sample time for more smoothing on the commands + # set tau << robot sample time for no smoothing on the commands joint_position_tau: 0.04 command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] # exponential moving average time constant for external joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the external torque measurements - # Set tau << robot sample time for no smoothing on the external torque measurements + # set tau > robot sample time for more smoothing on the external torque measurements + # set tau << robot sample time for no smoothing on the external torque measurements external_torque_tau: 0.04 # exponential moving average time constant for joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the raw torque measurements - # Set tau << robot sample time for no smoothing on the raw torque measurements + # set tau > robot sample time for more smoothing on the raw torque measurements + # set tau << robot sample time for no smoothing on the raw torque measurements measured_torque_tau: 0.04 - open_loop: true # KUKA works the best in open_loop control mode - + # for position control, LBR works best in open_loop control mode + # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode + open_loop: true estimated_ft_sensor: # estimates the external force-torque from the external joint torque values enabled: true # whether to enable the force-torque estimation update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 7d11414c..0a2f4ea2 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -67,6 +67,23 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, // initialize command state_interface_ptr_->set_state(robotState()); command_interface_ptr_->init_command(state_interface_ptr_->get_state()); + + // if robot is in impedance or Cartesian impedance control mode, override open_loop_ to false + // also refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226 + auto control_mode = robotState().getControlMode(); + if (control_mode == KUKA::FRI::EControlMode::JOINT_IMP_CONTROL_MODE || + control_mode == KUKA::FRI::EControlMode::CART_IMP_CONTROL_MODE) { + if (open_loop_) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Overriding open loop from '" + << (open_loop_ ? "true" : "false") + << "' to 'false' since LBR is in control mode '" + << EnumMaps::control_mode_map(control_mode) + << "'. Please refer to " + "[https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226]."); + open_loop_ = false; + } + } } void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index ebab71e4..21adfe2b 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -399,10 +399,6 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & info_.hardware_parameters["open_loop"].begin(), ::tolower); // convert to lower case parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; - std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), - info_.hardware_parameters["pid_antiwindup"].end(), - info_.hardware_parameters["pid_antiwindup"].begin(), - ::tolower); // convert to lower case parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]); From 6dba146a6bc6e23656c9284a38e1ec14c692f00c Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 18:07:00 +0000 Subject: [PATCH 7/7] control mode only available in commanding active (#236) (cherry picked from commit 0e6f07683a17508d3333de0484835c4b07d1886e) --- lbr_fri_ros2/src/async_client.cpp | 24 +++++++++++------------ lbr_ros2_control/src/system_interface.cpp | 6 ------ 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 0a2f4ea2..e4d84d10 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -67,7 +67,19 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, // initialize command state_interface_ptr_->set_state(robotState()); command_interface_ptr_->init_command(state_interface_ptr_->get_state()); +} + +void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; + +void AsyncClient::waitForCommand() { + KUKA::FRI::LBRClient::waitForCommand(); + state_interface_ptr_->set_state(robotState()); + command_interface_ptr_->init_command(state_interface_ptr_->get_state()); + command_interface_ptr_->buffered_command_to_fri(robotCommand(), + state_interface_ptr_->get_state()); +} +void AsyncClient::command() { // if robot is in impedance or Cartesian impedance control mode, override open_loop_ to false // also refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226 auto control_mode = robotState().getControlMode(); @@ -84,19 +96,7 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, open_loop_ = false; } } -} - -void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; - -void AsyncClient::waitForCommand() { - KUKA::FRI::LBRClient::waitForCommand(); - state_interface_ptr_->set_state(robotState()); - command_interface_ptr_->init_command(state_interface_ptr_->get_state()); - command_interface_ptr_->buffered_command_to_fri(robotCommand(), - state_interface_ptr_->get_state()); -} -void AsyncClient::command() { if (open_loop_) { state_interface_ptr_->set_state_open_loop(robotState(), command_interface_ptr_->get_command().joint_position); diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 21adfe2b..26e1e141 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -228,12 +228,6 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::OKGREEN << "Robot connected" << lbr_fri_ros2::ColorScheme::ENDC); - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), - "Control mode '" - << lbr_fri_ros2::EnumMaps::control_mode_map( - async_client_ptr_->get_state_interface()->get_state().control_mode) - .c_str() - << "'"); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Sample time %.3f s / %.1f Hz", async_client_ptr_->get_state_interface()->get_state().sample_time, 1. / async_client_ptr_->get_state_interface()->get_state().sample_time);