From 8d46c307f3b139bc8658a4a1a39c787f1757a39e Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Mon, 23 May 2016 21:35:50 +0200 Subject: [PATCH 01/19] Add *.pyc to the ignore file --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 From d0f91910d3d0bcf865eec22bece602d8cbe2ae3c Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Mon, 23 May 2016 21:36:19 +0200 Subject: [PATCH 02/19] Add a round option to the getl function. --- urx/robot.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/urx/robot.py b/urx/robot.py index 3e07d1e..882c0b3 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 pose def set_gravity(self, vector): if isinstance(vector, m3d.Vector): From 4b418b7fadec11f34c1951d504ad9b5d692ef209 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Fri, 17 Jun 2016 15:50:35 +0200 Subject: [PATCH 03/19] Add more robustness to comunication failure by raising limit from 10 to 100 Add support for simulation connection. --- urx/ursecmon.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/urx/ursecmon.py b/urx/ursecmon.py index 70c017a..acb486b 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) @@ -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 @@ -303,6 +304,8 @@ def run(self): and self._dict["RobotModeData"]["isRobotConnected"] is True \ and self._dict["RobotModeData"]["isPowerOnRobot"] is True: self.running = True + elif self.simulation is True: + self.running = True else: if self.running: self.logger.error("Robot not running: " + str(self._dict["RobotModeData"])) From 995657731b0702c5803ecb0882dabadcf6d57510 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Fri, 17 Jun 2016 15:51:00 +0200 Subject: [PATCH 04/19] Add more data tages from the RT feed. --- urx/urrtmon.py | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index ac87966..dfce4f6 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -46,7 +46,12 @@ 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._tcp = None self._tcp_force = None self.__recvTime = 0 @@ -157,7 +162,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,10 +170,19 @@ 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 if self._csys: with self._csys_lock: From b8b23fb310b40eb76020849b207c498b209d7a73 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Wed, 22 Jun 2016 17:16:21 +0200 Subject: [PATCH 05/19] Add exampel og data logging. Need to be exported to seperate thread. --- urx/urrtmon.py | 196 ++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 194 insertions(+), 2 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index dfce4f6..3c63c5c 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -25,6 +25,9 @@ class URRTMonitor(threading.Thread): + # Struct for revision of the UR controller giving 1024 bytes + rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ6ddi6ii3ddddddd6dQIIIIdddddIIddiddIIid') + # Struct for revision of the UR controller giving 692 bytes rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') @@ -35,6 +38,7 @@ class URRTMonitor(threading.Thread): def __init__(self, urHost): threading.Thread.__init__(self) self.logger = logging.getLogger(self.__class__.__name__) + self.dataLog = logging.getLogger("DataLog") self.daemon = True self._stop_event = True self._dataEvent = threading.Condition() @@ -51,6 +55,7 @@ def __init__(self, urHost): 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 @@ -62,6 +67,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: @@ -146,7 +152,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 >= 1024: + unp = self.rtstruct1024.unpack(payload[:self.rtstruct1024.size]) + elif pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) elif pkgsize >= 540: unp = self.rtstruct540.unpack(payload[:self.rtstruct540.size]) @@ -171,19 +179,133 @@ def __recv_rt_data(self): self._ctrlTimestamp - self._last_ctrl_ts) self._last_ctrl_ts = self._ctrlTimestamp self._qTarget = np.array(unp[1:7]) #Target joint positions + self.dataLog.info('target_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qTarget) + self._qdtarget = np.array(unp[7:13]) #Target joint velocities + self.dataLog.info('target_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdtarget) + self._qddtarget = np.array(unp[13:19]) #Target joint accelerations + self.dataLog.info('target_qdd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qddtarget) + self._current_target = np.array(unp[19:25]) #Target joint currents + self.dataLog.info('target_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_target) + self._moment_target = np.array(unp[25:31]) #Target joint moments (torques) + self.dataLog.info('target_moment;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._moment_target) + self._qActual = np.array(unp[31:37]) # Actual joint positions + self.dataLog.info('actual_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qActual) + self._qdactual = np.array(unp[37:43]) #Actual joint velocities + self.dataLog.info('actual_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdactual) + self._current_actual = np.array(unp[43:49]) #Actual joint currents + self.dataLog.info('actual_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_actual) + self._joint_control_output = np.array(unp[49:55]) #Joint control currents + self.dataLog.info('joint_control_output;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_control_output) + 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.dataLog.info('actual_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_pose) + self._actual_TCP_speed = np.array(unp[61:67]) #Actual speed of the tool given in Cartesian coordinates + self.dataLog.info('actual_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_speed) + self._tcp_force = np.array(unp[67:73]) #Generalized forces in the TCP + self.dataLog.info('actual_TCP_force;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp_force) + 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.dataLog.info('target_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._target_TCP_speed) + + self._actual_digital_input_bits = unp[85] #Current state of the digital inputs. + self.dataLog.info('actual_digital_input_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_input_bits) + + self._joint_temperatures = np.array(unp[86:92]) #Temperature of each joint in degrees Celsius + self.dataLog.info('joint_temperatures;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_temperatures) + + self._actual_execution_time = unp[92] #Controller real-time thread execution time + self.dataLog.info('actual_execution_time;%s;%s', self._ctrlTimestamp, self._actual_execution_time) + + self._robot_mode = unp[93] #Robot mode + self.dataLog.info('robot_mode;%s;%s', self._ctrlTimestamp, self._robot_mode) + + self._joint_mode = np.array(unp[94:100]) #Joint control modes + self.dataLog.info('joint_mode;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_mode) + + self._safety_mode = unp[100] #Safety mode + self.dataLog.info('safety_mode;%s;%s', self._ctrlTimestamp, self._safety_mode) + + self._actual_tool_accelerometer = np.array(unp[101:104]) #Tool x, y and z accelerometer values + self.dataLog.info('actual_tool_accelerometer;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_tool_accelerometer) + + self._speed_scaling = unp[105] #Speed scaling of the trajectory limiter + self.dataLog.info('speed_scaling;%s;%s', self._ctrlTimestamp, self._speed_scaling) + + self._target_speed_fraction = unp[106] #Target speed fraction + self.dataLog.info('target_speed_fraction;%s;%s', self._ctrlTimestamp, self._target_speed_fraction) + + self._actual_momentum = unp[107] #Norm of Cartesian linear momentum + self.dataLog.info('actual_momentum;%s;%s', self._ctrlTimestamp, self._actual_momentum) + + self._actual_main_voltagee = unp[108] #Safety Control Board: Main voltage + self.dataLog.info('actual_main_voltage;%s;%s', self._ctrlTimestamp, self._actual_main_voltagee) + + self._actual_robot_voltage = unp[109] #Safety Control Board: Robot voltage (48V) + self.dataLog.info('actual_robot_voltage;%s;%s', self._ctrlTimestamp, self._actual_robot_voltage) + + self._actual_robot_current = unp[110] #Safety Control Board: Robot current + self.dataLog.info('actual_robot_current;%s;%s', self._ctrlTimestamp, self._actual_robot_current) + + self._actual_joint_voltage = np.array(unp[111:117]) #Actual joint voltages + self.dataLog.info('actual_joint_voltage;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_joint_voltage) + + self._actual_digital_output_bits = unp[117] #Digital outputs + self.dataLog.info('actual_digital_output_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_output_bits) + + self._runtime_state = unp[118] #Program state + self.dataLog.info('runtime_state;%s;%s', self._ctrlTimestamp, self._runtime_state) + + self._robot_status_bits = unp[119] #Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed + self.dataLog.info('robot_status_bits;%s;%s', self._ctrlTimestamp, self._robot_status_bits) + + self._safety_status_bits = unp[120] #Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety + self.dataLog.info('safety_status_bits;%s;%s', self._ctrlTimestamp, self._safety_status_bits) + + self._analog_io_types = unp[121] #Bits 0-3: analog input 0 | analog input 1 | analog output 0 | analog output 1, {0=current[A], 1=voltage[V]} + self.dataLog.info('analog_io_types;%s;%s', self._ctrlTimestamp, self._analog_io_types) + + self._standard_analog_input0 = unp[122] #Standard analog input 0 [A or V] + self.dataLog.info('standard_analog_input0;%s;%s', self._ctrlTimestamp, self._standard_analog_input0) + + self._standard_analog_input1 = unp[123] #Standard analog input 1 [A or V] + self.dataLog.info('standard_analog_input1;%s;%s', self._ctrlTimestamp, self._standard_analog_input1) + + self._standard_analog_output0 = unp[124] #Standard analog output 0 [A or V] + self.dataLog.info('standard_analog_output0;%s;%s', self._ctrlTimestamp, self._standard_analog_output0) + + self._standard_analog_output1 = unp[125] #Standard analog output 1 [A or V] + self.dataLog.info('standard_analog_output1;%s;%s', self._ctrlTimestamp, self._standard_analog_output1) + + self._io_current = unp[126] #I/O current [A] + self.dataLog.info('io_current;%s;%s', self._ctrlTimestamp, self._io_current) + + self._euromap67_input_bits = unp[127] #Euromap67 input bits + self.dataLog.info('euromap67_input_bits;%s;%s', self._ctrlTimestamp, self._euromap67_input_bits) + + self._euromap67_output_bits = unp[128] #Euromap67 output bits + self.dataLog.info('euromap67_output_bits;%s;%s', self._ctrlTimestamp, self._euromap67_output_bits) + + self._euromap67_24V_voltage = unp[129] #Euromap 24V voltage [V] + self.dataLog.info('euromap67_24V_voltage;%s;%s', self._ctrlTimestamp, self._euromap67_24V_voltage) + + self._euromap67_24V_current = unp[130] #Euromap 24V current [A] + self.dataLog.info('euromap67_24V_current;%s;%s', self._ctrlTimestamp, self._euromap67_24V_current) + + self._tool_mode = unp[131] #Tool mode + self.dataLog.info('tool_mode;%s;%s', self._ctrlTimestamp, self._tool_mode) + if self._csys: with self._csys_lock: # might be a godd idea to remove dependancy on m3d @@ -200,6 +322,9 @@ def __recv_rt_data(self): with self._dataEvent: self._dataEvent.notifyAll() + self.dataLog.info('target_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp) + + def start_buffering(self): """ start buffering all data from controller @@ -268,3 +393,70 @@ def run(self): self._rtSock.close() +# class URRTlogger(threading.Thread,URRTMonitor): +# +# def __init__(self): +# threading.Thread.__init__(self) +# self.dataLog = logging.getLogger("DataLog") +# self._stop_event = True +# +# +# def logdata(self): +# self.wait() +# with self._dataAccess: +# self.dataLog.info('target_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qTarget) +# self.dataLog.info('target_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdtarget) +# self.dataLog.info('target_qdd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qddtarget) +# self.dataLog.info('target_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_target) +# self.dataLog.info('target_moment;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._moment_target) +# self.dataLog.info('actual_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qActual) +# self.dataLog.info('actual_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdactual) +# self.dataLog.info('actual_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_actual) +# self.dataLog.info('joint_control_output;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_control_output) +# self.dataLog.info('actual_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_pose) +# self.dataLog.info('actual_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_speed) +# self.dataLog.info('actual_TCP_force;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp_force) +# self.dataLog.info('target_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._target_TCP_speed) +# self.dataLog.info('actual_digital_input_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_input_bits) +# self.dataLog.info('joint_temperatures;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_temperatures) +# self.dataLog.info('actual_execution_time;%s;%s', self._ctrlTimestamp, self._actual_execution_time) +# self.dataLog.info('robot_mode;%s;%s', self._ctrlTimestamp, self._robot_mode) +# self.dataLog.info('joint_mode;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_mode) +# self.dataLog.info('safety_mode;%s;%s', self._ctrlTimestamp, self._safety_mode) +# self.dataLog.info('actual_tool_accelerometer;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_tool_accelerometer) +# self.dataLog.info('speed_scaling;%s;%s', self._ctrlTimestamp, self._speed_scaling) +# self.dataLog.info('target_speed_fraction;%s;%s', self._ctrlTimestamp, self._target_speed_fraction) +# self.dataLog.info('actual_momentum;%s;%s', self._ctrlTimestamp, self._actual_momentum) +# self.dataLog.info('actual_main_voltage;%s;%s', self._ctrlTimestamp, self._actual_main_voltagee) +# self.dataLog.info('actual_robot_voltage;%s;%s', self._ctrlTimestamp, self._actual_robot_voltage) +# self.dataLog.info('actual_robot_current;%s;%s', self._ctrlTimestamp, self._actual_robot_current) +# self.dataLog.info('actual_joint_voltage;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_joint_voltage) +# self.dataLog.info('actual_digital_output_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_output_bits) +# self.dataLog.info('runtime_state;%s;%s', self._ctrlTimestamp, self._runtime_state) +# self.dataLog.info('robot_status_bits;%s;%s', self._ctrlTimestamp, self._robot_status_bits) +# self.dataLog.info('safety_status_bits;%s;%s', self._ctrlTimestamp, self._safety_status_bits) +# self.dataLog.info('analog_io_types;%s;%s', self._ctrlTimestamp, self._analog_io_types) +# self.dataLog.info('standard_analog_input0;%s;%s', self._ctrlTimestamp, self._standard_analog_input0) +# self.dataLog.info('standard_analog_input1;%s;%s', self._ctrlTimestamp, self._standard_analog_input1) +# self.dataLog.info('standard_analog_output0;%s;%s', self._ctrlTimestamp, self._standard_analog_output0) +# self.dataLog.info('standard_analog_output1;%s;%s', self._ctrlTimestamp, self._standard_analog_output1) +# self.dataLog.info('io_current;%s;%s', self._ctrlTimestamp, self._io_current) +# self.dataLog.info('euromap67_input_bits;%s;%s', self._ctrlTimestamp, self._euromap67_input_bits) +# self.dataLog.info('euromap67_output_bits;%s;%s', self._ctrlTimestamp, self._euromap67_output_bits) +# self.dataLog.info('euromap67_24V_voltage;%s;%s', self._ctrlTimestamp, self._euromap67_24V_voltage) +# self.dataLog.info('euromap67_24V_current;%s;%s', self._ctrlTimestamp, self._euromap67_24V_current) +# self.dataLog.info('tool_mode;%s;%s', self._ctrlTimestamp, self._tool_mode) +# self.dataLog.info('target_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._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 From c647e8aa7248e9973d384a049bae373f37698777 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Wed, 22 Jun 2016 17:52:39 +0200 Subject: [PATCH 06/19] Add Logging to seperate thread. --- urx/urrobot.py | 3 + urx/urrtmon.py | 222 +++++++++++++++---------------------------------- 2 files changed, 70 insertions(+), 155 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index e909437..e6bc259 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -392,6 +392,7 @@ def close(self): self.secmon.close() if self.rtmon: self.rtmon.stop() + self.rtlog.stop() def set_freedrive(self, val): """ @@ -418,6 +419,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 3c63c5c..369eb5d 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -38,7 +38,6 @@ class URRTMonitor(threading.Thread): def __init__(self, urHost): threading.Thread.__init__(self) self.logger = logging.getLogger(self.__class__.__name__) - self.dataLog = logging.getLogger("DataLog") self.daemon = True self._stop_event = True self._dataEvent = threading.Condition() @@ -179,132 +178,48 @@ def __recv_rt_data(self): self._ctrlTimestamp - self._last_ctrl_ts) self._last_ctrl_ts = self._ctrlTimestamp self._qTarget = np.array(unp[1:7]) #Target joint positions - self.dataLog.info('target_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qTarget) - self._qdtarget = np.array(unp[7:13]) #Target joint velocities - self.dataLog.info('target_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdtarget) - self._qddtarget = np.array(unp[13:19]) #Target joint accelerations - self.dataLog.info('target_qdd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qddtarget) - self._current_target = np.array(unp[19:25]) #Target joint currents - self.dataLog.info('target_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_target) - self._moment_target = np.array(unp[25:31]) #Target joint moments (torques) - self.dataLog.info('target_moment;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._moment_target) - self._qActual = np.array(unp[31:37]) # Actual joint positions - self.dataLog.info('actual_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qActual) - self._qdactual = np.array(unp[37:43]) #Actual joint velocities - self.dataLog.info('actual_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdactual) - self._current_actual = np.array(unp[43:49]) #Actual joint currents - self.dataLog.info('actual_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_actual) - self._joint_control_output = np.array(unp[49:55]) #Joint control currents - self.dataLog.info('joint_control_output;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_control_output) - 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.dataLog.info('actual_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_pose) - self._actual_TCP_speed = np.array(unp[61:67]) #Actual speed of the tool given in Cartesian coordinates - self.dataLog.info('actual_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_speed) - self._tcp_force = np.array(unp[67:73]) #Generalized forces in the TCP - self.dataLog.info('actual_TCP_force;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp_force) - 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.dataLog.info('target_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._target_TCP_speed) - self._actual_digital_input_bits = unp[85] #Current state of the digital inputs. - self.dataLog.info('actual_digital_input_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_input_bits) - self._joint_temperatures = np.array(unp[86:92]) #Temperature of each joint in degrees Celsius - self.dataLog.info('joint_temperatures;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_temperatures) - self._actual_execution_time = unp[92] #Controller real-time thread execution time - self.dataLog.info('actual_execution_time;%s;%s', self._ctrlTimestamp, self._actual_execution_time) - self._robot_mode = unp[93] #Robot mode - self.dataLog.info('robot_mode;%s;%s', self._ctrlTimestamp, self._robot_mode) - self._joint_mode = np.array(unp[94:100]) #Joint control modes - self.dataLog.info('joint_mode;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_mode) - self._safety_mode = unp[100] #Safety mode - self.dataLog.info('safety_mode;%s;%s', self._ctrlTimestamp, self._safety_mode) - self._actual_tool_accelerometer = np.array(unp[101:104]) #Tool x, y and z accelerometer values - self.dataLog.info('actual_tool_accelerometer;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_tool_accelerometer) - self._speed_scaling = unp[105] #Speed scaling of the trajectory limiter - self.dataLog.info('speed_scaling;%s;%s', self._ctrlTimestamp, self._speed_scaling) - self._target_speed_fraction = unp[106] #Target speed fraction - self.dataLog.info('target_speed_fraction;%s;%s', self._ctrlTimestamp, self._target_speed_fraction) - self._actual_momentum = unp[107] #Norm of Cartesian linear momentum - self.dataLog.info('actual_momentum;%s;%s', self._ctrlTimestamp, self._actual_momentum) - self._actual_main_voltagee = unp[108] #Safety Control Board: Main voltage - self.dataLog.info('actual_main_voltage;%s;%s', self._ctrlTimestamp, self._actual_main_voltagee) - self._actual_robot_voltage = unp[109] #Safety Control Board: Robot voltage (48V) - self.dataLog.info('actual_robot_voltage;%s;%s', self._ctrlTimestamp, self._actual_robot_voltage) - self._actual_robot_current = unp[110] #Safety Control Board: Robot current - self.dataLog.info('actual_robot_current;%s;%s', self._ctrlTimestamp, self._actual_robot_current) - self._actual_joint_voltage = np.array(unp[111:117]) #Actual joint voltages - self.dataLog.info('actual_joint_voltage;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_joint_voltage) - self._actual_digital_output_bits = unp[117] #Digital outputs - self.dataLog.info('actual_digital_output_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_output_bits) - self._runtime_state = unp[118] #Program state - self.dataLog.info('runtime_state;%s;%s', self._ctrlTimestamp, self._runtime_state) - self._robot_status_bits = unp[119] #Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed - self.dataLog.info('robot_status_bits;%s;%s', self._ctrlTimestamp, self._robot_status_bits) - self._safety_status_bits = unp[120] #Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety - self.dataLog.info('safety_status_bits;%s;%s', self._ctrlTimestamp, self._safety_status_bits) - self._analog_io_types = unp[121] #Bits 0-3: analog input 0 | analog input 1 | analog output 0 | analog output 1, {0=current[A], 1=voltage[V]} - self.dataLog.info('analog_io_types;%s;%s', self._ctrlTimestamp, self._analog_io_types) - self._standard_analog_input0 = unp[122] #Standard analog input 0 [A or V] - self.dataLog.info('standard_analog_input0;%s;%s', self._ctrlTimestamp, self._standard_analog_input0) - self._standard_analog_input1 = unp[123] #Standard analog input 1 [A or V] - self.dataLog.info('standard_analog_input1;%s;%s', self._ctrlTimestamp, self._standard_analog_input1) - self._standard_analog_output0 = unp[124] #Standard analog output 0 [A or V] - self.dataLog.info('standard_analog_output0;%s;%s', self._ctrlTimestamp, self._standard_analog_output0) - self._standard_analog_output1 = unp[125] #Standard analog output 1 [A or V] - self.dataLog.info('standard_analog_output1;%s;%s', self._ctrlTimestamp, self._standard_analog_output1) - self._io_current = unp[126] #I/O current [A] - self.dataLog.info('io_current;%s;%s', self._ctrlTimestamp, self._io_current) - self._euromap67_input_bits = unp[127] #Euromap67 input bits - self.dataLog.info('euromap67_input_bits;%s;%s', self._ctrlTimestamp, self._euromap67_input_bits) - self._euromap67_output_bits = unp[128] #Euromap67 output bits - self.dataLog.info('euromap67_output_bits;%s;%s', self._ctrlTimestamp, self._euromap67_output_bits) - self._euromap67_24V_voltage = unp[129] #Euromap 24V voltage [V] - self.dataLog.info('euromap67_24V_voltage;%s;%s', self._ctrlTimestamp, self._euromap67_24V_voltage) - self._euromap67_24V_current = unp[130] #Euromap 24V current [A] - self.dataLog.info('euromap67_24V_current;%s;%s', self._ctrlTimestamp, self._euromap67_24V_current) - self._tool_mode = unp[131] #Tool mode - self.dataLog.info('tool_mode;%s;%s', self._ctrlTimestamp, self._tool_mode) if self._csys: with self._csys_lock: @@ -322,9 +237,6 @@ def __recv_rt_data(self): with self._dataEvent: self._dataEvent.notifyAll() - self.dataLog.info('target_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp) - - def start_buffering(self): """ start buffering all data from controller @@ -393,70 +305,70 @@ def run(self): self._rtSock.close() -# class URRTlogger(threading.Thread,URRTMonitor): -# -# def __init__(self): -# threading.Thread.__init__(self) -# self.dataLog = logging.getLogger("DataLog") -# self._stop_event = True -# -# -# def logdata(self): -# self.wait() -# with self._dataAccess: -# self.dataLog.info('target_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qTarget) -# self.dataLog.info('target_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdtarget) -# self.dataLog.info('target_qdd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qddtarget) -# self.dataLog.info('target_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_target) -# self.dataLog.info('target_moment;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._moment_target) -# self.dataLog.info('actual_q;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qActual) -# self.dataLog.info('actual_qd;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._qdactual) -# self.dataLog.info('actual_current;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._current_actual) -# self.dataLog.info('joint_control_output;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_control_output) -# self.dataLog.info('actual_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_pose) -# self.dataLog.info('actual_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_TCP_speed) -# self.dataLog.info('actual_TCP_force;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._tcp_force) -# self.dataLog.info('target_TCP_speed;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._target_TCP_speed) -# self.dataLog.info('actual_digital_input_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_input_bits) -# self.dataLog.info('joint_temperatures;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_temperatures) -# self.dataLog.info('actual_execution_time;%s;%s', self._ctrlTimestamp, self._actual_execution_time) -# self.dataLog.info('robot_mode;%s;%s', self._ctrlTimestamp, self._robot_mode) -# self.dataLog.info('joint_mode;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._joint_mode) -# self.dataLog.info('safety_mode;%s;%s', self._ctrlTimestamp, self._safety_mode) -# self.dataLog.info('actual_tool_accelerometer;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_tool_accelerometer) -# self.dataLog.info('speed_scaling;%s;%s', self._ctrlTimestamp, self._speed_scaling) -# self.dataLog.info('target_speed_fraction;%s;%s', self._ctrlTimestamp, self._target_speed_fraction) -# self.dataLog.info('actual_momentum;%s;%s', self._ctrlTimestamp, self._actual_momentum) -# self.dataLog.info('actual_main_voltage;%s;%s', self._ctrlTimestamp, self._actual_main_voltagee) -# self.dataLog.info('actual_robot_voltage;%s;%s', self._ctrlTimestamp, self._actual_robot_voltage) -# self.dataLog.info('actual_robot_current;%s;%s', self._ctrlTimestamp, self._actual_robot_current) -# self.dataLog.info('actual_joint_voltage;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._actual_joint_voltage) -# self.dataLog.info('actual_digital_output_bits;%s;%s', self._ctrlTimestamp, self._actual_digital_output_bits) -# self.dataLog.info('runtime_state;%s;%s', self._ctrlTimestamp, self._runtime_state) -# self.dataLog.info('robot_status_bits;%s;%s', self._ctrlTimestamp, self._robot_status_bits) -# self.dataLog.info('safety_status_bits;%s;%s', self._ctrlTimestamp, self._safety_status_bits) -# self.dataLog.info('analog_io_types;%s;%s', self._ctrlTimestamp, self._analog_io_types) -# self.dataLog.info('standard_analog_input0;%s;%s', self._ctrlTimestamp, self._standard_analog_input0) -# self.dataLog.info('standard_analog_input1;%s;%s', self._ctrlTimestamp, self._standard_analog_input1) -# self.dataLog.info('standard_analog_output0;%s;%s', self._ctrlTimestamp, self._standard_analog_output0) -# self.dataLog.info('standard_analog_output1;%s;%s', self._ctrlTimestamp, self._standard_analog_output1) -# self.dataLog.info('io_current;%s;%s', self._ctrlTimestamp, self._io_current) -# self.dataLog.info('euromap67_input_bits;%s;%s', self._ctrlTimestamp, self._euromap67_input_bits) -# self.dataLog.info('euromap67_output_bits;%s;%s', self._ctrlTimestamp, self._euromap67_output_bits) -# self.dataLog.info('euromap67_24V_voltage;%s;%s', self._ctrlTimestamp, self._euromap67_24V_voltage) -# self.dataLog.info('euromap67_24V_current;%s;%s', self._ctrlTimestamp, self._euromap67_24V_current) -# self.dataLog.info('tool_mode;%s;%s', self._ctrlTimestamp, self._tool_mode) -# self.dataLog.info('target_TCP_pose;%s;%s;%s;%s;%s;%s;%s', self._ctrlTimestamp, *self._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 +class URRTlogger(URRTMonitor, threading.Thread): + + def __init__(self, rtmon): + threading.Thread.__init__(self) + self.dataLog = logging.getLogger("DataLog") + self._stop_event = True + self.rtmon = rtmon + + def logdata(self): + self.rtmon.wait() + with self.rtmon._dataAccess: + 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 From 7b716263f4d05c75a5bbf8d9528a72e885e9e4dd Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Tue, 28 Jun 2016 08:48:48 +0200 Subject: [PATCH 07/19] Update rt data read function to enable reading digital input bits. --- urx/urrobot.py | 27 ++++++++++++- urx/urrtmon.py | 100 +++++++++++++++++++++++++++---------------------- 2 files changed, 80 insertions(+), 47 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index e6bc259..e479526 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -133,13 +133,36 @@ 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 + (Don't work yet!!!) + """ + raise Exception("This function is not yet tested and ready") + 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): """ diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 369eb5d..e899079 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -26,7 +26,8 @@ class URRTMonitor(threading.Thread): # Struct for revision of the UR controller giving 1024 bytes - rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ6ddi6ii3ddddddd6dQIIIIdddddIIddiddIIid') + rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddIIid') + #rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddII24i24d') # Struct for revision of the UR controller giving 692 bytes rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') @@ -220,6 +221,16 @@ def __recv_rt_data(self): self._euromap67_24V_voltage = unp[129] #Euromap 24V voltage [V] self._euromap67_24V_current = unp[130] #Euromap 24V current [A] self._tool_mode = unp[131] #Tool mode +# self._tool_analog_input_types = unp[132] #Output domain {0=current[A], 1=voltage[V]} - Bits 0-1: tool_analog_input_0 | tool_analog_input_1 +# self._tool_analog_input0 = unp[133] #Tool analog input 0 [A or V] +# self._tool_analog_input1 = unp[134] #Tool analog input 1 [A or V] +# self._tool_output_voltage = unp[135] #Tool output voltage [V] +# self._tool_output_current = unp[136] #Tool current [A] +# self._tcp_force_scalar = unp[137] #TCP force scalar [N] +# self._output_bit_registers0_to_31 = unp[138] #General purpose bits +# self._output_bit_registers32_to_63 = unp[139] #General purpose bits +# self._output_int_register_X = unp[140:164] #24 general purpose integer registers x[0..23] +# self._output_double_register_X = unp[164:188] #24 general purpose double registers x[0..23] if self._csys: with self._csys_lock: @@ -315,50 +326,49 @@ def __init__(self, rtmon): def logdata(self): self.rtmon.wait() - with self.rtmon._dataAccess: - 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) + 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 From ab1ceb3b9d7d0505b41a65aa6b1ca62e37c34a4d Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Tue, 28 Jun 2016 16:08:56 +0200 Subject: [PATCH 08/19] Bigfix on fi --- urx/urrobot.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index e479526..7071294 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -152,17 +152,15 @@ def set_tool_digital_out(self, output, val): val = "False" self.send_program('set_tool_digital_out(%s, %s)' % (output, val)) - def set_configurable_digital_out(self, output, val): + def set_configurable_digital_out(self, output, val): """ - set configurable digital output. val is a bool - (Don't work yet!!!) + set configurable digital output. val is a bool """ - raise Exception("This function is not yet tested and ready") if val in (True, 1): val = "True" else: val = "False" - self.send_program('set_configurable_digital_out(%s, %s)' % (output, val)) + self.send_program('set_configurable_digital_out(%s, %s)' % (output, val)) def get_analog_inputs(self): """ From 032eee5aa0adfa54fb2a3f0eb629db19cd7a0e7e Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Fri, 1 Jul 2016 12:43:25 +0200 Subject: [PATCH 09/19] Update indput. --- urx/urrtmon.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index e899079..01f0f3d 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -26,7 +26,8 @@ class URRTMonitor(threading.Thread): # Struct for revision of the UR controller giving 1024 bytes - rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddIIid') + #? ? | + rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6ddIIIIdddddIIddiddIIid') #rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddII24i24d') # Struct for revision of the UR controller giving 692 bytes From 191ee77a3aa6489020bd2ddd9bc4e087abfd31bc Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Fri, 1 Jul 2016 13:53:30 +0200 Subject: [PATCH 10/19] Update in data read --- urx/urrtmon.py | 70 ++++++++++++++++++++++++++------------------------ 1 file changed, 36 insertions(+), 34 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 01f0f3d..7feb756 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -27,7 +27,7 @@ class URRTMonitor(threading.Thread): # Struct for revision of the UR controller giving 1024 bytes #? ? | - rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6ddIIIIdddddIIddiddIIid') + rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6ddIIIIdddddIIddIIddiddII3id') #rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddII24i24d') # Struct for revision of the UR controller giving 692 bytes @@ -153,7 +153,9 @@ def __recv_rt_data(self): 'Received header telling that package is %s bytes long', pkgsize) payload = self.__recv_bytes(pkgsize - 4) - if pkgsize >= 1024: + if pkgsize > 1060: + print('Hi Li, please send me this number: ' + str(pkgsize)) + if pkgsize >= 1060: unp = self.rtstruct1024.unpack(payload[:self.rtstruct1024.size]) elif pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) @@ -200,38 +202,38 @@ def __recv_rt_data(self): self._joint_mode = np.array(unp[94:100]) #Joint control modes self._safety_mode = unp[100] #Safety mode self._actual_tool_accelerometer = np.array(unp[101:104]) #Tool x, y and z accelerometer values - self._speed_scaling = unp[105] #Speed scaling of the trajectory limiter - self._target_speed_fraction = unp[106] #Target speed fraction - self._actual_momentum = unp[107] #Norm of Cartesian linear momentum - self._actual_main_voltagee = unp[108] #Safety Control Board: Main voltage - self._actual_robot_voltage = unp[109] #Safety Control Board: Robot voltage (48V) - self._actual_robot_current = unp[110] #Safety Control Board: Robot current - self._actual_joint_voltage = np.array(unp[111:117]) #Actual joint voltages - self._actual_digital_output_bits = unp[117] #Digital outputs - self._runtime_state = unp[118] #Program state - self._robot_status_bits = unp[119] #Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed - self._safety_status_bits = unp[120] #Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety - self._analog_io_types = unp[121] #Bits 0-3: analog input 0 | analog input 1 | analog output 0 | analog output 1, {0=current[A], 1=voltage[V]} - self._standard_analog_input0 = unp[122] #Standard analog input 0 [A or V] - self._standard_analog_input1 = unp[123] #Standard analog input 1 [A or V] - self._standard_analog_output0 = unp[124] #Standard analog output 0 [A or V] - self._standard_analog_output1 = unp[125] #Standard analog output 1 [A or V] - self._io_current = unp[126] #I/O current [A] - self._euromap67_input_bits = unp[127] #Euromap67 input bits - self._euromap67_output_bits = unp[128] #Euromap67 output bits - self._euromap67_24V_voltage = unp[129] #Euromap 24V voltage [V] - self._euromap67_24V_current = unp[130] #Euromap 24V current [A] - self._tool_mode = unp[131] #Tool mode -# self._tool_analog_input_types = unp[132] #Output domain {0=current[A], 1=voltage[V]} - Bits 0-1: tool_analog_input_0 | tool_analog_input_1 -# self._tool_analog_input0 = unp[133] #Tool analog input 0 [A or V] -# self._tool_analog_input1 = unp[134] #Tool analog input 1 [A or V] -# self._tool_output_voltage = unp[135] #Tool output voltage [V] -# self._tool_output_current = unp[136] #Tool current [A] -# self._tcp_force_scalar = unp[137] #TCP force scalar [N] -# self._output_bit_registers0_to_31 = unp[138] #General purpose bits -# self._output_bit_registers32_to_63 = unp[139] #General purpose bits -# self._output_int_register_X = unp[140:164] #24 general purpose integer registers x[0..23] -# self._output_double_register_X = unp[164:188] #24 general purpose double registers x[0..23] + self._speed_scaling = unp[104] #Speed scaling of the trajectory limiter + self._target_speed_fraction = unp[105] #Target speed fraction + self._actual_momentum = unp[106] #Norm of Cartesian linear momentum + self._actual_main_voltagee = unp[107] #Safety Control Board: Main voltage + self._actual_robot_voltage = unp[108] #Safety Control Board: Robot voltage (48V) + self._actual_robot_current = unp[109] #Safety Control Board: Robot current + self._actual_joint_voltage = np.array(unp[110:116]) #Actual joint voltages + self._actual_digital_output_bits = unp[116] #Digital outputs + self._runtime_state = unp[117] #Program state + self._robot_status_bits = unp[118] #Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed + self._safety_status_bits = unp[119] #Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety + self._analog_io_types = unp[120] #Bits 0-3: analog input 0 | analog input 1 | analog output 0 | analog output 1, {0=current[A], 1=voltage[V]} + self._standard_analog_input0 = unp[121] #Standard analog input 0 [A or V] + self._standard_analog_input1 = unp[122] #Standard analog input 1 [A or V] + self._standard_analog_output0 = unp[123] #Standard analog output 0 [A or V] + self._standard_analog_output1 = unp[124] #Standard analog output 1 [A or V] + self._io_current = unp[125] #I/O current [A] + self._euromap67_input_bits = unp[126] #Euromap67 input bits + self._euromap67_output_bits = unp[127] #Euromap67 output bits + self._euromap67_24V_voltage = unp[128] #Euromap 24V voltage [V] + self._euromap67_24V_current = unp[129] #Euromap 24V current [A] + self._tool_mode = unp[130] #Tool mode + self._tool_analog_input_types = unp[131] #Output domain {0=current[A], 1=voltage[V]} - Bits 0-1: tool_analog_input_0 | tool_analog_input_1 + self._tool_analog_input0 = unp[132] #Tool analog input 0 [A or V] + self._tool_analog_input1 = unp[133] #Tool analog input 1 [A or V] + self._tool_output_voltage = unp[134] #Tool output voltage [V] + self._tool_output_current = unp[135] #Tool current [A] + self._tcp_force_scalar = unp[136] #TCP force scalar [N] +# self._output_bit_registers0_to_31 = unp[137] #General purpose bits +# self._output_bit_registers32_to_63 = unp[138] #General purpose bits +# self._output_int_register_X = unp[139:163] #24 general purpose integer registers x[0..23] +# self._output_double_register_X = unp[163:187] #24 general purpose double registers x[0..23] if self._csys: with self._csys_lock: From 6a6b7cf2d2af4ff75ef31b6f6061417ecce5a079 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Fri, 1 Jul 2016 14:32:30 +0200 Subject: [PATCH 11/19] Update --- urx/urrtmon.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 7feb756..d147ff6 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -230,8 +230,11 @@ def __recv_rt_data(self): self._tool_output_voltage = unp[134] #Tool output voltage [V] self._tool_output_current = unp[135] #Tool current [A] self._tcp_force_scalar = unp[136] #TCP force scalar [N] -# self._output_bit_registers0_to_31 = unp[137] #General purpose bits -# self._output_bit_registers32_to_63 = unp[138] #General purpose bits + self._output_bit_registers0_to_31 = unp[137] #General purpose bits + self._output_bit_registers32_to_63 = unp[138] #General purpose bits + self._output_int_register_X = unp[139:142] #24 general purpose integer registers x[0..23] + self._output_double_register_X = unp[142] #24 general purpose double registers x[0..23] + # self._output_int_register_X = unp[139:163] #24 general purpose integer registers x[0..23] # self._output_double_register_X = unp[163:187] #24 general purpose double registers x[0..23] From 9804f6e97ed25e3c49c9f63f0e59a825d42b4f64 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Mon, 4 Jul 2016 07:55:28 +0200 Subject: [PATCH 12/19] Real Time data interface updated to follow the interface description in the Excel files at this site: http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/remote-control-via-tcpip-16496/ --- urx/urrtmon.py | 152 ++++++++++++++++++++----------------------------- 1 file changed, 63 insertions(+), 89 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index d147ff6..e3d6481 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -25,10 +25,8 @@ class URRTMonitor(threading.Thread): - # Struct for revision of the UR controller giving 1024 bytes - #? ? | - rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6ddIIIIdddddIIddIIddiddII3id') - #rtstruct1024 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd6ddi6ii3ddddddd6dQIIIIdddddIIddiddII24i24d') + # Struct for revision of the UR controller giving 1060 bytes + rtstruct1060 = struct.Struct('>132d') # Struct for revision of the UR controller giving 692 bytes rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') @@ -153,10 +151,8 @@ def __recv_rt_data(self): 'Received header telling that package is %s bytes long', pkgsize) payload = self.__recv_bytes(pkgsize - 4) - if pkgsize > 1060: - print('Hi Li, please send me this number: ' + str(pkgsize)) if pkgsize >= 1060: - unp = self.rtstruct1024.unpack(payload[:self.rtstruct1024.size]) + unp = self.rtstruct1060.unpack(payload[:self.rtstruct1060.size]) elif pkgsize >= 692: unp = self.rtstruct692.unpack(payload[:self.rtstruct692.size]) elif pkgsize >= 540: @@ -198,45 +194,23 @@ def __recv_rt_data(self): 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._robot_mode = unp[93] #Robot mode - self._joint_mode = np.array(unp[94:100]) #Joint control modes - self._safety_mode = unp[100] #Safety mode - self._actual_tool_accelerometer = np.array(unp[101:104]) #Tool x, y and z accelerometer values - self._speed_scaling = unp[104] #Speed scaling of the trajectory limiter - self._target_speed_fraction = unp[105] #Target speed fraction - self._actual_momentum = unp[106] #Norm of Cartesian linear momentum - self._actual_main_voltagee = unp[107] #Safety Control Board: Main voltage - self._actual_robot_voltage = unp[108] #Safety Control Board: Robot voltage (48V) - self._actual_robot_current = unp[109] #Safety Control Board: Robot current - self._actual_joint_voltage = np.array(unp[110:116]) #Actual joint voltages - self._actual_digital_output_bits = unp[116] #Digital outputs - self._runtime_state = unp[117] #Program state - self._robot_status_bits = unp[118] #Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed - self._safety_status_bits = unp[119] #Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety - self._analog_io_types = unp[120] #Bits 0-3: analog input 0 | analog input 1 | analog output 0 | analog output 1, {0=current[A], 1=voltage[V]} - self._standard_analog_input0 = unp[121] #Standard analog input 0 [A or V] - self._standard_analog_input1 = unp[122] #Standard analog input 1 [A or V] - self._standard_analog_output0 = unp[123] #Standard analog output 0 [A or V] - self._standard_analog_output1 = unp[124] #Standard analog output 1 [A or V] - self._io_current = unp[125] #I/O current [A] - self._euromap67_input_bits = unp[126] #Euromap67 input bits - self._euromap67_output_bits = unp[127] #Euromap67 output bits - self._euromap67_24V_voltage = unp[128] #Euromap 24V voltage [V] - self._euromap67_24V_current = unp[129] #Euromap 24V current [A] - self._tool_mode = unp[130] #Tool mode - self._tool_analog_input_types = unp[131] #Output domain {0=current[A], 1=voltage[V]} - Bits 0-1: tool_analog_input_0 | tool_analog_input_1 - self._tool_analog_input0 = unp[132] #Tool analog input 0 [A or V] - self._tool_analog_input1 = unp[133] #Tool analog input 1 [A or V] - self._tool_output_voltage = unp[134] #Tool output voltage [V] - self._tool_output_current = unp[135] #Tool current [A] - self._tcp_force_scalar = unp[136] #TCP force scalar [N] - self._output_bit_registers0_to_31 = unp[137] #General purpose bits - self._output_bit_registers32_to_63 = unp[138] #General purpose bits - self._output_int_register_X = unp[139:142] #24 general purpose integer registers x[0..23] - self._output_double_register_X = unp[142] #24 general purpose double registers x[0..23] - -# self._output_int_register_X = unp[139:163] #24 general purpose integer registers x[0..23] -# self._output_double_register_X = unp[163:187] #24 general purpose double registers x[0..23] + 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: @@ -332,49 +306,49 @@ def __init__(self, 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) +# 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 From 82cb9c17fc4e5532424f4be7e25a99f042d6605f Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Thu, 7 Jul 2016 00:28:42 +0200 Subject: [PATCH 13/19] Minor bug fix in how the datalogger is closed --- urx/urrobot.py | 4 ++-- urx/urrtmon.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index 7071294..f78acdd 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -412,8 +412,8 @@ def close(self): self.logger.info("Closing sockets to robot") self.secmon.close() if self.rtmon: - self.rtmon.stop() - self.rtlog.stop() + self.rtlog.close() + self.rtmon.close() def set_freedrive(self, val): """ diff --git a/urx/urrtmon.py b/urx/urrtmon.py index e3d6481..640e1c3 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -29,7 +29,7 @@ class URRTMonitor(threading.Thread): rtstruct1060 = struct.Struct('>132d') # Struct for revision of the UR controller giving 692 bytes - rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dQ') + rtstruct692 = struct.Struct('>d6d6d6d6d6d6d6d6d18d6d6d6dd') # for revision of the UR controller giving 540 byte. Here TCP # pose is not included! @@ -300,13 +300,13 @@ class URRTlogger(URRTMonitor, threading.Thread): def __init__(self, rtmon): threading.Thread.__init__(self) - self.dataLog = logging.getLogger("DataLog") + 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_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) From 207d4fda78db603fd55b3a2dfc34f9d40b794dd2 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Sun, 10 Jul 2016 14:31:52 +0200 Subject: [PATCH 14/19] Update wait_for_move to have more narrow threshold but threshold to be dependent on joint attribute. --- urx/urrobot.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index f78acdd..33186f1 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -222,9 +222,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: From 19215524b2e2bd8b4252543f854acc9c3cf5a118 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Sun, 10 Jul 2016 14:40:24 +0200 Subject: [PATCH 15/19] Aditional comments --- urx/urrtmon.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 640e1c3..2f0648c 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -24,16 +24,19 @@ class URRTMonitor(threading.Thread): - - # Struct for revision of the UR controller giving 1060 bytes + ''' + 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('>d6d6d6d6d6d6d6d6d18d6d6d6dd') + # 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) From 141338e9352111e098a6ab283e4e2a3f151c295a Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Mon, 18 Jul 2016 11:54:40 +0200 Subject: [PATCH 16/19] Make getl return a numpy array. --- urx/robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/urx/robot.py b/urx/robot.py index 882c0b3..f3b7d31 100644 --- a/urx/robot.py +++ b/urx/robot.py @@ -206,7 +206,7 @@ def getl(self, wait=False, _log=True, roundto=False): if roundto: pose = [round(i, self.max_float_length) for i in pose] - return pose + return np.array(pose) def set_gravity(self, vector): if isinstance(vector, m3d.Vector): From df7ddd5a9b815731b22dff03b22a890943287c66 Mon Sep 17 00:00:00 2001 From: Martin Huus Bjerge Date: Tue, 23 Aug 2016 15:45:09 +0200 Subject: [PATCH 17/19] Monitor if RT is running --- urx/urrobot.py | 3 ++ urx/urrtmon.py | 27 +++++++++++--- urx/ursecmon.py | 98 +++++++++++++++++++++++++++---------------------- 3 files changed, 80 insertions(+), 48 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index 33186f1..39c3add 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): diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 2f0648c..734eca6 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -42,6 +42,7 @@ 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() @@ -291,12 +292,28 @@ 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): diff --git a/urx/ursecmon.py b/urx/ursecmon.py index acb486b..8477f48 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -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() @@ -271,49 +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 - elif self.simulation 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 @@ -330,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 """ @@ -338,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): @@ -431,3 +442,4 @@ def close(self): if self._s_secondary: with self._prog_queue_lock: self._s_secondary.close() + self.running = False From 6cf844a5581ca9fccec6bdf1bbfd3468c59285af Mon Sep 17 00:00:00 2001 From: Haijie Li Date: Mon, 29 Aug 2016 15:05:21 +0200 Subject: [PATCH 18/19] speedx, t_min update --- urx/urrobot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index 33186f1..3e3782f 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -281,7 +281,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): From b2feec8d377a7e1125d7d5be6fa459c2b8971899 Mon Sep 17 00:00:00 2001 From: martinbjerge Date: Fri, 3 Jan 2020 08:27:48 +0100 Subject: [PATCH 19/19] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) 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.