diff --git a/README.md b/README.md index 5690f090b..e729954e6 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,8 @@ 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. +--- +
Universal Robot family diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index 5df6e7796..05514afe0 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -57,6 +57,7 @@ meaning: - freedrive instruction (FREEDRIVE) - field 1: Freedrive mode (1: FREEDRIVE_MODE_START, -1: FREEDRIVE_MODE_STOP) + - target joint torques (TORQUE, see :ref:`direct_torque_control_mode`). 7 Control mode. Can be either of @@ -72,6 +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 -- :ref:`direct_torque_control_mode` (since PolyScope 5.23.0 / 10.10.0) ===== ===== .. note:: @@ -84,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 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 + 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. 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/doc/examples.rst b/doc/examples.rst index 159b2721c..ad4f6ac60 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -23,8 +23,10 @@ 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 + examples/direct_torque_control examples/trajectory_point_interface examples/ur_driver diff --git a/doc/examples/direct_torque_control.rst b/doc/examples/direct_torque_control.rst new file mode 100644 index 000000000..f4d385d4d --- /dev/null +++ b/doc/examples/direct_torque_control.rst @@ -0,0 +1,80 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/direct_torque_control.rst + +.. _direct_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/direct_torque_control.cpp + :language: c++ + :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 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. + +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/direct_torque_control.cpp + :language: c++ + :caption: examples/direct_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/direct_torque_control.cpp + :language: c++ + :caption: examples/direct_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/direct_torque_control.cpp + :language: c++ + :caption: examples/direct_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/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/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``. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 48452197d..4a12c021d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -44,6 +44,14 @@ 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(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) target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) diff --git a/examples/direct_torque_control.cpp b/examples/direct_torque_control.cpp new file mode 100644 index 000000000..f8c789e9b --- /dev/null +++ b/examples/direct_torque_control.cpp @@ -0,0 +1,161 @@ +// -- 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]); + } + + // 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"); + 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(); + // 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.", + robot_version.toString().c_str()); + return 0; + } + } + // --------------- INITIALIZATION END ------------------- + + 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 = { 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(); + 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 + // 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 = -torque_abs; + } + } + else if (passed_negative_part == false) + { + if (g_joint_positions[JOINT_INDEX] <= 0) + { + cmd_torque = torque_abs; + 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()); + 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"); + return 0; +} 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; +} 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/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 9c15d49dd..0c4ebc6ed 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 { @@ -156,6 +157,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. * @@ -186,15 +198,35 @@ 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 }; + /*! + * \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/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 2ba9efbc3..82acfef1d 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -715,6 +715,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 16db302d3..0ae724335 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -27,6 +27,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 @@ -53,6 +54,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 @@ -74,6 +76,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] @@ -85,6 +88,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 @@ -229,6 +233,32 @@ 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 + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + 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.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("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): @@ -752,6 +782,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 @@ -809,6 +845,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) @@ -827,6 +865,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 diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index d38933430..995205e1a 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -226,6 +226,34 @@ bool ScriptCommandInterface::endToolContact() return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) +{ + 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; + + 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_; @@ -276,5 +304,21 @@ void ScriptCommandInterface::messageCallback(const socket_t filedescriptor, char } } +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 08bc49a2f..dc5051339 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -36,6 +36,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" #include "ur_client_library/primary/primary_parser.h" +#include "ur_client_library/helpers.h" #include #include @@ -96,6 +97,17 @@ void UrDriver::init(const UrDriverConfiguration& config) 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); + } + 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); @@ -514,6 +526,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; @@ -645,4 +670,4 @@ std::deque UrDriver::getErrorCodes() { return primary_client_->getErrorCodes(); } -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 9950b1113..a365b6dd8 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -430,6 +430,12 @@ 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); } TEST_F(ReverseIntefaceTest, write_freedrive_control_message) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index abe395096..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)); } @@ -382,6 +387,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);