Update to RTDE version and simplify urscript

- Update the pd_controller example to use the new RTDE version
- Simplify direct_torque call in urscript

Author: urrsk

examples/pd_controller_example.cpp

@@ -103,6 +103,8 @@ bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_n
 
   urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
+  rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+
   bool first_pass = true;
   while (time < running_time)
   {
@@ -111,17 +113,17 @@ bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_n
     // robot will effectively be in charge of setting the frequency of this loop.
     // In a real-world application this thread should be scheduled with real-time priority in order
     // to ensure that this is called in time.
-    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage();
-    if (!data_pkg)
+    if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
     {
       URCL_LOG_WARN("Could not get fresh data package from robot");
       return false;
     }
     // Read current joint positions from robot data
-    if (!data_pkg->getData(actual_data_name, actual))
+    if (!data_pkg.getData(actual_data_name, actual))
     {
       // This throwing should never happen unless misconfigured
-      std::string error_msg = "Did not find" + actual_data_name + "in data sent from robot. This should not happen!";
+      std::string error_msg =
+          "Did not find '" + actual_data_name + "' in data sent from robot. This should not happen!";
       throw std::runtime_error(error_msg);
     }
 
@@ -146,7 +148,8 @@ bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_n
     bool ret = g_my_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100));
     if (!ret)
     {
-      URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+      URCL_LOG_ERROR("Could not send joint command. Make sure that the robot is in remote control mode and connected "
+                     "with a network cable.");
       return false;
     }
 

resources/external_control.urscript

@@ -275,6 +275,41 @@ thread speedThread():
   stopj(STOPJ_ACCELERATION)
 end
 
+# Helper function to call the direct torque control function depending on the software version
+def directTorqueControlCall(tau):
+  {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
+  {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %}
+  if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES:
+      {% if ROBOT_SOFTWARE_VERSION < v5.25.1 %}
+    popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope 5.25.1. This robot runs an older version.", error=True, blocking=True)
+      {% else %}
+    direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)
+      {% endif %}
+  elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY:
+    direct_torque(tau, friction_comp=deprecated_friction_compensation_enabled)
+  else:
+    direct_torque(tau)
+  end
+    {% else %}
+  popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True)
+    {% endif %}
+  {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %}
+  if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES:
+    {% if ROBOT_SOFTWARE_VERSION < v10.12.1 %}
+    popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope X 10.12.1. This robot runs an older version.", error=True, blocking=True)
+    {% else %}
+    direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)
+    {% endif %}
+  elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY:
+    direct_torque(tau, friction_comp=deprecated_friction_compensation_enabled)
+  else:
+    direct_torque(tau)
+  end
+  {% else %}
+  popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True)
+  {% endif %}
+end
+
 # Helpers for torque control
 def set_torque(target_torque):
   cmd_torque = target_torque
@@ -284,38 +319,8 @@ end
 thread torqueThread():
   textmsg("ExternalControl: Starting torque thread")
   while control_mode == MODE_TORQUE:
-    torque = cmd_torque
-    {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %}
-      {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %}
-    if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES:
-        {% if ROBOT_SOFTWARE_VERSION < v5.25.1 %}
-      popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope 5.25.1. This robot runs an older version.", error=True, blocking=True)
-        {% else %}
-      direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)
-        {% endif %}
-    elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY:
-      direct_torque(torque, friction_comp=deprecated_friction_compensation_enabled)
-    else:
-      direct_torque(torque) # Uses robot defaults
-    end
-      {% else %}
-    popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True)
-      {% endif %}
-    {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %}
-    if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES:
-      {% if ROBOT_SOFTWARE_VERSION < v10.12.1 %}
-      popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope X 10.12.1. This robot runs an older version.", error=True, blocking=True)
-      {% else %}
-      direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)
-      {% endif %}
-    elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY:
-      direct_torque(torque, friction_comp=deprecated_friction_compensation_enabled)
-    else:
-      direct_torque(torque) # Uses robot defaults
-    end
-    {% else %}
-    popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True)
-    {% endif %}
+    local tau = cmd_torque
+    directTorqueControlCall(tau)
   end
   textmsg("ExternalControl: torque thread ended")
   stopj(STOPJ_ACCELERATION)
@@ -782,13 +787,7 @@ 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 friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES:
-      direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)
-    elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY:
-      direct_torque(tau, friction_comp=deprecated_friction_compensation_enabled)
-    else:
-      direct_torque(tau)
-    end
+    directTorqueControlCall(tau)
   end 
   textmsg("PD Control thread ended")
   stopj(STOPJ_ACCELERATION)

tests/test_script_reader.cpp

@@ -507,6 +507,8 @@ TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion)
   data["SERVER_PORT_REPLACE"] = "50001";
   data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003";
   data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004";
+  data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)";
+  data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]";
 
   data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.22.0");
   std::string processed_script = reader.readScriptFile(existing_script_file, data);
@@ -534,6 +536,8 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523
   data["SERVER_PORT_REPLACE"] = "50001";
   data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003";
   data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004";
+  data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)";
+  data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]";
   data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.23.0");
 
   std::string processed_script = reader.readScriptFile(existing_script_file, data);
@@ -564,6 +568,8 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope101
   data["SERVER_PORT_REPLACE"] = "50001";
   data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003";
   data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004";
+  data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)";
+  data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]";
   data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.11.0");
 
   std::string processed_script = reader.readScriptFile(existing_script_file, data);
@@ -595,6 +601,8 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler)
   data["SERVER_PORT_REPLACE"] = "50001";
   data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003";
   data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004";
+  data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)";
+  data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]";
 
   data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.25.1");
   std::string processed_script = reader.readScriptFile(existing_script_file, data);
@@ -632,6 +640,8 @@ TEST_F(ScriptReaderTest, TestProduceAllScriptFiles)
   data["SERVER_PORT_REPLACE"] = "50001";
   data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003";
   data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004";
+  data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)";
+  data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]";
 
   // List of software versions to test
   std::vector<urcl::VersionInformation> software_versions = {