Direct torque friction scales support

.github/workflows/ci.yml

@@ -127,6 +127,14 @@ jobs:
           path: test_artifacts
           if-no-files-found: error
           retention-days: 10
+      - name: Upload generated scripts
+        uses: actions/upload-artifact@v6
+        if: ${{ always() }}
+        with:
+          name: ${{matrix.env.ROBOT_MODEL}}_${{matrix.env.URSIM_VERSION}}_generated_scripts
+          path: test_artifacts/generated_scripts
+          if-no-files-found: warn
+          retention-days: 10
 
   test_start_ursim:
     runs-on: ubuntu-latest

doc/architecture/script_command_interface.rst

@@ -23,6 +23,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
 - ``setFrictionCompensation()``: Set friction compensation for torque command.
 - ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing.
 - ``setGravity()``: Set the gravity vector for the robot.
+- ``setTcpOffset()``: Set the TCP offset of the robot.
+- ``setFrictionScales()``: Set viscous and Coulomb friction scale factors for direct torque control.
 
 Communication protocol
 ----------------------
@@ -54,6 +56,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 7: setFrictionCompensation
            - 8: ftRtdeInputEnable
            - 9: setGravity
+           - 10: setTcpOffset
+           - 11: setFrictionScales
    1-27   data fields specific to the command
    =====  =====
 
@@ -127,7 +131,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    1      No specific meaning / values ignored
    =====  =====
 
-.. table:: With setFrictionCompensation command
+.. table:: With setFrictionCompensation command (Deprecated, use setFrictionScales instead)
    :widths: auto
 
    =====  =====
@@ -157,6 +161,25 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    1-3    The gravity vector (towards the Earth's center), represented in robot's base frame (floating point)
    =====  =====
 
+.. table:: With setTcpOffset command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1-6    TCP offset as [x, y, z, rx, ry, rz] given in the flange coordinate system in SI units (meters and radians, floating point)
+   =====  =====
+
+.. table:: With setFrictionScales command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1-6    Viscous friction scale factors. One number per joint, range [0-1]. 0 means no compensation for that joint. 1 means full compensation. Default is [0.9, 0.9, 0.8, 0.9, 0.9, 0.9]. (floating point)
+   7-12   Coulomb friction scale factors. One number per joint, range [0-1]. 0 means no compensation for that joint. 1 means full compensation. Default is [0.8, 0.8, 0.7, 0.8, 0.8, 0.8]. (floating point)
+   =====  =====
+
 .. note::
    In URScript the ``socket_read_binary_integer()`` function is used to read the data from the
    script command socket. The first index in that function's return value is the number of integers read,

examples/direct_torque_control.cpp

@@ -29,6 +29,8 @@
 // -- END LICENSE BLOCK ------------------------------------------------
 
 #include <ur_client_library/example_robot_wrapper.h>
+#include "ur_client_library/ur/instruction_executor.h"
+#include <cmath>
 #include <cstddef>
 
 // In a real-world example it would be better to get those values from command line parameters / a
@@ -40,6 +42,12 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 
 const size_t JOINT_INDEX = 5;  // Joint index to control, in this case joint 6 (index 5)
 
+// This example will apply a sinusoidal torque to JOINT_INDEX, while the other joints are kept at 0.0.
+constexpr double frequency = 0.2;  // [Hz]
+constexpr double amplitude = 2.5;  // [Nm]
+constexpr double omega = 2 * M_PI * frequency;
+constexpr double start_position = 0.0;  // [rad]
+
 std::unique_ptr<urcl::ExampleRobotWrapper> g_my_robot;
 urcl::vector6d_t g_joint_positions;
 
@@ -70,37 +78,69 @@ int main(int argc, char* argv[])
     return 1;
   }
 
