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" __version__ = "0.9.0"
from urx.robot import Robot, RobotException, URRobot from urx.robot import Robot, RobotException, URRobot

103
urx/robot.py
View File

@ -19,7 +19,7 @@ except ImportError:
MATH3D = False MATH3D = False
print("python-math3d library could not be found on this computer, disabling use of matrices and path blending") print("python-math3d library could not be found on this computer, disabling use of matrices and path blending")
from urx import urrtmon from urx import urrtmon
from urx import ursecmon from urx import ursecmon
@ -28,6 +28,7 @@ class RobotException(Exception):
class URRobot(object): class URRobot(object):
""" """
Python interface to socket interface of UR robot. Python interface to socket interface of UR robot.
programs are send to port 3002 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 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 Rmq: A program sent to the robot i executed immendiatly and any running program is stopped
""" """
def __init__(self, host, useRTInterface=False): def __init__(self, host, useRTInterface=False):
self.logger = logging.getLogger(self.__class__.__name__) self.logger = logging.getLogger(self.__class__.__name__)
self.host = host self.host = host
self.csys = None self.csys = None
self.logger.info("Opening secondary monitor socket") 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 self.rtmon = None
if useRTInterface: if useRTInterface:
self.rtmon = self.get_realtime_monitor() self.rtmon = self.get_realtime_monitor()
#the next 3 values must be conservative! otherwise we may wait forever # 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 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 # It seems URScript is limited in the character length of floats it accepts
self.max_float_length = 6 # FIXME: check max length!!! 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.secmon.wait() # make sure we get data from robot before letting clients access our methods
def __repr__(self): def __repr__(self):
return "Robot Object (IP=%s, state=%s)" % (self.host, self.secmon.get_all_data()["RobotModeData"]) return "Robot Object (IP=%s, state=%s)" % (self.host, self.secmon.get_all_data()["RobotModeData"])
@ -61,7 +62,7 @@ class URRobot(object):
def __str__(self): def __str__(self):
return self.__repr__() return self.__repr__()
def is_running(self): def is_running(self):
return self.secmon.running return self.secmon.running
def is_program_running(self): def is_program_running(self):
@ -117,21 +118,21 @@ class URRobot(object):
cog.insert(0, weight) cog.insert(0, weight)
prog = "set_payload({}, ({},{},{}))".format(*cog) prog = "set_payload({}, ({},{},{}))".format(*cog)
else: else:
prog = "set_payload(%s)" % weight prog = "set_payload(%s)" % weight
self.send_program(prog) self.send_program(prog)
def set_gravity(self, vector): def set_gravity(self, vector):
""" """
set direction of gravity set direction of gravity
""" """
prog = "set_gravity(%s)" % list(vector) prog = "set_gravity(%s)" % list(vector)
self.send_program(prog) self.send_program(prog)
def send_message(self, msg): def send_message(self, msg):
""" """
send message to the GUI log tab on the robot controller send message to the GUI log tab on the robot controller
""" """
prog = "textmsg(%s)" % msg prog = "textmsg(%s)" % msg
self.send_program(prog) self.send_program(prog)
def set_digital_out(self, output, val): def set_digital_out(self, output, val):
@ -146,13 +147,13 @@ class URRobot(object):
def get_analog_inputs(self): def get_analog_inputs(self):
""" """
get analog input get analog input
""" """
return self.secmon.get_analog_inputs() return self.secmon.get_analog_inputs()
def get_analog_in(self, nb, wait=False): def get_analog_in(self, nb, wait=False):
""" """
get analog input get analog input
""" """
return self.secmon.get_analog_in(nb, wait=wait) return self.secmon.get_analog_in(nb, wait=wait)
@ -178,14 +179,14 @@ class URRobot(object):
""" """
set analog output, val is a float set analog output, val is a float
""" """
prog = "set_analog_output(%s, %s)" % (output, val) prog = "set_analog_output(%s, %s)" % (output, val)
self.send_program(prog) self.send_program(prog)
def set_tool_voltage(self, val): def set_tool_voltage(self, val):
""" """
set voltage to be delivered to the tool, val is 0, 12 or 24 set voltage to be delivered to the tool, val is 0, 12 or 24
""" """
prog = "set_tool_voltage(%s)" % (val) prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog) self.send_program(prog)
def wait_for_move(self, radius=0, target=None): def wait_for_move(self, radius=0, target=None):
@ -194,7 +195,7 @@ class URRobot(object):
radius and target args are ignored radius and target args are ignored
""" """
self.logger.debug("Waiting for move completion") self.logger.debug("Waiting for move completion")
# it is necessary to wait since robot may takes a while to get into running state, # it is necessary to wait since robot may takes a while to get into running state,
for _ in range(3): for _ in range(3):
self.secmon.wait() self.secmon.wait()
while True: while True:
@ -203,9 +204,9 @@ class URRobot(object):
jts = self.secmon.get_joint_data(wait=True) jts = self.secmon.get_joint_data(wait=True)
finished = True finished = True
for i in range(0, 6): for i in range(0, 6):
#Rmq: q_target is an interpolated target we have no control over # 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: 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) 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 finished = False
break break
if finished and not self.secmon.is_program_running(): if finished and not self.secmon.is_program_running():
@ -246,7 +247,7 @@ class URRobot(object):
if relative: if relative:
l = self.getj() l = self.getj()
joints = [v + l[i] for i, v in enumerate(joints)] joints = [v + l[i] for i, v in enumerate(joints)]
joints = [round(j, self.max_float_length) for j in joints] joints = [round(j, self.max_float_length) for j in joints]
joints.append(acc) joints.append(acc)
joints.append(vel) joints.append(vel)
joints.append(radius) joints.append(radius)
@ -260,7 +261,7 @@ class URRobot(object):
def movel(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False): def movel(self, tpose, acc=0.01, vel=0.01, radius=0, wait=True, relative=False):
""" """
linear move linear move
""" """
if relative: if relative:
l = self.getl() l = self.getl()
@ -328,7 +329,7 @@ class URRobot(object):
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True): def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
""" """
Concatenate several movel commands and applies a blending radius Concatenate several movel commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
""" """
header = "def myProg():\n" header = "def myProg():\n"
end = "end\n" end = "end\n"
@ -337,7 +338,7 @@ class URRobot(object):
for idx, pose in enumerate(pose_list): for idx, pose in enumerate(pose_list):
pose.append(acc) pose.append(acc)
pose.append(vel) pose.append(vel)
if idx != (len(pose_list) -1): if idx != (len(pose_list) - 1):
pose.append(radius) pose.append(radius)
else: else:
pose.append(0) pose.append(0)
@ -367,11 +368,11 @@ class URRobot(object):
self.secmon.cleanup() self.secmon.cleanup()
if self.rtmon: if self.rtmon:
self.rtmon.stop() 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): def set_freedrive(self, val):
""" """
set robot in freedrive/brackdrive mode where an operator can jogg set robot in freedrive/brackdrive mode where an operator can jogg
the robot to wished pose the robot to wished pose
""" """
if val: if val:
@ -392,33 +393,33 @@ class URRobot(object):
""" """
if not self.rtmon: if not self.rtmon:
self.logger.info("Opening real-time monitor socket") 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.start()
self.rtmon.set_csys(self.csys) self.rtmon.set_csys(self.csys)
return self.rtmon return self.rtmon
class Robot(URRobot): class Robot(URRobot):
""" """
Generic Python interface to an industrial robot. Generic Python interface to an industrial robot.
Compared to the URRobot class, this class adds the possibilty to work directly with matrices Compared to the URRobot class, this class adds the possibilty to work directly with matrices
and includes support for calibrating the robot coordinate system and includes support for calibrating the robot coordinate system
""" """
def __init__(self, host, useRTInterface=False): def __init__(self, host, useRTInterface=False):
URRobot.__init__(self, host, useRTInterface) URRobot.__init__(self, host, useRTInterface)
self.default_linear_acceleration = 0.01 self.default_linear_acceleration = 0.01
self.default_linear_velocity = 0.01 self.default_linear_velocity = 0.01
self.csys_dict = {} self.csys_dict = {}
self.csys = None self.csys = None
self.set_csys("Robot", m3d.Transform()) #identity self.set_csys("Robot", m3d.Transform()) # identity
def set_tcp(self, tcp): def set_tcp(self, tcp):
""" """
set robot flange to tool tip transformation set robot flange to tool tip transformation
""" """
if type(tcp) == m3d.Transform: if isinstance(tcp, m3d.Transform):
tcp = tcp.pose_vector tcp = tcp.pose_vector
URRobot.set_tcp(self, tcp) URRobot.set_tcp(self, tcp)
@ -442,7 +443,7 @@ class Robot(URRobot):
set tool orientation using a orientation matric from math3d set tool orientation using a orientation matric from math3d
or a orientation vector or a orientation vector
""" """
if type(orient) is not m3d.Orientation: if not isinstance(orient, m3d.Orientation):
orient = m3d.Orientation(orient) orient = m3d.Orientation(orient)
trans = self.get_pose() trans = self.get_pose()
trans.orient = orient trans.orient = orient
@ -453,7 +454,7 @@ class Robot(URRobot):
move tool in base coordinate, keeping orientation move tool in base coordinate, keeping orientation
""" """
t = m3d.Transform() t = m3d.Transform()
if not type(vect) is m3d.Vector: if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += m3d.Vector(vect) t.pos += m3d.Vector(vect)
return self.add_pose_base(t, acc, vel, radius, wait=wait) 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 move tool in tool coordinate, keeping orientation
""" """
t = m3d.Transform() t = m3d.Transform()
if not type(vect) is m3d.Vector: if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += vect t.pos += vect
return self.add_pose_tool(t, acc, vel, radius, wait=wait) 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 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) vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.set_pose(trans, acc, vel, radius, wait=wait) return self.set_pose(trans, acc, vel, radius, wait=wait)
@ -482,22 +483,21 @@ class Robot(URRobot):
move tcp to point and orientation defined by a transformation move tcp to point and orientation defined by a transformation
if process is True, movep is used instead of movel if process is True, movep is used instead of movel
if radius is not 0 and wait is True, the method will return when tcp if radius is not 0 and wait is True, the method will return when tcp
is radius close to the target. It is then possible to send another command is radius close to the target. It is then possible to send another command
and the controller will blend the path. Blending only works with process(movep). (BROKEN!) and the controller will blend the path. Blending only works with process(movep). (BROKEN!)
""" """
if not acc: if not acc:
acc = self.default_linear_acceleration acc = self.default_linear_acceleration
if not vel: if not vel:
vel = self.default_linear_velocity vel = self.default_linear_velocity
t = self.csys * trans t = self.csys * trans
if process: if process:
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius)
else: else:
pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait, radius=radius) 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) return self.csys.inverse * m3d.Transform(pose)
def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False): def add_pose_base(self, trans, acc=None, vel=None, radius=0, wait=True, process=False):
""" """
Add transform expressed in base coordinate Add transform expressed in base coordinate
@ -517,7 +517,7 @@ class Robot(URRobot):
get current transform from base to to tcp get current transform from base to to tcp
""" """
pose = URRobot.getl(self, wait) pose = URRobot.getl(self, wait)
trans = self.csys.inverse * m3d.Transform(pose) trans = self.csys.inverse * m3d.Transform(pose)
return trans return trans
def get_orientation(self, wait=False): def get_orientation(self, wait=False):
@ -565,7 +565,7 @@ class Robot(URRobot):
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True): def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True):
""" """
Concatenate several movep commands and applies a blending radius Concatenate several movep commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
""" """
new_poses = [] new_poses = []
for pose in pose_list: for pose in pose_list:
@ -606,7 +606,7 @@ class Robot(URRobot):
return t.pose_vector.tolist() return t.pose_vector.tolist()
def set_gravity(self, vector): def set_gravity(self, vector):
if type(vector) == m3d.Vector: if isinstance(vector, m3d.Vector):
vector = vector.list vector = vector.list
return URRobot.set_gravity(self, vector) return URRobot.set_gravity(self, vector)
@ -616,7 +616,7 @@ class Robot(URRobot):
if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method) if radius is not 0, returns when tcp is radius closed to target(using mathd3d dist method)
""" """
self.logger.debug("Waiting for move completion") self.logger.debug("Waiting for move completion")
# it is necessary to wait since robot may takes a while to get into running state, # it is necessary to wait since robot may takes a while to get into running state,
for _ in range(3): for _ in range(3):
self.secmon.wait() self.secmon.wait()
target = m3d.Transform(target) target = m3d.Transform(target)
@ -631,12 +631,5 @@ class Robot(URRobot):
return return
if not MATH3D: if not MATH3D:
Robot = URRobot Robot = URRobot

