From d5e7d18628d416c4fe6674f6ac8dde5856ff8a41 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 8 Oct 2025 14:21:24 +0200 Subject: [PATCH 1/7] Add serializing for missing types --- .../comm/package_serializer.h | 42 +++++ tests/test_package_serializer.cpp | 166 +++++++++++++++++- 2 files changed, 206 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/comm/package_serializer.h b/include/ur_client_library/comm/package_serializer.h index 7745da9da..3cd897690 100644 --- a/include/ur_client_library/comm/package_serializer.h +++ b/include/ur_client_library/comm/package_serializer.h @@ -30,7 +30,11 @@ #define UR_CLIENT_LIBRARY_PACKAGE_SERIALIZER_H_INCLUDED #include +#include #include +#include + +#include "ur_client_library/types.h" namespace urcl { @@ -71,6 +75,7 @@ class PackageSerializer */ static size_t serialize(uint8_t* buffer, double val) { + static_assert(sizeof(double) == sizeof(uint64_t), "Unexpected double size"); size_t size = sizeof(double); uint64_t inner; std::memcpy(&inner, &val, size); @@ -79,6 +84,25 @@ class PackageSerializer return size; } + /*! + * \brief A serialization method for float values. + * + * \param buffer The buffer to write the serialization into. + * \param val The value to serialize. + * + * \returns Size in byte of the serialization. + */ + static size_t serialize(uint8_t* buffer, float val) + { + static_assert(sizeof(float) == sizeof(uint32_t), "Unexpected float size"); + size_t size = sizeof(float); + uint32_t inner; + std::memcpy(&inner, &val, size); + inner = encode(inner); + std::memcpy(buffer, &inner, size); + return size; + } + /*! * \brief A serialization method for strings. * @@ -98,6 +122,24 @@ class PackageSerializer return val.size(); } + /*! + * \brief A serialization method for std::arrays (works for urcl::vector6d_t, etc). + * + * \param buffer The buffer to write the serialization into. + * \param val The string to serialize. + * + * \returns Size in byte of the serialization. + */ + template + static size_t serialize(uint8_t* buffer, const std::array& val) + { + for (size_t i = 0; i < N; ++i) + { + serialize(buffer + i * sizeof(T), val[i]); + } + return N * sizeof(T); + } + private: template static T encode(T val) diff --git a/tests/test_package_serializer.cpp b/tests/test_package_serializer.cpp index 5542e9bc0..779712790 100644 --- a/tests/test_package_serializer.cpp +++ b/tests/test_package_serializer.cpp @@ -69,7 +69,26 @@ TEST(package_serializer, serialize_int32) { uint8_t buffer[sizeof(int32_t)]; size_t expected_size = sizeof(int32_t); - size_t actual_size = comm::PackageSerializer::serialize(buffer, 2341); + size_t actual_size = comm::PackageSerializer::serialize(buffer, -2341); + + EXPECT_EQ(expected_size, actual_size); + int32_t val; + std::memcpy(&val, buffer, sizeof(int32_t)); + int32_t decoded = be32toh(val); + EXPECT_EQ(decoded, -2341); + + uint8_t expected_buffer[] = { 0xFF, 0xFF, 0xF6, 0xDB }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected_buffer[i], buffer[i]); + } +} + +TEST(package_serializer, serialize_uint32) +{ + uint8_t buffer[sizeof(uint32_t)]; + size_t expected_size = sizeof(uint32_t); + size_t actual_size = comm::PackageSerializer::serialize(buffer, 2341); EXPECT_EQ(expected_size, actual_size); @@ -80,9 +99,152 @@ TEST(package_serializer, serialize_int32) } } +TEST(package_serializer, serialize_int16) +{ + uint8_t buffer[sizeof(int16_t)]; + size_t expected_size = sizeof(int16_t); + size_t actual_size = comm::PackageSerializer::serialize(buffer, -2341); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected_buffer[] = { 0xF6, 0xDB }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected_buffer[i], buffer[i]); + } +} + +TEST(package_serializer, serialize_uint16) +{ + uint8_t buffer[sizeof(uint16_t)]; + size_t expected_size = sizeof(uint16_t); + size_t actual_size = comm::PackageSerializer::serialize(buffer, 2341); + + EXPECT_EQ(expected_size, actual_size); + + uint8_t expected_buffer[] = { 0x09, 0x25 }; + for (unsigned int i = 0; i < actual_size; ++i) + { + EXPECT_EQ(expected_buffer[i], buffer[i]); + } +} + +TEST(package_serializer, serialize_vector6d) +{ + vector6d_t target{ 1.0, 3.14, 42, -2.345, 2.0, 0 }; + uint8_t buffer[sizeof(vector6d_t)]; + size_t expected_size = sizeof(vector6d_t); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + for (size_t i = 0; i < target.size(); ++i) + { + uint64_t tmp; + std::memcpy(&tmp, &buffer[i * sizeof(double)], sizeof(double)); + uint64_t decoded = be64toh(tmp); + double decoded_double; + std::memcpy(&decoded_double, &decoded, sizeof(double)); + EXPECT_DOUBLE_EQ(decoded_double, target[i]); + } +} + +TEST(package_serializer, serialize_vector3d) +{ + vector3d_t target{ 1.0, 3.14, -42 }; + uint8_t buffer[sizeof(vector3d_t)]; + size_t expected_size = sizeof(vector3d_t); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + for (size_t i = 0; i < target.size(); ++i) + { + uint64_t tmp; + std::memcpy(&tmp, &buffer[i * sizeof(double)], sizeof(double)); + uint64_t decoded = be64toh(tmp); + double decoded_double; + std::memcpy(&decoded_double, &decoded, sizeof(double)); + EXPECT_DOUBLE_EQ(decoded_double, target[i]); + } +} + +TEST(package_serializer, serialize_vector6int32) +{ + vector6int32_t target{ 1, 0, -42 }; + uint8_t buffer[sizeof(target)]; + size_t expected_size = sizeof(target); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + for (size_t i = 0; i < target.size(); ++i) + { + int32_t tmp; + std::memcpy(&tmp, &buffer[i * sizeof(int32_t)], sizeof(int32_t)); + int32_t decoded = be32toh(tmp); + EXPECT_DOUBLE_EQ(decoded, target[i]); + } +} + +TEST(package_serializer, serialize_vector6uint32) +{ + vector6uint32_t target{ 1, 0, 42 }; + uint8_t buffer[sizeof(target)]; + size_t expected_size = sizeof(target); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + for (size_t i = 0; i < target.size(); ++i) + { + uint32_t tmp; + std::memcpy(&tmp, &buffer[i * sizeof(uint32_t)], sizeof(uint32_t)); + uint32_t decoded = be32toh(tmp); + EXPECT_DOUBLE_EQ(decoded, target[i]); + } +} + +TEST(package_serializer, serialize_float) +{ + float target = -2.345f; + uint8_t buffer[sizeof(target)]; + size_t expected_size = sizeof(target); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + uint32_t tmp; + std::memcpy(&tmp, &buffer, sizeof(uint32_t)); + uint32_t decoded = be32toh(tmp); + float decoded_float; + std::memcpy(&decoded_float, &decoded, sizeof(float)); + EXPECT_DOUBLE_EQ(decoded_float, target); +} + +TEST(package_serializer, serialize_bool) +{ + bool target = false; + uint8_t buffer[sizeof(target)]; + size_t expected_size = sizeof(target); + size_t actual_size = comm::PackageSerializer::serialize(buffer, target); + + EXPECT_EQ(expected_size, actual_size); + + bool decoded_bool; + std::memcpy(&decoded_bool, &buffer, sizeof(uint8_t)); + EXPECT_DOUBLE_EQ(decoded_bool, target); + + target = false; + actual_size = comm::PackageSerializer::serialize(buffer, target); + EXPECT_EQ(expected_size, actual_size); + std::memcpy(&decoded_bool, &buffer, sizeof(uint8_t)); + EXPECT_DOUBLE_EQ(decoded_bool, target); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} +} \ No newline at end of file From 21c64b5f0741e476300b8c114cea3520446ac3e0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 8 Oct 2025 14:52:05 +0200 Subject: [PATCH 2/7] Add sendExternalForceTorque --- doc/examples.rst | 1 + doc/examples/external_fts_through_rtde.rst | 165 ++++++++++++ examples/CMakeLists.txt | 4 + examples/external_fts_through_rtde.cpp | 252 +++++++++++++++++++ examples/resources/rtde_input_recipe.txt | 1 + include/ur_client_library/comm/tcp_server.h | 4 +- include/ur_client_library/rtde/rtde_writer.h | 12 + src/rtde/rtde_writer.cpp | 14 ++ tests/test_rtde_writer.cpp | 55 ++-- 9 files changed, 488 insertions(+), 20 deletions(-) create mode 100644 doc/examples/external_fts_through_rtde.rst create mode 100644 examples/external_fts_through_rtde.cpp diff --git a/doc/examples.rst b/doc/examples.rst index ad4f6ac60..dd90634c9 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -23,6 +23,7 @@ may be running forever until manually stopped. examples/primary_pipeline examples/primary_pipeline_calibration examples/rtde_client + examples/external_fts_through_rtde examples/script_command_interface examples/script_sender examples/spline_example diff --git a/doc/examples/external_fts_through_rtde.rst b/doc/examples/external_fts_through_rtde.rst new file mode 100644 index 000000000..ce7503e93 --- /dev/null +++ b/doc/examples/external_fts_through_rtde.rst @@ -0,0 +1,165 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/external_fts_through_rtde.rst + +External FTS example +==================== + +It is possible to feed data of an external Force-Torque-Sensor (FTS) into the robot controller +through RTDE. This feature has to be explicitly enabled by calling the URScript function +`ft_rtde_input_enable +`_. + +Usually, that functionality would be used with a URCap running on the robot but data can also be +routed through an external computer with a matching driver for that FTS. Keep in mind that the +latency introduced by the network connection may have a negative impact on the performance. + +Another use case could be to inject force-torque measurements into a simulated robot for testing +purposes. + +This example allows setting force values that are sent to the robot as external ft measurements. +The terminal user interface (TUI) is pretty simple: + +- Pressing the keys ``x``, ``y``, ``z``, ``a``, ``b``, or ``c`` will increase the respective + force/torque component by 10 (N or Nm). (``a``, ``b``, and ``c`` correspond to torques around the + x, y, and z axis respectively.) +- Pressing the keys ``X``, ``Y``, ``Z``, ``A``, ``B``, or ``C`` will decrease the respective + force/torque component by 10 (N or Nm). +- Pressing ``0`` will reset all force/torque components to zero. +- Pressing ``q`` will exit the program. + +The example's source code can be found in `external_fts_through_rtde.cpp +`_. + + +.. note:: This example requires the robot to be in *remote control mode*. + +Robot initialization +-------------------- + +The robot initialization is handed off to the ``ExampleRobotWrapper`` class: + +.. literalinclude:: ../../examples/external_fts_through_rtde.cpp + :language: c++ + :caption: external_fts_through_rtde/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: g_my_robot = std::make_unique + :end-before: // Enable using the force-torque + +In order to use the RTDE input for the FTS, we have to enable it first by calling the +``ft_rtde_input_enable`` function. This is done by sending a script to the robot through the +``sendScript()`` method of the ``UrDriver`` class. + +.. literalinclude:: ../../examples/external_fts_through_rtde.cpp + :language: c++ + :caption: external_fts_through_rtde/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: // Enable using the force-torque + :end-at: g_my_robot->getPrimaryClient()->sendScript("ft_rtde_input_enable(True)"); + +RTDE communication is moved to a separate thread in order to use the main thread for handling +keyboard input commands. + +Input key handling +------------------ + +For handling the keyboard input, define a ``getch()`` function that reads a single character from +the terminal without waiting for a newline character. This has to be done differently on Windows +and Linux (and other unix-like systems), therefore the code is split using preprocessor directives. + +.. literalinclude:: ../../examples/external_fts_through_rtde.cpp + :language: c++ + :caption: external_fts_through_rtde/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: // Platform-specific implementation of getch() + :end-at: #endif + +Setting the external force-torque values +---------------------------------------- + +The result from the tui function is synchonized to the RTDE communication thread using a buffer ``g_FT_VEC`` and a mutex. + +.. literalinclude:: ../../examples/external_fts_through_rtde.cpp + :language: c++ + :caption: external_fts_through_rtde/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: std::scoped_lock lock(g_FT_VEC_MUTEX); + :end-at: g_FT_VEC = local_ft_vec; + +.. literalinclude:: ../../examples/external_fts_through_rtde.cpp + :language: c++ + :caption: external_fts_through_rtde/freedrive_example.cpp + :linenos: + :lineno-match: + :start-at: if (g_FT_VEC_MUTEX.try_lock()) + :end-at: } + +Note that the ``try_lock()`` method is used in the RTDE thread to avoid blocking the thread if the +tui thread is currently holding the lock. The new values will be available in a later cycle then. + +.. note:: + The values reported from the robot are rotated to the base coordinate system of the robot. + Hence, they will not be the same as the ones sent to the robot. Align the robot's TCP the same + as the base coordinate frame + +Example output +-------------- +The following shows an example output. +.. code:: + + [1760084283.005655] INFO ur_client_library/src/primary/primary_client.cpp 67: Starting primary client pipeline + [1760084283.056577] INFO ur_client_library/src/ur/dashboard_client.cpp 76: Connected: Universal Robots Dashboard Server + + [1760084283.060021] INFO ur_client_library/src/example_robot_wrapper.cpp 201: Robot ready to start a program + [1760084283.060652] INFO ur_client_library/src/helpers.cpp 95: SCHED_FIFO OK, priority 99 + [1760084283.061806] INFO ur_client_library/src/rtde/rtde_client.cpp 139: Negotiated RTDE protocol version to 2. + [1760084283.061944] INFO ur_client_library/src/rtde/rtde_client.cpp 291: Setting up RTDE communication with frequency 500.000000 + Program running: false + + [1760084284.095662] INFO ur_client_library/src/primary/primary_client.cpp 67: Starting primary client pipeline + [1760084284.225584] INFO ur_client_library/src/control/reverse_interface.cpp 225: Robot connected to reverse interface. Ready to receive control commands. + Program running: true + + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + [1760084284.225944] INFO ur_client_library/src/helpers.cpp 95: SCHED_FIFO OK, priority 99 + [1760084284.227630] INFO ur_client_library/src/control/reverse_interface.cpp 238: Connection to reverse interface dropped. + Program running: false + + Force-torque reported by robot: [0, 0, 0, 0, 0, 0] + Force-torque reported by robot: [0, 0, 0, 0, 0, 0] + <'x' pressed> + Artificial FT input: [10, 0, 0, 0, 0, 0] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + <'y' pressed> + Artificial FT input: [10, 10, 0, 0, 0, 0] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + <'y' pressed> + Artificial FT input: [10, 20, 0, 0, 0, 0] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0] + Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0] + Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0] + Force-torque reported by robot: [10, 20, 2.05103e-09, 0, 0, 0] + <'Y' pressed> + Artificial FT input: [10, 10, 0, 0, 0, 0] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + Force-torque reported by robot: [10, 10, -3.67436e-15, 0, 0, 0] + Force-torque reported by robot: [10, 10, -3.67436e-15, 0, 0, 0] + <'c' pressed> + Artificial FT input: [10, 10, 0, 0, 0, 10] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + Force-torque reported by robot: [10, 10, -3.67436e-15, 2.05104e-09, -2.05103e-09, 10] + <'X' pressed> + Artificial FT input: [0, 10, 0, 0, 0, 10] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + Force-torque reported by robot: [2.05103e-09, 10, 2.05103e-09, 2.05104e-09, -2.05103e-09, 10] + <'0' pressed> + Artificial FT input: [0, 0, 0, 0, 0, 0] + Press x,y,z to increase the respective axes, 0 for reset, q for quit. + Force-torque reported by robot: [0, 0, 0, 0, 0, 0] + Force-torque reported by robot: [0, 0, 0, 0, 0, 0] + <'q' pressed> + Artificial FT input: [0, 0, 0, 0, 0, 0] + [1760084296.656255] INFO ur_client_library/src/primary/primary_client.cpp 61: Stopping primary client pipeline diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4a12c021d..2d52ae3dc 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -59,3 +59,7 @@ target_link_libraries(trajectory_point_interface_example ur_client_library::urcl add_executable(instruction_executor instruction_executor.cpp) target_link_libraries(instruction_executor ur_client_library::urcl) + +add_executable(external_fts_through_rtde +external_fts_through_rtde.cpp) +target_link_libraries(external_fts_through_rtde ur_client_library::urcl) diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp new file mode 100644 index 000000000..fa4b3ad2b --- /dev/null +++ b/examples/external_fts_through_rtde.cpp @@ -0,0 +1,252 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include + +#include "ur_client_library/example_robot_wrapper.h" +#include "ur_client_library/types.h" + +// Platform-specific implementation of getch() +#if defined(_WIN32) || defined(_WIN64) +# include +char getch() +{ + return _getch(); // Windows-specific +} +#else +# include +# include +char getch() +{ + termios oldt, newt; + char ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + std::ignore = read(STDIN_FILENO, &ch, 1); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +} +#endif + +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"; + +std::unique_ptr g_my_robot; +std::atomic g_RUNNING = true; +urcl::vector6d_t g_FT_VEC{ 0, 0, 0, 0, 0, 0 }; +std::mutex g_FT_VEC_MUTEX; + +using namespace urcl; + +void ftInputTui() +{ + const std::string instructions = "Press x,y,z to increase the respective axes, 0 for reset, q for quit."; + urcl::vector6d_t local_ft_vec = g_FT_VEC; + while (g_RUNNING) + { + std::cout << instructions << std::endl; + char ch = getch(); + + std::cout << "<'" << ch << "' pressed>" << std::endl; + + switch (ch) + { + case 'x': + { + local_ft_vec[0] += 10; + break; + } + case 'y': + { + local_ft_vec[1] += 10; + break; + } + case 'z': + { + local_ft_vec[2] += 10; + break; + } + case 'a': + { + local_ft_vec[3] += 10; + break; + } + case 'b': + { + local_ft_vec[4] += 10; + break; + } + case 'c': + { + local_ft_vec[5] += 10; + break; + } + case 'X': + { + local_ft_vec[0] -= 10; + break; + } + case 'Y': + { + local_ft_vec[1] -= 10; + break; + } + case 'Z': + { + local_ft_vec[2] -= 10; + break; + } + case 'A': + { + local_ft_vec[3] -= 10; + break; + } + case 'B': + { + local_ft_vec[4] -= 10; + break; + } + case 'C': + { + local_ft_vec[5] -= 10; + break; + } + case '0': + { + local_ft_vec = { 0, 0, 0, 0, 0, 0 }; + break; + } + case 'q': + { + g_RUNNING = false; + } + default: + break; + } + if (g_RUNNING) + { + std::scoped_lock lock(g_FT_VEC_MUTEX); + g_FT_VEC = local_ft_vec; + } + std::cout << "Artificial FT input: " << local_ft_vec << std::endl; + } +} + +void RTDEWorker(const int second_to_run) +{ + g_my_robot->startRTDECommununication(false); + + vector6d_t actual_tcp_force; + auto start_time = std::chrono::steady_clock::now(); + while (g_RUNNING && + (second_to_run <= 0 || + std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < + second_to_run)) + { + urcl::vector6d_t local_ft_vec = g_FT_VEC; + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); + if (data_pkg) + { + // Data fields in the data package are accessed by their name. Only names present in the + // output recipe can be accessed. Otherwise this function will return false. + data_pkg->getData("actual_TCP_force", actual_tcp_force); + // Throttle output to once per second + if (std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() % + 1000 < + 2) + std::cout << "Force-torque reported by robot: " << actual_tcp_force << std::endl; + } + else + { + // The client isn't connected properly anymore / doesn't receive any data anymore. Stop the + // program. + std::cout << "Could not get fresh data package from robot" << std::endl; + return; + } + + if (g_FT_VEC_MUTEX.try_lock()) + { + local_ft_vec = g_FT_VEC; + g_FT_VEC_MUTEX.unlock(); + } + if (!g_my_robot->getUrDriver()->getRTDEWriter().sendExternalForceTorque(local_ft_vec)) + { + // This will happen for example, when the required keys are not configured inside the input + // recipe. + std::cout << "\033[1;31mSending RTDE data failed." << "\033[0m\n" << std::endl; + return; + } + } +} + +int main(int argc, char* argv[]) +{ + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + // Parse how may seconds to run + int second_to_run = -1; + if (argc > 2) + { + second_to_run = std::stoi(argv[2]); + } + + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, true); + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + // Enable using the force-torque input through RTDE in the robot controller + g_my_robot->getPrimaryClient()->sendScript("ft_rtde_input_enable(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); + + // Modify the artificial force-torque input through keyboard input + ftInputTui(); + + g_RUNNING = false; + g_my_robot->getUrDriver()->stopControl(); + rtde_thread.join(); + + return 0; +} diff --git a/examples/resources/rtde_input_recipe.txt b/examples/resources/rtde_input_recipe.txt index c00dc059d..4e7d678e0 100644 --- a/examples/resources/rtde_input_recipe.txt +++ b/examples/resources/rtde_input_recipe.txt @@ -10,3 +10,4 @@ standard_analog_output_mask standard_analog_output_type standard_analog_output_0 standard_analog_output_1 +external_force_torque diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h index 238bea13e..7a74bd0fd 100644 --- a/include/ur_client_library/comm/tcp_server.h +++ b/include/ur_client_library/comm/tcp_server.h @@ -186,7 +186,7 @@ class TCPServer uint32_t max_clients_allowed_; std::vector client_fds_; - static const int INPUT_BUFFER_SIZE = 100; + static const int INPUT_BUFFER_SIZE = 4096; char input_buffer_[INPUT_BUFFER_SIZE]; std::function new_connection_callback_; @@ -197,4 +197,4 @@ class TCPServer } // namespace comm } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_TCP_SERVER_H_INCLUDED \ No newline at end of file diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index cec8b7e2d..d5af8e5df 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -168,6 +168,18 @@ class RTDEWriter */ bool sendInputDoubleRegister(uint32_t register_id, double value); + /*! + * \brief Creates a package to request setting a new value for the robot's external force/torque + * interface. + * + * Note: This requires that the ft_rtde_input_enable() function was called on the robot side. + * + * \param external_force_torque The new external force/torque as a vector6d_t + * + * \returns Success of the package creation + */ + bool sendExternalForceTorque(const vector6d_t& external_force_torque); + private: uint8_t pinToMask(uint8_t pin); comm::URStream* stream_; diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 206c4823d..2df149505 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -356,5 +356,19 @@ bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value) return success; } +bool RTDEWriter::sendExternalForceTorque(const vector6d_t& external_force_torque) +{ + std::lock_guard guard(package_mutex_); + bool success = package_.setData("external_force_torque", external_force_torque); + if (success) + { + if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) + { + return false; + } + } + return success; +} + } // namespace rtde_interface } // namespace urcl diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index bd289508c..e0b7e76e6 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -40,7 +40,7 @@ using namespace urcl; class RTDEWriterTest : public ::testing::Test { protected: - using input_types = std::variant; + using input_types = std::variant; void SetUp() { @@ -108,23 +108,22 @@ class RTDEWriterTest : public ::testing::Test return false; } - std::vector input_recipe_ = { - "speed_slider_mask", - "speed_slider_fraction", - "standard_digital_output_mask", - "standard_digital_output", - "configurable_digital_output_mask", - "configurable_digital_output", - "tool_digital_output_mask", - "tool_digital_output", - "standard_analog_output_mask", - "standard_analog_output_type", - "standard_analog_output_0", - "standard_analog_output_1", - "input_bit_register_65", - "input_int_register_25", - "input_double_register_25", - }; + std::vector input_recipe_ = { "speed_slider_mask", + "speed_slider_fraction", + "standard_digital_output_mask", + "standard_digital_output", + "configurable_digital_output_mask", + "configurable_digital_output", + "tool_digital_output_mask", + "tool_digital_output", + "standard_analog_output_mask", + "standard_analog_output_type", + "standard_analog_output_0", + "standard_analog_output_1", + "input_bit_register_65", + "input_int_register_25", + "input_double_register_25", + "external_force_torque" }; std::unique_ptr writer_; std::unique_ptr server_; std::unique_ptr> stream_; @@ -164,6 +163,7 @@ class RTDEWriterTest : public ::testing::Test { "input_bit_register_65", bool() }, { "input_int_register_25", int32_t() }, { "input_double_register_25", double() }, + { "external_force_torque", vector6d_t() }, }; }; @@ -441,6 +441,25 @@ TEST_F(RTDEWriterTest, send_input_double_register) EXPECT_FALSE(writer_->sendInputDoubleRegister(register_id, send_register_value)); } +TEST_F(RTDEWriterTest, send_external_force_torque) +{ + vector6d_t send_external_force_torque = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + EXPECT_TRUE(writer_->sendExternalForceTorque(send_external_force_torque)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("external_force_torque")); + + vector6d_t received_external_force_torque = std::get(parsed_data_["external_force_torque"]); + + EXPECT_EQ(send_external_force_torque[0], received_external_force_torque[0]); + EXPECT_EQ(send_external_force_torque[1], received_external_force_torque[1]); + EXPECT_EQ(send_external_force_torque[2], received_external_force_torque[2]); + EXPECT_EQ(send_external_force_torque[3], received_external_force_torque[3]); + EXPECT_EQ(send_external_force_torque[4], received_external_force_torque[4]); + EXPECT_EQ(send_external_force_torque[5], received_external_force_torque[5]); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 510558d909a7db5b4282f63f2ec863250187197f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Oct 2025 10:51:05 +0200 Subject: [PATCH 3/7] Fix method name to comply with CSG --- examples/external_fts_through_rtde.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index fa4b3ad2b..0fa8bc124 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -164,7 +164,7 @@ void ftInputTui() } } -void RTDEWorker(const int second_to_run) +void rtdeWorker(const int second_to_run) { g_my_robot->startRTDECommununication(false); @@ -239,7 +239,7 @@ int main(int argc, char* argv[]) // 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); + std::thread rtde_thread(rtdeWorker, second_to_run); // Modify the artificial force-torque input through keyboard input ftInputTui(); From 8e594ea213b99bd44135ab7876e0e725b9371b35 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Oct 2025 11:20:31 +0200 Subject: [PATCH 4/7] Avoid collision with existing getch() function on win --- doc/examples/external_fts_through_rtde.rst | 4 ++-- examples/external_fts_through_rtde.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/examples/external_fts_through_rtde.rst b/doc/examples/external_fts_through_rtde.rst index ce7503e93..7e9b41ded 100644 --- a/doc/examples/external_fts_through_rtde.rst +++ b/doc/examples/external_fts_through_rtde.rst @@ -63,7 +63,7 @@ keyboard input commands. Input key handling ------------------ -For handling the keyboard input, define a ``getch()`` function that reads a single character from +For handling the keyboard input, define a ``getChar()`` function that reads a single character from the terminal without waiting for a newline character. This has to be done differently on Windows and Linux (and other unix-like systems), therefore the code is split using preprocessor directives. @@ -72,7 +72,7 @@ and Linux (and other unix-like systems), therefore the code is split using prepr :caption: external_fts_through_rtde/freedrive_example.cpp :linenos: :lineno-match: - :start-at: // Platform-specific implementation of getch() + :start-at: // Platform-specific implementation of getChar() :end-at: #endif Setting the external force-torque values diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index 0fa8bc124..53c0ebf6f 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -34,17 +34,17 @@ #include "ur_client_library/example_robot_wrapper.h" #include "ur_client_library/types.h" -// Platform-specific implementation of getch() +// Platform-specific implementation of getChar() #if defined(_WIN32) || defined(_WIN64) # include -char getch() +char getChar() { return _getch(); // Windows-specific } #else # include # include -char getch() +char getChar() { termios oldt, newt; char ch; @@ -77,7 +77,7 @@ void ftInputTui() while (g_RUNNING) { std::cout << instructions << std::endl; - char ch = getch(); + char ch = getChar(); std::cout << "<'" << ch << "' pressed>" << std::endl; From f8aeb87c7887189422871949b01d56563eebdcd1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 13 Oct 2025 09:27:05 +0200 Subject: [PATCH 5/7] Apply suggestions from code review Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> --- examples/external_fts_through_rtde.cpp | 2 +- include/ur_client_library/comm/package_serializer.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index 53c0ebf6f..6c860b35c 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -72,7 +72,7 @@ using namespace urcl; void ftInputTui() { - const std::string instructions = "Press x,y,z to increase the respective axes, 0 for reset, 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) { diff --git a/include/ur_client_library/comm/package_serializer.h b/include/ur_client_library/comm/package_serializer.h index 3cd897690..01fa85751 100644 --- a/include/ur_client_library/comm/package_serializer.h +++ b/include/ur_client_library/comm/package_serializer.h @@ -126,7 +126,7 @@ class PackageSerializer * \brief A serialization method for std::arrays (works for urcl::vector6d_t, etc). * * \param buffer The buffer to write the serialization into. - * \param val The string to serialize. + * \param val The array to serialize. * * \returns Size in byte of the serialization. */ From fb66319c1864137026699f5ebd597c9dfea5e53b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 17 Oct 2025 08:21:45 +0200 Subject: [PATCH 6/7] Add ft_rtde_input_enable to ScriptCommandInterface --- doc/architecture/script_command_interface.rst | 20 +++++++- examples/external_fts_through_rtde.cpp | 9 ++-- examples/resources/rtde_input_only_ft.txt | 1 + examples/resources/rtde_input_recipe.txt | 1 - .../control/script_command_interface.h | 21 ++++++++ include/ur_client_library/ur/ur_driver.h | 20 ++++++++ resources/external_control.urscript | 18 ++++++- src/control/script_command_interface.cpp | 41 ++++++++++++++- src/ur/ur_driver.cpp | 14 ++++++ tests/test_script_command_interface.cpp | 50 ++++++++++++++++++- 10 files changed, 185 insertions(+), 10 deletions(-) create mode 100644 examples/resources/rtde_input_only_ft.txt diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index cd82f7c1d..6a419d54b 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -21,6 +21,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun `_ 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 diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index 6c860b35c..5c99880c0 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -31,6 +31,7 @@ #include #include +#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 g_my_robot; std::atomic 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); diff --git a/examples/resources/rtde_input_only_ft.txt b/examples/resources/rtde_input_only_ft.txt new file mode 100644 index 000000000..26e797973 --- /dev/null +++ b/examples/resources/rtde_input_only_ft.txt @@ -0,0 +1 @@ +external_force_torque diff --git a/examples/resources/rtde_input_recipe.txt b/examples/resources/rtde_input_recipe.txt index 4e7d678e0..c00dc059d 100644 --- a/examples/resources/rtde_input_recipe.txt +++ b/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 diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 0c4ebc6ed..5fead319f 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/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 }; /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 82acfef1d..15a4fd252 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/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. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 0ae724335..4d8de8a13 100644 --- a/resources/external_control.urscript +++ b/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 \ No newline at end of file +# NODE_CONTROL_LOOP_ENDS diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 995205e1a..01342c293 100644 --- a/src/control/script_command_interface.cpp +++ b/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(round(sensor_mass * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + + for (auto const& mass_component : sensor_measuring_offset) + { + val = htobe32(static_cast(round(mass_component * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + for (auto const& cog_component : sensor_cog) + { + val = htobe32(static_cast(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 \ No newline at end of file diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e9020e215..e42c2c026 100644 --- a/src/ur/ur_driver.cpp +++ b/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; diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 22cb2bc70..04cdc1507 100644 --- a/tests/test_script_command_interface.cpp +++ b/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 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(message[0]); + + // Test sensor mass + double received_sensor_mass = static_cast(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(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(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(); -} +} \ No newline at end of file From cbbf94fa99b006f18060b8108ac4193e4c123d71 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 17 Oct 2025 08:52:44 +0200 Subject: [PATCH 7/7] Rephrase note about f/t coordinate system --- doc/examples/external_fts_through_rtde.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/examples/external_fts_through_rtde.rst b/doc/examples/external_fts_through_rtde.rst index 7e9b41ded..a9827796e 100644 --- a/doc/examples/external_fts_through_rtde.rst +++ b/doc/examples/external_fts_through_rtde.rst @@ -101,8 +101,8 @@ tui thread is currently holding the lock. The new values will be available in a .. note:: The values reported from the robot are rotated to the base coordinate system of the robot. - Hence, they will not be the same as the ones sent to the robot. Align the robot's TCP the same - as the base coordinate frame + Hence, they will not be the same as the ones sent to the robot. For them to be the same, align + the TCP frame with robots base frame. Example output --------------