Commit 39a2f4b8 authored by Sam Calisch's avatar Sam Calisch

testing primary vs. secondary client interface

parent 714ad4af
...@@ -35,20 +35,26 @@ class UR10(object): ...@@ -35,20 +35,26 @@ class UR10(object):
'255':'Idle'} '255':'Idle'}
safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd'] safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd']
def __init__(self,name,ip,port): def __init__(self,name,ip):
self.name = name self.name = name
self.ip = ip self.ip = ip
self.port = port
self.last_packet_time = 0 self.last_packet_time = 0
self.socket_opened = False self.socket_opened = False
def open_socket(self): def open_socket(self,port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.ip, self.port)) self.socket.connect((self.ip, port))
self.socket_opened = True self.socket_opened = True
print "Opened socket: ",self.ip, port
def open_primary_socket(self):
self.socket_primary = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket_primary.connect((self.ip, 30001))
print "Opened socket: ",self.ip, 30001
def close_socket(self): def close_socket(self):
self.socket.close() self.socket.close()
self.socket_opened = False self.socket_opened = False
print "Socket closed" def close_primary_socket(self):
self.socket_primary.close()
print "Primary socket closed"
def run_program(self,program_name): def run_program(self,program_name):
def recieve(): def recieve():
reply = '' reply = ''
...@@ -76,15 +82,11 @@ class UR10(object): ...@@ -76,15 +82,11 @@ class UR10(object):
self.socket.settimeout(None) self.socket.settimeout(None)
self.close_socket() self.close_socket()
def flush(self): def flush_secondary(self):
print "flushing" print "flushing"
if self.port == 30002: header = self.socket.recv(5)
header = self.socket.recv(5) header = UR10.header_struct.unpack(header)
header = UR10.header_struct.unpack(header) self.socket.recv(header[1])
self.socket.recv(header[1])
else:
print "Don't know how to flush port %d"%self.port
NotImplementedError
def send(self,command): def send(self,command):
if not self.socket_opened: if not self.socket_opened:
assert(0) #socket not open assert(0) #socket not open
...@@ -92,7 +94,7 @@ class UR10(object): ...@@ -92,7 +94,7 @@ class UR10(object):
self.socket.send(command) self.socket.send(command)
def update_state_secondary(self,packet,packet_type): 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]) #print "Updating %s with packet type %d: %s"%(self.name,packet_type,UR10.subpackages[str(packet_type)][2])
if packet_type == 4: if packet_type == 4:
self.tool_vector = asarray(packet[:6]) self.tool_vector = asarray(packet[:6])
...@@ -110,34 +112,66 @@ class UR10(object): ...@@ -110,34 +112,66 @@ class UR10(object):
#print "\tPackage type %d not known."%subtype #print "\tPackage type %d not known."%subtype
self.socket.recv(sublength-5) self.socket.recv(sublength-5)
return remaining-sublength return remaining-sublength
def read_packet(self): def read_packet_secondary(self):
if self.port == 30002: #Secondary client interface (100 ms packets)
#Secondary client interface (100 ms packets) header = self.socket.recv(5)
header = self.socket.recv(5) header = UR10.header_struct.unpack(header)
header = UR10.header_struct.unpack(header) if header[1]!=16: #if we're not at packet start, flush
if header[1]!=16: #if we're not at packet start, flush print header
self.flush() self.flush_secondary()
else: else:
print "%s packet interval: %.1f ms"%(self.name,1000*(time.time()-self.last_packet_time)) #print "%s packet interval: %.1f ms"%(self.name,1000*(time.time()-self.last_packet_time))
self.last_packet_time = time.time() self.last_packet_time = time.time()
remaining = header[0]-5 remaining = header[0]-5
while(remaining>0): while(remaining>0):
remaining = self.handle_subpackage_secondary(remaining) remaining = self.handle_subpackage_secondary(remaining)
assert(remaining>=0) assert(remaining>=0)
elif self.port == 30003: def read_packet_primary(self):
#Real time interface (8 ms packets) header = self.socket_primary.recv(5)
pass #header = UR10.header_struct.unpack(header)
else: print header
NotImplementedError #if header[1]!=16: #if we're not at packet start, flush
def send(self,cmd): # self.flush_secondary()
assert(self.socket_opened) #else:
self.socket.send(cmd) # #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)
def stream_script(self,fn):
'''
Stream a script to the robot -- scripts are expected to be written as a function
followed by a call of this function. After streaming the file, the
'''
f = open(fn,'r')
self.open_socket(30002)
self.open_primary_socket()
for i in range(10): self.read_packet_secondary() #establish starting position
for l in f.readlines():
self.send(l)
print l.strip('\n')
print "Done sending"
while True:
try:
self.read_packet_secondary()
self.read_packet_primary()
#stdout.write("[%.4f,%.4f,%.4f,%.4f,%.4f,%.4f]\r"%tuple(self.tool_vector) )
#stdout.flush()
except(KeyboardInterrupt):
print "stopping"
self.send("stop program\n")
break
self.close_socket()
self.close_primary_socket()
#not yet incorporated #not yet incorporated
def print_packet(packet): def print_packet(packet):
print "\033[2J" print "\033[2J"
print "\033[H" print "\033[H"
......
...@@ -3,27 +3,9 @@ from UR10 import * ...@@ -3,27 +3,9 @@ from UR10 import *
#Sam Calisch, 2015 #Sam Calisch, 2015
#Center for Bits and Atoms, MIT #Center for Bits and Atoms, MIT
roy = UR10("Roy","192.168.1.52",30002)
siegfried = UR10("Siegfried","192.168.1.51",30002) siegfried = UR10("Siegfried","192.168.1.51",30002)
#roy.open_socket()
#siegfried.open_socket()
siegfried.run_program('/programs/partner_test.urp') 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." print "Thank you."
from UR10 import *
from numpy import *
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
siegfried = UR10("Siegfried","192.168.1.51")
siegfried.open_socket(30002)
last = time.time()
for i in range(10): siegfried.read_packet_secondary() #establish starting position
print "[%f, %f, %f, %f, %f, %f]"%tuple(siegfried.tool_vector)
siegfried.close_socket()
print "Thank you."
...@@ -5,14 +5,15 @@ from numpy import * ...@@ -5,14 +5,15 @@ from numpy import *
#Center for Bits and Atoms, MIT #Center for Bits and Atoms, MIT
#roy = UR10("Roy","192.168.1.52",30002) #roy = UR10("Roy","192.168.1.52")
siegfried = UR10("Siegfried","192.168.1.51",30002) siegfried = UR10("Siegfried","192.168.1.51")
#roy.open_socket() #roy.open_socket()
siegfried.open_socket() siegfried.open_socket()
period = 10 period = 10
t = arange(0,period,.1) t = arange(0,period,.1)
z_traj = .01*sin(2*pi*t/period) z_traj = .1*sin(2*pi*t/period)
v_traj = 2*.1*2*pi/period*cos(2*pi*t/period)
last = time.time() last = time.time()
...@@ -25,16 +26,18 @@ try: ...@@ -25,16 +26,18 @@ try:
while(True): while(True):
#roy.read_packet() #roy.read_packet()
siegfried.read_packet() siegfried.read_packet()
#new_p = start+array([0,0,z_traj[i],0,0,0]) new_p = start+array([0,0,z_traj[i],0,0,0])
if i==20: new_p.append(.1)
new_p = start+array([0,0,.1,0,0,0]) #move_cmd = 'movep(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.%f,r=.025)\n'%(tuple(new_p)+(abs(v_traj[i]),))
move_cmd = 'movej(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.1)\n'%tuple(new_p) #move_cmd = 'movep(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.%f,r=.025)\n'%(tuple(new_p)+tuple([.1]))
print move_cmd #move_cmd = 'movep(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.05,r=.025)\n'%tuple(new_p)
siegfried.send(move_cmd) print move_cmd
#siegfried.send('set_digital_out(2,True)\n') siegfried.send(move_cmd)
#speed_cmd = 'speedl([%f, %f, %f, 0, 0, 0],a=.2,t_min=.05)\n'%(0,0,z_traj[i])
#print speed_cmd
#siegfried.send(speed_cmd)
print "[%.4f,%.4f,%.4f,%.4f,%.4f,%.4f]"%tuple(siegfried.tool_vector-start) print "[%.4f,%.4f,%.4f,%.4f,%.4f,%.4f]"%tuple(siegfried.tool_vector-start)
#i = (i+1)%shape(t)[0] i = (i+1)%shape(t)[0]
i = (i+1)#%shape(t)[0]
except(KeyboardInterrupt): except(KeyboardInterrupt):
pass pass
#roy.close_socket() #roy.close_socket()
......
...@@ -6,8 +6,8 @@ from sys import stdout ...@@ -6,8 +6,8 @@ from sys import stdout
#Sam Calisch, 2015 #Sam Calisch, 2015
#Center for Bits and Atoms, MIT #Center for Bits and Atoms, MIT
HOST = "192.168.1.52" HOST = "192.168.1.51"
PORT = 30002 PORT = 30001
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT)) s.connect((HOST, PORT))
...@@ -98,7 +98,7 @@ def handle_subpackage(s,remaining): ...@@ -98,7 +98,7 @@ def handle_subpackage(s,remaining):
data = s.recv(sublength-5) data = s.recv(sublength-5)
#print '\t',subpackage[0].unpack(data) #print '\t',subpackage[0].unpack(data)
except(KeyError): except(KeyError):
#print "\tPackage type %d not known."%subtype print "\tPackage type %d not known."%subtype
s.recv(sublength-5) s.recv(sublength-5)
return remaining-sublength return remaining-sublength
......
def stream_test_1():
$ 1 "Setup"
set_standard_analog_input_domain(0, 1)
set_standard_analog_input_domain(1, 1)
set_tool_analog_input_domain(0, 1)
set_tool_analog_input_domain(1, 1)
set_analog_outputdomain(0, 0)
set_analog_outputdomain(1, 0)
set_tool_voltage(12)
set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])
set_payload(0.0)
set_gravity([0.0, 0.0, 9.82])
$ 2 "First move"
movej(p[-0.689757, -0.087677, 0.232186, -1.209382, -1.209335, 1.209032],a=1.2, v=0.1)
$ 3 "Second move"
movej(p[-0.689757, -0.087677, 0.332186, -1.209382, -1.209335, 1.209032],a=1.2, v=0.1)
end
stream_test_1()
from UR10 import *
from numpy import *
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
siegfried = UR10("Siegfried","192.168.1.51")
siegfried.stream_script('stream_test_1.script')
\ No newline at end of file
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