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

29
urx/robot.py
View File

@ -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,6 +37,7 @@ 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
@ -54,7 +56,6 @@ class URRobot(object):
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"])
@ -398,14 +399,14 @@ class URRobot(object):
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
@ -418,7 +419,7 @@ class Robot(URRobot):
""" """
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)
@ -494,10 +495,9 @@ class Robot(URRobot):
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
@ -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)
@ -631,12 +631,5 @@ class Robot(URRobot):
return return
if not MATH3D: if not MATH3D:
Robot = URRobot Robot = URRobot

65
urx/urrtmon.py
View File

@ -11,7 +11,6 @@ __credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"]
__license__ = "GPLv3" __license__ = "GPLv3"
import logging import logging
import socket import socket
import struct import struct
@ -23,13 +22,14 @@ 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):
@ -45,7 +45,7 @@ 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
@ -68,7 +68,7 @@ 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:
@ -137,17 +137,21 @@ 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(
'Received header telling that package is %d bytes long' %
pkgsize)
payload = self.__recv_bytes(pkgsize - 4) 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:
@ -157,8 +161,13 @@ class URRTMonitor(threading.Thread):
#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,15 +258,21 @@ 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!')

20
urx/ursecmon.py
View File

@ -20,19 +20,20 @@ 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
@ -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__)
@ -226,12 +228,11 @@ class SecondaryMonitor(Thread):
""" """
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
@ -278,7 +279,7 @@ class SecondaryMonitor(Thread):
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:
@ -388,7 +389,6 @@ 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()
@ -397,7 +397,3 @@ class SecondaryMonitor(Thread):
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()