From d37f211e0c1d3c2f849691dc5ef4d961741248eb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 25 Jun 2025 15:10:18 +0200 Subject: [PATCH 1/7] Add torque_command statement only on supported software versions --- resources/external_control.urscript | 20 ++++++++++++++++++++ src/ur/ur_driver.cpp | 2 ++ 2 files changed, 22 insertions(+) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 75005b471..a370594d6 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -270,7 +270,17 @@ thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: torque = cmd_torque + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} torque_command(torque, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} + torque_command(torque, friction_comp=friction_compensation_enabled) + {% else %} + popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} end textmsg("ExternalControl: torque thread ended") stopj(STOPJ_ACCELERATION) @@ -680,7 +690,17 @@ thread PDControlThread(): local q_err = cmd_servo_q - get_actual_joint_positions() local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() tau = clamp_array(tau, max_joint_torques) + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + torque_command(tau, friction_comp=friction_compensation_enabled) + {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} torque_command(tau, friction_comp=friction_compensation_enabled) + {% else %} + popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) + {% endif %} + {% else %} + popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} end textmsg("PD Control thread ended") stopj(STOPJ_ACCELERATION) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e670b4f5b..5523f1649 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -154,6 +154,8 @@ void UrDriver::init(const UrDriverConfiguration& config) max_torques_ss << control::getMaxTorquesFromRobotType(robot_type); data[MAX_JOINT_TORQUE_REPLACE] = max_torques_ss.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); + script_reader_.reset(new control::ScriptReader()); std::string prog = script_reader_->readScriptFile(config.script_file, data); From 0d5a29e874487ca4a4e27bb91f7773829b443a9e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 26 Jun 2025 09:15:56 +0200 Subject: [PATCH 2/7] More version checks --- resources/external_control.urscript | 8 ++++++-- src/ur/ur_driver.cpp | 10 +++++++++- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index a370594d6..965b5173f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -688,12 +688,14 @@ end thread PDControlThread(): while control_mode == MODE_PD_CONTROLLER_JOINT or control_mode == MODE_PD_CONTROLLER_TASK: local q_err = cmd_servo_q - get_actual_joint_positions() - local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() - tau = clamp_array(tau, max_joint_torques) {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) torque_command(tau, friction_comp=friction_compensation_enabled) {% elif ROBOT_SOFTWARE_VERSION >= v10.10.0 %} + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) torque_command(tau, friction_comp=friction_compensation_enabled) {% else %} popup("Torque control is only supported from software 10.10.0 and upwards.", error=True, blocking=True) @@ -780,8 +782,10 @@ thread script_commands(): friction_compensation_enabled = True end elif command == SET_PD_CONTROLLER_GAINS: + {% if ROBOT_SOFTWARE_VERSION > v5.10.0 %} pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + {% endif %} elif command == SET_MAX_JOINT_TORQUES: max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 5523f1649..6e861ba3e 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -147,7 +147,15 @@ void UrDriver::init(const UrDriverConfiguration& config) const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); std::stringstream pd_gains_ss; - pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + if (robot_version_.major == 5 && robot_version_.minor < 10) + { + // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. + pd_gains_ss << 0; + } + else + { + pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + } data[PD_CONTROLLER_GAINS_REPLACE] = pd_gains_ss.str(); std::stringstream max_torques_ss; From 6ecbff6a1aba94bb597f5a2952a13d8eec92b4ce Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 26 Jun 2025 10:08:12 +0200 Subject: [PATCH 3/7] Fix version check to also include CB3 --- src/ur/ur_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 6e861ba3e..b0b157cd8 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -147,7 +147,7 @@ void UrDriver::init(const UrDriverConfiguration& config) const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); std::stringstream pd_gains_ss; - if (robot_version_.major == 5 && robot_version_.minor < 10) + if (robot_version_ < urcl::VersionInformation::fromString("5.10.0")) { // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. pd_gains_ss << 0; From 1ed35743f1bee8747830fcf3e552971cf7e9323d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 26 Jun 2025 10:42:47 +0200 Subject: [PATCH 4/7] Skip examples that require torque control on older PolyScope --- examples/pd_controller_example.cpp | 11 +++++++++++ examples/script_command_interface.cpp | 9 +++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp index a959a4e7b..4cdbb2568 100644 --- a/examples/pd_controller_example.cpp +++ b/examples/pd_controller_example.cpp @@ -185,6 +185,17 @@ int main(int argc, char* argv[]) return 1; } + { + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) + { + URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", + robot_version.toString().c_str()); + return 0; + } + } + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); URCL_LOG_INFO("Move the robot to initial position"); diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index a5e9637d7..3ed721794 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -78,8 +78,13 @@ void sendScriptCommands() g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); }); - run_cmd("Setting max joint torques", - []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version > urcl::VersionInformation::fromString("10.10.0") || + (robot_version.major == 5 && robot_version.minor >= 23)) + { + run_cmd("Setting max joint torques", + []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); + } } URCL_LOG_INFO("Script command thread finished."); } From 087c4617c23864a5161e95b586f0969dc08b7f44 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Jul 2025 08:36:01 +0200 Subject: [PATCH 5/7] Removed unnecessary check --- examples/script_command_interface.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 3ed721794..000a1c619 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -78,13 +78,9 @@ void sendScriptCommands() g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); }); - auto robot_version = g_my_robot->getUrDriver()->getVersion(); - if (robot_version > urcl::VersionInformation::fromString("10.10.0") || - (robot_version.major == 5 && robot_version.minor >= 23)) - { - run_cmd("Setting max joint torques", - []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); - } + // The following will have no effect on PolyScope < 5.23 / 10.10 + run_cmd("Setting max joint torques", + []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } URCL_LOG_INFO("Script command thread finished."); } From e1dd6f0884871a760c9ec3e1dc315db85f114ca1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Jul 2025 09:26:48 +0200 Subject: [PATCH 6/7] Add function to check / specify version requirements for a script command --- .../control/script_command_interface.h | 20 +++++++++++++++++++ src/control/script_command_interface.cpp | 17 +++++++++++++++- 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index a13e4aaf5..8decb5568 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -31,6 +31,7 @@ #include "ur_client_library/control/reverse_interface.h" #include "ur_client_library/ur/tool_communication.h" +#include "ur_client_library/ur/version_information.h" namespace urcl { @@ -235,6 +236,25 @@ class ScriptCommandInterface : public ReverseInterface SET_MAX_JOINT_TORQUES = 9, ///< Set max joint torques }; + /*! + * \brief Checks if the robot version is higher than the minimum required version for Polyscope 5 + * or Polyscope X. + * + * If the robot version is lower than the minimum required version, this function + * will log a warning message. + * In case of a PolyScope 5 robot, the robot's software version will be checked against \p + * min_polyscope5, and in case of a PolyScope X robot, it will be checked against \p + * min_polyscopeX. + * + * \param min_polyscope5 Minimum required version for PolyScope 5 + * \param min_polyscopeX Minimum required version for PolyScope X + * \param command_name Name of the command being checked, used for logging + * + * \returns True if the robot version is higher than the versions provided, false otherwise. + */ + bool robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, + const VersionInformation& min_polyscopeX, const std::string& command_name); + bool client_connected_; static const int MAX_MESSAGE_LENGTH = 28; diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 7435044f2..e948de0ce 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -356,6 +356,21 @@ void ScriptCommandInterface::messageCallback(const socket_t filedescriptor, char nbytesrecv); } } +bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInformation& min_polyscope5, + const VersionInformation& min_polyscopeX, + const std::string& command_name) +{ + if (robot_software_version_ < min_polyscope5 || + (robot_software_version_.major > 5 && robot_software_version_ < min_polyscopeX)) + { + URCL_LOG_WARN("%s is only available for robots with PolyScope %s / %s or " + "later. This robot's version is %s. This command will have no effect.", + command_name.c_str(), min_polyscope5.toString().c_str(), min_polyscopeX.toString().c_str(), + robot_software_version_.toString().c_str()); + return false; + } + return true; +} } // namespace control -} // namespace urcl \ No newline at end of file +} // namespace urcl From 866449ee2ab60005747642439dd0e0b303d23351 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Jul 2025 11:11:49 +0200 Subject: [PATCH 7/7] Add warnings to new commands --- src/control/script_command_interface.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index e948de0ce..673be8cb5 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -228,6 +228,8 @@ bool ScriptCommandInterface::endToolContact() bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 2; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -251,6 +253,8 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 13; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer; @@ -283,6 +287,8 @@ bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, co bool ScriptCommandInterface::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) { + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); const int message_length = 7; uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buffer;