Allow using the actual torques as joint efforts

Author: urfeex

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -400,6 +400,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   std::unique_ptr<urcl::rtde_interface::DataPackage> data_package_buffer_;
   std::function<bool()> get_data_package;
+
+  bool use_currents_as_efforts_ = false;
 };
 }  // namespace ur_robot_driver
 

ur_robot_driver/launch/ur_rsp.launch.py

@@ -74,6 +74,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(
         [
@@ -191,6 +192,8 @@ def generate_launch_description():
             "trajectory_port:=",
             trajectory_port,
             " ",
+            "use_currents_as_efforts:=",
+            use_currents_as_efforts,
         ]
     )
     robot_description = {
@@ -457,6 +460,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,6 +26,7 @@ safety_mode
 robot_status_bits
 safety_status_bits
 actual_current
+actual_current_as_torque
 tcp_offset
 payload
 payload_cog

ur_robot_driver/src/hardware_interface.cpp

@@ -676,6 +676,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") {
@@ -858,6 +863,17 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing InstructionExecutor");
   instruction_executor_ = std::make_shared<urcl::InstructionExecutor>(ur_driver_);
 
+  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);
 
   // Start async thread for sending motion primitives
@@ -971,7 +987,11 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
     packet_read_ = true;
     readData(data_package_buffer_, "actual_q", urcl_joint_positions_);
     readData(data_package_buffer_, "actual_qd", urcl_joint_velocities_);
-    readData(data_package_buffer_, "actual_current", urcl_joint_efforts_);
+    if (use_currents_as_efforts_) {
+      readData(data_package_buffer_, "actual_current", urcl_joint_efforts_);
+    } else {
+      readData(data_package_buffer_, "actual_current_as_torque", urcl_joint_efforts_);
+    }
     readData(data_package_buffer_, "target_speed_fraction", target_speed_fraction_);
     readData(data_package_buffer_, "speed_scaling", speed_scaling_);
     readData(data_package_buffer_, "runtime_state", runtime_state_);

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -29,6 +29,7 @@
     robot_receive_timeout:=0.04
     rw_rate:=0
     verify_robot_model:=true
+    use_currents_as_efforts:=true
     ">
     <!-- rw_rate of 0 uses the rate of the controller_manager -->
 
@@ -77,6 +78,7 @@
           <param name="tool_tcp_port">${tool_tcp_port}</param>
           <param name="robot_receive_timeout">${robot_receive_timeout}</param>
           <param name="verify_robot_model">${verify_robot_model}</param>
+          <param name="use_currents_as_efforts">${use_currents_as_efforts}</param>
         </xacro:unless>
       </hardware>
 

ur_robot_driver/urdf/ur.urdf.xacro

@@ -34,6 +34,8 @@
   <xacro:arg name="script_sender_port" default="50002"/>
   <xacro:arg name="trajectory_port" default="50003"/>
   <xacro:arg name="verify_robot_model" default="true" />
+  <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" />
@@ -104,6 +106,7 @@
     trajectory_port="$(arg trajectory_port)"
     ur_type="$(arg ur_type)"
     verify_robot_model="$(arg verify_robot_model)"
+    use_currents_as_efforts="$(arg use_currents_as_efforts)"
   />
 
 </robot>