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/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 3f03855f9..a7c90ecae 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -125,11 +125,11 @@ class DataPackage : public RTDEPackage * \returns True on success, false if the field cannot be found inside the package. */ template - bool getData(const std::string& name, T& val) + bool getData(const std::string& name, T& val) const { if (data_.find(name) != data_.end()) { - val = std::get(data_[name]); + val = std::get(data_.at(name)); } else { @@ -149,13 +149,13 @@ class DataPackage : public RTDEPackage * \returns True on success, false if the field cannot be found inside the package. */ template - bool getData(const std::string& name, std::bitset& val) + bool getData(const std::string& name, std::bitset& val) const { static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable"); if (data_.find(name) != data_.end()) { - val = std::bitset(std::get(data_[name])); + val = std::bitset(std::get(data_.at(name))); } else { @@ -179,7 +179,7 @@ class DataPackage : public RTDEPackage { if (data_.find(name) != data_.end()) { - data_[name] = val; + data_.at(name) = val; } else { diff --git a/include/ur_client_library/ur/datatypes.h b/include/ur_client_library/ur/datatypes.h index 4a7fec779..3a4ba1f22 100644 --- a/include/ur_client_library/ur/datatypes.h +++ b/include/ur_client_library/ur/datatypes.h @@ -95,6 +95,7 @@ enum class RobotType : int8_t UR10 = 2, UR3 = 3, UR16 = 4, + UR18 = 5, UR8LONG = 6, UR20 = 7, UR30 = 8, 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/package.xml b/package.xml index 1139d22e2..f586cf5f1 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ ur_client_library - 2.3.0 + 2.4.0 Standalone C++ library for accessing Universal Robots interfaces. This has been forked off the ur_robot_driver. Thomas Timm Andersen Simon Rasmussen 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/scripts/start_ursim.sh b/scripts/start_ursim.sh index 29d5ccfef..93a5fdb51 100755 --- a/scripts/start_ursim.sh +++ b/scripts/start_ursim.sh @@ -43,7 +43,7 @@ help() echo echo "Syntax: `basename "$0"` [-m|s|h]" echo "options:" - echo " -m Robot model. One of [ur3, ur3e, ur5, ur5e, ur7e, ur8long, ur10e, ur12e, ur16e, ur15, ur20, ur30]. Defaults to ur5e." + echo " -m Robot model. One of [ur3, ur3e, ur5, ur5e, ur7e, ur8long, ur10e, ur12e, ur16e, ur15, ur18, ur20, ur30]. Defaults to ur5e." echo " -v URSim version that should be used. See https://hub.docker.com/r/universalrobots/ursim_e-series/tags for available versions. Defaults to 'latest'" @@ -86,7 +86,7 @@ get_series_from_model() ur3e|ur5e|ur7e|ur10e|ur12e|ur16e) ROBOT_SERIES=e-series ;; - ur8long|ur15|ur20|ur30) + ur8long|ur15|ur18|ur20|ur30) ROBOT_SERIES=e-series ;; *) @@ -125,7 +125,7 @@ strip_robot_model() ROBOT_MODEL=${robot_model^^} else ROBOT_MODEL=${robot_model^^} - # UR8LONG, UR15, UR20 and UR30 need no further adjustment + # UR8LONG, UR15, UR18, UR20 and UR30 need no further adjustment if [[ "$robot_model" = @(ur3e|ur5e|ur10e|ur16e) ]]; then ROBOT_MODEL=$(echo "${ROBOT_MODEL:0:$((${#ROBOT_MODEL}-1))}") elif [[ "$robot_model" = @(ur7e|ur12e) ]]; then @@ -153,8 +153,8 @@ validate_parameters() local MIN_UR30="5.15.0" local MIN_UR7e="5.22.0" # and UR12e local MIN_UR7e_X="10.9.0" # and UR12e - local MIN_UR8LONG="5.23.0" - local MIN_UR8LONG_X="10.11.0" + local MIN_UR8LONG="5.23.0" # and UR18 + local MIN_UR8LONG_X="10.11.0" # and UR18 local URSIM_VERSION_CHECK="$URSIM_VERSION" if [[ "$URSIM_VERSION" == "latest" ]]; then @@ -184,7 +184,7 @@ validate_parameters() verlte "$MIN_CB3" "$URSIM_VERSION_CHECK" && return 0 ;; e-series) - if [[ $ROBOT_MODEL != @(ur3e|ur5e|ur7e|ur8long|ur10e|ur12e|ur16e|ur15|ur20|ur30) ]]; then + if [[ $ROBOT_MODEL != @(ur3e|ur5e|ur7e|ur8long|ur10e|ur12e|ur16e|ur15|ur18|ur20|ur30) ]]; then echo "$ROBOT_MODEL is no valid e-series model!" && exit 1 fi if [[ $ROBOT_MODEL == "ur15" ]]; then @@ -195,7 +195,7 @@ validate_parameters() MIN_VERSION=$MIN_UR30 elif [[ $ROBOT_MODEL == "ur7e" || $ROBOT_MODEL == "ur12e" ]]; then MIN_VERSION=$MIN_UR7e - elif [[ $ROBOT_MODEL == "ur8long" ]]; then + elif [[ $ROBOT_MODEL == "ur8long" || $ROBOT_MODEL == "ur18" ]]; then MIN_VERSION=$MIN_UR8LONG else MIN_VERSION=$MIN_E_SERIES @@ -206,11 +206,11 @@ validate_parameters() echo "PolyscopeX is only supported from version $MIN_POLYSCOPE_X onwards" exit 1 fi - if [[ $ROBOT_MODEL != @(ur3e|ur5e|ur7e|ur8long|ur10e|ur12e|ur16e|ur15|ur20|ur30) ]]; then + if [[ $ROBOT_MODEL != @(ur3e|ur5e|ur7e|ur8long|ur10e|ur12e|ur16e|ur15|ur18|ur20|ur30) ]]; then echo "$ROBOT_MODEL is no valid PolyscopeX model!" && exit 1 elif [[ $ROBOT_MODEL == "ur7e" || $ROBOT_MODEL == "ur12e" ]]; then MIN_VERSION=$MIN_UR7e_X - elif [[ $ROBOT_MODEL == "ur8long" ]]; then + elif [[ $ROBOT_MODEL == "ur8long" || $ROBOT_MODEL == "ur18" ]]; then MIN_VERSION=$MIN_UR8LONG_X elif [[ $ROBOT_MODEL == "ur15" ]]; then MIN_VERSION=$MIN_UR15_X 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/rtde/data_package.cpp b/src/rtde/data_package.cpp index efa5b6699..a92ae14e6 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -34,6 +34,184 @@ namespace urcl namespace rtde_interface { std::unordered_map DataPackage::g_type_list{ + // INPUTS + { "speed_slider_mask", uint32_t() }, + { "speed_slider_fraction", double() }, + { "standard_digital_output_mask", uint8_t() }, + { "standard_digital_output", uint8_t() }, + { "configurable_digital_output_mask", uint8_t() }, + { "configurable_digital_output", uint8_t() }, + { "standard_analog_output_mask", uint8_t() }, + { "standard_analog_output_type", uint8_t() }, + { "standard_analog_output_0", double() }, + { "standard_analog_output_1", double() }, + { "external_force_torque", vector6d_t() }, + + // INPUT / OUTPUT + { "input_bit_registers0_to_31", uint32_t() }, + { "input_bit_registers32_to_63", uint32_t() }, + { "input_bit_register_64", bool() }, + { "input_bit_register_65", bool() }, + { "input_bit_register_66", bool() }, + { "input_bit_register_67", bool() }, + { "input_bit_register_68", bool() }, + { "input_bit_register_69", bool() }, + { "input_bit_register_70", bool() }, + { "input_bit_register_71", bool() }, + { "input_bit_register_72", bool() }, + { "input_bit_register_73", bool() }, + { "input_bit_register_74", bool() }, + { "input_bit_register_75", bool() }, + { "input_bit_register_76", bool() }, + { "input_bit_register_77", bool() }, + { "input_bit_register_78", bool() }, + { "input_bit_register_79", bool() }, + { "input_bit_register_80", bool() }, + { "input_bit_register_81", bool() }, + { "input_bit_register_82", bool() }, + { "input_bit_register_83", bool() }, + { "input_bit_register_84", bool() }, + { "input_bit_register_85", bool() }, + { "input_bit_register_86", bool() }, + { "input_bit_register_87", bool() }, + { "input_bit_register_88", bool() }, + { "input_bit_register_89", bool() }, + { "input_bit_register_90", bool() }, + { "input_bit_register_91", bool() }, + { "input_bit_register_92", bool() }, + { "input_bit_register_93", bool() }, + { "input_bit_register_94", bool() }, + { "input_bit_register_95", bool() }, + { "input_bit_register_96", bool() }, + { "input_bit_register_97", bool() }, + { "input_bit_register_98", bool() }, + { "input_bit_register_99", bool() }, + { "input_bit_register_100", bool() }, + { "input_bit_register_101", bool() }, + { "input_bit_register_102", bool() }, + { "input_bit_register_103", bool() }, + { "input_bit_register_104", bool() }, + { "input_bit_register_105", bool() }, + { "input_bit_register_106", bool() }, + { "input_bit_register_107", bool() }, + { "input_bit_register_108", bool() }, + { "input_bit_register_109", bool() }, + { "input_bit_register_110", bool() }, + { "input_bit_register_111", bool() }, + { "input_bit_register_112", bool() }, + { "input_bit_register_113", bool() }, + { "input_bit_register_114", bool() }, + { "input_bit_register_115", bool() }, + { "input_bit_register_116", bool() }, + { "input_bit_register_117", bool() }, + { "input_bit_register_118", bool() }, + { "input_bit_register_119", bool() }, + { "input_bit_register_120", bool() }, + { "input_bit_register_121", bool() }, + { "input_bit_register_122", bool() }, + { "input_bit_register_123", bool() }, + { "input_bit_register_124", bool() }, + { "input_bit_register_125", bool() }, + { "input_bit_register_126", bool() }, + { "input_bit_register_127", bool() }, + { "input_int_register_0", int32_t() }, + { "input_int_register_1", int32_t() }, + { "input_int_register_2", int32_t() }, + { "input_int_register_3", int32_t() }, + { "input_int_register_4", int32_t() }, + { "input_int_register_5", int32_t() }, + { "input_int_register_6", int32_t() }, + { "input_int_register_7", int32_t() }, + { "input_int_register_8", int32_t() }, + { "input_int_register_9", int32_t() }, + { "input_int_register_10", int32_t() }, + { "input_int_register_11", int32_t() }, + { "input_int_register_12", int32_t() }, + { "input_int_register_13", int32_t() }, + { "input_int_register_14", int32_t() }, + { "input_int_register_15", int32_t() }, + { "input_int_register_16", int32_t() }, + { "input_int_register_17", int32_t() }, + { "input_int_register_18", int32_t() }, + { "input_int_register_19", int32_t() }, + { "input_int_register_20", int32_t() }, + { "input_int_register_21", int32_t() }, + { "input_int_register_22", int32_t() }, + { "input_int_register_23", int32_t() }, + { "input_int_register_24", int32_t() }, + { "input_int_register_25", int32_t() }, + { "input_int_register_26", int32_t() }, + { "input_int_register_27", int32_t() }, + { "input_int_register_28", int32_t() }, + { "input_int_register_29", int32_t() }, + { "input_int_register_30", int32_t() }, + { "input_int_register_31", int32_t() }, + { "input_int_register_32", int32_t() }, + { "input_int_register_33", int32_t() }, + { "input_int_register_34", int32_t() }, + { "input_int_register_35", int32_t() }, + { "input_int_register_36", int32_t() }, + { "input_int_register_37", int32_t() }, + { "input_int_register_38", int32_t() }, + { "input_int_register_39", int32_t() }, + { "input_int_register_40", int32_t() }, + { "input_int_register_41", int32_t() }, + { "input_int_register_42", int32_t() }, + { "input_int_register_43", int32_t() }, + { "input_int_register_44", int32_t() }, + { "input_int_register_45", int32_t() }, + { "input_int_register_46", int32_t() }, + { "input_int_register_47", int32_t() }, + { "input_double_register_0", double() }, + { "input_double_register_1", double() }, + { "input_double_register_2", double() }, + { "input_double_register_3", double() }, + { "input_double_register_4", double() }, + { "input_double_register_5", double() }, + { "input_double_register_6", double() }, + { "input_double_register_7", double() }, + { "input_double_register_8", double() }, + { "input_double_register_9", double() }, + { "input_double_register_10", double() }, + { "input_double_register_11", double() }, + { "input_double_register_12", double() }, + { "input_double_register_13", double() }, + { "input_double_register_14", double() }, + { "input_double_register_15", double() }, + { "input_double_register_16", double() }, + { "input_double_register_17", double() }, + { "input_double_register_18", double() }, + { "input_double_register_19", double() }, + { "input_double_register_20", double() }, + { "input_double_register_21", double() }, + { "input_double_register_22", double() }, + { "input_double_register_23", double() }, + { "input_double_register_24", double() }, + { "input_double_register_25", double() }, + { "input_double_register_26", double() }, + { "input_double_register_27", double() }, + { "input_double_register_28", double() }, + { "input_double_register_29", double() }, + { "input_double_register_30", double() }, + { "input_double_register_31", double() }, + { "input_double_register_32", double() }, + { "input_double_register_33", double() }, + { "input_double_register_34", double() }, + { "input_double_register_35", double() }, + { "input_double_register_36", double() }, + { "input_double_register_37", double() }, + { "input_double_register_38", double() }, + { "input_double_register_39", double() }, + { "input_double_register_40", double() }, + { "input_double_register_41", double() }, + { "input_double_register_42", double() }, + { "input_double_register_43", double() }, + { "input_double_register_44", double() }, + { "input_double_register_45", double() }, + { "input_double_register_46", double() }, + { "input_double_register_47", double() }, + + // OUTPUTS { "timestamp", double() }, { "target_q", vector6d_t() }, { "target_qd", vector6d_t() }, @@ -44,15 +222,21 @@ std::unordered_map DataPackage::g_ { "actual_qd", vector6d_t() }, { "actual_current", vector6d_t() }, { "actual_current_window", vector6d_t() }, + { "actual_current_as_torque", vector6d_t() }, { "joint_control_output", vector6d_t() }, { "actual_TCP_pose", vector6d_t() }, { "actual_TCP_speed", vector6d_t() }, { "actual_TCP_force", vector6d_t() }, { "target_TCP_pose", vector6d_t() }, { "target_TCP_speed", vector6d_t() }, + { "tcp_offset", vector6d_t() }, + { "actual_TCP_acceleration", vector6d_t() }, + { "target_TCP_acceleration", vector6d_t() }, { "actual_digital_input_bits", uint64_t() }, + { "actual_configurable_digital_input_bits", uint64_t() }, { "joint_temperatures", vector6d_t() }, { "actual_execution_time", double() }, + { "target_execution_time", double() }, { "robot_mode", int32_t() }, { "joint_mode", vector6int32_t() }, { "safety_mode", int32_t() }, @@ -66,6 +250,7 @@ std::unordered_map DataPackage::g_ { "actual_robot_current", double() }, { "actual_joint_voltage", vector6d_t() }, { "actual_digital_output_bits", uint64_t() }, + { "actual_configurable_digital_output_bits", uint64_t() }, { "runtime_state", uint32_t() }, { "elbow_position", vector3d_t() }, { "elbow_velocity", vector3d_t() }, @@ -77,84 +262,8 @@ std::unordered_map DataPackage::g_ { "standard_analog_output0", double() }, { "standard_analog_output1", double() }, { "io_current", double() }, - { "euromap67_input_bits", uint32_t() }, - { "euromap67_output_bits", uint32_t() }, - { "euromap67_24V_voltage", double() }, - { "euromap67_24V_current", double() }, - { "tool_mode", uint32_t() }, - { "tool_analog_input_types", uint32_t() }, - { "tool_analog_input0", double() }, - { "tool_analog_input1", double() }, - { "tool_output_voltage", int32_t() }, - { "tool_output_current", double() }, - { "tool_temperature", double() }, - { "tcp_force_scalar", double() }, { "output_bit_registers0_to_31", uint32_t() }, { "output_bit_registers32_to_63", uint32_t() }, - { "output_bit_register_0", bool() }, - { "output_bit_register_1", bool() }, - { "output_bit_register_2", bool() }, - { "output_bit_register_3", bool() }, - { "output_bit_register_4", bool() }, - { "output_bit_register_5", bool() }, - { "output_bit_register_6", bool() }, - { "output_bit_register_7", bool() }, - { "output_bit_register_8", bool() }, - { "output_bit_register_9", bool() }, - { "output_bit_register_10", bool() }, - { "output_bit_register_11", bool() }, - { "output_bit_register_12", bool() }, - { "output_bit_register_13", bool() }, - { "output_bit_register_14", bool() }, - { "output_bit_register_15", bool() }, - { "output_bit_register_16", bool() }, - { "output_bit_register_17", bool() }, - { "output_bit_register_18", bool() }, - { "output_bit_register_19", bool() }, - { "output_bit_register_20", bool() }, - { "output_bit_register_21", bool() }, - { "output_bit_register_22", bool() }, - { "output_bit_register_23", bool() }, - { "output_bit_register_24", bool() }, - { "output_bit_register_25", bool() }, - { "output_bit_register_26", bool() }, - { "output_bit_register_27", bool() }, - { "output_bit_register_28", bool() }, - { "output_bit_register_29", bool() }, - { "output_bit_register_30", bool() }, - { "output_bit_register_31", bool() }, - { "output_bit_register_32", bool() }, - { "output_bit_register_33", bool() }, - { "output_bit_register_34", bool() }, - { "output_bit_register_35", bool() }, - { "output_bit_register_36", bool() }, - { "output_bit_register_37", bool() }, - { "output_bit_register_38", bool() }, - { "output_bit_register_39", bool() }, - { "output_bit_register_40", bool() }, - { "output_bit_register_41", bool() }, - { "output_bit_register_42", bool() }, - { "output_bit_register_43", bool() }, - { "output_bit_register_44", bool() }, - { "output_bit_register_45", bool() }, - { "output_bit_register_46", bool() }, - { "output_bit_register_47", bool() }, - { "output_bit_register_48", bool() }, - { "output_bit_register_49", bool() }, - { "output_bit_register_50", bool() }, - { "output_bit_register_51", bool() }, - { "output_bit_register_52", bool() }, - { "output_bit_register_53", bool() }, - { "output_bit_register_54", bool() }, - { "output_bit_register_55", bool() }, - { "output_bit_register_56", bool() }, - { "output_bit_register_57", bool() }, - { "output_bit_register_58", bool() }, - { "output_bit_register_59", bool() }, - { "output_bit_register_60", bool() }, - { "output_bit_register_61", bool() }, - { "output_bit_register_62", bool() }, - { "output_bit_register_63", bool() }, { "output_bit_register_64", bool() }, { "output_bit_register_65", bool() }, { "output_bit_register_66", bool() }, @@ -315,256 +424,38 @@ std::unordered_map DataPackage::g_ { "output_double_register_45", double() }, { "output_double_register_46", double() }, { "output_double_register_47", double() }, - { "input_bit_registers0_to_31", uint32_t() }, - { "input_bit_registers32_to_63", uint32_t() }, - { "input_bit_register_0", bool() }, - { "input_bit_register_1", bool() }, - { "input_bit_register_2", bool() }, - { "input_bit_register_3", bool() }, - { "input_bit_register_4", bool() }, - { "input_bit_register_5", bool() }, - { "input_bit_register_6", bool() }, - { "input_bit_register_7", bool() }, - { "input_bit_register_8", bool() }, - { "input_bit_register_9", bool() }, - { "input_bit_register_10", bool() }, - { "input_bit_register_11", bool() }, - { "input_bit_register_12", bool() }, - { "input_bit_register_13", bool() }, - { "input_bit_register_14", bool() }, - { "input_bit_register_15", bool() }, - { "input_bit_register_16", bool() }, - { "input_bit_register_17", bool() }, - { "input_bit_register_18", bool() }, - { "input_bit_register_19", bool() }, - { "input_bit_register_20", bool() }, - { "input_bit_register_21", bool() }, - { "input_bit_register_22", bool() }, - { "input_bit_register_23", bool() }, - { "input_bit_register_24", bool() }, - { "input_bit_register_25", bool() }, - { "input_bit_register_26", bool() }, - { "input_bit_register_27", bool() }, - { "input_bit_register_28", bool() }, - { "input_bit_register_29", bool() }, - { "input_bit_register_30", bool() }, - { "input_bit_register_31", bool() }, - { "input_bit_register_32", bool() }, - { "input_bit_register_33", bool() }, - { "input_bit_register_34", bool() }, - { "input_bit_register_35", bool() }, - { "input_bit_register_36", bool() }, - { "input_bit_register_37", bool() }, - { "input_bit_register_38", bool() }, - { "input_bit_register_39", bool() }, - { "input_bit_register_40", bool() }, - { "input_bit_register_41", bool() }, - { "input_bit_register_42", bool() }, - { "input_bit_register_43", bool() }, - { "input_bit_register_44", bool() }, - { "input_bit_register_45", bool() }, - { "input_bit_register_46", bool() }, - { "input_bit_register_47", bool() }, - { "input_bit_register_48", bool() }, - { "input_bit_register_49", bool() }, - { "input_bit_register_50", bool() }, - { "input_bit_register_51", bool() }, - { "input_bit_register_52", bool() }, - { "input_bit_register_53", bool() }, - { "input_bit_register_54", bool() }, - { "input_bit_register_55", bool() }, - { "input_bit_register_56", bool() }, - { "input_bit_register_57", bool() }, - { "input_bit_register_58", bool() }, - { "input_bit_register_59", bool() }, - { "input_bit_register_60", bool() }, - { "input_bit_register_61", bool() }, - { "input_bit_register_62", bool() }, - { "input_bit_register_63", bool() }, - { "input_bit_register_64", bool() }, - { "input_bit_register_65", bool() }, - { "input_bit_register_66", bool() }, - { "input_bit_register_67", bool() }, - { "input_bit_register_68", bool() }, - { "input_bit_register_69", bool() }, - { "input_bit_register_70", bool() }, - { "input_bit_register_71", bool() }, - { "input_bit_register_72", bool() }, - { "input_bit_register_73", bool() }, - { "input_bit_register_74", bool() }, - { "input_bit_register_75", bool() }, - { "input_bit_register_76", bool() }, - { "input_bit_register_77", bool() }, - { "input_bit_register_78", bool() }, - { "input_bit_register_79", bool() }, - { "input_bit_register_80", bool() }, - { "input_bit_register_81", bool() }, - { "input_bit_register_82", bool() }, - { "input_bit_register_83", bool() }, - { "input_bit_register_84", bool() }, - { "input_bit_register_85", bool() }, - { "input_bit_register_86", bool() }, - { "input_bit_register_87", bool() }, - { "input_bit_register_88", bool() }, - { "input_bit_register_89", bool() }, - { "input_bit_register_90", bool() }, - { "input_bit_register_91", bool() }, - { "input_bit_register_92", bool() }, - { "input_bit_register_93", bool() }, - { "input_bit_register_94", bool() }, - { "input_bit_register_95", bool() }, - { "input_bit_register_96", bool() }, - { "input_bit_register_97", bool() }, - { "input_bit_register_98", bool() }, - { "input_bit_register_99", bool() }, - { "input_bit_register_100", bool() }, - { "input_bit_register_101", bool() }, - { "input_bit_register_102", bool() }, - { "input_bit_register_103", bool() }, - { "input_bit_register_104", bool() }, - { "input_bit_register_105", bool() }, - { "input_bit_register_106", bool() }, - { "input_bit_register_107", bool() }, - { "input_bit_register_108", bool() }, - { "input_bit_register_109", bool() }, - { "input_bit_register_110", bool() }, - { "input_bit_register_111", bool() }, - { "input_bit_register_112", bool() }, - { "input_bit_register_113", bool() }, - { "input_bit_register_114", bool() }, - { "input_bit_register_115", bool() }, - { "input_bit_register_116", bool() }, - { "input_bit_register_117", bool() }, - { "input_bit_register_118", bool() }, - { "input_bit_register_119", bool() }, - { "input_bit_register_120", bool() }, - { "input_bit_register_121", bool() }, - { "input_bit_register_122", bool() }, - { "input_bit_register_123", bool() }, - { "input_bit_register_124", bool() }, - { "input_bit_register_125", bool() }, - { "input_bit_register_126", bool() }, - { "input_bit_register_127", bool() }, - { "input_int_register_0", int32_t() }, - { "input_int_register_1", int32_t() }, - { "input_int_register_2", int32_t() }, - { "input_int_register_3", int32_t() }, - { "input_int_register_4", int32_t() }, - { "input_int_register_5", int32_t() }, - { "input_int_register_6", int32_t() }, - { "input_int_register_7", int32_t() }, - { "input_int_register_8", int32_t() }, - { "input_int_register_9", int32_t() }, - { "input_int_register_10", int32_t() }, - { "input_int_register_11", int32_t() }, - { "input_int_register_12", int32_t() }, - { "input_int_register_13", int32_t() }, - { "input_int_register_14", int32_t() }, - { "input_int_register_15", int32_t() }, - { "input_int_register_16", int32_t() }, - { "input_int_register_17", int32_t() }, - { "input_int_register_18", int32_t() }, - { "input_int_register_19", int32_t() }, - { "input_int_register_20", int32_t() }, - { "input_int_register_21", int32_t() }, - { "input_int_register_22", int32_t() }, - { "input_int_register_23", int32_t() }, - { "input_int_register_24", int32_t() }, - { "input_int_register_25", int32_t() }, - { "input_int_register_26", int32_t() }, - { "input_int_register_27", int32_t() }, - { "input_int_register_28", int32_t() }, - { "input_int_register_29", int32_t() }, - { "input_int_register_30", int32_t() }, - { "input_int_register_31", int32_t() }, - { "input_int_register_32", int32_t() }, - { "input_int_register_33", int32_t() }, - { "input_int_register_34", int32_t() }, - { "input_int_register_35", int32_t() }, - { "input_int_register_36", int32_t() }, - { "input_int_register_37", int32_t() }, - { "input_int_register_38", int32_t() }, - { "input_int_register_39", int32_t() }, - { "input_int_register_40", int32_t() }, - { "input_int_register_41", int32_t() }, - { "input_int_register_42", int32_t() }, - { "input_int_register_43", int32_t() }, - { "input_int_register_44", int32_t() }, - { "input_int_register_45", int32_t() }, - { "input_int_register_46", int32_t() }, - { "input_int_register_47", int32_t() }, - { "input_double_register_0", double() }, - { "input_double_register_1", double() }, - { "input_double_register_2", double() }, - { "input_double_register_3", double() }, - { "input_double_register_4", double() }, - { "input_double_register_5", double() }, - { "input_double_register_6", double() }, - { "input_double_register_7", double() }, - { "input_double_register_8", double() }, - { "input_double_register_9", double() }, - { "input_double_register_10", double() }, - { "input_double_register_11", double() }, - { "input_double_register_12", double() }, - { "input_double_register_13", double() }, - { "input_double_register_14", double() }, - { "input_double_register_15", double() }, - { "input_double_register_16", double() }, - { "input_double_register_17", double() }, - { "input_double_register_18", double() }, - { "input_double_register_19", double() }, - { "input_double_register_20", double() }, - { "input_double_register_21", double() }, - { "input_double_register_22", double() }, - { "input_double_register_23", double() }, - { "input_double_register_24", double() }, - { "input_double_register_25", double() }, - { "input_double_register_26", double() }, - { "input_double_register_27", double() }, - { "input_double_register_28", double() }, - { "input_double_register_29", double() }, - { "input_double_register_30", double() }, - { "input_double_register_31", double() }, - { "input_double_register_32", double() }, - { "input_double_register_33", double() }, - { "input_double_register_34", double() }, - { "input_double_register_35", double() }, - { "input_double_register_36", double() }, - { "input_double_register_37", double() }, - { "input_double_register_38", double() }, - { "input_double_register_39", double() }, - { "input_double_register_40", double() }, - { "input_double_register_41", double() }, - { "input_double_register_42", double() }, - { "input_double_register_43", double() }, - { "input_double_register_44", double() }, - { "input_double_register_45", double() }, - { "input_double_register_46", double() }, - { "input_double_register_47", double() }, - { "speed_slider_mask", uint32_t() }, - { "speed_slider_fraction", double() }, - { "standard_digital_output_mask", uint8_t() }, - { "standard_digital_output", uint8_t() }, - { "configurable_digital_output_mask", uint8_t() }, - { "configurable_digital_output", uint8_t() }, - { "tool_digital_output_mask", uint8_t() }, + { "actual_robot_energy_consumed", double() }, + { "actual_robot_braking_energy_dissipated", double() }, + { "encoder0_raw", int32_t() }, + { "encoder1_raw", int32_t() }, + { "euromap67_input_bits", uint32_t() }, + { "euromap67_output_bits", uint32_t() }, + { "euromap67_24V_voltage", double() }, + { "euromap67_24V_current", double() }, + { "tool_mode", uint32_t() }, + { "tool_analog_input_types", uint32_t() }, + { "tool_analog_input0", double() }, + { "tool_analog_input1", double() }, + { "tool_output_voltage", int32_t() }, + { "tool_output_current", double() }, + { "tool_temperature", double() }, { "tool_output_mode", uint8_t() }, { "tool_digital_output0_mode", uint8_t() }, { "tool_digital_output1_mode", uint8_t() }, - { "tool_digital_output", uint8_t() }, + { "tcp_force_scalar", double() }, + { "joint_position_deviation_ratio", double() }, + { "collision_detection_ratio", double() }, + { "ft_raw_wrench", vector6d_t() }, + { "wrench_calc_from_currents", vector6d_t() }, { "payload", double() }, { "payload_cog", vector3d_t() }, { "payload_inertia", vector6d_t() }, { "script_control_line", uint32_t() }, - { "ft_raw_wrench", vector6d_t() }, - { "joint_position_deviation_ratio", double() }, - { "collision_detection_ratio", double() }, { "time_scale_source", int32_t() }, - { "standard_analog_output_mask", uint8_t() }, - { "standard_analog_output_type", uint8_t() }, - { "standard_analog_output_0", double() }, - { "standard_analog_output_1", double() }, - { "tcp_offset", vector6d_t() }, + + // NOT IN OFFICIAL DOCS + { "tool_digital_output_mask", uint8_t() }, + { "tool_digital_output", uint8_t() }, }; void rtde_interface::DataPackage::initEmpty() diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index c804041ad..425dc981d 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -287,7 +287,7 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) unsigned int num_retries = 0; size_t size; size_t written; - uint8_t buffer[8192]; + uint8_t buffer[65536]; URCL_LOG_INFO("Setting up RTDE communication with frequency %f", target_frequency_); while (num_retries < MAX_REQUEST_RETRIES) @@ -717,4 +717,4 @@ std::vector RTDEClient::splitVariableTypes(const std::string& varia return result; } } // namespace rtde_interface -} // 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..e9020e215 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 @@ -51,8 +52,6 @@ static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE"); static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE"); 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"); UrDriver::~UrDriver() { @@ -72,6 +71,8 @@ void UrDriver::init(const UrDriverConfiguration& config) socket_reconnection_timeout_ = config.socket_reconnection_timeout; rtde_initialization_attempts_ = config.rtde_initialization_attempts; rtde_initialization_timeout_ = config.rtde_initialization_timeout; + force_mode_gain_scale_factor_ = config.force_mode_gain_scaling; + force_mode_damping_factor_ = config.force_mode_damping; URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); @@ -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/resources/exhaustive_rtde_output_recipe.txt b/tests/resources/exhaustive_rtde_output_recipe.txt index 0d87fc963..e0df0aa2a 100644 --- a/tests/resources/exhaustive_rtde_output_recipe.txt +++ b/tests/resources/exhaustive_rtde_output_recipe.txt @@ -8,15 +8,21 @@ actual_q actual_qd actual_current actual_current_window +actual_current_as_torque joint_control_output actual_TCP_pose actual_TCP_speed actual_TCP_force target_TCP_pose target_TCP_speed +tcp_offset +actual_TCP_acceleration +target_TCP_acceleration actual_digital_input_bits +actual_configurable_digital_input_bits joint_temperatures actual_execution_time +target_execution_time robot_mode joint_mode safety_mode @@ -30,6 +36,7 @@ actual_robot_voltage actual_robot_current actual_joint_voltage actual_digital_output_bits +actual_configurable_digital_output_bits runtime_state elbow_position elbow_velocity @@ -41,6 +48,10 @@ standard_analog_input1 standard_analog_output0 standard_analog_output1 io_current +actual_robot_energy_consumed +actual_robot_braking_energy_dissipated +encoder0_raw +encoder1_raw euromap67_input_bits euromap67_output_bits euromap67_24V_voltage @@ -52,7 +63,19 @@ tool_analog_input1 tool_output_voltage tool_output_current tool_temperature +tool_output_mode +tool_digital_output0_mode +tool_digital_output1_mode tcp_force_scalar +joint_position_deviation_ratio +collision_detection_ratio +ft_raw_wrench +wrench_calc_from_currents +payload +payload_cog +payload_inertia +script_control_line +time_scale_source output_bit_registers0_to_31 output_bit_registers32_to_63 output_bit_register_64 @@ -281,6 +304,54 @@ input_bit_register_124 input_bit_register_125 input_bit_register_126 input_bit_register_127 +input_int_register_0 +input_int_register_1 +input_int_register_2 +input_int_register_3 +input_int_register_4 +input_int_register_5 +input_int_register_6 +input_int_register_7 +input_int_register_8 +input_int_register_9 +input_int_register_10 +input_int_register_11 +input_int_register_12 +input_int_register_13 +input_int_register_14 +input_int_register_15 +input_int_register_16 +input_int_register_17 +input_int_register_18 +input_int_register_19 +input_int_register_20 +input_int_register_21 +input_int_register_22 +input_int_register_23 +input_int_register_24 +input_int_register_25 +input_int_register_26 +input_int_register_27 +input_int_register_28 +input_int_register_29 +input_int_register_30 +input_int_register_31 +input_int_register_32 +input_int_register_33 +input_int_register_34 +input_int_register_35 +input_int_register_36 +input_int_register_37 +input_int_register_38 +input_int_register_39 +input_int_register_40 +input_int_register_41 +input_int_register_42 +input_int_register_43 +input_int_register_44 +input_int_register_45 +input_int_register_46 +input_int_register_47 input_double_register_0 input_double_register_1 input_double_register_2 @@ -329,14 +400,3 @@ input_double_register_44 input_double_register_45 input_double_register_46 input_double_register_47 -tool_output_mode -tool_digital_output0_mode -tool_digital_output1_mode -payload -payload_cog -payload_inertia -script_control_line -ft_raw_wrench -joint_position_deviation_ratio -collision_detection_ratio -time_scale_source 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); diff --git a/tests/test_start_ursim.bats b/tests/test_start_ursim.bats index 227f3ab92..ab368f616 100644 --- a/tests/test_start_ursim.bats +++ b/tests/test_start_ursim.bats @@ -265,6 +265,24 @@ setup() { [ $status -eq 0 ] } +@test "test ur18 min version" { + run test_input_handling -m ur18 -v 3.14.3 + echo "$output" + [ $status -eq 1 ] + run test_input_handling -m ur18 -v 5.22.3 + echo "$output" + [ $status -eq 1 ] + run test_input_handling -m ur18 -v 5.23.0 + echo "$output" + [ $status -eq 0 ] + run test_input_handling -m ur18 -v 10.10.0 + echo "$output" + [ $status -eq 1 ] + run test_input_handling -m ur18 -v 10.11.0 + echo "$output" + [ $status -eq 0 ] +} + @test "unsupported versions raise error" { run main -v 1.2.3 -t echo "$output"