Commit 00af2344 authored by Sam Calisch's avatar Sam Calisch

started a repo for host pc code with universal arms

parents
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
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)
siegfried = UR10("Siegfried","192.168.1.51",30002)
roy.open_socket()
siegfried.open_socket()
last = time.time()
try:
while(True):
roy.read_packet()
siegfried.read_packet()
#s1.send("set_digital_out(2,True)\n") #only for testing response
except(KeyboardInterrupt):
pass
roy.close_socket()
siegfried.close_socket()
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.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."
import socket
import argparse
def main(args):
f = file(args.filename,'r')
values = [l.strip('\n') for l in f.readlines()]
#print values
f.close()
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((args.ip, args.port))
if __name__=="__main__":
parser = argparse.ArgumentParser(description='Serve a set of values from a csv file')
parser.add_argument('-f','--filename', default="values.csv", help="filename for csv file of values")
parser.add_argument('-ip','--ip',default="192.168.1.51",help='ip address of robot') parser.add_argument('-p','--port',default="30002",help='port for tcpip socket')
args = parser.parse_args()
main(args)
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