Commit 1f40150c authored by Sam Calisch's avatar Sam Calisch

scripting point measuring and following... fun

parent 956b9832
......@@ -41,6 +41,7 @@ class UR10(object):
self.ip = ip
self.last_packet_time = 0
self.socket_opened = False
self.tool_vector = 0
def open_socket(self,port):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.ip, port))
......@@ -76,6 +77,8 @@ class UR10(object):
#for line in reply: print line
self.socket.settimeout(None)
self.close_socket()
def initialize_state(self,n=10):
for i in range(n): self.read_packet_secondary() #establish starting position
def flush_secondary(self):
#print "flushing"
......@@ -132,8 +135,10 @@ class UR10(object):
data = message_struct.unpack(data)
data = "".join(map(chr,data[remaining-21:]))
print "Control software built: ",data
elif header[2]==10:
print "Runtime exception: ",data
else:
print "Unknown robot message: ",header
print "Unknown robot message:",header
return 0
def read_packet_secondary(self):
......@@ -155,19 +160,18 @@ class UR10(object):
else:
self.flush_secondary()
#TODO: implement global variables setup message!
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
Stream a script to the robot -- scripts are expected to be written as a function.
'''
f = open(fn,'r')
self.open_socket(30001)
for i in range(10): self.read_packet_secondary() #establish starting position
self.initialize_state()
for l in f.readlines():
self.send(l)
print l.strip('\n')
#print l.strip('\n')
print "Done sending"
while True:
try:
......@@ -177,6 +181,8 @@ class UR10(object):
except(KeyboardInterrupt):
self.send("stop program\n")
break
except:
break
self.close_socket()
......
......@@ -9,10 +9,151 @@ 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])
$ 3 "First move"
movej(p[-0.689757, -0.087677, 0.232186, -1.209382, -1.209335, 1.209032],a=1.2, v=0.1)
$ 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"
socket_close("stream")
socket_open("127.0.0.1",63351,"stream")
def rq_print_serial_number():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET SNU","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_print_firmware_version():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET FWV","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_print_prod_year():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET PYE","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_set_zero():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("SET ZRO","acc")
sleep(0.1)
socket_close("acc")
end
end
def rq_get_version():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET VER","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
global alpha=.95 #smoothing coefficient. zero -> no smoothing
global Fx=0.0
global Fy=0.0
global Fz=0.0
global Mx=0.0
global My=0.0
global Mz=0.0
global Fz_max=2
global z_increment=0.0005
global safe_z=.02
global var_x=.142
global var_y=.185
global var_d=.015
global var_xx=.525
global var_yy=.275
global var_r = .01
global position_increm=p[-0.501350, -0.475087, 0.193433, -1.209338, -1.209378, 1.209117]
def push_z_axis():
thread Thread_while_68():
while (True):
global position_increm=get_forward_kin()
position_increm[2] = position_increm[2]-z_increment
movel(position_increm,1.2,0.01,0,0.0001)
end
end
if (norm(Fx)<Fz_max):
global thread_handler_68=run Thread_while_68()
while (norm(Fx)<Fz_max):
sync()
end
kill thread_handler_68
end
end
thread Thread_1():
while (True):
while(1):
rq_vals = socket_read_ascii_float(6,"stream")
if(rq_vals[0] != 0):
Fx = (1-alpha)*rq_vals[1]+alpha*Fx
Fy = (1-alpha)*rq_vals[2]+alpha*Fy
Fz = (1-alpha)*rq_vals[3]+alpha*Fz
Mx = (1-alpha)*rq_vals[4]+alpha*Mx
My = (1-alpha)*rq_vals[5]+alpha*My
Mz = (1-alpha)*rq_vals[6]+alpha*Mz
end
end
end
end
def set_z(p):
up = pose_add(get_forward_kin(),p[0,0,.01,0,0,0])
movel(up,a=.5,v=.05,t=0,r=.0001)
movel(p,a=.5, v=.3,t=0,r=.0001)
sleep(.25)
push_z_axis()
sleep(.1)
movel(p,a=.5,v=.05,t=0,r=.0001)
return pose_add(position_increm,p[0,0,.0015,0,0,0])
end
def process_loop(approach,pts):
movel(approach,a=.5,v=.2,r=.01)
movel(pts[0],a=.5,v=.03,r=0)
stopl(.25)
sleep(.1)
movep(pts[0],a=.5,v=.03,r=0)
i=0
while i<get_list_length(pts):
movep(pts[i],a=.5,v=.03,r=var_r)
i = i+1
end
movep(pts[0],a=.5,v=.03,r=0)
movel(approach,a=.5,v=.05,r=0)
end
threadId_Thread_1 = run Thread_1()
rq_set_zero() #zero load cell
$ 3 "measuring"
app_1 = p[-0.501350, -0.475087, 0.193433, -1.209338, -1.209378, 1.209117]
app_1 = pose_add(app_1,p[-.02,.02,0,0,0,0])
global c1 = set_z(app_1)
global c2=set_z(pose_add(app_1,p[0,var_xx,0,0,0,0]))
global c3=set_z(pose_add(app_1,p[-var_yy,var_xx,0,0,0,0]))
global c4=set_z(pose_add(app_1,p[-var_yy,0,0,0,0,0]))
global app_2=pose_add(app_1,p[-var_yy/2+var_y/2,var_xx/2-var_d/2,0,0,0,0])
global d1=set_z(app_2)
global d2=set_z(pose_add(app_1,p[-var_yy/2+var_y/2,var_xx/2+var_d/2,0,0,0,0]))
global d3=set_z(pose_add(app_1,p[-var_yy/2,var_xx/2+var_x/2,0,0,0,0]))
global d4=set_z(pose_add(app_1,p[-var_yy/2-var_y/2,var_xx/2+var_d/2,0,0,0,0]))
global d5=set_z(pose_add(app_1,p[-var_yy/2-var_y/2,var_xx/2-var_d/2,0,0,0,0]))
global d6=set_z(pose_add(app_1,p[-var_yy/2,var_xx/2-var_x/2,0,0,0,0]))
$ 4 "tracing"
process_loop(app_1,[c1,c2,c3,c4])
process_loop(app_2,[d1,d2,d3,d4,d5,d6])
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