257 lines
8.3 KiB
Python
257 lines
8.3 KiB
Python
'''
|
|
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"
|
|
|
|
|
|
import logging
|
|
import socket
|
|
import struct
|
|
import time
|
|
import threading
|
|
from copy import deepcopy
|
|
|
|
import numpy as np
|
|
|
|
import math3d as m3d
|
|
|
|
|
|
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):
|
|
threading.Thread.__init__(self)
|
|
self.logger = logging.getLogger(self.__class__.__name__)
|
|
self.daemon = True
|
|
self._stop_event = True
|
|
self._dataEvent = threading.Condition()
|
|
self._dataAccess = threading.Lock()
|
|
self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
|
self._urHost = urHost
|
|
# Package data variables
|
|
self._timestamp = None
|
|
self._ctrlTimestamp = None
|
|
self._qActual = None
|
|
self._qTarget = None
|
|
self._tcp = None
|
|
self._tcp_force = None
|
|
self.__recvTime = 0
|
|
self._last_ctrl_ts = 0
|
|
#self._last_ts = 0
|
|
self._buffering = False
|
|
self._buffer_lock = threading.Lock()
|
|
self._buffer = []
|
|
self._csys = None
|
|
self._csys_lock = threading.Lock()
|
|
|
|
def set_csys(self, csys):
|
|
with self._csys_lock:
|
|
self._csys = csys
|
|
|
|
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 = b''
|
|
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_pose(self, wait=False, timestamp=False, ctrlTimestamp=False):
|
|
""" Return the tool pose values."""
|
|
if wait:
|
|
self.wait()
|
|
with self._dataAccess:
|
|
tcf = self._tcp
|
|
if ctrlTimestamp or timestamp:
|
|
ret = [tcf]
|
|
if timestamp:
|
|
ret.insert(-1, self._timestamp)
|
|
if ctrlTimestamp:
|
|
ret.insert(-1, self._ctrlTimestamp)
|
|
return ret
|
|
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]
|
|
self.logger.debug(
|
|
'Received header telling that package is %s 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:
|
|
self.logger.warning(
|
|
'Error, Received packet of length smaller than 540: %s ',
|
|
pkgsize)
|
|
return
|
|
|
|
with self._dataAccess:
|
|
self._timestamp = timestamp
|
|
# it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller??
|
|
# if (self._timestamp - self._last_ts) > 0.010:
|
|
#self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts))
|
|
#self._last_ts = self._timestamp
|
|
self._ctrlTimestamp = np.array(unp[0])
|
|
if self._last_ctrl_ts != 0 and (
|
|
self._ctrlTimestamp -
|
|
self._last_ctrl_ts) > 0.010:
|
|
self.logger.warning(
|
|
"Error the controller failed to send us a packet: time since last packet %s s ",
|
|
self._ctrlTimestamp - self._last_ctrl_ts)
|
|
self._last_ctrl_ts = self._ctrlTimestamp
|
|
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])
|
|
|
|
if self._csys:
|
|
with self._csys_lock:
|
|
# might be a godd idea to remove dependancy on m3d
|
|
tcp = self._csys * m3d.Transform(self._tcp)
|
|
self._tcp = tcp.pose_vector
|
|
if self._buffering:
|
|
with self._buffer_lock:
|
|
self._buffer.append(
|
|
(self._timestamp,
|
|
self._ctrlTimestamp,
|
|
self._tcp,
|
|
self._qActual)) # FIXME use named arrays of allow to configure what data to buffer
|
|
|
|
with self._dataEvent:
|
|
self._dataEvent.notifyAll()
|
|
|
|
def start_buffering(self):
|
|
"""
|
|
start buffering all data from controller
|
|
"""
|
|
self._buffer = []
|
|
self._buffering = True
|
|
|
|
def stop_buffering(self):
|
|
self._buffering = False
|
|
|
|
def try_pop_buffer(self):
|
|
"""
|
|
return oldest value in buffer
|
|
"""
|
|
with self._buffer_lock:
|
|
if len(self._buffer) > 0:
|
|
return self._buffer.pop(0)
|
|
else:
|
|
return None
|
|
|
|
def pop_buffer(self):
|
|
"""
|
|
return oldest value in buffer
|
|
"""
|
|
while True:
|
|
with self._buffer_lock:
|
|
if len(self._buffer) > 0:
|
|
return self._buffer.pop(0)
|
|
time.sleep(0.001)
|
|
|
|
def get_buffer(self):
|
|
"""
|
|
return a copy of the entire buffer
|
|
"""
|
|
with self._buffer_lock:
|
|
return deepcopy(self._buffer)
|
|
|
|
def get_all_data(self, wait=True):
|
|
"""
|
|
return all data parsed from robot as a dict
|
|
"""
|
|
if wait:
|
|
self.wait()
|
|
with self._dataAccess:
|
|
return dict(
|
|
timestamp=self._timestamp,
|
|
ctrltimestamp=self._ctrlTimestamp,
|
|
qActual=self._qActual,
|
|
qTarget=self._qTarget,
|
|
tcp=self._tcp,
|
|
tcp_force=self._tcp_force)
|
|
|
|
def stop(self):
|
|
#print(self.__class__.__name__+': Stopping')
|
|
self._stop_event = True
|
|
|
|
def close(self):
|
|
self.stop()
|
|
self.join()
|
|
|
|
def run(self):
|
|
self._stop_event = False
|
|
self._rtSock.connect((self._urHost, 30003))
|
|
while not self._stop_event:
|
|
self.__recv_rt_data()
|
|
self._rtSock.close()
|
|
|
|
|