From 22eaec6e422b15c3bf4618600542798f9aee763b Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Tue, 28 Oct 2014 12:22:50 +0000
Subject: [PATCH] version-aware implementation for deserialize and
deserializeRT in order to support v3.0
---
ur_driver/src/ur_driver/deserialize.py | 117 ++++++++++++--
ur_driver/src/ur_driver/deserializeRT.py | 189 ++++++++++++++++++++++-
2 files changed, 290 insertions(+), 16 deletions(-)
diff --git a/ur_driver/src/ur_driver/deserialize.py b/ur_driver/src/ur_driver/deserialize.py
index 253a5fe..8b68758 100644
--- a/ur_driver/src/ur_driver/deserialize.py
+++ b/ur_driver/src/ur_driver/deserialize.py
@@ -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 == 37:
+ 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,23 @@ 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', '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.target_speed_fraction, rmd.speed_scaling) = struct.unpack_from("!IBQ???????Bdd", 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']
@@ -100,6 +131,7 @@ class JointData(object):
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 +147,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:
+ return MasterboardData_V18.unpack(buf)
+ elif plen == 68:
+ 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 +172,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 +188,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()
+ (_, _,
+ 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("!IBiibbddbbddffffBB", 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 +224,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_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 +259,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 +270,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
@@ -203,16 +299,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',
diff --git a/ur_driver/src/ur_driver/deserializeRT.py b/ur_driver/src/ur_driver/deserializeRT.py
index 5fe97d7..fc083e8 100644
--- a/ur_driver/src/ur_driver/deserializeRT.py
+++ b/ur_driver/src/ur_driver/deserializeRT.py
@@ -1,7 +1,22 @@
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
--
GitLab