Currents as efforts

ur_robot_driver/doc/hardware_interface.rst

@@ -26,6 +26,10 @@ The UR hardware interface supports the following control modes:
 - **Velocity control**: The robot's joints are controlled by specifying target velocities.
 - **Effort control**: The robot's joints are controlled by specifying target efforts (torques).
   (Only available when running PolyScope >= 5.23.0 / 10.10.0)
+- **Effort state interfaces**: Joint efforts are reported as currents by default
+  To report the currents as torques (RTDE field ``actual_current_as_torque``, requires PolyScope >=
+  5.23.0 / 10.11.0), set ``use_currents_as_efforts`` to ``false``.
+  See :doc:`../hardware_interface_parameters` for more information.
 - **Force control**: The robot's end-effector is controlled by specifying target forces
   in Cartesian space.
 - **Freedrive mode**: The robot can be moved freely by the user without any active control.

ur_robot_driver/doc/hardware_interface_parameters.rst

@@ -117,6 +117,23 @@ Tool voltage that will be set as soon as the UR-Program on the robot is started.
 
 This can also be configured using the robot teach pendant. Remember to save the installation on the robot to keep the setting after reboot.
 
+use_currents_as_efforts (default: "true")
+------------------------------------------
+
+Selects which RTDE values are exported on the joint effort state interfaces.
+
+* When set to ``false``, the driver reports the joint torques from the RTDE field
+  ``actual_current_as_torque``. This requires PolyScope >= 5.23.0 / 10.11.0. On older software
+  versions, configuring the hardware interface will fail.
+* When set to ``true`` (default), the driver keeps the previous behavior and reports motor currents from
+  ``actual_current`` as efforts.
+* When set to ``false`` on robot software versions < 5.23.0 / 10.11.0, the driver will
+  fail to initialize, printing an error about the missing variable ``actual_current_as_torque`` on
+  the robot.
+
+This parameter can also be set through the ``use_currents_as_efforts`` launch argument of
+``ur_control.launch.py`` / ``ur_rsp.launch.py``.
+
 use_tool_communication (Required)
 ---------------------------------
 

ur_robot_driver/doc/usage/force_torque_control.rst

@@ -56,8 +56,11 @@ with one value per joint.
 
 .. note::
 
-   The ``effort`` field in ``sensor_msgs/JointState`` (published by the ``joint_state_broadcaster``)
-   contains motor currents, not physical joint torques.
+   The ``effort`` field in ``sensor_msgs/JointState`` (published by the
+   ``joint_state_broadcaster``) contains motor currents, not physical joint torques by default. On
+   newer robot software versions (>= 5.23.0 / 10.11.0), the driver can report the actual joint
+   torques as efforts. To achieve that, set the hardware parameter ``use_currents_as_efforts`` to
+   ``false``. See :doc:`../hardware_interface_parameters` for details.
 
 Friction Compensation
 ^^^^^^^^^^^^^^^^^^^^^

ur_robot_driver/doc/usage/startup.rst

@@ -34,6 +34,9 @@ Other important arguments are:
 * ``use_mock_hardware`` (default: *false* ) - Use simple hardware emulator from ros2_control. Useful for testing launch files, descriptions, etc.
 * ``headless_mode`` (default: *false*) - Start driver in :ref:`headless_mode`.
 * ``launch_rviz`` (default: *true*) - Start RViz together with the driver.
+* ``use_currents_as_efforts`` (default: *false*) - Report motor currents as joint efforts.
+  When ``false``, joint torques from the robot are used instead (requires PolyScope >= 5.23.0 /
+  10.11.0). See :doc:`../hardware_interface_parameters`.
 * ``initial_joint_controller`` (default: *joint_trajectory_controller*) - Use this if you
   want to start the robot with another controller.
 

ur_robot_driver/doc/usage/utility_controllers.rst

@@ -19,12 +19,16 @@ joint_state_broadcaster
 
 Type: `joint_state_broadcaster/JointStateBroadcaster <https://control.ros.org/rolling/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html>`_
 
-Publishes all joints' positions, velocities, and motor currents as ``sensor_msgs/JointState`` on the ``joint_states`` topic. This broadcaster is read-only and can run alongside any other controller.
+Publishes all joints' positions, velocities, and efforts as ``sensor_msgs/JointState`` on the
+``joint_states`` topic. This broadcaster is read-only and can run alongside any other controller.
 
 .. note::
 
