UR10.py 7.92 KB
Newer Older
1 2 3 4
import socket
import time
import struct
from sys import stdout
Sam Calisch's avatar
Sam Calisch committed
5
from numpy import *
6 7 8 9 10 11

#Sam Calisch, 2015
#Center for Bits and Atoms, MIT

class UR10(object):
	header_struct = struct.Struct('>iB')
12
	robot_message_header_struct = struct.Struct('>qBB') #time stamps on program labels are -1? the uint64_t should be unsigned...
13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38
	subpackages = {
		'0':(struct.Struct('>'+'Q'+7*'?'+2*'B'+2*'d'),38,"Robot Mode"),
		'1':(struct.Struct( '>'+6*(3*'d'+4*'f'+'B') ),251,"Joint Data"),
		'2':(struct.Struct('>'+2*'b'+2*'d'+'f'+'B'+2*'f'+'B'),37,"Tool Data"),
		'3':(struct.Struct('>'+2*'i'+2*'b'+2*'d'+2*'b'+2*'d'+4*'f'+2*'B'+'b'+'L'),72,"Masterboard Data"),
		'4':(struct.Struct('>'+12*'d'),101,"Cartesian Info"),
		'6':(struct.Struct('>'+53*'d'+4*'i'),445,"Config Data"),
		'7':(struct.Struct('>'+7*'d'),61,"Force Data")} 
	robot_modes = ['Disconnected','Confirm Safety','Booting','Power Off','Power On',\
			'Idle','Backdrive','Running','Updating Firmware']
	joint_modes = {
		'236':'Shutting Down',
		'237':'D Calibration',
		'238':'Backdrive',
		'239':'Power Off',
		'245':'Not Responding',
		'246':'Initialization',
		'247':'Booting',
		'248':'D Calibration Error',
		'249':'Bootloader',
		'250':'Calibration',
		'252':'Fault',
		'253':'Running',
		'255':'Idle'}
	safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd']

39
	def __init__(self,name,ip):
40 41 42 43
		self.name = name
		self.ip = ip
		self.last_packet_time = 0
		self.socket_opened = False
Sam Calisch's avatar
Sam Calisch committed
44
		self.tool_vector = []
45
	def open_socket(self,port):
46
		self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
47
		self.socket.connect((self.ip, port))
48
		self.socket_opened = True
49
		print "Opened socket: ",self.ip, port
50 51 52
	def close_socket(self):
		self.socket.close()
		self.socket_opened = False
53
		print "Socket closed"
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
	def run_program(self,program_name):
		def recieve():
			reply = ''
			while 1:
				try:			
					new = self.socket.recv(1)
					reply += new
				except(socket.timeout):				
					break
			return reply
		if self.socket_opened == True:
			RuntimeError #socket already open
		else:
			self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
			self.socket.settimeout(.25)
			self.socket.connect((self.ip, 29999))
			self.socket_opened = True
			self.socket.send("load %s\n"%program_name)
			reply = recieve().split("\n")	
			for line in reply: print line		
			if reply[1][:15]=="Loading program":
				self.socket.send("play")
				#reply = recieve().split('\n')
				#for line in reply: print line
			self.socket.settimeout(None)
			self.close_socket()
80 81
	def initialize_state(self,n=10):
		for i in range(n): self.read_packet_secondary() #establish starting position
82

83
	def flush_secondary(self):
84
		#print "flushing"
85
		header = self.socket.recv(5)
86 87 88
		if len(header)==5:
			header = UR10.header_struct.unpack(header)
			self.socket.recv(header[1])
Sam Calisch's avatar
Sam Calisch committed
89 90 91 92 93
	def send(self,command):
		if not self.socket_opened:
			assert(0) #socket not open
		else:
			self.socket.send(command)
94 95

	def update_state_secondary(self,packet,packet_type):
96
		#print "Updating %s with packet type %d: %s"%(self.name,packet_type,UR10.subpackages[str(packet_type)][2])
Sam Calisch's avatar
Sam Calisch committed
97 98
		if packet_type == 4:
			self.tool_vector = asarray(packet[:6])
99

100
	def handle_subpackage_robot_state(self,remaining):
