More version checks

Author: urfeex

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

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;