Add support for the g-Series 48V T2 tool connector

doc/architecture/script_command_interface.rst

@@ -13,7 +13,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
 
 - ``zeroFTSensor()``: Zeros the force/torque sensor.
 - ``setPayload()``: Set the active payload mass and center of gravity.
-- ``setToolVoltage()``: Set the voltage of the tool output.
+- ``setToolVoltage()``: Set the voltage of the tool output on connector T0.
 - ``startForceMode()`` / ``endForceMode()``: Start and end a force mode. See the `force mode
   example <https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/force_mode_example.cpp>`_ for more information.
 - ``startToolContact()`` / ``endToolContact()``: Start and end a tool contact mode. See the `tool
@@ -26,6 +26,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
 - ``setTcpOffset()``: Set the TCP offset of the robot.
 - ``setFrictionScales()``: Set viscous and Coulomb friction scale factors for direct torque control.
 - ``setTargetPayload()``: Set the active payload mass, center of gravity, inertia matrix and transition time.
+- ``setToolVoltageT2()``: Set the tool voltage on connector T2. Requires PolyScope X 10.15.0 or later and Tool Flange
+  V2, as reported by the RTDE robot properties. PolyScope 5 does not support connector T2.
 
 Communication protocol
 ----------------------
@@ -60,6 +62,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 10: setTcpOffset
            - 11: setFrictionScales
            - 12: setTargetPayload
+           - 13: setToolVoltageT2
    1-27   data fields specific to the command
    =====  =====
 
@@ -194,6 +197,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    11     Payload transition time in seconds (floating point)
    =====  =====
 
+.. table:: With setToolVoltageT2 command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1      Voltage in V (Has to be 0, 24 or 48)
+   =====  =====
+
 .. 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/script_command_interface.cpp

@@ -46,6 +46,7 @@ std::unique_ptr<ExampleRobotWrapper> g_my_robot;
 bool g_HEADLESS = true;
 bool g_running = false;
 bool g_support_set_friction_scales = false;
+bool g_support_set_tool_t2_voltage = false;
 
 void sendScriptCommands()
 {
@@ -84,6 +85,14 @@ void sendScriptCommands()
       g_my_robot->getUrDriver()->setTargetPayload(2.0, { 0.0, 0.0, 0.1 }, { 0.003, 0.003, 0.003, 0.0, 0.0, 0.0 }, 0.5);
     });
 
+    if (g_support_set_tool_t2_voltage)
+    {
+      run_cmd("Setting tool T2 voltage to 48V",
+              []() { g_my_robot->getUrDriver()->setToolVoltageT2(urcl::ToolVoltage::_48V); });
+      run_cmd("Setting tool T2 voltage to 0V",
+              []() { g_my_robot->getUrDriver()->setToolVoltageT2(urcl::ToolVoltage::OFF); });
+    }
+
     if (g_support_set_friction_scales)
     {
       // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards.
@@ -135,6 +144,13 @@ int main(int argc, char* argv[])
                   version.toString().c_str());
   }
 
+  // Connector T2 only exists on Tool Flange V2
+  g_support_set_tool_t2_voltage = g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() == ToolFlangeType::V2;
+  if (!g_support_set_tool_t2_voltage)
+  {
+    URCL_LOG_INFO("This robot does not report Tool Flange V2. Skipping tool T2 voltage commands.");
+  }
+
   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

@@ -116,14 +116,27 @@ class ScriptCommandInterface : public ReverseInterface
   bool setGravity(const vector3d_t* gravity);
 
   /*!
-   * \brief Set the tool voltage.
+   * \brief Set the tool voltage on connector T0.
    *
-   * \param voltage Tool voltage
+   * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES.
    *
-   * \returns True, if the write was performed successfully, false otherwise.
+   * \returns True, if the write was performed successfully, false if the voltage is invalid or the write fails.
    */
   bool setToolVoltage(const ToolVoltage voltage);
 
+  /*!
+   * \brief Set the tool voltage on connector T2 (Tool Flange V2).
+   *
+   * Requires PolyScope X 10.15.0 or later; PolyScope 5 does not support connector T2. This does not
+   * check the tool flange; UrDriver::setToolVoltageT2() does.
+   *
+   * \param voltage Tool voltage. Must be one of \ref T2_TOOL_VOLTAGES.
+   *
+   * \returns True, if the write was performed successfully, false if the voltage is invalid, the version is
+   * unsupported, or the write fails.
+   */
+  bool setToolVoltageT2(const ToolVoltage voltage);
+
   /*!
    * \brief Set robot to be controlled in force mode.
    *
@@ -279,6 +292,7 @@ class ScriptCommandInterface : public ReverseInterface
     SET_TCP_OFFSET = 10,            ///< Set TCP offset
     SET_FRICTION_SCALES = 11,       ///< Set viscous and Coulomb friction scales for direct_torque
     SET_TARGET_PAYLOAD = 12,        ///< Set target payload
+    SET_TOOL_T2_VOLTAGE = 13,       ///< Set tool voltage on connector T2
   };
 
   /*!
@@ -300,6 +314,30 @@ class ScriptCommandInterface : public ReverseInterface
   bool robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5,
                                          const VersionInformation& min_polyscopeX, const std::string& command_name);
 
+  /*!
+   * \brief Checks if the robot runs PolyScope X of at least \p min_polyscopeX, for commands that PolyScope 5
+   * does not support in any version.
+   *
+   * If not, this function will log a warning message.
+   *
+   * \param min_polyscopeX Minimum required version for PolyScope X
+   * \param command_name Name of the command being checked, used for logging
+   *
+   * \returns True if the robot version is at least \p min_polyscopeX, false otherwise.
+   */
+  bool robotVersionSupportsPolyscopeXCommandOrWarn(const VersionInformation& min_polyscopeX,
+                                                   const std::string& command_name);
+
+  /*!
+   * \brief Write a script command and one integer argument, padded with zeros to \ref MAX_MESSAGE_LENGTH.
+   *
+   * \param command Script command id
+   * \param argument Argument already scaled the way the external control script expects it
+   *
+   * \returns True if the write was performed successfully, false otherwise.
+   */
+  bool writeCommandWithArgument(const ScriptCommand command, const int32_t argument);
+
   std::atomic<bool> client_connected_;
   static const int MAX_MESSAGE_LENGTH = 28;
 

include/ur_client_library/ur/tool_communication.h

@@ -31,7 +31,13 @@
 #define UR_CLIENT_LIBRARY_UR_TOOL_COMMUNICATION_H_INCLUDED
 
 #include "ur_client_library/types.h"
+#include <algorithm>
+#include <array>
+#include <iterator>
+#include <optional>
 #include <set>
+#include <string>
+#include <string_view>
 
 namespace urcl
 {
@@ -42,9 +48,65 @@ enum class ToolVoltage : int
 {
   OFF = 0,    ///< 0V
   _12V = 12,  ///< 12V
-  _24V = 24   ///< 24V
+  _24V = 24,  ///< 24V
+  _48V = 48   ///< 48V
 };
 
+/*!
+ * \brief Voltages the standard tool connector (T0) accepts.
+ *
+ * Connector T0 does not support 48 V.
+ */
+inline constexpr std::array<ToolVoltage, 3> T0_TOOL_VOLTAGES{ ToolVoltage::OFF, ToolVoltage::_12V, ToolVoltage::_24V };
+
+/*!
+ * \brief Voltages connector T2 accepts.
+ */
+inline constexpr std::array<ToolVoltage, 3> T2_TOOL_VOLTAGES{ ToolVoltage::OFF, ToolVoltage::_24V, ToolVoltage::_48V };
+
+/*!
+ * \brief Decimal voltage of a \ref ToolVoltage, including values outside the enum.
+ */
+inline std::string toString(const ToolVoltage voltage)
+{
+  return std::to_string(toUnderlying(voltage));
+}
+
+/*!
+ * \brief True when \p voltage is one of \p allowed.
+ */
+template <typename Range>
+bool isAllowedToolVoltage(const ToolVoltage voltage, const Range& allowed)
+{
+  return std::any_of(std::begin(allowed), std::end(allowed),
+                     [voltage](const ToolVoltage candidate) { return candidate == voltage; });
+}
+
+/*!
+ * \brief Rejection text for a voltage that is not in \p allowed.
+ *
+ * The allowed values are rendered with \ref toString, so the message stays in step with the list.
+ */
+template <typename Range>
+std::string toolVoltageNotAllowedMessage(const std::string_view connector, const ToolVoltage voltage,
+                                         const Range& allowed)
+{
+  std::string allowed_text;
+  const std::size_t count = static_cast<std::size_t>(std::distance(std::begin(allowed), std::end(allowed)));
+  std::size_t index = 0;
+  for (const ToolVoltage candidate : allowed)
+  {
+    if (index > 0)
+    {
+      allowed_text += (index + 1 == count) ? " or " : ", ";
+    }
+    allowed_text += toString(candidate);
+    ++index;
+  }
+  return "The " + std::string(connector) + " voltage should be " + allowed_text + ". The voltage is " +
+         toString(voltage) + ".";
+}
+
 /*!
  * \brief Possible values for th parity flag
  */
@@ -127,13 +189,12 @@ class ToolCommSetup
   using TxIdleCharsT = Limited<float>;
 
   /*!
-   * \brief Setup the tool voltage that will be configured on the robot. This will not immediately
+   * \brief Setup the tool voltage on connector T0 that will be configured on the robot. This will not immediately
    * change values on the robot, it will only be stored inside the ToolCommSetup object.
+   *
+   * \param tool_voltage must be one of \ref T0_TOOL_VOLTAGES or an exception will be thrown.
    */
-  void setToolVoltage(const ToolVoltage tool_voltage)
-  {
-    tool_voltage_ = tool_voltage;
-  }
+  void setToolVoltage(const ToolVoltage tool_voltage);
 
   /*!
    * \brief Return the tool voltage currently stored
@@ -143,6 +204,25 @@ class ToolCommSetup
     return tool_voltage_;
   }
 
+  /*!
+   * \brief Setup the tool voltage on connector T2 that will be configured on the robot. This will not immediately
+   * change values on the robot, it will only be stored inside the ToolCommSetup object.
+   *
+   * Connector T2 requires PolyScope X 10.15.0 or later and a robot whose RTDE robot properties report Tool Flange
+   * V2. If it is set and the robot does not report Tool Flange V2, the UrDriver constructor will throw.
+   *
+   * \param tool_voltage must be one of \ref T2_TOOL_VOLTAGES or an exception will be thrown.
+   */
+  void setToolVoltageT2(const ToolVoltage tool_voltage);
+
+  /*!
+   * \brief Return the tool voltage on connector T2 currently stored, or no value if it has not been set.
+   */
+  std::optional<ToolVoltage> getToolVoltageT2() const
+  {
+    return tool_voltage_t2_;
+  }
+
   /*!
    * \brief Setup the tool communication parity that will be configured on the robot. This will not immediately
    * change values on the robot, it will only be stored inside the ToolCommSetup object.
@@ -239,6 +319,7 @@ class ToolCommSetup
                                                 static_cast<uint32_t>(5e6) };
 
   ToolVoltage tool_voltage_;
+  std::optional<ToolVoltage> tool_voltage_t2_;
   Parity parity_;
   uint32_t baud_rate_;
   StopBitsT stop_bits_;

include/ur_client_library/ur/ur_driver.h

@@ -677,15 +677,30 @@ class UrDriver
   bool setGravity(const vector3d_t& gravity);
 
   /*!
-   * \brief Set the tool voltage. Note: It requires the external control script to be running or the robot to be in
-   * headless mode.
+   * \brief Set the tool voltage on connector T0. Note: It requires the external control script to be running or the
+   * robot to be in headless mode.
    *
-   * \param voltage tool voltage.
+   * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES.
    *
    * \returns True on successful write.
    */
   bool setToolVoltage(const ToolVoltage voltage);
 
+  /*!
+   * \brief Set the tool voltage on connector T2 (Tool Flange V2).
+   *
+   * Requires PolyScope X 10.15.0 or later and a robot whose RTDE robot properties report Tool Flange V2. PolyScope 5
+   * does not support connector T2.
+   * Note: It requires the external control script to be running or the robot to be in remote_control / headless mode.
+   * State can be read back via RTDE output fields tool_output_voltage_2 and tool_output_current_2.
+   *
+   * \param voltage Tool voltage. Must be one of \ref T2_TOOL_VOLTAGES.
+   *
+   * \returns True on successful write, false if voltage is invalid, the RTDE robot properties do not report Tool
+   * Flange V2, or the write fails.
+   */
+  bool setToolVoltageT2(const ToolVoltage voltage);
+
   /*!
    * \brief Start the robot to be controlled in force mode.
    *

resources/external_control.urscript

@@ -79,6 +79,7 @@ SET_GRAVITY = 9
 SET_TCP_OFFSET = 10
 SET_FRICTION_SCALES = 11
 SET_TARGET_PAYLOAD = 12
+SET_TOOL_T2_VOLTAGE = 13
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -1011,6 +1012,13 @@ thread script_commands():
       elif command == SET_TCP_OFFSET:
         tcp_offset = p[raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate]
         set_tcp(tcp_offset)
+      elif command == SET_TOOL_T2_VOLTAGE:
+        tool_voltage_t2 = raw_command[2] / MULT_jointstate
+        {% if ROBOT_SOFTWARE_VERSION >= v10.15.0 %}
+        set_power_output("T2_V", tool_voltage_t2)
+        {% else %}
+        textmsg("Tool voltage T2 requires PolyScope X 10.15.0 or later. Ignored.")
+        {% endif %}
       end
     end
   end