101 102 103 104 105 106 107 108 109 110 111 112 113
		header = self.socket.recv(5)
		header = UR10.header_struct.unpack(header)
		sublength = header[0]
		subtype = header[1]
		try:		
			subpackage = UR10.subpackages[str(subtype)]
			assert(subpackage[1] == sublength ) #make sure lengths match
			data = self.socket.recv(sublength-5)
			self.update_state_secondary(subpackage[0].unpack(data),subtype)			
		except(KeyError):
			#print "\tPackage type %d not known."%subtype
			self.socket.recv(sublength-5)
		return remaining-sublength
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137

	def handle_subpackage_robot_message(self,remaining):
		header = self.socket.recv(10); remaining -= 10
		header = UR10.robot_message_header_struct.unpack(header)
		data = self.socket.recv(remaining)
		if header[2]==1:
			#label message
			message_struct = struct.Struct(">i"+(remaining-4)*"b")
			data = message_struct.unpack(data)
			data = [header[0],data[0],"".join(map(chr,data[1:]))]
			print "$ %d %s"%(data[1],data[2])
		elif header[2]==7:
			#key message
			message_struct = struct.Struct(">iiB"+(remaining-9)*"b")
			data = message_struct.unpack(data)
			data = [header[0],data[0],data[1],data[2],"".join(map(chr,data[3:3+data[2]])),"".join(map(chr,data[3+data[2]:]))]
			print data[4]+": "+data[5]
		elif header[2]==3:
			#version message
			#message_struct = struct.Struct(">bbbBBi"+(remaining-9)*"b")
			message_struct = struct.Struct(remaining*"b") #who cares
			data = message_struct.unpack(data)
			data = "".join(map(chr,data[remaining-21:]))
			print "Control software built: ",data	
138 139
		elif header[2]==10:
			print "Runtime exception: ",data	
140
		else:
141
			print "Unknown robot message:",header
142 143
		return 0

144
	def read_packet_secondary(self):
145
		#Primary/secondary client interface (100 ms packets)	
146 147
		header = self.socket.recv(5)
		header = UR10.header_struct.unpack(header)
148
		if header[1] == 16: #if we're not at packet start, flush	
149
			#print "%s packet interval: %.1f ms"%(self.name,1000*(time.time()-self.last_packet_time))
150
			#self.last_packet_time = time.time()	
151 152
			remaining = header[0]-5
			while(remaining>0):
153
				remaining = self.handle_subpackage_robot_state(remaining)
154
				assert(remaining>=0)
155 156 157 158 159 160 161 162
		elif header[1] == 20:
			remaining = header[0]-5
			while(remaining>0):
				remaining = self.handle_subpackage_robot_message(remaining)
				assert(remaining>=0)
		else:
			self.flush_secondary()

163
	#TODO: implement global variables setup message!
164

165 166
	def stream_script(self,fn):
		'''
167
		Stream a script to the robot -- scripts are expected to be written as a function. 
168 169
		'''
		f = open(fn,'r')
170
		self.open_socket(30001)
171
		self.initialize_state()
172 173
		for l in f.readlines():
			self.send(l)
174
			#print l.strip('\n')
175 176 177 178 179 180 181 182 183
		print "Done sending"
		while True:
			try:
				self.read_packet_secondary()
				#stdout.write("[%.4f,%.4f,%.4f,%.4f,%.4f,%.4f]\r"%tuple(self.tool_vector) )
				#stdout.flush()
			except(KeyboardInterrupt):
				self.send("stop program\n")
				break
184 185
			except:
				break
186 187
		self.close_socket()

188 189 190 191



#not yet incorporated
192

193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231
def print_packet(packet):
	print "\033[2J"
	print "\033[H"
	print "Message Size:\t%d"%packet[0]
	print "Time:\t\t%f"%packet[1]
	print "q target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[2:8])
	print "qd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[8:14])
	print "qdd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[14:20])
	print "I target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[20:26])
	print "M target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[26:32])
	print "q actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[32:38])
	print "qd actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[38:44])
	print "I actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[44:50])
	print "I control:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[50:56])
	print "Tool vec act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[56:62])
	print "Tool speed act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[62:68])
	print "TCP force:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[68:74])
	print "Tool vec targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[74:80])
	print "TCP speed targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[80:86])
	print "Digital Ins:\t%s"%('{0:08b}'.format(packet[86]))
	print "Motor temps:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[87:93])
	print "Control Timer:\t%f"%packet[93]
	#94 unused
	print "Robot mode:\t%s"%robot_modes[int(packet[95])]
	print "Joint 1 mode:\t%s"%joint_modes['%d'%packet[96]]
	print "Joint 2 mode:\t%s"%joint_modes['%d'%packet[97]]
	print "Joint 3 mode:\t%s"%joint_modes['%d'%packet[98]]
	print "Joint 4 mode:\t%s"%joint_modes['%d'%packet[99]]
	print "Joint 5 mode:\t%s"%joint_modes['%d'%packet[100]]
	print "Joint 6 mode:\t%s"%joint_modes['%d'%packet[101]]
	print "Safety mode:\t%s"%safety_modes[int(packet[102])-1]
	print "Tool accel:\t(%f,%f,%f)"%tuple(packet[109:112])
	print "Speed scale:\t%f"%packet[118]
	print "Lin Momentum:\t%f"%packet[119]
	print "Main voltage:\t%f"%packet[122]
	print "Robot voltage:\t%f"%packet[123]
	print "Robot current:\t%f"%packet[124]
	print "Joint voltage:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[125:131])