Add set_target_payload support to ScriptCommandInterface (UniversalRobots#515)

Adds support for the URScript `set_target_payload()` command through the
`ScriptCommandInterface`, allowing users to set the payload mass, center
of gravity, inertia matrix, and transition time.

Author: srvald

doc/architecture/script_command_interface.rst

@@ -25,6 +25,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
 - ``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.
+- ``setTargetPayload()``: Set the active payload mass, center of gravity, inertia matrix and transition time.
 
 Communication protocol
 ----------------------
@@ -58,6 +59,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 9: setGravity
            - 10: setTcpOffset
            - 11: setFrictionScales
+           - 12: setTargetPayload
    1-27   data fields specific to the command
    =====  =====
 
@@ -77,7 +79,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    index  meaning
    =====  =====
    1      Payload mass in kg (floating point)
-   2-4    Payload center of gravity in m, displacement from the toolmpount (floating point)
+   2-4    Payload center of gravity in m, displacement from the tool mount (floating point)
    =====  =====
 
 .. table:: With setToolVoltage command
@@ -180,6 +182,18 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    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)
    =====  =====
 
+.. table:: With setTargetPayload command
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1      Payload mass in kg (floating point)
+   2-4    Payload center of gravity in m, displacement from the tool mount (floating point)
+   5-10   Payload inertia matrix [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] in kg·m² (floating point)
+   11     Payload transition time in seconds (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,
@@ -188,9 +202,9 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    function. E.g. reading the boolean value for friction compensation in the
    ``setFrictionCompensation`` command would be done by accessing index 2 of the result array.
 
-   All floating point data is encoded into an integer representation and has to be divided by the
-   ``MULT_JOINTSTATE`` constant to get the actual floating point value. This constant is defined in
-   ``ReverseInterface`` class.
+   All floating point data is encoded into an integer representation and has to be divided by either the
+   ``MULT_JOINTSTATE`` or ``MULT_TIME`` constant to get the actual floating point value. These constants
+   are defined in the ``ReverseInterface`` class.
 
 Data sent from the robot
 ^^^^^^^^^^^^^^^^^^^^^^^^

examples/script_command_interface.cpp

@@ -80,6 +80,9 @@ void sendScriptCommands()
     run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); });
     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 }); });
