Merge pull request #135 from ipa-fxm/version_aware_deserializing_hydro

version-aware implementation for deserialize and deserializeRT in order ...

ur_driver/src/ur_driver/deserialize.py

@@ -50,7 +50,7 @@ class ToolMode(object):
     BOOTLOADER = 249
     RUNNING = 253
     IDLE = 255
-    
+
 class MasterSafetyState(object):
     UNDEFINED = 0
     BOOTLOADER = 1
@@ -68,14 +68,29 @@ class MasterOnOffState(object):
     ON = 2
     TURNING_OFF = 3
 
+#this class handles different protocol versions
 class RobotModeData(object):
+    @staticmethod
+    def unpack(buf):
+        rmd = RobotModeData()
+        (plen, ptype) = struct.unpack_from("!IB", buf)
+        if plen == 29:
+            return RobotModeData_V18.unpack(buf)
+        elif plen == 38:
+            return RobotModeData_V30.unpack(buf)
+        else:
+            print "RobotModeData has wrong length: " + str(plen)
+            return rmd
+
+#this parses RobotModeData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
+class RobotModeData_V18(object):
     __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
                  'power_on_robot', 'emergency_stopped',
                  'security_stopped', 'program_running', 'program_paused',
                  'robot_mode', 'speed_fraction']
     @staticmethod
     def unpack(buf):
