From 09e446ec708e9cabb3d79f90f27d880e1cedaa94 Mon Sep 17 00:00:00 2001
From: Olivier R-D <olivier@dummy.com>
Date: Fri, 20 Sep 2013 12:30:40 +0200
Subject: [PATCH] add test_all script, small cleanup

---
 examples/test_all.py         | 62 ++++++++++++++++++++++++++++++++++++
 urx/__init__.py              |  2 +-
 urx/{urrobot.py => robot.py} |  3 +-
 3 files changed, 64 insertions(+), 3 deletions(-)
 create mode 100644 examples/test_all.py
 rename urx/{urrobot.py => robot.py} (99%)

diff --git a/examples/test_all.py b/examples/test_all.py
new file mode 100644
index 0000000..1eb382b
--- /dev/null
+++ b/examples/test_all.py
@@ -0,0 +1,62 @@
+"""
+Testing script that runs many of the urx methods, while attempting to keep robot pose around its starting pose
+"""
+
+from math import pi
+import time
+
+import urx
+import logging
+
+if __name__ == "__main__":
+    rob = urx.Robot("192.168.1.6", logLevel=logging.WARNING)
+    rob.set_tcp((0, 0, 0, 0, 0, 0))
+    rob.set_payload(0.5, (0, 0, 0))
+    try:
+        l = 0.05
+        v = 0.05
+        a = 0.3
+        r = 0.01
+        print("Digital out 0 and 1 are: ", rob.get_digital_out(0), rob.get_digital_out(1) )
+        j = rob.getj()
+        print("Initial joint configuration is ", j)
+        t = rob.get_transform()
+        print("Transformation from base to tcp is: ", t)
+        print("Translating in x")
+        rob.translate((l, 0, 0), acc=a, vel=v)
+        pose = rob.getl()
+        print("robot tcp is at: ", pose)
+        print("moving in z")
+        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")
+        pose[0] -= l
+        p1 = pose[:]
+        pose[2] -= l
+        p2 = pose[:]
+        rob.movels([p1, p2], vel=v, acc=a, radius=r)
+
+        print("rotate tcp around around base z ")
+        t.orient.rotate_zb(pi / 8)
+        rob.apply_transform(t, vel=v, acc=a)
+
+        print("moving in tool z")
+        rob.translate_tool((0, 0, l), vel=v, acc=a)
+
+        print("moving in tool -z using speed command")
+        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("Sending robot back to original position")
+        rob.movej(j, acc=0.8, vel=0.2) 
+
+
+    finally:
+        rob.cleanup()
+
diff --git a/urx/__init__.py b/urx/__init__.py
index 9da131b..d12d82a 100644
--- a/urx/__init__.py
+++ b/urx/__init__.py
@@ -5,5 +5,5 @@ __version__ = "0.8"
 
 
 
-from .urrobot import Robot, RobotException, URRobot
+from .robot import Robot, RobotException, URRobot
 
diff --git a/urx/urrobot.py b/urx/robot.py
similarity index 99%
rename from urx/urrobot.py
rename to urx/robot.py
index df2c521..bee94f4 100644
--- a/urx/urrobot.py
+++ b/urx/robot.py
@@ -550,7 +550,6 @@ class Robot(URRobot):
         pose = self.get_transform()
         v = self.csys.orient * pose.orient * m3d.Vector(velocities[:3])
         w = self.csys.orient * pose.orient * m3d.Vector(velocities[3:])
-        print(pose, v, w)
         URRobot.speedl(self, np.concatenate((v.data, w.data)), acc, min_time)
 
     def movel(self, pose, acc=None, vel=None, wait=True, relative=False, radius=0.01):
@@ -578,7 +577,7 @@ class Robot(URRobot):
         return current transformation from tcp to current csys
         """
         t = self.get_transform(wait)
-        return t.pose_vector
+        return t.pose_vector.tolist()
 
     def movels(self, pose_list, acc=0.01, vel=0.01 , radius=0.01, wait=True):
         """