remove permanent command re-initialization (#236)

lbr-stack · Dec 14, 2024 · 779817b · 779817b
1 parent 1832b44
commit 779817b
Show file tree

Hide file tree

Showing 5 changed files with 64 additions and 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
@@ -38,6 +38,7 @@ class BaseCommandInterface {
   void log_info() const;
 
 protected:
+  bool command_initialized_;
   std::unique_ptr<CommandGuard> 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
@@ -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
@@ -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
@@ -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
@@ -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)) {