+    run_cmd("Setting payload to 2.0kg with custom inertia and 0.5s transition time", []() {
+      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_friction_scales)
     {

include/ur_client_library/control/reverse_interface.h

@@ -82,6 +82,7 @@ class ReverseInterface
 {
 public:
   static const int32_t MULT_JOINTSTATE = 1000000;
+  static const int32_t MULT_TIME = 1000000;
 
   ReverseInterface() = delete;
   /*!

include/ur_client_library/control/script_command_interface.h

@@ -91,6 +91,20 @@ class ScriptCommandInterface : public ReverseInterface
    */
   bool setPayload(const double mass, const vector3d_t* cog);
 
+  /*!
+   * \brief Set the target payload mass, center of gravity and inertia matrix
+   *
+   * \param mass mass in kilograms
+   * \param cog  Center of Gravity, a vector [CoGx, CoGy, CoGz] specifying the displacement (in meters) from the
+   * toolmount
+   * \param inertia Inertia matrix elements [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]
+   * \param transition_time Duration of the payload property changes in seconds
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setTargetPayload(const double mass, const vector3d_t* cog, const vector6d_t* inertia,
+                        const double transition_time = 0.0);
+
   /*!
    * \brief Set the gravity vector
    *
@@ -264,6 +278,7 @@ class ScriptCommandInterface : public ReverseInterface
     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
+    SET_TARGET_PAYLOAD = 12,        ///< Set target payload
   };
 
   /*!

include/ur_client_library/control/trajectory_point_interface.h

@@ -63,7 +63,6 @@ std::string trajectoryResultToString(const TrajectoryResult result);
 class TrajectoryPointInterface : public ReverseInterface
 {
 public:
-  static const int32_t MULT_TIME = 1000000;
   static const int MESSAGE_LENGTH = 21;
 
   TrajectoryPointInterface() = delete;

include/ur_client_library/ur/ur_driver.h

@@ -631,6 +631,21 @@ class UrDriver
    */
   bool setPayload(const float mass, const vector3d_t& cog);
 
+  /*!
+   * \brief Set the target payload mass, center of gravity and inertia matrix
+   *
+   * \param mass mass in kilograms
+   * \param cog  Center of Gravity, a vector [CoGx, CoGy, CoGz] specifying the displacement (in meters) from the
+   * toolmount
+   * \param inertia Inertia matrix elements [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]
+   * \param transition_time Duration of the payload property changes in seconds
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setTargetPayload(const float mass, const vector3d_t& cog,
+                        const vector6d_t& inertia = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+                        const double transition_time = 0.0);
+
   /*!
    * \brief Set the gravity vector. Note: It requires the external control script to be running or
    * the robot to be in headless mode.

resources/external_control.urscript

@@ -67,6 +67,7 @@ FT_RTDE_INPUT_ENABLE = 8
 SET_GRAVITY = 9
 SET_TCP_OFFSET = 10
 SET_FRICTION_SCALES = 11
+SET_TARGET_PAYLOAD = 12
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -938,6 +939,17 @@ thread script_commands():
         friction_compensation_mode = FRICTION_COMP_MODE_FRICTION_SCALES
         viscous_scale = [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]
         coulomb_scale = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate]
+      elif command == SET_TARGET_PAYLOAD:  
+        mass = raw_command[2] / MULT_jointstate  
+        cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate]  
+        inertia = [raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate, raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate]
+        transition_time = raw_command[12] / MULT_time  
+        {% if ROBOT_SOFTWARE_VERSION >= v5.10.0 %}
+          set_target_payload(mass, cog, inertia, transition_time)
+        {% else %}
+          set_payload(mass, cog)
+          textmsg("PolyScope < 5.10.0. Inertia and transition_time ignored.")
+        {% endif %}
       elif command == FT_RTDE_INPUT_ENABLE:
         if raw_command[2] == 0:
           enabled = False

src/control/script_command_interface.cpp

@@ -91,6 +91,44 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::setTargetPayload(const double mass, const vector3d_t* cog, const vector6d_t* inertia,
+                                              const double transition_time)
+{
+  const int message_length = 12;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_TARGET_PAYLOAD));
+  b_pos += append(b_pos, val);
+
+  val = htobe32(static_cast<int32_t>(round(mass * MULT_JOINTSTATE)));
+  b_pos += append(b_pos, val);
+
+  for (auto const& center_of_mass : *cog)
+  {
+    val = htobe32(static_cast<int32_t>(round(center_of_mass * MULT_JOINTSTATE)));
+    b_pos += append(b_pos, val);
+  }
+
+  for (auto const& inertia_val : *inertia)
+  {
+    val = htobe32(static_cast<int32_t>(round(inertia_val * MULT_JOINTSTATE)));
+    b_pos += append(b_pos, val);
+  }
+
+  val = htobe32(static_cast<int32_t>(round(transition_time * MULT_TIME)));
+  b_pos += append(b_pos, val);
+
+  // writing zeros to allow usage with other script commands
+  for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++)
+  {
+    val = htobe32(0);
+    b_pos += append(b_pos, val);
+  }
+  size_t written;
+
+  return server_.write(client_fd_, buffer, sizeof(buffer), written);
+}
+
 bool ScriptCommandInterface::setGravity(const vector3d_t* gravity)
 {
   const int message_length = 4;

src/ur/ur_driver.cpp

@@ -313,6 +313,41 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog)
   }
 }
 
+bool UrDriver::setTargetPayload(const float mass, const vector3d_t& cog, const vector6d_t& inertia,
+                                const double transition_time)
+{
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setTargetPayload(mass, &cog, &inertia, transition_time);
+  }
+  else
+  {
+    URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series "
+                  "robots this will only work, if the robot is in remote_control mode.");
+
+    std::stringstream cmd;
+    cmd.imbue(std::locale::classic());  // Make sure, decimal divider is actually '.'
+
+    if ((getVersion().major > 5) || (getVersion().major == 5 && getVersion().minor >= 10))
+    {
+      cmd << "sec setup():" << std::endl
+          << " set_target_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "] , ["
+          << inertia[0] << ", " << inertia[1] << ", " << inertia[2] << ", " << inertia[3] << ", " << inertia[4] << ", "
+          << inertia[5] << "] , " << transition_time << ")" << std::endl
+          << "end";
+    }
+    else
+    {
+      cmd << "sec setup():" << std::endl
+          << " set_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "])" << std::endl
+          << " textmsg(\"PolyScope < 5.10.0. Inertia and transition_time ignored.\")" << std::endl
+          << "end";
+    }
+
+    return sendScript(cmd.str());
+  }
+}
+
 bool UrDriver::setGravity(const vector3d_t& gravity)
 {
   if (script_command_interface_->clientConnected())

tests/test_script_command_interface.cpp

@@ -225,6 +225,54 @@ TEST_F(ScriptCommandInterfaceTest, test_set_payload)
   EXPECT_EQ(message_sum, expected_message_sum);
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_set_target_payload)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  double mass = 1.0;
+  vector3d_t cog = { 0.2, 0.3, 0.1 };
+  vector6d_t inertia = { 0.4, 0.7, 0.8, 0.2, 0.5, 0.6 };
+  double transition_time = 0.002;
+  script_command_interface_->setTargetPayload(mass, &cog, &inertia, transition_time);
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 12 is set target payload
+  int32_t expected_command = 12;
+  EXPECT_EQ(command, expected_command);
+
+  // Test mass
+  double received_mass = (double)message[0] / script_command_interface_->MULT_JOINTSTATE;
+  EXPECT_EQ(received_mass, mass);
+
+  // Test cog
+  vector3d_t received_cog;
+  for (unsigned int i = 0; i < cog.size(); ++i)
+  {
+    received_cog[i] = (double)message[i + 1] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_cog[i], cog[i]);
+  }
+
+  // Test inertia
+  vector6d_t received_inertia;
+  for (unsigned int i = 0; i < inertia.size(); ++i)
+  {
+    received_inertia[i] = (double)message[i + 4] / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_inertia[i], inertia[i]);
+  }
+
+  // Test transition time
+  double received_transition_time = (double)message[10] / script_command_interface_->MULT_TIME;
+  EXPECT_EQ(received_transition_time, transition_time);
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + 11, std::end(message), 0);
+  int32_t expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+}
+
 TEST_F(ScriptCommandInterfaceTest, test_set_tool_voltage)
 {
   // Wait for the client to connect to the server