Commit e54f6ce1 authored by Sam Calisch's avatar Sam Calisch

figured out connectivity

parent be12923c
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 = 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()
f = open('trajectory-logging.txt','w')
f.write('hello')
def log_dev(packet):
#print "\033[2J"
#print "\033[H"
#print "Message Size:\t%d"%packet[0]
msg = (packet[1],) + tuple(packet[2:8]) + tuple(packet[8:14]) + tuple(packet[32:38]) + tuple(packet[38:44])
f.write("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n"%msg)
try:
while(True):
data = s.recv(1044)
log_dev(struct.unpack(data))
#print "Packet interval: %.3f ms"%(1000*(time.time()-last))
#last = time.time()
except(KeyboardInterrupt):
pass
s.close()
f.close()
print "Thank you."
import numpy as np
import cv2
from UR10 import *
import threading
roy = UR10("Roy","192.168.1.52")
#roy.open_socket(30002)
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
h,w,d = np.shape(frame)
def process_frame():
while(True):
ret, frame = cap.read()
# Our operations on the frame come here
#gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
tv = roy.tool_vector
if len(tv):
cv2.putText(frame,"%.4f, %.4f, %.4f"%tuple(tv[:3]),(0,h-40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0),2)
cv2.putText(frame,"%.4f, %.4f, %.4f"%tuple(tv)[3:],(0,h-10), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255),2)
cv2.imshow('frame',frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
#roy.read_packet_secondary()
t = threading.Thread(target=process_frame)
t.start()
roy.stream_script('ur_opencv_test.script')
cap.release()
cv2.destroyAllWindows()
\ No newline at end of file
import socket
import sys
import struct
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('192.168.1.50',30002))
s.listen(1)
print "socket listening"
conn,addr = s.accept()
print 'Connected with ' + addr[0] + ":" + str(addr[1])
#conn.sendall("turn")
while 1:
try:
#low = int(conn.recv(1))
data = ''
while(len(data)<2):
data += conn.recv(1)
data = struct.unpack('h',data)[0]
print data
except(KeyboardInterrupt):
break
s.close()
print "closed socket"
\ 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