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..000a1c619 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -78,6 +78,7 @@ 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 }); }); + // 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 }); }); } 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/resources/external_control.urscript b/resources/external_control.urscript index 75005b471..965b5173f 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) @@ -678,9 +688,21 @@ 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() + {% 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) + {% 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) @@ -760,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/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 7435044f2..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; @@ -356,6 +362,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 diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e670b4f5b..b0b157cd8 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -147,13 +147,23 @@ 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_ < 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; + } + 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; 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);