Added service to set force mode

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -53,6 +53,7 @@
 #include "ur_msgs/srv/set_io.hpp"
 #include "ur_msgs/srv/set_speed_slider_fraction.hpp"
 #include "ur_msgs/srv/set_payload.hpp"
+#include "ur_msgs/srv/set_force_mode.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/bool.hpp"
@@ -73,6 +74,32 @@ enum CommandInterfaces
   PAYLOAD_COG_Y = 27,
   PAYLOAD_COG_Z = 28,
   PAYLOAD_ASYNC_SUCCESS = 29,
+  FORCE_MODE_TASK_FRAME_X = 30,
+  FORCE_MODE_TASK_FRAME_Y = 31,
+  FORCE_MODE_TASK_FRAME_Z = 32,
+  FORCE_MODE_TASK_FRAME_RX = 33,
+  FORCE_MODE_TASK_FRAME_RY = 34,
+  FORCE_MODE_TASK_FRAME_RZ = 35,
+  FORCE_MODE_SELECTION_VECTOR_X = 36,
+  FORCE_MODE_SELECTION_VECTOR_Y = 37,
+  FORCE_MODE_SELECTION_VECTOR_Z = 38,
+  FORCE_MODE_SELECTION_VECTOR_RX = 39,
+  FORCE_MODE_SELECTION_VECTOR_RY = 40,
+  FORCE_MODE_SELECTION_VECTOR_RZ = 41,
+  FORCE_MODE_WRENCH_X = 42,
+  FORCE_MODE_WRENCH_Y = 43,
+  FORCE_MODE_WRENCH_Z = 44,
+  FORCE_MODE_WRENCH_RX = 45,
+  FORCE_MODE_WRENCH_RY = 46,
+  FORCE_MODE_WRENCH_RZ = 47,
+  FORCE_MODE_TYPE = 48,
+  FORCE_MODE_LIMITS_X = 49,
+  FORCE_MODE_LIMITS_Y = 50,
+  FORCE_MODE_LIMITS_Z = 51,
+  FORCE_MODE_LIMITS_RX = 52,
+  FORCE_MODE_LIMITS_RY = 53,
+  FORCE_MODE_LIMITS_RZ = 54,
+  FORCE_MODE_ASYNC_SUCCESS = 55,
 };
 
 enum StateInterfaces
@@ -125,6 +152,9 @@ class GPIOController : public controller_interface::ControllerInterface
   bool setPayload(const ur_msgs::srv::SetPayload::Request::SharedPtr req,
                   ur_msgs::srv::SetPayload::Response::SharedPtr resp);
 
+  bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+                    ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
+
   void publishIO();
 
   void publishToolData();
@@ -150,6 +180,7 @@ class GPIOController : public controller_interface::ControllerInterface
   rclcpp::Service<ur_msgs::srv::SetSpeedSliderFraction>::SharedPtr set_speed_slider_srv_;
   rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
   rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
+  rclcpp::Service<ur_msgs::srv::SetForceMode>::SharedPtr set_force_mode_srv_;
 
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

ur_controllers/src/gpio_controller.cpp

@@ -78,6 +78,34 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
   config.names.emplace_back("payload/cog.z");
   config.names.emplace_back("payload/payload_async_success");
 
+  // force mode
+  config.names.emplace_back("force_mode/task_frame_x");
+  config.names.emplace_back("force_mode/task_frame_y");
+  config.names.emplace_back("force_mode/task_frame_z");
+  config.names.emplace_back("force_mode/task_frame_rx");
+  config.names.emplace_back("force_mode/task_frame_ry");
+  config.names.emplace_back("force_mode/task_frame_rz");
+  config.names.emplace_back("force_mode/selection_vector_x");
+  config.names.emplace_back("force_mode/selection_vector_y");
+  config.names.emplace_back("force_mode/selection_vector_z");
+  config.names.emplace_back("force_mode/selection_vector_rx");
+  config.names.emplace_back("force_mode/selection_vector_ry");
+  config.names.emplace_back("force_mode/selection_vector_rz");
+  config.names.emplace_back("force_mode/wrench_x");
+  config.names.emplace_back("force_mode/wrench_y");
+  config.names.emplace_back("force_mode/wrench_z");
+  config.names.emplace_back("force_mode/wrench_rx");
+  config.names.emplace_back("force_mode/wrench_ry");
+  config.names.emplace_back("force_mode/wrench_rz");
+  config.names.emplace_back("force_mode/type");
+  config.names.emplace_back("force_mode/limits_x");
+  config.names.emplace_back("force_mode/limits_y");
+  config.names.emplace_back("force_mode/limits_z");
+  config.names.emplace_back("force_mode/limits_rx");
+  config.names.emplace_back("force_mode/limits_ry");
+  config.names.emplace_back("force_mode/limits_rz");
+  config.names.emplace_back("force_mode/force_mode_async_success");
+
   return config;
 }
 
@@ -274,6 +302,10 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
 
     set_payload_srv_ = get_node()->create_service<ur_msgs::srv::SetPayload>(
         "~/set_payload", std::bind(&GPIOController::setPayload, this, std::placeholders::_1, std::placeholders::_2));
+
+    set_force_mode_srv_ = get_node()->create_service<ur_msgs::srv::SetForceMode>(
+        "~/set_force_mode",
+        std::bind(&GPIOController::setForceMode, this, std::placeholders::_1, std::placeholders::_2));
   } catch (...) {
     return LifecycleNodeInterface::CallbackReturn::ERROR;
   }