-        rmd = RobotModeData()
+        rmd = RobotModeData_V18()
         (_, _,
          rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
          rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
@@ -83,7 +98,24 @@ class RobotModeData(object):
          rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
         return rmd
 
-# Don't use T_micro (obsolete). For retrocompatibility purposes. 
+#this parses RobotModeData for versions >=3.0 (i.e. 3.0)
+class RobotModeData_V30(object):
+    __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
+                 'power_on_robot', 'emergency_stopped',
+                 'security_stopped', 'program_running', 'program_paused',
+                 'robot_mode', 'control_mode', 'target_speed_fraction',
+                 'speed_scaling']
+    @staticmethod
+    def unpack(buf):
+        rmd = RobotModeData_V30()
+        (_, _,
+         rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
+         rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
+         rmd.program_running, rmd.program_paused, rmd.robot_mode, rmd.control_mode,
+         rmd.target_speed_fraction, rmd.speed_scaling) = struct.unpack_from("!IBQ???????BBdd", buf)
+        return rmd
+
+#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
 class JointData(object):
     __slots__ = ['q_actual', 'q_target', 'qd_actual',
                  'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
@@ -94,12 +126,13 @@ class JointData(object):
         for i in range(6):
             jd = JointData()
             (jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
-             jd.T_motor, jd.T_micro, 
+             jd.T_motor, jd.T_micro,
              jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
             offset += 41
             all_joints.append(jd)
         return all_joints
 
+#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
 class ToolData(object):
     __slots__ = ['analog_input_range2', 'analog_input_range3',
                  'analog_input2', 'analog_input3',
@@ -115,7 +148,23 @@ class ToolData(object):
          td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
         return td
 
+#this class handles different protocol versions
 class MasterboardData(object):
+    @staticmethod
+    def unpack(buf):
+        md = MasterboardData()
+        (plen, ptype) = struct.unpack_from("!IB", buf)
+        if (plen == 64) or (plen == 76): # Euromap67 interface = 12 bytes
+            return MasterboardData_V18.unpack(buf)
+        elif (plen == 72) or (plen == 92): # Euromap67 interface = 20 bytes
+            return MasterboardData_V30.unpack(buf)
+        else:
+            print "MasterboardData has wrong length: " + str(plen)
+            print "Euromap67Interface is ignored"
+            return md
+
+#this parses MasterboardData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
+class MasterboardData_V18(object):
     __slots__ = ['digital_input_bits', 'digital_output_bits',
                  'analog_input_range0', 'analog_input_range1',
                  'analog_input0', 'analog_input1',
@@ -124,10 +173,10 @@ class MasterboardData(object):
                  'masterboard_temperature',
                  'robot_voltage_48V', 'robot_current',
                  'master_io_current', 'master_safety_state',
-                 'master_onoff_state']
+                 'master_onoff_state']#subsequent slots related to 'euromap' ignored
     @staticmethod
     def unpack(buf):
-        md = MasterboardData()
+        md = MasterboardData_V18()
         (_, _,
          md.digital_input_bits, md.digital_output_bits,
          md.analog_input_range0, md.analog_input_range1,
@@ -140,6 +189,33 @@ class MasterboardData(object):
          md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
         return md
 
+#this parses MasterboardData for versions >=3.0 (i.e. 3.0)
+class MasterboardData_V30(object):
+    __slots__ = ['digital_input_bits', 'digital_output_bits',
+                 'analog_input_range0', 'analog_input_range1',
+                 'analog_input0', 'analog_input1',
+                 'analog_output_domain0', 'analog_output_domain1',
+                 'analog_output0', 'analog_output1',
+                 'masterboard_temperature',
+                 'robot_voltage_48V', 'robot_current',
+                 'master_io_current', 'safety_mode',
+                 'in_reduced_mode']#subsequent slots related to 'euromap' ignored
+    @staticmethod
+    def unpack(buf):
+        md = MasterboardData_V30()
+        (_, _, _UNDOCUMENTED_,
+         md.digital_input_bits, md.digital_output_bits,
+         md.analog_input_range0, md.analog_input_range1,
+         md.analog_input0, md.analog_input1,
+         md.analog_output_domain0, md.analog_output_domain1,
+         md.analog_output0, md.analog_output1,
+         md.masterboard_temperature,
+         md.robot_voltage_48V, md.robot_current,
+         md.master_io_current, md.safety_mode,
+         md.in_reduced_mode) = struct.unpack_from("!IBIiibbddbbddffffBB", buf)
+        return md
+
+#this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
 class CartesianInfo(object):
     __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
     @staticmethod
@@ -149,21 +225,26 @@ class CartesianInfo(object):
          ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
         return ci
 
+#this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
+#KinematicsInfo is not available in 1.6 and 1.7
 class KinematicsInfo(object):
     @staticmethod
     def unpack(buf):
         return KinematicsInfo()
 
+#helper class for ConfigurationData
 class JointLimitData(object):
     __slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
 
+#this parses ConfigurationData for versions (i.e. 1.8, 3.0)
+#ConfigurationData is not available in 1.6 and 1.7
 class ConfigurationData(object):
     __slots__ = ['joint_limit_data',
-                 'v_joint_default', 'a_joint_default', 
+                 'v_joint_default', 'a_joint_default',
                  'v_tool_default', 'a_tool_default', 'eq_radius',
                  'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
                  'masterboard_version', 'controller_box_type',
-                 'robot_type', 'robot_subtype']
+                 'robot_type', 'robot_subtype']#in v1.8 there is an additional slot 'motor_type' for each joint, which currently is ignored!
     @staticmethod
     def unpack(buf):
         cd = ConfigurationData()
@@ -179,6 +260,8 @@ class ConfigurationData(object):
          cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
         return cd
 
+#this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
+#KinematicsInfo is not available in 1.6 and 1.7
 class ForceModeData(object):
     __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz', 'robot_dexterity']
     @staticmethod
@@ -188,6 +271,20 @@ class ForceModeData(object):
          fmd.robot_dexterity) = struct.unpack_from("!IBddddddd", buf)
         return fmd
 
+#this class handles different protocol versions
+class AdditionalInfo(object):
+    @staticmethod
+    def unpack(buf):
+        ai = AdditionalInfo()
+        (plen, ptype) = struct.unpack_from("!IB", buf)
+        if plen == 10:
+            return AdditionalInfoOld.unpack(buf)
+        elif plen == 7:
+            return AdditionalInfoNew.unpack(buf)
+        else:
+            print "AdditionalInfo has wrong length: " + str(plen)
+            return ai
+
 class AdditionalInfoOld(object):
     __slots__ = ['ctrl_bits', 'teach_button']
     @staticmethod
@@ -195,7 +292,7 @@ class AdditionalInfoOld(object):
         ai = AdditionalInfoOld()
         (_,_,ai.ctrl_bits, ai.teach_button) = struct.unpack_from("!IBIB", buf)
         return ai
-        
+
 class AdditionalInfoNew(object):
     __slots__ = ['teach_button_enabled','teach_button_pressed']
     @staticmethod
@@ -203,16 +300,7 @@ class AdditionalInfoNew(object):
         ai = AdditionalInfoNew()
         (_,_,ai.teach_button_enabled, ai.teach_button_pressed) = struct.unpack_from("!IBBB", buf)
         return ai
-        
-class AdditionalInfo(object):
-    @staticmethod
-    def unpack(buf):
-        ai = AdditionalInfo()
-        (plen, ptype) = struct.unpack_from("!IB", buf)
-        if plen == 10:
-            return AdditionalInfoOld.unpack(buf)
-        else:
-            return AdditionalInfoNew.unpack(buf)
+
 
 class RobotState(object):
     __slots__ = ['robot_mode_data', 'joint_data', 'tool_data',

ur_driver/src/ur_driver/deserializeRT.py

 import struct
 import copy
 
+#this class handles different protocol versions
 class RobotStateRT(object):
+    @staticmethod
+    def unpack(buf):
+        rs = RobotStateRT()
+        (plen, ptype) = struct.unpack_from("!IB", buf)
+        if plen == 812:
+            return RobotStateRT_V18.unpack(buf)
+        elif plen == 1044:
+            return RobotStateRT_V30.unpack(buf)
+        else:
+            print "RobotStateRT has wrong length: " + str(plen)
+            return rs
+
+#this parses RobotStateRT for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
+class RobotStateRT_V18(object):
     __slots__ = ['time', 
                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
                  'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values', 
@@ -20,7 +35,7 @@ class RobotStateRT(object):
             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
 
-        rs = RobotStateRT()
+        rs = RobotStateRT_V18()
         #time: 1x double (1x 8byte)
         rs.time = struct.unpack_from("!d",buf, offset)[0]
         offset+=8
@@ -116,3 +131,175 @@ class RobotStateRT(object):
         rs.joint_modes = copy.deepcopy(all_values)
 
         return rs
+
+#this parses RobotStateRT for versions >=3.0 (i.e. 3.0)
+class RobotStateRT_V30(object):
+    __slots__ = ['time', 
+                 'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
+                 'q_actual', 'qd_actual', 'i_actual', 'i_control', 
+                 'tool_vector_actual', 'tcp_speed_actual', 'tcp_force', 
+                 'tool_vector_target', 'tcp_speed_target', 
+                 'digital_input_bits', 'motor_temperatures', 'controller_timer', 
+                 'test_value', 
+                 'robot_mode', 'joint_modes', 'safety_mode', 
+                 #6xd: unused
+                 'tool_acc_values', 
+                 #6xd: unused
+                 'speed_scaling', 'linear_momentum_norm', 
+                 #2xd: unused
+                 'v_main', 'v_robot', 'i_robot', 'v_actual']
+
+    @staticmethod
+    def unpack(buf):
+        offset = 0
+        message_size = struct.unpack_from("!i", buf, offset)[0]
+        offset+=4
+        if message_size != len(buf):
+            print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
+            raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
+
+        rs = RobotStateRT_V30()
+        #time: 1x double (1x 8byte)
+        rs.time = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #q_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.q_target = copy.deepcopy(all_values)
+        
+        #qd_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.qd_target = copy.deepcopy(all_values)
+        
+        #qdd_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.qdd_target = copy.deepcopy(all_values)
+        
+        #i_target: 6x double (6x 8byte) 
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.i_target = copy.deepcopy(all_values)
+        
+        #m_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.m_target = copy.deepcopy(all_values)
+        
+        #q_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.q_actual = copy.deepcopy(all_values)
+        
+        #qd_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.qd_actual = copy.deepcopy(all_values)
+        
+        #i_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.i_actual = copy.deepcopy(all_values)
+        
+        #i_control: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.i_control = copy.deepcopy(all_values)
+        
+        #tool_vector_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.tool_vector_actual = copy.deepcopy(all_values)
+
+        #tcp_speed_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.tcp_speed_actual = copy.deepcopy(all_values)
+        
+        #tcp_force: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.tcp_force = copy.deepcopy(all_values)
+        
+        #tool_vector_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.tool_vector_target = copy.deepcopy(all_values)
+        
+        #tcp_speed_target: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.tcp_speed_target = copy.deepcopy(all_values)
+        
+        #digital_input_bits: 1x double (1x 8byte) ?
+        rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+
+        #motor_temperatures: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.motor_temperatures = copy.deepcopy(all_values)
+        
+        #controller_timer: 1x double (1x 8byte)
+        rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #test_value: 1x double (1x 8byte)
+        rs.test_value = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #robot_mode: 1x double (1x 8byte)
+        rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #joint_modes: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.joint_modes = copy.deepcopy(all_values)
+        
+        #safety_mode: 1x double (1x 8byte)
+        rs.safety_mode = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #unused: 6x double (6x 8byte)
+        offset+=48
+        
+        #tool_acc_values: 3x double (3x 8byte)
+        all_values = list(struct.unpack_from("!ddd",buf, offset))
+        offset+=3*8
+        rs.tool_acc_values = copy.deepcopy(all_values)
+        
+        #unused: 6x double (6x 8byte)
+        offset+=48
+        
+        #speed_scaling: 1x double (1x 8byte)
+        rs.speed_scaling = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #linear_momentum_norm: 1x double (1x 8byte)
+        rs.linear_momentum_norm = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #unused: 2x double (2x 8byte)
+        offset+=16
+        
+        #v_main: 1x double (1x 8byte)
+        rs.v_main = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #v_robot: 1x double (1x 8byte)
+        rs.v_robot = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #i_robot: 1x double (1x 8byte)
+        rs.i_robot = struct.unpack_from("!d",buf, offset)[0]
+        offset+=8
+        
+        #v_actual: 6x double (6x 8byte)
+        all_values = list(struct.unpack_from("!dddddd",buf, offset))
+        offset+=6*8
+        rs.v_actual = copy.deepcopy(all_values)
+        
+        return rs