ur_driver: deserializeRT.py Source File

Go to the documentation of this file.
00001 import struct
00002 import copy
00003 
00004 #this class handles different protocol versions
00005 class RobotStateRT(object):
00006     @staticmethod
00007     def unpack(buf):
00008         rs = RobotStateRT()
00009         (plen, ptype) = struct.unpack_from("!IB", buf)
00010         if plen == 812:
00011             return RobotStateRT_V18.unpack(buf)
00012         elif plen == 1044:
00013             return RobotStateRT_V30.unpack(buf)
00014         else:
00015             print "RobotStateRT has wrong length: " + str(plen)
00016             return rs
00017 
00018 #this parses RobotStateRT for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
00019 class RobotStateRT_V18(object):
00020     __slots__ = ['time', 
00021                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
00022                  'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values', 
00023                  'unused', 
00024                  'tcp_force', 'tool_vector', 'tcp_speed', 
00025                  'digital_input_bits', 'motor_temperatures', 'controller_timer', 
00026                  'test_value', 
00027                  'robot_mode', 'joint_modes']
00028 
00029     @staticmethod
00030     def unpack(buf):
00031         offset = 0
00032         message_size = struct.unpack_from("!i", buf, offset)[0]
00033         offset+=4
00034         if message_size != len(buf):
00035             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
00036             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
00037 
00038         rs = RobotStateRT_V18()
00039         #time: 1x double (1x 8byte)
00040         rs.time = struct.unpack_from("!d",buf, offset)[0]
00041         offset+=8
00042         
00043         #q_target: 6x double (6x 8byte)
00044         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00045         offset+=6*8
00046         rs.q_target = copy.deepcopy(all_values)
00047         
00048         #qd_target: 6x double (6x 8byte)
00049         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00050         offset+=6*8
00051         rs.qd_target = copy.deepcopy(all_values)
00052         
00053         #qdd_target: 6x double (6x 8byte)
00054         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00055         offset+=6*8
00056         rs.qdd_target = copy.deepcopy(all_values)
00057         
00058         #i_target: 6x double (6x 8byte) 
00059         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00060         offset+=6*8
00061         rs.i_target = copy.deepcopy(all_values)
00062         
00063         #m_target: 6x double (6x 8byte)
00064         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00065         offset+=6*8
00066         rs.m_target = copy.deepcopy(all_values)
00067         
00068         #q_actual: 6x double (6x 8byte)
00069         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00070         offset+=6*8
00071         rs.q_actual = copy.deepcopy(all_values)
00072         
00073         #qd_actual: 6x double (6x 8byte)
00074         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00075         offset+=6*8
00076         rs.qd_actual = copy.deepcopy(all_values)
00077         
00078         #i_actual: 6x double (6x 8byte)
00079         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00080         offset+=6*8
00081         rs.i_actual = copy.deepcopy(all_values)
00082         
00083         #tool_acc_values: 3x double (3x 8byte)
00084         all_values = list(struct.unpack_from("!ddd",buf, offset))
00085         offset+=3*8
00086         rs.tool_acc_values = copy.deepcopy(all_values)
00087         
00088         #unused: 15x double (15x 8byte)
00089         offset+=120
00090         rs.unused = []
00091         
00092         #tcp_force: 6x double (6x 8byte)
00093         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00094         offset+=6*8
00095         rs.tcp_force = copy.deepcopy(all_values)
00096         
00097         #tool_vector: 6x double (6x 8byte)
00098         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00099         offset+=6*8
00100         rs.tool_vector = copy.deepcopy(all_values)
00101         
00102         #tcp_speed: 6x double (6x 8byte)
00103         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00104         offset+=6*8
00105         rs.tcp_speed = copy.deepcopy(all_values)
00106         
00107         #digital_input_bits: 1x double (1x 8byte) ?
00108         rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
00109         offset+=8
00110 
00111         #motor_temperatures: 6x double (6x 8byte)
00112         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00113         offset+=6*8
00114         rs.motor_temperatures = copy.deepcopy(all_values)
00115         
00116         #controller_timer: 1x double (1x 8byte)
00117         rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
00118         offset+=8
00119         
00120         #test_value: 1x double (1x 8byte)
00121         rs.test_value = struct.unpack_from("!d",buf, offset)[0]
00122         offset+=8
00123         
00124         #robot_mode: 1x double (1x 8byte)
00125         rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
00126         offset+=8
00127         
00128         #joint_mode: 6x double (6x 8byte)
00129         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00130         offset+=6*8
00131         rs.joint_modes = copy.deepcopy(all_values)
00132 
00133         return rs
00134 
00135 #this parses RobotStateRT for versions >=3.0 (i.e. 3.0)
00136 class RobotStateRT_V30(object):
00137     __slots__ = ['time', 
00138                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
00139                  'q_actual', 'qd_actual', 'i_actual', 'i_control', 
00140                  'tool_vector_actual', 'tcp_speed_actual', 'tcp_force', 
00141                  'tool_vector_target', 'tcp_speed_target', 
00142                  'digital_input_bits', 'motor_temperatures', 'controller_timer', 
00143                  'test_value', 
00144                  'robot_mode', 'joint_modes', 'safety_mode', 
00145                  #6xd: unused
00146                  'tool_acc_values', 
00147                  #6xd: unused
00148                  'speed_scaling', 'linear_momentum_norm', 
00149                  #2xd: unused
00150                  'v_main', 'v_robot', 'i_robot', 'v_actual']
00151 
00152     @staticmethod
00153     def unpack(buf):
00154         offset = 0
00155         message_size = struct.unpack_from("!i", buf, offset)[0]
00156         offset+=4
00157         if message_size != len(buf):
00158             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
00159             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
00160 
00161         rs = RobotStateRT_V30()
00162         #time: 1x double (1x 8byte)
00163         rs.time = struct.unpack_from("!d",buf, offset)[0]
00164         offset+=8
00165         
00166         #q_target: 6x double (6x 8byte)
00167         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00168         offset+=6*8
00169         rs.q_target = copy.deepcopy(all_values)
00170         
00171         #qd_target: 6x double (6x 8byte)
00172         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00173         offset+=6*8
00174         rs.qd_target = copy.deepcopy(all_values)
00175         
00176         #qdd_target: 6x double (6x 8byte)
00177         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00178         offset+=6*8
00179         rs.qdd_target = copy.deepcopy(all_values)
00180         
00181         #i_target: 6x double (6x 8byte) 
00182         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00183         offset+=6*8
00184         rs.i_target = copy.deepcopy(all_values)
00185         
00186         #m_target: 6x double (6x 8byte)
00187         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00188         offset+=6*8
00189         rs.m_target = copy.deepcopy(all_values)
00190         
00191         #q_actual: 6x double (6x 8byte)
00192         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00193         offset+=6*8
00194         rs.q_actual = copy.deepcopy(all_values)
00195         
00196         #qd_actual: 6x double (6x 8byte)
00197         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00198         offset+=6*8
00199         rs.qd_actual = copy.deepcopy(all_values)
00200         
00201         #i_actual: 6x double (6x 8byte)
00202         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00203         offset+=6*8
00204         rs.i_actual = copy.deepcopy(all_values)
00205         
00206         #i_control: 6x double (6x 8byte)
00207         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00208         offset+=6*8
00209         rs.i_control = copy.deepcopy(all_values)
00210         
00211         #tool_vector_actual: 6x double (6x 8byte)
00212         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00213         offset+=6*8
00214         rs.tool_vector_actual = copy.deepcopy(all_values)
00215 
00216         #tcp_speed_actual: 6x double (6x 8byte)
00217         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00218         offset+=6*8
00219         rs.tcp_speed_actual = copy.deepcopy(all_values)
00220         
00221         #tcp_force: 6x double (6x 8byte)
00222         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00223         offset+=6*8
00224         rs.tcp_force = copy.deepcopy(all_values)
00225         
00226         #tool_vector_target: 6x double (6x 8byte)
00227         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00228         offset+=6*8
00229         rs.tool_vector_target = copy.deepcopy(all_values)
00230         
00231         #tcp_speed_target: 6x double (6x 8byte)
00232         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00233         offset+=6*8
00234         rs.tcp_speed_target = copy.deepcopy(all_values)
00235         
00236         #digital_input_bits: 1x double (1x 8byte) ?
00237         rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
00238         offset+=8
00239 
00240         #motor_temperatures: 6x double (6x 8byte)
00241         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00242         offset+=6*8
00243         rs.motor_temperatures = copy.deepcopy(all_values)
00244         
00245         #controller_timer: 1x double (1x 8byte)
00246         rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
00247         offset+=8
00248         
00249         #test_value: 1x double (1x 8byte)
00250         rs.test_value = struct.unpack_from("!d",buf, offset)[0]
00251         offset+=8
00252         
00253         #robot_mode: 1x double (1x 8byte)
00254         rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
00255         offset+=8
00256         
00257         #joint_modes: 6x double (6x 8byte)
00258         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00259         offset+=6*8
00260         rs.joint_modes = copy.deepcopy(all_values)
00261         
00262         #safety_mode: 1x double (1x 8byte)
00263         rs.safety_mode = struct.unpack_from("!d",buf, offset)[0]
00264         offset+=8
00265         
00266         #unused: 6x double (6x 8byte)
00267         offset+=48
00268         
00269         #tool_acc_values: 3x double (3x 8byte)
00270         all_values = list(struct.unpack_from("!ddd",buf, offset))
00271         offset+=3*8
00272         rs.tool_acc_values = copy.deepcopy(all_values)
00273         
00274         #unused: 6x double (6x 8byte)
00275         offset+=48
00276         
00277         #speed_scaling: 1x double (1x 8byte)
00278         rs.speed_scaling = struct.unpack_from("!d",buf, offset)[0]
00279         offset+=8
00280         
00281         #linear_momentum_norm: 1x double (1x 8byte)
00282         rs.linear_momentum_norm = struct.unpack_from("!d",buf, offset)[0]
00283         offset+=8
00284         
00285         #unused: 2x double (2x 8byte)
00286         offset+=16
00287         
00288         #v_main: 1x double (1x 8byte)
00289         rs.v_main = struct.unpack_from("!d",buf, offset)[0]
00290         offset+=8
00291         
00292         #v_robot: 1x double (1x 8byte)
00293         rs.v_robot = struct.unpack_from("!d",buf, offset)[0]
00294         offset+=8
00295         
00296         #i_robot: 1x double (1x 8byte)
00297         rs.i_robot = struct.unpack_from("!d",buf, offset)[0]
00298         offset+=8
00299         
00300         #v_actual: 6x double (6x 8byte)
00301         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00302         offset+=6*8
00303         rs.v_actual = copy.deepcopy(all_values)
00304         
00305         return rs