@@ -413,6 +445,44 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
   return true;
 }
 
+bool GPIOController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+                                  ur_msgs::srv::SetForceMode::Response::SharedPtr resp)
+{
+  if (req->task_frame.size() != 6 || req->selection_vector.size() != 6 || req->wrench.size() != 6 ||
+      req->limits.size() != 6) {
+    RCLCPP_WARN(get_node()->get_logger(), "Size of received force mode message is wrong");
+    resp->success = false;
+    return false;
+  }
+
+  // reset success flag
+  command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+
+  for (size_t i = 0; i < 6; i++) {
+    command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X + i].set_value(req->task_frame[i]);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X + i].set_value(req->selection_vector[i]);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X + i].set_value(req->wrench[i]);
+    command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X + i].set_value(req->limits[i]);
+  }
+  command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type));
+
+  while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
+    // Asynchronous wait until the hardware interface has set the force mode
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+
+  resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value());
+
+  if (resp->success) {
+    RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully");
+  } else {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode");
+    return false;
+  }
+
+  return true;
+}
+
 void GPIOController::initMsgs()
 {
   io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());

ur_robot_driver/CMakeLists.txt

@@ -32,6 +32,7 @@ find_package(std_srvs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(ur_client_library REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
+find_package(ur_msgs REQUIRED)
 
 include_directories(include)
 
@@ -48,6 +49,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tf2_geometry_msgs
   ur_client_library
   ur_dashboard_msgs
+  ur_msgs
 )
 
 add_library(ur_robot_driver_plugin

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -189,6 +189,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   double payload_mass_;
   double payload_async_success_;
 
+  // force mode parameters
+  urcl::vector6d_t force_mode_task_frame_;
+  urcl::vector6d_t force_mode_selection_vector_;
+  urcl::vector6d_t force_mode_wrench_;
+  urcl::vector6d_t force_mode_limits_;
+  double force_mode_type_;
+  double force_mode_async_success_;
+
   // copy of non double values
   std::array<double, 18> actual_dig_out_bits_copy_;
   std::array<double, 18> actual_dig_in_bits_copy_;

ur_robot_driver/src/hardware_interface.cpp

@@ -254,6 +254,58 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
   command_interfaces.emplace_back(
       hardware_interface::CommandInterface("payload", "payload_async_success", &payload_async_success_));
 
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_x", &force_mode_task_frame_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_y", &force_mode_task_frame_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_z", &force_mode_task_frame_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_rx", &force_mode_task_frame_[3]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_ry", &force_mode_task_frame_[4]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "task_frame_rz", &force_mode_task_frame_[5]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_x", &force_mode_selection_vector_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_y", &force_mode_selection_vector_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_z", &force_mode_selection_vector_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_rx", &force_mode_selection_vector_[3]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_ry", &force_mode_selection_vector_[4]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "selection_vector_rz", &force_mode_selection_vector_[5]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_x", &force_mode_wrench_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_y", &force_mode_wrench_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_z", &force_mode_wrench_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_rx", &force_mode_wrench_[3]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_ry", &force_mode_wrench_[4]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "wrench_rz", &force_mode_wrench_[5]));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface("force_mode", "type", &force_mode_type_));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_x", &force_mode_limits_[0]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_y", &force_mode_limits_[1]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_z", &force_mode_limits_[2]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_rx", &force_mode_limits_[3]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_ry", &force_mode_limits_[4]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "limits_rz", &force_mode_limits_[5]));
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface("force_mode", "force_mode_async_success", &force_mode_async_success_));
+
   for (size_t i = 0; i < 18; ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i]));
@@ -596,6 +648,14 @@ void URPositionHardwareInterface::initAsyncIO()
 
   payload_mass_ = NO_NEW_CMD_;
   payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
+
+  for (size_t i = 0; i < 6; i++) {
+    force_mode_task_frame_[i] = NO_NEW_CMD_;
+    force_mode_selection_vector_[i] = static_cast<uint32_t>(NO_NEW_CMD_);
+    force_mode_wrench_[i] = NO_NEW_CMD_;
+    force_mode_limits_[i] = NO_NEW_CMD_;
+  }
+  force_mode_type_ = static_cast<unsigned int>(NO_NEW_CMD_);
 }
 
 void URPositionHardwareInterface::checkAsyncIO()
@@ -656,6 +716,24 @@ void URPositionHardwareInterface::checkAsyncIO()
     payload_mass_ = NO_NEW_CMD_;
     payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
   }
+
+  if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) &&
+      !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) &&
+      ur_driver_ != nullptr) {
+    urcl::vector6uint32_t force_mode_selection_vector;
+    for (size_t i = 0; i < 6; i++) {
+      force_mode_selection_vector[i] = force_mode_selection_vector_[i];
+    }
+    force_mode_async_success_ = ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector,
+                                                           force_mode_wrench_, force_mode_type_, force_mode_limits_);
+    for (size_t i = 0; i < 6; i++) {
+      force_mode_task_frame_[i] = NO_NEW_CMD_;
+      force_mode_selection_vector_[i] = static_cast<uint32_t>(NO_NEW_CMD_);
+      force_mode_wrench_[i] = NO_NEW_CMD_;
+      force_mode_limits_[i] = NO_NEW_CMD_;
+    }
+    force_mode_type_ = static_cast<unsigned int>(NO_NEW_CMD_);
+  }
 }
 
 void URPositionHardwareInterface::updateNonDoubleValues()