looking at this fork... DO NOT MERGE

.gitignore

@@ -1,3 +1,4 @@
 docs/_*
 build
-.idea/
+.idea/
+urx/__pycache__/*.pyc

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.

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):

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
 

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,19 +173,49 @@ 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:
                 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._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()
+