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

use movep as default, small cleanups many places

Browse Source
This commit is contained in:
Olivier R-D
2013-06-19 09:20:33 +02:00
parent 3f728e3e02
commit d497364919
4 changed files with 70 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
examples/test_movep.py
View File

@@ -1,22 +1,39 @@
import time
import urx import urx
import logging import logging
if __name__ == "__main__": if __name__ == "__main__":
rob = urx.Robot("192.168.128.120", logLevel=logging.INFO) rob = urx.Robot("192.168.128.120", logLevel=logging.INFO)
try: try:
l = 0.05 l = 0.1
v = 0.05 v = 0.07
a = 0.3 a = 0.1
r = 0.005 r = 0.05
pose = rob.getl() pose = rob.getl()
pose[2] += l pose[2] += l
rob.movep(pose, acc=a, vel=v, radius=r, wait=False) rob.movep(pose, acc=a, vel=v, radius=r, wait=False)
while True:
p = rob.getl(wait=True)
if p[2] > pose[2] - 0.05:
break
pose[1] += l pose[1] += l
rob.movep(pose, acc=a, vel=v, radius=r, wait=False) rob.movep(pose, acc=a, vel=v, radius=r, wait=False)
while True:
p = rob.getl(wait=True)
if p[1] > pose[1] - 0.05:
break
pose[2] -= l pose[2] -= l
rob.movep(pose, acc=a, vel=v, radius=r, wait=False) rob.movep(pose, acc=a, vel=v, radius=r, wait=False)
while True:
p = rob.getl(wait=True)
if p[2] < pose[2] + 0.05:
break
pose[1] -= l pose[1] -= l
rob.movep(pose, acc=a, vel=v, radius=0, wait=False) rob.movep(pose, acc=a, vel=v, radius=0, wait=True)
finally: finally:
rob.cleanup() rob.cleanup()

7
urx/tracker.py
View File

