adeck/python-urx-getl-rt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

autopep8 --max-line-length 900 -a -a -i robot.py

Browse Source
This commit is contained in:
Olivier R-D 2015-04-10 11:17:43 +02:00
parent 22f0d5d79b
commit 747f07287d
4 changed files with 159 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
urx/__init__.py
View File

@ -4,6 +4,4 @@ Python library to control an UR robot through its TCP/IP interface
__version__ = "0.9.0"
from urx.robot import Robot, RobotException, URRobot

53
urx/robot.py
View File

@ -28,6 +28,7 @@ class RobotException(Exception):
class URRobot(object):
"""
Python interface to socket interface of UR robot.
programs are send to port 3002
@ -36,24 +37,24 @@ class URRobot(object):
The RT interfaces is only used for the get_force related methods
Rmq: A program sent to the robot i executed immendiatly and any running program is stopped
"""
def __init__(self, host, useRTInterface=False):
self.logger = logging.getLogger(self.__class__.__name__)
self.host = host
self.csys = None
self.logger.info("Opening secondary monitor socket")
self.secmon = ursecmon.SecondaryMonitor(self.host) #data from robot at 10Hz
self.secmon = ursecmon.SecondaryMonitor(self.host) # data from robot at 10Hz
self.rtmon = None
if useRTInterface:
self.rtmon = self.get_realtime_monitor()
#the next 3 values must be conservative! otherwise we may wait forever
self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion
# the next 3 values must be conservative! otherwise we may wait forever
self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion
# It seems URScript is limited in the character length of floats it accepts
self.max_float_length = 6 # FIXME: check max length!!!
self.secmon.wait() # make sure we get data from robot before letting clients access our methods
self.max_float_length = 6 # FIXME: check max length!!!
self.secmon.wait() # make sure we get data from robot before letting clients access our methods
def __repr__(self):
return "Robot Object (IP=%s, state=%s)" % (self.host, self.secmon.get_all_data()["RobotModeData"])
@ -203,9 +204,9 @@ class URRobot(object):
jts = self.secmon.get_joint_data(wait=True)
finished = True
for i in range(0, 6):
#Rmq: q_target is an interpolated target we have no control over
if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon:
self.logger.debug("Waiting for end move, q_actual is %s, q_target is %s, diff is %s, epsilon is %s", jts["q_actual%s"%i], jts["q_target%s"%i], jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon)
# Rmq: q_target is an interpolated target we have no control over
if abs(jts["q_actual%s" % i] - jts["q_target%s" % i]) > self.joinEpsilon:
self.logger.debug("Waiting for end move, q_actual is %s, q_target is %s, diff is %s, epsilon is %s", jts["q_actual%s" % i], jts["q_target%s" % i], jts["q_actual%s" % i] - jts["q_target%s" % i], self.joinEpsilon)
finished = False
break
if finished and not self.secmon.is_program_running():
@ -337,7 +338,7 @@ class URRobot(object):
for idx, pose in enumerate(pose_list):
pose.append(acc)
pose.append(vel)
if idx != (len(pose_list) -1):
if idx != (len(pose_list) - 1):
pose.append(radius)
else:
pose.append(0)
@ -367,7 +368,7 @@ class URRobot(object):
self.secmon.cleanup()
if self.rtmon:
self.rtmon.stop()
shutdown = cleanup #this might be wrong since we could also shutdown the robot hardware from this script
shutdown = cleanup # this might be wrong since we could also shutdown the robot hardware from this script
def set_freedrive(self, val):
"""
@ -392,33 +393,33 @@ class URRobot(object):
"""
if not self.rtmon:
self.logger.info("Opening real-time monitor socket")
self.rtmon = urrtmon.URRTMonitor(self.host)# som information is only available on rt interface
self.rtmon = urrtmon.URRTMonitor(self.host) # som information is only available on rt interface
self.rtmon.start()
self.rtmon.set_csys(self.csys)
return self.rtmon
class Robot(URRobot):
"""
Generic Python interface to an industrial robot.
Compared to the URRobot class, this class adds the possibilty to work directly with matrices
and includes support for calibrating the robot coordinate system
"""
def __init__(self, host, useRTInterface=False):
URRobot.__init__(self, host, useRTInterface)
self.default_linear_acceleration = 0.01
self.default_linear_velocity = 0.01
self.csys_dict = {}
self.csys = None
self.set_csys("Robot", m3d.Transform()) #identity
self.set_csys("Robot", m3d.Transform()) # identity
def set_tcp(self, tcp):
"""
set robot flange to tool tip transformation
"""
if type(tcp) == m3d.Transform:
if isinstance(tcp, m3d.Transform):
tcp = tcp.pose_vector
URRobot.set_tcp(self, tcp)
@ -442,7 +443,7 @@ class Robot(URRobot):
set tool orientation using a orientation matric from math3d
or a orientation vector
"""
if type(orient) is not m3d.Orientation:
if not isinstance(orient, m3d.Orientation):
orient = m3d.Orientation(orient)
trans = self.get_pose()
trans.orient = orient
@ -453,7 +454,7 @@ class Robot(URRobot):
move tool in base coordinate, keeping orientation
"""
t = m3d.Transform()
if not type(vect) is m3d.Vector:
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
t.pos += m3d.Vector(vect)
return self.add_pose_base(t, acc, vel, radius, wait=wait)
@ -463,7 +464,7 @@ class Robot(URRobot):
move tool in tool coordinate, keeping orientation
"""
t = m3d.Transform()
if not type(vect) is m3d.Vector:
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
t.pos += vect
return self.add_pose_tool(t, acc, vel, radius, wait=wait)
@ -472,7 +473,7 @@ class Robot(URRobot):
"""
set tool to given pos, keeping constant orientation
"""
if not type(vect) is m3d.Vector:
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.set_pose(trans, acc, vel, radius, wait=wait)
@ -494,10 +495,9 @@ class Robot(URRobot):
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
else:
pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
if pose != None: #movel does not return anything when wait is False
if pose is not None: # movel does not return anything when wait is False
return self.csys.inverse * m3d.Transform(pose)
def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
"""
Add transform expressed in base coordinate
@ -606,7 +606,7 @@ class Robot(URRobot):
return t.pose_vector.tolist()
def set_gravity(self, vector):
if type(vector) == m3d.Vector:
if isinstance(vector, m3d.Vector):
vector = vector.list
return URRobot.set_gravity(self, vector)
@ -631,12 +631,5 @@ class Robot(URRobot):
return
if not MATH3D:
Robot = URRobot

