00001 import struct
00002
00003 class PackageType(object):
00004 ROBOT_MODE_DATA = 0
00005 JOINT_DATA = 1
00006 TOOL_DATA = 2
00007 MASTERBOARD_DATA = 3
00008 CARTESIAN_INFO = 4
00009 KINEMATICS_INFO = 5
00010 CONFIGURATION_DATA = 6
00011 FORCE_MODE_DATA = 7
00012 ADDITIONAL_INFO = 8
00013
00014 class RobotMode(object):
00015 RUNNING = 0
00016 FREEDRIVE = 1
00017 READY = 2
00018 INITIALIZING = 3
00019 SECURITY_STOPPED = 4
00020 EMERGENCY_STOPPED = 5
00021 FATAL_ERROR = 6
00022 NO_POWER = 7
00023 NOT_CONNECTED = 8
00024 SHUTDOWN = 9
00025 SAFEGUARD_STOP = 10
00026
00027 class JointMode(object):
00028 PART_D_CALIBRATION = 237
00029 BACKDRIVE = 238
00030 POWER_OFF = 239
00031 EMERGENCY_STOPPED = 240
00032 CALVAL_INITIALIZATION = 241
00033 ERROR = 242
00034 FREEDRIVE = 243
00035 SIMULATED = 244
00036 NOT_RESPONDING = 245
00037 MOTOR_INITIALISATION = 246
00038 ADC_CALIBRATION = 247
00039 DEAD_COMMUTATION = 248
00040 BOOTLOADER = 249
00041 CALIBRATION = 250
00042 STOPPED = 251
00043 FAULT = 252
00044 RUNNING = 253
00045 INITIALIZATION = 254
00046 IDLE = 255
00047
00048 class ToolMode(object):
00049 BOOTLOADER = 249
00050 RUNNING = 253
00051 IDLE = 255
00052
00053 class MasterSafetyState(object):
00054 UNDEFINED = 0
00055 BOOTLOADER = 1
00056 FAULT = 2
00057 BOOTING = 3
00058 INITIALIZING = 4
00059 ROBOT_EMERGENCY_STOP = 5
00060 EXTERNAL_EMERGENCY_STOP = 6
00061 SAFEGUARD_STOP = 7
00062 OK = 8
00063
00064 class MasterOnOffState(object):
00065 OFF = 0
00066 TURNING_ON = 1
00067 ON = 2
00068 TURNING_OFF = 3
00069
00070 class RobotModeData(object):
00071 __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
00072 'power_on_robot', 'emergency_stopped',
00073 'security_stopped', 'program_running', 'program_paused',
00074 'robot_mode', 'speed_fraction']
00075 @staticmethod
00076 def unpack(buf):
00077 rmd = RobotModeData()
00078 (_, _,
00079 rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
00080 rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
00081 rmd.program_running, rmd.program_paused, rmd.robot_mode,
00082 rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
00083 return rmd
00084
00085
00086 class JointData(object):
00087 __slots__ = ['q_actual', 'q_target', 'qd_actual',
00088 'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
00089 @staticmethod
00090 def unpack(buf):
00091 all_joints = []
00092 offset = 5
00093 for i in range(6):
00094 jd = JointData()
00095 (jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
00096 jd.T_motor, jd.T_micro,
00097 jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
00098 offset += 41
00099 all_joints.append(jd)
00100 return all_joints
00101
00102 class ToolData(object):
00103 __slots__ = ['analog_input_range2', 'analog_input_range3',
00104 'analog_input2', 'analog_input3',
00105 'tool_voltage_48V', 'tool_output_voltage', 'tool_current',
00106 'tool_temperature', 'tool_mode']
00107 @staticmethod
00108 def unpack(buf):
00109 td = ToolData()
00110 (_, _,
00111 td.analog_input_range2, td.analog_input_range3,
00112 td.analog_input2, td.analog_input3,
00113 td.tool_voltage_48V, td.tool_output_voltage, td.tool_current,
00114 td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
00115 return td
00116
00117 class MasterboardData(object):
00118 __slots__ = ['digital_input_bits', 'digital_output_bits',
00119 'analog_input_range0', 'analog_input_range1',
00120 'analog_input0', 'analog_input1',
00121 'analog_output_domain0', 'analog_output_domain1',
00122 'analog_output0', 'analog_output1',
00123 'masterboard_temperature',
00124 'robot_voltage_48V', 'robot_current',
00125 'master_io_current', 'master_safety_state',
00126 'master_onoff_state']
00127 @staticmethod
00128 def unpack(buf):
00129 md = MasterboardData()
00130 (_, _,
00131 md.digital_input_bits, md.digital_output_bits,
00132 md.analog_input_range0, md.analog_input_range1,
00133 md.analog_input0, md.analog_input1,
00134 md.analog_output_domain0, md.analog_output_domain1,
00135 md.analog_output0, md.analog_output1,
00136 md.masterboard_temperature,
00137 md.robot_voltage_48V, md.robot_current,
00138 md.master_io_current, md.master_safety_state,
00139 md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
00140 return md
00141
00142 class CartesianInfo(object):
00143 __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
00144 @staticmethod
00145 def unpack(buf):
00146 ci = CartesianInfo()
00147 (_, _,
00148 ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
00149 return ci
00150
00151 class KinematicsInfo(object):
00152 @staticmethod
00153 def unpack(buf):
00154 return KinematicsInfo()
00155
00156 class JointLimitData(object):
00157 __slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
00158
00159 class ConfigurationData(object):
00160 __slots__ = ['joint_limit_data',
00161 'v_joint_default', 'a_joint_default',
00162 'v_tool_default', 'a_tool_default', 'eq_radius',
00163 'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
00164 'masterboard_version', 'controller_box_type',
00165 'robot_type', 'robot_subtype']
00166 @staticmethod
00167 def unpack(buf):
00168 cd = ConfigurationData()
00169 cd.joint_limit_data = []
00170 for i in range(6):
00171 jld = JointLimitData()
00172 (jld.min_limit, jld.max_limit) = struct.unpack_from("!dd", buf, 5+16*i)
00173 (jld.max_speed, jld.max_acceleration) = struct.unpack_from("!dd", buf, 5+16*6+16*i)
00174 cd.joint_limit_data.append(jld)
00175 (cd.v_joint_default, cd.a_joint_default, cd.v_tool_default, cd.a_tool_default,
00176 cd.eq_radius) = struct.unpack_from("!ddddd", buf, 5+32*6)
00177 (cd.masterboard_version, cd.controller_box_type, cd.robot_type,
00178 cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
00179 return cd
00180
00181 class ForceModeData(object):
00182 __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz', 'robot_dexterity']
00183 @staticmethod
00184 def unpack(buf):
00185 fmd = ForceModeData()
00186 (_, _, fmd.x, fmd.y, fmd.z, fmd.rx, fmd.ry, fmd.rz,
00187 fmd.robot_dexterity) = struct.unpack_from("!IBddddddd", buf)
00188 return fmd
00189
00190 class AdditionalInfo(object):
00191 __slots__ = ['ctrl_bits', 'teach_button']
00192 @staticmethod
00193 def unpack(buf):
00194 ai = AdditionalInfo()
00195 (_, _, ai.ctrl_bits, ai.teach_button) = struct.unpack_from("!IBIB", buf)
00196 return ai
00197
00198 class RobotState(object):
00199 __slots__ = ['robot_mode_data', 'joint_data', 'tool_data',
00200 'masterboard_data', 'cartesian_info',
00201 'kinematics_info', 'configuration_data',
00202 'force_mode_data', 'additional_info',
00203 'unknown_ptypes']
00204
00205 def __init__(self):
00206 self.unknown_ptypes = []
00207
00208 @staticmethod
00209 def unpack(buf):
00210 length, mtype = struct.unpack_from("!IB", buf)
00211 if length != len(buf):
00212 raise Exception("Could not unpack packet: length field is incorrect")
00213 if mtype != 16:
00214 if mtype == 20:
00215 print "Likely a syntax error:"
00216 print buf[:2048]
00217 raise Exception("Fatal error when unpacking RobotState packet")
00218
00219 rs = RobotState()
00220 offset = 5
00221 while offset < len(buf):
00222 length, ptype = struct.unpack_from("!IB", buf, offset)
00223 assert offset + length <= len(buf)
00224 package_buf = buffer(buf, offset, length)
00225 offset += length
00226
00227 if ptype == PackageType.ROBOT_MODE_DATA:
00228 rs.robot_mode_data = RobotModeData.unpack(package_buf)
00229 elif ptype == PackageType.JOINT_DATA:
00230 rs.joint_data = JointData.unpack(package_buf)
00231 elif ptype == PackageType.TOOL_DATA:
00232 rs.tool_data = ToolData.unpack(package_buf)
00233 elif ptype == PackageType.MASTERBOARD_DATA:
00234 rs.masterboard_data = MasterboardData.unpack(package_buf)
00235 elif ptype == PackageType.CARTESIAN_INFO:
00236 rs.cartesian_info = CartesianInfo.unpack(package_buf)
00237 elif ptype == PackageType.KINEMATICS_INFO:
00238 rs.kinematics_info = KinematicsInfo.unpack(package_buf)
00239 elif ptype == PackageType.CONFIGURATION_DATA:
00240 rs.configuration_data = ConfigurationData.unpack(package_buf)
00241 elif ptype == PackageType.FORCE_MODE_DATA:
00242 rs.force_mode_data = ForceModeData.unpack(package_buf)
00243 elif ptype == PackageType.ADDITIONAL_INFO:
00244 rs.additional_info = AdditionalInfo.unpack(package_buf)
00245 else:
00246 rs.unknown_ptypes.append(ptype)
00247 return rs
00248
00249 def pstate(o, indent=''):
00250 for s in o.__slots__:
00251 child = getattr(o, s, None)
00252 if child is None:
00253 print "%s%s: None" % (indent, s)
00254 elif hasattr(child, '__slots__'):
00255 print "%s%s:" % (indent, s)
00256 pstate(child, indent + ' ')
00257 elif hasattr(child, '__iter__'):
00258 print "%s%s:" % (indent, s)
00259 for i, c in enumerate(child):
00260 print "%s [%i]:" % (indent, i)
00261 pstate(c, indent + ' ')
00262 else:
00263 print "%s%s: %s" % (indent, s, child)