Add ft_rtde_input_enable to ScriptCommandInterface

Author: urfeex

doc/architecture/script_command_interface.rst

@@ -21,6 +21,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun
   <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.
+- ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing.
 
 Communication protocol
 ----------------------
@@ -50,6 +51,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
            - 5: startToolContact
            - 6: endToolContact
            - 7: setFrictionCompensation
+           - 8: ftRtdeInputEnable
    1-27   data fields specific to the command
    =====  =====
 
@@ -132,11 +134,25 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr
    1      friction_compensation_enabled enable/disable friction compensation for torque command.
    =====  =====
 
+.. table:: With ftRtdeInputEnable command. See script manual for details.
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   1      ft_rtde_input_enabled enable/disable FT RTDE input processing.
+   2      sensor_mass in kg (floating point)
+   3-5    sensor_mesurement_offset in m, displacement from the tool flange (3d floating point)
+   6-9    sensor_cog in m, displacement from the tool flange (3d 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,
-   so the actual data starts at index 1. The indices in the table above are shifted by one when
-   accessing the result array of the URScript function.
+   so the actual data starts at index 1. The first data entry is always the command type, hence the
+   indices in the table above are shifted by one when accessing the result array of the URScript
+   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

examples/external_fts_through_rtde.cpp

@@ -31,6 +31,7 @@
 #include <cstdio>
 #include <mutex>
 
+#include "ur_client_library/control/script_command_interface.h"
 #include "ur_client_library/example_robot_wrapper.h"
 #include "ur_client_library/types.h"
 
@@ -61,7 +62,7 @@ char getChar()
 const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
 const std::string SCRIPT_FILE = "resources/external_control.urscript";
 const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
-const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
+const std::string INPUT_RECIPE = "examples/resources/rtde_input_only_ft.txt";
 
 std::unique_ptr<urcl::ExampleRobotWrapper> g_my_robot;
 std::atomic<bool> g_RUNNING = true;
@@ -72,7 +73,8 @@ using namespace urcl;
 
 void ftInputTui()
 {
-  const std::string instructions = "Press x,y,z to increase the respective translational axes, a,b,c to increase the rotational axes, 0 for reset and q for quit.";
+  const std::string instructions = "Press x,y,z to increase the respective translational axes, a,b,c to increase the "
+                                   "rotational axes, 0 for reset and q for quit.";
   urcl::vector6d_t local_ft_vec = g_FT_VEC;
   while (g_RUNNING)
   {
@@ -235,8 +237,7 @@ int main(int argc, char* argv[])
   }
 
   // Enable using the force-torque input through RTDE in the robot controller
-  g_my_robot->getPrimaryClient()->sendScript("ft_rtde_input_enable(True)");
-
+  g_my_robot->getUrDriver()->ftRtdeInputEnable(true);
   // The RTDE thread sends the force-torque data to the robot and receives the wrench data from the
   // robot.
   std::thread rtde_thread(rtdeWorker, second_to_run);

examples/resources/rtde_input_only_ft.txt

@@ -0,0 +1 @@
+external_force_torque

examples/resources/rtde_input_recipe.txt

@@ -10,4 +10,3 @@ standard_analog_output_mask
 standard_analog_output_type
 standard_analog_output_0
 standard_analog_output_1
-external_force_torque

include/ur_client_library/control/script_command_interface.h

@@ -168,6 +168,26 @@ class ScriptCommandInterface : public ReverseInterface
    */
   bool setFrictionCompensation(const bool friction_compensation_enabled);
 
+  /*!
+   * \brief Enable or disable RTDE input for the force torque sensor.
+   *
+   * When enabled, the force torque sensor values will be read from the RTDE input registers instead of the actual
+   * sensor. This can be used to connect an external force torque sensor to the robot or to
+   * simulate a force torque sensor when using URSim.
+   *
+   * \param enabled Whether to enable or disable RTDE input for the force torque sensor.
+   * \param sensor_mass Mass of the force torque sensor in kilograms.
+   * \param sensor_measuring_offset The offset of the force torque sensor in meters, a vector [x, y, z] specifying the
+   * displacement from the tool flange frame.
+   * \param sensor_cog The center of gravity of the force torque sensor in meters, a vector [x, y, z] specifying the
+   * displacement from the tool flange frame.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool ftRtdeInputEnable(const bool enabled, const double sensor_mass = 0.0,
+                         const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 },
+                         const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 });
+
   /*!
    * \brief  Returns whether a client/robot is connected to this server.
    *
@@ -206,6 +226,7 @@ class ScriptCommandInterface : public ReverseInterface
     START_TOOL_CONTACT = 5,         ///< Start detecting tool contact
     END_TOOL_CONTACT = 6,           ///< End detecting tool contact
     SET_FRICTION_COMPENSATION = 7,  ///< Set friction compensation
+    FT_RTDE_INPUT_ENABLE = 8,       ///< Enable FT RTDE input
   };
 
   /*!

include/ur_client_library/ur/ur_driver.h

@@ -726,6 +726,26 @@ class UrDriver
    */
   bool setFrictionCompensation(const bool friction_compensation_enabled);
 
+  /*!
+   * \brief Enable or disable RTDE input for the force torque sensor.
+   *
+   * When enabled, the force torque sensor values will be read from the RTDE input registers instead of the actual
+   * sensor. This can be used to connect an external force torque sensor to the robot or to
+   * simulate a force torque sensor when using URSim.
+   *
+   * \param enabled Whether to enable or disable RTDE input for the force torque sensor.
+   * \param sensor_mass Mass of the force torque sensor in kilograms.
+   * \param sensor_measuring_offset The offset of the force torque sensor in meters, a vector [x, y, z] specifying the
+   * displacement from the tool flange frame.
+   * \param sensor_cog The center of gravity of the force torque sensor in meters, a vector [x, y, z] specifying the
+   * displacement from the tool flange frame.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool ftRtdeInputEnable(const bool enabled, const double sensor_mass = 0.0,
+                         const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 },
+                         const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 });
+
   /*!
    * \brief Write a keepalive signal only.
    *

resources/external_control.urscript

@@ -55,6 +55,7 @@ END_FORCE_MODE = 4
 START_TOOL_CONTACT = 5
 END_TOOL_CONTACT = 6
 SET_FRICTION_COMPENSATION = 7
+FT_RTDE_INPUT_ENABLE = 8
 SCRIPT_COMMAND_DATA_DIMENSION = 28
 
 FREEDRIVE_MODE_START = 1
@@ -788,6 +789,21 @@ thread script_commands():
         else:
           friction_compensation_enabled = True
         end
+      elif command == FT_RTDE_INPUT_ENABLE:
+        if raw_command[2] == 0:
+          enabled = False
+        else:
+          enabled = True
+        end
+        sensor_mass = raw_command[3] / MULT_jointstate
+        sensor_offset = [raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate]
+        sensor_cog = [raw_command[7] / MULT_jointstate, raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate]
+        textmsg("calling ft_rtde_input_enable(",
+          str_cat(enabled, str_cat(", ",
+            str_cat(sensor_mass, str_cat(", ",
+              str_cat(to_str(sensor_offset), str_cat(", ",
+                str_cat(to_str(sensor_cog), ")"))))))))
+        ft_rtde_input_enable(enabled, sensor_mass, sensor_offset, sensor_cog)
       end
     end
   end
@@ -917,4 +933,4 @@ socket_close("reverse_socket")
 socket_close("trajectory_socket")
 socket_close("script_command_socket")
 
-# NODE_CONTROL_LOOP_ENDS
+# NODE_CONTROL_LOOP_ENDS

src/control/script_command_interface.cpp

@@ -254,6 +254,45 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double sensor_mass,
+                                               const vector3d_t& sensor_measuring_offset, const vector3d_t& sensor_cog)
+{
+  const int message_length = 9;
+  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
+  uint8_t* b_pos = buffer;
+
+  int32_t val = htobe32(toUnderlying(ScriptCommand::FT_RTDE_INPUT_ENABLE));
+  b_pos += append(b_pos, val);
+
+  val = htobe32(enabled);
+  b_pos += append(b_pos, val);
+
+  val = htobe32(static_cast<int32_t>(round(sensor_mass * MULT_JOINTSTATE)));
+  b_pos += append(b_pos, val);
+
+  for (auto const& mass_component : sensor_measuring_offset)
+  {
+    val = htobe32(static_cast<int32_t>(round(mass_component * MULT_JOINTSTATE)));
+    b_pos += append(b_pos, val);
+  }
+
+  for (auto const& cog_component : sensor_cog)
+  {
+    val = htobe32(static_cast<int32_t>(round(cog_component * 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_;
@@ -321,4 +360,4 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo
 }
 
 }  // namespace control
-}  // namespace urcl
+}  // namespace urcl

src/ur/ur_driver.cpp

@@ -539,6 +539,20 @@ bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled)
   }
 }
 
+bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass,
+                                 const vector3d_t& sensor_measuring_offset, const vector3d_t& sensor_cog)
+{
+  if (script_command_interface_->clientConnected())
+  {
+    return script_command_interface_->ftRtdeInputEnable(enabled, sensor_mass, sensor_measuring_offset, sensor_cog);
+  }
+  else
+  {
+    URCL_LOG_ERROR("Script command interface is not running. Unable to set ft_rtde_input_enable.");
+    return 0;
+  }
+}
+
 bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout)
 {
   vector6d_t* fake = nullptr;

tests/test_script_command_interface.cpp

@@ -426,9 +426,57 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation)
   EXPECT_EQ(message_sum, expected_message_sum);
 }
 
+TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable)
+{
+  // Wait for the client to connect to the server
+  waitForClientConnection();
+
+  double sensor_mass = 1.42;
+  vector3d_t sensor_measuring_offset = { 0.1, 0.2, 0.3 };
+  vector3d_t sensor_cog = { 0.01, 0.02, 0.03 };
+  script_command_interface_->ftRtdeInputEnable(true, sensor_mass, sensor_measuring_offset, sensor_cog);
+
+  int32_t command;
+  std::vector<int32_t> message;
+  client_->readMessage(command, message);
+
+  // 8 is ft rtde input enable
+  int32_t expected_command = 8;
+  EXPECT_EQ(command, expected_command);
+
+  // Test enabled
+  bool received_enabled = static_cast<bool>(message[0]);
+
+  // Test sensor mass
+  double received_sensor_mass = static_cast<double>(message[1]) / script_command_interface_->MULT_JOINTSTATE;
+  EXPECT_EQ(received_sensor_mass, sensor_mass);
+
+  // Test sensor measuring offset
+  vector3d_t received_sensor_measuring_offset;
+  for (unsigned int i = 0; i < sensor_measuring_offset.size(); ++i)
+  {
+    received_sensor_measuring_offset[i] =
+        static_cast<double>(message[2 + i]) / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_sensor_measuring_offset[i], sensor_measuring_offset[i]);
+  }
+
+  // Test sensor cog
+  vector3d_t received_sensor_cog;
+  for (unsigned int i = 0; i < sensor_cog.size(); ++i)
+  {
+    received_sensor_cog[i] = static_cast<double>(message[5 + i]) / script_command_interface_->MULT_JOINTSTATE;
+    EXPECT_EQ(received_sensor_cog[i], sensor_cog[i]);
+  }
+
+  // The rest of the message should be zero
+  int32_t message_sum = std::accumulate(std::begin(message) + 8, 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);
 
   return RUN_ALL_TESTS();
-}
+}