Commit fde8dd96 authored by Sam Calisch's avatar Sam Calisch

created separate class file for UR10 related things, broke old tests, implemented dashboard server

parent 00af2344
import socket
import time
import struct
from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
class UR10(object):
header_struct = struct.Struct('>iB')
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']
def __init__(self,name,ip,port):
self.name = name
self.ip = ip
self.port = port
self.last_packet_time = 0
self.socket_opened = False
def open_socket(self):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.ip, self.port))
self.socket_opened = True
def close_socket(self):
self.socket.close()
self.socket_opened = False
print "Socket closed"
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()
def flush(self):
print "flushing"
if self.port == 30002:
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
self.socket.recv(header[1])
else:
print "Don't know how to flush port %d"%self.port
NotImplementedError
def update_state_secondary(self,packet,packet_type):
print "Updating %s with packet type %d: %s"%(self.name,packet_type,UR10.subpackages[str(packet_type)][2])
def handle_subpackage_secondary(self,remaining):
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
def read_packet(self):
if self.port == 30002:
#Secondary client interface (100 ms packets)
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
if header[1]!=16: #if we're not at packet start, flush
self.flush()
else:
print "%s packet interval: %.1f ms"%(self.name,1000*(time.time()-self.last_packet_time))
self.last_packet_time = time.time()
remaining = header[0]-5
while(remaining>0):
remaining = self.handle_subpackage_secondary(remaining)
assert(remaining>=0)
elif self.port == 30003:
#Real time interface (8 ms packets)
pass
else:
NotImplementedError
def send(self,cmd):
assert(self.socket_opened)
self.socket.send(cmd)
#not yet incorporated
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])
from UR10 import *
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
roy = UR10("Roy","192.168.1.52",30002)
siegfried = UR10("Siegfried","192.168.1.51",30002)
#roy.open_socket()
#siegfried.open_socket()
siegfried.run_program('/programs/partner_test.urp')
'''
last = time.time()
try:
while(True):
roy.read_packet()
siegfried.read_packet()
except(KeyboardInterrupt):
pass
roy.close_socket()
siegfried.close_socket()
'''
print "Thank you."
import socket
import time
import struct
from sys import stdout
from UR10 import *
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
class UR10(object):
header_struct = struct.Struct('>iB')
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']
def __init__(self,name,ip,port):
self.name = name
self.ip = ip
self.port = port
self.last_packet_time = 0
def open_socket(self):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.ip, self.port))
def close_socket(self):
self.socket.close()
def flush(self):
print "flushing"
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
self.socket.recv(header[1])
def update_state(self,packet,packet_type):
print "Updating %s with packet type %d: %s"%(self.name,packet_type,UR10.subpackages[str(packet_type)][2])
def handle_subpackage(self,remaining):
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(subpackage[0].unpack(data),subtype)
except(KeyError):
#print "\tPackage type %d not known."%subtype
self.socket.recv(sublength-5)
return remaining-sublength
def read_packet(self):
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
if header[1]!=16: #if we're not at packet start, flush
self.flush()
else:
print "%s packet interval: %.1f ms"%(self.name,1000*(time.time()-self.last_packet_time))
self.last_packet_time = time.time()
remaining = header[0]-5
while(remaining>0):
remaining = self.handle_subpackage(remaining)
assert(remaining>=0)
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])
roy = UR10("Roy","192.168.1.52",30002)
......@@ -138,7 +14,6 @@ try:
while(True):
roy.read_packet()
siegfried.read_packet()
#s1.send("set_digital_out(2,True)\n") #only for testing response
except(KeyboardInterrupt):
pass
......
import socket
import time
import struct
from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
HOST = "192.168.1.51"
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()
try:
while(True):
data = s.recv(1044)
print_packet(struct.unpack(data))
print "Packet interval: %.3f ms"%(1000*(time.time()-last))
last = time.time()
except(KeyboardInterrupt):
pass
s.close()
print "Thank you."
import socket
import time
import struct
from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
HOST = "192.168.1.52"
PORT = 30002
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])
header_struct = struct.Struct('>iB')
robot_mode_struct = struct.Struct('>'+'Q'+7*'?'+2*'B'+2*'d')
joint_data_struct = struct.Struct( '>'+6*(3*'d'+4*'f'+'B') )
tool_data_struct = struct.Struct('>'+2*'b'+2*'d'+'f'+'B'+2*'f'+'B')
masterboard_data_struct = struct.Struct('>'+2*'i'+2*'b'+2*'d'+2*'b'+2*'d'+4*'f'+2*'B'+'b'+'L')
cartesian_info_struct = struct.Struct('>'+12*'d')
config_data_struct = struct.Struct('>'+53*'d'+4*'i')
force_data_struct = struct.Struct('>'+7*'d')
subpackages = {
'0':(robot_mode_struct,38,"Robot Mode"),
'1':(joint_data_struct,251,"Joint Data"),
'2':(tool_data_struct,37,"Tool Data"),
'3':(masterboard_data_struct,72,"Masterboard Data"),
'4':(cartesian_info_struct,101,"Cartesian Info"),
'6':(config_data_struct,445,"Config Data"),
'7':(force_data_struct,61,"Force Data")
}
def handle_subpackage(s,remaining):
header = s.recv(5)
header = header_struct.unpack(header)
sublength = header[0]
subtype = header[1]
try:
subpackage = subpackages[str(subtype)]
assert(subpackage[1] == sublength ) #make sure lengths match
print "\t",subpackage[2]
data = s.recv(sublength-5)
#print '\t',subpackage[0].unpack(data)
except(KeyError):
#print "\tPackage type %d not known."%subtype
s.recv(sublength-5)
return remaining-sublength
def flush(s):
print "flushing"
header = s.recv(5)
header = header_struct.unpack(header)
s.recv(header[1])
last = time.time()
try:
while(True):
s.send("set_digital_out(2,True)\n") #only for testing response
header = s.recv(5)
header = header_struct.unpack(header)
if header[1]!=16: #if we're not at packet start, flush
flush(s)
else:
print "Packet interval: %.1f ms"%(1000*(time.time()-last))
last = time.time()
remaining = header[0]-5
while(remaining>0):
remaining = handle_subpackage(s,remaining)
assert(remaining>=0)
except(KeyboardInterrupt):
pass
s.close()
print "Thank you."
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment