From 1832b443e37ead2f951bf19ad7e7ef41d605793a Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 11:35:15 +0000 Subject: [PATCH 1/3] thread_priority.hpp deprecated (https://github.com/ros-controls/realtime_tools/pull/178) --- 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 779817b728f2ee0110ed27b69cff18e6781ca186 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 15:05:05 +0000 Subject: [PATCH 2/3] remove permanent command re-initialization (#236) --- .../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 250611cc7c2c05314e0fae30f98f62a418df66d8 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 15:47:57 +0000 Subject: [PATCH 3/3] override open_loop to false for compliant control modes (#226) --- .../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"]);