From 282cb0a59aeb4477168304f5505b7288cdc1441d Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Fri, 12 May 2023 14:00:18 -0500 Subject: [PATCH 1/8] Firmware 5.9 parsing is added. --- .history/urx/urrtmon_20230512131439.py | 403 +++++++++++++++++++++++++ .history/urx/urrtmon_20230512135935.py | 403 +++++++++++++++++++++++++ urx/urrtmon.py | 44 ++- 3 files changed, 848 insertions(+), 2 deletions(-) create mode 100644 .history/urx/urrtmon_20230512131439.py create mode 100644 .history/urx/urrtmon_20230512135935.py diff --git a/.history/urx/urrtmon_20230512131439.py b/.history/urx/urrtmon_20230512131439.py new file mode 100644 index 0000000..edebcca --- /dev/null +++ b/.history/urx/urrtmon_20230512131439.py @@ -0,0 +1,403 @@ +''' +Module for implementing a UR controller real-time monitor over socket port 30003. +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 +Parsing for Firmware 5.9 is added by Byeongdu Lee +''' +import logging +import socket +import struct +import time +import threading +from copy import deepcopy + +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): + + # Struct for revision of the UR controller giving 692 bytes + rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') + + # for revision of the UR controller giving 540 byte. Here TCP + # pose is not included! + rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') + + rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') + rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') + + def __init__(self, urHost, urFirm=None): + threading.Thread.__init__(self) + self.logger = logging.getLogger(self.__class__.__name__) + self.daemon = True + self._stop_event = True + self._dataEvent = threading.Condition() + self._dataAccess = threading.Lock() + self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + self._urHost = urHost + self.urFirm = urFirm + # Package data variables + self._timestamp = None + self._ctrlTimestamp = None + self._qActual = None + self._qTarget = None + self._tcp = None + self._tcp_force = None + self._joint_temperature = None + self._joint_voltage = None + self._joint_current = None + self._main_voltage = None + self._robot_voltage = None + self._robot_current = None + self._qdTarget = None + self._qddTarget = None + self._iTarget = None + self._mTarget = None + self._qdActual = None + self._tcp_speed = None + self._tcp = None + self._robot_mode = None + self._joint_modes = None + self._digital_outputs = None + self._program_state = None + self._safety_status = None + + self.__recvTime = 0 + self._last_ctrl_ts = 0 + # self._last_ts = 0 + self._buffering = False + self._buffer_lock = threading.Lock() + self._buffer = [] + self._csys = None + self._csys_lock = threading.Lock() + + def set_csys(self, csys): + with self._csys_lock: + self._csys = csys + + def __recv_bytes(self, nBytes): + ''' Facility method for receiving exactly "nBytes" bytes from + the robot connector socket.''' + # Record the time of arrival of the first of the stream block + recvTime = 0 + pkg = b'' + while len(pkg) < nBytes: + pkg += self._rtSock.recv(nBytes - len(pkg)) + if recvTime == 0: + recvTime = time.time() + self.__recvTime = recvTime + return pkg + + def wait(self): + with self._dataEvent: + self._dataEvent.wait() + + def q_actual(self, wait=False, timestamp=False): + """ Get the actual joint position vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qActual + else: + return self._qActual + getActual = q_actual + + def qd_actual(self, wait=False, timestamp=False): + """ Get the actual joint velocity vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qdActual + else: + return self._qdActual + + def q_target(self, wait=False, timestamp=False): + """ Get the target joint position vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qTarget + else: + return self._qTarget + getTarget = q_target + + def tcp_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): + """ Return the tool pose values.""" + if wait: + self.wait() + with self._dataAccess: + tcf = self._tcp + if ctrlTimestamp or timestamp: + ret = [tcf] + if timestamp: + ret.insert(-1, self._timestamp) + if ctrlTimestamp: + ret.insert(-1, self._ctrlTimestamp) + return ret + else: + return tcf + getTCP = tcp_pose + + def tcp_force(self, wait=False, timestamp=False): + """ Get the tool force. The returned tool force is a + six-vector of three forces and three moments.""" + if wait: + self.wait() + with self._dataAccess: + # tcf = self._fwkin(self._qActual) + tcp_force = self._tcp_force + if timestamp: + return self._timestamp, tcp_force + else: + return tcp_force + getTCPForce = tcp_force + + def joint_temperature(self, wait=False, timestamp=False): + """ Get the joint temperature.""" + if wait: + self.wait() + with self._dataAccess: + joint_temperature = self._joint_temperature + if timestamp: + return self._timestamp, joint_temperature + else: + return joint_temperature + getJOINTTemperature = joint_temperature + + def joint_voltage(self, wait=False, timestamp=False): + """ Get the joint voltage.""" + if wait: + self.wait() + with self._dataAccess: + joint_voltage = self._joint_voltage + if timestamp: + return self._timestamp, joint_voltage + else: + return joint_voltage + getJOINTVoltage = joint_voltage + + def joint_current(self, wait=False, timestamp=False): + """ Get the joint current.""" + if wait: + self.wait() + with self._dataAccess: + joint_current = self._joint_current + if timestamp: + return self._timestamp, joint_current + else: + return joint_current + getJOINTCurrent = joint_current + + def main_voltage(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Main voltage.""" + if wait: + self.wait() + with self._dataAccess: + main_voltage = self._main_voltage + if timestamp: + return self._timestamp, main_voltage + else: + return main_voltage + getMAINVoltage = main_voltage + + def robot_voltage(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Robot voltage (48V).""" + if wait: + self.wait() + with self._dataAccess: + robot_voltage = self._robot_voltage + if timestamp: + return self._timestamp, robot_voltage + else: + return robot_voltage + getROBOTVoltage = robot_voltage + + def robot_current(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Robot current.""" + if wait: + self.wait() + with self._dataAccess: + robot_current = self._robot_current + if timestamp: + return self._timestamp, robot_current + else: + return robot_current + getROBOTCurrent = robot_current + + def __recv_rt_data(self): + head = self.__recv_bytes(4) + # Record the timestamp for this logical package + timestamp = self.__recvTime + pkgsize = struct.unpack('>i', head)[0] + self.logger.debug( + 'Received header telling that package is %s bytes long', + pkgsize) + payload = self.__recv_bytes(pkgsize - 4) + if self.urFirm is not None: + if self.urFirm == 5.1: + unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) + if self.urFirm == 5.9: + unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) + else: + if pkgsize >= 692: + unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) + elif pkgsize >= 540: + unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) + else: + self.logger.warning( + 'Error, Received packet of length smaller than 540: %s ', + pkgsize) + return + + + with self._dataAccess: + self._timestamp = timestamp + # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? + # if (self._timestamp - self._last_ts) > 0.010: + # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) + # self._last_ts = self._timestamp + self._ctrlTimestamp = 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 ", + self._ctrlTimestamp - self._last_ctrl_ts) + self._last_ctrl_ts = self._ctrlTimestamp + self._qActual = np.array(unp[31:37]) + self._qdActual = np.array(unp[37:43]) + self._qTarget = np.array(unp[1:7]) + self._tcp_force = np.array(unp[67:73]) + self._tcp = np.array(unp[73:79]) + self._joint_current = np.array(unp[43:49]) + if self.urFirm >= 3.1: + self._joint_temperature = np.array(unp[86:92]) + self._joint_voltage = np.array(unp[124:130]) + self._main_voltage = unp[121] + self._robot_voltage = unp[122] + self._robot_current = unp[123] + + if self.urFirm>= 5.9: + self._qdTarget = np.array(unp[7:13]) + self._qddTarget = np.array(unp[13:19]) + self._iTarget = np.array(unp[19:25]) + self._mTarget = np.array(unp[25:31]) + self._tcp_speed = np.array(unp[61:67]) + self._joint_current = np.array(unp[49:55]) + self._joint_voltage = np.array(unp[124:130]) + self._robot_mode = unp[94] + self._joint_modes = np.array(unp[95:101]) + self._digital_outputs = unp[130] + self._program_state = unp[131] + self._safety_status = unp[138] + + if self._csys: + with self._csys_lock: + # might be a godd idea to remove dependancy on m3d + tcp = self._csys.inverse * m3d.Transform(self._tcp) + self._tcp = tcp.pose_vector + if self._buffering: + with self._buffer_lock: + self._buffer.append( + (self._timestamp, + self._ctrlTimestamp, + self._tcp, + self._qActual)) # FIXME use named arrays of allow to configure what data to buffer + + with self._dataEvent: + self._dataEvent.notifyAll() + + def start_buffering(self): + """ + start buffering all data from controller + """ + self._buffer = [] + self._buffering = True + + def stop_buffering(self): + self._buffering = False + + def try_pop_buffer(self): + """ + return oldest value in buffer + """ + with self._buffer_lock: + if len(self._buffer) > 0: + return self._buffer.pop(0) + else: + return None + + def pop_buffer(self): + """ + return oldest value in buffer + """ + while True: + with self._buffer_lock: + if len(self._buffer) > 0: + return self._buffer.pop(0) + time.sleep(0.001) + + def get_buffer(self): + """ + return a copy of the entire buffer + """ + with self._buffer_lock: + return deepcopy(self._buffer) + + def get_all_data(self, wait=True): + """ + return all data parsed from robot as a dict + """ + if wait: + self.wait() + with self._dataAccess: + return dict( + timestamp=self._timestamp, + ctrltimestamp=self._ctrlTimestamp, + qActual=self._qActual, + qTarget=self._qTarget, + qdActual=self._qdActual, + qdTarget=self._qdTarget, + tcp=self._tcp, + tcp_force=self._tcp_force, + tcp_speed=self._tcp_speed, + joint_temperature=self._joint_temperature, + joint_voltage=self._joint_voltage, + joint_current=self._joint_current, + joint_modes=self._joint_modes, + robot_modes=self._robot_mode, + main_voltage=self._main_voltage, + robot_voltage=self._robot_voltage, + robot_current=self._robot_current, + digital_outputs=self._digital_outputs, + program_state=self._program_state, + safety_status=self._safety_status) + getALLData = get_all_data + + def stop(self): + # print(self.__class__.__name__+': Stopping') + self._stop_event = True + + def close(self): + self.stop() + self.join() + + def run(self): + self._stop_event = False + self._rtSock.connect((self._urHost, 30003)) + while not self._stop_event: + self.__recv_rt_data() + self._rtSock.close() diff --git a/.history/urx/urrtmon_20230512135935.py b/.history/urx/urrtmon_20230512135935.py new file mode 100644 index 0000000..4131c88 --- /dev/null +++ b/.history/urx/urrtmon_20230512135935.py @@ -0,0 +1,403 @@ +''' +Module for implementing a UR controller real-time monitor over socket port 30003. +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 +Parsing for Firmware 5.9 is added by Byeongdu Lee +''' +import logging +import socket +import struct +import time +import threading +from copy import deepcopy + +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): + + # Struct for revision of the UR controller giving 692 bytes + rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') + + # for revision of the UR controller giving 540 byte. Here TCP + # pose is not included! + rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') + + rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') + rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') + + def __init__(self, urHost, urFirm=None): + threading.Thread.__init__(self) + self.logger = logging.getLogger(self.__class__.__name__) + self.daemon = True + self._stop_event = True + self._dataEvent = threading.Condition() + self._dataAccess = threading.Lock() + self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + self._urHost = urHost + self.urFirm = urFirm + # Package data variables + self._timestamp = None + self._ctrlTimestamp = None + self._qActual = None + self._qTarget = None + self._tcp = None + self._tcp_force = None + self._joint_temperature = None + self._joint_voltage = None + self._joint_current = None + self._main_voltage = None + self._robot_voltage = None + self._robot_current = None + self._qdTarget = None + self._qddTarget = None + self._iTarget = None + self._mTarget = None + self._qdActual = None + self._tcp_speed = None + self._tcp = None + self._robot_mode = None + self._joint_modes = None + self._digital_outputs = None + self._program_state = None + self._safety_status = None + + self.__recvTime = 0 + self._last_ctrl_ts = 0 + # self._last_ts = 0 + self._buffering = False + self._buffer_lock = threading.Lock() + self._buffer = [] + self._csys = None + self._csys_lock = threading.Lock() + + def set_csys(self, csys): + with self._csys_lock: + self._csys = csys + + def __recv_bytes(self, nBytes): + ''' Facility method for receiving exactly "nBytes" bytes from + the robot connector socket.''' + # Record the time of arrival of the first of the stream block + recvTime = 0 + pkg = b'' + while len(pkg) < nBytes: + pkg += self._rtSock.recv(nBytes - len(pkg)) + if recvTime == 0: + recvTime = time.time() + self.__recvTime = recvTime + return pkg + + def wait(self): + with self._dataEvent: + self._dataEvent.wait() + + def q_actual(self, wait=False, timestamp=False): + """ Get the actual joint position vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qActual + else: + return self._qActual + getActual = q_actual + + def qd_actual(self, wait=False, timestamp=False): + """ Get the actual joint velocity vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qdActual + else: + return self._qdActual + + def q_target(self, wait=False, timestamp=False): + """ Get the target joint position vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qTarget + else: + return self._qTarget + getTarget = q_target + + def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): + """ Return the tool pose values.""" + if wait: + self.wait() + with self._dataAccess: + tcf = self._tcp + if ctrlTimestamp or timestamp: + ret = [tcf] + if timestamp: + ret.insert(-1, self._timestamp) + if ctrlTimestamp: + ret.insert(-1, self._ctrlTimestamp) + return ret + else: + return tcf + getTCF = tcf_pose + + def tcf_force(self, wait=False, timestamp=False): + """ Get the tool force. The returned tool force is a + six-vector of three forces and three moments.""" + if wait: + self.wait() + with self._dataAccess: + # tcf = self._fwkin(self._qActual) + tcf_force = self._tcp_force + if timestamp: + return self._timestamp, tcf_force + else: + return tcf_force + getTCFForce = tcf_force + + def joint_temperature(self, wait=False, timestamp=False): + """ Get the joint temperature.""" + if wait: + self.wait() + with self._dataAccess: + joint_temperature = self._joint_temperature + if timestamp: + return self._timestamp, joint_temperature + else: + return joint_temperature + getJOINTTemperature = joint_temperature + + def joint_voltage(self, wait=False, timestamp=False): + """ Get the joint voltage.""" + if wait: + self.wait() + with self._dataAccess: + joint_voltage = self._joint_voltage + if timestamp: + return self._timestamp, joint_voltage + else: + return joint_voltage + getJOINTVoltage = joint_voltage + + def joint_current(self, wait=False, timestamp=False): + """ Get the joint current.""" + if wait: + self.wait() + with self._dataAccess: + joint_current = self._joint_current + if timestamp: + return self._timestamp, joint_current + else: + return joint_current + getJOINTCurrent = joint_current + + def main_voltage(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Main voltage.""" + if wait: + self.wait() + with self._dataAccess: + main_voltage = self._main_voltage + if timestamp: + return self._timestamp, main_voltage + else: + return main_voltage + getMAINVoltage = main_voltage + + def robot_voltage(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Robot voltage (48V).""" + if wait: + self.wait() + with self._dataAccess: + robot_voltage = self._robot_voltage + if timestamp: + return self._timestamp, robot_voltage + else: + return robot_voltage + getROBOTVoltage = robot_voltage + + def robot_current(self, wait=False, timestamp=False): + """ Get the Safety Control Board: Robot current.""" + if wait: + self.wait() + with self._dataAccess: + robot_current = self._robot_current + if timestamp: + return self._timestamp, robot_current + else: + return robot_current + getROBOTCurrent = robot_current + + def __recv_rt_data(self): + head = self.__recv_bytes(4) + # Record the timestamp for this logical package + timestamp = self.__recvTime + pkgsize = struct.unpack('>i', head)[0] + self.logger.debug( + 'Received header telling that package is %s bytes long', + pkgsize) + payload = self.__recv_bytes(pkgsize - 4) + if self.urFirm is not None: + if self.urFirm == 5.1: + unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) + if self.urFirm == 5.9: + unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) + else: + if pkgsize >= 692: + unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) + elif pkgsize >= 540: + unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) + else: + self.logger.warning( + 'Error, Received packet of length smaller than 540: %s ', + pkgsize) + return + + + with self._dataAccess: + self._timestamp = timestamp + # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? + # if (self._timestamp - self._last_ts) > 0.010: + # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) + # 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 ", + self._ctrlTimestamp - self._last_ctrl_ts) + self._last_ctrl_ts = self._ctrlTimestamp + self._qActual = np.array(unp[31:37]) + self._qdActual = np.array(unp[37:43]) + self._qTarget = np.array(unp[1:7]) + self._tcp_force = np.array(unp[67:73]) + self._tcp = np.array(unp[73:79]) + self._joint_current = np.array(unp[43:49]) + if self.urFirm >= 3.1: + self._joint_temperature = np.array(unp[86:92]) + self._joint_voltage = np.array(unp[124:130]) + self._main_voltage = unp[121] + self._robot_voltage = unp[122] + self._robot_current = unp[123] + + if self.urFirm>= 5.9: + self._qdTarget = np.array(unp[7:13]) + self._qddTarget = np.array(unp[13:19]) + self._iTarget = np.array(unp[19:25]) + self._mTarget = np.array(unp[25:31]) + self._tcp_speed = np.array(unp[61:67]) + self._joint_current = np.array(unp[49:55]) + self._joint_voltage = np.array(unp[124:130]) + self._robot_mode = unp[94] + self._joint_modes = np.array(unp[95:101]) + self._digital_outputs = unp[130] + self._program_state = unp[131] + self._safety_status = unp[138] + + if self._csys: + with self._csys_lock: + # might be a godd idea to remove dependancy on m3d + tcp = self._csys.inverse * m3d.Transform(self._tcp) + self._tcp = tcp.pose_vector + if self._buffering: + with self._buffer_lock: + self._buffer.append( + (self._timestamp, + self._ctrlTimestamp, + self._tcp, + self._qActual)) # FIXME use named arrays of allow to configure what data to buffer + + with self._dataEvent: + self._dataEvent.notifyAll() + + def start_buffering(self): + """ + start buffering all data from controller + """ + self._buffer = [] + self._buffering = True + + def stop_buffering(self): + self._buffering = False + + def try_pop_buffer(self): + """ + return oldest value in buffer + """ + with self._buffer_lock: + if len(self._buffer) > 0: + return self._buffer.pop(0) + else: + return None + + def pop_buffer(self): + """ + return oldest value in buffer + """ + while True: + with self._buffer_lock: + if len(self._buffer) > 0: + return self._buffer.pop(0) + time.sleep(0.001) + + def get_buffer(self): + """ + return a copy of the entire buffer + """ + with self._buffer_lock: + return deepcopy(self._buffer) + + def get_all_data(self, wait=True): + """ + return all data parsed from robot as a dict + """ + if wait: + self.wait() + with self._dataAccess: + return dict( + timestamp=self._timestamp, + ctrltimestamp=self._ctrlTimestamp, + qActual=self._qActual, + qTarget=self._qTarget, + qdActual=self._qdActual, + qdTarget=self._qdTarget, + tcp=self._tcp, + tcp_force=self._tcp_force, + tcp_speed=self._tcp_speed, + joint_temperature=self._joint_temperature, + joint_voltage=self._joint_voltage, + joint_current=self._joint_current, + joint_modes=self._joint_modes, + robot_modes=self._robot_mode, + main_voltage=self._main_voltage, + robot_voltage=self._robot_voltage, + robot_current=self._robot_current, + digital_outputs=self._digital_outputs, + program_state=self._program_state, + safety_status=self._safety_status) + getALLData = get_all_data + + def stop(self): + # print(self.__class__.__name__+': Stopping') + self._stop_event = True + + def close(self): + self.stop() + self.join() + + def run(self): + self._stop_event = False + self._rtSock.connect((self._urHost, 30003)) + while not self._stop_event: + self.__recv_rt_data() + self._rtSock.close() diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 096bbec..4131c88 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -1,8 +1,10 @@ ''' Module for implementing a UR controller real-time monitor over socket port 30003. 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. +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 +Parsing for Firmware 5.9 is added by Byeongdu Lee ''' import logging import socket @@ -31,6 +33,7 @@ class URRTMonitor(threading.Thread): rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') + rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') def __init__(self, urHost, urFirm=None): threading.Thread.__init__(self) @@ -56,6 +59,19 @@ class URRTMonitor(threading.Thread): self._main_voltage = None self._robot_voltage = None self._robot_current = None + self._qdTarget = None + self._qddTarget = None + self._iTarget = None + self._mTarget = None + self._qdActual = None + self._tcp_speed = None + self._tcp = None + self._robot_mode = None + self._joint_modes = None + self._digital_outputs = None + self._program_state = None + self._safety_status = None + self.__recvTime = 0 self._last_ctrl_ts = 0 # self._last_ts = 0 @@ -233,6 +249,8 @@ class URRTMonitor(threading.Thread): if self.urFirm is not None: if self.urFirm == 5.1: unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) + if self.urFirm == 5.9: + unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) else: if pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) @@ -272,6 +290,20 @@ class URRTMonitor(threading.Thread): self._robot_voltage = unp[122] self._robot_current = unp[123] + if self.urFirm>= 5.9: + self._qdTarget = np.array(unp[7:13]) + self._qddTarget = np.array(unp[13:19]) + self._iTarget = np.array(unp[19:25]) + self._mTarget = np.array(unp[25:31]) + self._tcp_speed = np.array(unp[61:67]) + self._joint_current = np.array(unp[49:55]) + self._joint_voltage = np.array(unp[124:130]) + self._robot_mode = unp[94] + self._joint_modes = np.array(unp[95:101]) + self._digital_outputs = unp[130] + self._program_state = unp[131] + self._safety_status = unp[138] + if self._csys: with self._csys_lock: # might be a godd idea to remove dependancy on m3d @@ -337,14 +369,22 @@ class URRTMonitor(threading.Thread): ctrltimestamp=self._ctrlTimestamp, qActual=self._qActual, qTarget=self._qTarget, + qdActual=self._qdActual, + qdTarget=self._qdTarget, tcp=self._tcp, tcp_force=self._tcp_force, + tcp_speed=self._tcp_speed, joint_temperature=self._joint_temperature, joint_voltage=self._joint_voltage, joint_current=self._joint_current, + joint_modes=self._joint_modes, + robot_modes=self._robot_mode, main_voltage=self._main_voltage, robot_voltage=self._robot_voltage, - robot_current=self._robot_current) + robot_current=self._robot_current, + digital_outputs=self._digital_outputs, + program_state=self._program_state, + safety_status=self._safety_status) getALLData = get_all_data def stop(self): From e6106cabc0df53cf5ca1bef116002de3358116ca Mon Sep 17 00:00:00 2001 From: Byeongdulee <45212115+Byeongdulee@users.noreply.github.com> Date: Fri, 12 May 2023 15:42:32 -0500 Subject: [PATCH 2/8] Delete .history/urx directory --- .history/urx/urrtmon_20230512131439.py | 403 ------------------------- .history/urx/urrtmon_20230512135935.py | 403 ------------------------- 2 files changed, 806 deletions(-) delete mode 100644 .history/urx/urrtmon_20230512131439.py delete mode 100644 .history/urx/urrtmon_20230512135935.py diff --git a/.history/urx/urrtmon_20230512131439.py b/.history/urx/urrtmon_20230512131439.py deleted file mode 100644 index edebcca..0000000 --- a/.history/urx/urrtmon_20230512131439.py +++ /dev/null @@ -1,403 +0,0 @@ -''' -Module for implementing a UR controller real-time monitor over socket port 30003. -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 -Parsing for Firmware 5.9 is added by Byeongdu Lee -''' -import logging -import socket -import struct -import time -import threading -from copy import deepcopy - -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): - - # Struct for revision of the UR controller giving 692 bytes - rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') - - # for revision of the UR controller giving 540 byte. Here TCP - # pose is not included! - rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') - - rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') - rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') - - def __init__(self, urHost, urFirm=None): - threading.Thread.__init__(self) - self.logger = logging.getLogger(self.__class__.__name__) - self.daemon = True - self._stop_event = True - self._dataEvent = threading.Condition() - self._dataAccess = threading.Lock() - self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) - self._urHost = urHost - self.urFirm = urFirm - # Package data variables - self._timestamp = None - self._ctrlTimestamp = None - self._qActual = None - self._qTarget = None - self._tcp = None - self._tcp_force = None - self._joint_temperature = None - self._joint_voltage = None - self._joint_current = None - self._main_voltage = None - self._robot_voltage = None - self._robot_current = None - self._qdTarget = None - self._qddTarget = None - self._iTarget = None - self._mTarget = None - self._qdActual = None - self._tcp_speed = None - self._tcp = None - self._robot_mode = None - self._joint_modes = None - self._digital_outputs = None - self._program_state = None - self._safety_status = None - - self.__recvTime = 0 - self._last_ctrl_ts = 0 - # self._last_ts = 0 - self._buffering = False - self._buffer_lock = threading.Lock() - self._buffer = [] - self._csys = None - self._csys_lock = threading.Lock() - - def set_csys(self, csys): - with self._csys_lock: - self._csys = csys - - def __recv_bytes(self, nBytes): - ''' Facility method for receiving exactly "nBytes" bytes from - the robot connector socket.''' - # Record the time of arrival of the first of the stream block - recvTime = 0 - pkg = b'' - while len(pkg) < nBytes: - pkg += self._rtSock.recv(nBytes - len(pkg)) - if recvTime == 0: - recvTime = time.time() - self.__recvTime = recvTime - return pkg - - def wait(self): - with self._dataEvent: - self._dataEvent.wait() - - def q_actual(self, wait=False, timestamp=False): - """ Get the actual joint position vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qActual - else: - return self._qActual - getActual = q_actual - - def qd_actual(self, wait=False, timestamp=False): - """ Get the actual joint velocity vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qdActual - else: - return self._qdActual - - def q_target(self, wait=False, timestamp=False): - """ Get the target joint position vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qTarget - else: - return self._qTarget - getTarget = q_target - - def tcp_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): - """ Return the tool pose values.""" - if wait: - self.wait() - with self._dataAccess: - tcf = self._tcp - if ctrlTimestamp or timestamp: - ret = [tcf] - if timestamp: - ret.insert(-1, self._timestamp) - if ctrlTimestamp: - ret.insert(-1, self._ctrlTimestamp) - return ret - else: - return tcf - getTCP = tcp_pose - - def tcp_force(self, wait=False, timestamp=False): - """ Get the tool force. The returned tool force is a - six-vector of three forces and three moments.""" - if wait: - self.wait() - with self._dataAccess: - # tcf = self._fwkin(self._qActual) - tcp_force = self._tcp_force - if timestamp: - return self._timestamp, tcp_force - else: - return tcp_force - getTCPForce = tcp_force - - def joint_temperature(self, wait=False, timestamp=False): - """ Get the joint temperature.""" - if wait: - self.wait() - with self._dataAccess: - joint_temperature = self._joint_temperature - if timestamp: - return self._timestamp, joint_temperature - else: - return joint_temperature - getJOINTTemperature = joint_temperature - - def joint_voltage(self, wait=False, timestamp=False): - """ Get the joint voltage.""" - if wait: - self.wait() - with self._dataAccess: - joint_voltage = self._joint_voltage - if timestamp: - return self._timestamp, joint_voltage - else: - return joint_voltage - getJOINTVoltage = joint_voltage - - def joint_current(self, wait=False, timestamp=False): - """ Get the joint current.""" - if wait: - self.wait() - with self._dataAccess: - joint_current = self._joint_current - if timestamp: - return self._timestamp, joint_current - else: - return joint_current - getJOINTCurrent = joint_current - - def main_voltage(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Main voltage.""" - if wait: - self.wait() - with self._dataAccess: - main_voltage = self._main_voltage - if timestamp: - return self._timestamp, main_voltage - else: - return main_voltage - getMAINVoltage = main_voltage - - def robot_voltage(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Robot voltage (48V).""" - if wait: - self.wait() - with self._dataAccess: - robot_voltage = self._robot_voltage - if timestamp: - return self._timestamp, robot_voltage - else: - return robot_voltage - getROBOTVoltage = robot_voltage - - def robot_current(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Robot current.""" - if wait: - self.wait() - with self._dataAccess: - robot_current = self._robot_current - if timestamp: - return self._timestamp, robot_current - else: - return robot_current - getROBOTCurrent = robot_current - - def __recv_rt_data(self): - head = self.__recv_bytes(4) - # Record the timestamp for this logical package - timestamp = self.__recvTime - pkgsize = struct.unpack('>i', head)[0] - self.logger.debug( - 'Received header telling that package is %s bytes long', - pkgsize) - payload = self.__recv_bytes(pkgsize - 4) - if self.urFirm is not None: - if self.urFirm == 5.1: - unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) - if self.urFirm == 5.9: - unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) - else: - if pkgsize >= 692: - unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) - elif pkgsize >= 540: - unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) - else: - self.logger.warning( - 'Error, Received packet of length smaller than 540: %s ', - pkgsize) - return - - - with self._dataAccess: - self._timestamp = timestamp - # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? - # if (self._timestamp - self._last_ts) > 0.010: - # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) - # self._last_ts = self._timestamp - self._ctrlTimestamp = 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 ", - self._ctrlTimestamp - self._last_ctrl_ts) - self._last_ctrl_ts = self._ctrlTimestamp - self._qActual = np.array(unp[31:37]) - self._qdActual = np.array(unp[37:43]) - self._qTarget = np.array(unp[1:7]) - self._tcp_force = np.array(unp[67:73]) - self._tcp = np.array(unp[73:79]) - self._joint_current = np.array(unp[43:49]) - if self.urFirm >= 3.1: - self._joint_temperature = np.array(unp[86:92]) - self._joint_voltage = np.array(unp[124:130]) - self._main_voltage = unp[121] - self._robot_voltage = unp[122] - self._robot_current = unp[123] - - if self.urFirm>= 5.9: - self._qdTarget = np.array(unp[7:13]) - self._qddTarget = np.array(unp[13:19]) - self._iTarget = np.array(unp[19:25]) - self._mTarget = np.array(unp[25:31]) - self._tcp_speed = np.array(unp[61:67]) - self._joint_current = np.array(unp[49:55]) - self._joint_voltage = np.array(unp[124:130]) - self._robot_mode = unp[94] - self._joint_modes = np.array(unp[95:101]) - self._digital_outputs = unp[130] - self._program_state = unp[131] - self._safety_status = unp[138] - - if self._csys: - with self._csys_lock: - # might be a godd idea to remove dependancy on m3d - tcp = self._csys.inverse * m3d.Transform(self._tcp) - self._tcp = tcp.pose_vector - if self._buffering: - with self._buffer_lock: - self._buffer.append( - (self._timestamp, - self._ctrlTimestamp, - self._tcp, - self._qActual)) # FIXME use named arrays of allow to configure what data to buffer - - with self._dataEvent: - self._dataEvent.notifyAll() - - def start_buffering(self): - """ - start buffering all data from controller - """ - self._buffer = [] - self._buffering = True - - def stop_buffering(self): - self._buffering = False - - def try_pop_buffer(self): - """ - return oldest value in buffer - """ - with self._buffer_lock: - if len(self._buffer) > 0: - return self._buffer.pop(0) - else: - return None - - def pop_buffer(self): - """ - return oldest value in buffer - """ - while True: - with self._buffer_lock: - if len(self._buffer) > 0: - return self._buffer.pop(0) - time.sleep(0.001) - - def get_buffer(self): - """ - return a copy of the entire buffer - """ - with self._buffer_lock: - return deepcopy(self._buffer) - - def get_all_data(self, wait=True): - """ - return all data parsed from robot as a dict - """ - if wait: - self.wait() - with self._dataAccess: - return dict( - timestamp=self._timestamp, - ctrltimestamp=self._ctrlTimestamp, - qActual=self._qActual, - qTarget=self._qTarget, - qdActual=self._qdActual, - qdTarget=self._qdTarget, - tcp=self._tcp, - tcp_force=self._tcp_force, - tcp_speed=self._tcp_speed, - joint_temperature=self._joint_temperature, - joint_voltage=self._joint_voltage, - joint_current=self._joint_current, - joint_modes=self._joint_modes, - robot_modes=self._robot_mode, - main_voltage=self._main_voltage, - robot_voltage=self._robot_voltage, - robot_current=self._robot_current, - digital_outputs=self._digital_outputs, - program_state=self._program_state, - safety_status=self._safety_status) - getALLData = get_all_data - - def stop(self): - # print(self.__class__.__name__+': Stopping') - self._stop_event = True - - def close(self): - self.stop() - self.join() - - def run(self): - self._stop_event = False - self._rtSock.connect((self._urHost, 30003)) - while not self._stop_event: - self.__recv_rt_data() - self._rtSock.close() diff --git a/.history/urx/urrtmon_20230512135935.py b/.history/urx/urrtmon_20230512135935.py deleted file mode 100644 index 4131c88..0000000 --- a/.history/urx/urrtmon_20230512135935.py +++ /dev/null @@ -1,403 +0,0 @@ -''' -Module for implementing a UR controller real-time monitor over socket port 30003. -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 -Parsing for Firmware 5.9 is added by Byeongdu Lee -''' -import logging -import socket -import struct -import time -import threading -from copy import deepcopy - -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): - - # Struct for revision of the UR controller giving 692 bytes - rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') - - # for revision of the UR controller giving 540 byte. Here TCP - # pose is not included! - rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') - - rtstruct5_1 = struct.Struct('>d1d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d') - rtstruct5_9 = struct.Struct('>d6d6d6d6d6d6d6d6d6d6d6d6d6d6d1d6d1d1d1d6d1d6d3d6d1d1d1d1d1d1d1d6d1d1d3d3d1d') - - def __init__(self, urHost, urFirm=None): - threading.Thread.__init__(self) - self.logger = logging.getLogger(self.__class__.__name__) - self.daemon = True - self._stop_event = True - self._dataEvent = threading.Condition() - self._dataAccess = threading.Lock() - self._rtSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self._rtSock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) - self._urHost = urHost - self.urFirm = urFirm - # Package data variables - self._timestamp = None - self._ctrlTimestamp = None - self._qActual = None - self._qTarget = None - self._tcp = None - self._tcp_force = None - self._joint_temperature = None - self._joint_voltage = None - self._joint_current = None - self._main_voltage = None - self._robot_voltage = None - self._robot_current = None - self._qdTarget = None - self._qddTarget = None - self._iTarget = None - self._mTarget = None - self._qdActual = None - self._tcp_speed = None - self._tcp = None - self._robot_mode = None - self._joint_modes = None - self._digital_outputs = None - self._program_state = None - self._safety_status = None - - self.__recvTime = 0 - self._last_ctrl_ts = 0 - # self._last_ts = 0 - self._buffering = False - self._buffer_lock = threading.Lock() - self._buffer = [] - self._csys = None - self._csys_lock = threading.Lock() - - def set_csys(self, csys): - with self._csys_lock: - self._csys = csys - - def __recv_bytes(self, nBytes): - ''' Facility method for receiving exactly "nBytes" bytes from - the robot connector socket.''' - # Record the time of arrival of the first of the stream block - recvTime = 0 - pkg = b'' - while len(pkg) < nBytes: - pkg += self._rtSock.recv(nBytes - len(pkg)) - if recvTime == 0: - recvTime = time.time() - self.__recvTime = recvTime - return pkg - - def wait(self): - with self._dataEvent: - self._dataEvent.wait() - - def q_actual(self, wait=False, timestamp=False): - """ Get the actual joint position vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qActual - else: - return self._qActual - getActual = q_actual - - def qd_actual(self, wait=False, timestamp=False): - """ Get the actual joint velocity vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qdActual - else: - return self._qdActual - - def q_target(self, wait=False, timestamp=False): - """ Get the target joint position vector.""" - if wait: - self.wait() - with self._dataAccess: - if timestamp: - return self._timestamp, self._qTarget - else: - return self._qTarget - getTarget = q_target - - def tcf_pose(self, wait=False, timestamp=False, ctrlTimestamp=False): - """ Return the tool pose values.""" - if wait: - self.wait() - with self._dataAccess: - tcf = self._tcp - if ctrlTimestamp or timestamp: - ret = [tcf] - if timestamp: - ret.insert(-1, self._timestamp) - if ctrlTimestamp: - ret.insert(-1, self._ctrlTimestamp) - return ret - else: - return tcf - getTCF = tcf_pose - - def tcf_force(self, wait=False, timestamp=False): - """ Get the tool force. The returned tool force is a - six-vector of three forces and three moments.""" - if wait: - self.wait() - with self._dataAccess: - # tcf = self._fwkin(self._qActual) - tcf_force = self._tcp_force - if timestamp: - return self._timestamp, tcf_force - else: - return tcf_force - getTCFForce = tcf_force - - def joint_temperature(self, wait=False, timestamp=False): - """ Get the joint temperature.""" - if wait: - self.wait() - with self._dataAccess: - joint_temperature = self._joint_temperature - if timestamp: - return self._timestamp, joint_temperature - else: - return joint_temperature - getJOINTTemperature = joint_temperature - - def joint_voltage(self, wait=False, timestamp=False): - """ Get the joint voltage.""" - if wait: - self.wait() - with self._dataAccess: - joint_voltage = self._joint_voltage - if timestamp: - return self._timestamp, joint_voltage - else: - return joint_voltage - getJOINTVoltage = joint_voltage - - def joint_current(self, wait=False, timestamp=False): - """ Get the joint current.""" - if wait: - self.wait() - with self._dataAccess: - joint_current = self._joint_current - if timestamp: - return self._timestamp, joint_current - else: - return joint_current - getJOINTCurrent = joint_current - - def main_voltage(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Main voltage.""" - if wait: - self.wait() - with self._dataAccess: - main_voltage = self._main_voltage - if timestamp: - return self._timestamp, main_voltage - else: - return main_voltage - getMAINVoltage = main_voltage - - def robot_voltage(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Robot voltage (48V).""" - if wait: - self.wait() - with self._dataAccess: - robot_voltage = self._robot_voltage - if timestamp: - return self._timestamp, robot_voltage - else: - return robot_voltage - getROBOTVoltage = robot_voltage - - def robot_current(self, wait=False, timestamp=False): - """ Get the Safety Control Board: Robot current.""" - if wait: - self.wait() - with self._dataAccess: - robot_current = self._robot_current - if timestamp: - return self._timestamp, robot_current - else: - return robot_current - getROBOTCurrent = robot_current - - def __recv_rt_data(self): - head = self.__recv_bytes(4) - # Record the timestamp for this logical package - timestamp = self.__recvTime - pkgsize = struct.unpack('>i', head)[0] - self.logger.debug( - 'Received header telling that package is %s bytes long', - pkgsize) - payload = self.__recv_bytes(pkgsize - 4) - if self.urFirm is not None: - if self.urFirm == 5.1: - unp = self.rtstruct5_1.unpack(payload[:self.rtstruct5_1.size]) - if self.urFirm == 5.9: - unp = self.rtstruct5_9.unpack(payload[:self.rtstruct5_9.size]) - else: - if pkgsize >= 692: - unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) - elif pkgsize >= 540: - unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) - else: - self.logger.warning( - 'Error, Received packet of length smaller than 540: %s ', - pkgsize) - return - - - with self._dataAccess: - self._timestamp = timestamp - # it seems that packet often arrives packed as two... maybe TCP_NODELAY is not set on UR controller?? - # if (self._timestamp - self._last_ts) > 0.010: - # self.logger.warning("Error the we did not receive a packet for {}s ".format( self._timestamp - self._last_ts)) - # 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 ", - self._ctrlTimestamp - self._last_ctrl_ts) - self._last_ctrl_ts = self._ctrlTimestamp - self._qActual = np.array(unp[31:37]) - self._qdActual = np.array(unp[37:43]) - self._qTarget = np.array(unp[1:7]) - self._tcp_force = np.array(unp[67:73]) - self._tcp = np.array(unp[73:79]) - self._joint_current = np.array(unp[43:49]) - if self.urFirm >= 3.1: - self._joint_temperature = np.array(unp[86:92]) - self._joint_voltage = np.array(unp[124:130]) - self._main_voltage = unp[121] - self._robot_voltage = unp[122] - self._robot_current = unp[123] - - if self.urFirm>= 5.9: - self._qdTarget = np.array(unp[7:13]) - self._qddTarget = np.array(unp[13:19]) - self._iTarget = np.array(unp[19:25]) - self._mTarget = np.array(unp[25:31]) - self._tcp_speed = np.array(unp[61:67]) - self._joint_current = np.array(unp[49:55]) - self._joint_voltage = np.array(unp[124:130]) - self._robot_mode = unp[94] - self._joint_modes = np.array(unp[95:101]) - self._digital_outputs = unp[130] - self._program_state = unp[131] - self._safety_status = unp[138] - - if self._csys: - with self._csys_lock: - # might be a godd idea to remove dependancy on m3d - tcp = self._csys.inverse * m3d.Transform(self._tcp) - self._tcp = tcp.pose_vector - if self._buffering: - with self._buffer_lock: - self._buffer.append( - (self._timestamp, - self._ctrlTimestamp, - self._tcp, - self._qActual)) # FIXME use named arrays of allow to configure what data to buffer - - with self._dataEvent: - self._dataEvent.notifyAll() - - def start_buffering(self): - """ - start buffering all data from controller - """ - self._buffer = [] - self._buffering = True - - def stop_buffering(self): - self._buffering = False - - def try_pop_buffer(self): - """ - return oldest value in buffer - """ - with self._buffer_lock: - if len(self._buffer) > 0: - return self._buffer.pop(0) - else: - return None - - def pop_buffer(self): - """ - return oldest value in buffer - """ - while True: - with self._buffer_lock: - if len(self._buffer) > 0: - return self._buffer.pop(0) - time.sleep(0.001) - - def get_buffer(self): - """ - return a copy of the entire buffer - """ - with self._buffer_lock: - return deepcopy(self._buffer) - - def get_all_data(self, wait=True): - """ - return all data parsed from robot as a dict - """ - if wait: - self.wait() - with self._dataAccess: - return dict( - timestamp=self._timestamp, - ctrltimestamp=self._ctrlTimestamp, - qActual=self._qActual, - qTarget=self._qTarget, - qdActual=self._qdActual, - qdTarget=self._qdTarget, - tcp=self._tcp, - tcp_force=self._tcp_force, - tcp_speed=self._tcp_speed, - joint_temperature=self._joint_temperature, - joint_voltage=self._joint_voltage, - joint_current=self._joint_current, - joint_modes=self._joint_modes, - robot_modes=self._robot_mode, - main_voltage=self._main_voltage, - robot_voltage=self._robot_voltage, - robot_current=self._robot_current, - digital_outputs=self._digital_outputs, - program_state=self._program_state, - safety_status=self._safety_status) - getALLData = get_all_data - - def stop(self): - # print(self.__class__.__name__+': Stopping') - self._stop_event = True - - def close(self): - self.stop() - self.join() - - def run(self): - self._stop_event = False - self._rtSock.connect((self._urHost, 30003)) - while not self._stop_event: - self.__recv_rt_data() - self._rtSock.close() From 523a5804b3d8aefa60ef9a2a21c81885a5ec2a65 Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Fri, 12 May 2023 15:59:56 -0500 Subject: [PATCH 3/8] .history removed. --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 715a5c6..ef5a5a9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ docs/_* build .idea/ +.history/ *.pyc From 2cef8a7f2a5a2a9f4c4cfa775254bd39ed64d468 Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Thu, 15 Jun 2023 14:27:22 -0500 Subject: [PATCH 4/8] Added a function to read a variable from the pendant. --- urx/urscript.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/urx/urscript.py b/urx/urscript.py index a7d6cd7..2382cb3 100755 --- a/urx/urscript.py +++ b/urx/urscript.py @@ -142,6 +142,15 @@ class URScript(object): self.add_line_to_program(msg) self._sync() + def _socket_get_var2var(self, var, varout, socket_name, prefix = ''): + msg = "{}{} = socket_get_var(\"{}\",\"{}\")".format(prefix, varout, var, socket_name) + self.add_line_to_program(msg) + + def _socket_send_byte(self, byte, socket_name): + msg = "socket_send_byte(\"{}\",\"{}\")".format(str(byte), socket_name) # noqa + self.add_line_to_program(msg) + self._sync() + def _sync(self): msg = "sync()" self.add_line_to_program(msg) From 58e1a1dad4a1075bf08d9eb5aaf6e74674ce1d81 Mon Sep 17 00:00:00 2001 From: s12idb Date: Wed, 12 Jul 2023 12:56:47 -0500 Subject: [PATCH 5/8] Update .gitignore --- .gitignore | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.gitignore b/.gitignore index ef5a5a9..7536d70 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,9 @@ build .idea/ .history/ *.pyc +dist/urx-0.11.0-py3.10.egg +urx.egg-info/dependency_links.txt +urx.egg-info/PKG-INFO +urx.egg-info/requires.txt +urx.egg-info/SOURCES.txt +urx.egg-info/top_level.txt From 80e0c2e91714c3d95c22487a126e76621f022ed9 Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Tue, 25 Jul 2023 09:43:55 -0500 Subject: [PATCH 6/8] tool functions are added. --- .gitignore | 2 +- urx/urrobot.py | 21 +++++++++++++++++++++ urx/urscript.py | 24 ++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 7536d70..ff7ce8d 100644 --- a/.gitignore +++ b/.gitignore @@ -3,7 +3,7 @@ build .idea/ .history/ *.pyc -dist/urx-0.11.0-py3.10.egg +dist/*.egg urx.egg-info/dependency_links.txt urx.egg-info/PKG-INFO urx.egg-info/requires.txt diff --git a/urx/urrobot.py b/urx/urrobot.py index 3086fe7..56d124a 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -254,6 +254,27 @@ class URRobot(object): prog = "set_tool_voltage(%s)" % (val) self.send_program(prog) + def set_tool_digital_in(self, input_id): + """ + set voltage to be delivered to the tool, val is 0, 12 or 24 + """ + prog = "set_tool_digital_in(%s)" % (input_id) + self.send_program(prog) + + def set_tool_digital_out(self, input_id, signal_level): + """ + set voltage to be delivered to the tool, val is 0, 12 or 24 + """ + prog = "set_tool_digital_out(%s, %s)" % (input_id, signal_level) + self.send_program(prog) + + def set_tool_communication(self, enabled=True, baud_rate=9600, parity=0, stop_bits=1, rx_idle_chars=1.0, tx_idle_chars=3.5): + """ + set voltage to be delivered to the tool, val is 0, 12 or 24 + """ + prog = "set_tool_communication(%s, %s, %s, %s, %s, %s)" % (enabled, baud_rate, parity, stop_bits, rx_idle_chars, tx_idle_chars) + self.send_program(prog) + def _wait_for_move(self, target, threshold=None, timeout=5, joints=False): """ wait for a move to complete. Unfortunately there is no good way to know when a move has finished diff --git a/urx/urscript.py b/urx/urscript.py index 2382cb3..40b40ad 100755 --- a/urx/urscript.py +++ b/urx/urscript.py @@ -150,6 +150,30 @@ class URScript(object): msg = "socket_send_byte(\"{}\",\"{}\")".format(str(byte), socket_name) # noqa self.add_line_to_program(msg) self._sync() + + def _get_tool_digital_in(self, input_id): + assert(input_id in [0, 1]) + msg = "get_standard_digital_in({})".format(input_id) + self.add_line_to_program(msg) + + def _set_tool_digital_out(self, input_id, signal_level): + assert(input_id in [0, 1]) + assert(signal_level in [True, False]) + msg = "set_standard_digital_out({}, {})".format(input_id, signal_level) + self.add_line_to_program(msg) + + def _set_tool_communication(self, enabled=True, baud_rate=9600, parity=0, stop_bits=1, rx_idle_chars=1.5, tx_idle_chars=3.5): + # stop_bits:Thenumberofstopbits(int).Validvalues:1,2. + # rx_idle_chars:AmountofcharstheRXunitinthetoolshouldwaitbeforemarkingamessage asover/sendingittothePC(float).Validvalues:min=1.0max=40.0. + # tx_idle_chars: + # set_tool_communication(True,115200,1,2,1.0,3.5) + # :9600,19200,38400,57600,115200, 1000000,2000000,5000000 + assert(baud_rate in [9600,19200,38400,57600,115200, 1000000,2000000,5000000]) + assert(parity in [0, 1, 2]) + assert(stop_bits in [1, 2]) + assert(enabled in [True, False]) + msg = "set_tool_communication({}, {}, {}, {}, {}, {})".format(enabled, baud_rate, parity, stop_bits, rx_idle_chars, tx_idle_chars) + self.add_line_to_program(msg) def _sync(self): msg = "sync()" From 96ec07723646297c031810f7ebad3778db46f404 Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Wed, 26 Jul 2023 07:52:08 -0500 Subject: [PATCH 7/8] Update urrobot.py --- urx/urrobot.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index 56d124a..b990c8d 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -251,26 +251,28 @@ class URRobot(object): """ set voltage to be delivered to the tool, val is 0, 12 or 24 """ + assert(val in [0, 12, 24]) prog = "set_tool_voltage(%s)" % (val) self.send_program(prog) - def set_tool_digital_in(self, input_id): - """ - set voltage to be delivered to the tool, val is 0, 12 or 24 - """ - prog = "set_tool_digital_in(%s)" % (input_id) - self.send_program(prog) + # def get_tool_digital_in(self, input_id): + # """ + # get tool digitial input + # """ + # prog = "get_tool_digital_in(%s)" % (input_id) + # self.send_program(prog) def set_tool_digital_out(self, input_id, signal_level): """ - set voltage to be delivered to the tool, val is 0, 12 or 24 + set tool digital output """ + assert(input_id in [0, 1]) prog = "set_tool_digital_out(%s, %s)" % (input_id, signal_level) self.send_program(prog) def set_tool_communication(self, enabled=True, baud_rate=9600, parity=0, stop_bits=1, rx_idle_chars=1.0, tx_idle_chars=3.5): """ - set voltage to be delivered to the tool, val is 0, 12 or 24 + set tool RS485 communication protocol. """ prog = "set_tool_communication(%s, %s, %s, %s, %s, %s)" % (enabled, baud_rate, parity, stop_bits, rx_idle_chars, tx_idle_chars) self.send_program(prog) From dd25611844b155d9f6f149f4d1d6bec4729bd9ea Mon Sep 17 00:00:00 2001 From: Byeongdulee Date: Thu, 27 Jul 2023 13:53:14 -0500 Subject: [PATCH 8/8] Functions for tool I/O are added. --- urx/urrobot.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index b990c8d..f08cfc4 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -255,13 +255,6 @@ class URRobot(object): prog = "set_tool_voltage(%s)" % (val) self.send_program(prog) - # def get_tool_digital_in(self, input_id): - # """ - # get tool digitial input - # """ - # prog = "get_tool_digital_in(%s)" % (input_id) - # self.send_program(prog) - def set_tool_digital_out(self, input_id, signal_level): """ set tool digital output