autopep8 --max-line-length 900 -a -a -i robot.py
This commit is contained in:
		| @@ -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
									
									
									
									
									
								
							
							
						
						
									
										29
									
								
								urx/robot.py
									
									
									
									
									
								
							| @@ -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 | ||||||
|  |  | ||||||
|  |  | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
| @@ -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,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() | ||||||
|  |  | ||||||
|  |  | ||||||
|  |  | ||||||
|  |  | ||||||
|   | |||||||
		Reference in New Issue
	
	Block a user