From 304db933069accd2d8ce3b78c34987d15fbfd64b Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Thu, 8 Oct 2026 13:14:05 +0000 Subject: [PATCH 1/7] Add setToolVoltageT2 for connector T2 on Tool Flange V2 Connector T2 is only available on robots that report Tool Flange V2 through the RTDE robot properties, which requires PolyScope X 10.15.0 or later. PolyScope 5 does not support it. - Add ToolVoltage::_48V and SET_TOOL_T2_VOLTAGE (13) to the script command interface and external_control.urscript, using set_power_output("T2_V", ...). - UrDriver::setToolVoltageT2() checks the flange type from the RTDE writer before using the script command interface or the plain script fallback. - ScriptCommandInterface::setToolVoltage() and setToolVoltageT2() reject voltages that are not valid for the connector. - Add the tool_output_voltage_2 / tool_output_current_2 fields to the fake RTDE server and cover the writer flange reset on reconnect. --- doc/architecture/script_command_interface.rst | 14 +++- examples/script_command_interface.cpp | 16 +++++ .../control/script_command_interface.h | 34 +++++++++- .../ur_client_library/ur/tool_communication.h | 3 +- include/ur_client_library/ur/ur_driver.h | 21 +++++- resources/external_control.urscript | 8 +++ src/control/script_command_interface.cpp | 59 +++++++++++++++++ src/ur/ur_driver.cpp | 40 +++++++++++ tests/fake_rtde_server.cpp | 2 + tests/test_rtde_client_fake_server.cpp | 20 ++++++ tests/test_rtde_client_reconnect.cpp | 2 + tests/test_script_command_interface.cpp | 66 +++++++++++++++++++ tests/test_script_reader.cpp | 35 ++++++++++ tests/test_ur_driver.cpp | 21 ++++++ 14 files changed, 333 insertions(+), 8 deletions(-) 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..82d3609d4 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 (OFF = 0V, _12V = 12V, _24V = 24V) * - * \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 (OFF = 0V, _24V = 24V, _48V = 48V) + * + * \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,20 @@ 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); + 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..e423e0cf4 100644 --- a/include/ur_client_library/ur/tool_communication.h +++ b/include/ur_client_library/ur/tool_communication.h @@ -42,7 +42,8 @@ enum class ToolVoltage : int { OFF = 0, ///< 0V _12V = 12, ///< 12V - _24V = 24 ///< 24V + _24V = 24, ///< 24V + _48V = 48 ///< 48V }; /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 53f54fdb3..801b224da 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 (OFF = 0V, _12V = 12V, _24V = 24V). * * \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 (OFF = 0V, _24V = 24V, _48V = 48V). + * + * \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..188920608 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -156,6 +156,16 @@ bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) { + switch (voltage) + { + case ToolVoltage::OFF: + case ToolVoltage::_12V: + case ToolVoltage::_24V: + break; + default: + URCL_LOG_ERROR("The tool voltage should be 0, 12 or 24. The tool voltage is %d", toUnderlying(voltage)); + return false; + } const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -176,6 +186,42 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setToolVoltageT2(const ToolVoltage voltage) +{ + switch (voltage) + { + case ToolVoltage::OFF: + case ToolVoltage::_24V: + case ToolVoltage::_48V: + break; + default: + URCL_LOG_ERROR("The tool T2 voltage should be 0, 24 or 48. The voltage is %d", toUnderlying(voltage)); + return false; + } + if (!robotVersionSupportsPolyscopeXCommandOrWarn(urcl::VersionInformation::fromString("10.15.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_TOOL_T2_VOLTAGE)); + b_pos += append(b_pos, val); + + val = htobe32(toUnderlying(voltage) * MULT_JOINTSTATE); + b_pos += append(b_pos, val); + + // writing zeros to allow usage with other script commands + for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + bool ScriptCommandInterface::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 +537,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/ur_driver.cpp b/src/ur/ur_driver.cpp index dd59dfa03..3497ee69c 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -417,6 +417,46 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) return sendScript(cmd.str()); } } + +bool UrDriver::setToolVoltageT2(const ToolVoltage voltage) +{ + switch (voltage) + { + case ToolVoltage::OFF: + case ToolVoltage::_24V: + case ToolVoltage::_48V: + break; + default: + std::stringstream ss; + ss << "The tool T2 voltage should be 0, 24 or 48. The voltage is " << toUnderlying(voltage); + URCL_LOG_ERROR(ss.str().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/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 328b7f85a..4a08ee5bc 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" }, 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_ur_driver.cpp b/tests/test_ur_driver.cpp index 825cd97dc..d4c214fe0 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -409,6 +409,27 @@ 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)); } +TEST_F(UrDriverTest, set_tool_t2_voltage_flange_v1_rejected) +{ + if (g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() != ToolFlangeType::V1) + { + GTEST_SKIP() << "The robot does not report Tool Flange V1."; + } + EXPECT_FALSE(g_my_robot->getUrDriver()->setToolVoltageT2(ToolVoltage::_48V)); +} + +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; From 4cf3f7e3a3798c66d4859e49e12bb2c271176b35 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 9 Oct 2026 12:28:45 +0000 Subject: [PATCH 2/7] Add integration tests for setToolVoltageT2 On a robot that reports Tool Flange V2, set connector T2 to 24 V, 48 V and 0 V through the script command interface, and to 24 V and 0 V through the plain script fallback. Each step is checked against RTDE tool_output_voltage_2, read by a separate RTDE client because older controllers reject that field. tool_output_voltage_2 is a measured value: on a 10.16.0 robot it read 23 V for 24 V and took about a second to decay after switching off. The check therefore accepts 5 V around the setpoint and waits up to 5 s. Robots that do not report Tool Flange V2 must refuse the command. Verified on a 10.16.0 robot with Tool Flange V2. The 10.15.0 URSim tested so far reports 0 V on tool_output_voltage_2 for every setpoint, so the V2 tests fail there. --- tests/test_ur_driver.cpp | 93 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 89 insertions(+), 4 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index d4c214fe0..0ec6d83d6 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -409,13 +410,97 @@ 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)); } -TEST_F(UrDriverTest, set_tool_t2_voltage_flange_v1_rejected) +// Older controllers reject tool_output_voltage_2 in a recipe, so it cannot be part of OUTPUT_RECIPE_VECTOR. +class ToolVoltageT2Reader { - if (g_my_robot->getUrDriver()->getRTDEWriter().getToolFlangeType() != ToolFlangeType::V1) +public: + ToolVoltageT2Reader() + : client_(g_ROBOT_IP, notifier_, std::vector{ "timestamp", "tool_output_voltage_2" }, + std::vector{}) { - GTEST_SKIP() << "The robot does not report Tool Flange V1."; } - EXPECT_FALSE(g_my_robot->getUrDriver()->setToolVoltageT2(ToolVoltage::_48V)); + + bool start() + { + return client_.init() && client_.start(); + } + + // tool_output_voltage_2 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. + bool waitFor(const ToolVoltage expected, const std::chrono::milliseconds timeout = std::chrono::seconds(5)) + { + const int32_t tolerance = 5; + rtde_interface::DataPackage data_pkg(client_.getOutputRecipe()); + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + if (client_.getDataPackage(data_pkg, std::chrono::milliseconds(100)) && + data_pkg.getData("tool_output_voltage_2", last_voltage_) && + std::abs(last_voltage_ - toUnderlying(expected)) <= tolerance) + { + return true; + } + } + return false; + } + + int32_t lastVoltage() const + { + return last_voltage_; + } + +private: + comm::INotifier notifier_; + rtde_interface::RTDEClient client_; + int32_t last_voltage_ = -1; +}; + +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."; + } + ToolVoltageT2Reader reader; + ASSERT_TRUE(reader.start()); + + for (const ToolVoltage voltage : { ToolVoltage::_24V, ToolVoltage::_48V, ToolVoltage::OFF }) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); + EXPECT_TRUE(reader.waitFor(voltage)) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " + << reader.lastVoltage() << " V"; + } +} + +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."; + } + ToolVoltageT2Reader reader; + ASSERT_TRUE(reader.start()); + + 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)); + + for (const ToolVoltage voltage : { ToolVoltage::_24V, ToolVoltage::OFF }) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); + EXPECT_TRUE(reader.waitFor(voltage)) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " + << reader.lastVoltage() << " V"; + } +} + +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) From b8674d7f66b76093d43b0dc566e23e35548e6bf5 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 9 Oct 2026 13:38:57 +0000 Subject: [PATCH 3/7] Validate tool voltages in ToolCommSetup and add T2 voltage ToolCommSetup::setToolVoltage now rejects voltages connector T0 does not support, so the UrDriver startup script can no longer render set_tool_voltage(48). ToolCommSetup also gets an optional T2 voltage (0, 24 or 48 V) which UrDriver applies at startup with set_power_output("T2_V", ...) when the robot reports Tool Flange V2, and throws otherwise. --- .../ur_client_library/ur/tool_communication.h | 31 +++++-- src/ur/tool_communication.cpp | 28 ++++++ src/ur/ur_driver.cpp | 17 +++- tests/CMakeLists.txt | 5 ++ tests/test_tool_communication.cpp | 85 +++++++++++++++++++ 5 files changed, 157 insertions(+), 9 deletions(-) create mode 100644 tests/test_tool_communication.cpp diff --git a/include/ur_client_library/ur/tool_communication.h b/include/ur_client_library/ur/tool_communication.h index e423e0cf4..29cbb8e2e 100644 --- a/include/ur_client_library/ur/tool_communication.h +++ b/include/ur_client_library/ur/tool_communication.h @@ -31,6 +31,7 @@ #define UR_CLIENT_LIBRARY_UR_TOOL_COMMUNICATION_H_INCLUDED #include "ur_client_library/types.h" +#include #include namespace urcl @@ -128,13 +129,13 @@ 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 OFF, _12V or _24V or an exception will be thrown. Connector T0 does not support + * _48V. */ - void setToolVoltage(const ToolVoltage tool_voltage) - { - tool_voltage_ = tool_voltage; - } + void setToolVoltage(const ToolVoltage tool_voltage); /*! * \brief Return the tool voltage currently stored @@ -144,6 +145,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 OFF, _24V or _48V 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. @@ -240,6 +260,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/src/ur/tool_communication.cpp b/src/ur/tool_communication.cpp index d44596dcf..48aafdea7 100644 --- a/src/ur/tool_communication.cpp +++ b/src/ur/tool_communication.cpp @@ -40,6 +40,34 @@ ToolCommSetup::ToolCommSetup() { } +void ToolCommSetup::setToolVoltage(const ToolVoltage tool_voltage) +{ + switch (tool_voltage) + { + case ToolVoltage::OFF: + case ToolVoltage::_12V: + case ToolVoltage::_24V: + tool_voltage_ = tool_voltage; + break; + default: + throw std::runtime_error("Provided tool voltage is not allowed. The tool voltage should be 0, 12 or 24."); + } +} + +void ToolCommSetup::setToolVoltageT2(const ToolVoltage tool_voltage) +{ + switch (tool_voltage) + { + case ToolVoltage::OFF: + case ToolVoltage::_24V: + case ToolVoltage::_48V: + tool_voltage_t2_ = tool_voltage; + break; + default: + throw std::runtime_error("Provided tool T2 voltage is not allowed. The tool T2 voltage should be 0, 24 or 48."); + } +} + 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 3497ee69c..ebb55bc79 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,13 +400,10 @@ 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) { case ToolVoltage::OFF: - break; case ToolVoltage::_12V: - break; case ToolVoltage::_24V: break; default: @@ -404,7 +413,7 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) return false; } - if (script_command_interface_->clientConnected()) + if (script_command_interface_ != nullptr && script_command_interface_->clientConnected()) { return script_command_interface_->setToolVoltage(voltage); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index beda5d0eb..ca8b2da16 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -416,6 +416,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/test_tool_communication.cpp b/tests/test_tool_communication.cpp new file mode 100644 index 000000000..17b09d600 --- /dev/null +++ b/tests/test_tool_communication.cpp @@ -0,0 +1,85 @@ +// -- 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(ToolCommSetupTest, set_tool_voltage_accepts_t0_voltages) +{ + ToolCommSetup setup; + for (const ToolVoltage voltage : { ToolVoltage::_12V, ToolVoltage::_24V, ToolVoltage::OFF }) + { + 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); + + EXPECT_THROW(setup.setToolVoltage(ToolVoltage::_48V), std::runtime_error); + EXPECT_THROW(setup.setToolVoltage(static_cast(5)), std::runtime_error); + 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 : { ToolVoltage::_24V, ToolVoltage::_48V, ToolVoltage::OFF }) + { + 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); + + EXPECT_THROW(setup.setToolVoltageT2(ToolVoltage::_12V), std::runtime_error); + EXPECT_THROW(setup.setToolVoltageT2(static_cast(5)), std::runtime_error); + EXPECT_EQ(setup.getToolVoltageT2(), ToolVoltage::_48V); +} From 63dbf64e8e25623c7a1c89104b5088908df9644c Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 9 Oct 2026 14:16:08 +0000 Subject: [PATCH 4/7] Add robot-free UrDriver tool voltage tests Run UrDriver against the fake primary and RTDE servers to cover the tool voltage paths no CI URSim reaches: the T2 voltage in the startup program, the startup check for Tool Flange V2, and setToolVoltage / setToolVoltageT2 through the script command interface and the script fallback. The fake RTDE server can now report the tool flange type and the URControl version. The tests skip if the default robot ports are in use. --- tests/CMakeLists.txt | 9 + tests/fake_rtde_server.cpp | 30 ++- tests/fake_rtde_server.h | 15 ++ tests/test_ur_driver_tool_voltage.cpp | 306 ++++++++++++++++++++++++++ 4 files changed, 353 insertions(+), 7 deletions(-) create mode 100644 tests/test_ur_driver_tool_voltage.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ca8b2da16..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) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 4a08ee5bc..de29f0c51 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -627,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_); @@ -781,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) { @@ -795,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); @@ -1027,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: @@ -1041,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_); @@ -1052,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; } @@ -1131,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_ur_driver_tool_voltage.cpp b/tests/test_ur_driver_tool_voltage.cpp new file mode 100644 index 000000000..604f63f1e --- /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, so another robot or simulator listening on them (such as a + // URSim container publishing its ports) makes these tests impossible rather than failed. + 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&) + { + GTEST_SKIP() << "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)); +} From fa8deeab4cfea1083a38d53312fbed897ac2fdc8 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 12 Oct 2026 12:50:08 +0000 Subject: [PATCH 5/7] Define shared T0 and T2 tool voltage lists for validation and error text --- .../control/script_command_interface.h | 4 +- .../ur_client_library/ur/tool_communication.h | 65 ++++++++++++++++++- include/ur_client_library/ur/ur_driver.h | 4 +- src/control/script_command_interface.cpp | 24 ++----- src/ur/tool_communication.cpp | 22 ++----- src/ur/ur_driver.cpp | 26 ++------ tests/test_tool_communication.cpp | 59 +++++++++++++++-- tests/test_ur_driver.cpp | 2 +- 8 files changed, 139 insertions(+), 67 deletions(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 82d3609d4..9a5191df6 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -118,7 +118,7 @@ class ScriptCommandInterface : public ReverseInterface /*! * \brief Set the tool voltage on connector T0. * - * \param voltage Tool voltage (OFF = 0V, _12V = 12V, _24V = 24V) + * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES. * * \returns True, if the write was performed successfully, false if the voltage is invalid or the write fails. */ @@ -130,7 +130,7 @@ class ScriptCommandInterface : public ReverseInterface * 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 (OFF = 0V, _24V = 24V, _48V = 48V) + * \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. diff --git a/include/ur_client_library/ur/tool_communication.h b/include/ur_client_library/ur/tool_communication.h index 29cbb8e2e..2242e8047 100644 --- a/include/ur_client_library/ur/tool_communication.h +++ b/include/ur_client_library/ur/tool_communication.h @@ -31,8 +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 { @@ -47,6 +52,61 @@ enum class ToolVoltage : int _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 */ @@ -132,8 +192,7 @@ class ToolCommSetup * \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 OFF, _12V or _24V or an exception will be thrown. Connector T0 does not support - * _48V. + * \param tool_voltage must be one of \ref T0_TOOL_VOLTAGES or an exception will be thrown. */ void setToolVoltage(const ToolVoltage tool_voltage); @@ -152,7 +211,7 @@ class ToolCommSetup * 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 OFF, _24V or _48V or an exception will be thrown. + * \param tool_voltage must be one of \ref T2_TOOL_VOLTAGES or an exception will be thrown. */ void setToolVoltageT2(const ToolVoltage tool_voltage); diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 801b224da..50ca75276 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -680,7 +680,7 @@ class UrDriver * \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 (OFF = 0V, _12V = 12V, _24V = 24V). + * \param voltage Tool voltage. Must be one of \ref T0_TOOL_VOLTAGES. * * \returns True on successful write. */ @@ -694,7 +694,7 @@ class UrDriver * 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 (OFF = 0V, _24V = 24V, _48V = 48V). + * \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. diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 188920608..1cf852e04 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -156,15 +156,10 @@ bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) { - switch (voltage) + if (!isAllowedToolVoltage(voltage, T0_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - case ToolVoltage::_12V: - case ToolVoltage::_24V: - break; - default: - URCL_LOG_ERROR("The tool voltage should be 0, 12 or 24. The tool voltage is %d", toUnderlying(voltage)); - return false; + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool", voltage, T0_TOOL_VOLTAGES).c_str()); + return false; } const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; @@ -188,15 +183,10 @@ bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) bool ScriptCommandInterface::setToolVoltageT2(const ToolVoltage voltage) { - switch (voltage) - { - case ToolVoltage::OFF: - case ToolVoltage::_24V: - case ToolVoltage::_48V: - break; - default: - URCL_LOG_ERROR("The tool T2 voltage should be 0, 24 or 48. The voltage is %d", toUnderlying(voltage)); - return false; + 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__)) { diff --git a/src/ur/tool_communication.cpp b/src/ur/tool_communication.cpp index 48aafdea7..e133ed568 100644 --- a/src/ur/tool_communication.cpp +++ b/src/ur/tool_communication.cpp @@ -42,30 +42,20 @@ ToolCommSetup::ToolCommSetup() void ToolCommSetup::setToolVoltage(const ToolVoltage tool_voltage) { - switch (tool_voltage) + if (!isAllowedToolVoltage(tool_voltage, T0_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - case ToolVoltage::_12V: - case ToolVoltage::_24V: - tool_voltage_ = tool_voltage; - break; - default: - throw std::runtime_error("Provided tool voltage is not allowed. The tool voltage should be 0, 12 or 24."); + throw std::runtime_error(toolVoltageNotAllowedMessage("tool", tool_voltage, T0_TOOL_VOLTAGES)); } + tool_voltage_ = tool_voltage; } void ToolCommSetup::setToolVoltageT2(const ToolVoltage tool_voltage) { - switch (tool_voltage) + if (!isAllowedToolVoltage(tool_voltage, T2_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - case ToolVoltage::_24V: - case ToolVoltage::_48V: - tool_voltage_t2_ = tool_voltage; - break; - default: - throw std::runtime_error("Provided tool T2 voltage is not allowed. The tool T2 voltage should be 0, 24 or 48."); + 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) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index ebb55bc79..9fb07d2f0 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -400,17 +400,10 @@ bool UrDriver::setGravity(const vector3d_t& gravity) bool UrDriver::setToolVoltage(const ToolVoltage voltage) { - switch (voltage) + if (!isAllowedToolVoltage(voltage, T0_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - case ToolVoltage::_12V: - 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_ != nullptr && script_command_interface_->clientConnected()) @@ -429,17 +422,10 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) bool UrDriver::setToolVoltageT2(const ToolVoltage voltage) { - switch (voltage) + if (!isAllowedToolVoltage(voltage, T2_TOOL_VOLTAGES)) { - case ToolVoltage::OFF: - case ToolVoltage::_24V: - case ToolVoltage::_48V: - break; - default: - std::stringstream ss; - ss << "The tool T2 voltage should be 0, 24 or 48. The voltage is " << toUnderlying(voltage); - URCL_LOG_ERROR(ss.str().c_str()); - return false; + URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool T2", voltage, T2_TOOL_VOLTAGES).c_str()); + return false; } const ToolFlangeType flange = rtde_client_->getWriter().getToolFlangeType(); diff --git a/tests/test_tool_communication.cpp b/tests/test_tool_communication.cpp index 17b09d600..62f506a4e 100644 --- a/tests/test_tool_communication.cpp +++ b/tests/test_tool_communication.cpp @@ -36,10 +36,23 @@ 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 : { ToolVoltage::_12V, ToolVoltage::_24V, ToolVoltage::OFF }) + for (const ToolVoltage voltage : T0_TOOL_VOLTAGES) { EXPECT_NO_THROW(setup.setToolVoltage(voltage)); EXPECT_EQ(setup.getToolVoltage(), voltage); @@ -53,8 +66,25 @@ TEST(ToolCommSetupTest, set_tool_voltage_rejects_voltages_t0_does_not_support) ToolCommSetup setup; setup.setToolVoltage(ToolVoltage::_24V); - EXPECT_THROW(setup.setToolVoltage(ToolVoltage::_48V), std::runtime_error); - EXPECT_THROW(setup.setToolVoltage(static_cast(5)), std::runtime_error); + 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); } @@ -67,7 +97,7 @@ TEST(ToolCommSetupTest, tool_voltage_t2_is_unset_by_default) TEST(ToolCommSetupTest, set_tool_voltage_t2_accepts_t2_voltages) { ToolCommSetup setup; - for (const ToolVoltage voltage : { ToolVoltage::_24V, ToolVoltage::_48V, ToolVoltage::OFF }) + for (const ToolVoltage voltage : T2_TOOL_VOLTAGES) { EXPECT_NO_THROW(setup.setToolVoltageT2(voltage)); EXPECT_EQ(setup.getToolVoltageT2(), voltage); @@ -79,7 +109,24 @@ TEST(ToolCommSetupTest, set_tool_voltage_t2_rejects_voltages_t2_does_not_support ToolCommSetup setup; setup.setToolVoltageT2(ToolVoltage::_48V); - EXPECT_THROW(setup.setToolVoltageT2(ToolVoltage::_12V), std::runtime_error); - EXPECT_THROW(setup.setToolVoltageT2(static_cast(5)), std::runtime_error); + 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 0ec6d83d6..1781d0960 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -464,7 +464,7 @@ TEST_F(UrDriverTest, set_tool_voltage_t2) ToolVoltageT2Reader reader; ASSERT_TRUE(reader.start()); - for (const ToolVoltage voltage : { ToolVoltage::_24V, ToolVoltage::_48V, ToolVoltage::OFF }) + for (const ToolVoltage voltage : T2_TOOL_VOLTAGES) { ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); EXPECT_TRUE(reader.waitFor(voltage)) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " From f41b46dfd3251dc201ddd0c05be5fbf0bff20494 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 12 Oct 2026 12:57:05 +0000 Subject: [PATCH 6/7] Share the script command write between setToolVoltage and setToolVoltageT2 --- .../control/script_command_interface.h | 10 +++++ src/control/script_command_interface.cpp | 40 +++++++------------ 2 files changed, 24 insertions(+), 26 deletions(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 9a5191df6..d477911c6 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -328,6 +328,16 @@ class ScriptCommandInterface : public ReverseInterface 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/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 1cf852e04..1d327332e 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -154,20 +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) { - if (!isAllowedToolVoltage(voltage, T0_TOOL_VOLTAGES)) - { - URCL_LOG_ERROR("%s", toolVoltageNotAllowedMessage("tool", voltage, T0_TOOL_VOLTAGES).c_str()); - 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_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 @@ -181,6 +176,16 @@ 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)) @@ -192,24 +197,7 @@ bool ScriptCommandInterface::setToolVoltageT2(const ToolVoltage voltage) { 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_TOOL_T2_VOLTAGE)); - b_pos += append(b_pos, val); - - val = htobe32(toUnderlying(voltage) * MULT_JOINTSTATE); - b_pos += append(b_pos, val); - - // writing zeros to allow usage with other script commands - for (size_t i = message_length; i < MAX_MESSAGE_LENGTH; i++) - { - val = htobe32(0); - b_pos += append(b_pos, val); - } - size_t written; - - return server_.write(client_fd_, buffer, sizeof(buffer), written); + return writeCommandWithArgument(ScriptCommand::SET_TOOL_T2_VOLTAGE, toUnderlying(voltage) * MULT_JOINTSTATE); } bool ScriptCommandInterface::startForceMode(const vector6d_t* task_frame, const vector6uint32_t* selection_vector, From 2af6e0e3b2d6bb441daa270fb23afbbbdc997a89 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 12 Oct 2026 14:33:11 +0000 Subject: [PATCH 7/7] Read T2 tool voltage through the driver's RTDE client in tests Replace the extra RTDE client with a recipe on the driver and wait with urcl::waitFor. Fail the robot-free tests when the default ports are already in use. --- tests/test_ur_driver.cpp | 88 ++++++++++----------------- tests/test_ur_driver_tool_voltage.cpp | 6 +- 2 files changed, 35 insertions(+), 59 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 1781d0960..37829ea0e 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -410,50 +411,41 @@ 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 in a recipe, so it cannot be part of OUTPUT_RECIPE_VECTOR. -class ToolVoltageT2Reader +// 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) { -public: - ToolVoltageT2Reader() - : client_(g_ROBOT_IP, notifier_, std::vector{ "timestamp", "tool_output_voltage_2" }, - std::vector{}) - { - } - - bool start() - { - return client_.init() && client_.start(); - } + 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); - // tool_output_voltage_2 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. - bool waitFor(const ToolVoltage expected, const std::chrono::milliseconds timeout = std::chrono::seconds(5)) + const int32_t tolerance = 5; + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + for (const ToolVoltage voltage : voltages) { - const int32_t tolerance = 5; - rtde_interface::DataPackage data_pkg(client_.getOutputRecipe()); - const auto deadline = std::chrono::steady_clock::now() + timeout; - while (std::chrono::steady_clock::now() < deadline) + ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); + int32_t reported = -1; + bool reached = false; + try { - if (client_.getDataPackage(data_pkg, std::chrono::milliseconds(100)) && - data_pkg.getData("tool_output_voltage_2", last_voltage_) && - std::abs(last_voltage_ - toUnderlying(expected)) <= tolerance) - { - return true; - } + 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; } - return false; - } - - int32_t lastVoltage() const - { - return last_voltage_; + catch (const TimeoutException&) + { + } + EXPECT_TRUE(reached) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " << reported << " V"; } - -private: - comm::INotifier notifier_; - rtde_interface::RTDEClient client_; - int32_t last_voltage_ = -1; -}; +} TEST_F(UrDriverTest, set_tool_voltage_t2) { @@ -461,15 +453,7 @@ TEST_F(UrDriverTest, set_tool_voltage_t2) { GTEST_SKIP() << "The robot does not report Tool Flange V2."; } - ToolVoltageT2Reader reader; - ASSERT_TRUE(reader.start()); - - for (const ToolVoltage voltage : T2_TOOL_VOLTAGES) - { - ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); - EXPECT_TRUE(reader.waitFor(voltage)) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " - << reader.lastVoltage() << " V"; - } + expectToolOutputVoltages2(T2_TOOL_VOLTAGES); } TEST_F(UrDriverTest, set_tool_voltage_t2_fallback_script) @@ -478,20 +462,12 @@ TEST_F(UrDriverTest, set_tool_voltage_t2_fallback_script) { GTEST_SKIP() << "The robot does not report Tool Flange V2."; } - ToolVoltageT2Reader reader; - ASSERT_TRUE(reader.start()); - 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)); - for (const ToolVoltage voltage : { ToolVoltage::_24V, ToolVoltage::OFF }) - { - ASSERT_TRUE(g_my_robot->getUrDriver()->setToolVoltageT2(voltage)); - EXPECT_TRUE(reader.waitFor(voltage)) << "Expected " << toUnderlying(voltage) << " V on T2, the robot reports " - << reader.lastVoltage() << " V"; - } + expectToolOutputVoltages2(std::array{ ToolVoltage::_24V, ToolVoltage::OFF }); } TEST_F(UrDriverTest, set_tool_voltage_t2_rejected_without_flange_v2) diff --git a/tests/test_ur_driver_tool_voltage.cpp b/tests/test_ur_driver_tool_voltage.cpp index 604f63f1e..12e466036 100644 --- a/tests/test_ur_driver_tool_voltage.cpp +++ b/tests/test_ur_driver_tool_voltage.cpp @@ -72,8 +72,8 @@ constexpr std::chrono::seconds SCRIPT_TIMEOUT{ 5 }; class UrDriverToolVoltageTest : public ::testing::Test { protected: - // UrDriver always connects to the default ports, so another robot or simulator listening on them (such as a - // URSim container publishing its ports) makes these tests impossible rather than failed. + // 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 }) @@ -84,7 +84,7 @@ class UrDriverToolVoltageTest : public ::testing::Test } catch (const std::system_error&) { - GTEST_SKIP() << "Port " << port << " is in use, so the fake robot cannot be started."; + FAIL() << "Port " << port << " is in use, so the fake robot cannot be started."; } } }