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):
'255':'Idle'}
safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd']
def __init__(self,name,ip,port):
def __init__(self,name,ip):
self.name = name
self.ip = ip
self.port = port
self.last_packet_time = 0
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.connect((self.ip, self.port))
self.socket.connect((self.ip, port))
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):
self.socket.close()
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 recieve():
reply = ''
......@@ -76,15 +82,11 @@ class UR10(object):
self.socket.settimeout(None)
self.close_socket()
def flush(self):
def flush_secondary(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 send(self,command):
if not self.socket_opened:
assert(0) #socket not open
......@@ -92,7 +94,7 @@ class UR10(object):
self.socket.send(command)
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:
self.tool_vector = asarray(packet[:6])
......@@ -110,34 +112,66 @@ class UR10(object):
#print "\tPackage type %d not known."%subtype
self.socket.recv(sublength-5)
return remaining-sublength
def read_packet(self):
if self.port == 30002:
def read_packet_secondary(self):
#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()
print header
self.flush_secondary()
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()
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)
def read_packet_primary(self):
header = self.socket_primary.recv(5)
#header = UR10.header_struct.unpack(header)
print header
#if header[1]!=16: #if we're not at packet start, flush
# self.flush_secondary()
#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)
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
def print_packet(packet):
print "\033[2J"
print "\033[H"
......
......@@ -3,27 +3,9 @@ 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."
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 *
#Center for Bits and Atoms, MIT
#roy = UR10("Roy","192.168.1.52",30002)
siegfried = UR10("Siegfried","192.168.1.51",30002)
#roy = UR10("Roy","192.168.1.52")
siegfried = UR10("Siegfried","192.168.1.51")
#roy.open_socket()
siegfried.open_socket()
period = 10
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()
......@@ -25,16 +26,18 @@ try:
while(True):
#roy.read_packet()
siegfried.read_packet()
#new_p = start+array([0,0,z_traj[i],0,0,0])
if i==20:
new_p = start+array([0,0,.1,0,0,0])
move_cmd = 'movej(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.1)\n'%tuple(new_p)
new_p = start+array([0,0,z_traj[i],0,0,0])
new_p.append(.1)
#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 = 'movep(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.%f,r=.025)\n'%(tuple(new_p)+tuple([.1]))
#move_cmd = 'movep(p[%f, %f, %f, %f, %f, %f],a=1.2,v=.05,r=.025)\n'%tuple(new_p)
print move_cmd
siegfried.send(move_cmd)
#siegfried.send('set_digital_out(2,True)\n')
#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)
#i = (i+1)%shape(t)[0]
i = (i+1)#%shape(t)[0]
i = (i+1)%shape(t)[0]
except(KeyboardInterrupt):
pass
#roy.close_socket()
......
......@@ -6,8 +6,8 @@ from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
HOST = "192.168.1.52"
PORT = 30002
HOST = "192.168.1.51"
PORT = 30001
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
......@@ -98,7 +98,7 @@ def handle_subpackage(s,remaining):
data = s.recv(sublength-5)
#print '\t',subpackage[0].unpack(data)
except(KeyError):
#print "\tPackage type %d not known."%subtype
print "\tPackage type %d not known."%subtype
s.recv(sublength-5)
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