override open_loop to false for compliant control modes (#226)

(cherry picked from commit 250611c)

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