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