adeck/python-urx-getl-rt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

removed tracker and moved functionnality in urrtmon

Browse Source
This commit is contained in:
Olivier R-D 2013-08-25 16:15:42 +02:00
parent 64726bf804
commit 75d3184637
4 changed files with 42 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
urx/__init__.py
View File

@ -6,5 +6,4 @@ __version__ = "0.8"
from .urx import Robot, RobotException, URRobot
from .tracker import Tracker

106
urx/tracker.py
View File

@ -1,106 +0,0 @@
"""
tracks moves a UR robot over the UR real-time port
"""
__author__ = "Olivier Roulet-Dubonnet"
__copyright__ = "Copyright 2011-2013, Sintef Raufoss Manufacturing"
__credits__ = ["Olivier Roulet-Dubonnet"]
__license__ = "GPLv3"
import time
from multiprocessing import Process, Queue, Event
import numpy as np
from urx import urrtmon
MATH3D = True
try:
import math3d
except ImportError:
MATH3D = False
print("pymath3d library could not be found on this computer, disabling use of matrices")
class Tracker(Process):
def __init__(self, robot_host):
self.host = robot_host
self._queue = Queue()
Process.__init__(self, args=(self._queue,))
self._stop_event = Event()
self._finished = Event()
self._data = []
if MATH3D:
self.calibration = math3d.Transform()
self.inverse = self.calibration.inverse()
def _log(self, *args):
print(self.__class__.__name__, ": ".join([str(i) for i in args]))
def set_csys(self, cal):
if MATH3D:
self.calibration = cal
self.inverse = self.calibration.inverse()
def _save_data(self):
result = np.array(self._data, dtype=[ ('timestamp', 'float64'), ('ctrlTimestamp', 'float64'), ('pose', '6float64'), ('joints', '6float64') ])
if MATH3D:
nposes = []
for pose in result["pose"]:
trf = self.inverse * math3d.Transform(pose)
nposes.append(trf.pose_vector)
result["pose"] = np.array(nposes)
self._queue.put(result)
def run_old(self):
self._data = []
rtmon = urrtmon.URRTMonitor(self.host)
rtmon.start()
while not self._stop_event.is_set():
timestamp, ctrlTimestamp, pose = rtmon.tcf_pose(wait=True, timestamp=True, ctrlTimestamp=True)
joints = rtmon.q_actual(wait=False, timestamp=False)
self._data.append((timestamp, ctrlTimestamp, pose, joints))
self._save_data()
self._finished.set()
def run(self):
self._data = []
rtmon = urrtmon.URRTMonitor(self.host)
rtmon.start()
rtmon.start_buffering()
while not self._stop_event.is_set():
data = rtmon.pop_buffer()
if data:
self._data.append(data)
else:
time.sleep(0.002)
self._save_data()
rtmon.stop_buffering()
rtmon.stop()
self._finished.set()
def start_acquisition(self):
self.start()
def stop_acquisition(self):
self._stop_event.set()
stop = stop_acquisition
def get_result(self):
self._stop_event.set()
while not self._finished.is_set():
time.sleep(0.01)
return self._queue.get().copy()
if __name__ == "__main__":
p = Tracker()
try:
p.start()
time.sleep(3)
p.stop()
a = p.get_result()
print("Result is: ", a)
finally:
p.stop()

34
urx/urrtmon.py
View File

