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

even more changes to _wait_for_move, add threshold parameters

Browse Source
This commit is contained in:
Olivier R-D 2015-06-19 09:43:46 +02:00
parent 5e4c8a0696
commit e4d77578c3
2 changed files with 67 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
urx/robot.py
View File

@ -40,7 +40,7 @@ class Robot(URRobot):
"""
self.csys = transform
def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True):
def set_orientation(self, orient, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
set tool orientation using a orientation matric from math3d
or a orientation vector
@ -49,9 +49,9 @@ class Robot(URRobot):
orient = m3d.Orientation(orient)
trans = self.get_pose()
trans.orient = orient
self.set_pose(trans, acc, vel, wait=wait)
self.set_pose(trans, acc, vel, wait=wait, threshold=threshold)
def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True):
def translate_tool(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
move tool in tool coordinate, keeping orientation
"""
@ -59,7 +59,7 @@ class Robot(URRobot):
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
t.pos += vect
return self.add_pose_tool(t, acc, vel, wait=wait)
return self.add_pose_tool(t, acc, vel, wait=wait, threshold=threshold)
def back(self, z=0.05, acc=0.01, vel=0.01):
"""
@ -67,27 +67,27 @@ 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):
def set_pos(self, vect, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
set tool to given pos, keeping constant orientation
"""
if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), m3d.Vector(vect))
return self.set_pose(trans, acc, vel, wait=wait)
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):
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
Move Circular: Move to position (circular in tool-space)
see UR documentation
"""
pose_via = self.csys * m3d.Transform(pose_via)
pose_to = self.csys * m3d.Transform(pose_to)
pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait)
pose = URRobot.movec(self, pose_via.pose_vector, pose_to.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold)
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"):
def set_pose(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
"""
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
@ -95,23 +95,23 @@ class Robot(URRobot):
"""
self.logger.debug("Setting pose to %s", trans.pose_vector)
t = self.csys * trans
pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait)
pose = URRobot.movex(self, command, t.pose_vector, acc=acc, vel=vel, wait=wait, threshold=threshold)
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"):
def add_pose_base(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
"""
Add transform expressed in base coordinate
"""
pose = self.get_pose()
return self.set_pose(trans * pose, acc, vel, wait=wait, command=command)
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"):
def add_pose_tool(self, trans, acc=0.01, vel=0.01, wait=True, command="movel", threshold=0.001):
"""
Add transform expressed in tool coordinate
"""
pose = self.get_pose()
return self.set_pose(pose * trans, acc, vel, wait=wait, command=command)
return self.set_pose(pose * trans, acc, vel, wait=wait, command=command, threshold=threshold)
def get_pose(self, wait=False, _log=True):
"""
@ -155,18 +155,18 @@ 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):
def movex(self, command, pose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
"""
Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string
"""
t = m3d.Transform(pose)
if relative:
return self.add_pose_base(t, acc, vel, wait=wait, command=command)
return self.add_pose_base(t, acc, vel, wait=wait, command=command, threshold=threshold)
else:
return self.set_pose(t, acc, vel, wait=wait, command=command)
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):
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
"""
Concatenate several movex commands and applies a blending radius
pose_list is a list of pose.
@ -178,17 +178,17 @@ class Robot(URRobot):
t = self.csys * m3d.Transform(pose)
pose = t.pose_vector
new_poses.append(pose)
return URRobot.movexs(self, command, new_poses, acc, vel, radius, wait=wait)
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):
def movel_tool(self, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
move linear to given pose in tool coordinate
"""
return self.movex_tool("movel", pose, acc=acc, vel=vel, wait=wait)
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):
def movex_tool(self, command, pose, acc=0.01, vel=0.01, wait=True, threshold=0.001):
t = m3d.Transform(pose)
self.add_pose_tool(t, acc, vel, wait=wait, command=command)
self.add_pose_tool(t, acc, vel, wait=wait, command=command, threshold=threshold)
def getl(self, wait=False, _log=True):
"""

76
urx/urrobot.py
View File

@ -185,28 +185,29 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog)
def _wait_for_move(self, target, radius=0.001, timeout=2):
def _wait_for_move(self, target, threshold=0.001, timeout=2):
"""
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
'radius' we return.
if radius is not reached within timeout, and exceptino is raised
'threshold' we return.
if threshold is not reached within timeout, and exceptino is raised
"""
self.logger.info("Waiting for move completion using radius %s and target %s", radius, target)
self.logger.debug("Waiting for move completion using threshold %s and target %s", threshold, target)
count = 0
while True:
if not self.is_running():
raise RobotException("Robot stopped")
dist = self._get_dist(target)
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
if dist < radius:
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)))
if self.secmon.is_program_running():
count = 0
def _get_dist(self, target):
pose = URRobot.getl(self, wait=True)
@ -217,21 +218,30 @@ class URRobot(object):
dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like
return dist ** 0.5
def _wait_for_move_joints(self, radius, target):
#FIXME raise exception if target not reached after x seconds
self.logger.debug("Waiting for joint move completion with radius %s and target %s", radius, target)
while True:
if not self.is_running():
raise RobotException("Robot stopped")
def _get_joints_dist(self, target):
joints = self.getj(wait=True)
dist = 0
for i in range(6):
dist += (target[i] - joints[i]) ** 2
dist = dist ** 0.5
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius)
if dist < radius:
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):
"""
@ -259,7 +269,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):
def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=0.001):
"""
move in joint space
"""
@ -269,26 +279,26 @@ class URRobot(object):
prog = self._format_move("movej", joints, acc, vel)
self.send_program(prog)
if wait:
self._wait_for_move_joints(0.001, joints[:6])
self._wait_for_move_joints(joints[:6], threshold=threshold)
return self.getj()
def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False):
def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
"""
Send a movel command to the robot. See URScript documentation.
"""
return self.movex("movel", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
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):
def movep(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
"""
Send a movep command to the robot. See URScript documentation.
"""
return self.movex("movep", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
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):
def servoc(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
"""
Send a servoc command to the robot. See URScript documentation.
"""
return self.movex("servoc", tpose, acc=acc, vel=vel, wait=wait, relative=relative)
return self.movex("servoc", tpose, acc=acc, vel=vel, wait=wait, relative=relative, threshold=threshold)
def _format_move(self, command, tpose, acc, vel, radius=0, prefix=""):
tpose = [round(i, self.max_float_length) for i in tpose]
@ -297,7 +307,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):
def movex(self, command, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=0.001):
"""
Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string
@ -308,7 +318,7 @@ class URRobot(object):
prog = self._format_move(command, tpose, acc, vel, prefix="p")
self.send_program(prog)
if wait:
self._wait_for_move(tpose[:6])
self._wait_for_move(tpose[:6], threshold=threshold)
return self.getl()
def getl(self, wait=False, _log=True):
@ -322,7 +332,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):
def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=0.001):
"""
Move Circular: Move to position (circular in tool-space)
see UR documentation
@ -332,19 +342,19 @@ class URRobot(object):
prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, "0")
self.send_program(prog)
if wait:
self._wait_for_move(pose_to)
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):
def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
"""
Concatenate several movel commands and applies a blending radius
pose_list is a list of pose.
This method is usefull since any new command from python
to robot make the robot stop
"""
return self.movexs("movel", pose_list, acc, vel, radius, wait)
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):
def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=0.001):
"""
Concatenate several movex commands and applies a blending radius
pose_list is a list of pose.
@ -361,7 +371,7 @@ class URRobot(object):
prog += end
self.send_program(prog)
if wait:
self._wait_for_move(target=pose_list[-1])
self._wait_for_move(target=pose_list[-1], threshold=threshold)
return self.getl()
def stopl(self, acc=0.5):