-   The effort field contains the currents reported by the joints and not the actual efforts in a
-   physical sense.
+   The ``effort`` field in ``sensor_msgs/JointState`` (published by the
+   ``joint_state_broadcaster``) contains motor currents, not physical joint torques by default. On
+   newer robot software versions (>= 5.23.0 / 10.11.0), the driver can report the actual joint
+   torques as efforts. To achieve that, set the hardware parameter ``use_currents_as_efforts`` to
+   ``false``. See :doc:`../hardware_interface_parameters` for details.
 
 speed_scaling_state_broadcaster
 -------------------------------

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -403,6 +403,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_control.launch.py

@@ -516,6 +516,16 @@ 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."
+            ),
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             name="update_rate_config_file",

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/src/hardware_interface.cpp

@@ -701,6 +701,9 @@ 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"]);
 
+  use_currents_as_efforts_ = ((info_.hardware_parameters["use_currents_as_efforts"] != "false") &&
+                              (info_.hardware_parameters["use_currents_as_efforts"] != "False"));
+
   // 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") {
@@ -797,13 +800,31 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   }
 
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
+  std::string ur_type = info_.hardware_parameters["ur_type"];
+  auto expected_type = robotTypeFromString(ur_type);
   try {
+    auto input_recipe = urcl::rtde_interface::RTDEClient::readRecipe(input_recipe_filename);
+    auto output_recipe = urcl::rtde_interface::RTDEClient::readRecipe(output_recipe_filename);
+
+    if (!use_currents_as_efforts_) {
+      if (expected_type.robot_series == urcl::RobotSeries::CB3) {
+        RCLCPP_WARN_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "Using actual joint torques as efforts "
+                                                                              "requested on a CB3 robot. This is not "
+                                                                              "supported and will fail to initialize. "
+                                                                              "Please set the parameter "
+                                                                              "'use_currents_as_efforts' to true.");
+      }
+      if (std::find(output_recipe.begin(), output_recipe.end(), "actual_current_as_torque") == output_recipe.end()) {
+        output_recipe.push_back("actual_current_as_torque");
+      }
+    }
+
     rtde_comm_has_been_started_ = false;
     urcl::UrDriverConfiguration driver_config;
     driver_config.robot_ip = robot_ip;
     driver_config.script_file = script_filename;
-    driver_config.output_recipe_file = output_recipe_filename;
-    driver_config.input_recipe_file = input_recipe_filename;
+    driver_config.output_recipe = output_recipe;
+    driver_config.input_recipe = input_recipe;
     driver_config.headless_mode = headless_mode;
     driver_config.reverse_port = static_cast<uint32_t>(reverse_port);
     driver_config.script_sender_port = static_cast<uint32_t>(script_sender_port);
@@ -818,12 +839,26 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
         std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1);
     ur_driver_ = std::make_shared<urcl::UrDriver>(driver_config);
     if (ur_driver_->getControlFrequency() != info_.rw_rate) {
-      ur_driver_->resetRTDEClient(output_recipe_filename, input_recipe_filename, info_.rw_rate);
+      ur_driver_->resetRTDEClient(output_recipe, input_recipe, info_.rw_rate);
     }
     data_package_buffer_ = std::make_unique<rtde::DataPackage>(ur_driver_->getRTDEOutputRecipe());
   } catch (urcl::ToolCommNotAvailable& e) {
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "See parameter use_tool_communication");
 
+    return hardware_interface::CallbackReturn::ERROR;
+  } catch (urcl::RTDEInvalidKeyException& e) {
+    RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), e.what());
+    if (std::find(e.invalid_keys.begin(), e.invalid_keys.end(), "actual_current_as_torque") != e.invalid_keys.end()) {
+      RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), "The robot declined the RTDE key "
+                                                                             "'actual_current_as_torque'. This is "
+                                                                             "required for "
+                                                                             "using actual joint torques as efforts. "
+                                                                             "Please use a newer version of the UR "
+                                                                             "robot software "
+                                                                             "(5.23.0 / 10.11.0 or newer) or set the "
+                                                                             "parameter 'use_currents_as_efforts' to "
+                                                                             "true.");
+    }
     return hardware_interface::CallbackReturn::ERROR;
   } catch (urcl::UrException& e) {
     RCLCPP_FATAL_STREAM(rclcpp::get_logger("URPositionHardwareInterface"), e.what());
@@ -839,8 +874,6 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   get_robot_software_version_build_ = version_info_.build;
   get_robot_software_version_bugfix_ = version_info_.bugfix;
 
-  std::string ur_type = info_.hardware_parameters["ur_type"];
-  auto expected_type = robotTypeFromString(ur_type);
   auto robot_type = ur_driver_->getPrimaryClient()->getRobotType();
   auto robot_series = ur_driver_->getPrimaryClient()->getRobotSeries();
 
@@ -883,6 +916,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
@@ -996,7 +1040,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>