add more tests in test_all.py

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()
+        initj = rob.getj()
-        print("Initial joint configuration is ", j)
+        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:

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:
+        if not radius or not MATH3D:
-                return self._wait_for_move()
+            return self._wait_for_move()
-            else:
+        else:
-                self._wait_for_move_radius(radius, target)
+            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: