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

@@ -93,6 +93,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 }); });
   }

src/control/script_command_interface.cpp

@@ -387,6 +387,8 @@ bool ScriptCommandInterface::setTcpOffset(const vector6d_t& offset)
 
 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;
@@ -419,6 +421,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;

src/ur/ur_driver.cpp

@@ -145,24 +145,29 @@ void UrDriver::init(const UrDriverConfiguration& config)
   data[BEGIN_REPLACE] = begin_replace.str();
 
   data["ROBOT_SOFTWARE_VERSION"] = getVersion();
-
-  script_reader_.reset(new control::ScriptReader());
-  std::string prog = script_reader_->readScriptFile(config.script_file, data);
-
   const RobotType robot_type = primary_client_->getRobotType();
 
   const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type);
+  std::stringstream pd_gains_ss;
+  if (robot_version_ < urcl::VersionInformation::fromString("5.10.0"))
   {
-    std::stringstream ss;
-    ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")";
-    data[PD_CONTROLLER_GAINS_REPLACE] = ss.str();
+    // 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
   {
-    std::stringstream ss;
-    ss << control::getMaxTorquesFromRobotType(robot_type);
-    data[MAX_JOINT_TORQUE_REPLACE] = ss.str();
+    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);
 
   if (in_headless_mode_)
   {