From 9e091e1dfd3db8579aa880bf5381569d871d65c0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Sat, 14 Dec 2024 15:05:05 +0000 Subject: [PATCH] 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)) {