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 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 set tool orientation using a orientation matric from math3d
or a orientation vector or a orientation vector
@ -49,9 +49,9 @@ class Robot(URRobot):
orient = m3d.Orientation(orient) orient = m3d.Orientation(orient)
trans = self.get_pose() trans = self.get_pose()
trans.orient = orient 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 move tool in tool coordinate, keeping orientation
""" """
@ -59,7 +59,7 @@ class Robot(URRobot):
if not isinstance(vect, m3d.Vector): if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
t.pos += 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): 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) 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 set tool to given pos, keeping constant orientation
""" """
if not isinstance(vect, m3d.Vector): if not isinstance(vect, m3d.Vector):
vect = m3d.Vector(vect) vect = m3d.Vector(vect)
trans = m3d.Transform(self.get_orientation(), 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) Move Circular: Move to position (circular in tool-space)
see UR documentation see UR documentation
""" """
pose_via = self.csys * m3d.Transform(pose_via) pose_via = self.csys * m3d.Transform(pose_via)
pose_to = self.csys * m3d.Transform(pose_to) 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: if pose is not None:
return self.csys.inverse * m3d.Transform(pose) 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 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 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) self.logger.debug("Setting pose to %s", trans.pose_vector)
t = self.csys * trans 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: if pose is not None:
return self.csys.inverse * m3d.Transform(pose) 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 Add transform expressed in base coordinate
""" """
pose = self.get_pose() 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 Add transform expressed in tool coordinate
""" """
pose = self.get_pose() 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): 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:]) w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
URRobot.speedl(self, np.concatenate((v.array, w.array)), acc, min_time) 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 Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string sends whatever is defined in 'command' string
""" """
t = m3d.Transform(pose) t = m3d.Transform(pose)
if relative: 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: 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 Concatenate several movex commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
@ -178,17 +178,17 @@ class Robot(URRobot):
t = self.csys * m3d.Transform(pose) t = self.csys * m3d.Transform(pose)
pose = t.pose_vector pose = t.pose_vector
new_poses.append(pose) 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 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) 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): def getl(self, wait=False, _log=True):
""" """

78
urx/urrobot.py
View File

@ -185,28 +185,29 @@ class URRobot(object):
prog = "set_tool_voltage(%s)" % (val) prog = "set_tool_voltage(%s)" % (val)
self.send_program(prog) 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 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 so for every received data from robot we compute a dist equivalent and when it is lower than
'radius' we return. 'threshold' we return.
if radius is not reached within timeout, and exceptino is raised 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 count = 0
while True: while True:
if not self.is_running(): if not self.is_running():
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
dist = self._get_dist(target) dist = self._get_dist(target)
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius) self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
if dist < radius: if self.secmon.is_program_running():
count = 0
continue
if dist < threshold:
self.logger.debug("move has ended") self.logger.debug("move has ended")
return return
count += 1 count += 1
if count > timeout * 10: 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))) 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): def _get_dist(self, target):
pose = URRobot.getl(self, wait=True) pose = URRobot.getl(self, wait=True)
@ -216,22 +217,31 @@ class URRobot(object):
for i in range(3, 6): for i in range(3, 6):
dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like dist += ((target[i] - pose[i]) / 5) ** 2 # arbitraty length like
return dist ** 0.5 return dist ** 0.5
def _get_joints_dist(self, target):
joints = self.getj(wait=True)
dist = 0
for i in range(6):
dist += (target[i] - joints[i]) ** 2
return dist ** 0.5
def _wait_for_move_joints(self, radius, target): def _wait_for_move_joints(self, target, threshold, timeout=2):
#FIXME raise exception if target not reached after x seconds self.logger.debug("Waiting for joint move completion with threshold %s and target %s", threshold, target)
self.logger.debug("Waiting for joint move completion with radius %s and target %s", radius, target) count = 0
while True: while True:
if not self.is_running(): if not self.is_running():
raise RobotException("Robot stopped") raise RobotException("Robot stopped")
joints = self.getj(wait=True) dist = self._get_joints_dist(target)
dist = 0 self.logger.debug("distance to target is: %s, target dist is %s", dist, threshold)
for i in range(6): if self.secmon.is_program_running():
dist += (target[i] - joints[i]) ** 2 count = 0
dist = dist ** 0.5 continue
self.logger.debug("distance to target is: %s, target dist is %s", dist, radius) if dist < threshold:
if dist < radius:
self.logger.debug("move has ended") self.logger.debug("move has ended")
return 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): def getj(self, wait=False):
""" """
@ -259,7 +269,7 @@ class URRobot(object):
""" """
return self.speedx("speedj", velocities, acc, min_time) 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 move in joint space
""" """
@ -269,26 +279,26 @@ class URRobot(object):
prog = self._format_move("movej", joints, acc, vel) prog = self._format_move("movej", joints, acc, vel)
self.send_program(prog) self.send_program(prog)
if wait: if wait:
self._wait_for_move_joints(0.001, joints[:6]) self._wait_for_move_joints(joints[:6], threshold=threshold)
return self.getj() 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. 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. 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. 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=""): def _format_move(self, command, tpose, acc, vel, radius=0, prefix=""):
tpose = [round(i, self.max_float_length) for i in tpose] tpose = [round(i, self.max_float_length) for i in tpose]
@ -297,7 +307,7 @@ class URRobot(object):
tpose.append(radius) tpose.append(radius)
return "{}({}[{},{},{},{},{},{}], a={}, v={}, r={})".format(command, prefix, *tpose) 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 Send a move command to the robot. since UR robotene have several methods this one
sends whatever is defined in 'command' string sends whatever is defined in 'command' string
@ -308,7 +318,7 @@ class URRobot(object):
prog = self._format_move(command, tpose, acc, vel, prefix="p") prog = self._format_move(command, tpose, acc, vel, prefix="p")
self.send_program(prog) self.send_program(prog)
if wait: if wait:
self._wait_for_move(tpose[:6]) self._wait_for_move(tpose[:6], threshold=threshold)
return self.getl() return self.getl()
def getl(self, wait=False, _log=True): def getl(self, wait=False, _log=True):
@ -322,7 +332,7 @@ class URRobot(object):
self.logger.debug("Received pose from robot: %s", pose) self.logger.debug("Received pose from robot: %s", pose)
return 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) Move Circular: Move to position (circular in tool-space)
see UR documentation 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") prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, "0")
self.send_program(prog) self.send_program(prog)
if wait: if wait:
self._wait_for_move(pose_to) self._wait_for_move(pose_to, threshold=threshold)
return self.getl() 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 Concatenate several movel commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
This method is usefull since any new command from python This method is usefull since any new command from python
to robot make the robot stop 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 Concatenate several movex commands and applies a blending radius
pose_list is a list of pose. pose_list is a list of pose.
@ -361,7 +371,7 @@ class URRobot(object):
prog += end prog += end
self.send_program(prog) self.send_program(prog)
if wait: if wait:
self._wait_for_move(target=pose_list[-1]) self._wait_for_move(target=pose_list[-1], threshold=threshold)
return self.getl() return self.getl()
def stopl(self, acc=0.5): def stopl(self, acc=0.5):