@ -17,6 +17,7 @@ import socket
import struct
import time
import threading
from copy import deepcopy
import numpy as np
@ -56,11 +57,12 @@ class URRTMonitor(threading.Thread):
self._buffering = False
self._buffer_lock = threading.Lock()
self._buffer = []
self._buffer_size_max = 100
self._current_csys = None
self._csys = None
self._csys_lock = threading.Lock()
def set_current_csys(self, csys):
self._current_csys = csys
def set_csys(self, csys):
with self._csys_lock:
self._csys = csys
def __recv_bytes(self, nBytes):
''' Facility method for receiving exactly "nBytes" bytes from
@ -158,34 +160,44 @@ class URRTMonitor(threading.Thread):
self._tcp_force = np.array(unp[67:73])
self._tcp = np.array(unp[73:79])
if self._current_csys:
tcp = self._current_csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d
if self._csys:
with self._csys_lock:
tcp = self._csys * m3d.Transform(self._tcp)#might be a godd idea to remove dependancy on m3d
self._tcp = tcp.pose_vector
if self._buffering:
with self._buffer_lock:
self._buffer.append((self._timestamp, self._ctrlTimestamp, self._tcp, self._qActual))#FIXME use named arrays of allow to configure what data to buffer
if len(self._buffer) > self._buffer_size_max:
self.logger.warning("Warning: buffer full")
self._buffer.pop()
with self._dataEvent:
self._dataEvent.notifyAll()
def start_buffering(self, maxsize=100):
def start_buffering(self):
"""
start buffering all data from controller
"""
self._buffer = []
self._buffering = True
self._buffer_size_max = maxsize
def stop_buffering(self):
self._buffering = False
def pop_buffer(self):
"""
return oldest value in buffer
"""
with self._buffer_lock:
if len(self._buffer) > 0:
return self._buffer.pop(0)
else:
return None
def get_buffer(self):
"""
return a copy of the entire buffer
"""
with self._buffer_lock:
return deepcopy(self._buffer)
def get_all_data(self, wait=True):
"""
return all data parsed from robot as a dict

38
urx/urx.py
View File

@ -23,7 +23,6 @@ except ImportError:
from urx import urrtmon
from urx import ursecmon
from urx import tracker
class RobotException(Exception):
@ -45,21 +44,18 @@ class URRobot(object):
logging.basicConfig()
self.logger.setLevel(logLevel)
self.host = host
self.csys = None
self.logger.info("Opening secondary monitor socket")
self.secmon = ursecmon.SecondaryMonitor(self.host, logLevel=logLevel, parserLogLevel=parserLogLevel) #data from robot at 10Hz
self.rtmon = None
if useRTInterface:
self.logger.info("Opening real-time monitor socket")
self.rtmon = urrtmon.URRTMonitor(self.host)# som information is only available on rt interface
else:
self.rtmon = None
self.rtmon = self.get_realtime_monitor()
#the next 3 values must be conservative! otherwise we may wait forever
self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion
#URScript is limited in the character length of floats it accepts
self.max_float_length = 6 # FIXME: check max length!!!
if useRTInterface:
self.rtmon.start()
self.secmon.wait() # make sure we get data to not suprise clients
@ -240,7 +236,7 @@ class URRobot(object):
for i in range(0, 6):
#Rmq: q_target is an interpolated target we have no control over
if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.joinEpsilon:
print("Waiting for end move, q_actual is {}, q_target is {}, diff is {}, epsilon is {}".format( jts["q_actual%s"%i], jts["q_target%s"%i] , jts["q_actual%s"%i] - jts["q_target%s"%i], self.radialEpsilon))
print("Waiting for end move, q_actual is {}, q_target is {}, diff is {}, epsilon is {}".format( jts["q_actual%s"%i], jts["q_target%s"%i] , jts["q_actual%s"%i] - jts["q_target%s"%i], self.joinEpsilon))
finished = False
break
if finished and not self.secmon.is_program_running():
@ -382,7 +378,7 @@ class URRobot(object):
self.secmon.cleanup()
if self.rtmon:
self.rtmon.stop()
shutdown = cleanup #this might be wrong since we could also shutdown the robot from this script
shutdown = cleanup #this might be wrong since we could also shutdown the robot hardware from this script
def set_freedrive(self, val):
if val:
@ -396,6 +392,19 @@ class URRobot(object):
else:
self.send_program("set real")
def get_realtime_monitor(self):
"""
return a pointer to the realtime monitor object
usefull to track robot position for example
"""
if not self.rtmon:
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.rtmon.set_csys(self.csys)
return self.rtmon
class Robot(URRobot):
@ -412,7 +421,6 @@ class Robot(URRobot):
self.csys = None
self.csys_inv = None
self.set_csys("Robot", m3d.Transform()) #identity
self.tracker = None
def set_tcp(self, tcp):
if type(tcp) == m3d.Transform:
@ -590,14 +598,6 @@ class Robot(URRobot):
vector = vector.list
return URRobot.set_gravity(self, vector)
def get_tracker(self):
"""
return an object able to track robot move for logging
"""
t = tracker.Tracker(self.host)
t.set_csys(self.csys)
return t
if not MATH3D:
Robot = URRobot