Add a resolved acceleration control mode

.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,

doc/examples/direct_torque_control.rst

@@ -6,17 +6,17 @@ Torque control example
 ======================
 
 In the ``examples`` subfolder you will find a minimal example for commanding torques to the robot.
-It moves the robot in the 5th axis back and forth, while reading the joint positions. To run it
-make sure to
+It applies a sinusoidal torque to the 6th joint (index 5) while reading the joint positions. To run
+it make sure to
 
 * have an instance of a robot controller / URSim running at the configured IP address (or adapt the
   address to your needs)
-* have PolyScope version 5.23.0 / 10.10.0 or later installed on the robot.
+* have PolyScope version 5.25.1 / 10.12.1 or later installed on the robot.
 * run it from the package's main folder (the one where the README.md file is stored), as for
   simplicity reasons it doesn't use any sophisticated method to locate the required files.
 
-This page will walk you through the `full_driver.cpp
-<https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/full_driver.cpp>`_
+This page will walk you through the `direct_torque_control.cpp
+<https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/direct_torque_control.cpp>`_
 example.
 
 Initialization
@@ -34,39 +34,69 @@ recipes.
    :end-at: // --------------- INITIALIZATION END -------------------
 
 .. note::
-   This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the direct_torque_control
-   mode which is only available in these versions and later. If you try to run it on an older
-   software version, this example will print an error and exit.
+   This example requires PolyScope version 5.25.1 / 10.12.1 or later, as it uses
+   ``setFrictionScales()`` which is only available in these versions and later. If you try to run it
+   on an older software version, this example will skip gracefully.
 
-Robot control loop
-------------------
+Friction compensation setup
+----------------------------
+
+Before starting the control loop, friction compensation scales are configured using
+``setFrictionScales()``. Since this example only controls a single joint (``JOINT_INDEX``), friction
+compensation for the other joints is set to zero to keep them more steady. The controlled joint
+gets full compensation (scale factor 1.0) for both viscous and Coulomb friction.
 
-This example reads the robot's joint positions, commands a torque for the 5th axis and sends that
-back as a joint command for the next cycle. This way, the robot will move its wrist first until a
-positive limit and then back to 0.
+.. literalinclude:: ../../examples/direct_torque_control.cpp
+   :language: c++
+   :caption: examples/direct_torque_control.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Scale each individual joint's friction compensation
+   :end-at: setFrictionScales(viscous_scale, coulomb_scale);
 
-To read the joint data, the driver's RTDE client is used:
+Move to start position
+----------------------
 
+Before entering the torque control loop, the robot is moved to a known start position using
+the ``InstructionExecutor``. This ensures a predictable initial state for the sinusoidal motion.
+First, RTDE communication is started and the current joint positions are read. Then, the target
+position for ``JOINT_INDEX`` is set to ``start_position`` and the robot is moved there using
+``moveJ()``.
 
 .. literalinclude:: ../../examples/direct_torque_control.cpp
    :language: c++
    :caption: examples/direct_torque_control.cpp
    :linenos:
    :lineno-match:
    :start-at: // Once RTDE communication is started
-   :end-before: // Open loop control
+   :end-at: instruction_executor->moveJ(g_joint_positions, 1.4, 1.04, 0.1);
 
-The first read operation will initialize the target buffer with the current robot position. Next,
-the target joint torques are set based on the current joint positions:
+Robot control loop
+------------------
+
+The main control loop applies a sinusoidal torque to the controlled joint for 10 full periods.
+In each cycle, the latest RTDE data package is read using ``getDataPackage()``. This call blocks
+until new data arrives from the robot (with an internal timeout), so the robot's RTDE cycle
+effectively controls the timing of this loop. The current joint positions are then extracted from
+the received package:
+
+.. literalinclude:: ../../examples/direct_torque_control.cpp
+   :language: c++
+   :caption: examples/direct_torque_control.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Run 10 periods of the sinusoidal
+   :end-before: // Open loop control
 
+The target torque for the controlled joint is computed as a sinusoidal function of time:
 
 .. literalinclude:: ../../examples/direct_torque_control.cpp
    :language: c++
    :caption: examples/direct_torque_control.cpp
    :linenos:
    :lineno-match:
    :start-at: // Open loop control
-   :end-at: target_torques[JOINT_INDEX] = cmd_torque;
+   :end-at: target_torques[JOINT_INDEX] = amplitude * std::sin(omega * time);
 
 To send the control command, the robot's :ref:`reverse_interface` is used via the
 ``writeJointCommand()`` function:

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,67 @@ 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_INFO("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 +157,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 +167,8 @@ 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. Make sure that the robot is in remote control mode and connected "
+                     "with a network cable.");
       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,8 @@ 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("Could not send joint command. Make sure that the robot is in remote control mode and connected "
+                     "with a network cable.");
       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_INFO("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.");