trajectory-logging-HACK.py 3.36 KB
Newer Older
Sam Calisch's avatar
Sam Calisch committed
1 2 3 4
import socket
import time
import struct
from sys import stdout
Sam Calisch's avatar
Sam Calisch committed
5
from UR10 import *
Sam Calisch's avatar
Sam Calisch committed
6 7 8 9 10 11 12 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 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76

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

HOST = "192.168.1.52"
PORT = 30003
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))

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']

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])


#values = '>I'+85*'d'+'q'+7*'d'+7*'q'+30*'d'
values = '>I'+85*'d'+'q'+44*'d'
struct = struct.Struct(values)
last = time.time()
f = open('trajectory-logging.txt','w')
Sam Calisch's avatar
Sam Calisch committed
77 78


Sam Calisch's avatar
Sam Calisch committed
79 80 81 82
def log_dev(packet):
	#print "\033[2J"
	#print "\033[H"
	#print "Message Size:\t%d"%packet[0]
83 84
	msg = (packet[1],) + tuple(packet[56:62]) + tuple(packet[62:68]) + tuple(packet[74:80]) + tuple(packet[80:86]) + tuple(packet[109:112])
	f.write( ("%f,"*(1+4*6+3)+"\n")%msg)
Sam Calisch's avatar
Sam Calisch committed
85

Sam Calisch's avatar
Sam Calisch committed
86 87 88
roy = UR10("Roy","192.168.1.52")
roy.stream_script('accel_test.script')

Sam Calisch's avatar
Sam Calisch committed
89 90 91 92 93 94 95 96 97 98 99 100
try:
	while(True):
		data = s.recv(1044)
		log_dev(struct.unpack(data))
		#print "Packet interval: %.3f ms"%(1000*(time.time()-last))
		#last = time.time()

except(KeyboardInterrupt):
	pass
s.close()
f.close()
print "Thank you."