diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 4527f0d33..fac7b0c3b 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -13,7 +13,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun - ``zeroFTSensor()``: Zeros the force/torque sensor. - ``setPayload()``: Set the active payload mass and center of gravity. -- ``setToolVoltage()``: Set the voltage of the tool output. +- ``setToolVoltage()``: Set the voltage of the tool output on connector T0. - ``startForceMode()`` / ``endForceMode()``: Start and end a force mode. See the `force mode example `_ for more information. - ``startToolContact()`` / ``endToolContact()``: Start and end a tool contact mode. See the `tool @@ -26,6 +26,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun - ``setTcpOffset()``: Set the TCP offset of the robot. - ``setFrictionScales()``: Set viscous and Coulomb friction scale factors for direct torque control. - ``setTargetPayload()``: Set the active payload mass, center of gravity, inertia matrix and transition time. +- ``setToolVoltageT2()``: Set the tool voltage on connector T2. Requires PolyScope X 10.15.0 or later and Tool Flange + V2, as reported by the RTDE robot properties. PolyScope 5 does not support connector T2. Communication protocol ---------------------- @@ -60,6 +62,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 10: setTcpOffset - 11: setFrictionScales - 12: setTargetPayload + - 13: setToolVoltageT2 1-27 data fields specific to the command ===== ===== @@ -194,6 +197,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 11 Payload transition time in seconds (floating point) ===== ===== +.. table:: With setToolVoltageT2 command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1 Voltage in V (Has to be 0, 24 or 48) + ===== ===== + .. note:: In URScript the ``socket_read_binary_integer()`` function is used to read the data from the script command socket. The first index in that function's return value is the number of integers read, diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 60580aaf6..68b589150 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -46,6 +46,7 @@ std::unique_ptr g_my_robot; bool g_HEADLESS = true; bool g_running = false; bool g_support_set_friction_scales = false; +bool g_support_set_tool_t2_voltage = false; void sendScriptCommands() { @@ -84,6 +85,14 @@ void sendScriptCommands() g_my_robot->getUrDriver()->setTargetPayload(2.0, { 0.0, 0.0, 0.1 }, { 0.003, 0.003, 0.003, 0.0, 0.0, 0.0 }, 0.5); }); + if (g_support_set_tool_t2_voltage) + { + run_cmd("Setting tool T2 voltage to 48V", + []() { g_my_robot->getUrDriver()->setToolVoltageT2(urcl::ToolVoltage::_48V); }); + run_cmd("Setting tool T2 voltage to 0V", + []() { g_my_robot->getUrDriver()->setToolVoltageT2(urcl::ToolVoltage::OFF); }); + } + if (g_support_set_friction_scales) { // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards. @@ -135,6 +144,13 @@ int main(int argc, char* argv[]) version.toString().c_str()); } + // Connector T2 only exists on Tool Flange V2 + g_support_set_tool_t2_voltage = g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() == ToolFlangeType::V2; + if (!g_support_set_tool_t2_voltage) + { + URCL_LOG_INFO("This robot does not report Tool Flange V2. Skipping tool T2 voltage commands."); + } + if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index d314fe28c..d477911c6 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -116,14 +116,27 @@ class ScriptCommandInterface : public ReverseInterface bool setGravity(const vector3d_t* gravity); /*! - * \brief Set the tool voltage. + * \brief Set the tool voltage on connector T0. * - * \param voltage Tool voltage + * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES. * - * \returns True, if the write was performed successfully, false otherwise. + * \returns True, if the write was performed successfully, false if the voltage is invalid or the write fails. */ bool setToolVoltage(const ToolVoltage voltage); + /*! + * \brief Set the tool voltage on connector T2 (Tool Flange V2). + * + * Requires PolyScope X 10.15.0 or later; PolyScope 5 does not support connector T2. This does not + * check the tool flange; UrDriver::setToolVoltageT2() does. + * + * \param voltage Tool voltage. Must be one of \ref T2_TOOL_VOLTAGES. + * + * \returns True, if the write was performed successfully, false if the voltage is invalid, the version is + * unsupported, or the write fails. + */ + bool setToolVoltageT2(const ToolVoltage voltage); + /*! * \brief Set robot to be controlled in force mode. * @@ -279,6 +292,7 @@ class ScriptCommandInterface : public ReverseInterface SET_TCP_OFFSET = 10, ///< Set TCP offset SET_FRICTION_SCALES = 11, ///< Set viscous and Coulomb friction scales for direct_torque SET_TARGET_PAYLOAD = 12, ///< Set target payload + SET_TOOL_T2_VOLTAGE = 13, ///< Set tool voltage on connector T2 }; /*! @@ -300,6 +314,30 @@ class ScriptCommandInterface : public ReverseInterface bool robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, const VersionInformation& min_polyscopeX, const std::string& command_name); + /*! + * \brief Checks if the robot runs PolyScope X of at least \p min_polyscopeX, for commands that PolyScope 5 + * does not support in any version. + * + * If not, this function will log a warning message. + * + * \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 at least \p min_polyscopeX, false otherwise. + */ + bool robotVersionSupportsPolyscopeXCommandOrWarn(const VersionInformation& min_polyscopeX, + const std::string& command_name); + + /*! + * \brief Write a script command and one integer argument, padded with zeros to \ref MAX_MESSAGE_LENGTH. + * + * \param command Script command id + * \param argument Argument already scaled the way the external control script expects it + * + * \returns True if the write was performed successfully, false otherwise. + */ + bool writeCommandWithArgument(const ScriptCommand command, const int32_t argument); + std::atomic client_connected_; static const int MAX_MESSAGE_LENGTH = 28; diff --git a/include/ur_client_library/ur/tool_communication.h b/include/ur_client_library/ur/tool_communication.h index ab5d65465..2242e8047 100644 --- a/include/ur_client_library/ur/tool_communication.h +++ b/include/ur_client_library/ur/tool_communication.h @@ -31,7 +31,13 @@ #define UR_CLIENT_LIBRARY_UR_TOOL_COMMUNICATION_H_INCLUDED #include "ur_client_library/types.h" +#include +#include +#include +#include #include +#include +#include namespace urcl { @@ -42,9 +48,65 @@ enum class ToolVoltage : int { OFF = 0, ///< 0V _12V = 12, ///< 12V - _24V = 24 ///< 24V + _24V = 24, ///< 24V + _48V = 48 ///< 48V }; +/*! + * \brief Voltages the standard tool connector (T0) accepts. + * + * Connector T0 does not support 48 V. + */ +inline constexpr std::array T0_TOOL_VOLTAGES{ ToolVoltage::OFF, ToolVoltage::_12V, ToolVoltage::_24V }; + +/*! + * \brief Voltages connector T2 accepts. + */ +inline constexpr std::array T2_TOOL_VOLTAGES{ ToolVoltage::OFF, ToolVoltage::_24V, ToolVoltage::_48V }; + +/*! + * \brief Decimal voltage of a \ref ToolVoltage, including values outside the enum. + */ +inline std::string toString(const ToolVoltage voltage) +{ + return std::to_string(toUnderlying(voltage)); +} + +/*! + * \brief True when \p voltage is one of \p allowed. + */ +template +bool isAllowedToolVoltage(const ToolVoltage voltage, const Range& allowed) +{ + return std::any_of(std::begin(allowed), std::end(allowed), + [voltage](const ToolVoltage candidate) { return candidate == voltage; }); +} + +/*! + * \brief Rejection text for a voltage that is not in \p allowed. + * + * The allowed values are rendered with \ref toString, so the message stays in step with the list. + */ +template +std::string toolVoltageNotAllowedMessage(const std::string_view connector, const ToolVoltage voltage, + const Range& allowed) +{ + std::string allowed_text; + const std::size_t count = static_cast(std::distance(std::begin(allowed), std::end(allowed))); + std::size_t index = 0; + for (const ToolVoltage candidate : allowed) + { + if (index > 0) + { + allowed_text += (index + 1 == count) ? " or " : ", "; + } + allowed_text += toString(candidate); + ++index; + } + return "The " + std::string(connector) + " voltage should be " + allowed_text + ". The voltage is " + + toString(voltage) + "."; +} + /*! * \brief Possible values for th parity flag */ @@ -127,13 +189,12 @@ class ToolCommSetup using TxIdleCharsT = Limited; /*! - * \brief Setup the tool voltage that will be configured on the robot. This will not immediately + * \brief Setup the tool voltage on connector T0 that will be configured on the robot. This will not immediately * change values on the robot, it will only be stored inside the ToolCommSetup object. + * + * \param tool_voltage must be one of \ref T0_TOOL_VOLTAGES or an exception will be thrown. */ - void setToolVoltage(const ToolVoltage tool_voltage) - { - tool_voltage_ = tool_voltage; - } + void setToolVoltage(const ToolVoltage tool_voltage); /*! * \brief Return the tool voltage currently stored @@ -143,6 +204,25 @@ class ToolCommSetup return tool_voltage_; } + /*! + * \brief Setup the tool voltage on connector T2 that will be configured on the robot. This will not immediately + * change values on the robot, it will only be stored inside the ToolCommSetup object. + * + * Connector T2 requires PolyScope X 10.15.0 or later and a robot whose RTDE robot properties report Tool Flange + * V2. If it is set and the robot does not report Tool Flange V2, the UrDriver constructor will throw. + * + * \param tool_voltage must be one of \ref T2_TOOL_VOLTAGES or an exception will be thrown. + */ + void setToolVoltageT2(const ToolVoltage tool_voltage); + + /*! + * \brief Return the tool voltage on connector T2 currently stored, or no value if it has not been set. + */ + std::optional getToolVoltageT2() const + { + return tool_voltage_t2_; + } + /*! * \brief Setup the tool communication parity that will be configured on the robot. This will not immediately * change values on the robot, it will only be stored inside the ToolCommSetup object. @@ -239,6 +319,7 @@ class ToolCommSetup static_cast(5e6) }; ToolVoltage tool_voltage_; + std::optional tool_voltage_t2_; Parity parity_; uint32_t baud_rate_; StopBitsT stop_bits_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 53f54fdb3..50ca75276 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -677,15 +677,30 @@ class UrDriver bool setGravity(const vector3d_t& gravity); /*! - * \brief Set the tool voltage. Note: It requires the external control script to be running or the robot to be in - * headless mode. + * \brief Set the tool voltage on connector T0. Note: It requires the external control script to be running or the + * robot to be in headless mode. * - * \param voltage tool voltage. + * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES. * * \returns True on successful write. */ bool setToolVoltage(const ToolVoltage voltage); + /*! + * \brief Set the tool voltage on connector T2 (Tool Flange V2). + * + * Requires PolyScope X 10.15.0 or later and a robot whose RTDE robot properties report Tool Flange V2. PolyScope 5 + * does not support connector T2. + * Note: It requires the external control script to be running or the robot to be in remote_control / headless mode. + * State can be read back via RTDE output fields tool_output_voltage_2 and tool_output_current_2. + * + * \param voltage Tool voltage. Must be one of \ref T2_TOOL_VOLTAGES. + * + * \returns True on successful write, false if voltage is invalid, the RTDE robot properties do not report Tool + * Flange V2, or the write fails. + */ + bool setToolVoltageT2(const ToolVoltage voltage); + /*! * \brief Start the robot to be controlled in force mode. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 083b5f94f..a048b94cf 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -79,6 +79,7 @@ SET_GRAVITY = 9 SET_TCP_OFFSET = 10 SET_FRICTION_SCALES = 11 SET_TARGET_PAYLOAD = 12 +SET_TOOL_T2_VOLTAGE = 13 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -1011,6 +1012,13 @@ thread script_commands(): elif command == SET_TCP_OFFSET: tcp_offset = p[raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] set_tcp(tcp_offset) + elif command == SET_TOOL_T2_VOLTAGE: + tool_voltage_t2 = raw_command[2] / MULT_jointstate + {% if ROBOT_SOFTWARE_VERSION >= v10.15.0 %} + set_power_output("T2_V", tool_voltage_t2) + {% else %} + textmsg("Tool voltage T2 requires PolyScope X 10.15.0 or later. Ignored.") + {% endif %} end end end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 600839848..1d327332e 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -154,15 +154,15 @@ bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) return server_.write(client_fd_, buffer, sizeof(buffer), written); } -bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) +bool ScriptCommandInterface::writeCommandWithArgument(const ScriptCommand command, const int32_t argument) { 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_TOOL_VOLTAGE)); + int32_t val = htobe32(toUnderlying(command)); b_pos += append(b_pos, val); - val = htobe32(toUnderlying(voltage) * MULT_JOINTSTATE); + val = htobe32(argument); b_pos += append(b_pos, val); // writing zeros to allow usage with other script commands @@ -176,6 +176,30 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) +{ + if (!isAllowedToolVoltage(voltage, T0_TOOL_VOLTAGES)) + { + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool", voltage, T0_TOOL_VOLTAGES).c_str()); + return false; + } + return writeCommandWithArgument(ScriptCommand::SET_TOOL_VOLTAGE, toUnderlying(voltage) * MULT_JOINTSTATE); +} + +bool ScriptCommandInterface::setToolVoltageT2(const ToolVoltage voltage) +{ + if (!isAllowedToolVoltage(voltage, T2_TOOL_VOLTAGES)) + { + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool T2", voltage, T2_TOOL_VOLTAGES).c_str()); + return false; + } + if (!robotVersionSupportsPolyscopeXCommandOrWarn(urcl::VersionInformation::fromString("10.15.0"), __func__)) + { + return false; + } + return writeCommandWithArgument(ScriptCommand::SET_TOOL_T2_VOLTAGE, toUnderlying(voltage) * MULT_JOINTSTATE); +} + bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector, const vector6d_t* wrench, const unsigned int type, const vector6d_t* limits, double damping_factor, double gain_scaling_factor) @@ -491,5 +515,18 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo return true; } +bool ScriptCommandInterface::robotVersionSupportsPolyscopeXCommandOrWarn(const VersionInformation& min_polyscopeX, + const std::string& command_name) +{ + if (robot_software_version_ < min_polyscopeX) + { + URCL_LOG_WARN("%s is only available for robots with PolyScope X %s or later. This robot's version is %s. This " + "command will have no effect.", + command_name.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 diff --git a/src/ur/tool_communication.cpp b/src/ur/tool_communication.cpp index d44596dcf..e133ed568 100644 --- a/src/ur/tool_communication.cpp +++ b/src/ur/tool_communication.cpp @@ -40,6 +40,24 @@ ToolCommSetup::ToolCommSetup() { } +void ToolCommSetup::setToolVoltage(const ToolVoltage tool_voltage) +{ + if (!isAllowedToolVoltage(tool_voltage, T0_TOOL_VOLTAGES)) + { + throw std::runtime_error(toolVoltageNotAllowedMessage("tool", tool_voltage, T0_TOOL_VOLTAGES)); + } + tool_voltage_ = tool_voltage; +} + +void ToolCommSetup::setToolVoltageT2(const ToolVoltage tool_voltage) +{ + if (!isAllowedToolVoltage(tool_voltage, T2_TOOL_VOLTAGES)) + { + throw std::runtime_error(toolVoltageNotAllowedMessage("tool T2", tool_voltage, T2_TOOL_VOLTAGES)); + } + tool_voltage_t2_ = tool_voltage; +} + void ToolCommSetup::setBaudRate(const uint32_t baud_rate) { if (baud_rates_allowed_.find(baud_rate) != baud_rates_allowed_.end()) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index dd59dfa03..9fb07d2f0 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -137,6 +137,18 @@ void UrDriver::init(const UrDriverConfiguration& config) begin_replace << "set_tool_voltage(" << static_cast::type>(config.tool_comm_setup->getToolVoltage()) << ")\n"; + const std::optional tool_voltage_t2 = config.tool_comm_setup->getToolVoltageT2(); + if (tool_voltage_t2.has_value()) + { + const ToolFlangeType flange = rtde_client_->getWriter().getToolFlangeType(); + if (flange != ToolFlangeType::V2) + { + throw UrException("Tool voltage on connector T2 requested, but it is only available on Tool Flange V2. The " + "robot properties report " + + toolFlangeTypeString(flange) + ". Please check your configuration."); + } + begin_replace << "set_power_output(\"T2_V\", " << toUnderlying(*tool_voltage_t2) << ")\n"; + } begin_replace << "set_tool_communication(" << "True" << ", " << config.tool_comm_setup->getBaudRate() << ", " << static_cast::type>(config.tool_comm_setup->getParity()) << ", " << config.tool_comm_setup->getStopBits() << ", " << config.tool_comm_setup->getRxIdleChars() << ", " @@ -388,23 +400,13 @@ bool UrDriver::setGravity(const vector3d_t& gravity) bool UrDriver::setToolVoltage(const ToolVoltage voltage) { - // Test that the tool voltage is either 0, 12 or 24. - switch (voltage) + if (!isAllowedToolVoltage(voltage, T0_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - break; - case ToolVoltage::_12V: - break; - case ToolVoltage::_24V: - break; - default: - std::stringstream ss; - ss << "The tool voltage should be 0, 12 or 24. The tool voltage is " << toUnderlying(voltage); - URCL_LOG_ERROR(ss.str().c_str()); - return false; + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool", voltage, T0_TOOL_VOLTAGES).c_str()); + return false; } - if (script_command_interface_->clientConnected()) + if (script_command_interface_ != nullptr && script_command_interface_->clientConnected()) { return script_command_interface_->setToolVoltage(voltage); } @@ -417,6 +419,39 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) return sendScript(cmd.str()); } } + +bool UrDriver::setToolVoltageT2(const ToolVoltage voltage) +{ + if (!isAllowedToolVoltage(voltage, T2_TOOL_VOLTAGES)) + { + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool T2", voltage, T2_TOOL_VOLTAGES).c_str()); + return false; + } + + const ToolFlangeType flange = rtde_client_->getWriter().getToolFlangeType(); + if (flange != ToolFlangeType::V2) + { + URCL_LOG_ERROR("Tool voltage on connector T2 is only available on Tool Flange V2. The robot properties report %s.", + toolFlangeTypeString(flange).c_str()); + return false; + } + + if (script_command_interface_ != nullptr && script_command_interface_->clientConnected()) + { + return script_command_interface_->setToolVoltageT2(voltage); + } + else + { + URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. This will only " + "work if the robot is in remote_control mode."); + std::stringstream cmd; + cmd << "sec setup():" << std::endl + << " set_power_output(\"T2_V\", " << toUnderlying(voltage) << ")" << std::endl + << "end"; + return sendScript(cmd.str()); + } +} + // Function for e-series robots (Needs both damping factor and gain scaling factor) bool UrDriver::startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench, const unsigned int type, const vector6d_t& limits, diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index beda5d0eb..36f016703 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -274,6 +274,15 @@ set_tests_properties(RTDEClientReconnectTest.destructor_not_blocked_by_stuck_rec RTDEClientReconnectTest.reconnect_gives_up_when_the_handshake_keeps_failing PROPERTIES TIMEOUT 60) +# Covers UrDriver's tool voltage handling against the in-process fake primary and RTDE servers, so it +# runs without a robot. +add_executable(ur_driver_tool_voltage_tests test_ur_driver_tool_voltage.cpp fake_primary_server.cpp + fake_rtde_server.cpp) +target_link_libraries(ur_driver_tool_voltage_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET ur_driver_tool_voltage_tests + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +) + add_executable(tcp_server_tests test_tcp_server.cpp) if (MSVC) target_compile_options(tcp_server_tests PRIVATE /Zc:lambda) @@ -416,6 +425,11 @@ target_link_libraries(types_tests PRIVATE ur_client_library::urcl GTest::gtest_m gtest_add_tests(TARGET types_tests ) +add_executable(tool_communication_tests test_tool_communication.cpp) +target_link_libraries(tool_communication_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET tool_communication_tests +) + add_executable(datatypes_tests test_datatypes.cpp) target_link_libraries(datatypes_tests PRIVATE ur_client_library::urcl GTest::gtest_main) gtest_add_tests(TARGET datatypes_tests diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 328b7f85a..de29f0c51 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -421,7 +421,9 @@ const std::unordered_map VARIABLE_TYPES{ { "tool_analog_input0", "DOUBLE" }, { "tool_analog_input1", "DOUBLE" }, { "tool_output_voltage", "INT32" }, + { "tool_output_voltage_2", "INT32" }, { "tool_output_current", "DOUBLE" }, + { "tool_output_current_2", "DOUBLE" }, { "tool_temperature", "DOUBLE" }, { "tool_output_mode", "UINT8" }, { "tool_digital_output0_mode", "UINT8" }, @@ -625,6 +627,19 @@ void RTDEServer::setSoftwareVersionAsUint32(const bool as_uint32) software_version_as_uint32_ = as_uint32; } +void RTDEServer::setReportedToolFlangeType(const ToolFlangeType type) +{ + std::lock_guard lock(negotiation_mutex_); + tool_flange_type_ = type; +} + +void RTDEServer::setReportedUrControlVersion(const uint32_t major, const uint32_t minor, const uint32_t bugfix, + const uint32_t build) +{ + std::lock_guard lock(negotiation_mutex_); + urcontrol_version_ = { major, minor, bugfix, build }; +} + void RTDEServer::queueStartReplyBeforeReadProperties() { std::lock_guard lock(negotiation_mutex_); @@ -779,9 +794,11 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa { // The client only asks once, so every queued message has to go out now for it to see them all std::deque text_messages; + std::array version; { std::lock_guard lock(negotiation_mutex_); text_messages.swap(pending_text_messages_); + version = urcontrol_version_; } for (const std::string& text_message : text_messages) { @@ -793,11 +810,10 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa size_t send_size = 0; send_size += rtde_interface::PackageHeader::serializeHeader( send_buffer, rtde_interface::PackageType::RTDE_GET_URCONTROL_VERSION, 4 * sizeof(uint32_t)); - uint32_t version = 10; - send_size += serializer.serialize(send_buffer + send_size, version); // major - send_size += serializer.serialize(send_buffer + send_size, version); // minor - send_size += serializer.serialize(send_buffer + send_size, version); // bugfix - send_size += serializer.serialize(send_buffer + send_size, version); // build + for (const uint32_t part : version) // major, minor, bugfix, build + { + send_size += serializer.serialize(send_buffer + send_size, part); + } size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); @@ -1025,7 +1041,7 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa URCL_LOG_WARN("Received Text message which usually shouldn't be sent to the RTDE server."); break; } - // Mimics a configurable controller (defaults to 10.15.0) on a CB5 with a standard tool flange. Like the + // Mimics a configurable controller (defaults to 10.15.0) on a CB5 with a configurable tool flange. Like the // real one it reports NOT_FOUND for unknown names and then sends no values at all, so tests can check the // client only asks for names the controller has. case rtde_interface::PackageType::RTDE_READ_PROPERTIES: @@ -1039,6 +1055,7 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa uint16_t minor = 15; uint16_t bugfix = 0; bool as_uint32 = false; + ToolFlangeType tool_flange_type = ToolFlangeType::V1; unsigned start_replies = 0; { std::lock_guard lock(negotiation_mutex_); @@ -1050,6 +1067,7 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa minor = software_version_minor_; bugfix = software_version_bugfix_; as_uint32 = software_version_as_uint32_; + tool_flange_type = tool_flange_type_; start_replies = pending_start_replies_before_read_properties_; pending_start_replies_before_read_properties_ = 0; } @@ -1129,7 +1147,7 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa } else if (name == "v1.robot_arm.tool_flange.type") { - const uint32_t encoded = static_cast(1) << 24; + const uint32_t encoded = static_cast(tool_flange_type) << 24; values_size += serializer.serialize(values + values_size, encoded); } } diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index b6d58b52a..cc6a4b666 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -11,6 +12,7 @@ #include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/rtde/rtde_package.h" #include "ur_client_library/rtde/rtde_parser.h" +#include "ur_client_library/ur/datatypes.h" namespace urcl { @@ -128,6 +130,17 @@ class RTDEServer */ void setSoftwareVersionAsUint32(const bool as_uint32); + /*! + * \brief Tool flange type reported for v1.robot_arm.tool_flange.type. Defaults to V1. + */ + void setReportedToolFlangeType(const ToolFlangeType type); + + /*! + * \brief Version reported in the RTDE_GET_URCONTROL_VERSION answer. Defaults to 10.10.10.10. + */ + void setReportedUrControlVersion(const uint32_t major, const uint32_t minor, const uint32_t bugfix, + const uint32_t build); + /*! * \brief Sends an accepted RTDE_CONTROL_PACKAGE_START reply ahead of the next RTDE_READ_PROPERTIES * answer: a package that is neither the answer nor a text message. @@ -196,6 +209,8 @@ class RTDEServer uint16_t software_version_minor_ = 15; uint16_t software_version_bugfix_ = 0; bool software_version_as_uint32_ = false; + ToolFlangeType tool_flange_type_ = ToolFlangeType::V1; + std::array urcontrol_version_{ 10, 10, 10, 10 }; unsigned pending_start_replies_before_read_properties_ = 0; std::deque pending_after_start_text_messages_; bool accept_start_ = true; diff --git a/tests/test_rtde_client_fake_server.cpp b/tests/test_rtde_client_fake_server.cpp index 60fab83d1..d4aec2d38 100644 --- a/tests/test_rtde_client_fake_server.cpp +++ b/tests/test_rtde_client_fake_server.cpp @@ -1405,6 +1405,26 @@ TEST_F(RTDEClientFakeServerTest, non_data_package_during_boot_check_retries_the_ EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); } +TEST_F(RTDEClientFakeServerTest, setup_outputs_with_tool_power_t2_fields) +{ + const std::vector output_recipe{ "timestamp", "tool_output_voltage_2", "tool_output_current_2" }; + auto client = makeClient(output_recipe, INPUT_RECIPE, RTDE_FREQUENCY); + ASSERT_TRUE(client->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10))); + ASSERT_TRUE(client->start(true)); + + rtde_interface::DataPackage output(output_recipe); + ASSERT_TRUE(client->getDataPackage(output, std::chrono::seconds(1))); + EXPECT_TRUE(output.isTyped()); + EXPECT_EQ(output.getDataType("tool_output_voltage_2"), rtde_interface::DataType::INT32); + EXPECT_EQ(output.getDataType("tool_output_current_2"), rtde_interface::DataType::DOUBLE); + + int32_t voltage = 0; + double current = 0.0; + EXPECT_TRUE(output.getData("tool_output_voltage_2", voltage)); + EXPECT_TRUE(output.getData("tool_output_current_2", current)); + EXPECT_TRUE(client->pause()); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp index 91b0f97a1..d56167542 100644 --- a/tests/test_rtde_client_reconnect.cpp +++ b/tests/test_rtde_client_reconnect.cpp @@ -205,6 +205,7 @@ TEST_F(RTDEClientReconnectTest, reconnect_reads_the_robot_properties_again) ASSERT_TRUE(client_->init(0, std::chrono::milliseconds(123), 3, std::chrono::milliseconds(100))); const auto properties = client_->getRobotProperties(); ASSERT_TRUE(properties.has_value()); + EXPECT_EQ(client_->getWriter().getToolFlangeType(), ToolFlangeType::V1); client_->start(); server_.reset(); @@ -216,6 +217,7 @@ TEST_F(RTDEClientReconnectTest, reconnect_reads_the_robot_properties_again) server_->setHighestAcceptedProtocolVersion(2); ASSERT_TRUE(waitForState(rtde_interface::ClientState::RUNNING)) << "the client did not reconnect"; EXPECT_FALSE(client_->getRobotProperties().has_value()); + EXPECT_EQ(client_->getWriter().getToolFlangeType(), ToolFlangeType::UNKNOWN); EXPECT_TRUE(properties->getSoftwareVersion().has_value()); EXPECT_EQ(properties->getSoftwareVersion()->major, 10u); EXPECT_TRUE(server_->propertyRequests().empty()); diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 63a4f07fd..0f24d14e2 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -298,6 +298,49 @@ TEST_F(ScriptCommandInterfaceTest, test_set_tool_voltage) EXPECT_EQ(message_sum, expected_message_sum); } +TEST_F(ScriptCommandInterfaceTest, test_set_tool_voltage_rejects_invalid_voltage) +{ + waitForClientConnection(); + + EXPECT_FALSE(script_command_interface_->setToolVoltage(ToolVoltage::_48V)); + EXPECT_FALSE(script_command_interface_->setToolVoltage(static_cast(5))); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_tool_t2_voltage_rejects_invalid_voltage) +{ + waitForClientConnection(); + + EXPECT_FALSE(script_command_interface_->setToolVoltageT2(ToolVoltage::_12V)); + EXPECT_FALSE(script_command_interface_->setToolVoltageT2(static_cast(5))); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_tool_t2_voltage) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + ToolVoltage voltage = ToolVoltage::_48V; + bool result = script_command_interface_->setToolVoltageT2(voltage); + ASSERT_TRUE(result); + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 13 is set tool T2 voltage + int32_t expected_command = 13; + EXPECT_EQ(command, expected_command); + + // Test tool T2 voltage + ASSERT_FALSE(message.empty()); + int received_voltage = message[0] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_voltage, toUnderlying(voltage)); + + // 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); +} + TEST_F(ScriptCommandInterfaceTest, test_force_mode) { // Wait for the client to connect to the server @@ -684,6 +727,29 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_scales_returns_false_on_old old_client->close(); } +TEST_F(ScriptCommandInterfaceTest, test_set_tool_t2_voltage_returns_false_on_unsupported_version) +{ + // PolyScope 5 has no connector T2 in any version, PolyScope X from 10.15.0 + const std::vector> unsupported{ { "10.14.0", 50006 }, + { "5.26.0", 50007 }, + { "5.99.0", 50008 } }; + for (const auto& [version, port] : unsupported) + { + control::ReverseInterfaceConfig config; + config.port = port; + config.robot_software_version = VersionInformation::fromString(version); + control::ScriptCommandInterface old_version_interface(config); + std::unique_ptr old_client(new Client(port)); + + waitFor([&old_version_interface]() { return old_version_interface.clientConnected(); }, + std::chrono::milliseconds(1000)); + + EXPECT_FALSE(old_version_interface.setToolVoltageT2(ToolVoltage::_48V)) << "version " << version; + + old_client->close(); + } +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 71fd62b6c..41013b0bd 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -644,6 +644,41 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler) std::string::npos); } +TEST_F(ScriptReaderTest, TestToolT2VoltageScriptRendering) +{ + std::string existing_script_file = "../resources/external_control.urscript"; + ScriptReader reader; + ScriptReader::DataDict data; + data["BEGIN_REPLACE"] = ""; + data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); + data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC); + data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; + data["SERVER_IP_REPLACE"] = "1.2.3.4"; + data["SERVER_PORT_REPLACE"] = "50001"; + data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; + data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + + // PolyScope 10.15.0 supports set_power_output + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.15.0"); + std::string processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_NE(processed_script.find("SET_TOOL_T2_VOLTAGE = 13"), std::string::npos); + EXPECT_NE(processed_script.find("set_power_output(\"T2_V\", tool_voltage_t2)"), std::string::npos); + + // Older version 10.14.0 does not call set_power_output + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.14.0"); + processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_EQ(processed_script.find("set_power_output(\"T2_V\", tool_voltage_t2)"), std::string::npos); + + // PolyScope 5 does not support connector T2 in any version + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.26.0"); + processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_EQ(processed_script.find("set_power_output(\"T2_V\", tool_voltage_t2)"), std::string::npos); + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.99.0"); + processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_EQ(processed_script.find("set_power_output(\"T2_V\", tool_voltage_t2)"), std::string::npos); +} + // --- Security regression tests ------------------------------------------------------------------- namespace diff --git a/tests/test_tool_communication.cpp b/tests/test_tool_communication.cpp new file mode 100644 index 000000000..62f506a4e --- /dev/null +++ b/tests/test_tool_communication.cpp @@ -0,0 +1,132 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 + +using namespace urcl; + +TEST(ToolVoltageTest, rejection_message_lists_allowed_voltages) +{ + EXPECT_EQ(toString(ToolVoltage::_48V), "48"); + EXPECT_EQ(toString(static_cast(5)), "5"); + EXPECT_EQ(toolVoltageNotAllowedMessage("tool", ToolVoltage::_48V, T0_TOOL_VOLTAGES), "The tool voltage should " + "be 0, 12 or 24. The " + "voltage is 48."); + EXPECT_EQ(toolVoltageNotAllowedMessage("tool T2", static_cast(5), T2_TOOL_VOLTAGES), "The tool T2 " + "voltage should be " + "0, 24 or 48. The " + "voltage is 5."); +} + +TEST(ToolCommSetupTest, set_tool_voltage_accepts_t0_voltages) +{ + ToolCommSetup setup; + for (const ToolVoltage voltage : T0_TOOL_VOLTAGES) + { + EXPECT_NO_THROW(setup.setToolVoltage(voltage)); + EXPECT_EQ(setup.getToolVoltage(), voltage); + } +} + +// UrDriver renders the stored voltage as set_tool_voltage() for connector T0 at startup, so a voltage T0 does not +// support must never be stored. +TEST(ToolCommSetupTest, set_tool_voltage_rejects_voltages_t0_does_not_support) +{ + ToolCommSetup setup; + setup.setToolVoltage(ToolVoltage::_24V); + + try + { + setup.setToolVoltage(ToolVoltage::_48V); + FAIL() << "Expected std::runtime_error"; + } + catch (const std::runtime_error& error) + { + EXPECT_EQ(std::string(error.what()), toolVoltageNotAllowedMessage("tool", ToolVoltage::_48V, T0_TOOL_VOLTAGES)); + } + try + { + setup.setToolVoltage(static_cast(5)); + FAIL() << "Expected std::runtime_error"; + } + catch (const std::runtime_error& error) + { + EXPECT_EQ(std::string(error.what()), + toolVoltageNotAllowedMessage("tool", static_cast(5), T0_TOOL_VOLTAGES)); + } + EXPECT_EQ(setup.getToolVoltage(), ToolVoltage::_24V); +} + +TEST(ToolCommSetupTest, tool_voltage_t2_is_unset_by_default) +{ + ToolCommSetup setup; + EXPECT_FALSE(setup.getToolVoltageT2().has_value()); +} + +TEST(ToolCommSetupTest, set_tool_voltage_t2_accepts_t2_voltages) +{ + ToolCommSetup setup; + for (const ToolVoltage voltage : T2_TOOL_VOLTAGES) + { + EXPECT_NO_THROW(setup.setToolVoltageT2(voltage)); + EXPECT_EQ(setup.getToolVoltageT2(), voltage); + } +} + +TEST(ToolCommSetupTest, set_tool_voltage_t2_rejects_voltages_t2_does_not_support) +{ + ToolCommSetup setup; + setup.setToolVoltageT2(ToolVoltage::_48V); + + try + { + setup.setToolVoltageT2(ToolVoltage::_12V); + FAIL() << "Expected std::runtime_error"; + } + catch (const std::runtime_error& error) + { + EXPECT_EQ(std::string(error.what()), toolVoltageNotAllowedMessage("tool T2", ToolVoltage::_12V, T2_TOOL_VOLTAGES)); + } + try + { + setup.setToolVoltageT2(static_cast(5)); + FAIL() << "Expected std::runtime_error"; + } + catch (const std::runtime_error& error) + { + EXPECT_EQ(std::string(error.what()), + toolVoltageNotAllowedMessage("tool T2", static_cast(5), T2_TOOL_VOLTAGES)); + } + EXPECT_EQ(setup.getToolVoltageT2(), ToolVoltage::_48V); +} diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 825cd97dc..37829ea0e 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -33,7 +33,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -409,6 +411,86 @@ TEST_F(UrDriverTest, set_target_payload) EXPECT_TRUE(g_my_robot->getUrDriver()->setTargetPayload(0, { 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, 0.002)); } +// Older controllers reject tool_output_voltage_2, so it stays out of OUTPUT_RECIPE_VECTOR. +// The value is measured: on a robot it reads e.g. 23 V for 24 V, and with nothing connected it takes +// seconds to decay after switching off. 5 V still tells 0, 24 and 48 V apart. +template +void expectToolOutputVoltages2(const Voltages& voltages) +{ + std::vector output_recipe = OUTPUT_RECIPE_VECTOR; + output_recipe.emplace_back("tool_output_voltage_2"); + g_my_robot->getUrDriver()->resetRTDEClient(output_recipe, INPUT_RECIPE_VECTOR); + g_my_robot->getUrDriver()->startRTDECommunication(true); + + const int32_t tolerance = 5; + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + for (const ToolVoltage voltage : voltages) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); + int32_t reported = -1; + bool reached = false; + try + { + urcl::waitFor( + [&]() { + return g_my_robot->getUrDriver()->getDataPackage(data_pkg) && + data_pkg.getData("tool_output_voltage_2", reported) && + std::abs(reported - toUnderlying(voltage)) <= tolerance; + }, + std::chrono::seconds(5)); + reached = true; + } + catch (const TimeoutException&) + { + } + EXPECT_TRUE(reached) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " << reported << " V"; + } +} + +TEST_F(UrDriverTest, set_tool_voltage_t2) +{ + if (g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() != ToolFlangeType::V2) + { + GTEST_SKIP() << "The robot does not report Tool Flange V2."; + } + expectToolOutputVoltages2(T2_TOOL_VOLTAGES); +} + +TEST_F(UrDriverTest, set_tool_voltage_t2_fallback_script) +{ + if (g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() != ToolFlangeType::V2) + { + GTEST_SKIP() << "The robot does not report Tool Flange V2."; + } + g_my_robot->getUrDriver()->stopControl(); + ASSERT_TRUE(g_my_robot->waitForProgramNotRunning(1000)); + // See set_tcp_offset: the script command interface disconnects shortly after the program stops. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + expectToolOutputVoltages2(std::array{ ToolVoltage::_24V, ToolVoltage::OFF }); +} + +TEST_F(UrDriverTest, set_tool_voltage_t2_rejected_without_flange_v2) +{ + if (g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() == ToolFlangeType::V2) + { + GTEST_SKIP() << "The robot reports Tool Flange V2."; + } + EXPECT_FALSE(g_my_robot->getUrDriver()->setToolVoltageT2(ToolVoltage::_24V)); +} + +TEST_F(UrDriverTest, set_tool_t2_voltage_invalid_voltage) +{ + // T2 only supports 0, 24, 48 V. 12 V is invalid for T2. + EXPECT_FALSE(g_my_robot->getUrDriver()->setToolVoltageT2(ToolVoltage::_12V)); +} + +TEST_F(UrDriverTest, set_tool_voltage_t0_rejects_48v) +{ + // T0 only supports 0, 12, 24 V. 48 V is invalid for T0. + EXPECT_FALSE(g_my_robot->getUrDriver()->setToolVoltage(ToolVoltage::_48V)); +} + TEST(UrDriverInitTest, setting_connection_limits_works_correctly) { UrDriverConfiguration config; diff --git a/tests/test_ur_driver_tool_voltage.cpp b/tests/test_ur_driver_tool_voltage.cpp new file mode 100644 index 000000000..12e466036 --- /dev/null +++ b/tests/test_ur_driver_tool_voltage.cpp @@ -0,0 +1,306 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 ------------------------------------------------ + +// Drives UrDriver's tool voltage handling against the fake primary and RTDE servers, so no robot is +// needed. In headless mode the startup program and the fallback script code both go out over the +// primary interface, where the fake server captures them. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "fake_primary_server.h" +#include "fake_rtde_server.h" + +using namespace urcl; + +namespace +{ +const std::string SCRIPT_FILE = "../resources/external_control.urscript"; +const std::vector OUTPUT_RECIPE{ "timestamp", "actual_q", "target_speed_fraction", "runtime_state" }; +const std::vector INPUT_RECIPE{ "speed_slider_mask", "speed_slider_fraction" }; + +// ConfigurationData payload without the optional control box and tool flange fields. Its content does not matter, +// UrDriver only waits for one to arrive. +constexpr size_t CONFIGURATION_DATA_PAYLOAD_BYTES = 440; + +constexpr std::chrono::seconds SCRIPT_TIMEOUT{ 5 }; +} // namespace + +class UrDriverToolVoltageTest : public ::testing::Test +{ +protected: + // UrDriver always connects to the default ports. If one is already taken, the fake robot cannot + // be started and the test fails. + void SetUp() override + { + for (const int port : { static_cast(UR_RTDE_PORT), primary_interface::UR_PRIMARY_PORT }) + { + try + { + comm::TCPServer probe(port, 1, std::chrono::milliseconds(10)); + } + catch (const std::system_error&) + { + FAIL() << "Port " << port << " is in use, so the fake robot cannot be started."; + } + } + } + + void TearDown() override + { + driver_.reset(); + stopSendingConfigurationData(); + primary_server_.reset(); + rtde_server_.reset(); + } + + void startServers(const ToolFlangeType tool_flange_type) + { + rtde_server_ = std::make_unique(UR_RTDE_PORT); + rtde_server_->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + rtde_server_->setHighestAcceptedProtocolVersion(3); + rtde_server_->setReportedToolFlangeType(tool_flange_type); + rtde_server_->setReportedUrControlVersion(10, 15, 0, 0); + + primary_server_ = std::make_unique(primary_interface::UR_PRIMARY_PORT); + primary_server_->setScriptCallback([this](const std::string& data) { + std::lock_guard lock(received_mutex_); + received_ += data; + }); + + // UrDriver's constructor waits for configuration data, so keep sending it until the driver is up. + sending_configuration_data_ = true; + configuration_data_thread_ = std::thread([this]() { + const std::vector payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0); + while (sending_configuration_data_) + { + primary_server_->sendRobotState({ { primary_interface::RobotStateType::CONFIGURATION_DATA, payload } }); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + }); + } + + void stopSendingConfigurationData() + { + sending_configuration_data_ = false; + if (configuration_data_thread_.joinable()) + { + configuration_data_thread_.join(); + } + } + + UrDriverConfiguration makeConfig(std::unique_ptr tool_comm_setup = nullptr) + { + UrDriverConfiguration config; + config.robot_ip = "127.0.0.1"; + config.script_file = SCRIPT_FILE; + config.output_recipe = OUTPUT_RECIPE; + config.input_recipe = INPUT_RECIPE; + config.headless_mode = true; + config.tool_comm_setup = std::move(tool_comm_setup); + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(100); + return config; + } + + void startDriver(std::unique_ptr tool_comm_setup = nullptr) + { + driver_ = std::make_unique(makeConfig(std::move(tool_comm_setup))); + stopSendingConfigurationData(); + } + + bool waitForReceived(const std::string& expected) + { + const auto deadline = std::chrono::steady_clock::now() + SCRIPT_TIMEOUT; + while (std::chrono::steady_clock::now() < deadline) + { + if (received().find(expected) != std::string::npos) + { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return received().find(expected) != std::string::npos; + } + + std::string received() + { + std::lock_guard lock(received_mutex_); + return received_; + } + + void clearReceived() + { + std::lock_guard lock(received_mutex_); + received_.clear(); + } + + std::unique_ptr rtde_server_; + std::unique_ptr primary_server_; + std::unique_ptr driver_; + + std::atomic sending_configuration_data_{ false }; + std::thread configuration_data_thread_; + + std::mutex received_mutex_; + std::string received_; +}; + +TEST_F(UrDriverToolVoltageTest, startup_program_sets_t2_voltage_on_flange_v2) +{ + startServers(ToolFlangeType::V2); + auto tool_comm_setup = std::make_unique(); + tool_comm_setup->setToolVoltage(ToolVoltage::_24V); + tool_comm_setup->setToolVoltageT2(ToolVoltage::_48V); + + startDriver(std::move(tool_comm_setup)); + + EXPECT_TRUE(waitForReceived("set_tool_voltage(24)")); + EXPECT_TRUE(waitForReceived("set_power_output(\"T2_V\", 48)")); +} + +TEST_F(UrDriverToolVoltageTest, startup_program_leaves_t2_alone_when_not_set) +{ + startServers(ToolFlangeType::V2); + auto tool_comm_setup = std::make_unique(); + tool_comm_setup->setToolVoltage(ToolVoltage::_12V); + + startDriver(std::move(tool_comm_setup)); + + ASSERT_TRUE(waitForReceived("set_tool_voltage(12)")); + // The program's script command handler sets T2 from a variable, so only look for literal voltages. + for (const char* voltage : { "0", "24", "48" }) + { + EXPECT_EQ(received().find(std::string("set_power_output(\"T2_V\", ") + voltage + ")"), std::string::npos); + } +} + +TEST_F(UrDriverToolVoltageTest, startup_throws_for_t2_voltage_without_flange_v2) +{ + startServers(ToolFlangeType::V1); + auto tool_comm_setup = std::make_unique(); + tool_comm_setup->setToolVoltageT2(ToolVoltage::_24V); + + EXPECT_THROW(startDriver(std::move(tool_comm_setup)), UrException); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_t2_falls_back_to_script_on_flange_v2) +{ + startServers(ToolFlangeType::V2); + startDriver(); + clearReceived(); + + EXPECT_TRUE(driver_->setToolVoltageT2(ToolVoltage::_24V)); + EXPECT_TRUE(waitForReceived("set_power_output(\"T2_V\", 24)")); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_t2_uses_script_command_interface_when_connected) +{ + startServers(ToolFlangeType::V2); + const UrDriverConfiguration config = makeConfig(); + startDriver(); + clearReceived(); + + // Stands in for the external control program connecting to the script command interface. + comm::TCPSocket script_command_client; + ASSERT_TRUE(script_command_client.connect("127.0.0.1", static_cast(config.script_command_port), 1)); + timeval timeout{ 1, 0 }; + script_command_client.setReceiveTimeout(timeout); + // The connection is accepted on the server's own thread. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + ASSERT_TRUE(driver_->setToolVoltageT2(ToolVoltage::_48V)); + + int32_t command_and_voltage[2]; + size_t read = 0; + uint8_t* buffer = reinterpret_cast(command_and_voltage); + size_t total = 0; + while (total < sizeof(command_and_voltage) && + script_command_client.read(buffer + total, sizeof(command_and_voltage) - total, read)) + { + total += read; + } + ASSERT_EQ(total, sizeof(command_and_voltage)); + EXPECT_EQ(be32toh(command_and_voltage[0]), 13); // SET_TOOL_T2_VOLTAGE + EXPECT_EQ(be32toh(command_and_voltage[1]) / control::ReverseInterface::MULT_JOINTSTATE, 48); + EXPECT_EQ(received().find("T2_V"), std::string::npos); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_t2_rejected_without_flange_v2) +{ + startServers(ToolFlangeType::V1); + startDriver(); + clearReceived(); + + EXPECT_FALSE(driver_->setToolVoltageT2(ToolVoltage::_24V)); + EXPECT_EQ(received().find("T2_V"), std::string::npos); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_t2_rejects_invalid_voltage) +{ + startServers(ToolFlangeType::V2); + startDriver(); + + EXPECT_FALSE(driver_->setToolVoltageT2(ToolVoltage::_12V)); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_falls_back_to_script) +{ + startServers(ToolFlangeType::V1); + startDriver(); + clearReceived(); + + EXPECT_TRUE(driver_->setToolVoltage(ToolVoltage::_12V)); + EXPECT_TRUE(waitForReceived("set_tool_voltage(12)")); +} + +TEST_F(UrDriverToolVoltageTest, set_tool_voltage_rejects_48v) +{ + startServers(ToolFlangeType::V2); + startDriver(); + + EXPECT_FALSE(driver_->setToolVoltage(ToolVoltage::_48V)); +}