Added ScriptCommandInterface command setTcpOffset (#444)

simply calls the urscript command set_tcp()

---------

Co-authored-by: david.karlsboeck <david.karlsboeck@gmail.com>

Author: dkar-sto

include/ur_client_library/control/script_command_interface.h

@@ -167,6 +167,15 @@ class ScriptCommandInterface : public ReverseInterface
    */
   bool endToolContact();
 
+  /*!
+   * \brief Set the TCP offset
+   *
+   * \param offset TCP offset as [x, y, z, rx, ry, rz] given in the flange coordinate system.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setTcpOffset(const vector6d_t& offset);
+
   /*!
    * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
    * if false it will not.
@@ -238,6 +247,7 @@ class ScriptCommandInterface : public ReverseInterface
     SET_FRICTION_COMPENSATION = 7,  ///< Set friction compensation
     FT_RTDE_INPUT_ENABLE = 8,       ///< Enable FT RTDE input
     SET_GRAVITY = 9,                ///< Set gravity vector
+    SET_TCP_OFFSET = 10,            ///< Set TCP offset
   };
 
   /*!
@@ -259,7 +269,7 @@ class ScriptCommandInterface : public ReverseInterface
   bool robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5,
                                          const VersionInformation& min_polyscopeX, const std::string& command_name);
 
-  bool client_connected_;
+  std::atomic<bool> client_connected_;
   static const int MAX_MESSAGE_LENGTH = 28;
 
   std::function<void(ToolContactResult)> handle_tool_contact_result_;

include/ur_client_library/ur/ur_driver.h

@@ -763,6 +763,13 @@ class UrDriver
    */
   bool endToolContact();
 
+  /*!
+   * \brief Set the TCP offset of the robot.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool setTcpOffset(const vector6d_t& tcp_offset);
+
   /*!
    * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction,
    * if false it will not.

resources/external_control.urscript

@@ -57,6 +57,7 @@ END_TOOL_CONTACT = 6
 SET_FRICTION_COMPENSATION = 7
 FT_RTDE_INPUT_ENABLE = 8
 SET_GRAVITY = 9
+SET_TCP_OFFSET = 10
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -822,6 +823,9 @@ thread script_commands():
         # This has a known error that the resulting torques are computed with opposite sign.
         enable_external_ft_sensor(enabled, sensor_mass, sensor_offset, sensor_cog)
         {% endif %}
+      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)
       end
     end
   end

src/control/script_command_interface.cpp

@@ -318,6 +318,32 @@ bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::setTcpOffset(const vector6d_t& offset)
+{
+  const int message_length = 7;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+
+  int32_t val = htobe32(toUnderlying(ScriptCommand::SET_TCP_OFFSET));
+  b_pos += append(b_pos, val);
+
+  for (auto const& frame : offset)
+  {
+    val = htobe32(static_cast<int32_t>(round(frame * 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::clientConnected()
 {
   return client_connected_;

src/ur/ur_driver.cpp

@@ -591,6 +591,19 @@ bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass,
   }
 }
 
+bool UrDriver::setTcpOffset(const vector6d_t& tcp_offset)
+{
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->setTcpOffset(tcp_offset);
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script command interface is not running. Unable to set TCP offset.");
+    return false;
+  }
+}
+
 bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout)
 {
   vector6d_t* fake = nullptr;

tests/test_script_command_interface.cpp

@@ -511,6 +511,36 @@ TEST_F(ScriptCommandInterfaceTest, test_set_gravity)
   EXPECT_EQ(message_sum, expected_message_sum);
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_set_tcp_offset)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  vector6d_t tcp_offset = { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 };
+  script_command_interface_->setTcpOffset(tcp_offset);
+
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 10 is set tcp offset
+  int32_t expected_command = 10;
+  EXPECT_EQ(command, expected_command);
+
+  // Test tcp offset
+  vector6d_t received_tcp_offset;
+  for (unsigned int i = 0; i < tcp_offset.size(); ++i)
+  {
+    received_tcp_offset[i] = static_cast<double>(message[i]) / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_tcp_offset[i], tcp_offset[i]);
+  }
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + 6, std::end(message), 0);
+  int32_t expected_message_sum = 0;
+  EXPECT_EQ(message_sum, expected_message_sum);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

tests/test_ur_driver.cpp

@@ -34,6 +34,7 @@
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_client_library/example_robot_wrapper.h>
 #include <algorithm>
+#include <thread>
 #include "test_utils.h"
 
 using namespace urcl;
@@ -331,6 +332,46 @@ TEST_F(UrDriverTest, read_error_code)
   EXPECT_NO_THROW(g_my_robot->getPrimaryClient()->commandUnlockProtectiveStop());
 }
 
+TEST_F(UrDriverTest, set_tcp_offset)
+{
+  ASSERT_TRUE(g_my_robot->getUrDriver()->setTcpOffset({ 0, 0, 0, 0, 0, 0 }));
+
+  vector6d_t tcp_offset = { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 };
+  ASSERT_TRUE(g_my_robot->getUrDriver()->setTcpOffset(tcp_offset));
+
+  rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+  vector6d_t tcp_offset_received;
+  for (int i = 0; i < 10; ++i)
+  {
+    ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg));
+
+    data_pkg.getData("tcp_offset", tcp_offset_received);
+    if (tcp_offset_received == tcp_offset)
+    {
+      break;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+  }
+  g_my_robot->getUrDriver()->getDataPackage(data_pkg);
+  for (unsigned int i = 0; i < tcp_offset.size(); ++i)
+  {
+    EXPECT_DOUBLE_EQ(tcp_offset_received[i], tcp_offset[i]);
+  }
+
+  // Stop program on robot
+  g_my_robot->getUrDriver()->stopControl();
+  g_my_robot->waitForProgramNotRunning(1000);
+
+  // TODO (feex): We cannot see from the outside whether the script command interface is connected. There is a race
+  // condition between the reverse interface disconnection callback and the script_command_interface disconnect
+  // callback. For now, we will just have to wait a bit to ensure that the script command interface
+  // is disconnected before trying to use the script command interface.
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+  // Check that we can't set TCP offset when the program isn't running
+  ASSERT_FALSE(g_my_robot->getUrDriver()->setTcpOffset(tcp_offset));
+}
+
 TEST(UrDriverInitTest, setting_connection_limits_works_correctly)
 {
   UrDriverConfiguration config;