@@ -1,3 +1,10 @@
"""
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 import time
from multiprocessing import Process, Queue, Event from multiprocessing import Process, Queue, Event

9
urx/ursecmon.py
View File

@@ -233,8 +233,7 @@ class SecondaryMonitor(Thread):
check program execution status in the secondary client data packet we get from the robot check program execution status in the secondary client data packet we get from the robot
This interface uses only data from the secondary client interface (see UR doc) This interface uses only data from the secondary client interface (see UR doc)
Only the last connected client is the primary client, Only the last connected client is the primary client,
so this is not guaranted and we cannot rely on information only so this is not guaranted and we cannot rely on information to the primary client.
send to the primary client(like program execution start ?!?!?)
""" """
while not self._trystop: while not self._trystop:
@@ -278,13 +277,13 @@ class SecondaryMonitor(Thread):
returns something that looks like a packet, nothing is guaranted returns something that looks like a packet, nothing is guaranted
""" """
while True: while True:
#self.logger.debug("data queue size is: {}".format(len(self._dataqueue))) self.logger.debug("data queue size is: {}".format(len(self._dataqueue)))
ans = self._parser.find_first_packet(self._dataqueue[:]) ans = self._parser.find_first_packet(self._dataqueue[:])
if ans: if ans:
self._dataqueue = ans[1] self._dataqueue = ans[1]
return ans[0] return ans[0]
else: else:
#self.logger.debug("Could not find packet in received data") self.logger.debug("Could not find packet in received data")
tmp = self._s_secondary.recv(1024) tmp = self._s_secondary.recv(1024)
self._dataqueue += tmp self._dataqueue += tmp
@@ -339,6 +338,8 @@ class SecondaryMonitor(Thread):
def cleanup(self): def cleanup(self):
self._trystop = True self._trystop = True
self.join() self.join()
#with self._dataEvent: #wake up any thread that may be waiting for data before we close. Should we do that?
#self._dataEvent.notifyAll()
if self._s_secondary: if self._s_secondary:
with self._prog_queue_lock: with self._prog_queue_lock:
self._s_secondary.close() self._s_secondary.close()

88
urx/urx.py
View File

@@ -6,22 +6,19 @@ http://support.universal-robots.com/URRobot/RemoteAccess
from __future__ import absolute_import # necessary for import tricks to work with python2 from __future__ import absolute_import # necessary for import tricks to work with python2
__author__ = "Olivier Roulet-Dubonnet" __author__ = "Olivier Roulet-Dubonnet"
__copyright__ = "Copyright 2011-2013, Sintef Raufoss Manufacturing Roulet-Dubonnet" __copyright__ = "Copyright 2011-2013, Sintef Raufoss Manufacturing"
__credits__ = ["Olivier Roulet-Dubonnet"] __credits__ = ["Olivier Roulet-Dubonnet"]
__license__ = "GPLv3" __license__ = "GPLv3"
import time
import logging import logging
m3d = True MATH3D = True
try: try:
import math3d as m3d import math3d as m3d
import numpy as np import numpy as np
except ImportError: except ImportError:
m3d = False MATH3D = False
print("pymath3d library could not be found on this computer, disabling use of matrices") print("pymath3d library could not be found on this computer, disabling use of matrices")
from urx import urrtmon from urx import urrtmon
@@ -58,9 +55,7 @@ class URRobot(object):
else: else:
self.rtmon = None self.rtmon = None
#the next 3 values must be conservative! otherwise we may wait forever #the next 3 values must be conservative! otherwise we may wait forever
self.jointEpsilon = 0.05 # precision of joint movem used to wait for move completion self.joinEpsilon = 0.01 # precision of joint movem used to wait for move completion
self.linearEpsilon = 0.0005 # precision of linear movement, used to wait for move completion
self.radialEpsilon = 0.05 # precision of radial movement, used to wait for move completion
#URScript is limited in the character length of floats it accepts #URScript is limited in the character length of floats it accepts
self.max_float_length = 6 # FIXME: check max length!!! self.max_float_length = 6 # FIXME: check max length!!!
if useRTInterface: if useRTInterface:
@@ -227,15 +222,23 @@ class URRobot(object):
self.wait_for_move() self.wait_for_move()
return self.getj() return self.getj()
def wait_for_move(self): def wait_for_move(self):
time.sleep(0.5)# it is important to sleep since robot may takes a while to get into running state, for a physical move 0.5s is very low """
wait until a move is completed
"""
# it is important to sleep since robot may takes a while to get into running state,
# for a physical move 0.5s is very short
for i in range(3):
self.secmon.wait()
while True: while True:
if not self.is_running(): if not self.is_running():
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
jts = self.secmon.get_joint_data(wait=True) jts = self.secmon.get_joint_data(wait=True)
finished = True finished = True
for i in range(0, 6): for i in range(0, 6):
if abs(jts["q_actual%s"%i] - jts["q_target%s"%i]) > self.radialEpsilon: #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.radialEpsilon))
finished = False finished = False
break break
@@ -338,14 +341,14 @@ class URRobot(object):
self.wait_for_move() self.wait_for_move()
return self.getl() return self.getl()
def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): def moveps(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True):
""" """
Concatenate several movel commands and applies a blending radius Concatenate several movel commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
""" """
header = "def myProg():\n" header = "def myProg():\n"
end = "end\n" end = "end\n"
template = "movel(p[{},{},{},{},{},{}], a={}, v={}, r={})\n" template = "movep(p[{},{},{},{},{},{}], a={}, v={}, r={})\n"
prog = header prog = header
for idx, pose in enumerate(pose_list): for idx, pose in enumerate(pose_list):
pose.append(acc) pose.append(acc)
@@ -372,31 +375,6 @@ class URRobot(object):
def stop(self): def stop(self):
self.stopj() self.stopj()
def _eq(self, l1, l2):
"""
robot joints precision is 0.01, donot give anything smaller!!!
"""
for i in range(0, len(l1)):
if abs(l1[i] -l2[i]) > self.jointEpsilon:
return False
return True
def _eqpose(self, l1, l2):
"""
epsilonl is for x,y,z
epsilonr is for a,b,c
robot joints precision is 0.01, do not give anything smaller!!!
"""
for i in range(0, 3):
if abs(l1[i] - l2[i]) > self.linearEpsilon:
#print "param: ", i, "val: ", l1[i], "-", l2[i] , "=", abs(l1[i] -l2[i]), " is not under ", self.linearEpsilon
return False
for i in range(3, 6):
if abs(l1[i] - l2[i]) > self.radialEpsilon:
#print "param: ", i, "val: ", l1[i], "-", l2[i] , "=", abs(l1[i] -l2[i]), " is not under ", self.radialEpsilon
return False
return True
def cleanup(self): def cleanup(self):
self.logger.info("Closing sockets to robot") self.logger.info("Closing sockets to robot")
self.secmon.cleanup() self.secmon.cleanup()
@@ -488,32 +466,36 @@ class Robot(URRobot):
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect)) trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.apply_transform(trans, acc, vel, wait) return self.apply_transform(trans, acc, vel, wait)
def apply_transform(self, trans, acc=None, vel=None, wait=True): def apply_transform(self, trans, acc=None, vel=None, wait=True, process=True):
""" """
move tcp to point and orientation defined by a transformation move tcp to point and orientation defined by a transformation
if process is True, movep is used instead of movel
""" """
if not acc: if not acc:
acc = self.default_linear_acceleration acc = self.default_linear_acceleration
if not vel: if not vel:
vel = self.default_linear_velocity vel = self.default_linear_velocity
t = self.csys * trans t = self.csys * trans
pose = URRobot.movel(self, t.pose_vector, acc, vel, wait) if process:
pose = URRobot.movep(self, t.pose_vector, acc=acc, vel=vel, wait=wait)
else:
pose = URRobot.movel(self, t.pose_vector, acc=acc, vel=vel, wait=wait)
if pose != None : #movel does not return anything when wait is False if pose != None : #movel does not return anything when wait is False
return self.csys_inv * m3d.Transform(pose) return self.csys_inv * m3d.Transform(pose)
def add_transform_base(self, trans, acc=None, vel=None, wait=True): def add_transform_base(self, trans, acc=None, vel=None, wait=True, process=True):
""" """
Add transform expressed in base coordinate Add transform expressed in base coordinate
""" """
pose = self.get_transform() pose = self.get_transform()
return self.apply_transform(trans * pose, acc, vel, wait) return self.apply_transform(trans * pose, acc, vel, wait, process)
def add_transform_tool(self, trans, acc=None, vel=None, wait=True): def add_transform_tool(self, trans, acc=None, vel=None, wait=True, process=True):
""" """
Add transform expressed in tool coordinate Add transform expressed in tool coordinate
""" """
pose = self.get_transform() pose = self.get_transform()
return self.apply_transform(pose * trans, acc, vel, wait) return self.apply_transform(pose * trans, acc, vel, wait, process)
def get_transform(self, wait=False): def get_transform(self, wait=False):
""" """
@@ -558,22 +540,22 @@ class Robot(URRobot):
def movel(self, pose, acc=None, vel=None, wait=True, relative=False, radius=0.01): def movel(self, pose, acc=None, vel=None, wait=True, relative=False, radius=0.01):
""" """
move linear to given pose in current csys move linear to given pose in current csys
if pose is a list of poses then movels is called if pose is a list of poses then moveps is called
""" """
if type(pose[0]) == list: if type(pose[0]) in (list, tuple):
return self.movels(pose, acc, vel, radius, wait) return self.moveps(pose, acc, vel, radius, wait)
t = m3d.Transform(pose) t = m3d.Transform(pose)
if relative: if relative:
return self.add_transform_base(t, acc, vel, wait) return self.add_transform_base(t, acc, vel, wait, process=False)
else: else:
return self.apply_transform(t, acc, vel, wait) return self.apply_transform(t, acc, vel, wait, process=False)
def movel_tool(self, pose, acc=None, vel=None, wait=True): def movel_tool(self, pose, acc=None, vel=None, wait=True):
""" """
move linear to given pose in tool coordinate move linear to given pose in tool coordinate
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
self.add_transform_tool(t, acc, vel, wait) self.add_transform_tool(t, acc, vel, wait, process=False)
def getl(self, wait=False): def getl(self, wait=False):
""" """
@@ -582,9 +564,9 @@ class Robot(URRobot):
t = self.get_transform(wait) t = self.get_transform(wait)
return t.pose_vector return t.pose_vector
def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True): def moveps(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True):
""" """
Concatenate several movel commands and applies a blending radius Concatenate several movep commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
""" """
new_poses = [] new_poses = []
@@ -609,7 +591,7 @@ class Robot(URRobot):
return t return t
if not m3d: if not MATH3D:
Robot = URRobot Robot = URRobot
if __name__ == "__main__": if __name__ == "__main__":