89
urx/urrtmon.py
View File

@ -11,10 +11,9 @@ __credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"]
__license__ = "GPLv3" __license__ = "GPLv3"
import logging import logging
import socket import socket
import struct import struct
import time import time
import threading import threading
from copy import deepcopy from copy import deepcopy
@ -23,19 +22,20 @@ import numpy as np
import math3d as m3d import math3d as m3d
class URRTMonitor(threading.Thread): 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') rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ')
## for revision of the UR controller giving 540 byte. Here TCP # for revision of the UR controller giving 540 byte. Here TCP
## pose is not included! # pose is not included!
rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d')
def __init__(self, urHost, loglevel=logging.WARNING): def __init__(self, urHost, loglevel=logging.WARNING):
threading.Thread.__init__(self) threading.Thread.__init__(self)
self.logger = logging.getLogger(self.__class__.__name__) 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() logging.basicConfig()
self.logger.setLevel(loglevel) self.logger.setLevel(loglevel)
self.daemon = True self.daemon = True
@ -45,13 +45,13 @@ class URRTMonitor(threading.Thread):
self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self._urHost = urHost self._urHost = urHost
## Package data variables # Package data variables
self._timestamp = None self._timestamp = None
self._ctrlTimestamp = None self._ctrlTimestamp = None
self._qActual = None self._qActual = None
self._qTarget = None self._qTarget = None
self._tcp = None self._tcp = None
self._tcp_force = None self._tcp_force = None
self.__recvTime = 0 self.__recvTime = 0
self._last_ctrl_ts = 0 self._last_ctrl_ts = 0
#self._last_ts = 0 #self._last_ts = 0
@ -68,13 +68,13 @@ class URRTMonitor(threading.Thread):
def __recv_bytes(self, nBytes): def __recv_bytes(self, nBytes):
''' Facility method for receiving exactly "nBytes" bytes from ''' Facility method for receiving exactly "nBytes" bytes from
the robot connector socket.''' 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 recvTime = 0
pkg = b'' pkg = b''
while len(pkg) < nBytes: while len(pkg) < nBytes:
pkg += self._rtSock.recv(nBytes - len(pkg)) pkg += self._rtSock.recv(nBytes - len(pkg))
if recvTime == 0: if recvTime == 0:
recvTime = time.time() recvTime = time.time()
self.__recvTime = recvTime self.__recvTime = recvTime
return pkg return pkg
@ -92,7 +92,7 @@ class URRTMonitor(threading.Thread):
else: else:
return self._qActual return self._qActual
getActual = q_actual getActual = q_actual
def q_target(self, wait=False, timestamp=False): def q_target(self, wait=False, timestamp=False):
""" Get the target joint position vector.""" """ Get the target joint position vector."""
if wait: if wait:
@ -103,7 +103,7 @@ class URRTMonitor(threading.Thread):
else: else:
return self._qTarget return self._qTarget
getTarget = q_target getTarget = q_target
def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False):
""" Return the tool pose values.""" """ Return the tool pose values."""
if wait: if wait:
@ -137,28 +137,37 @@ class URRTMonitor(threading.Thread):
def __recv_rt_data(self): def __recv_rt_data(self):
head = self.__recv_bytes(4) head = self.__recv_bytes(4)
## Record the timestamp for this logical package # Record the timestamp for this logical package
timestamp = self.__recvTime timestamp = self.__recvTime
pkgsize = struct.unpack('>i', head)[0] pkgsize = struct.unpack('>i', head)[0]
self.logger.debug('Received header telling that package is %d bytes long'%pkgsize) self.logger.debug(
payload = self.__recv_bytes(pkgsize-4) 'Received header telling that package is %d bytes long' %
pkgsize)
payload = self.__recv_bytes(pkgsize - 4)
if pkgsize >= 692: if pkgsize >= 692:
unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size])
elif pkgsize >= 540: elif pkgsize >= 540:
unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size])
else: 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 return
with self._dataAccess: with self._dataAccess:
self._timestamp = timestamp self._timestamp = timestamp
# it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? # 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: # 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.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts))
#self._last_ts = self._timestamp #self._last_ts = self._timestamp
self._ctrlTimestamp = np.array(unp[0]) self._ctrlTimestamp = np.array(unp[0])
if self._last_ctrl_ts != 0 and (self._ctrlTimestamp - self._last_ctrl_ts) > 0.010: if self._last_ctrl_ts != 0 and (
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._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._last_ctrl_ts = self._ctrlTimestamp
self._qActual = np.array(unp[31:37]) self._qActual = np.array(unp[31:37])
self._qTarget = np.array(unp[1:7]) self._qTarget = np.array(unp[1:7])
@ -167,11 +176,16 @@ class URRTMonitor(threading.Thread):
if self._csys: if self._csys:
with self._csys_lock: 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 self._tcp = tcp.pose_vector
if self._buffering: if self._buffering:
with self._buffer_lock: 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: with self._dataEvent:
self._dataEvent.notifyAll() self._dataEvent.notifyAll()
@ -206,7 +220,6 @@ class URRTMonitor(threading.Thread):
return self._buffer.pop(0) return self._buffer.pop(0)
time.sleep(0.001) time.sleep(0.001)
def get_buffer(self): def get_buffer(self):
""" """
return a copy of the entire buffer return a copy of the entire buffer
@ -221,7 +234,13 @@ class URRTMonitor(threading.Thread):
if wait: if wait:
self.wait() self.wait()
with self._dataAccess: 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): def stop(self):
#print(self.__class__.__name__+': Stopping') #print(self.__class__.__name__+': Stopping')
@ -239,20 +258,26 @@ class URRTMonitor(threading.Thread):
self._rtSock.close() self._rtSock.close()
def startupInteractive(): def startupInteractive():
from optparse import OptionParser from optparse import OptionParser
from IPython import embed from IPython import embed
## Require the urhost arg. # Require the urhost arg.
parser = OptionParser() parser = OptionParser()
parser.add_option('--debug', action='store_true', default=False, dest='debug') parser.add_option(
parser.add_option('--start', action='store_true', default=False, dest='start') '--debug',
action='store_true',
default=False,
dest='debug')
parser.add_option(
'--start',
action='store_true',
default=False,
dest='start')
opts, args = parser.parse_args() opts, args = parser.parse_args()
if len(args) != 1: if len(args) != 1:
raise Exception('Must have argument with ur-host name or ip!') raise Exception('Must have argument with ur-host name or ip!')
urHost = args[0] 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 # # Start the connectors
urRTMon = URRTMonitor(urHost, debug=opts.debug) urRTMon = URRTMonitor(urHost, debug=opts.debug)
# # Register for hard shutdown # # Register for hard shutdown

112
urx/ursecmon.py
View File

@ -14,25 +14,26 @@ __license__ = "GPLv3"
from threading import Thread, Condition, Lock from threading import Thread, Condition, Lock
import logging import logging
import struct import struct
import socket import socket
from copy import copy from copy import copy
import time import time
class ParsingException(Exception): class ParsingException(Exception):
def __init__(self, *args): def __init__(self, *args):
Exception.__init__(self, *args) Exception.__init__(self, *args)
class TimeoutException(Exception): class TimeoutException(Exception):
def __init__(self, *args): def __init__(self, *args):
Exception.__init__(self, *args) Exception.__init__(self, *args)
class ParserUtils(object): class ParserUtils(object):
def __init__(self): def __init__(self):
self.logger = logging.getLogger(__name__) self.logger = logging.getLogger(__name__)
self.is_v30 = False 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 parse a packet from the UR socket and return a dictionary with the data
""" """
allData = {} allData = {}
#print "Total size ", len(data) # print "Total size ", len(data)
while data: while data:
psize, ptype, pdata, data = self.analyze_header(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: if ptype == 16:
allData["SecondaryClientData"] = self._get_data(pdata, "!iB", ("size", "type")) 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: elif ptype == 0:
# this parses RobotModeData for versions >=3.0 (i.e. 3.0) # this parses RobotModeData for versions >=3.0 (i.e. 3.0)
if psize == 38: if psize == 38:
@ -68,14 +69,14 @@ class ParserUtils(object):
elif ptype == 5: elif ptype == 5:
allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type")) allData["LaserPointer(OBSOLETE)"] = self._get_data(pdata, "iBddd", ("size", "type"))
elif ptype == 3: 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: elif ptype == 2:
allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode")) allData["ToolData"] = self._get_data(pdata, "iBbbddfBffB", ("size", "type", "analoginputRange2", "analoginputRange3", "analogInput2", "analogInput3", "toolVoltage48V", "toolOutputVoltage", "toolCurrent", "toolTemperature", "toolMode"))
#elif ptype == 8: # elif ptype == 8:
#allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) #allData["varMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
#elif ptype == 7: # elif ptype == 7:
#allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText")) #allData["keyMessage"] = self._get_data(pdata, "!iBQbb iiBAcAc", ("size", "type", "timestamp", "source", "robotMessageType", "code", "argument", "titleSize", "messageTitle", "messageText"))
elif ptype == 20: elif ptype == 20:
tmp = self._get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType")) 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 names args are strings used to store values
""" """
tmpdata = copy(data) tmpdata = copy(data)
fmt = fmt.strip() # space may confuse us fmt = fmt.strip() # space may confuse us
d = dict() d = dict()
i = 0 i = 0
j = 0 j = 0
@ -118,32 +119,32 @@ class ParserUtils(object):
f = fmt[j] f = fmt[j]
if f in (" ", "!", ">", "<"): if f in (" ", "!", ">", "<"):
j += 1 j += 1
elif f == "A": #we got an array elif f == "A": # we got an array
# first we need to find its size # 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) arraysize = len(tmpdata)
else: # size should be given in last element else: # size should be given in last element
asn = names[i-1] asn = names[i - 1]
if not asn.endswith("Size"): if not asn.endswith("Size"):
raise ParsingException("Error, array without size ! %s %s" % (asn, i)) raise ParsingException("Error, array without size ! %s %s" % (asn, i))
else: else:
arraysize = d[asn] arraysize = d[asn]
d[names[i]] = tmpdata[0:arraysize] 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:] tmpdata = tmpdata[arraysize:]
j += 2 j += 2
i += 1 i += 1
else: else:
fmtsize = struct.calcsize(fmt[j]) fmtsize = struct.calcsize(fmt[j])
#print "reading ", f , i, j, fmtsize, len(tmpdata) # print "reading ", f , i, j, fmtsize, len(tmpdata)
if len(tmpdata) < fmtsize: #seems to happen on windows 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) 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] 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:] tmpdata = tmpdata[fmtsize:]
j += 1 j += 1
i += 1 i += 1
return d return d
def get_header(self, data): def get_header(self, data):
return struct.unpack("!iB", data[0:5]) return struct.unpack("!iB", data[0:5])
@ -156,10 +157,10 @@ class ParserUtils(object):
raise ParsingException("Packet size %s smaller than header size (5 bytes)" % len(data)) raise ParsingException("Packet size %s smaller than header size (5 bytes)" % len(data))
else: else:
psize, ptype = self.get_header(data) psize, ptype = self.get_header(data)
if psize < 5: if psize < 5:
raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize) raise ParsingException("Error, declared length of data smaller than its own header(5): ", psize)
elif psize > len(data): 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:] return psize, ptype, data[:psize], data[psize:]
def find_first_packet(self, data): 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) self.logger.debug("Got packet with size %s and type %s", psize, ptype)
if counter: if counter:
self.logger.info("Remove {0} bytes of garbage at begining of packet".format(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:]) return (data[:psize], data[psize:])
else: else:
#packet is not complete #packet is not complete
@ -194,11 +195,12 @@ class ParserUtils(object):
return None return None
class SecondaryMonitor(Thread): class SecondaryMonitor(Thread):
""" """
Monitor data from secondary port and send programs to robot Monitor data from secondary port and send programs to robot
""" """
def __init__(self, host): def __init__(self, host):
Thread.__init__(self) Thread.__init__(self)
self.logger = logging.getLogger(self.__class__.__name__) self.logger = logging.getLogger(self.__class__.__name__)
@ -211,32 +213,31 @@ class SecondaryMonitor(Thread):
self._prog_queue = [] self._prog_queue = []
self._prog_queue_lock = Lock() self._prog_queue_lock = Lock()
self._dataqueue = bytes() self._dataqueue = bytes()
self._trystop = False # to stop thread self._trystop = False # to stop thread
self.running = False #True when robot is on and listening self.running = False # True when robot is on and listening
self._dataEvent = Condition() self._dataEvent = Condition()
self.lastpacket_timestamp = 0 self.lastpacket_timestamp = 0
self.start() 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): def send_program(self, prog):
""" """
send program to robot in URRobot format send program to robot in URRobot format
If another program is send while a program is running the first program is aborded. If another program is send while a program is running the first program is aborded.
""" """
prog.strip() prog.strip()
self.logger.debug("Sending program: " + prog) self.logger.debug("Sending program: " + prog)
if type(prog) != bytes: if not isinstance(prog, bytes):
prog = prog.encode() prog = prog.encode()
with self._prog_queue_lock: with self._prog_queue_lock:
self._prog_queue.append(prog + b"\n") self._prog_queue.append(prog + b"\n")
def run(self): def run(self):
""" """
check program execution status in the secondary client data packet we get from the robot check program execution status in the secondary client data packet we get from the robot
This interface uses only data from the secondary client interface (see UR doc) This interface uses only data from the secondary client interface (see UR doc)
Only the last connected client is the primary client, Only the last connected client is the primary client,
so this is not guaranted and we cannot rely on information to the primary client. so this is not guaranted and we cannot rely on information to the primary client.
""" """
@ -245,12 +246,12 @@ class SecondaryMonitor(Thread):
if len(self._prog_queue) > 0: if len(self._prog_queue) > 0:
prog = self._prog_queue.pop(0) prog = self._prog_queue.pop(0)
self._s_secondary.send(prog) self._s_secondary.send(prog)
data = self._get_data() data = self._get_data()
try: try:
tmpdict = self._parser.parse(data) tmpdict = self._parser.parse(data)
with self._dictLock: with self._dictLock:
self._dict = tmpdict self._dict = tmpdict
except ParsingException as ex: except ParsingException as ex:
self.logger.warn("Error parsing one packet from urrobot: " + str(ex)) self.logger.warn("Error parsing one packet from urrobot: " + str(ex))
continue continue
@ -259,26 +260,26 @@ class SecondaryMonitor(Thread):
self.logger.warn("Got a packet from robot without RobotModeData, strange ...") self.logger.warn("Got a packet from robot without RobotModeData, strange ...")
continue continue
self.lastpacket_timestamp = time.time() self.lastpacket_timestamp = time.time()
if self._parser.is_v30: if self._parser.is_v30:
if self._dict["RobotModeData"]["robotMode"] == 7 \ if self._dict["RobotModeData"]["robotMode"] == 7 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \ and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \ and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True: and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True self.running = True
else: else:
if self._dict["RobotModeData"]["robotMode"] == 0 \ if self._dict["RobotModeData"]["robotMode"] == 0 \
and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \ and self._dict["RobotModeData"]["isRealRobotEnabled"] == True \
and self._dict["RobotModeData"]["isEmergencyStopped"] == False \ and self._dict["RobotModeData"]["isEmergencyStopped"] == False \
and self._dict["RobotModeData"]["isSecurityStopped"] == False \ and self._dict["RobotModeData"]["isSecurityStopped"] == False \
and self._dict["RobotModeData"]["isRobotConnected"] == True \ and self._dict["RobotModeData"]["isRobotConnected"] == True \
and self._dict["RobotModeData"]["isPowerOnRobot"] == True: and self._dict["RobotModeData"]["isPowerOnRobot"] == True:
self.running = True self.running = True
else: else:
if self.running == True: if self.running:
self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) self.logger.error("Robot not running: " + str(self._dict["RobotModeData"]))
self.running = False self.running = False
with self._dataEvent: with self._dataEvent:
@ -344,7 +345,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalOutputBits"] output = self._dict["MasterBoardData"]["digitalOutputBits"]
mask = 1 << nb mask = 1 << nb
if output & mask: if output & mask:
return 1 return 1
else: else:
return 0 return 0
@ -355,7 +356,7 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
output = self._dict["MasterBoardData"]["digitalInputBits"] output = self._dict["MasterBoardData"]["digitalInputBits"]
mask = 1 << nb mask = 1 << nb
if output & mask: if output & mask:
return 1 return 1
else: else:
return 0 return 0
@ -388,16 +389,11 @@ class SecondaryMonitor(Thread):
with self._dictLock: with self._dictLock:
return self._dict["RobotModeData"]["isProgramRunning"] return self._dict["RobotModeData"]["isProgramRunning"]
def cleanup(self): def cleanup(self):
self._trystop = True self._trystop = True
self.join() self.join()
#with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that? # with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that?
#self._dataEvent.notifyAll() # self._dataEvent.notifyAll()
if self._s_secondary: if self._s_secondary:
with self._prog_queue_lock: with self._prog_queue_lock:
self._s_secondary.close() self._s_secondary.close()