diff --git a/.gitignore b/.gitignore index 399450f..2b59632 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ docs/_* build -.idea/ \ No newline at end of file +.idea/ +urx/__pycache__/*.pyc \ No newline at end of file diff --git a/README.md b/README.md index b629029..4821791 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ +**** This repository is no longer maintained **** + urx is a python library to control the robots from 'Universal robot'. It is published under the GPL license and comes with absolutely no guarantee. It is meant as an easy to use module for pick and place operations, although it has been used for welding and other sensor based applications that do not require high control frequency. diff --git a/urx/robot.py b/urx/robot.py index 3e07d1e..f3b7d31 100644 --- a/urx/robot.py +++ b/urx/robot.py @@ -197,12 +197,16 @@ def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=Non t = m3d.Transform(pose) self.add_pose_tool(t, acc, vel, wait=wait, command=command, threshold=threshold) - def getl(self, wait=False, _log=True): + def getl(self, wait=False, _log=True, roundto=False): """ return current transformation from tcp to current csys """ t = self.get_pose(wait, _log) - return t.pose_vector.tolist() + pose = t.pose_vector.tolist() + if roundto: + pose = [round(i, self.max_float_length) for i in pose] + + return np.array(pose) def set_gravity(self, vector): if isinstance(vector, m3d.Vector): diff --git a/urx/urrobot.py b/urx/urrobot.py index e909437..8c6b0bf 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -38,6 +38,7 @@ def __init__(self, host, use_rt=False): self.secmon = ursecmon.SecondaryMonitor(self.host) # data from robot at 10Hz self.rtmon = None + self.rtlog = None if use_rt: self.rtmon = self.get_realtime_monitor() # precision of joint movem used to wait for move completion @@ -59,6 +60,8 @@ def is_running(self): Return True if robot is running (not necessary running a program, it might be idle) """ +# if self.rtmon is not None: +# return self.rtmon.is_running() and self.secmon.running return self.secmon.running def is_program_running(self): @@ -133,13 +136,34 @@ def send_message(self, msg): def set_digital_out(self, output, val): """ - set digital output. val is a bool + set standard digital output. val is a bool """ if val in (True, 1): val = "True" else: val = "False" - self.send_program('digital_out[%s]=%s' % (output, val)) + #self.send_program('digital_out[%s]=%s' % (output, val)) + self.send_program('set_standard_digital_out(%s, %s)' % (output, val)) + + def set_tool_digital_out(self, output, val): + """ + set tool digital output. val is a bool + """ + if val in (True, 1): + val = "True" + else: + val = "False" + self.send_program('set_tool_digital_out(%s, %s)' % (output, val)) + + def set_configurable_digital_out(self, output, val): + """ + set configurable digital output. val is a bool + """ + if val in (True, 1): + val = "True" + else: + val = "False" + self.send_program('set_configurable_digital_out(%s, %s)' % (output, val)) def get_analog_inputs(self): """ @@ -201,9 +225,14 @@ def _wait_for_move(self, target, threshold=None, timeout=5, joints=False): self.logger.debug("Waiting for move completion using threshold %s and target %s", threshold, target) start_dist = self._get_dist(target, joints) if threshold is None: - threshold = start_dist * 0.8 - if threshold < 0.001: # roboten precision is limited - threshold = 0.001 + if joints: + threshold = start_dist * 0.1 + if threshold < 0.005: # roboten precision is limited + threshold = 0.005 + else: + threshold = start_dist * 0.01 + if threshold < 0.001: # roboten precision is limited + threshold = 0.001 self.logger.debug("No threshold set, setting it to %s", threshold) count = 0 while True: @@ -255,7 +284,7 @@ def speedx(self, command, velocities, acc, min_time): vels = [round(i, self.max_float_length) for i in velocities] vels.append(acc) vels.append(min_time) - prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels) + prog = "{}([{},{},{},{},{},{}], a={}, t={})".format(command, *vels) self.send_program(prog) def speedl(self, velocities, acc, min_time): @@ -391,7 +420,8 @@ def close(self): self.logger.info("Closing sockets to robot") self.secmon.close() if self.rtmon: - self.rtmon.stop() + self.rtlog.close() + self.rtmon.close() def set_freedrive(self, val): """ @@ -418,6 +448,8 @@ def get_realtime_monitor(self): self.logger.info("Opening real-time monitor socket") self.rtmon = urrtmon.URRTMonitor(self.host) # som information is only available on rt interface self.rtmon.start() + self.rtlog = urrtmon.URRTlogger(self.rtmon) + self.rtlog.start() self.rtmon.set_csys(self.csys) return self.rtmon diff --git a/urx/urrtmon.py b/urx/urrtmon.py index ac87966..734eca6 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -24,18 +24,25 @@ class URRTMonitor(threading.Thread): + ''' + Documentation to Real Time Client interface can be found here: + http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/remote-control-via-tcpip-16496/ + ''' + # Struct for revision of the UR controller giving 1060 bytes (132 double values) + rtstruct1060 = struct.Struct('>132d') - # Struct for revision of the UR controller giving 692 bytes - rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') + # Struct for revision of the UR controller giving 692 bytes(86 double values) + rtstruct692 = struct.Struct('>86d') # for revision of the UR controller giving 540 byte. Here TCP - # pose is not included! - rtstruct540 = struct.Struct('>d6d6d6d6d6d6d6d6d18d') + # pose is not included! (67 double values) + rtstruct540 = struct.Struct('>67d') def __init__(self, urHost): threading.Thread.__init__(self) self.logger = logging.getLogger(self.__class__.__name__) self.daemon = True + self.running = False self._stop_event = True self._dataEvent = threading.Condition() self._dataAccess = threading.Lock() @@ -46,7 +53,13 @@ def __init__(self, urHost): self._timestamp = None self._ctrlTimestamp = None self._qActual = None - self._qTarget = None + self._qTarget = None #Target joint positions + self._qdtarget = None #Target joint velocities + self._qddtarget = None #Target joint accelerations + self._current_target = None #Target joint currents + self._moment_target = None #Target joint moments (torques) + self._actual_digital_input_bits = None + self._tcp = None self._tcp_force = None self.__recvTime = 0 @@ -57,6 +70,7 @@ def __init__(self, urHost): self._buffer = [] self._csys = None self._csys_lock = threading.Lock() + self.aa = 0 def set_csys(self, csys): with self._csys_lock: @@ -141,7 +155,9 @@ def __recv_rt_data(self): 'Received header telling that package is %s bytes long', pkgsize) payload = self.__recv_bytes(pkgsize - 4) - if pkgsize >= 692: + if pkgsize >= 1060: + unp = self.rtstruct1060.unpack(payload[:self.rtstruct1060.size]) + elif pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) elif pkgsize >= 540: unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) @@ -157,7 +173,7 @@ def __recv_rt_data(self): # 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]) + self._ctrlTimestamp = np.array(unp[0]) #Time elapsed since the controller was started [s] if self._last_ctrl_ts != 0 and ( self._ctrlTimestamp - self._last_ctrl_ts) > 0.010: @@ -165,11 +181,41 @@ def __recv_rt_data(self): "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._qTarget = np.array(unp[1:7]) - self._tcp_force = np.array(unp[67:73]) - self._tcp = np.array(unp[73:79]) - + self._qTarget = np.array(unp[1:7]) #Target joint positions + self._qdtarget = np.array(unp[7:13]) #Target joint velocities + self._qddtarget = np.array(unp[13:19]) #Target joint accelerations + self._current_target = np.array(unp[19:25]) #Target joint currents + self._moment_target = np.array(unp[25:31]) #Target joint moments (torques) + self._qActual = np.array(unp[31:37]) # Actual joint positions + self._qdactual = np.array(unp[37:43]) #Actual joint velocities + self._current_actual = np.array(unp[43:49]) #Actual joint currents + self._joint_control_output = np.array(unp[49:55]) #Joint control currents + self._actual_TCP_pose = np.array(unp[55:61]) #Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + self._actual_TCP_speed = np.array(unp[61:67]) #Actual speed of the tool given in Cartesian coordinates + self._tcp_force = np.array(unp[67:73]) #Generalized forces in the TCP + self._tcp = np.array(unp[73:79]) #Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + self._target_TCP_speed = np.array(unp[79:85]) #Target speed of the tool given in Cartesian coordinates + self._actual_digital_input_bits = unp[85] #Current state of the digital inputs. + self._joint_temperatures = np.array(unp[86:92]) #Temperature of each joint in degrees Celsius + self._actual_execution_time = unp[92] #Controller real-time thread execution time + self._URTest1 = unp[93] #A value used by Universal Robots software only + self._robot_mode = unp[94] #Robot mode + self._joint_mode = np.array(unp[95:101]) #Joint control modes + self._safety_mode = unp[101] #Safety mode + self._URTest2 = unp[102:108] #Used by Universal Robots software only + self._actual_tool_accelerometer = np.array(unp[108:111]) #Tool x, y and z accelerometer values + self._URTest3 = unp[111:117] #Used by Universal Robots software only + self._speed_scaling = unp[117] #Speed scaling of the trajectory limiter + self._actual_momentum = unp[118] #Norm of Cartesian linear momentum + self._URTest4 = unp[119] #Used by Universal Robots software only + self._URTest5 = unp[120] #Used by Universal Robots software only + self._actual_main_voltagee = unp[121] #Masterboard: Main voltage + self._actual_robot_voltage = unp[122] #Masterboard: Robot voltage (48V) + self._actual_robot_current = unp[123] #Masterboard: Robot current + self._actual_joint_voltage = np.array(unp[124:130]) #Actual joint voltages + self._actual_digital_output_bits = unp[130] #Digital outputs + self._Program_state = unp[131] #Program state + if self._csys: with self._csys_lock: # might be a godd idea to remove dependancy on m3d @@ -246,11 +292,93 @@ def close(self): self.stop() self.join() + def is_running(self): + ''' + Return True if Real Time Client (RT) interface is running + ''' + return self.running + 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() + try: + self._stop_event = False + self._rtSock.connect((self._urHost, 30003)) + while not self._stop_event: + self.__recv_rt_data() + self.running = True + self._rtSock.close() + except: + if self.running: + self.running = False + self.logger.error("RTDE interface stopped running") + + self.running = False + with self._dataEvent: + self._dataEvent.notifyAll() +class URRTlogger(URRTMonitor, threading.Thread): + + def __init__(self, rtmon): + threading.Thread.__init__(self) + self.dataLog = logging.getLogger("RTC_Data_Log") + self._stop_event = True + self.rtmon = rtmon + + def logdata(self): + self.rtmon.wait() +# self.dataLog.info('target_q;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._qTarget) +# self.dataLog.info('target_qd;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._qdtarget) +# self.dataLog.info('target_qdd;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._qddtarget) +# self.dataLog.info('target_current;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._current_target) +# self.dataLog.info('target_moment;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._moment_target) +# self.dataLog.info('actual_q;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._qActual) +# self.dataLog.info('actual_qd;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._qdactual) +# self.dataLog.info('actual_current;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._current_actual) +# self.dataLog.info('joint_control_output;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._joint_control_output) +# self.dataLog.info('actual_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._actual_TCP_pose) +# self.dataLog.info('actual_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._actual_TCP_speed) +# self.dataLog.info('actual_TCP_force;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._tcp_force) +# self.dataLog.info('target_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._target_TCP_speed) +# self.dataLog.info('actual_digital_input_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_digital_input_bits) +# self.dataLog.info('joint_temperatures;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._joint_temperatures) +# self.dataLog.info('actual_execution_time;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_execution_time) +# #? self.dataLog.info('robot_mode;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._robot_mode) +# #? self.dataLog.info('joint_mode;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._joint_mode) +# #? self.dataLog.info('safety_mode;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._safety_mode) +# #? self.dataLog.info('actual_tool_accelerometer;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._actual_tool_accelerometer) +# self.dataLog.info('speed_scaling;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._speed_scaling) +# self.dataLog.info('target_speed_fraction;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._target_speed_fraction) +# self.dataLog.info('actual_momentum;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_momentum) +# self.dataLog.info('actual_main_voltage;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_main_voltagee) +# self.dataLog.info('actual_robot_voltage;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_robot_voltage) +# self.dataLog.info('actual_robot_current;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_robot_current) +# self.dataLog.info('actual_joint_voltage;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._actual_joint_voltage) +# self.dataLog.info('actual_digital_output_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._actual_digital_output_bits) +# self.dataLog.info('runtime_state;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._runtime_state) +# self.dataLog.info('robot_status_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._robot_status_bits) +# self.dataLog.info('safety_status_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._safety_status_bits) +# self.dataLog.info('analog_io_types;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._analog_io_types) +# self.dataLog.info('standard_analog_input0;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._standard_analog_input0) +# self.dataLog.info('standard_analog_input1;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._standard_analog_input1) +# self.dataLog.info('standard_analog_output0;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._standard_analog_output0) +# self.dataLog.info('standard_analog_output1;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._standard_analog_output1) +# self.dataLog.info('io_current;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._io_current) +# self.dataLog.info('euromap67_input_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._euromap67_input_bits) +# self.dataLog.info('euromap67_output_bits;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._euromap67_output_bits) +# self.dataLog.info('euromap67_24V_voltage;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._euromap67_24V_voltage) +# self.dataLog.info('euromap67_24V_current;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._euromap67_24V_current) +# self.dataLog.info('tool_mode;%s;%s', self.rtmon._ctrlTimestamp, self.rtmon._tool_mode) +# self.dataLog.info('target_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self.rtmon._ctrlTimestamp, *self.rtmon._tcp) + + def stop(self): + self._stop_event = True + + def close(self): + self.stop() + self.join() + + def run(self): + self._stop_event = False + while not self._stop_event: + self.logdata() + \ No newline at end of file diff --git a/urx/ursecmon.py b/urx/ursecmon.py index 70c017a..8477f48 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -193,7 +193,7 @@ def find_first_packet(self, data): returns None if none found """ counter = 0 - limit = 10 + limit = 100 while True: if len(data) >= 5: psize, ptype = self.get_header(data) @@ -233,7 +233,7 @@ def __init__(self, host): self._dictLock = Lock() self.host = host secondary_port = 30002 # Secondary client interface on Universal Robots - self._s_secondary = socket.create_connection((self.host, secondary_port), timeout=0.5) + self._s_secondary = socket.create_connection((self.host, secondary_port), timeout=1.5) self._prog_queue = [] self._prog_queue_lock = Lock() self._dataqueue = bytes() @@ -241,6 +241,7 @@ def __init__(self, host): self.running = False # True when robot is on and listening self._dataEvent = Condition() self.lastpacket_timestamp = 0 + self.simulation = False self.start() self.wait() # make sure we got some data before someone calls us @@ -270,47 +271,59 @@ def run(self): so this is not guaranted and we cannot rely on information to the primary client. """ while not self._trystop: - with self._prog_queue_lock: - if len(self._prog_queue) > 0: - data = self._prog_queue.pop(0) - self._s_secondary.send(data.program) - with data.condition: - data.condition.notify_all() - - data = self._get_data() try: - tmpdict = self._parser.parse(data) - with self._dictLock: - self._dict = tmpdict - except ParsingException as ex: - self.logger.warning("Error parsing one packet from urrobot: %s", ex) - continue - - if "RobotModeData" not in self._dict: - self.logger.warning("Got a packet from robot without RobotModeData, strange ...") - continue - - self.lastpacket_timestamp = time.time() - - rmode = 0 - if self._parser.version >= (3, 0): - rmode = 7 - - if self._dict["RobotModeData"]["robotMode"] == rmode \ - and self._dict["RobotModeData"]["isRealRobotEnabled"] is True \ - and self._dict["RobotModeData"]["isEmergencyStopped"] is False \ - and self._dict["RobotModeData"]["isSecurityStopped"] is False \ - and self._dict["RobotModeData"]["isRobotConnected"] is True \ - and self._dict["RobotModeData"]["isPowerOnRobot"] is True: - self.running = True - else: + with self._prog_queue_lock: + if len(self._prog_queue) > 0: + data = self._prog_queue.pop(0) + self._s_secondary.send(data.program) + with data.condition: + data.condition.notify_all() + + data = self._get_data() + try: + tmpdict = self._parser.parse(data) + with self._dictLock: + self._dict = tmpdict + except ParsingException as ex: + self.logger.warning("Error parsing one packet from urrobot: %s", ex) + continue + + if "RobotModeData" not in self._dict: + self.logger.warning("Got a packet from robot without RobotModeData, strange ...") + continue + + self.lastpacket_timestamp = time.time() + + rmode = 0 + if self._parser.version >= (3, 0): + rmode = 7 + + if self._dict["RobotModeData"]["robotMode"] == rmode \ + and self._dict["RobotModeData"]["isRealRobotEnabled"] is True \ + and self._dict["RobotModeData"]["isEmergencyStopped"] is False \ + and self._dict["RobotModeData"]["isSecurityStopped"] is False \ + and (self._dict["RobotModeData"]["isRobotConnected"] or self.simulation) \ + and self._dict["RobotModeData"]["isPowerOnRobot"] is True: + self.running = True + else: + if self.running: + self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) + self.running = False + with self._dataEvent: + #print("X: new data") + self._dataEvent.notifyAll() + except: if self.running: - self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) - self.running = False - with self._dataEvent: - #print("X: new data") - self._dataEvent.notifyAll() - + self.running = False + self.logger.error("Secondary interface stopped running") + + self.running = False + with self._dataEvent: + self._dataEvent.notifyAll() + + + + def _get_data(self): """ returns something that looks like a packet, nothing is guaranted @@ -327,7 +340,7 @@ def _get_data(self): tmp = self._s_secondary.recv(1024) self._dataqueue += tmp - def wait(self, timeout=0.5): + def wait(self, timeout=2.): """ wait for next data packet from robot """ @@ -335,6 +348,7 @@ def wait(self, timeout=0.5): with self._dataEvent: self._dataEvent.wait(timeout) if tstamp == self.lastpacket_timestamp: + self.running = False raise TimeoutException("Did not receive a valid data packet from robot in {}".format(timeout)) def get_cartesian_info(self, wait=False): @@ -428,3 +442,4 @@ def close(self): if self._s_secondary: with self._prog_queue_lock: self._s_secondary.close() + self.running = False