From ebe30d24973b4d926ab9e936198817fe8be2768e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 15 May 2025 10:48:06 +0200 Subject: [PATCH 01/23] Add notice about torque_control beta status --- README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/README.md b/README.md index 2994c31c0..a1ad47ce8 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,27 @@ The library has no external dependencies besides the standard C++ libraries such to make it easy to integrate and maintain. It also serves as the foundation for the ROS and ROS 2 drivers. +--- + +> [!IMPORTANT] +> The **torque_control** branch is a **beta feature**. At the current time, you will need to +> participate in the public beta program at +> [https://ur.centercode.com/key/PolyScope5Beta](https://ur.centercode.com/key/PolyScope5Beta) to +> use it. It will not work with any released robot software version. At a later time this will +> require a certain robot software version to work and the new features will not work with older +> software versions. However, old functionality will still be working with older robot software as +> noted in the version requirements below. +> +> This being a beta feature, the API is not to be considered stable. Anything developed on this +> branch might be changing in a breaking way until it is merged into the master branch. +> +> Also, please keep the beta status in mind when operating the robot. Parts of the software +> involved in the beta-feature might not be tested to the usual extent and might show unexpected +> behavior in edge cases. +> +> Documentation for the new features may also be missing initially. This will be added before +> merging things to the master branch. +
Universal Robot family From 7b01a7ffeb318738864a568e25b0f4f8405392cb Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Mon, 19 May 2025 12:01:43 +0200 Subject: [PATCH 02/23] Added command to set friction compensation for the torque command (#323) --- doc/architecture/script_command_interface.rst | 11 ++++++ .../control/script_command_interface.h | 26 +++++++++---- include/ur_client_library/ur/ur_driver.h | 11 ++++++ resources/external_control.urscript | 8 ++++ src/control/script_command_interface.cpp | 23 +++++++++++ src/ur/ur_driver.cpp | 13 +++++++ tests/test_script_command_interface.cpp | 39 +++++++++++++++++++ 7 files changed, 124 insertions(+), 7 deletions(-) diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index f52aa48b2..cd82f7c1d 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -20,6 +20,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun contact example `_ for more information. +- ``setFrictionCompensation()``: Set friction compensation for torque command. Communication protocol ---------------------- @@ -48,6 +49,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 4: endForceMode - 5: startToolContact - 6: endToolContact + - 7: setFrictionCompensation 1-27 data fields specific to the command ===== ===== @@ -121,6 +123,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1 No specific meaning / values ignored ===== ===== +.. table:: With setFrictionCompensation command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1 friction_compensation_enabled enable/disable friction compensation for torque command. + ===== ===== + .. 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, diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 2512c0247..98eacdcc6 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -147,6 +147,17 @@ class ScriptCommandInterface : public ReverseInterface */ bool endToolContact(); + /*! + * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, + * if false it will not. + * + * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be + * used when calling torque_command + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! * \brief Returns whether a client/robot is connected to this server. * @@ -177,13 +188,14 @@ class ScriptCommandInterface : public ReverseInterface enum class ScriptCommand : int32_t { - ZERO_FTSENSOR = 0, ///< Zero force torque sensor - SET_PAYLOAD = 1, ///< Set payload - SET_TOOL_VOLTAGE = 2, ///< Set tool voltage - START_FORCE_MODE = 3, ///< Start force mode - END_FORCE_MODE = 4, ///< End force mode - START_TOOL_CONTACT = 5, ///< Start detecting tool contact - END_TOOL_CONTACT = 6, ///< End detecting tool contact + ZERO_FTSENSOR = 0, ///< Zero force torque sensor + SET_PAYLOAD = 1, ///< Set payload + SET_TOOL_VOLTAGE = 2, ///< Set tool voltage + START_FORCE_MODE = 3, ///< Start force mode + END_FORCE_MODE = 4, ///< End force mode + START_TOOL_CONTACT = 5, ///< Start detecting tool contact + END_TOOL_CONTACT = 6, ///< End detecting tool contact + SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation }; bool client_connected_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index d064c8128..58ba23f69 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -714,6 +714,17 @@ class UrDriver */ bool endToolContact(); + /*! + * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, + * if false it will not. + * + * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be + * used when calling torque_command + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 266b8c8b8..8e67212f3 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -49,6 +49,7 @@ START_FORCE_MODE = 3 END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 +SET_FRICTION_COMPENSATION = 7 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -81,6 +82,7 @@ global spline_qdd = [0, 0, 0, 0, 0, 0] global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 +global friction_compensation_enabled = True # Global thread variables thread_move = 0 @@ -691,6 +693,12 @@ thread script_commands(): socket_send_int(UNTIL_TOOL_CONTACT_RESULT_CANCELED, "script_command_socket") end tool_contact_running = False + elif command == SET_FRICTION_COMPENSATION: + if raw_command[2] == 0: + friction_compensation_enabled = False + else: + friction_compensation_enabled = True + end end end end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index ca2fb6d8e..0a4363494 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -222,6 +222,29 @@ bool ScriptCommandInterface::endToolContact() return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) +{ + const int message_length = 2; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_FRICTION_COMPENSATION)); + b_pos += append(b_pos, val); + + val = htobe32(friction_compensation_enabled); + 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_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e61082293..6e30579c7 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -527,6 +527,19 @@ bool UrDriver::endToolContact() } } +bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setFrictionCompensation(friction_compensation_enabled); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set friction compensation."); + 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 d834502a8..1f1c02744 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -382,6 +382,45 @@ TEST_F(ScriptCommandInterfaceTest, test_tool_contact_callback) EXPECT_EQ(toUnderlying(received_result_), toUnderlying(send_result)); } +TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + script_command_interface_->setFrictionCompensation(true); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 7 is set friction compensation + int32_t expected_command = 7; + EXPECT_EQ(command, expected_command); + + int32_t expected_friction_compensation = 1; + EXPECT_EQ(message[0], expected_friction_compensation); + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); + + script_command_interface_->setFrictionCompensation(false); + + message.clear(); + client_->readMessage(command, message); + + EXPECT_EQ(command, expected_command); + + expected_friction_compensation = 0; + EXPECT_EQ(message[0], expected_friction_compensation); + + // The rest of the message should be zero + message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0); + expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 01761be3c730f28f1c2125cd5cdbbb7068e832ec Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Tue, 20 May 2025 11:05:24 +0200 Subject: [PATCH 03/23] Add torque control to urscript (#325) The PR adds `MODE_TORQUE` to the supported control modes and extends the external control urscript with a torque thread that applies the latest `cmd_torque` as torque command. --- include/ur_client_library/comm/control_mode.h | 8 ++++--- resources/external_control.urscript | 23 +++++++++++++++++++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 62dd706fb..51156da6f 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -51,8 +51,9 @@ enum class ControlMode : int32_t MODE_POSE = 5, ///< Set when cartesian pose control is active. MODE_FREEDRIVE = 6, ///< Set when freedrive mode is active. MODE_TOOL_IN_CONTACT = - 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() - END ///< This is not an actual control mode, but used internally to get the number of control modes + 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() + MODE_TORQUE = 8, ///< Set when torque control is active. + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -63,7 +64,8 @@ class ControlModeTypes public: // Control modes that require realtime communication static const inline std::vector REALTIME_CONTROL_MODES = { - ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE + ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE }; // Control modes that doesn't require realtime communication diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 8e67212f3..181e9e617 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -25,6 +25,7 @@ MODE_SPEEDL = 4 MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 +MODE_TORQUE = 8 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -71,6 +72,7 @@ JOINT_IGNORE_SPEED = 20.0 global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +global cmd_torque = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_servo_q = get_joint_positions() global cmd_servo_q_last = cmd_servo_q global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -227,6 +229,22 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end +# Helpers for torque control +def set_torque(target_torque): + cmd_torque = target_torque + control_mode = MODE_TORQUE +end + +thread torqueThread(): + textmsg("ExternalControl: Starting torque thread") + while control_mode == MODE_TORQUE: + torque = cmd_torque + torque_command(torque, friction_comp=friction_compensation_enabled) + end + textmsg("ExternalControl: torque thread ended") + stopj(STOPJ_ACCELERATION) +end + # Function return value (bool) determines whether the robot is moving after this spline segment or # not. def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=False): @@ -756,6 +774,8 @@ while control_mode > MODE_STOPPED: thread_move = run servoThread() elif control_mode == MODE_SPEEDJ: thread_move = run speedThread() + elif control_mode == MODE_TORQUE: + thread_move = run torqueThread() elif control_mode == MODE_FORWARD: kill thread_move stopj(STOPJ_ACCELERATION) @@ -774,6 +794,9 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_SPEEDJ: qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] set_speed(qd) + elif control_mode == MODE_TORQUE: + torque = [params_mult[2]/ MULT_jointstate, params_mult[3]/ MULT_jointstate, params_mult[4]/ MULT_jointstate, params_mult[5]/ MULT_jointstate, params_mult[6]/ MULT_jointstate, params_mult[7]/ MULT_jointstate] + set_torque(torque) elif control_mode == MODE_FORWARD: if params_mult[2] == TRAJECTORY_MODE_RECEIVE: kill thread_trajectory From 0d33b7d0c4f15ace119a7bae83a8b42a969d44b1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 20 May 2025 15:21:40 +0200 Subject: [PATCH 04/23] Add an example using the script_command_interface (#326) This adds an example for using the script_command interface. --- doc/examples.rst | 1 + doc/examples/script_command_interface.rst | 71 +++++++++++ examples/CMakeLists.txt | 4 + examples/script_command_interface.cpp | 145 ++++++++++++++++++++++ 4 files changed, 221 insertions(+) create mode 100644 doc/examples/script_command_interface.rst create mode 100644 examples/script_command_interface.cpp diff --git a/doc/examples.rst b/doc/examples.rst index 159b2721c..a2a362800 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/script_command_interface examples/script_sender examples/spline_example examples/tool_contact_example diff --git a/doc/examples/script_command_interface.rst b/doc/examples/script_command_interface.rst new file mode 100644 index 000000000..a90147185 --- /dev/null +++ b/doc/examples/script_command_interface.rst @@ -0,0 +1,71 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/script_command_interface.rst + +Script command interface +======================== + +The :ref:`script_command_interface` allows sending predefined commands to the robot while there is +URScript running that is connected to it. + +An example to utilize the script command interface can be found in the `freedrive_example.cpp `_. + +In order to use the ``ScriptCommandInterface``, there has to be a script code running on the robot +that connects to the ``ScriptCommandInterface``. This happens as part of the big +`external_control.urscript `_. In order to reuse that with this example, we will create a full +``UrDriver`` and leverage the ``ScriptCommandInterface`` through this. + +At first, we create a ``ExampleRobotWrapper`` object in order to initialize communication with the +robot. + +.. literalinclude:: ../../examples/script_command_interface.cpp + :language: c++ + :caption: examples/script_command_interface.cpp + :linenos: + :lineno-match: + :start-at: g_my_robot = + :end-at: std::thread script_command_send_thread(sendScriptCommands); + +The script commands will be sent in a separate thread which will be explained later. + +Since the connection to the script command interface runs as part of the bigger external_control +script, we'll wrap the calls alongside a full ``ExampleRobotWrapper``. Hence, we'll have to send +keepalive signals regularly to keep the script running: + +.. literalinclude:: ../../examples/script_command_interface.cpp + :language: c++ + :caption: examples/script_command_interface.cpp + :linenos: + :lineno-match: + :start-at: std::chrono::duration time_done(0); + :end-at: g_my_robot->getUrDriver()->stopControl(); + +Sending script commands +----------------------- + +Once the script is running on the robot, a connection to the driver's script command interface +should be established. The ``UrDriver`` forwards most calls of the ``ScriptCommandInterface`` and +we will use that interface in this example. To send a script command, we can e.g. use +``g_my_robot->getUrDriver()->zeroFTSensor()``. + +In the example, we have wrapped the calls into a lambda function that will wait a specific timeout, +print a log output what command will be sent and then call the respective command: + +.. literalinclude:: ../../examples/script_command_interface.cpp + :language: c++ + :caption: examples/script_command_interface.cpp + :linenos: + :lineno-match: + :start-at: run_cmd( + :end-before: URCL_LOG_INFO("Script command thread finished."); + +The lambda itself looks like this: + +.. literalinclude:: ../../examples/script_command_interface.cpp + :language: c++ + :caption: examples/script_command_interface.cpp + :linenos: + :lineno-match: + :start-at: auto run_cmd = + :end-before: // Keep running all commands in a loop + +For a list of all available script commands, please refer to the ``ScriptCommandInterface`` class +`here `_. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 48452197d..e27ea26d8 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -44,6 +44,10 @@ add_executable(script_sender_example script_sender.cpp) target_link_libraries(script_sender_example ur_client_library::urcl) +add_executable(script_command_interface_example +script_command_interface.cpp) +target_link_libraries(script_command_interface_example ur_client_library::urcl) + add_executable(trajectory_point_interface_example trajectory_point_interface.cpp) target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp new file mode 100644 index 000000000..6ada63f2f --- /dev/null +++ b/examples/script_command_interface.cpp @@ -0,0 +1,145 @@ +// -- 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/ur/tool_communication.h" + +#include +#include + +using namespace urcl; + +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; +bool g_HEADLESS = true; +bool g_running = false; + +void sendScriptCommands() +{ + auto run_cmd = [](const std::string& log_output, std::function func) { + const std::chrono::seconds timeout(3); + if (g_running) + { + // We wait a fixed time so that not each command is run directly behind each other. + // This is done for example purposes only, so users can follow the effect on the teach + // pendant. + std::this_thread::sleep_for(timeout); + URCL_LOG_INFO(log_output.c_str()); + func(); + } + }; + + // Keep running all commands in a loop until g_running is set to false + while (g_running) + { + run_cmd("Setting tool voltage to 24V", + []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::_24V); }); + run_cmd("Enabling tool contact mode", []() { g_my_robot->getUrDriver()->startToolContact(); }); + run_cmd("Setting friction_compensation variable to `false`", + []() { g_my_robot->getUrDriver()->setFrictionCompensation(false); }); + run_cmd("Setting tool voltage to 0V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::OFF); }); + run_cmd("Zeroing the force torque sensor", []() { g_my_robot->getUrDriver()->zeroFTSensor(); }); + run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); + run_cmd("Setting friction_compensation variable to `true`", + []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); + } + URCL_LOG_INFO("Script command thread finished."); +} + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + // Parse how many seconds to run + auto second_to_run = std::chrono::seconds(0); + if (argc > 2) + { + second_to_run = std::chrono::seconds(std::stoi(argv[2])); + } + + // Parse whether to run in headless mode + // When not using headless mode, the global variables can be watched on the teach pendant. + if (argc > 3) + { + g_HEADLESS = std::string(argv[3]) == "true" || std::string(argv[3]) == "1" || std::string(argv[3]) == "True" || + std::string(argv[3]) == "TRUE"; + } + + g_my_robot = + std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp"); + + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + // We will send script commands from a separate thread. That will stay active as long as + // g_running is true. + g_running = true; + std::thread script_command_send_thread(sendScriptCommands); + + // We will need to keep the script running on the robot. As we use the "usual" external_control + // urscript, we'll have to send keepalive signals as long as we want to keep it active. + std::chrono::duration time_done(0); + std::chrono::duration timeout(second_to_run); + auto stopwatch_last = std::chrono::steady_clock::now(); + auto stopwatch_now = stopwatch_last; + while ((time_done < timeout || second_to_run.count() == 0) && g_my_robot->isHealthy()) + { + g_my_robot->getUrDriver()->writeKeepalive(); + + stopwatch_now = std::chrono::steady_clock::now(); + time_done += stopwatch_now - stopwatch_last; + stopwatch_last = stopwatch_now; + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(1.0 / g_my_robot->getUrDriver()->getControlFrequency()))); + } + + URCL_LOG_INFO("Timeout reached."); + g_my_robot->getUrDriver()->stopControl(); + + // Stop the script command thread + g_running = false; + script_command_send_thread.join(); + + return 0; +} From 9cecbff0acd3c48fc84f814ffe2d96fd6bf4b297 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 09:00:57 +0200 Subject: [PATCH 05/23] =?UTF-8?q?Added=20two=20new=20control=20modes=20PD?= =?UTF-8?q?=20controller=20in=20joint=20space=20and=20PD=20contr=E2=80=A6?= =?UTF-8?q?=20(#336)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit …oller in task space The pd controller in joint space will accept joint targets and compute joint torques based on PD control scheme. The pd controller in task space will accept task space targets and use inverse kinematics to compute joint targets which are then used to compute joint torques based on PD control scheme. --------- Co-authored-by: Felix Exner --- examples/CMakeLists.txt | 4 + examples/pd_controller_example.cpp | 241 ++++++++++++++++++ include/ur_client_library/comm/control_mode.h | 15 +- .../torque_command_controller_parameters.h | 97 +++++++ resources/external_control.urscript | 52 ++++ src/ur/ur_driver.cpp | 33 +++ tests/test_reverse_interface.cpp | 18 ++ 7 files changed, 455 insertions(+), 5 deletions(-) create mode 100644 examples/pd_controller_example.cpp create mode 100644 include/ur_client_library/control/torque_command_controller_parameters.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e27ea26d8..f13be169a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -55,3 +55,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(pd_controller_example +pd_controller_example.cpp) +target_link_libraries(pd_controller_example ur_client_library::urcl) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp new file mode 100644 index 000000000..a959a4e7b --- /dev/null +++ b/examples/pd_controller_example.cpp @@ -0,0 +1,241 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- 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 "ur_client_library/example_robot_wrapper.h" +#include "ur_client_library/ur/dashboard_client.h" +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/types.h" +#include "ur_client_library/ur/instruction_executor.h" + +#include +#include +#include +#include + +using namespace urcl; + +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; + +void signal_callback_handler(int signum) +{ + std::cout << "Caught signal " << signum << std::endl; + if (g_my_robot != nullptr) + { + // Stop control of the robot + g_my_robot->getUrDriver()->stopControl(); + } + // Terminate program + exit(signum); +} + +struct DataStorage +{ + std::vector target; + std::vector actual; + std::vector time; + + void write_to_file(const std::string& filename) + { + std::fstream output(filename, std::ios::out); + output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," + "actual3,actual4,actual5\n"; + for (size_t i = 0; i < time.size(); ++i) + { + output << time[i]; + for (auto& t : target[i]) + { + output << "," << t; + } + for (auto& a : actual[i]) + { + output << "," << a; + } + output << "\n"; + } + output.close(); + } +}; + +bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_name, + const comm::ControlMode control_mode, const urcl::vector6d_t& amplitude) +{ + const int32_t running_time = 13; + double time = 0.0; + + // Reserve space for expected amount of data + data_storage.actual.reserve(running_time * 500); + data_storage.target.reserve(running_time * 500); + data_storage.time.reserve(running_time * 500); + + urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + bool first_pass = true; + while (time < running_time) + { + const auto t_start = std::chrono::high_resolution_clock::now(); + // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the + // robot will effectively be in charge of setting the frequency of this loop. + // In a real-world application this thread should be scheduled with real-time priority in order + // to ensure that this is called in time. + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); + if (!data_pkg) + { + URCL_LOG_WARN("Could not get fresh data package from robot"); + return false; + } + // Read current joint positions from robot data + if (!data_pkg->getData(actual_data_name, actual)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find" + actual_data_name + "in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + if (first_pass) + { + target = actual; + start = actual; + for (size_t i = 0; i < start.size(); ++i) + { + start[i] = start[i] - amplitude[i]; + } + first_pass = false; + } + + for (size_t i = 0; i < target.size(); ++i) + { + target[i] = start[i] + amplitude[i] * cos(time); + } + + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. + bool ret = g_my_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100)); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return false; + } + + // Increment time and log data + const auto t_end = std::chrono::high_resolution_clock::now(); + const double elapsed_time_ms = std::chrono::duration(t_end - t_start).count() / 1000; + time = time + elapsed_time_ms; + data_storage.actual.push_back(actual); + data_storage.target.push_back(target); + data_storage.time.push_back(time); + } + + return true; +} + +int main(int argc, char* argv[]) +{ + // This will make sure that we stop controlling the robot if the user presses ctrl-c + signal(SIGINT, signal_callback_handler); + + urcl::setLogLevel(urcl::LogLevel::INFO); + + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + const bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode); + + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); + + URCL_LOG_INFO("Move the robot to initial position"); + instruction_executor->moveJ(urcl::vector6d_t{ 0, -1.67, -0.65, -1.59, 1.61, 5.09 }, 0.5, 0.2, 5); + + DataStorage joint_controller_storage; + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + g_my_robot->getUrDriver()->startRTDECommunication(); + URCL_LOG_INFO("Start controlling the robot using the PD controller"); + const bool completed_joint_control = + pd_control_loop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, + { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); + if (!completed_joint_control) + { + URCL_LOG_ERROR("Didn't complete pd control in joint space"); + g_my_robot->getUrDriver()->stopControl(); + return 1; + } + + // Read the rtde packages in the background while moving to initial position for PD control in task space + g_my_robot->startConsumingRTDEData(); + + URCL_LOG_INFO("Move the robot to initial position for task space PD control"); + instruction_executor->moveJ(urcl::vector6d_t{ 0.03, -0.98, -1.83, -1.70, 1.37, 5.12 }, 0.5, 0.2, 5); + + DataStorage task_controller_storage; + + g_my_robot->stopConsumingRTDEData(); + URCL_LOG_INFO("Start controlling the robot using the PD controller with task space targets"); + const bool completed_task_control = + pd_control_loop(task_controller_storage, "actual_TCP_pose", comm::ControlMode::MODE_PD_CONTROLLER_TASK, + { 0.05, 0.05, 0.05, 0.02, 0.02, 0.02 }); + if (!completed_task_control) + { + URCL_LOG_ERROR("Didn't complete PD control in task space"); + g_my_robot->getUrDriver()->stopControl(); + return 1; + } + + // Consume data in the background while writing the logged data to a file + g_my_robot->startConsumingRTDEData(); + g_my_robot->getUrDriver()->stopControl(); + URCL_LOG_INFO("Movement done"); + + // Write data to files, can be used for benchmarking the controller (commented out as default) + // joint_controller_storage.write_to_file("examples/pd_controller_joint.csv"); + // task_controller_storage.write_to_file("examples/pd_controller_task.csv"); + g_my_robot->stopConsumingRTDEData(); + + return 0; +} diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 51156da6f..f1da507ac 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -53,7 +53,9 @@ enum class ControlMode : int32_t MODE_TOOL_IN_CONTACT = 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() MODE_TORQUE = 8, ///< Set when torque control is active. - END ///< This is not an actual control mode, but used internally to get the number of control modes + MODE_PD_CONTROLLER_JOINT = 9, ///< Set when PD control in joint space is active + MODE_PD_CONTROLLER_TASK = 10, ///< Set when PD control in task space is active + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -63,10 +65,13 @@ class ControlModeTypes { public: // Control modes that require realtime communication - static const inline std::vector REALTIME_CONTROL_MODES = { - ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, - ControlMode::MODE_TORQUE - }; + static const inline std::vector REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ, + ControlMode::MODE_SPEEDJ, + ControlMode::MODE_SPEEDL, + ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE, + ControlMode::MODE_PD_CONTROLLER_JOINT, + ControlMode::MODE_PD_CONTROLLER_TASK }; // Control modes that doesn't require realtime communication static const inline std::vector NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE, diff --git a/include/ur_client_library/control/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h new file mode 100644 index 000000000..5a4a7be95 --- /dev/null +++ b/include/ur_client_library/control/torque_command_controller_parameters.h @@ -0,0 +1,97 @@ +#include "ur_client_library/ur/datatypes.h" +#include "ur_client_library/log.h" + +#include + +namespace urcl +{ +namespace control +{ + +struct PDControllerGains +{ + vector6d_t kp; + vector6d_t kd; +}; + +// UR3 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 560.0, 560.0, 350.0, 163.0, 163.0, 163.0 }, + /*.kd*/ { 47.32, 47.32, 37.42, 25.5, 25.5, 25.5 } +}; + +// UR5 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 900.0, 900.0, 900.0, 500.0, 500.0, 500.0 }, + /*.kd*/ { 60.0, 60.0, 60.0, 44.72, 44.72, 44.72 } +}; + +// UR10 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 1300.0, 1300.0, 900.0, 560.0, 560.0, 560.0 }, + /*.kd*/ { 72.11, 72.11, 60.0, 47.32, 47.32, 47.32 } +}; + +constexpr vector6d_t MAX_UR3_JOINT_TORQUE = { 54.0, 54.0, 28.0, 9.0, 9.0, 9.0 }; + +constexpr vector6d_t MAX_UR5_JOINT_TORQUE = { 150.0, 150.0, 150.0, 28.0, 28.0, 28.0 }; + +constexpr vector6d_t MAX_UR10_JOINT_TORQUE = { 330.0, 330.0, 150.0, 54.0, 54.0, 54.0 }; + +/*! + * \brief Get pd gains for specific robot type + * + * \param robot_type Robot type to get gains for + * + * \returns PD gains for the specific robot type + */ +inline PDControllerGains getPdGainsFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR5: + return UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR10: + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + default: + std::stringstream ss; + ss << "Default gains has not been tuned for robot type " << robotTypeString(robot_type) + << ". Defaulting to UR10 gains."; + URCL_LOG_WARN(ss.str().c_str()); + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + } +} + +/*! + * \brief Get max torques specific robot type + * + * \param robot_type Robot type to get max torque for + * + * \returns max torque for the specific robot type + */ +inline vector6d_t getMaxTorquesFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return MAX_UR3_JOINT_TORQUE; + case RobotType::UR5: + return MAX_UR5_JOINT_TORQUE; + case RobotType::UR10: + return MAX_UR10_JOINT_TORQUE; + case RobotType::UR16: + // Same joints as ur10 + return MAX_UR10_JOINT_TORQUE; + default: + std::stringstream ss; + ss << "Max joint torques not collected for robot type " << robotTypeString(robot_type) + << ". Defaulting to UR10 max joint torques."; + URCL_LOG_WARN(ss.str().c_str()); + return MAX_UR10_JOINT_TORQUE; + } +} + +} // namespace control +} // namespace urcl \ No newline at end of file diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 181e9e617..df0bbcdb2 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -26,6 +26,8 @@ MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 +MODE_PD_CONTROLLER_JOINT = 9 +MODE_PD_CONTROLLER_TASK = 10 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -85,6 +87,8 @@ global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 global friction_compensation_enabled = True +global pd_controller_gains = {{PD_CONTROLLER_GAINS_REPLACE}} +global max_joint_torques = {{MAX_JOINT_TORQUE_REPLACE}} # Global thread variables thread_move = 0 @@ -154,6 +158,31 @@ def terminateProgram(): halt end +### +# @brief Function to clamp each element of a list in between +- clamp value. +# +# @param values array is the list of values to clamp +# @param clampvalues array is the list of clamp values +# +# @returns array of values within +- clamp value +### +def clamp_array(values, clampValues): + if length(values) != length(clampValues): + popup("List of values has different length than list of clamp values.", error = True, blocking = True) + end + ret = values + j = 0 + while j < length(values): + if values[j] > clampValues[j]: + ret[j] = clampValues[j] + elif values[j] < -clampValues[j]: + ret[j] = -clampValues[j] + end + j = j + 1 + end + return ret +end + def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING if targetWithinLimits(cmd_servo_q, q, steptime): @@ -644,6 +673,17 @@ thread servoThreadP(): stopj(STOPJ_ACCELERATION) end +thread PDControlThread(): + while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: + local q_err = cmd_servo_q - get_actual_joint_positions() + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) + torque_command(tau, friction_comp=friction_compensation_enabled) + end + textmsg("PD Control thread ended") + stopj(STOPJ_ACCELERATION) +end + def tool_contact_detection(): # Detect tool contact in the directions that the TCP is moving step_back = tool_contact(direction = get_actual_tcp_speed()) @@ -784,6 +824,12 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_POSE: cmd_servo_q = get_joint_positions() thread_move = run servoThreadP() + elif control_mode == MODE_PD_CONTROLLER_JOINT: + cmd_servo_q = get_joint_positions() + thread_move = run PDControlThread() + elif control_mode == MODE_PD_CONTROLLER_TASK: + cmd_servo_q = get_joint_positions() + thread_move = run PDControlThread() end end @@ -824,6 +870,12 @@ while control_mode > MODE_STOPPED: textmsg("Leaving freedrive mode") end_freedrive_mode() end + elif control_mode == MODE_PD_CONTROLLER_JOINT: + q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(q) + elif control_mode == MODE_PD_CONTROLLER_TASK: + pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(get_inverse_kin(pose, cmd_servo_q)) end # Tool contact is running, but hasn't been detected if tool_contact_running == True and control_mode != MODE_TOOL_IN_CONTACT: diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 6e30579c7..438f3fb1c 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -34,6 +34,8 @@ #include "ur_client_library/ur/ur_driver.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/primary/primary_parser.h" +#include "ur_client_library/control/torque_command_controller_parameters.h" +#include "ur_client_library/helpers.h" #include #include @@ -51,6 +53,8 @@ static const std::string TRAJECTORY_PORT_REPLACE("{{TRAJECTORY_SERVER_PORT_REPLA static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}"); static const std::string FORCE_MODE_SET_DAMPING_REPLACE("{{FORCE_MODE_SET_DAMPING_REPLACE}}"); static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("{{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}"); +static const std::string PD_CONTROLLER_GAINS_REPLACE("{{PD_CONTROLLER_GAINS_REPLACE}}"); +static const std::string MAX_JOINT_TORQUE_REPLACE("{{MAX_JOINT_TORQUE_REPLACE}}"); void UrDriver::init(const UrDriverConfiguration& config) { @@ -146,6 +150,35 @@ void UrDriver::init(const UrDriverConfiguration& config) script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port)); startPrimaryClientCommunication(); + + std::chrono::milliseconds timeout(1000); + try + { + waitFor([this]() { return primary_client_->getConfigurationData() != nullptr; }, timeout); + } + catch (const TimeoutException&) + { + throw TimeoutException("Could not get configuration package within timeout, are you connected to the robot?", + timeout); + } + + const RobotType robot_type = primary_client_->getRobotType(); + + if (prog.find(PD_CONTROLLER_GAINS_REPLACE) != std::string::npos) + { + const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); + std::stringstream ss; + ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + prog.replace(prog.find(PD_CONTROLLER_GAINS_REPLACE), PD_CONTROLLER_GAINS_REPLACE.length(), ss.str()); + } + + if (prog.find(MAX_JOINT_TORQUE_REPLACE) != std::string::npos) + { + std::stringstream ss; + ss << control::getMaxTorquesFromRobotType(robot_type); + prog.replace(prog.find(MAX_JOINT_TORQUE_REPLACE), MAX_JOINT_TORQUE_REPLACE.length(), ss.str()); + } + if (in_headless_mode_) { full_robot_program_ = "stop program\n"; diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 73466cfde..af05b2dcc 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -395,6 +395,24 @@ TEST_F(ReverseIntefaceTest, write_control_mode) expected_control_mode = comm::ControlMode::MODE_UNINITIALIZED; EXPECT_THROW(reverse_interface_->write(&pos, expected_control_mode), UrException); + + expected_control_mode = comm::ControlMode::MODE_TORQUE; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); + + expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_JOINT; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); + + expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_TASK; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); } TEST_F(ReverseIntefaceTest, write_freedrive_control_message) From f45dda1c761d173cfb1a9a8eca32a42b1e0dfaad Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 11:33:22 +0200 Subject: [PATCH 06/23] Added commands to set pd controller gains and maximum joint torques (#342) Co-authored-by: Felix Exner --- doc/architecture/script_command_interface.rst | 23 ++++++ examples/script_command_interface.cpp | 6 ++ .../control/script_command_interface.h | 28 ++++++++ include/ur_client_library/ur/ur_driver.h | 26 +++++++ resources/external_control.urscript | 7 ++ src/control/script_command_interface.cpp | 58 +++++++++++++++ src/ur/ur_driver.cpp | 48 +++++++++++++ tests/test_script_command_interface.cpp | 70 +++++++++++++++++++ 8 files changed, 266 insertions(+) diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index cd82f7c1d..68a35ac10 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -21,6 +21,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun `_ for more information. - ``setFrictionCompensation()``: Set friction compensation for torque command. +- ``setPDControllerGains()``: Set PD gains for the PD control loop running in the external control script. +- ``setMaxJointTorques()``: Set Max joint torques for the PD control loop running in the external control script. Communication protocol ---------------------- @@ -50,6 +52,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 5: startToolContact - 6: endToolContact - 7: setFrictionCompensation + - 8: setPDControllerGains + - 9: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -132,6 +136,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 setPDControllerGains command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 Kp gains for each joint, used in the PD control loop running in the external control script (floating point) + 7-12 Kd gains for each joint, used in the PD control loop running in the external control script (floating point) + ===== ===== + +.. table:: With setMaxJointTorques command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 max_joint_torques for each joint, used to clamp the torques between +-max_joint_torques before applying them to the robot in the PD control loop running in the external control script (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, diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2f..a5e9637d7 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -74,6 +74,12 @@ void sendScriptCommands() run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); run_cmd("Setting friction_compensation variable to `true`", []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); + run_cmd("Setting PD controller gains", []() { + g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, + { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); + }); + run_cmd("Setting max joint torques", + []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } URCL_LOG_INFO("Script command thread finished."); } diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 98eacdcc6..a0bf8f5a8 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -158,6 +158,32 @@ class ScriptCommandInterface : public ReverseInterface */ bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * torque_command function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t* max_joint_torques); + /*! * \brief Returns whether a client/robot is connected to this server. * @@ -196,6 +222,8 @@ 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 + SET_PD_CONTROLLER_GAINS = 8, ///< Set PD controller gains + SET_MAX_JOINT_TORQUES = 9, ///< Set max joint torques }; bool client_connected_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 58ba23f69..4c20c1f8a 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -725,6 +725,32 @@ class UrDriver */ bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * torque_command function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t& max_joint_torques); + /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index df0bbcdb2..56892fb28 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -53,6 +53,8 @@ END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 +SET_PD_CONTROLLER_GAINS = 8 +SET_MAX_JOINT_TORQUES = 9 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -757,6 +759,11 @@ thread script_commands(): else: friction_compensation_enabled = True end + elif command == SET_PD_CONTROLLER_GAINS: + pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] + pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + elif command == SET_MAX_JOINT_TORQUES: + max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end end end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 0a4363494..a9dfc8059 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -245,6 +245,64 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) +{ + const int message_length = 13; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PD_CONTROLLER_GAINS)); + b_pos += append(b_pos, val); + + for (auto const& p_gain : *kp) + { + val = htobe32(static_cast(round(p_gain * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + for (auto const& d_gain : *kd) + { + val = htobe32(static_cast(round(d_gain * 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::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) +{ + const int message_length = 7; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_MAX_JOINT_TORQUES)); + b_pos += append(b_pos, val); + + for (auto const& max_torque : *max_joint_torques) + { + val = htobe32(static_cast(round(max_torque * 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_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 438f3fb1c..0c096af6c 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -573,6 +573,54 @@ bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) } } +bool UrDriver::setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd) +{ + if (!std::all_of(kp.begin(), kp.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kp should be larger than zero"); + } + + if (!std::all_of(kd.begin(), kd.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kd should be larger than zero"); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setPDControllerGains(&kp, &kd); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set PD Controller gains."); + return 0; + } +} + +bool UrDriver::setMaxJointTorques(const urcl::vector6d_t& max_joint_torques) +{ + const urcl::vector6d_t max_torques_for_robot_type = + control::getMaxTorquesFromRobotType(primary_client_->getRobotType()); + if (!std::equal(max_joint_torques.begin(), max_joint_torques.end(), max_torques_for_robot_type.begin(), + [](double v1, double v2) { return v1 <= v2 && v1 >= 0.0; })) + { + std::stringstream ss; + ss << "The max joint torques should be smaller or equal to the maximum joint torques for the robot type and larger " + "than zero. Provided max joint torques " + << max_joint_torques << " and maximum joint torques for the robot type " << max_torques_for_robot_type; + throw InvalidRange(ss.str().c_str()); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setMaxJointTorques(&max_joint_torques); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set max joint torques."); + 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 10115bb1a..26ff6077f 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -421,6 +421,76 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation) EXPECT_EQ(message_sum, expected_message_sum); } +TEST_F(ScriptCommandInterfaceTest, test_set_pd_controller_gains) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t kp = { 220.2, 220.2, 300.0, 10.32, 10.32, 10.32 }; + urcl::vector6d_t kd = { 29.68, 29.68, 35.0, 6.4, 6.4, 6.4 }; + script_command_interface_->setPDControllerGains(&kp, &kd); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 8 is set PD controller gains + int32_t expected_command = 8; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& p_gain : kp) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(p_gain, received_gain); + message_idx = message_idx + 1; + } + + for (auto& d_gain : kd) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(d_gain, received_gain); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_max_joint_torques) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t max_joint_torques = { 100.0, 150.0, 21.2, 10.32, 10.32, 10.32 }; + script_command_interface_->setMaxJointTorques(&max_joint_torques); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 9 is set max joint torques + int32_t expected_command = 9; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& max_torque : max_joint_torques) + { + const double received_max_torque = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(max_torque, received_max_torque); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, 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); From 46c1dd613561f58e9c56351f224c89120f4b07d5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Jul 2025 13:56:37 +0200 Subject: [PATCH 07/23] Add torque_command statement only on supported software versions (#348) This way all non-torque-control-related things should work on software versions not supporting torque control --- examples/pd_controller_example.cpp | 11 +++++++++ examples/script_command_interface.cpp | 1 + .../control/script_command_interface.h | 20 ++++++++++++++++ resources/external_control.urscript | 24 +++++++++++++++++++ src/control/script_command_interface.cpp | 23 +++++++++++++++++- src/ur/ur_driver.cpp | 12 +++++++++- 6 files changed, 89 insertions(+), 2 deletions(-) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp index a959a4e7b..4cdbb2568 100644 --- a/examples/pd_controller_example.cpp +++ b/examples/pd_controller_example.cpp @@ -185,6 +185,17 @@ int main(int argc, char* argv[]) return 1; } + { + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) + { + URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", + robot_version.toString().c_str()); + return 0; + } + } + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); URCL_LOG_INFO("Move the robot to initial position"); diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index a5e9637d7..000a1c619 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -78,6 +78,7 @@ void sendScriptCommands() g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); }); + // The following will have no effect on PolyScope < 5.23 / 10.10 run_cmd("Setting max joint torques", []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index a13e4aaf5..8decb5568 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -31,6 +31,7 @@ #include "ur_client_library/control/reverse_interface.h" #include "ur_client_library/ur/tool_communication.h" +#include "ur_client_library/ur/version_information.h" namespace urcl { @@ -235,6 +236,25 @@ class ScriptCommandInterface : public ReverseInterface SET_MAX_JOINT_TORQUES = 9, ///< Set max joint torques }; + /*! + * \brief Checks if the robot version is higher than the minimum required version for Polyscope 5 + * or Polyscope X. + * + * If the robot version is lower than the minimum required version, this function + * will log a warning message. + * In case of a PolyScope 5 robot, the robot's software version will be checked against \p + * min_polyscope5, and in case of a PolyScope X robot, it will be checked against \p + * min_polyscopeX. + * + * \param min_polyscope5 Minimum required version for PolyScope 5 + * \param min_polyscopeX Minimum required version for PolyScope X + * \param command_name Name of the command being checked, used for logging + * + * \returns True if the robot version is higher than the versions provided, false otherwise. + */ + bool robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, + const VersionInformation& min_polyscopeX, const std::string& command_name); + bool client_connected_; static const int MAX_MESSAGE_LENGTH = 28; diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 75005b471..965b5173f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -270,7 +270,17 @@ thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: torque = cmd_torque + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} torque_command(torque, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} + torque_command(torque, friction_comp=friction_compensation_enabled) + {% else %} + popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} end textmsg("ExternalControl: torque thread ended") stopj(STOPJ_ACCELERATION) @@ -678,9 +688,21 @@ end thread PDControlThread(): while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: local q_err = cmd_servo_q - get_actual_joint_positions() + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) + torque_command(tau, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() tau = clamp_array(tau, max_joint_torques) torque_command(tau, friction_comp=friction_compensation_enabled) + {% else %} + popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} end textmsg("PD Control thread ended") stopj(STOPJ_ACCELERATION) @@ -760,8 +782,10 @@ thread script_commands(): friction_compensation_enabled = True end elif command == SET_PD_CONTROLLER_GAINS: + {% if ROBOT_SOFTWARE_VERSION > v5.10.0 %} pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + {% endif %} elif command == SET_MAX_JOINT_TORQUES: max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 7435044f2..673be8cb5 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -228,6 +228,8 @@ bool ScriptCommandInterface::endToolContact() bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -251,6 +253,8 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 13; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -283,6 +287,8 @@ bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, co bool ScriptCommandInterface::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 7; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -356,6 +362,21 @@ void ScriptCommandInterface::messageCallback(const socket_t filedescriptor, char nbytesrecv); } } +bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, + const VersionInformation& min_polyscopeX, + const std::string& command_name) +{ + if (robot_software_version_ < min_polyscope5 || + (robot_software_version_.major > 5 && robot_software_version_ < min_polyscopeX)) + { + URCL_LOG_WARN("%s is only available for robots with PolyScope %s / %s or " + "later. This robot's version is %s. This command will have no effect.", + command_name.c_str(), min_polyscope5.toString().c_str(), min_polyscopeX.toString().c_str(), + robot_software_version_.toString().c_str()); + return false; + } + return true; +} } // namespace control -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e670b4f5b..b0b157cd8 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -147,13 +147,23 @@ void UrDriver::init(const UrDriverConfiguration& config) const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); std::stringstream pd_gains_ss; - pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + if (robot_version_ < urcl::VersionInformation::fromString("5.10.0")) + { + // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. + pd_gains_ss << 0; + } + else + { + pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + } data[PD_CONTROLLER_GAINS_REPLACE] = pd_gains_ss.str(); std::stringstream max_torques_ss; max_torques_ss << control::getMaxTorquesFromRobotType(robot_type); data[MAX_JOINT_TORQUE_REPLACE] = max_torques_ss.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); + script_reader_.reset(new control::ScriptReader()); std::string prog = script_reader_->readScriptFile(config.script_file, data); From 59375edf45b8f8116758e28bf619bdad545e0a63 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 9 Jul 2025 10:03:36 +0200 Subject: [PATCH 08/23] Add torque control example (#353) Add an example using direct torque control. --- doc/examples.rst | 1 + doc/examples/torque_control.rst | 80 ++++++++++++++++++ examples/CMakeLists.txt | 4 + examples/torque_control.cpp | 145 ++++++++++++++++++++++++++++++++ 4 files changed, 230 insertions(+) create mode 100644 doc/examples/torque_control.rst create mode 100644 examples/torque_control.cpp diff --git a/doc/examples.rst b/doc/examples.rst index a2a362800..9e37daf6d 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -27,5 +27,6 @@ may be running forever until manually stopped. examples/script_sender examples/spline_example examples/tool_contact_example + examples/torque_control examples/trajectory_point_interface examples/ur_driver diff --git a/doc/examples/torque_control.rst b/doc/examples/torque_control.rst new file mode 100644 index 000000000..36c606ec4 --- /dev/null +++ b/doc/examples/torque_control.rst @@ -0,0 +1,80 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/torque_control.rst + +.. _torque_control_example: + +Torque control example +====================== + +In the ``examples`` subfolder you will find a minimal example for commanding torques to the robot. +It moves the robot in the 5th axis back and forth, while reading the joint positions. To run it +make sure to + +* have an instance of a robot controller / URSim running at the configured IP address (or adapt the + address to your needs) +* have PolyScope version 5.23.0 / 10.10.0 or later installed on the robot. +* run it from the package's main folder (the one where the README.md file is stored), as for + simplicity reasons it doesn't use any sophisticated method to locate the required files. + +This page will walk you through the `full_driver.cpp +`_ +example. + +Initialization +-------------- + +At first, we create a ``ExampleRobotWrapper`` object giving it the robot's IP address, script file and RTDE +recipes. + +.. literalinclude:: ../../examples/torque_control.cpp + :language: c++ + :caption: examples/torque_control.cpp + :linenos: + :lineno-match: + :start-at: bool headless_mode = true; + :end-at: // --------------- INITIALIZATION END ------------------- + +.. note:: + This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the torque_control + mode which is only available in these versions and later. If you try to run it on an older + software version, this example will print an error and exit. + +Robot control loop +------------------ + +This example reads the robot's joint positions, commands a torque for the 5th axis and sends that +back as a joint command for the next cycle. This way, the robot will move its wrist first until a +positive limit and then back to 0. + +To read the joint data, the driver's RTDE client is used: + + +.. literalinclude:: ../../examples/torque_control.cpp + :language: c++ + :caption: examples/torque_control.cpp + :linenos: + :lineno-match: + :start-at: // Once RTDE communication is started + :end-before: // Open loop control + +The first read operation will initialize the target buffer with the current robot position. Next, +the target joint torques are set based on the current joint positions: + + +.. literalinclude:: ../../examples/torque_control.cpp + :language: c++ + :caption: examples/torque_control.cpp + :linenos: + :lineno-match: + :start-at: // Open loop control + :end-at: target_torques[JOINT_INDEX] = cmd_torque; + +To send the control command, the robot's :ref:`reverse_interface` is used via the +``writeJointCommand()`` function: + +.. literalinclude:: ../../examples/torque_control.cpp + :language: c++ + :caption: examples/torque_control.cpp + :linenos: + :lineno-match: + :start-at: // Setting the RobotReceiveTimeout + :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f13be169a..8714d5b5b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -48,6 +48,10 @@ add_executable(script_command_interface_example script_command_interface.cpp) target_link_libraries(script_command_interface_example ur_client_library::urcl) +add_executable(torque_control_example + torque_control.cpp) +target_link_libraries(torque_control_example ur_client_library::urcl) + add_executable(trajectory_point_interface_example trajectory_point_interface.cpp) target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) diff --git a/examples/torque_control.cpp b/examples/torque_control.cpp new file mode 100644 index 000000000..c8922bf1f --- /dev/null +++ b/examples/torque_control.cpp @@ -0,0 +1,145 @@ +// -- 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 + +// In a real-world example it would be better to get those values from command line parameters / a +// better configuration system such as Boost.Program_options +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 size_t JOINT_INDEX = 5; // Joint index to control, in this case joint 6 (index 5) + +std::unique_ptr g_my_robot; +urcl::vector6d_t g_joint_positions; + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + // Torque control requires Software version 5.23 / 10.10 or higher. Error and exit on older + // software versions. + { + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) + { + URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", + robot_version.toString().c_str()); + return 0; + } + } + // --------------- INITIALIZATION END ------------------- + + double cmd_torque = 2.0; // Target torque [Nm] for joint 6 + bool passed_negative_part = false; + bool passed_positive_part = false; + URCL_LOG_INFO("Start moving the robot"); + urcl::vector6d_t target_torques = { 2.0, 0, 0, 0, 0, 0 }; + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + g_my_robot->getUrDriver()->startRTDECommunication(); + while (!(passed_positive_part && passed_negative_part)) + { + // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the + // robot will effectively be in charge of setting the frequency of this loop. + // In a real-world application this thread should be scheduled with real-time priority in order + // to ensure that this is called in time. + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); + if (!data_pkg) + { + URCL_LOG_WARN("Could not get fresh data package from robot"); + return 1; + } + // Read current joint positions from robot data + if (!data_pkg->getData("actual_q", g_joint_positions)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + // Open loop control. The target is incremented with a constant each control loop + if (passed_positive_part == false) + { + if (g_joint_positions[JOINT_INDEX] >= 2) + { + passed_positive_part = true; + cmd_torque = -2.0; + } + } + else if (passed_negative_part == false) + { + if (g_joint_positions[JOINT_INDEX] <= 0) + { + cmd_torque = 2.0; + passed_negative_part = true; + } + } + target_torques[JOINT_INDEX] = cmd_torque; + + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more + // reliable on non-realtime systems. Use with caution in productive applications. Having it + // this high means that the robot will continue a motion for this given time if no new command + // is sent / the connection is interrupted. + bool ret = g_my_robot->getUrDriver()->writeJointCommand(target_torques, urcl::comm::ControlMode::MODE_TORQUE, + urcl::RobotReceiveTimeout::millisec(100)); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + return 1; + } + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + } + g_my_robot->getUrDriver()->stopControl(); + URCL_LOG_INFO("Movement done"); + return 0; +} From c0d4ee575956c4f32f0fa402723ba62c8e950c66 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 9 Jul 2025 10:47:53 +0200 Subject: [PATCH 09/23] Add a specific clang-tidy check And move clang-tidy check out of the industrial_ci run --- .github/workflows/clang-tidy-check.yml | 27 ++++++++++++++++++++++++++ .github/workflows/industrial-ci.yml | 2 -- 2 files changed, 27 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/clang-tidy-check.yml diff --git a/.github/workflows/clang-tidy-check.yml b/.github/workflows/clang-tidy-check.yml new file mode 100644 index 000000000..ae33b61a3 --- /dev/null +++ b/.github/workflows/clang-tidy-check.yml @@ -0,0 +1,27 @@ +name: clang-tidy-review + +# You can be more specific, but it currently only works on pull requests +on: [pull_request] + +jobs: + clang-tidy: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + + - uses: ZedThree/clang-tidy-review@v0.21.0 + id: review + with: + build_dir: build + cmake_command: cmake . -DCMAKE_EXPORT_COMPILE_COMMANDS=on + config_file: .clang-tidy + clang_tidy_checks: "" + + # Uploads an artefact containing clang_fixes.json + - uses: ZedThree/clang-tidy-review/upload@v0.21.0 + id: upload-review + + # If there are any comments, fail the check + - if: steps.review.outputs.total_comments > 0 + run: exit 1 diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 78cf375b6..309207169 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -41,5 +41,3 @@ jobs: DOWNSTREAM_WORKSPACE: ${{matrix.ROS_DISTRO.DOWNSTREAM_WORKSPACE}} ROS_DISTRO: ${{matrix.ROS_DISTRO.NAME}} ROS_REPO: ${{matrix.ROS_REPO}} - CLANG_TIDY: pedantic - CLANG_TIDY_ARGS: '--extra-arg=-std=c++17' # needed for Humble From b8a63cb9329cd3732bf232744ed4768cbbf563f7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 9 Jul 2025 11:04:11 +0200 Subject: [PATCH 10/23] Revert "Add a specific clang-tidy check" This reverts commit c0d4ee575956c4f32f0fa402723ba62c8e950c66. --- .github/workflows/clang-tidy-check.yml | 27 -------------------------- .github/workflows/industrial-ci.yml | 2 ++ 2 files changed, 2 insertions(+), 27 deletions(-) delete mode 100644 .github/workflows/clang-tidy-check.yml diff --git a/.github/workflows/clang-tidy-check.yml b/.github/workflows/clang-tidy-check.yml deleted file mode 100644 index ae33b61a3..000000000 --- a/.github/workflows/clang-tidy-check.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: clang-tidy-review - -# You can be more specific, but it currently only works on pull requests -on: [pull_request] - -jobs: - clang-tidy: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - uses: ZedThree/clang-tidy-review@v0.21.0 - id: review - with: - build_dir: build - cmake_command: cmake . -DCMAKE_EXPORT_COMPILE_COMMANDS=on - config_file: .clang-tidy - clang_tidy_checks: "" - - # Uploads an artefact containing clang_fixes.json - - uses: ZedThree/clang-tidy-review/upload@v0.21.0 - id: upload-review - - # If there are any comments, fail the check - - if: steps.review.outputs.total_comments > 0 - run: exit 1 diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index 309207169..78cf375b6 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -41,3 +41,5 @@ jobs: DOWNSTREAM_WORKSPACE: ${{matrix.ROS_DISTRO.DOWNSTREAM_WORKSPACE}} ROS_DISTRO: ${{matrix.ROS_DISTRO.NAME}} ROS_REPO: ${{matrix.ROS_REPO}} + CLANG_TIDY: pedantic + CLANG_TIDY_ARGS: '--extra-arg=-std=c++17' # needed for Humble From 804e75abdce641473638aa64fe144a5650a60f8c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 9 Jul 2025 11:46:10 +0200 Subject: [PATCH 11/23] REVERT_ME: Switch version checks to 5.22.0 as that't the beta version (#356) Temporarily relax the version check for torque control features. The torque control features will be available from 5.23.0 on. However, as we are currently running a public beta program, where the reported software version is 5.22.0 we should relax the version check until 5.23.0 is actually released. --- examples/pd_controller_example.cpp | 2 +- examples/torque_control.cpp | 3 ++- resources/external_control.urscript | 4 ++-- src/control/script_command_interface.cpp | 9 ++++++--- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp index 4cdbb2568..f689e6ec0 100644 --- a/examples/pd_controller_example.cpp +++ b/examples/pd_controller_example.cpp @@ -187,7 +187,7 @@ int main(int argc, char* argv[]) { auto robot_version = g_my_robot->getUrDriver()->getVersion(); - if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + if (robot_version < urcl::VersionInformation::fromString("5.22.0") || // ToDo: Increase to 5.23.0 once released (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) { URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", diff --git a/examples/torque_control.cpp b/examples/torque_control.cpp index c8922bf1f..ae0008b13 100644 --- a/examples/torque_control.cpp +++ b/examples/torque_control.cpp @@ -67,7 +67,8 @@ int main(int argc, char* argv[]) // software versions. { auto robot_version = g_my_robot->getUrDriver()->getVersion(); - if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + // ToDo: Increase to 5.23.0 once released + if (robot_version < urcl::VersionInformation::fromString("5.22.0") || (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) { URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 965b5173f..7b56390c8 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -270,7 +270,7 @@ thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: torque = cmd_torque - {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION >= v5.22.0 %} # ToDo: Increase to 5.23.0 once released {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} torque_command(torque, friction_comp=friction_compensation_enabled) {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} @@ -688,7 +688,7 @@ end thread PDControlThread(): while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: local q_err = cmd_servo_q - get_actual_joint_positions() - {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION >= v5.22.0 %} # ToDo: Increase to 5.23.0 once released {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() tau = clamp_array(tau, max_joint_torques) diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 673be8cb5..ac519545f 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -228,7 +228,8 @@ bool ScriptCommandInterface::endToolContact() bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) { - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + // ToDo: Increase to 5.23.0 once released + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; @@ -253,7 +254,8 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) { - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + // ToDo: Increase to 5.23.0 once released + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 13; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; @@ -287,7 +289,8 @@ bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, co bool ScriptCommandInterface::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) { - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + // ToDo: Increase to 5.23.0 once released + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 7; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; From 2695b9a38ea4e30ccfbb37a7023e50c5a14018e3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 17 Jul 2025 11:14:54 +0200 Subject: [PATCH 12/23] Define torque abs value only once (#361) To make it easier to change the example for manual testing, let's define the target torque's absolute value in one place only. --- examples/torque_control.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/torque_control.cpp b/examples/torque_control.cpp index ae0008b13..ae885b0bb 100644 --- a/examples/torque_control.cpp +++ b/examples/torque_control.cpp @@ -78,11 +78,12 @@ int main(int argc, char* argv[]) } // --------------- INITIALIZATION END ------------------- - double cmd_torque = 2.0; // Target torque [Nm] for joint 6 + const double torque_abs = 2.5; + double cmd_torque = torque_abs; // Target torque [Nm] for joint 6 bool passed_negative_part = false; bool passed_positive_part = false; URCL_LOG_INFO("Start moving the robot"); - urcl::vector6d_t target_torques = { 2.0, 0, 0, 0, 0, 0 }; + urcl::vector6d_t target_torques = { 0, 0, 0, 0, 0, 0 }; // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main @@ -114,14 +115,14 @@ int main(int argc, char* argv[]) if (g_joint_positions[JOINT_INDEX] >= 2) { passed_positive_part = true; - cmd_torque = -2.0; + cmd_torque = -torque_abs; } } else if (passed_negative_part == false) { if (g_joint_positions[JOINT_INDEX] <= 0) { - cmd_torque = 2.0; + cmd_torque = torque_abs; passed_negative_part = true; } } From b1036b557ea1ceb1b1c30f4ecfb444c776dbe694 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 17 Jul 2025 12:05:10 +0200 Subject: [PATCH 13/23] Fix naming scheme in pd_controller_example (#357) The function names didn't match this repo's style guide. --- examples/pd_controller_example.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp index f689e6ec0..d9a8339d6 100644 --- a/examples/pd_controller_example.cpp +++ b/examples/pd_controller_example.cpp @@ -50,7 +50,7 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; std::unique_ptr g_my_robot; -void signal_callback_handler(int signum) +void signalCallbackHandler(int signum) { std::cout << "Caught signal " << signum << std::endl; if (g_my_robot != nullptr) @@ -68,7 +68,7 @@ struct DataStorage std::vector actual; std::vector time; - void write_to_file(const std::string& filename) + void writeToFile(const std::string& filename) { std::fstream output(filename, std::ios::out); output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," @@ -90,8 +90,8 @@ struct DataStorage } }; -bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_name, - const comm::ControlMode control_mode, const urcl::vector6d_t& amplitude) +bool pdControlLoop(DataStorage& data_storage, const std::string& actual_data_name, const comm::ControlMode control_mode, + const urcl::vector6d_t& amplitude) { const int32_t running_time = 13; double time = 0.0; @@ -165,7 +165,7 @@ bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_n int main(int argc, char* argv[]) { // This will make sure that we stop controlling the robot if the user presses ctrl-c - signal(SIGINT, signal_callback_handler); + signal(SIGINT, signalCallbackHandler); urcl::setLogLevel(urcl::LogLevel::INFO); @@ -209,8 +209,8 @@ int main(int argc, char* argv[]) g_my_robot->getUrDriver()->startRTDECommunication(); URCL_LOG_INFO("Start controlling the robot using the PD controller"); const bool completed_joint_control = - pd_control_loop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, - { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); + pdControlLoop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, + { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); if (!completed_joint_control) { URCL_LOG_ERROR("Didn't complete pd control in joint space"); @@ -229,8 +229,8 @@ int main(int argc, char* argv[]) g_my_robot->stopConsumingRTDEData(); URCL_LOG_INFO("Start controlling the robot using the PD controller with task space targets"); const bool completed_task_control = - pd_control_loop(task_controller_storage, "actual_TCP_pose", comm::ControlMode::MODE_PD_CONTROLLER_TASK, - { 0.05, 0.05, 0.05, 0.02, 0.02, 0.02 }); + pdControlLoop(task_controller_storage, "actual_TCP_pose", comm::ControlMode::MODE_PD_CONTROLLER_TASK, + { 0.05, 0.05, 0.05, 0.02, 0.02, 0.02 }); if (!completed_task_control) { URCL_LOG_ERROR("Didn't complete PD control in task space"); From 1ee3eab5499b74093ef824d14c3725d54a5fa465 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 11 Sep 2025 09:11:55 +0200 Subject: [PATCH 14/23] Rename torque_command to direct_torque (#362) The script command has been renamed in the controller. --- examples/torque_control.cpp | 14 ++++++++++++++ resources/external_control.urscript | 20 ++++++++++---------- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/examples/torque_control.cpp b/examples/torque_control.cpp index ae885b0bb..f8c789e9b 100644 --- a/examples/torque_control.cpp +++ b/examples/torque_control.cpp @@ -54,6 +54,13 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } + // Parse how many seconds to run + auto second_to_run = std::chrono::seconds(0); + if (argc > 2) + { + second_to_run = std::chrono::seconds(std::stoi(argv[2])); + } + bool headless_mode = true; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); @@ -89,6 +96,7 @@ int main(int argc, char* argv[]) // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. g_my_robot->getUrDriver()->startRTDECommunication(); + auto start_time = std::chrono::system_clock::now(); while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the @@ -140,6 +148,12 @@ int main(int argc, char* argv[]) return 1; } URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + if (second_to_run.count() > 0 && (std::chrono::system_clock::now() - start_time) > second_to_run) + { + URCL_LOG_WARN("Time limit reached, stopping movement. This is expected on a simualted robot, as it doesn't move " + "to torque commands."); + break; + } } g_my_robot->getUrDriver()->stopControl(); URCL_LOG_INFO("Movement done"); diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 7b56390c8..910702033 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -270,13 +270,13 @@ thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: torque = cmd_torque - {% if ROBOT_SOFTWARE_VERSION >= v5.22.0 %} # ToDo: Increase to 5.23.0 once released + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} - torque_command(torque, friction_comp=friction_compensation_enabled) - {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} - torque_command(torque, friction_comp=friction_compensation_enabled) + direct_torque(torque, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} + direct_torque(torque, friction_comp=friction_compensation_enabled) {% else %} - popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) {% endif %} {% else %} popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) @@ -688,17 +688,17 @@ end thread PDControlThread(): while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: local q_err = cmd_servo_q - get_actual_joint_positions() - {% if ROBOT_SOFTWARE_VERSION >= v5.22.0 %} # ToDo: Increase to 5.23.0 once released + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() tau = clamp_array(tau, max_joint_torques) - torque_command(tau, friction_comp=friction_compensation_enabled) - {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} + direct_torque(tau, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() tau = clamp_array(tau, max_joint_torques) - torque_command(tau, friction_comp=friction_compensation_enabled) + direct_torque(tau, friction_comp=friction_compensation_enabled) {% else %} - popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) {% endif %} {% else %} popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) From 5cf9064413fd24d89eb54fa3d612b29a39ec2961 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 19 Sep 2025 16:22:09 +0200 Subject: [PATCH 15/23] Remove PD controller for now It is still planned to add the PD controller. However, we would like it not to postpone merging torque_control to master. --- doc/architecture/script_command_interface.rst | 23 -- examples/CMakeLists.txt | 4 - examples/pd_controller_example.cpp | 252 ------------------ examples/script_command_interface.cpp | 7 - include/ur_client_library/comm/control_mode.h | 15 +- .../control/script_command_interface.h | 28 -- .../torque_command_controller_parameters.h | 97 ------- include/ur_client_library/ur/ur_driver.h | 26 -- resources/external_control.urscript | 73 ----- src/control/script_command_interface.cpp | 64 ----- src/ur/ur_driver.cpp | 70 ----- tests/test_reverse_interface.cpp | 12 - tests/test_script_command_interface.cpp | 70 ----- 13 files changed, 5 insertions(+), 736 deletions(-) delete mode 100644 examples/pd_controller_example.cpp delete mode 100644 include/ur_client_library/control/torque_command_controller_parameters.h diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 68a35ac10..cd82f7c1d 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -21,8 +21,6 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun `_ for more information. - ``setFrictionCompensation()``: Set friction compensation for torque command. -- ``setPDControllerGains()``: Set PD gains for the PD control loop running in the external control script. -- ``setMaxJointTorques()``: Set Max joint torques for the PD control loop running in the external control script. Communication protocol ---------------------- @@ -52,8 +50,6 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 5: startToolContact - 6: endToolContact - 7: setFrictionCompensation - - 8: setPDControllerGains - - 9: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -136,25 +132,6 @@ 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 setPDControllerGains command - :widths: auto - - ===== ===== - index meaning - ===== ===== - 1-6 Kp gains for each joint, used in the PD control loop running in the external control script (floating point) - 7-12 Kd gains for each joint, used in the PD control loop running in the external control script (floating point) - ===== ===== - -.. table:: With setMaxJointTorques command - :widths: auto - - ===== ===== - index meaning - ===== ===== - 1-6 max_joint_torques for each joint, used to clamp the torques between +-max_joint_torques before applying them to the robot in the PD control loop running in the external control script (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, diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 8714d5b5b..be72a0489 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -59,7 +59,3 @@ 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(pd_controller_example -pd_controller_example.cpp) -target_link_libraries(pd_controller_example ur_client_library::urcl) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp deleted file mode 100644 index d9a8339d6..000000000 --- a/examples/pd_controller_example.cpp +++ /dev/null @@ -1,252 +0,0 @@ -// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- - -// -- 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 "ur_client_library/example_robot_wrapper.h" -#include "ur_client_library/ur/dashboard_client.h" -#include "ur_client_library/ur/ur_driver.h" -#include "ur_client_library/types.h" -#include "ur_client_library/ur/instruction_executor.h" - -#include -#include -#include -#include - -using namespace urcl; - -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; - -void signalCallbackHandler(int signum) -{ - std::cout << "Caught signal " << signum << std::endl; - if (g_my_robot != nullptr) - { - // Stop control of the robot - g_my_robot->getUrDriver()->stopControl(); - } - // Terminate program - exit(signum); -} - -struct DataStorage -{ - std::vector target; - std::vector actual; - std::vector time; - - void writeToFile(const std::string& filename) - { - std::fstream output(filename, std::ios::out); - output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," - "actual3,actual4,actual5\n"; - for (size_t i = 0; i < time.size(); ++i) - { - output << time[i]; - for (auto& t : target[i]) - { - output << "," << t; - } - for (auto& a : actual[i]) - { - output << "," << a; - } - output << "\n"; - } - output.close(); - } -}; - -bool pdControlLoop(DataStorage& data_storage, const std::string& actual_data_name, const comm::ControlMode control_mode, - const urcl::vector6d_t& amplitude) -{ - const int32_t running_time = 13; - double time = 0.0; - - // Reserve space for expected amount of data - data_storage.actual.reserve(running_time * 500); - data_storage.target.reserve(running_time * 500); - data_storage.time.reserve(running_time * 500); - - urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - - bool first_pass = true; - while (time < running_time) - { - const auto t_start = std::chrono::high_resolution_clock::now(); - // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the - // robot will effectively be in charge of setting the frequency of this loop. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (!data_pkg) - { - URCL_LOG_WARN("Could not get fresh data package from robot"); - return false; - } - // Read current joint positions from robot data - if (!data_pkg->getData(actual_data_name, actual)) - { - // This throwing should never happen unless misconfigured - std::string error_msg = "Did not find" + actual_data_name + "in data sent from robot. This should not happen!"; - throw std::runtime_error(error_msg); - } - - if (first_pass) - { - target = actual; - start = actual; - for (size_t i = 0; i < start.size(); ++i) - { - start[i] = start[i] - amplitude[i]; - } - first_pass = false; - } - - for (size_t i = 0; i < target.size(); ++i) - { - target[i] = start[i] + amplitude[i] * cos(time); - } - - // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more - // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = g_my_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100)); - if (!ret) - { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); - return false; - } - - // Increment time and log data - const auto t_end = std::chrono::high_resolution_clock::now(); - const double elapsed_time_ms = std::chrono::duration(t_end - t_start).count() / 1000; - time = time + elapsed_time_ms; - data_storage.actual.push_back(actual); - data_storage.target.push_back(target); - data_storage.time.push_back(time); - } - - return true; -} - -int main(int argc, char* argv[]) -{ - // This will make sure that we stop controlling the robot if the user presses ctrl-c - signal(SIGINT, signalCallbackHandler); - - urcl::setLogLevel(urcl::LogLevel::INFO); - - // Parse the ip arguments if given - std::string robot_ip = DEFAULT_ROBOT_IP; - if (argc > 1) - { - robot_ip = std::string(argv[1]); - } - - const bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode); - - if (!g_my_robot->isHealthy()) - { - URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); - return 1; - } - - { - auto robot_version = g_my_robot->getUrDriver()->getVersion(); - if (robot_version < urcl::VersionInformation::fromString("5.22.0") || // ToDo: Increase to 5.23.0 once released - (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) - { - URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", - robot_version.toString().c_str()); - return 0; - } - } - - auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); - - URCL_LOG_INFO("Move the robot to initial position"); - instruction_executor->moveJ(urcl::vector6d_t{ 0, -1.67, -0.65, -1.59, 1.61, 5.09 }, 0.5, 0.2, 5); - - DataStorage joint_controller_storage; - - // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main - // loop. - g_my_robot->getUrDriver()->startRTDECommunication(); - URCL_LOG_INFO("Start controlling the robot using the PD controller"); - const bool completed_joint_control = - pdControlLoop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, - { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); - if (!completed_joint_control) - { - URCL_LOG_ERROR("Didn't complete pd control in joint space"); - g_my_robot->getUrDriver()->stopControl(); - return 1; - } - - // Read the rtde packages in the background while moving to initial position for PD control in task space - g_my_robot->startConsumingRTDEData(); - - URCL_LOG_INFO("Move the robot to initial position for task space PD control"); - instruction_executor->moveJ(urcl::vector6d_t{ 0.03, -0.98, -1.83, -1.70, 1.37, 5.12 }, 0.5, 0.2, 5); - - DataStorage task_controller_storage; - - g_my_robot->stopConsumingRTDEData(); - URCL_LOG_INFO("Start controlling the robot using the PD controller with task space targets"); - const bool completed_task_control = - pdControlLoop(task_controller_storage, "actual_TCP_pose", comm::ControlMode::MODE_PD_CONTROLLER_TASK, - { 0.05, 0.05, 0.05, 0.02, 0.02, 0.02 }); - if (!completed_task_control) - { - URCL_LOG_ERROR("Didn't complete PD control in task space"); - g_my_robot->getUrDriver()->stopControl(); - return 1; - } - - // Consume data in the background while writing the logged data to a file - g_my_robot->startConsumingRTDEData(); - g_my_robot->getUrDriver()->stopControl(); - URCL_LOG_INFO("Movement done"); - - // Write data to files, can be used for benchmarking the controller (commented out as default) - // joint_controller_storage.write_to_file("examples/pd_controller_joint.csv"); - // task_controller_storage.write_to_file("examples/pd_controller_task.csv"); - g_my_robot->stopConsumingRTDEData(); - - return 0; -} diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 000a1c619..6ada63f2f 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -74,13 +74,6 @@ void sendScriptCommands() run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); run_cmd("Setting friction_compensation variable to `true`", []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); - run_cmd("Setting PD controller gains", []() { - g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, - { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); - }); - // The following will have no effect on PolyScope < 5.23 / 10.10 - run_cmd("Setting max joint torques", - []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } URCL_LOG_INFO("Script command thread finished."); } diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index f1da507ac..51156da6f 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -53,9 +53,7 @@ enum class ControlMode : int32_t MODE_TOOL_IN_CONTACT = 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() MODE_TORQUE = 8, ///< Set when torque control is active. - MODE_PD_CONTROLLER_JOINT = 9, ///< Set when PD control in joint space is active - MODE_PD_CONTROLLER_TASK = 10, ///< Set when PD control in task space is active - END ///< This is not an actual control mode, but used internally to get the number of control modes + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -65,13 +63,10 @@ class ControlModeTypes { public: // Control modes that require realtime communication - static const inline std::vector REALTIME_CONTROL_MODES = { ControlMode::MODE_SERVOJ, - ControlMode::MODE_SPEEDJ, - ControlMode::MODE_SPEEDL, - ControlMode::MODE_POSE, - ControlMode::MODE_TORQUE, - ControlMode::MODE_PD_CONTROLLER_JOINT, - ControlMode::MODE_PD_CONTROLLER_TASK }; + static const inline std::vector REALTIME_CONTROL_MODES = { + ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, + ControlMode::MODE_TORQUE + }; // Control modes that doesn't require realtime communication static const inline std::vector NON_REALTIME_CONTROL_MODES = { ControlMode::MODE_IDLE, diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 8decb5568..0c4ebc6ed 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -168,32 +168,6 @@ class ScriptCommandInterface : public ReverseInterface */ bool setFrictionCompensation(const bool friction_compensation_enabled); - /*! - * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint - * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command - * function. The gains can be used to change the response of the controller. Be aware that changing the controller - * response can make it unstable. - * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for - * each robot model. - * - * \param kp A vector6d of proportional gains for each of the joints in the robot. - * \param kd A vector6d of derivative gains for each of the joints in the robot. - * - * \returns True, if the write was performed successfully, false otherwise. - */ - bool setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd); - - /*! - * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD - * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the - * torque_command function. - * - * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. - * - * \returns True, if the write was performed successfully, false otherwise. - */ - bool setMaxJointTorques(const urcl::vector6d_t* max_joint_torques); - /*! * \brief Returns whether a client/robot is connected to this server. * @@ -232,8 +206,6 @@ 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 - SET_PD_CONTROLLER_GAINS = 8, ///< Set PD controller gains - SET_MAX_JOINT_TORQUES = 9, ///< Set max joint torques }; /*! diff --git a/include/ur_client_library/control/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h deleted file mode 100644 index 5a4a7be95..000000000 --- a/include/ur_client_library/control/torque_command_controller_parameters.h +++ /dev/null @@ -1,97 +0,0 @@ -#include "ur_client_library/ur/datatypes.h" -#include "ur_client_library/log.h" - -#include - -namespace urcl -{ -namespace control -{ - -struct PDControllerGains -{ - vector6d_t kp; - vector6d_t kd; -}; - -// UR3 PD controller gains, needs to be tuned for the specific purpose. -constexpr PDControllerGains UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ - /*.kp*/ { 560.0, 560.0, 350.0, 163.0, 163.0, 163.0 }, - /*.kd*/ { 47.32, 47.32, 37.42, 25.5, 25.5, 25.5 } -}; - -// UR5 PD controller gains, needs to be tuned for the specific purpose. -constexpr PDControllerGains UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ - /*.kp*/ { 900.0, 900.0, 900.0, 500.0, 500.0, 500.0 }, - /*.kd*/ { 60.0, 60.0, 60.0, 44.72, 44.72, 44.72 } -}; - -// UR10 PD controller gains, needs to be tuned for the specific purpose. -constexpr PDControllerGains UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ - /*.kp*/ { 1300.0, 1300.0, 900.0, 560.0, 560.0, 560.0 }, - /*.kd*/ { 72.11, 72.11, 60.0, 47.32, 47.32, 47.32 } -}; - -constexpr vector6d_t MAX_UR3_JOINT_TORQUE = { 54.0, 54.0, 28.0, 9.0, 9.0, 9.0 }; - -constexpr vector6d_t MAX_UR5_JOINT_TORQUE = { 150.0, 150.0, 150.0, 28.0, 28.0, 28.0 }; - -constexpr vector6d_t MAX_UR10_JOINT_TORQUE = { 330.0, 330.0, 150.0, 54.0, 54.0, 54.0 }; - -/*! - * \brief Get pd gains for specific robot type - * - * \param robot_type Robot type to get gains for - * - * \returns PD gains for the specific robot type - */ -inline PDControllerGains getPdGainsFromRobotType(RobotType robot_type) -{ - switch (robot_type) - { - case RobotType::UR3: - return UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; - case RobotType::UR5: - return UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; - case RobotType::UR10: - return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; - default: - std::stringstream ss; - ss << "Default gains has not been tuned for robot type " << robotTypeString(robot_type) - << ". Defaulting to UR10 gains."; - URCL_LOG_WARN(ss.str().c_str()); - return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; - } -} - -/*! - * \brief Get max torques specific robot type - * - * \param robot_type Robot type to get max torque for - * - * \returns max torque for the specific robot type - */ -inline vector6d_t getMaxTorquesFromRobotType(RobotType robot_type) -{ - switch (robot_type) - { - case RobotType::UR3: - return MAX_UR3_JOINT_TORQUE; - case RobotType::UR5: - return MAX_UR5_JOINT_TORQUE; - case RobotType::UR10: - return MAX_UR10_JOINT_TORQUE; - case RobotType::UR16: - // Same joints as ur10 - return MAX_UR10_JOINT_TORQUE; - default: - std::stringstream ss; - ss << "Max joint torques not collected for robot type " << robotTypeString(robot_type) - << ". Defaulting to UR10 max joint torques."; - URCL_LOG_WARN(ss.str().c_str()); - return MAX_UR10_JOINT_TORQUE; - } -} - -} // namespace control -} // namespace urcl \ No newline at end of file diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 1d829844d..3b7cf2d02 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -726,32 +726,6 @@ class UrDriver */ bool setFrictionCompensation(const bool friction_compensation_enabled); - /*! - * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint - * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command - * function. The gains can be used to change the response of the controller. Be aware that changing the controller - * response can make it unstable. - * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for - * each robot model. - * - * \param kp A vector6d of proportional gains for each of the joints in the robot. - * \param kd A vector6d of derivative gains for each of the joints in the robot. - * - * \returns True, if the write was performed successfully, false otherwise. - */ - bool setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd); - - /*! - * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD - * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the - * torque_command function. - * - * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. - * - * \returns True, if the write was performed successfully, false otherwise. - */ - bool setMaxJointTorques(const urcl::vector6d_t& max_joint_torques); - /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 910702033..7718aca68 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -26,8 +26,6 @@ MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 -MODE_PD_CONTROLLER_JOINT = 9 -MODE_PD_CONTROLLER_TASK = 10 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -53,8 +51,6 @@ END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 -SET_PD_CONTROLLER_GAINS = 8 -SET_MAX_JOINT_TORQUES = 9 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -89,8 +85,6 @@ global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 global friction_compensation_enabled = True -global pd_controller_gains = {{PD_CONTROLLER_GAINS_REPLACE}} -global max_joint_torques = {{MAX_JOINT_TORQUE_REPLACE}} # Global thread variables thread_move = 0 @@ -160,31 +154,6 @@ def terminateProgram(): halt end -### -# @brief Function to clamp each element of a list in between +- clamp value. -# -# @param values array is the list of values to clamp -# @param clampvalues array is the list of clamp values -# -# @returns array of values within +- clamp value -### -def clamp_array(values, clampValues): - if length(values) != length(clampValues): - popup("List of values has different length than list of clamp values.", error = True, blocking = True) - end - ret = values - j = 0 - while j < length(values): - if values[j] > clampValues[j]: - ret[j] = clampValues[j] - elif values[j] < -clampValues[j]: - ret[j] = -clampValues[j] - end - j = j + 1 - end - return ret -end - def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING if targetWithinLimits(cmd_servo_q, q, steptime): @@ -685,29 +654,6 @@ thread servoThreadP(): stopj(STOPJ_ACCELERATION) end -thread PDControlThread(): - while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: - local q_err = cmd_servo_q - get_actual_joint_positions() - {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} - {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} - local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() - tau = clamp_array(tau, max_joint_torques) - direct_torque(tau, friction_comp=friction_compensation_enabled) - {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} - local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() - tau = clamp_array(tau, max_joint_torques) - direct_torque(tau, friction_comp=friction_compensation_enabled) - {% else %} - popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) - {% endif %} - {% else %} - popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) - {% endif %} - end - textmsg("PD Control thread ended") - stopj(STOPJ_ACCELERATION) -end - def tool_contact_detection(): # Detect tool contact in the directions that the TCP is moving step_back = tool_contact(direction = get_actual_tcp_speed()) @@ -781,13 +727,6 @@ thread script_commands(): else: friction_compensation_enabled = True end - elif command == SET_PD_CONTROLLER_GAINS: - {% if ROBOT_SOFTWARE_VERSION > v5.10.0 %} - pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] - pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] - {% endif %} - elif command == SET_MAX_JOINT_TORQUES: - max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end end end @@ -855,12 +794,6 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_POSE: cmd_servo_q = get_joint_positions() thread_move = run servoThreadP() - elif control_mode == MODE_PD_CONTROLLER_JOINT: - cmd_servo_q = get_joint_positions() - thread_move = run PDControlThread() - elif control_mode == MODE_PD_CONTROLLER_TASK: - cmd_servo_q = get_joint_positions() - thread_move = run PDControlThread() end end @@ -901,12 +834,6 @@ while control_mode > MODE_STOPPED: textmsg("Leaving freedrive mode") end_freedrive_mode() end - elif control_mode == MODE_PD_CONTROLLER_JOINT: - q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] - set_servo_setpoint(q) - elif control_mode == MODE_PD_CONTROLLER_TASK: - pose = p[params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] - set_servo_setpoint(get_inverse_kin(pose, cmd_servo_q)) end # Tool contact is running, but hasn't been detected if tool_contact_running == True and control_mode != MODE_TOOL_IN_CONTACT: diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index ac519545f..f33ab402d 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -252,70 +252,6 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens return server_.write(client_fd_, buffer, sizeof(buffer), written); } -bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) -{ - // ToDo: Increase to 5.23.0 once released - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), - urcl::VersionInformation::fromString("10.10.0"), __func__); - const int message_length = 13; - uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; - uint8_t* b_pos = buffer; - - int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PD_CONTROLLER_GAINS)); - b_pos += append(b_pos, val); - - for (auto const& p_gain : *kp) - { - val = htobe32(static_cast(round(p_gain * MULT_JOINTSTATE))); - b_pos += append(b_pos, val); - } - - for (auto const& d_gain : *kd) - { - val = htobe32(static_cast(round(d_gain * 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::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) -{ - // ToDo: Increase to 5.23.0 once released - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), - urcl::VersionInformation::fromString("10.10.0"), __func__); - const int message_length = 7; - uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; - uint8_t* b_pos = buffer; - - int32_t val = htobe32(toUnderlying(ScriptCommand::SET_MAX_JOINT_TORQUES)); - b_pos += append(b_pos, val); - - for (auto const& max_torque : *max_joint_torques) - { - val = htobe32(static_cast(round(max_torque * 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_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index b0b157cd8..dc5051339 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -36,7 +36,6 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" #include "ur_client_library/primary/primary_parser.h" -#include "ur_client_library/control/torque_command_controller_parameters.h" #include "ur_client_library/helpers.h" #include #include @@ -55,8 +54,6 @@ static const std::string TRAJECTORY_PORT_REPLACE("TRAJECTORY_SERVER_PORT_REPLACE static const std::string SCRIPT_COMMAND_PORT_REPLACE("SCRIPT_COMMAND_SERVER_PORT_REPLACE"); static const std::string FORCE_MODE_SET_DAMPING_REPLACE("FORCE_MODE_SET_DAMPING_REPLACE"); static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("FORCE_MODE_SET_GAIN_SCALING_REPLACE"); -static const std::string PD_CONTROLLER_GAINS_REPLACE("PD_CONTROLLER_GAINS_REPLACE"); -static const std::string MAX_JOINT_TORQUE_REPLACE("MAX_JOINT_TORQUE_REPLACE"); UrDriver::~UrDriver() { @@ -111,8 +108,6 @@ void UrDriver::init(const UrDriverConfiguration& config) timeout); } - const RobotType robot_type = primary_client_->getRobotType(); - control::ScriptReader::DataDict data; data[JOINT_STATE_REPLACE] = std::to_string(control::ReverseInterface::MULT_JOINTSTATE); data[TIME_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_TIME); @@ -145,23 +140,6 @@ void UrDriver::init(const UrDriverConfiguration& config) } data[BEGIN_REPLACE] = begin_replace.str(); - const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); - std::stringstream pd_gains_ss; - if (robot_version_ < urcl::VersionInformation::fromString("5.10.0")) - { - // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. - pd_gains_ss << 0; - } - else - { - pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; - } - data[PD_CONTROLLER_GAINS_REPLACE] = pd_gains_ss.str(); - - std::stringstream max_torques_ss; - max_torques_ss << control::getMaxTorquesFromRobotType(robot_type); - data[MAX_JOINT_TORQUE_REPLACE] = max_torques_ss.str(); - data["ROBOT_SOFTWARE_VERSION"] = getVersion(); script_reader_.reset(new control::ScriptReader()); @@ -561,54 +539,6 @@ bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) } } -bool UrDriver::setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd) -{ - if (!std::all_of(kp.begin(), kp.end(), [](double v) { return v >= 0.0; })) - { - throw InvalidRange("kp should be larger than zero"); - } - - if (!std::all_of(kd.begin(), kd.end(), [](double v) { return v >= 0.0; })) - { - throw InvalidRange("kd should be larger than zero"); - } - - if (script_command_interface_->clientConnected()) - { - return script_command_interface_->setPDControllerGains(&kp, &kd); - } - else - { - URCL_LOG_ERROR("Script command interface is not running. Unable to set PD Controller gains."); - return 0; - } -} - -bool UrDriver::setMaxJointTorques(const urcl::vector6d_t& max_joint_torques) -{ - const urcl::vector6d_t max_torques_for_robot_type = - control::getMaxTorquesFromRobotType(primary_client_->getRobotType()); - if (!std::equal(max_joint_torques.begin(), max_joint_torques.end(), max_torques_for_robot_type.begin(), - [](double v1, double v2) { return v1 <= v2 && v1 >= 0.0; })) - { - std::stringstream ss; - ss << "The max joint torques should be smaller or equal to the maximum joint torques for the robot type and larger " - "than zero. Provided max joint torques " - << max_joint_torques << " and maximum joint torques for the robot type " << max_torques_for_robot_type; - throw InvalidRange(ss.str().c_str()); - } - - if (script_command_interface_->clientConnected()) - { - return script_command_interface_->setMaxJointTorques(&max_joint_torques); - } - else - { - URCL_LOG_ERROR("Script command interface is not running. Unable to set max joint torques."); - return 0; - } -} - bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout) { vector6d_t* fake = nullptr; diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index ff4382acc..d30c1c5ca 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -403,18 +403,6 @@ TEST_F(ReverseIntefaceTest, write_control_mode) received_control_mode = client_->getControlMode(); EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); - - expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_JOINT; - reverse_interface_->write(&pos, expected_control_mode); - received_control_mode = client_->getControlMode(); - - EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); - - expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_TASK; - reverse_interface_->write(&pos, expected_control_mode); - received_control_mode = client_->getControlMode(); - - EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); } TEST_F(ReverseIntefaceTest, write_freedrive_control_message) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 196a58319..a34203ddb 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -421,76 +421,6 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation) EXPECT_EQ(message_sum, expected_message_sum); } -TEST_F(ScriptCommandInterfaceTest, test_set_pd_controller_gains) -{ - // Wait for the client to connect to the server - waitForClientConnection(); - - urcl::vector6d_t kp = { 220.2, 220.2, 300.0, 10.32, 10.32, 10.32 }; - urcl::vector6d_t kd = { 29.68, 29.68, 35.0, 6.4, 6.4, 6.4 }; - script_command_interface_->setPDControllerGains(&kp, &kd); - - int32_t command; - std::vector message; - client_->readMessage(command, message); - - // 8 is set PD controller gains - int32_t expected_command = 8; - EXPECT_EQ(command, expected_command); - - int32_t message_idx = 0; - - for (auto& p_gain : kp) - { - const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; - EXPECT_EQ(p_gain, received_gain); - message_idx = message_idx + 1; - } - - for (auto& d_gain : kd) - { - const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; - EXPECT_EQ(d_gain, received_gain); - message_idx = message_idx + 1; - } - - // The rest of the message should be zero - int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0); - int32_t expected_message_sum = 0; - EXPECT_EQ(message_sum, expected_message_sum); -} - -TEST_F(ScriptCommandInterfaceTest, test_set_max_joint_torques) -{ - // Wait for the client to connect to the server - waitForClientConnection(); - - urcl::vector6d_t max_joint_torques = { 100.0, 150.0, 21.2, 10.32, 10.32, 10.32 }; - script_command_interface_->setMaxJointTorques(&max_joint_torques); - - int32_t command; - std::vector message; - client_->readMessage(command, message); - - // 9 is set max joint torques - int32_t expected_command = 9; - EXPECT_EQ(command, expected_command); - - int32_t message_idx = 0; - - for (auto& max_torque : max_joint_torques) - { - const double received_max_torque = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; - EXPECT_EQ(max_torque, received_max_torque); - message_idx = message_idx + 1; - } - - // The rest of the message should be zero - int32_t message_sum = std::accumulate(std::begin(message) + message_idx, 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); From 1e7fc614e9ade9a9dc8809c15b19f11f1a01fa91 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 19 Sep 2025 16:25:33 +0200 Subject: [PATCH 16/23] Renamed torque_control_example to direct_torque_control_example --- doc/examples.rst | 2 +- ..._control.rst => direct_torque_control.rst} | 22 +++++++++---------- examples/CMakeLists.txt | 6 ++--- ..._control.cpp => direct_torque_control.cpp} | 0 4 files changed, 15 insertions(+), 15 deletions(-) rename doc/examples/{torque_control.rst => direct_torque_control.rst} (81%) rename examples/{torque_control.cpp => direct_torque_control.cpp} (100%) diff --git a/doc/examples.rst b/doc/examples.rst index 9e37daf6d..ad4f6ac60 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -27,6 +27,6 @@ may be running forever until manually stopped. examples/script_sender examples/spline_example examples/tool_contact_example - examples/torque_control + examples/direct_torque_control examples/trajectory_point_interface examples/ur_driver diff --git a/doc/examples/torque_control.rst b/doc/examples/direct_torque_control.rst similarity index 81% rename from doc/examples/torque_control.rst rename to doc/examples/direct_torque_control.rst index 36c606ec4..f4d385d4d 100644 --- a/doc/examples/torque_control.rst +++ b/doc/examples/direct_torque_control.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/torque_control.rst +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/direct_torque_control.rst -.. _torque_control_example: +.. _direct_torque_control_example: Torque control example ====================== @@ -25,16 +25,16 @@ Initialization At first, we create a ``ExampleRobotWrapper`` object giving it the robot's IP address, script file and RTDE recipes. -.. literalinclude:: ../../examples/torque_control.cpp +.. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ - :caption: examples/torque_control.cpp + :caption: examples/direct_torque_control.cpp :linenos: :lineno-match: :start-at: bool headless_mode = true; :end-at: // --------------- INITIALIZATION END ------------------- .. note:: - This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the torque_control + This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the direct_torque_control mode which is only available in these versions and later. If you try to run it on an older software version, this example will print an error and exit. @@ -48,9 +48,9 @@ positive limit and then back to 0. To read the joint data, the driver's RTDE client is used: -.. literalinclude:: ../../examples/torque_control.cpp +.. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ - :caption: examples/torque_control.cpp + :caption: examples/direct_torque_control.cpp :linenos: :lineno-match: :start-at: // Once RTDE communication is started @@ -60,9 +60,9 @@ The first read operation will initialize the target buffer with the current robo the target joint torques are set based on the current joint positions: -.. literalinclude:: ../../examples/torque_control.cpp +.. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ - :caption: examples/torque_control.cpp + :caption: examples/direct_torque_control.cpp :linenos: :lineno-match: :start-at: // Open loop control @@ -71,9 +71,9 @@ the target joint torques are set based on the current joint positions: To send the control command, the robot's :ref:`reverse_interface` is used via the ``writeJointCommand()`` function: -.. literalinclude:: ../../examples/torque_control.cpp +.. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ - :caption: examples/torque_control.cpp + :caption: examples/direct_torque_control.cpp :linenos: :lineno-match: :start-at: // Setting the RobotReceiveTimeout diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index be72a0489..4a12c021d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -48,9 +48,9 @@ add_executable(script_command_interface_example script_command_interface.cpp) target_link_libraries(script_command_interface_example ur_client_library::urcl) -add_executable(torque_control_example - torque_control.cpp) -target_link_libraries(torque_control_example ur_client_library::urcl) +add_executable(direct_torque_control_example + direct_torque_control.cpp) +target_link_libraries(direct_torque_control_example ur_client_library::urcl) add_executable(trajectory_point_interface_example trajectory_point_interface.cpp) diff --git a/examples/torque_control.cpp b/examples/direct_torque_control.cpp similarity index 100% rename from examples/torque_control.cpp rename to examples/direct_torque_control.cpp From 33602a0a8bdb5513763d46acf61cacc3be2d1ad8 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 19 Sep 2025 16:40:15 +0200 Subject: [PATCH 17/23] Add torque_control to reverse_interface documentation --- doc/architecture/reverse_interface.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index 5df6e7796..d9ed6bc3a 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -57,6 +57,9 @@ meaning: - freedrive instruction (FREEDRIVE) - field 1: Freedrive mode (1: FREEDRIVE_MODE_START, -1: FREEDRIVE_MODE_STOP) + - target joint torques (TORQUE). Note that this is **after** gravity compensation. A + vector of zeros will hold the current position given that the payload is known to the + controller. 7 Control mode. Can be either of @@ -72,6 +75,7 @@ meaning: - 7: TOOL_IN_CONTACT -- status - not meant to be sent. In tool contact mode this will encode whether tool contact has been established or not. + - 8: TORQUE -- Direct torque control mode (since PolyScope 5.23.0 / 10.10.0) ===== ===== .. note:: From f06c53d373816c4976c9b51524d59d46254a49c8 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 19 Sep 2025 16:46:18 +0200 Subject: [PATCH 18/23] Add version compatibility note --- doc/polyscope_compatibility.rst | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/doc/polyscope_compatibility.rst b/doc/polyscope_compatibility.rst index b5bd2c5f2..2f8abed69 100644 --- a/doc/polyscope_compatibility.rst +++ b/doc/polyscope_compatibility.rst @@ -3,6 +3,9 @@ |polyscope| version compatibility ================================= +Version-breaking changes +------------------------ + The table below shows breaking changes in the library compared to |polyscope| versions. Compatibility is listed for CB3 robots (versions 3.x.y) and e-Series robots (versions 5.x.y) respectively. @@ -44,3 +47,17 @@ table below or checkout the latest tag before the breaking changes were introduc See `Universal Robots External Control URCapX `_ .. |polyscope| replace:: PolyScope + +Features requiring a specific |polyscope| version +------------------------------------------------- + +Features in this section have been added in a backwards-compatible way. It is still possible to use +this library with an older compatible version, but trying to use one of the features below might +lead to a runtime exception. + +Torque control (From version 2.4.0) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The direct torque control mode is only available on |polyscope| 5.23.0 / 10.10.0 and later. This +includes the ``TORQUE`` control mode in the ``ReverseInterface`` as well as the +``setFrictionCompensation()`` function in the ``ScriptCommandInterface``. From e609a51babd21f458a209d2ef4d4701c47d83168 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 22 Sep 2025 10:50:33 +0200 Subject: [PATCH 19/23] Add a warning about high torques --- doc/architecture/reverse_interface.rst | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index d9ed6bc3a..233734490 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -57,9 +57,7 @@ meaning: - freedrive instruction (FREEDRIVE) - field 1: Freedrive mode (1: FREEDRIVE_MODE_START, -1: FREEDRIVE_MODE_STOP) - - target joint torques (TORQUE). Note that this is **after** gravity compensation. A - vector of zeros will hold the current position given that the payload is known to the - controller. + - target joint torques (TORQUE, see :ref:`direct_torque_control_mode`). 7 Control mode. Can be either of @@ -75,7 +73,7 @@ meaning: - 7: TOOL_IN_CONTACT -- status - not meant to be sent. In tool contact mode this will encode whether tool contact has been established or not. - - 8: TORQUE -- Direct torque control mode (since PolyScope 5.23.0 / 10.10.0) + - 8: TORQUE -- :ref:`direct_torque_control_mode` (since PolyScope 5.23.0 / 10.10.0) ===== ===== .. note:: @@ -88,4 +86,19 @@ meaning: ``MULT_JOINTSTATE`` constant to get the actual floating point value. This constant is defined in ``ReverseInterface`` class. -Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket". +Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE, TORQUE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket". + +.. _direct_torque_control_mode: + +Direct torque control mode +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Direct torque control mode is available since PolyScope version 5.23.0 / 10.10.0. It allows to command +joint torques directly to the robot. + +.. note:: Target tprques are given **after** gravity compensation. A vector of zeros will hold the current position + given that the payload is known to the controller. + +.. warning:: Direct torque control is a very low-level command interface. Commanding high torques in + free space can make the robot move very fast and hereby trigger a fault due to joint velocities + or the TCP speed violating the safety settings. Keep that in mind when using this mode. From f8107710b1f831104fd038c03100b0ca66c0a19a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 22 Sep 2025 10:51:37 +0200 Subject: [PATCH 20/23] Remove README note about beta program torque control is now available in released software versions, the API is considered stable and we do have documentation available. --- README.md | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/README.md b/README.md index 99cabab7e..e729954e6 100644 --- a/README.md +++ b/README.md @@ -10,25 +10,6 @@ drivers. --- -> [!IMPORTANT] -> The **torque_control** branch is a **beta feature**. At the current time, you will need to -> participate in the public beta program at -> [https://ur.centercode.com/key/PolyScope5Beta](https://ur.centercode.com/key/PolyScope5Beta) to -> use it. It will not work with any released robot software version. At a later time this will -> require a certain robot software version to work and the new features will not work with older -> software versions. However, old functionality will still be working with older robot software as -> noted in the version requirements below. -> -> This being a beta feature, the API is not to be considered stable. Anything developed on this -> branch might be changing in a breaking way until it is merged into the master branch. -> -> Also, please keep the beta status in mind when operating the robot. Parts of the software -> involved in the beta-feature might not be tested to the usual extent and might show unexpected -> behavior in edge cases. -> -> Documentation for the new features may also be missing initially. This will be added before -> merging things to the master branch. -
Universal Robot family From 21a63e3e01d29223b40793c783887dcd741d6218 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 30 Sep 2025 08:13:53 +0200 Subject: [PATCH 21/23] Apply suggestions from code review Co-authored-by: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> --- doc/architecture/reverse_interface.rst | 2 +- src/control/script_command_interface.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index 233734490..05514afe0 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -96,7 +96,7 @@ Direct torque control mode Direct torque control mode is available since PolyScope version 5.23.0 / 10.10.0. It allows to command joint torques directly to the robot. -.. note:: Target tprques are given **after** gravity compensation. A vector of zeros will hold the current position +.. note:: Target torques are given **after** gravity compensation. A vector of zeros will hold the current position given that the payload is known to the controller. .. warning:: Direct torque control is a very low-level command interface. Commanding high torques in diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index f33ab402d..f868c225f 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -228,8 +228,7 @@ bool ScriptCommandInterface::endToolContact() bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) { - // ToDo: Increase to 5.23.0 once released - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.22.0"), + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; From df110ef0507dc2cc76a5f475ca76a96394e36b4e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 30 Sep 2025 08:17:10 +0200 Subject: [PATCH 22/23] Return false from setFrictionCompensation if the command is not supported --- src/control/script_command_interface.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index f868c225f..995205e1a 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -228,8 +228,11 @@ bool ScriptCommandInterface::endToolContact() bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) { - robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), - urcl::VersionInformation::fromString("10.10.0"), __func__); + if (!robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__)) + { + return false; + } const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -300,6 +303,7 @@ void ScriptCommandInterface::messageCallback(const socket_t filedescriptor, char nbytesrecv); } } + bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, const VersionInformation& min_polyscopeX, const std::string& command_name) From 84ed404f23ae25d9342e03edf50eb08a797997f6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 30 Sep 2025 14:05:19 +0200 Subject: [PATCH 23/23] test_script_command_interface: set robot software version --- tests/test_script_command_interface.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index a34203ddb..22cb2bc70 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -31,6 +31,7 @@ #include #include #include +#include "ur_client_library/control/reverse_interface.h" #include #include @@ -102,7 +103,11 @@ class ScriptCommandInterfaceTest : public ::testing::Test void SetUp() { - script_command_interface_.reset(new control::ScriptCommandInterface(control::ReverseInterfaceConfig{ 50004 })); + control::ReverseInterfaceConfig config; + config.port = 50004; + // Assume, we have all features supported + config.robot_software_version = VersionInformation::fromString("99.99.9"); + script_command_interface_.reset(new control::ScriptCommandInterface(config)); client_.reset(new Client(50004)); }