Add set_target_payload support to ScriptCommandInterface

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/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_jointstate  
+        {% 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_JOINTSTATE)));
+  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_JOINTSTATE;
+  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

tests/test_ur_driver.cpp

@@ -374,6 +374,34 @@ TEST_F(UrDriverTest, set_tcp_offset)
   ASSERT_FALSE(g_my_robot->getUrDriver()->setTcpOffset(tcp_offset));
 }
 
+TEST_F(UrDriverTest, set_target_payload_fallback_script)
+{
+  g_my_robot->getUrDriver()->stopControl();
+  ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000));
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  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;
+
+  bool result = g_my_robot->getUrDriver()->setTargetPayload(mass, cog, inertia, transition_time);
+
+  EXPECT_TRUE(result);
+}
+
+TEST_F(UrDriverTest, set_target_payload)
+{
+  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;
+
+  bool result = g_my_robot->getUrDriver()->setTargetPayload(mass, cog, inertia, transition_time);
+
+  EXPECT_TRUE(result);
+}
+
 TEST(UrDriverInitTest, setting_connection_limits_works_correctly)
 {
   UrDriverConfiguration config;