77
urx/urrtmon.py
View File

@ -11,7 +11,6 @@ __credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"]
__license__ = "GPLv3"
import logging
import socket
import struct
@ -23,19 +22,20 @@ import numpy as np
import math3d as m3d
class URRTMonitor(threading.Thread):
## Struct for revision of the UR controller giving 692 bytes
# 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!
# for revision of the UR controller giving 540 byte. Here TCP
# pose is not included!
rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d')
def __init__(self, urHost, loglevel=logging.WARNING):
threading.Thread.__init__(self)
self.logger = logging.getLogger(self.__class__.__name__)
if len(logging.root.handlers) == 0: #dirty hack
if len(logging.root.handlers) == 0: # dirty hack
logging.basicConfig()
self.logger.setLevel(loglevel)
self.daemon = True
@ -45,7 +45,7 @@ class URRTMonitor(threading.Thread):
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
# Package data variables
self._timestamp = None
self._ctrlTimestamp = None
self._qActual = None
@ -68,13 +68,13 @@ class URRTMonitor(threading.Thread):
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
# 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()
recvTime = time.time()
self.__recvTime = recvTime
return pkg
@ -137,28 +137,37 @@ class URRTMonitor(threading.Thread):
def __recv_rt_data(self):
head = self.__recv_bytes(4)
## Record the timestamp for this logical package
# 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 %d bytes long'%pkgsize)
payload = self.__recv_bytes(pkgsize-4)
self.logger.debug(
'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:
self.logger.warning('Error, Received packet of length smaller than 540: ', pkgsize)
self.logger.warning(
'Error, Received packet of length smaller than 540: ',
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))
# 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 ".format( self._ctrlTimestamp - self._last_ctrl_ts))
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 ".format(
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])
@ -167,11 +176,16 @@ class URRTMonitor(threading.Thread):
if self._csys:
with self._csys_lock:
tcp = self._csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d
# 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
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()
@ -206,7 +220,6 @@ class URRTMonitor(threading.Thread):
return self._buffer.pop(0)
time.sleep(0.001)
def get_buffer(self):
"""
return a copy of the entire buffer
@ -221,7 +234,13 @@ class URRTMonitor(threading.Thread):
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)
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')
@ -239,20 +258,26 @@ class URRTMonitor(threading.Thread):
self._rtSock.close()
def startupInteractive():
from optparse import OptionParser
from IPython import embed
## Require the urhost arg.
# 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')
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 real-time socket inteface on "%s"'%urHost)
print('Connecting to UR real-time socket inteface on "%s"' % urHost)
# # Start the connectors
urRTMon = URRTMonitor(urHost, debug=opts.debug)
# # Register for hard shutdown

92
urx/ursecmon.py
View File

@ -20,19 +20,20 @@ from copy import copy
import time
class ParsingException(Exception):
def __init__(self, *args):
Exception.__init__(self, *args)
class TimeoutException(Exception):
def __init__(self, *args):
Exception.__init__(self, *args)
class ParserUtils(object):
def __init__(self):
self.logger = logging.getLogger(__name__)
self.is_v30 = False
@ -42,13 +43,13 @@ class ParserUtils(object):
parse a packet from the UR socket and return a dictionary with the data
"""
allData = {}
#print "Total size ", len(data)
# print "Total size ", len(data)
while data:
psize, ptype, pdata, data = self.analyze_header(data)
#print "We got packet with size %i and type %s" % (psize, ptype)
# print "We got packet with size %i and type %s" % (psize, ptype)
if ptype == 16:
allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type"))
data = (pdata + data)[5:] # This is the total size so we resend data to parser
data = (pdata + data)[5:] # This is the total size so we resend data to parser
elif ptype == 0:
# this parses RobotModeData for versions >=3.0 (i.e. 3.0)
if psize == 38:
@ -68,14 +69,14 @@ class ParserUtils(object):
elif ptype == 5:
allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type"))
elif ptype == 3:
allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb", ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent"))#, "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" ))
allData["MasterBoardData"] = self._get_data(pdata, "iBhhbbddbbddffffBBb", ("size", "type", "digitalInputBits", "digitalOutputBits", "analogInputRange0", "analogInputRange1", "analogInput0", "analogInput1", "analogInputDomain0", "analogInputDomain1", "analogOutput0", "analogOutput1", "masterBoardTemperature", "robotVoltage48V", "robotCurrent", "masterIOCurrent")) # , "masterSafetyState" ,"masterOnOffState", "euromap67InterfaceInstalled" ))
elif ptype == 2:
allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode"))
#elif ptype == 8:
#allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
#elif ptype == 7:
#allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
# elif ptype == 8:
#allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
# elif ptype == 7:
#allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
elif ptype == 20:
tmp = self._get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType"))
@ -110,7 +111,7 @@ class ParserUtils(object):
names args are strings used to store values
"""
tmpdata = copy(data)
fmt = fmt.strip() # space may confuse us
fmt = fmt.strip() # space may confuse us
d = dict()
i = 0
j = 0
@ -118,28 +119,28 @@ class ParserUtils(object):
f = fmt[j]
if f in (" ", "!", ">", "<"):
j += 1
elif f == "A": #we got an array
elif f == "A": # we got an array
# first we need to find its size
if j == len(fmt) - 2: # we are last element, size is the rest of data in packet
if j == len(fmt) - 2: # we are last element, size is the rest of data in packet
arraysize = len(tmpdata)
else: # size should be given in last element
asn = names[i-1]
else: # size should be given in last element
asn = names[i - 1]
if not asn.endswith("Size"):
raise ParsingException("Error, array without size ! %s %s" % (asn, i))
else:
arraysize = d[asn]
d[names[i]] = tmpdata[0:arraysize]
#print "Array is ", names[i], d[names[i]]
# print "Array is ", names[i], d[names[i]]
tmpdata = tmpdata[arraysize:]
j += 2
i += 1
else:
fmtsize = struct.calcsize(fmt[j])
#print "reading ", f , i, j, fmtsize, len(tmpdata)
if len(tmpdata) < fmtsize: #seems to happen on windows
# print "reading ", f , i, j, fmtsize, len(tmpdata)
if len(tmpdata) < fmtsize: # seems to happen on windows
raise ParsingException("Error, length of data smaller than advertized: ", len(tmpdata), fmtsize, "for names ", names, f, i, j)
d[names[i]] = struct.unpack("!" + f, tmpdata[0:fmtsize])[0]
#print names[i], d[names[i]]
# print names[i], d[names[i]]
tmpdata = tmpdata[fmtsize:]
j += 1
i += 1
@ -159,7 +160,7 @@ class ParserUtils(object):
if psize < 5:
raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize)
elif psize > len(data):
raise ParsingException("Error, length of data smaller (%s) than declared (%s)" %(len(data), psize))
raise ParsingException("Error, length of data smaller (%s) than declared (%s)" % (len(data), psize))
return psize, ptype, data[:psize], data[psize:]
def find_first_packet(self, data):
@ -183,7 +184,7 @@ class ParserUtils(object):
self.logger.debug("Got packet with size %s and type %s", psize, ptype)
if counter:
self.logger.info("Remove {0} bytes of garbage at begining of packet".format(counter))
#ok we we have somehting which looks like a packet"
# ok we we have somehting which looks like a packet"
return (data[:psize], data[psize:])
else:
#packet is not complete
@ -194,11 +195,12 @@ class ParserUtils(object):
return None
class SecondaryMonitor(Thread):
"""
Monitor data from secondary port and send programs to robot
"""
def __init__(self, host):
Thread.__init__(self)
self.logger = logging.getLogger(self.__class__.__name__)
@ -211,13 +213,13 @@ class SecondaryMonitor(Thread):
self._prog_queue = []
self._prog_queue_lock = Lock()
self._dataqueue = bytes()
self._trystop = False # to stop thread
self.running = False #True when robot is on and listening
self._trystop = False # to stop thread
self.running = False # True when robot is on and listening
self._dataEvent = Condition()
self.lastpacket_timestamp = 0
self.start()
self.wait()# make sure we got some data before someone calls us
self.wait() # make sure we got some data before someone calls us
def send_program(self, prog):
"""
@ -226,12 +228,11 @@ class SecondaryMonitor(Thread):
"""
prog.strip()
self.logger.debug("Sending program: " + prog)
if type(prog) != bytes:
if not isinstance(prog, bytes):
prog = prog.encode()
with self._prog_queue_lock:
self._prog_queue.append(prog + b"\n")
def run(self):
"""
check program execution status in the secondary client data packet we get from the robot
@ -263,22 +264,22 @@ class SecondaryMonitor(Thread):
if self._parser.is_v30:
if self._dict["RobotModeData"]["robotMode"] == 7 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True
else:
if self._dict["RobotModeData"]["robotMode"] == 0 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True
else:
if self.running == True:
if self.running:
self.logger.error("Robot not running: " + str(self._dict["RobotModeData"]))
self.running = False
with self._dataEvent:
@ -344,7 +345,7 @@ class SecondaryMonitor(Thread):
with self._dictLock:
output = self._dict["MasterBoardData"]["digitalOutputBits"]
mask = 1 << nb
if output & mask:
if output & mask:
return 1
else:
return 0
@ -355,7 +356,7 @@ class SecondaryMonitor(Thread):
with self._dictLock:
output = self._dict["MasterBoardData"]["digitalInputBits"]
mask = 1 << nb
if output & mask:
if output & mask:
return 1
else:
return 0
@ -388,16 +389,11 @@ class SecondaryMonitor(Thread):
with self._dictLock:
return self._dict["RobotModeData"]["isProgramRunning"]
def cleanup(self):
self._trystop = True
self.join()
#with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that?
#self._dataEvent.notifyAll()
# with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that?
# self._dataEvent.notifyAll()
if self._s_secondary:
with self._prog_queue_lock:
self._s_secondary.close()