diff --git a/urx/__init__.py b/urx/__init__.py index 7589a92..75ca124 100644 --- a/urx/__init__.py +++ b/urx/__init__.py @@ -1,11 +1,10 @@ """ Python library to control an UR robot through its TCP/IP interface """ +from urx.urrobot import RobotException, URRobot # noqa + __version__ = "0.9.0" - -from urx.urrobot import RobotException, URRobot - try: from urx.robot import Robot except ImportError as ex: diff --git a/urx/robotiq_two_finger_gripper.py b/urx/robotiq_two_finger_gripper.py index 84eecdf..5612b20 100644 --- a/urx/robotiq_two_finger_gripper.py +++ b/urx/robotiq_two_finger_gripper.py @@ -26,61 +26,62 @@ Future Features import logging + class Robotiq_Two_Finger_Gripper(object): - complete_program = "" - header = "def myProg():" + b"\n" - end = b"\n" + "end" - logger = False + complete_program = "" + header = "def myProg():" + b"\n" + end = b"\n" + "end" + logger = False - def __init__(self): - self.logger = logging.getLogger("urx") - self.reset() + def __init__(self): + self.logger = logging.getLogger("urx") + self.reset() - def reset(self): - self.complete_program = "" - self.add_line_to_program("set_analog_inputrange(0, 0)") - self.add_line_to_program("set_analog_inputrange(1, 0)") - self.add_line_to_program("set_analog_inputrange(2, 0)") - self.add_line_to_program("set_analog_inputrange(3, 0)") - self.add_line_to_program("set_analog_outputdomain(0, 0)") - self.add_line_to_program("set_analog_outputdomain(1, 0)") - self.add_line_to_program("set_tool_voltage(0)") - self.add_line_to_program("set_runstate_outputs([])") - self.add_line_to_program("set_payload(0.85)") #0.85 is the weight of the gripper in KG - self.add_line_to_program("socket_close(\"gripper_socket\")") - #self.add_line_to_program("sleep(1)") #in Robotiq's example they do a wait here... I haven't found it nec - self.add_line_to_program("socket_open(\"127.0.0.1\",63352,\"gripper_socket\")") - #self.add_line_to_program("sleep(1)") - self.add_line_to_program("socket_set_var(\"SEP\",255,\"gripper_socket\")") #Speed 0-255 is valid - self.add_line_to_program("sync()") - self.add_line_to_program("socket_set_var(\"FOR\",50,\"gripper_socket\")") #Force 0-255 is valid - self.add_line_to_program("sync()") - self.add_line_to_program("socket_set_var(\"ACT\",1,\"gripper_socket\")") # Activate robot - self.add_line_to_program("sync()") - self.add_line_to_program("socket_set_var(\"GTO\",1,\"gripper_socket\")") - self.add_line_to_program("sync()") + def reset(self): + self.complete_program = "" + self.add_line_to_program("set_analog_inputrange(0, 0)") + self.add_line_to_program("set_analog_inputrange(1, 0)") + self.add_line_to_program("set_analog_inputrange(2, 0)") + self.add_line_to_program("set_analog_inputrange(3, 0)") + self.add_line_to_program("set_analog_outputdomain(0, 0)") + self.add_line_to_program("set_analog_outputdomain(1, 0)") + self.add_line_to_program("set_tool_voltage(0)") + self.add_line_to_program("set_runstate_outputs([])") + self.add_line_to_program("set_payload(0.85)") # 0.85 is the weight of the gripper in KG + self.add_line_to_program("socket_close(\"gripper_socket\")") + # self.add_line_to_program("sleep(1)") # in Robotiq's example they do a wait here... I haven't found it nec + self.add_line_to_program("socket_open(\"127.0.0.1\",63352,\"gripper_socket\")") + # self.add_line_to_program("sleep(1)") + self.add_line_to_program("socket_set_var(\"SEP\",255,\"gripper_socket\")") # Speed 0-255 is valid + self.add_line_to_program("sync()") + self.add_line_to_program("socket_set_var(\"FOR\",50,\"gripper_socket\")") # Force 0-255 is valid + self.add_line_to_program("sync()") + self.add_line_to_program("socket_set_var(\"ACT\",1,\"gripper_socket\")") # Activate robot + self.add_line_to_program("sync()") + self.add_line_to_program("socket_set_var(\"GTO\",1,\"gripper_socket\")") + self.add_line_to_program("sync()") - def open_gripper(self): - self.add_line_to_program("socket_set_var(\"POS\",0,\"gripper_socket\")") #0 is open; range is 0-255 - self.add_line_to_program("sync()") - self.add_line_to_program("sleep(2)") + def open_gripper(self): + self.add_line_to_program("socket_set_var(\"POS\",0,\"gripper_socket\")") # 0 is open; range is 0-255 + self.add_line_to_program("sync()") + self.add_line_to_program("sleep(2)") - def close_gripper(self): - self.add_line_to_program("socket_set_var(\"POS\",255,\"gripper_socket\")") #255 is closed; range is 0-255 - self.add_line_to_program("sync()") - self.add_line_to_program("sleep(2)") + def close_gripper(self): + self.add_line_to_program("socket_set_var(\"POS\",255,\"gripper_socket\")") # 255 is closed; range is 0-255 + self.add_line_to_program("sync()") + self.add_line_to_program("sleep(2)") - def add_line_to_program(self,new_line): - if(self.complete_program != ""): - self.complete_program += b"\n" - self.complete_program += new_line + def add_line_to_program(self, new_line): + if(self.complete_program != ""): + self.complete_program += b"\n" + self.complete_program += new_line - def ret_program_to_run(self): - if(self.complete_program == ""): - self.logger.debug("robotiq_two_finger_gripper's program is empty") - return "" + def ret_program_to_run(self): + if(self.complete_program == ""): + self.logger.debug("robotiq_two_finger_gripper's program is empty") + return "" - prog = self.header - prog += self.complete_program - prog += self.end - return prog \ No newline at end of file + prog = self.header + prog += self.complete_program + prog += self.end + return prog diff --git a/urx/urrobot.py b/urx/urrobot.py index 05444cf..cbbcca0 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -208,7 +208,7 @@ class URRobot(object): start_dist = self._get_dist(target, joints) if threshold is None: threshold = start_dist * 0.8 - if threshold < 0.001: # roboten precision is limited + if threshold < 0.001: # roboten precision is limited threshold = 0.001 self.logger.debug("No threshold set, setting it to %s", threshold) count = 0 @@ -234,13 +234,13 @@ class URRobot(object): return self._get_lin_dist(target) def _get_lin_dist(self, target): - #FIXME: we have an issue here, it seems sometimes the axis angle received from robot + # FIXME: we have an issue here, it seems sometimes the axis angle received from robot pose = URRobot.getl(self, wait=True) dist = 0 for i in range(3): dist += (target[i] - pose[i]) ** 2 for i in range(3, 6): - dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like + dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like return dist ** 0.5 def _get_joints_dist(self, target): diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 56fe77f..6292a45 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -4,13 +4,6 @@ Confer http://support.universal-robots.com/Technical/RealTimeClientInterface Note: The packet lenght given in the web-page is 740. What is actually received from the controller is 692. It is assumed that the motor currents, the last group of 48 bytes, are not send. Originally Written by Morten Lind ''' - -__author__ = "Morten Lind, Olivier Roulet-Dubonnet" -__copyright__ = "Copyright 2011, NTNU/SINTEF Raufoss Manufacturing AS" -__credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"] -__license__ = "LGPLv3" - - import logging import socket import struct @@ -22,6 +15,11 @@ import numpy as np import math3d as m3d +__author__ = "Morten Lind, Olivier Roulet-Dubonnet" +__copyright__ = "Copyright 2011, NTNU/SINTEF Raufoss Manufacturing AS" +__credits__ = ["Morten Lind, Olivier Roulet-Dubonnet"] +__license__ = "LGPLv3" + class URRTMonitor(threading.Thread): @@ -51,7 +49,7 @@ class URRTMonitor(threading.Thread): self._tcp_force = None self.__recvTime = 0 self._last_ctrl_ts = 0 - #self._last_ts = 0 + # self._last_ts = 0 self._buffering = False self._buffer_lock = threading.Lock() self._buffer = [] @@ -138,7 +136,7 @@ class URRTMonitor(threading.Thread): timestamp = self.__recvTime pkgsize = struct.unpack('>i', head)[0] self.logger.debug( - 'Received header telling that package is %s bytes long', + 'Received header telling that package is %s bytes long', pkgsize) payload = self.__recv_bytes(pkgsize - 4) if pkgsize >= 692: @@ -155,14 +153,14 @@ class URRTMonitor(threading.Thread): self._timestamp = timestamp # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? # if (self._timestamp - self._last_ts) > 0.010: - #self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) - #self._last_ts = self._timestamp + # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) + # self._last_ts = self._timestamp self._ctrlTimestamp = np.array(unp[0]) if self._last_ctrl_ts != 0 and ( self._ctrlTimestamp - self._last_ctrl_ts) > 0.010: self.logger.warning( - "Error the controller failed to send us a packet: time since last packet %s s ", + "Error the controller failed to send us a packet: time since last packet %s s ", self._ctrlTimestamp - self._last_ctrl_ts) self._last_ctrl_ts = self._ctrlTimestamp self._qActual = np.array(unp[31:37]) @@ -239,7 +237,7 @@ class URRTMonitor(threading.Thread): tcp_force=self._tcp_force) def stop(self): - #print(self.__class__.__name__+': Stopping') + # print(self.__class__.__name__+': Stopping') self._stop_event = True def close(self): diff --git a/urx/ursecmon.py b/urx/ursecmon.py index de762b8..502a56c 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -1,10 +1,10 @@ """ This file contains 2 classes: - ParseUtils containing utilies to parse data from UR robot - - SecondaryMonitor, a class opening a socket to the robot and with methods to + - SecondaryMonitor, a class opening a socket to the robot and with methods to access data and send programs to the robot Both use data from the secondary port of the URRobot. -Only the last connected socket on 3001 is the primary client !!!! +Only the last connected socket on 3001 is the primary client !!!! So do not rely on it unless you know no other client is running (Hint the UR java interface is a client...) http://support.universal-robots.com/Technical/PrimaryAndSecondaryClientInterface """ @@ -69,7 +69,7 @@ class ParserUtils(object): self.version = (3, 0) allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling")) elif psize == 46: # It's 46 bytes in 3.2 - self.version = (3,2) + self.version = (3, 2) allData['RobotModeData'] = self._get_data(pdata, "!IBQ???????BBdd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "controlMode", "speedFraction", "speedScaling", "speedFractionLimit")) else: allData["RobotModeData"] = self._get_data(pdata, "!iBQ???????Bd", ("size", "type", "timestamp", "isRobotConnected", "isRealRobotEnabled", "isPowerOnRobot", "isEmergencyStopped", "isSecurityStopped", "isProgramRunning", "isProgramPaused", "robotMode", "speedFraction")) @@ -94,7 +94,7 @@ class ParserUtils(object): else: fmt = "iBhhbbddbbddffffBBb" # firmware < 3.0 - allData["MasterBoardData"] = self._get_data(pdata, fmt, ("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, fmt, ("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 == 9: @@ -104,9 +104,9 @@ class ParserUtils(object): elif ptype == 7 and self.version >= (3, 2): allData["ForceModeData"] = self._get_data(pdata, "iBddddddd", ("size", "type", "x", "y", "z", "rx", "ry", "rz", "robotDexterity")) # 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: - #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: tmp = self._get_data(pdata, "!iB Qbb", ("size", "type", "timestamp", "source", "robotMessageType")) @@ -217,11 +217,11 @@ class ParserUtils(object): # ok we we have somehting which looks like a packet" return (data[:psize], data[psize:]) else: - #packet is not complete + # packet is not complete self.logger.debug("Packet is not complete, advertised size is %s, received size is %s, type is %s", psize, len(data), ptype) return None else: - #self.logger.debug("data smaller than 5 bytes") + # self.logger.debug("data smaller than 5 bytes") return None @@ -314,7 +314,7 @@ class SecondaryMonitor(Thread): self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) self.running = False with self._dataEvent: - #print("X: new data") + # print("X: new data") self._dataEvent.notifyAll() def _get_data(self): @@ -322,14 +322,14 @@ class SecondaryMonitor(Thread): returns something that looks like a packet, nothing is guaranted """ while True: - #self.logger.debug("data queue size is: {}".format(len(self._dataqueue))) + # self.logger.debug("data queue size is: {}".format(len(self._dataqueue))) ans = self._parser.find_first_packet(self._dataqueue[:]) if ans: self._dataqueue = ans[1] - #self.logger.debug("found packet of size {}".format(len(ans[0]))) + # self.logger.debug("found packet of size {}".format(len(ans[0]))) return ans[0] else: - #self.logger.debug("Could not find packet in received data") + # self.logger.debug("Could not find packet in received data") tmp = self._s_secondary.recv(1024) self._dataqueue += tmp