How to process package from robot controller

Technical Questions Robot Communication
4

You can use the Struct library in python to unpack the byte stream, using the information in this excel sheet to tell you which info can be found where.

You may want to consider using the RTDE instead of the other client interfaces, that way you can customise the stream to only include the information you want, so you don’t need to sift through the whole lot.

Here’s a python example getting some basic positional info from the client interface stream. You’d probably want to step through the packet contents in a bit more of an organised manner, but this should give a rough idea of how to process the data:

#!/usr/bin/env python
# encoding: utf=8

""" 
#UR Controller Client Interface Datastream Reader
# For software version 3.x
#
# Datastream info found here: https://s3-eu-west-1.amazonaws.com/ur-support-site/16496/Client_Interface.xlsx
# Struct library used to extract data, info found here: https://docs.python.org/2/library/struct.html
"""

import socket, struct

def main():

	#Establish connection to controller
	HOST = '127.0.0.1'
	PORT = 30002

	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
	s.connect((HOST, PORT))

	while 1:
		#Loop forever, receive 4096 bytes of data (enough to store any packet)
		data = s.recv(4096)
		#initialise i to keep track of position in packet
		i = 0
		if data:
			#Print title to screen
			print '*******'
			print 'UR Controller Primary Client Interface Reader' 
			print '*******'
			#extract packet length, timestamp and packet type from start of packet and print to screen
			packlen =  (struct.unpack('!i', data[0:4]))[0]
			timestamp = (struct.unpack('!Q', data[10:18]))[0]
			packtype = (struct.unpack('!b', data[4]))[0] 
			print 'packet length: ' + str(packlen)
			print 'timestamp: ' + str(timestamp)
			print 'packet type: ' + str(packtype)
			print '*******'

			if packtype == 16:
				#if packet type is Robot State, loop until reached end of packet
				while i+5 < packlen:

					#extract length and type of message and print if desired
					msglen = (struct.unpack('!i', data[5+i:9+i]))[0] 
					msgtype = (struct.unpack('!b', data[9+i]))[0] 
		
					#print 'packet length: ' + str(msglen)
					#print 'message type: ' + str(msgtype)
					#print '*******'

					if msgtype == 1:
						#if message is joint data, create a list to store angles
						angle = [0]*6
						j = 0
						while j < 6:
							#cycle through joints and extract only current joint angle (double precision)  then print to screen
							#bytes 10 to 18 contain the j0 angle, each joint's data is 41 bytes long (so we skip j*41 each time)
							angle[j] = (struct.unpack('!d', data[10+i+(j*41):18+i+(j*41)]))[0]
							print 'Joint ' + str(j) + ' angle : ' + str(angle[j])
							j = j + 1
							 
						print '*******'
	
					elif msgtype == 4:
						#if message type is cartesian data, extract doubles for 6DOF pos of TCP and print to sc    reen
						x =  (struct.unpack('!d', data[10+i:18+i]))[0]
						y =  (struct.unpack('!d', data[18+i:26+i]))[0]
						z =  (struct.unpack('!d', data[26+i:34+i]))[0]
						rx =  (struct.unpack('!d', data[34+i:42+i]))[0]
						ry =  (struct.unpack('!d', data[42+i:50+i]))[0]
						rz =  (struct.unpack('!d', data[50+i:58+i]))[0]

						print 'X:  ' + str(x)
						print 'Y:  ' + str(y)
						print 'Z:  ' + str(z)
						print 'RX: ' + str(rx)
						print 'RY: ' + str(ry)
						print 'RZ: ' + str(rz)
						print '*******\n'
					#increment i by the length of the message so move onto next message in packet
					i = msglen + i

if    __name__ == '__main__':
    import sys
    main()
1 Like