add file to monitor realtime port of ur
This commit is contained in:
Olivier R-D
parent
e51a9e044c
commit
c33f38c4b4
219
urx/urrtmon.py
Normal file
219
urx/urrtmon.py
Normal file
@ -0,0 +1,219 @@
|
||||
'''
|
||||
Module for implementing a UR controller real-time monitor over socket port 30003.
|
||||
Confer http://support.universal-robots.com/Technical/RealTimeClientInterface
|
||||
Note: The packet lenght given in the web-page is 740. What is actually received from the controller is 692. It is assumed that the motor currents, the last group of 48 bytes, are not send.
|
||||
Originally Written by Morten Lind
|
||||
'''
|
||||
|
||||
__author__ = "Morten Lind, Olivier Roulet-Dubonnet"
|
||||
__copyright__ = "Copyright 2011, NTNU/SINTEF Raufoss Manufacturing AS"
|
||||
__credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"]
|
||||
__license__ = "GPLv3"
|
||||
__version__ = "0.1"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
|
||||
import os
|
||||
import socket
|
||||
import struct
|
||||
import time
|
||||
import threading
|
||||
import datetime
|
||||
|
||||
import numpy as np
|
||||
|
||||
class URRTMonitor(threading.Thread):
|
||||
|
||||
## Struct for revision of the UR controller giving 692 bytes
|
||||
rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ')
|
||||
|
||||
## for revision of the UR controller giving 540 byte. Here TCP
|
||||
## pose is not included!
|
||||
rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d')
|
||||
|
||||
def __init__(self, urHost, debug=False):
|
||||
threading.Thread.__init__(self)
|
||||
self.daemon = True
|
||||
self._stop = True
|
||||
self._debug = debug
|
||||
self._dataEvent = threading.Condition()
|
||||
self._dataAccess = threading.Lock()
|
||||
self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self._urHost = urHost
|
||||
## Package data variables
|
||||
self._timestamp = None
|
||||
self._qActual = None
|
||||
self._qTarget = None
|
||||
self._tcp = None
|
||||
self._tcp_force = None
|
||||
# self._fwkin = pymoco.kinematics.FrameComputer(
|
||||
# robotDef=pymoco.robot.getRobotType('UR6855A'))
|
||||
|
||||
def __recv_bytes(self, nBytes):
|
||||
''' Facility method for receiving exactly "nBytes" bytes from
|
||||
the robot connector socket.'''
|
||||
## Record the time of arrival of the first of the stream block
|
||||
recvTime = 0
|
||||
pkg = ''
|
||||
while len(pkg) < nBytes:
|
||||
pkg += self._rtSock.recv(nBytes - len(pkg))
|
||||
if recvTime == 0:
|
||||
recvTime = time.time()
|
||||
self.__recvTime = recvTime
|
||||
return pkg
|
||||
|
||||
def wait(self):
|
||||
with self._dataEvent:
|
||||
self._dataEvent.wait()
|
||||
|
||||
def q_actual(self, wait=False, timestamp=False):
|
||||
""" Get the actual joint position vector."""
|
||||
if wait:
|
||||
self.wait()
|
||||
with self._dataAccess:
|
||||
if timestamp:
|
||||
return self._timestamp, self._qActual
|
||||
else:
|
||||
return self._qActual
|
||||
getActual = q_actual
|
||||
|
||||
def q_target(self, wait=False, timestamp=False):
|
||||
""" Get the target joint position vector."""
|
||||
if wait:
|
||||
self.wait()
|
||||
with self._dataAccess:
|
||||
if timestamp:
|
||||
return self._timestamp, self._qTarget
|
||||
else:
|
||||
return self._qTarget
|
||||
getTarget = q_target
|
||||
|
||||
def tcf_vec(self, wait=False, timestamp=False):
|
||||
""" Compute the tool pose *6D-vector* based on the actual joint
|
||||
values."""
|
||||
tcfvec = np.zeros(6)
|
||||
tcf = self.tcf_pose(wait=wait, timestamp=False)
|
||||
tcfvec[:3] = tcf.pos.data
|
||||
tcfvec[3:] = tcf.orient.toRotationVector().data
|
||||
if timestamp:
|
||||
return self._timestamp, tcfvec
|
||||
else:
|
||||
return tcfvec
|
||||
getTCFVec = tcf_vec
|
||||
|
||||
def tcf_pose(self, wait=False, timestamp=False):
|
||||
""" Compute the tool pose *Transform* based on the actual joint
|
||||
values."""
|
||||
if wait:
|
||||
self.wait()
|
||||
with self._dataAccess:
|
||||
# tcf = self._fwkin(self._qActual)
|
||||
tcf = self._tcp
|
||||
if timestamp:
|
||||
return self._timestamp, tcf
|
||||
else:
|
||||
return tcf
|
||||
getTCF = tcf_pose
|
||||
|
||||
def tcf_force(self, wait=False, timestamp=False):
|
||||
""" Get the tool force. The returned tool force is a
|
||||
six-vector of three forces and three moments."""
|
||||
if wait:
|
||||
self.wait()
|
||||
with self._dataAccess:
|
||||
# tcf = self._fwkin(self._qActual)
|
||||
tcf_force = self._tcp_force
|
||||
if timestamp:
|
||||
return self._timestamp, tcf_force
|
||||
else:
|
||||
return tcf_force
|
||||
getTCFForce = tcf_force
|
||||
|
||||
def __recv_rt_data(self):
|
||||
head = self.__recv_bytes(4)
|
||||
## Record the timestamp for this logical package
|
||||
timestamp = self.__recvTime
|
||||
pkgsize = struct.unpack('>i', head)[0]
|
||||
if self._debug:
|
||||
print('Received header telling that package is %d bytes long'%pkgsize)
|
||||
payload = self.__recv_bytes(pkgsize-4)
|
||||
if pkgsize >= 692:
|
||||
unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size])
|
||||
elif pkgsize >= 540:
|
||||
unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size])
|
||||
else:
|
||||
print('Error, Received packet of length smaller than 540: ', pkgsize)
|
||||
return
|
||||
|
||||
with self._dataAccess:
|
||||
self._timestamp = timestamp
|
||||
self._qActual = np.array(unp[31:37])
|
||||
self._qTarget = np.array(unp[1:7])
|
||||
self._tcp_force = np.array(unp[67:73])
|
||||
self._tcp = np.array(unp[73:79])
|
||||
with self._dataEvent:
|
||||
self._dataEvent.notifyAll()
|
||||
|
||||
def stop(self):
|
||||
#print(self.__class__.__name__+': Stopping')
|
||||
self._stop = True
|
||||
|
||||
def cleanup(self):
|
||||
self.stop()
|
||||
self.join()
|
||||
|
||||
def run(self):
|
||||
self._stop = False
|
||||
self._rtSock.connect((self._urHost, 30003))
|
||||
while not self._stop:
|
||||
self.__recv_rt_data()
|
||||
self._rtSock.close()
|
||||
|
||||
def logDeviation(logDT=0.1):
|
||||
np.set_printoptions(precision=4)
|
||||
while True:
|
||||
print urRTMon.getTarget()-urRTMon.getActual()
|
||||
time.sleep(logDT)
|
||||
|
||||
i = 1
|
||||
def dumptcp():
|
||||
global i
|
||||
tcf = urRTMon.getTcp()
|
||||
pos = tcf[:3]
|
||||
rot = tcf[3:]
|
||||
filename = 'robot%d.txt'%i
|
||||
print('Dumping file: %s (at time %s)' % (filename, str(datetime.datetime.now())))
|
||||
f = file(filename,'w')
|
||||
f.write((3*'%.5f ' + '\n' + 3*'%.5f ') % tuple(tcf))
|
||||
i += 1
|
||||
|
||||
|
||||
def startupInteractive():
|
||||
global urRTMon
|
||||
from optparse import OptionParser
|
||||
## Require the urhost arg.
|
||||
parser = OptionParser()
|
||||
parser.add_option('--debug', action='store_true', default=False, dest='debug')
|
||||
parser.add_option('--start', action='store_true', default=False, dest='start')
|
||||
opts, args = parser.parse_args()
|
||||
if len(args) != 1:
|
||||
raise Exception('Must have argument with ur-host name or ip!')
|
||||
urHost = args[0]
|
||||
print('Connecting to UR Secondary socket inteface on "%s"'%urHost)
|
||||
# # Start the connectors
|
||||
urRTMon = URRTMonitor(urHost, debug=opts.debug)
|
||||
if opts.start:
|
||||
urRTMon.start()
|
||||
# # Register for hard shutdown
|
||||
import atexit
|
||||
atexit.register(urRTMon.stop)
|
||||
## Drop to interactive
|
||||
pystartfile = os.path.expanduser('~/.pythonstartup')
|
||||
if os.path.isfile(pystartfile):
|
||||
execfile(pystartfile)
|
||||
import code
|
||||
code.interact(None, None, globals())
|
||||
|
||||
if __name__ == '__main__':
|
||||
startupInteractive()
|
||||
Loading…
x
Reference in New Issue
Block a user