Backport 237 to humble

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)

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_;

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"
 

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()); };

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 {

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)) {

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)) {

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)) {

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"]);