From 74357b69423d0a693b202307911b3b4f3f26d0c7 Mon Sep 17 00:00:00 2001 From: Olivier R-D Date: Wed, 24 Jun 2015 11:20:18 +0200 Subject: [PATCH] compute threshold automatically, use m3d for dist --- setup.py | 2 +- urx/robot.py | 28 +++++++++++-------- urx/urrobot.py | 73 +++++++++++++++++++++++--------------------------- 3 files changed, 52 insertions(+), 51 deletions(-) diff --git a/setup.py b/setup.py index c7e1e70..3200cec 100644 --- a/setup.py +++ b/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", diff --git a/urx/robot.py b/urx/robot.py index cc45fa9..441165f 100644 --- a/urx/robot.py +++ b/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) diff --git a/urx/urrobot.py b/urx/urrobot.py index 1e837c0..85265ec 100644 --- a/urx/urrobot.py +++ b/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.