-  // Torque control requires Software version 5.23 / 10.10 or higher. Error and exit on older
-  // software versions.
+  // This example requires Software version 5.25.1 / 10.12.1 or higher. Skip gracefully on older
+  // software versions so that CI (which runs all examples) does not fail.
   {
     auto robot_version = g_my_robot->getUrDriver()->getVersion();
-    if (robot_version < urcl::VersionInformation::fromString("5.23.0") ||
-        (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0")))
+    bool version_supported = (robot_version.major == 5 && robot_version >= urcl::VersionInformation::fromString("5.25."
+                                                                                                                "1")) ||
+                             (robot_version.major >= 10 && robot_version >= urcl::VersionInformation::fromString("10."
+                                                                                                                 "12."
+                                                                                                                 "1"));
+    if (!version_supported)
     {
-      URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.",
-                     robot_version.toString().c_str());
+      URCL_LOG_WARN("This direct_torque control example requires a robot with at least version 5.25.1 / 10.12.1. Your "
+                    "robot has version %s. Skipping.",
+                    robot_version.toString().c_str());
       return 0;
     }
   }
   // --------------- INITIALIZATION END -------------------
 
-  const double torque_abs = 2.5;
-  double cmd_torque = torque_abs;  // Target torque [Nm] for joint 6
-  bool passed_negative_part = false;
-  bool passed_positive_part = false;
   URCL_LOG_INFO("Start moving the robot");
   urcl::vector6d_t target_torques = { 0, 0, 0, 0, 0, 0 };
 
+  // Scale each individual joint's friction compensation. This is supported from PolyScope 5.25.1 / PolyScope X 10.12.1
+  // and upwards.
+  urcl::vector6d_t viscous_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  urcl::vector6d_t coulomb_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+
+  // As this example only applies to JOINT_INDEX, we can set the other joints to 0.0 to disable friction compensation to
+  // make them more steady.
+  viscous_scale[JOINT_INDEX] = 1.0;
+  coulomb_scale[JOINT_INDEX] = 1.0;
+  g_my_robot->getUrDriver()->setFrictionScales(viscous_scale, coulomb_scale);
+
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
   // loop.
   g_my_robot->getUrDriver()->startRTDECommunication();
-  auto start_time = std::chrono::system_clock::now();
 
   urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+  if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
+  {
+    URCL_LOG_ERROR("Could not get fresh data package from robot");
+    return 1;
+  }
+  if (!data_pkg.getData("actual_q", g_joint_positions))
+  {
+    // This throwing should never happen unless misconfigured
+    std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
+    throw std::runtime_error(error_msg);
+  }
+  // Use motion primitives to move JOINT_INDEX to start position
+  g_joint_positions[JOINT_INDEX] = start_position;
+  auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_robot->getUrDriver());
+  instruction_executor->moveJ(g_joint_positions, 1.4, 1.04, 0.1);
+
+  auto start_time = std::chrono::system_clock::now();
+  constexpr double timestep = 0.002;  // [s]
+  double time = 0.0;
 
-  while (!(passed_positive_part && passed_negative_part))
+  // Run 10 periods of the sinusoidal
+  while (time < 10 * 2 * M_PI / omega)
   {
+    time += timestep;
     // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
     // robot will effectively be in charge of setting the frequency of this loop.
     // In a real-world application this thread should be scheduled with real-time priority in order
@@ -119,23 +159,7 @@ int main(int argc, char* argv[])
     }
 
     // Open loop control. The target is incremented with a constant each control loop
-    if (passed_positive_part == false)
-    {
-      if (g_joint_positions[JOINT_INDEX] >= 2)
-      {
-        passed_positive_part = true;
-        cmd_torque = -torque_abs;
-      }
-    }
-    else if (passed_negative_part == false)
-    {
-      if (g_joint_positions[JOINT_INDEX] <= 0)
-      {
-        cmd_torque = torque_abs;
-        passed_negative_part = true;
-      }
-    }
-    target_torques[JOINT_INDEX] = cmd_torque;
+    target_torques[JOINT_INDEX] = amplitude * std::sin(omega * time);
 
     // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more
     // reliable on non-realtime systems. Use with caution in productive applications. Having it
@@ -145,7 +169,7 @@ int main(int argc, char* argv[])
                                                             urcl::RobotReceiveTimeout::millisec(100));
     if (!ret)
     {
-      URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+      URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control or is the connection over WIFI?");
       return 1;
     }
     URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());

examples/full_driver.cpp

@@ -71,6 +71,7 @@ int main(int argc, char* argv[])
 
   // Increment depends on robot version
   double increment_constant = 0.0005;
+  std::cout << "Robot version: " << g_my_robot->getUrDriver()->getVersion().toString() << std::endl;
   if (g_my_robot->getUrDriver()->getVersion().major < 5)
   {
     increment_constant = 0.002;
@@ -137,7 +138,7 @@ int main(int argc, char* argv[])
                                                             RobotReceiveTimeout::millisec(100));
     if (!ret)
     {
-      URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+      URCL_LOG_ERROR("The connection timed out. Is the robot in remote control or is the connection over WIFI?");
       return 1;
     }
     URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());

examples/script_command_interface.cpp

@@ -45,6 +45,7 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
 std::unique_ptr<ExampleRobotWrapper> g_my_robot;
 bool g_HEADLESS = true;
 bool g_running = false;
+bool g_support_set_friction_scales = false;
 
 void sendScriptCommands()
 {
@@ -67,13 +68,27 @@ void sendScriptCommands()
     run_cmd("Setting tool voltage to 24V",
             []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::_24V); });
     run_cmd("Enabling tool contact mode", []() { g_my_robot->getUrDriver()->startToolContact(); });
-    run_cmd("Setting friction_compensation variable to `false`",
-            []() { g_my_robot->getUrDriver()->setFrictionCompensation(false); });
+
+    if (g_support_set_friction_scales)
+    {
+      // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards.
+      run_cmd("Turn off friction_compensation by setting the friction scales to zeroes",
+              []() { g_my_robot->getUrDriver()->setFrictionScales({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }); });
+    }
     run_cmd("Setting tool voltage to 0V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::OFF); });
     run_cmd("Zeroing the force torque sensor", []() { g_my_robot->getUrDriver()->zeroFTSensor(); });
     run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); });
