From 3b9cb8399be6f18888598b8099e065b560a553c0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 8 Oct 2025 14:21:24 +0200 Subject: [PATCH 1/4] 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 45398bd38c5e91188362b703a7b147c1aa0f33a1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 8 Oct 2025 14:52:05 +0200 Subject: [PATCH 2/4] 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 77a562d08..0916e1e4b 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -158,6 +158,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 ea543ab80..2f9f86c08 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -335,5 +335,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 456741cab87a1e24fe6d3d17ff05737b10536b9b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Oct 2025 10:51:05 +0200 Subject: [PATCH 3/4] 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 ba7be1a4f851bfb5ff76a3e4b5b45379629517e0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 10 Oct 2025 11:20:31 +0200 Subject: [PATCH 4/4] 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;