Allow using the actual torques as joint efforts

Author: urfeex

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -324,6 +324,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   const std::string TOOL_CONTACT_GPIO = "tool_contact";
 
   std::unordered_map<std::string, std::unordered_map<std::string, bool>> mode_compatibility_;
+
+  bool use_currents_as_efforts_ = false;
 };
 }  // namespace ur_robot_driver
 

ur_robot_driver/launch/ur_rsp.launch.py

@@ -73,6 +73,7 @@ def generate_launch_description():
     reverse_port = LaunchConfiguration("reverse_port")
     script_sender_port = LaunchConfiguration("script_sender_port")
     trajectory_port = LaunchConfiguration("trajectory_port")
+    use_currents_as_efforts = LaunchConfiguration("use_currents_as_efforts")
 
     script_filename = PathJoinSubstitution(
         [
@@ -184,6 +185,8 @@ def generate_launch_description():
             "trajectory_port:=",
             trajectory_port,
             " ",
+            "use_currents_as_efforts:=",
+            use_currents_as_efforts,
         ]
     )
     robot_description = {
@@ -442,6 +445,13 @@ def generate_launch_description():
             description="Port that will be opened for trajectory control.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_currents_as_efforts",
+            default_value="true",
+            description="Report motor currents as efforts. When set to false, the torques as reported from the robot are used. Note that this requires software 5.23.0 / 10.11.0.",
+        )
+    )
 
     return LaunchDescription(
         declared_arguments

ur_robot_driver/resources/rtde_output_recipe.txt

@@ -26,4 +26,5 @@ safety_mode
 robot_status_bits
 safety_status_bits
 actual_current
+actual_current_as_torque
 tcp_offset

ur_robot_driver/src/hardware_interface.cpp

@@ -511,6 +511,11 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   // The driver will offer an interface to receive the program's URScript on this port.
   const int script_sender_port = stoi(info_.hardware_parameters["script_sender_port"]);
 
+  // Newer software version (5.23.0 / 10.11.0) support reporting the actual joint torques. On older
+  // versions fall back to the currents, instead.
+  use_currents_as_efforts_ = ((info_.hardware_parameters["use_currents_as_efforts"] == "true") ||
+                              (info_.hardware_parameters["use_currents_as_efforts"] == "True"));
+
   // The ip address of the host the driver runs on
   std::string reverse_ip = info_.hardware_parameters["reverse_ip"];
   if (reverse_ip == "0.0.0.0") {
@@ -664,6 +669,17 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   get_robot_software_version_build_ = version_info_.build;
   get_robot_software_version_bugfix_ = version_info_.bugfix;
 
+  if (!use_currents_as_efforts_) {
+    if ((version_info_.major == 5 && version_info_.minor < 23) ||
+        (version_info_.major == 10 && version_info_.minor < 11) || version_info_.major < 5) {
+      RCLCPP_ERROR(get_logger(),
+                   "Driver configured to use actual torques as efforts, which is not supported by this software "
+                   "version %s. Please use version 5.23.0 / 10.11.0 or newer for this feature.",
+                   version_info_.toString().c_str());
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+  }
+
   async_thread_ = std::make_shared<std::thread>(&URPositionHardwareInterface::asyncThread, this);
 
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!");
@@ -774,7 +790,11 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
     packet_read_ = true;
     readData(data_pkg, "actual_q", urcl_joint_positions_);
     readData(data_pkg, "actual_qd", urcl_joint_velocities_);
-    readData(data_pkg, "actual_current", urcl_joint_efforts_);
+    if (use_currents_as_efforts_) {
+      readData(data_pkg, "actual_current", urcl_joint_efforts_);
+    } else {
+      readData(data_pkg, "actual_current_as_torque", urcl_joint_efforts_);
+    }
 
     readData(data_pkg, "target_speed_fraction", target_speed_fraction_);
     readData(data_pkg, "speed_scaling", speed_scaling_);

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -26,6 +26,7 @@
     trajectory_port:=50003
     non_blocking_read:=true
     robot_receive_timeout:=0.04
+    use_currents_as_efforts:=true
     ">
 
     <xacro:property name="config_kinematics_parameters" value="${xacro.load_yaml(kinematics_parameters_file)}"/>
@@ -71,6 +72,7 @@
           <param name="tool_device_name">${tool_device_name}</param>
           <param name="tool_tcp_port">${tool_tcp_port}</param>
           <param name="robot_receive_timeout">${robot_receive_timeout}</param>
+          <param name="use_currents_as_efforts">${use_currents_as_efforts}</param>
         </xacro:unless>
       </hardware>
 

ur_robot_driver/urdf/ur.urdf.xacro

@@ -33,6 +33,8 @@
   <xacro:arg name="reverse_port" default="50001"/>
   <xacro:arg name="script_sender_port" default="50002"/>
   <xacro:arg name="trajectory_port" default="50003"/>
+  <xacro:arg name="use_currents_as_efforts" default="true"/>
+
   <!--   tool communication related parameters-->
   <xacro:arg name="use_tool_communication" default="false" />
   <xacro:arg name="tool_voltage" default="0" />
@@ -101,6 +103,7 @@
     reverse_port="$(arg reverse_port)"
     script_sender_port="$(arg script_sender_port)"
     trajectory_port="$(arg trajectory_port)"
+    use_currents_as_efforts="$(arg use_currents_as_efforts)"
   />
 
 </robot>