-    run_cmd("Setting friction_compensation variable to `true`",
-            []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); });
+    run_cmd("Setting TCP offset to [0.0, 0.0, 0.10, 0.0, 0.0, 0.0]",
+            []() { g_my_robot->getUrDriver()->setTcpOffset({ 0.0, 0.0, 0.10, 0.0, 0.0, 0.0 }); });
+
+    if (g_support_set_friction_scales)
+    {
+      // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards.
+      run_cmd("Turn on friction_compensation by setting the friction scales to non-zero values", []() {
+        g_my_robot->getUrDriver()->setFrictionScales({ 0.9, 0.9, 0.8, 0.9, 0.9, 0.9 },
+                                                     { 0.8, 0.8, 0.7, 0.8, 0.8, 0.8 });
+      });
+    }
   }
   URCL_LOG_INFO("Script command thread finished.");
 }
@@ -106,6 +121,17 @@ int main(int argc, char* argv[])
   g_my_robot =
       std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp");
 
+  // Check if the robot version supports the friction scales command (requires 5.25.1 / 10.12.1)
+  auto version = g_my_robot->getUrDriver()->getVersion();
+  g_support_set_friction_scales = (version.major == 5 && version >= urcl::VersionInformation::fromString("5.25.1")) ||
+                                  (version.major >= 10 && version >= urcl::VersionInformation::fromString("10.12.1"));
+  if (!g_support_set_friction_scales)
+  {
+    URCL_LOG_WARN("Setting friction scales is not supported on this robot (version %s). "
+                  "Requires at least 5.25.1 / 10.12.1. Skipping friction scale commands.",
+                  version.toString().c_str());
+  }
+
   if (!g_my_robot->isHealthy())
   {
     URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");

include/ur_client_library/control/script_command_interface.h

@@ -177,16 +177,29 @@ class ScriptCommandInterface : public ReverseInterface
   bool setTcpOffset(const vector6d_t& offset);
 
   /*!
-   * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
-   * if false it will not.
+   * \brief Set friction compensation for the direct_torque. If true the direct_torque command will compensate for
+   * friction, if false it will not.
    *
    * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be
-   * used when calling torque_command
+   * used when calling direct_torque.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
   bool setFrictionCompensation(const bool friction_compensation_enabled);
 
+  /*!
+   * \brief Set viscous and Coulomb friction scale factors for direct_torque (per joint, range [0-1]).
+   *
+   * Requires PolyScope 5.25.1 / PolyScope X 10.12.1 or later. To disable friction compensation, pass both
+   * arrays as all zeros.
+   *
+   * \param viscous_scale Scale of viscous compensation per joint, range [0-1].
+   * \param coulomb_scale Scale of Coulomb compensation per joint, range [0-1].
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale);
+
   /*!
    * \brief Enable or disable RTDE input for the force torque sensor.
    *
@@ -248,6 +261,7 @@ class ScriptCommandInterface : public ReverseInterface
     FT_RTDE_INPUT_ENABLE = 8,       ///< Enable FT RTDE input
     SET_GRAVITY = 9,                ///< Set gravity vector
     SET_TCP_OFFSET = 10,            ///< Set TCP offset
+    SET_FRICTION_SCALES = 11,       ///< Set viscous and Coulomb friction scales for direct_torque
   };
 
   /*!

include/ur_client_library/ur/ur_driver.h

@@ -771,16 +771,34 @@ class UrDriver
   bool setTcpOffset(const vector6d_t& tcp_offset);
 
   /*!
-   * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
+   * \brief Set friction compensation for the direct_torque. If true the torque command will compensate for friction,
    * if false it will not.
    *
+   * \deprecated Use setFrictionScales() instead.
+   *
    * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be
-   * used when calling torque_command
+   * used when calling direct_torque.
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
+  [[deprecated("setFrictionCompensation is deprecated. Use setFrictionScales instead.")]]
   bool setFrictionCompensation(const bool friction_compensation_enabled);
 
+  /*!
+   * \brief Set viscous and Coulomb friction scale factors for direct_torque (per joint, range [0-1]).
+   *
+   * Values are per-joint scales for friction compensation. Zero means no compensation for that component.
+   * To disable friction compensation, pass both arrays as all zeros. Controller defaults when enabling
+   * are viscous_scale [0.9, 0.9, 0.8, 0.9, 0.9, 0.9] and coulomb_scale [0.8, 0.8, 0.7, 0.8, 0.8, 0.8].
+   * Requires PolyScope 5.25.1 / PolyScope X 10.12.1 or later.
+   *
+   * \param viscous_scale Scale of viscous compensation per joint, range [0-1].
+   * \param coulomb_scale Scale of Coulomb compensation per joint, range [0-1].
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale);
+
   /*!
    * \brief Enable or disable RTDE input for the force torque sensor.
    *