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

compute threshold automatically, use m3d for dist

Browse Source
This commit is contained in:
Olivier R-D 2015-06-24 11:20:18 +02:00
parent e4d77578c3
commit 74357b6942
3 changed files with 52 additions and 51 deletions

2
setup.py

@ -3,7 +3,7 @@ from distutils.command.install_data import install_data
setup (name = "urx",
version = "0.9.0",
version = "0.9.3",
description = "Python library to control an UR robot",
author = "Olivier Roulet-Dubonnet",
author_email = "olivier.roulet@gmail.com",

28
urx/robot.py

@ -26,6 +26,12 @@ class Robot(URRobot):
URRobot.__init__(self, host, use_rt)
self.csys = m3d.Transform()
def _get_lin_dist2(self, target):
pose = URRobot.getl(self, wait=True)
target = m3d.Transform(target)
pose = m3d.Transform(pose)
return pose.dist(target)
def set_tcp(self, tcp):
"""
set robot flange to tool tip transformation
@ -40,7 +46,7 @@ class Robot(URRobot):
"""
self.csys = transform
def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
set tool orientation using a orientation matric from math3d
or a orientation vector
@ -51,7 +57,7 @@ class Robot(URRobot):
trans.orient = orient
self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
move tool in tool coordinate, keeping orientation
"""
@ -67,7 +73,7 @@ class Robot(URRobot):
"""
self.translate_tool((0, 0, -z), acc=acc, vel=vel)
def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
set tool to given pos, keeping constant orientation
"""
@ -76,7 +82,7 @@ class Robot(URRobot):
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
Move Circular: Move to position (circular in tool-space)
see UR documentation
@ -87,7 +93,7 @@ class Robot(URRobot):
if pose is not None:
return self.csys.inverse * m3d.Transform(pose)
def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None):
"""
move tcp to point and orientation defined by a transformation
UR robots have several move commands, by default movel is used but it can be changed
@ -99,14 +105,14 @@ class Robot(URRobot):
if pose is not None:
return self.csys.inverse * m3d.Transform(pose)
def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None):
"""
Add transform expressed in base coordinate
"""
pose = self.get_pose()
return self.set_pose(trans * pose, acc, vel, wait=wait, command=command, threshold=threshold)
def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=None):
"""
Add transform expressed in tool coordinate
"""
@ -155,7 +161,7 @@ class Robot(URRobot):
w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time)
def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None):
"""
Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string
@ -166,7 +172,7 @@ class Robot(URRobot):
else:
return self.set_pose(t, acc, vel, wait=wait, command=command, threshold=threshold)
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None):
"""
Concatenate several movex commands and applies a blending radius
pose_list is a list of pose.
@ -180,13 +186,13 @@ class Robot(URRobot):
new_poses.append(pose)
return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait, threshold=threshold)
def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
move linear to given pose in tool coordinate
"""
return self.movex_tool("movel", pose, acc=acc, vel=vel, wait=wait, threshold=threshold)
def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=None):
t = m3d.Transform(pose)
self.add_pose_tool(t, acc, vel, wait=wait, command=command, threshold=threshold)

73
urx/urrobot.py

@ -185,31 +185,44 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog)
def _wait_for_move(self, target, threshold=0.001, timeout=2):
def _wait_for_move(self, target, threshold=None, timeout=5, joints=False):
"""
wait for a move to complete. Unfortunately there is no good way to know when a move has finished
so for every received data from robot we compute a dist equivalent and when it is lower than
'threshold' we return.
if threshold is not reached within timeout, and exceptino is raised
if threshold is not reached within timeout, an exception is raised
"""
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
self.logger.debug("No threshold set, setting it to %s", threshold)
count = 0
while True:
if not self.is_running():
raise RobotException("Robot stopped")
dist = self._get_dist(target)
dist = self._get_dist(target, joints)
self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
if self.secmon.is_program_running():
if not self.secmon.is_program_running():
if dist < threshold:
self.logger.debug("we are threshold(%s) close to target, move has ended", threshold)
return
count += 1
if count > timeout * 10:
raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, threshold is {}, target is {}, current pose is {}".format(timeout, dist, threshold, target, URRobot.getl(self)))
else:
count = 0
continue
if dist < threshold:
self.logger.debug("move has ended")
return
count += 1
if count > timeout * 10:
raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, target is {}, current pose is {}".format(timeout, dist, target, URRobot.getl(self)))
def _get_dist(self, target):
def _get_dist(self, target, joints=False):
if joints:
return self._get_joints_dist(target)
else:
return self._get_lin_dist(target)
def _get_lin_dist(self, target):
#FIXME: we have an issue here, it seems sometimes the axis angle received from robot
pose = URRobot.getl(self, wait=True)
dist = 0
for i in range(3):
@ -225,24 +238,6 @@ class URRobot(object):
dist += (target[i] - joints[i]) ** 2
return dist ** 0.5
def _wait_for_move_joints(self, target, threshold, timeout=2):
self.logger.debug("Waiting for joint move completion with threshold %s and target %s", threshold, target)
count = 0
while True:
if not self.is_running():
raise RobotException("Robot stopped")
dist = self._get_joints_dist(target)
self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
if self.secmon.is_program_running():
count = 0
continue
if dist < threshold:
self.logger.debug("move has ended")
return
count += 1
if count > timeout * 10:
raise RobotException("Goal not reached but no program has been running for {} seconds. dist is {}, target is {}, current pose is {}".format(timeout, dist, target, URRobot.getl(self)))
def getj(self, wait=False):
"""
get joints position
@ -269,7 +264,7 @@ class URRobot(object):
"""
return self.speedx("speedj", velocities, acc, min_time)
def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=0.001):
def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None):
"""
move in joint space
"""
@ -279,22 +274,22 @@ class URRobot(object):
prog = self._format_move("movej", joints, acc, vel)
self.send_program(prog)
if wait:
self._wait_for_move_joints(joints[:6], threshold=threshold)
self._wait_for_move(joints[:6], threshold=threshold, joints=True)
return self.getj()
def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None):
"""
Send a movel command to the robot. See URScript documentation.
"""
return self.movex("movel", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None):
"""
Send a movep command to the robot. See URScript documentation.
"""
return self.movex("movep", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None):
"""
Send a servoc command to the robot. See URScript documentation.
"""
@ -307,7 +302,7 @@ class URRobot(object):
tpose.append(radius)
return "{}({}[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, prefix, *tpose)
def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None):
"""
Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string
@ -332,7 +327,7 @@ class URRobot(object):
self.logger.debug("Received pose from robot: %s", pose)
return pose
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=None):
"""
Move Circular: Move to position (circular in tool-space)
see UR documentation
@ -345,7 +340,7 @@ class URRobot(object):
self._wait_for_move(pose_to, threshold=threshold)
return self.getl()
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None):
"""
Concatenate several movel commands and applies a blending radius
pose_list is a list of pose.
@ -354,7 +349,7 @@ class URRobot(object):
"""
return self.movexs("movel", pose_list, acc, vel, radius, wait, threshold=threshold)
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None):
"""
Concatenate several movex commands and applies a blending radius
pose_list is a list of pose.