Commit 956b9832 authored by Sam Calisch's avatar Sam Calisch

used primary client interface to do script streaming

parent 39a2f4b8
......@@ -9,6 +9,7 @@ from numpy import *
class UR10(object):
header_struct = struct.Struct('>iB')
robot_message_header_struct = struct.Struct('>qBB') #time stamps on program labels are -1? the uint64_t should be unsigned...
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"),
......@@ -45,16 +46,10 @@ class UR10(object):
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
def close_primary_socket(self):
self.socket_primary.close()
print "Primary socket closed"
print "Socket closed"
def run_program(self,program_name):
def recieve():
reply = ''
......@@ -83,10 +78,11 @@ class UR10(object):
self.close_socket()
def flush_secondary(self):
print "flushing"
#print "flushing"
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
self.socket.recv(header[1])
if len(header)==5:
header = UR10.header_struct.unpack(header)
self.socket.recv(header[1])
def send(self,command):
if not self.socket_opened:
assert(0) #socket not open
......@@ -98,7 +94,7 @@ class UR10(object):
if packet_type == 4:
self.tool_vector = asarray(packet[:6])
def handle_subpackage_secondary(self,remaining):
def handle_subpackage_robot_state(self,remaining):
header = self.socket.recv(5)
header = UR10.header_struct.unpack(header)
sublength = header[0]
......@@ -112,33 +108,53 @@ class UR10(object):
#print "\tPackage type %d not known."%subtype
self.socket.recv(sublength-5)
return remaining-sublength
def handle_subpackage_robot_message(self,remaining):
header = self.socket.recv(10); remaining -= 10
header = UR10.robot_message_header_struct.unpack(header)
data = self.socket.recv(remaining)
if header[2]==1:
#label message
message_struct = struct.Struct(">i"+(remaining-4)*"b")
data = message_struct.unpack(data)
data = [header[0],data[0],"".join(map(chr,data[1:]))]
print "$ %d %s"%(data[1],data[2])
elif header[2]==7:
#key message
message_struct = struct.Struct(">iiB"+(remaining-9)*"b")
data = message_struct.unpack(data)
data = [header[0],data[0],data[1],data[2],"".join(map(chr,data[3:3+data[2]])),"".join(map(chr,data[3+data[2]:]))]
print data[4]+": "+data[5]
elif header[2]==3:
#version message
#message_struct = struct.Struct(">bbbBBi"+(remaining-9)*"b")
message_struct = struct.Struct(remaining*"b") #who cares
data = message_struct.unpack(data)
data = "".join(map(chr,data[remaining-21:]))
print "Control software built: ",data
else:
print "Unknown robot message: ",header
return 0
def read_packet_secondary(self):
#Secondary client interface (100 ms packets)
#Primary/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
print header
self.flush_secondary()
else:
if header[1] == 16: #if we're not at packet start, flush
#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
while(remaining>0):
remaining = self.handle_subpackage_secondary(remaining)
remaining = self.handle_subpackage_robot_state(remaining)
assert(remaining>=0)
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)
elif header[1] == 20:
remaining = header[0]-5
while(remaining>0):
remaining = self.handle_subpackage_robot_message(remaining)
assert(remaining>=0)
else:
self.flush_secondary()
def stream_script(self,fn):
......@@ -147,8 +163,7 @@ class UR10(object):
followed by a call of this function. After streaming the file, the
'''
f = open(fn,'r')
self.open_socket(30002)
self.open_primary_socket()
self.open_socket(30001)
for i in range(10): self.read_packet_secondary() #establish starting position
for l in f.readlines():
self.send(l)
......@@ -157,15 +172,12 @@ class UR10(object):
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()
......
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)
......@@ -10,9 +9,10 @@ def stream_test_1():
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"
$ 3 "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"
$ 4 "Second move"
movej(p[-0.689757, -0.087677, 0.332186, -1.209382, -1.209335, 1.209032],a=1.2, v=0.1)
$ 5 "Done"
end
stream_test_1()
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