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 == 756:
00011             return RobotStateRT_V15.unpack(buf)        
00012         elif plen == 812:
00013             return RobotStateRT_V18.unpack(buf)
00014         elif plen == 1044:
00015             return RobotStateRT_V30.unpack(buf)
00016         else:
00017             print "RobotStateRT has wrong length: " + str(plen)
00018             return rs
00019 
00020 #this parses RobotStateRT for versions = v1.5
00021 #http://wiki03.lynero.net/Technical/RealTimeClientInterface?foswiki_redirect_cache=9b4574b30760f720c6f79c5f1f2203dd
00022 class RobotStateRT_V15(object):
00023     __slots__ = ['time', 
00024                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
00025                  'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values', 
00026                  'unused', 
00027                  'tcp_force', 'tool_vector', 'tcp_speed', 
00028                  'digital_input_bits', 'motor_temperatures', 'controller_timer', 
00029                  'test_value']
00030 
00031     @staticmethod
00032     def unpack(buf):
00033         offset = 0
00034         message_size = struct.unpack_from("!i", buf, offset)[0]
00035         offset+=4
00036         if message_size != len(buf):
00037             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
00038             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
00039 
00040         rs = RobotStateRT_V15()
00041         #time: 1x double (1x 8byte)
00042         rs.time = struct.unpack_from("!d",buf, offset)[0]
00043         offset+=8
00044         
00045         #q_target: 6x double (6x 8byte)
00046         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00047         offset+=6*8
00048         rs.q_target = copy.deepcopy(all_values)
00049         
00050         #qd_target: 6x double (6x 8byte)
00051         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00052         offset+=6*8
00053         rs.qd_target = copy.deepcopy(all_values)
00054         
00055         #qdd_target: 6x double (6x 8byte)
00056         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00057         offset+=6*8
00058         rs.qdd_target = copy.deepcopy(all_values)
00059         
00060         #i_target: 6x double (6x 8byte) 
00061         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00062         offset+=6*8
00063         rs.i_target = copy.deepcopy(all_values)
00064         
00065         #m_target: 6x double (6x 8byte)
00066         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00067         offset+=6*8
00068         rs.m_target = copy.deepcopy(all_values)
00069         
00070         #q_actual: 6x double (6x 8byte)
00071         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00072         offset+=6*8
00073         rs.q_actual = copy.deepcopy(all_values)
00074         
00075         #qd_actual: 6x double (6x 8byte)
00076         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00077         offset+=6*8
00078         rs.qd_actual = copy.deepcopy(all_values)
00079         
00080         #i_actual: 6x double (6x 8byte)
00081         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00082         offset+=6*8
00083         rs.i_actual = copy.deepcopy(all_values)
00084 
00085         ###
00086         
00087         #tool_acc_values: 3x double (3x 8byte)
00088         all_values = list(struct.unpack_from("!ddd",buf, offset))
00089         offset+=3*8
00090         rs.tool_acc_values = copy.deepcopy(all_values)
00091         
00092         #unused: 15x double (15x 8byte)
00093         offset+=120
00094         rs.unused = []
00095         
00096         #tcp_force: 6x double (6x 8byte)
00097         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00098         offset+=6*8
00099         rs.tcp_force = copy.deepcopy(all_values)
00100         
00101         #tool_vector: 6x double (6x 8byte)
00102         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00103         offset+=6*8
00104         rs.tool_vector = copy.deepcopy(all_values)
00105         
00106         #tcp_speed: 6x double (6x 8byte)
00107         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00108         offset+=6*8
00109         rs.tcp_speed = copy.deepcopy(all_values)
00110         
00111         #digital_input_bits: 1x double (1x 8byte) ?
00112         rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
00113         offset+=8
00114 
00115         #motor_temperatures: 6x double (6x 8byte)
00116         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00117         offset+=6*8
00118         rs.motor_temperatures = copy.deepcopy(all_values)
00119         
00120         #controller_timer: 1x double (1x 8byte)
00121         rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
00122         offset+=8
00123         
00124         #test_value: 1x double (1x 8byte)
00125         rs.test_value = struct.unpack_from("!d",buf, offset)[0]
00126         offset+=8
00127 
00128         return rs  
00129 
00130 
00131 #this parses RobotStateRT for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
00132 class RobotStateRT_V18(object):
00133     __slots__ = ['time', 
00134                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
00135                  'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values', 
00136                  'unused', 
00137                  'tcp_force', 'tool_vector', 'tcp_speed', 
00138                  'digital_input_bits', 'motor_temperatures', 'controller_timer', 
00139                  'test_value', 
00140                  'robot_mode', 'joint_modes']
00141 
00142     @staticmethod
00143     def unpack(buf):
00144         offset = 0
00145         message_size = struct.unpack_from("!i", buf, offset)[0]
00146         offset+=4
00147         if message_size != len(buf):
00148             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
00149             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
00150 
00151         rs = RobotStateRT_V18()
00152         #time: 1x double (1x 8byte)
00153         rs.time = struct.unpack_from("!d",buf, offset)[0]
00154         offset+=8
00155         
00156         #q_target: 6x double (6x 8byte)
00157         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00158         offset+=6*8
00159         rs.q_target = copy.deepcopy(all_values)
00160         
00161         #qd_target: 6x double (6x 8byte)
00162         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00163         offset+=6*8
00164         rs.qd_target = copy.deepcopy(all_values)
00165         
00166         #qdd_target: 6x double (6x 8byte)
00167         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00168         offset+=6*8
00169         rs.qdd_target = copy.deepcopy(all_values)
00170         
00171         #i_target: 6x double (6x 8byte) 
00172         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00173         offset+=6*8
00174         rs.i_target = copy.deepcopy(all_values)
00175         
00176         #m_target: 6x double (6x 8byte)
00177         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00178         offset+=6*8
00179         rs.m_target = copy.deepcopy(all_values)
00180         
00181         #q_actual: 6x double (6x 8byte)
00182         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00183         offset+=6*8
00184         rs.q_actual = copy.deepcopy(all_values)
00185         
00186         #qd_actual: 6x double (6x 8byte)
00187         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00188         offset+=6*8
00189         rs.qd_actual = copy.deepcopy(all_values)
00190         
00191         #i_actual: 6x double (6x 8byte)
00192         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00193         offset+=6*8
00194         rs.i_actual = copy.deepcopy(all_values)
00195         
00196         #tool_acc_values: 3x double (3x 8byte)
00197         all_values = list(struct.unpack_from("!ddd",buf, offset))
00198         offset+=3*8
00199         rs.tool_acc_values = copy.deepcopy(all_values)
00200         
00201         #unused: 15x double (15x 8byte)
00202         offset+=120
00203         rs.unused = []
00204         
00205         #tcp_force: 6x double (6x 8byte)
00206         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00207         offset+=6*8
00208         rs.tcp_force = copy.deepcopy(all_values)
00209         
00210         #tool_vector: 6x double (6x 8byte)
00211         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00212         offset+=6*8
00213         rs.tool_vector = copy.deepcopy(all_values)
00214         
00215         #tcp_speed: 6x double (6x 8byte)
00216         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00217         offset+=6*8
00218         rs.tcp_speed = copy.deepcopy(all_values)
00219         
00220         #digital_input_bits: 1x double (1x 8byte) ?
00221         rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
00222         offset+=8
00223 
00224         #motor_temperatures: 6x double (6x 8byte)
00225         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00226         offset+=6*8
00227         rs.motor_temperatures = copy.deepcopy(all_values)
00228         
00229         #controller_timer: 1x double (1x 8byte)
00230         rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
00231         offset+=8
00232         
00233         #test_value: 1x double (1x 8byte)
00234         rs.test_value = struct.unpack_from("!d",buf, offset)[0]
00235         offset+=8
00236         
00237         #robot_mode: 1x double (1x 8byte)
00238         rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
00239         offset+=8
00240         
00241         #joint_mode: 6x double (6x 8byte)
00242         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00243         offset+=6*8
00244         rs.joint_modes = copy.deepcopy(all_values)
00245 
00246         return rs
00247 
00248 #this parses RobotStateRT for versions >=3.0 (i.e. 3.0)
00249 class RobotStateRT_V30(object):
00250     __slots__ = ['time', 
00251                  'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', 
00252                  'q_actual', 'qd_actual', 'i_actual', 'i_control', 
00253                  'tool_vector_actual', 'tcp_speed_actual', 'tcp_force', 
00254                  'tool_vector_target', 'tcp_speed_target', 
00255                  'digital_input_bits', 'motor_temperatures', 'controller_timer', 
00256                  'test_value', 
00257                  'robot_mode', 'joint_modes', 'safety_mode', 
00258                  #6xd: unused
00259                  'tool_acc_values', 
00260                  #6xd: unused
00261                  'speed_scaling', 'linear_momentum_norm', 
00262                  #2xd: unused
00263                  'v_main', 'v_robot', 'i_robot', 'v_actual']
00264 
00265     @staticmethod
00266     def unpack(buf):
00267         offset = 0
00268         message_size = struct.unpack_from("!i", buf, offset)[0]
00269         offset+=4
00270         if message_size != len(buf):
00271             print("MessageSize: ", message_size, "; BufferSize: ", len(buf))
00272             raise Exception("Could not unpack RobotStateRT packet: length field is incorrect")
00273 
00274         rs = RobotStateRT_V30()
00275         #time: 1x double (1x 8byte)
00276         rs.time = struct.unpack_from("!d",buf, offset)[0]
00277         offset+=8
00278         
00279         #q_target: 6x double (6x 8byte)
00280         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00281         offset+=6*8
00282         rs.q_target = copy.deepcopy(all_values)
00283         
00284         #qd_target: 6x double (6x 8byte)
00285         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00286         offset+=6*8
00287         rs.qd_target = copy.deepcopy(all_values)
00288         
00289         #qdd_target: 6x double (6x 8byte)
00290         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00291         offset+=6*8
00292         rs.qdd_target = copy.deepcopy(all_values)
00293         
00294         #i_target: 6x double (6x 8byte) 
00295         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00296         offset+=6*8
00297         rs.i_target = copy.deepcopy(all_values)
00298         
00299         #m_target: 6x double (6x 8byte)
00300         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00301         offset+=6*8
00302         rs.m_target = copy.deepcopy(all_values)
00303         
00304         #q_actual: 6x double (6x 8byte)
00305         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00306         offset+=6*8
00307         rs.q_actual = copy.deepcopy(all_values)
00308         
00309         #qd_actual: 6x double (6x 8byte)
00310         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00311         offset+=6*8
00312         rs.qd_actual = copy.deepcopy(all_values)
00313         
00314         #i_actual: 6x double (6x 8byte)
00315         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00316         offset+=6*8
00317         rs.i_actual = copy.deepcopy(all_values)
00318         
00319         #i_control: 6x double (6x 8byte)
00320         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00321         offset+=6*8
00322         rs.i_control = copy.deepcopy(all_values)
00323         
00324         #tool_vector_actual: 6x double (6x 8byte)
00325         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00326         offset+=6*8
00327         rs.tool_vector_actual = copy.deepcopy(all_values)
00328 
00329         #tcp_speed_actual: 6x double (6x 8byte)
00330         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00331         offset+=6*8
00332         rs.tcp_speed_actual = copy.deepcopy(all_values)
00333         
00334         #tcp_force: 6x double (6x 8byte)
00335         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00336         offset+=6*8
00337         rs.tcp_force = copy.deepcopy(all_values)
00338         
00339         #tool_vector_target: 6x double (6x 8byte)
00340         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00341         offset+=6*8
00342         rs.tool_vector_target = copy.deepcopy(all_values)
00343         
00344         #tcp_speed_target: 6x double (6x 8byte)
00345         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00346         offset+=6*8
00347         rs.tcp_speed_target = copy.deepcopy(all_values)
00348         
00349         #digital_input_bits: 1x double (1x 8byte) ?
00350         rs.digital_input_bits = struct.unpack_from("!d",buf, offset)[0]
00351         offset+=8
00352 
00353         #motor_temperatures: 6x double (6x 8byte)
00354         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00355         offset+=6*8
00356         rs.motor_temperatures = copy.deepcopy(all_values)
00357         
00358         #controller_timer: 1x double (1x 8byte)
00359         rs.controller_timer = struct.unpack_from("!d",buf, offset)[0]
00360         offset+=8
00361         
00362         #test_value: 1x double (1x 8byte)
00363         rs.test_value = struct.unpack_from("!d",buf, offset)[0]
00364         offset+=8
00365         
00366         #robot_mode: 1x double (1x 8byte)
00367         rs.robot_mode = struct.unpack_from("!d",buf, offset)[0]
00368         offset+=8
00369         
00370         #joint_modes: 6x double (6x 8byte)
00371         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00372         offset+=6*8
00373         rs.joint_modes = copy.deepcopy(all_values)
00374         
00375         #safety_mode: 1x double (1x 8byte)
00376         rs.safety_mode = struct.unpack_from("!d",buf, offset)[0]
00377         offset+=8
00378         
00379         #unused: 6x double (6x 8byte)
00380         offset+=48
00381         
00382         #tool_acc_values: 3x double (3x 8byte)
00383         all_values = list(struct.unpack_from("!ddd",buf, offset))
00384         offset+=3*8
00385         rs.tool_acc_values = copy.deepcopy(all_values)
00386         
00387         #unused: 6x double (6x 8byte)
00388         offset+=48
00389         
00390         #speed_scaling: 1x double (1x 8byte)
00391         rs.speed_scaling = struct.unpack_from("!d",buf, offset)[0]
00392         offset+=8
00393         
00394         #linear_momentum_norm: 1x double (1x 8byte)
00395         rs.linear_momentum_norm = struct.unpack_from("!d",buf, offset)[0]
00396         offset+=8
00397         
00398         #unused: 2x double (2x 8byte)
00399         offset+=16
00400         
00401         #v_main: 1x double (1x 8byte)
00402         rs.v_main = struct.unpack_from("!d",buf, offset)[0]
00403         offset+=8
00404         
00405         #v_robot: 1x double (1x 8byte)
00406         rs.v_robot = struct.unpack_from("!d",buf, offset)[0]
00407         offset+=8
00408         
00409         #i_robot: 1x double (1x 8byte)
00410         rs.i_robot = struct.unpack_from("!d",buf, offset)[0]
00411         offset+=8
00412         
00413         #v_actual: 6x double (6x 8byte)
00414         all_values = list(struct.unpack_from("!dddddd",buf, offset))
00415         offset+=6*8
00416         rs.v_actual = copy.deepcopy(all_values)
00417         
00418         return rs