diff --git a/ur_driver/src/ur_driver/deserializeRT.py b/ur_driver/src/ur_driver/deserializeRT.py index fc083e8574fd516fddd5e0152342615be8cca2d8..ff6750cd42c10f8403cd9b91c0025b1656b83a47 100644 --- a/ur_driver/src/ur_driver/deserializeRT.py +++ b/ur_driver/src/ur_driver/deserializeRT.py @@ -7,7 +7,9 @@ class RobotStateRT(object): def unpack(buf): rs = RobotStateRT() (plen, ptype) = struct.unpack_from("!IB", buf) - if plen == 812: + if plen == 756: + return RobotStateRT_V15.unpack(buf) + elif plen == 812: return RobotStateRT_V18.unpack(buf) elif plen == 1044: return RobotStateRT_V30.unpack(buf) @@ -15,6 +17,117 @@ class RobotStateRT(object): print "RobotStateRT has wrong length: " + str(plen) return rs +#this parses RobotStateRT for versions = v1.5 +#http://wiki03.lynero.net/Technical/RealTimeClientInterface?foswiki_redirect_cache=9b4574b30760f720c6f79c5f1f2203dd +class RobotStateRT_V15(object): + __slots__ = ['time', + 'q_target', 'qd_target', 'qdd_target', 'i_target', 'm_target', + 'q_actual', 'qd_actual', 'i_actual', 'tool_acc_values', + 'unused', + 'tcp_force', 'tool_vector', 'tcp_speed', + 'digital_input_bits', 'motor_temperatures', 'controller_timer', + 'test_value'] + + @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_V15() + #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) + + ### + + #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: 15x double (15x 8byte) + offset+=120 + rs.unused = [] + + #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: 6x double (6x 8byte) + all_values = list(struct.unpack_from("!dddddd",buf, offset)) + offset+=6*8 + rs.tool_vector = copy.deepcopy(all_values) + + #tcp_speed: 6x double (6x 8byte) + all_values = list(struct.unpack_from("!dddddd",buf, offset)) + offset+=6*8 + rs.tcp_speed = 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 + + return rs + + #this parses RobotStateRT for versions <= v1.8 (i.e. 1.6, 1.7, 1.8) class RobotStateRT_V18(object): __slots__ = ['time',