From 60181ca9f9a65ea842631e4caeddbc2ed2552304 Mon Sep 17 00:00:00 2001 From: Olivier R-D Date: Fri, 20 Dec 2013 12:56:52 +0100 Subject: [PATCH] add more tests in test_all.py --- examples/test_all.py | 50 +++++++++++++++++++++++++++++++++++++++----- urx/robot.py | 39 ++++++++++++++++++++++++---------- 2 files changed, 73 insertions(+), 16 deletions(-) diff --git a/examples/test_all.py b/examples/test_all.py index 645fe5c..6824faf 100644 --- a/examples/test_all.py +++ b/examples/test_all.py @@ -4,12 +4,26 @@ Testing script that runs many of the urx methods, while attempting to keep robot from math import pi import time +import sys import urx import logging +if sys.version_info[0] < 3: #support python v2 + input = raw_input + +def wait(): + global do_wait + if do_wait: + input() + if __name__ == "__main__": - rob = urx.Robot("192.168.1.6", logLevel=logging.WARNING) + if len(sys.argv) > 1: + do_wait = False + else: + do_wait = True + + rob = urx.Robot("192.168.1.6", logLevel=logging.INFO) rob.set_tcp((0, 0, 0, 0, 0, 0)) rob.set_payload(0.5, (0, 0, 0)) try: @@ -19,21 +33,26 @@ if __name__ == "__main__": r = 0.01 print("Digital out 0 and 1 are: ", rob.get_digital_out(0), rob.get_digital_out(1) ) print("Analog inputs are: ", rob.get_analog_inputs()) - j = rob.getj() - print("Initial joint configuration is ", j) + initj = rob.getj() + print("Initial joint configuration is ", initj) t = rob.get_transform() print("Transformation from base to tcp is: ", t) + print("Translating in x") + wait() rob.translate((l, 0, 0), acc=a, vel=v) pose = rob.getl() print("robot tcp is at: ", pose) + print("moving in z") + wait() pose[2] += l rob.movel(pose, acc=a, vel=v, wait=False) print("Waiting for end move") rob.wait_for_move() - #FIXME add movep if controller version newer than XX + print("Moving through several points with a radius") + wait() pose[0] -= l p1 = pose[:] pose[2] -= l @@ -41,21 +60,42 @@ if __name__ == "__main__": rob.movels([p1, p2], vel=v, acc=a, radius=r) print("rotate tcp around around base z ") + wait() t.orient.rotate_zb(pi / 8) rob.apply_transform(t, vel=v, acc=a) print("moving in tool z") + wait() rob.translate_tool((0, 0, l), vel=v, acc=a) print("moving in tool -z using speed command") + wait() rob.speedl_tool((0, 0, -v, 0, 0, 0), acc=a, min_time=3) print("Waiting 2 seconds2") time.sleep(2) print("stop robot") rob.stopj() + print("Test movep, it will fail on older controllers") + wait() + init = rob.get_transform() + pose = init.copy() + for _ in range(3): + pose.pos[0] += l + rob.movep(pose, acc=a, vel=v, radius=r) + rob.movep(init, acc=a, vel=v, radius=r)#back to start + + + print("Test movec") + wait() + via = init.copy() + via.pos[0] += l + to = via.copy() + to.pos[1] += l + rob.movec(via, to, acc=a, vel=v, radius=r) + print("Sending robot back to original position") - rob.movej(j, acc=0.8, vel=0.2) + rob.movej(initj, acc=0.8, vel=0.2) finally: diff --git a/urx/robot.py b/urx/robot.py index a3918ea..77029e0 100644 --- a/urx/robot.py +++ b/urx/robot.py @@ -190,13 +190,12 @@ class URRobot(object): self.logger.debug("Waiting for move completion") # it is necessary to wait 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): + for _ in range(3): self.secmon.wait() - if not radius or not MATH3D: - return self._wait_for_move() - else: - self._wait_for_move_radius(radius, target) - + if not radius or not MATH3D: + return self._wait_for_move() + else: + return self._wait_for_move_radius(radius, target) def _wait_for_move(self): while True: @@ -215,15 +214,26 @@ class URRobot(object): return def _wait_for_move_radius(self, radius, target): + print("Wait move with radius") + target = m3d.Transform(target) while True: if not self.is_running(): raise RobotException("Robot stopped") pose = self.get_pose(wait=True) dist = pose.dist(target) + print("dist is ", dist, radius) if (dist < radius) or not self.secmon.is_program_running(): self.logger.debug("move has ended") return + def dist(self, poseA, poseB): + """ + Return the metric distance between two poses as unweighted combined linear + and angular distance. + This would be better not to rely on math3d just for that method... + """ + raise NotImplementedError + def getj(self, wait=False): """ get joints position @@ -325,15 +335,14 @@ class URRobot(object): def get_pose(self, wait=False): return self.getl(wait) - def movec(self, pose, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): + def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): """ Move Circular: Move to position (circular in tool-space) see UR documentation """ - pose = [round(i, self.max_float_length) for i in pose] pose_via = [round(i, self.max_float_length) for i in pose_via] pose_to = [round(i, self.max_float_length) for i in pose_to] - prog = "movec(p%s, p%s, p%s, a=%s, v=%s, r=%s)" % (pose, pose_via, pose_to, acc, vel, radius) + prog = "movec(p%s, p%s, a=%s, v=%s, r=%s)" % (pose_via, pose_to, acc, vel, radius) self.send_program(prog) if not wait: return None @@ -564,14 +573,21 @@ class Robot(URRobot): move linear to given pose in current csys if pose is a list of poses then movels is called """ - if type(pose[0]) in (list, tuple): - return self.movels(pose, acc, vel, radius, wait=wait) t = m3d.Transform(pose) if relative: return self.add_transform_base(t, acc, vel, wait=wait, process=False) else: return self.apply_transform(t, acc, vel, radius, wait=wait, process=False) + def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, radius=0, wait=True): + """ + Move Circular: Move to position (circular in tool-space) + see UR documentation + """ + via = self.csys * m3d.Transform(pose_via) + to = self.csys * m3d.Transform(pose_to) + return URRobot.movec(self, via.pose_vector, to.pose_vector, acc=acc, vel=vel, radius=radius, wait=wait) + def movel_tool(self, pose, acc=None, vel=None, wait=True): """ move linear to given pose in tool coordinate @@ -580,6 +596,7 @@ class Robot(URRobot): self.add_transform_tool(t, acc, vel, wait=wait, process=False) def movep(self, pose, acc=None, vel=None, radius=0, wait=True, relative=False): + pose = m3d.Transform(pose) if relative: return self.add_transform_base(pose, acc, vel, wait=wait, process=True, radius=radius) else: