Direct torque control (#381)

This PR adds the functionality to command the robot by sending joint
torques directly.

---------

Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com>
Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com>

Author: 3 people

README.md

@@ -8,6 +8,8 @@ The library has no external dependencies besides the standard C++ libraries such
 to make it easy to integrate and maintain. It also serves as the foundation for the ROS and ROS 2
 drivers.
 
+---
+
 <!-- markdownlint-disable MD033 -->
 <div align="center">
   <img src="doc/resources/family_photo.png" alt="Universal Robot family" style="width: 90%;"/>

doc/architecture/reverse_interface.rst

@@ -57,6 +57,7 @@ meaning:
            - freedrive instruction (FREEDRIVE)
 
              - field 1: Freedrive mode (1: FREEDRIVE_MODE_START, -1: FREEDRIVE_MODE_STOP)
+           - target joint torques (TORQUE, see :ref:`direct_torque_control_mode`).
 
    7      Control mode. Can be either of
 
@@ -72,6 +73,7 @@ meaning:
            - 7: TOOL_IN_CONTACT -- status - not meant to be sent.
              In tool contact mode this will
              encode whether tool contact has been established or not.
+           - 8: TORQUE -- :ref:`direct_torque_control_mode` (since PolyScope 5.23.0 / 10.10.0)
    =====  =====
 
 .. note::
@@ -84,4 +86,19 @@ meaning:
    ``MULT_JOINTSTATE`` constant to get the actual floating point value. This constant is defined in
    ``ReverseInterface`` class.
 
-Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket".
+Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE, TORQUE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket".
+
+.. _direct_torque_control_mode:
+
+Direct torque control mode
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Direct torque control mode is available since PolyScope version 5.23.0 / 10.10.0. It allows to command
+joint torques directly to the robot.
+
+.. note:: Target torques are given **after** gravity compensation. A vector of zeros will hold the current position
+   given that the payload is known to the controller.
+
+.. warning:: Direct torque control is a very low-level command interface. Commanding high torques in
+   free space can make the robot move very fast and hereby trigger a fault due to joint velocities
+   or the TCP speed violating the safety settings. Keep that in mind when using this mode.

doc/architecture/script_command_interface.rst

@@ -20,6 +20,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
   contact example
   <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/tool_contact_example.cpp>`_
   for more information.
+- ``setFrictionCompensation()``: Set friction compensation for torque command.
 
 Communication protocol
 ----------------------
@@ -48,6 +49,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 4: endForceMode
            - 5: startToolContact
            - 6: endToolContact
+           - 7: setFrictionCompensation
    1-27   data fields specific to the command
    =====  =====
 
@@ -121,6 +123,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    1      No specific meaning / values ignored
    =====  =====
 
+.. table:: With setFrictionCompensation command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1      friction_compensation_enabled enable/disable friction compensation for torque command.
+   =====  =====
+
 .. 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.rst

@@ -23,8 +23,10 @@ may be running forever until manually stopped.
    examples/primary_pipeline
    examples/primary_pipeline_calibration
    examples/rtde_client
+   examples/script_command_interface
    examples/script_sender
    examples/spline_example
    examples/tool_contact_example
+   examples/direct_torque_control
    examples/trajectory_point_interface
    examples/ur_driver

doc/examples/direct_torque_control.rst

@@ -0,0 +1,80 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/direct_torque_control.rst
+
+.. _direct_torque_control_example:
+
+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
+
+* 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.
+* 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>`_
+example.
+
+Initialization
+--------------
+
+At first, we create a ``ExampleRobotWrapper`` object giving it the robot's IP address, script file and RTDE
+recipes.
+
+.. literalinclude:: ../../examples/direct_torque_control.cpp
+   :language: c++
+   :caption: examples/direct_torque_control.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: bool headless_mode = true;
+   :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.
+
+Robot control loop
+------------------
+
+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.
+
+To read the joint data, the driver's RTDE client is used:
+
+
+.. 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
+
+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:
+
+
+.. 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;
+
+To send the control command, the robot's :ref:`reverse_interface` is used via the
+``writeJointCommand()`` function:
+
+.. literalinclude:: ../../examples/direct_torque_control.cpp
+   :language: c++
+   :caption: examples/direct_torque_control.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Setting the RobotReceiveTimeout
+   :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());

doc/examples/script_command_interface.rst

@@ -0,0 +1,71 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/script_command_interface.rst
+
+Script command interface
+========================
+
+The :ref:`script_command_interface` allows sending predefined commands to the robot while there is
+URScript running that is connected to it.
+
+An example to utilize the script command interface can be found in the `freedrive_example.cpp <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/script_command_interface.cpp>`_.
+
+In order to use the ``ScriptCommandInterface``, there has to be a script code running on the robot
+that connects to the ``ScriptCommandInterface``. This happens as part of the big
+`external_control.urscript <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/resources/external_control.urscript>`_. In order to reuse that with this example, we will create a full
+``UrDriver`` and leverage the ``ScriptCommandInterface`` through this.
+
+At first, we create a ``ExampleRobotWrapper`` object in order to initialize communication with the
+robot.
+
+.. literalinclude:: ../../examples/script_command_interface.cpp
+   :language: c++
+   :caption: examples/script_command_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: g_my_robot =
+   :end-at: std::thread script_command_send_thread(sendScriptCommands);
+
+The script commands will be sent in a separate thread which will be explained later.
+
+Since the connection to the script command interface runs as part of the bigger external_control
+script, we'll wrap the calls alongside a full ``ExampleRobotWrapper``. Hence, we'll have to send
+keepalive signals regularly to keep the script running:
+
+.. literalinclude:: ../../examples/script_command_interface.cpp
+   :language: c++
+   :caption: examples/script_command_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: std::chrono::duration<double> time_done(0);
+   :end-at: g_my_robot->getUrDriver()->stopControl();
+
+Sending script commands
+-----------------------
+
+Once the script is running on the robot, a connection to the driver's script command interface
+should be established. The ``UrDriver`` forwards most calls of the ``ScriptCommandInterface`` and
+we will use that interface in this example. To send a script command, we can e.g. use
+``g_my_robot->getUrDriver()->zeroFTSensor()``.
+
+In the example, we have wrapped the calls into a lambda function that will wait a specific timeout,
+print a log output what command will be sent and then call the respective command:
+
+.. literalinclude:: ../../examples/script_command_interface.cpp
+   :language: c++
+   :caption: examples/script_command_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: run_cmd(
+   :end-before: URCL_LOG_INFO("Script command thread finished.");
+
+The lambda itself looks like this:
+
+.. literalinclude:: ../../examples/script_command_interface.cpp
+   :language: c++
+   :caption: examples/script_command_interface.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: auto run_cmd =
+   :end-before: // Keep running all commands in a loop
+
+For a list of all available script commands, please refer to the ``ScriptCommandInterface`` class
+`here <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/include/ur_client_library/control/script_command_interface.h>`_.

doc/polyscope_compatibility.rst

@@ -3,6 +3,9 @@
 |polyscope| version compatibility
 =================================
 
+Version-breaking changes
+------------------------
+
 The table below shows breaking changes in the library compared to |polyscope| versions. Compatibility
 is listed for CB3 robots (versions 3.x.y) and e-Series robots (versions 5.x.y) respectively.
 
@@ -44,3 +47,17 @@ table below or checkout the latest tag before the breaking changes were introduc
        See `Universal Robots External Control URCapX <https://github.com/UniversalRobots/Universal_Robots_ExternalControl_URCapX>`_
 
 .. |polyscope| replace:: PolyScope
+
+Features requiring a specific |polyscope| version
+-------------------------------------------------
+
+Features in this section have been added in a backwards-compatible way. It is still possible to use
+this library with an older compatible version, but trying to use one of the features below might
+lead to a runtime exception.
+
+Torque control (From version 2.4.0)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The direct torque control mode is only available on |polyscope| 5.23.0 / 10.10.0 and later. This
+includes the ``TORQUE`` control mode in the ``ReverseInterface`` as well as the
+``setFrictionCompensation()`` function in the ``ScriptCommandInterface``.

examples/CMakeLists.txt

@@ -44,6 +44,14 @@ add_executable(script_sender_example
 script_sender.cpp)
 target_link_libraries(script_sender_example ur_client_library::urcl)
 
+add_executable(script_command_interface_example
+script_command_interface.cpp)
+target_link_libraries(script_command_interface_example ur_client_library::urcl)
+
+add_executable(direct_torque_control_example
+  direct_torque_control.cpp)
+target_link_libraries(direct_torque_control_example ur_client_library::urcl)
+
 add_executable(trajectory_point_interface_example
 trajectory_point_interface.cpp)
 target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)