From 19fbe3adb77e74c764e2d84e8ca33529046ef9c9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 17 Oct 2024 09:18:21 +0200 Subject: [PATCH 1/3] Allow resetting the RTDE client --- include/ur_client_library/ur/ur_driver.h | 37 +++++++++++++++++++-- src/ur/ur_driver.cpp | 42 +++++++++++++++++++----- 2 files changed, 67 insertions(+), 12 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 1dd2bf55..f0f9b0a3 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -195,7 +195,7 @@ class UrDriver uint32_t getControlFrequency() const { - return rtde_frequency_; + return rtde_client_->getTargetFrequency(); } /*! @@ -487,6 +487,36 @@ class UrDriver script_command_interface_->setToolContactResultCallback(tool_contact_result_cb); } + /** + * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication + * with the maximum available frequency, this enables users to start RTDE communication with a + * custom rate. + * + * Note: After calling this function startRTDECommunication() has to be called again to actually + * start RTDE communication. + * + * \param output_recipe Vector containing the output recipe + * \param input_recipe Vector containing the input recipe + * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency. + */ + void resetRTDEClient(const std::vector& output_recipe, const std::vector& input_recipe, + double target_frequency = 0.0); + + /** + * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication + * with the maximum available frequency, this enables users to start RTDE communication with a + * custom rate. + * + * Note: After calling this function startRTDECommunication() has to be called again to actually + * start RTDE communication. + * + * \param output_recipe_filename Filename where the output recipe is stored in. + * \param input_recipe_filename Filename where the input recipe is stored in. + * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency. + */ + void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, + double target_frequency = 0.0); + private: static std::string readScriptFile(const std::string& filename); /*! @@ -498,7 +528,9 @@ class UrDriver */ bool reconnectSecondaryStream(); - int rtde_frequency_; + void initRTDE(); + void setupReverseInterface(const uint32_t reverse_port); + comm::INotifier notifier_; std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; @@ -510,7 +542,6 @@ class UrDriver uint32_t servoj_gain_; double servoj_lookahead_time_; - std::chrono::milliseconds step_time_; std::function handle_program_state_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 5038f798..90a33c17 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -61,7 +61,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling) : servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) - , step_time_(std::chrono::milliseconds(8)) , handle_program_state_(handle_program_state) , robot_ip_(robot_ip) { @@ -78,13 +77,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; - if (!rtde_client_->init()) - { - throw UrException("Initialization of RTDE client went wrong."); - } - - rtde_frequency_ = rtde_client_->getMaxFrequency(); - step_time_ = std::chrono::milliseconds(1000 / rtde_frequency_); + initRTDE(); + setupReverseInterface(reverse_port); // Figure out the ip automatically if the user didn't provide it std::string local_ip = reverse_ip.empty() ? rtde_client_->getIP() : reverse_ip; @@ -210,7 +204,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ URCL_LOG_DEBUG("Created script sender"); } - reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state, step_time_)); trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port)); script_command_interface_.reset(new control::ScriptCommandInterface(script_command_port)); @@ -621,4 +614,35 @@ void UrDriver::setKeepaliveCount(const uint32_t count) reverse_interface_->setKeepaliveCount(count); } +void UrDriver::resetRTDEClient(const std::vector& output_recipe, + const std::vector& input_recipe, double target_frequency) +{ + rtde_client_.reset( + new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe, target_frequency)); + initRTDE(); +} + +void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, + double target_frequency) +{ + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_filename, input_recipe_filename, + target_frequency)); + initRTDE(); +} + +void UrDriver::initRTDE() +{ + if (!rtde_client_->init()) + { + throw UrException("Initialization of RTDE client went wrong."); + } +} + +void UrDriver::setupReverseInterface(const uint32_t reverse_port) +{ + auto rtde_frequency = rtde_client_->getTargetFrequency(); + auto step_time = std::chrono::milliseconds(static_cast(1000 / rtde_frequency)); + reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); +} + } // namespace urcl From 89a4fcc25bf60043379f89d46e76ed14eb3e0c70 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 13 Nov 2024 08:39:56 +0100 Subject: [PATCH 2/3] Add test for resetting the RTDE client --- tests/test_ur_driver.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index d255cc32..3bfcacc0 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -372,6 +372,13 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure) EXPECT_TRUE(g_ur_driver_->sendRobotProgram()); } +TEST_F(UrDriverTest, reset_rtde_client) +{ + double target_frequency = 50; + g_ur_driver_->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); + ASSERT_EQ(g_ur_driver_->getControlFrequency(), target_frequency); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[]) From 82d85088913c1a206721629bf4a744197470a8b3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 13 Nov 2024 08:46:21 +0100 Subject: [PATCH 3/3] Add a note about resetting the frequency --- doc/architecture.rst | 5 +++++ doc/conf.py | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/doc/architecture.rst b/doc/architecture.rst index 9da37fe1..95843592 100644 --- a/doc/architecture.rst +++ b/doc/architecture.rst @@ -63,6 +63,11 @@ sure to * run it from the package's main folder, as for simplicity reasons it doesn't use any sophisticated method to locate the required files. +.. note:: + The ``URDriver`` class creates a ``RTDEClient`` during initialization using the provided + recipes and utilizing the robot model's maximum frequency. If you would like to use a different + frequency, please use the ``resetRTDEClient()`` method after the ``UrDriver`` object has been + created. RTDEWriter ^^^^^^^^^^ diff --git a/doc/conf.py b/doc/conf.py index 257f406f..9fee3cfd 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -27,7 +27,7 @@ # -- Project information ----------------------------------------------------- project = "ur_client_library" -copyright = "2022, Felix Exner" +copyright = "2022, Universal Robots A/S" author = "Felix Exner" # The short X.Y version