Add torque_command statement only on supported software versions (#348)

This way all non-torque-control-related things should work on software
versions not supporting torque control

Author: urfeex

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<urcl::InstructionExecutor>(g_my_robot->getUrDriver());
 
   URCL_LOG_INFO("Move the robot to initial position");

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 }); });
   }

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;
 

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

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
+}  // namespace urcl

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);