From 1a0b33490fcb327f0850e37068cc56c69e0d3bb3 Mon Sep 17 00:00:00 2001
From: Olivier R-D <olivier@dummy.com>
Date: Sun, 21 Apr 2013 09:24:59 +0200
Subject: [PATCH] add suppor for several ref coordinate systems

---
 urx/tracker.py |  2 +-
 urx/urx.py     | 33 +++++++++++++++++++++++----------
 2 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/urx/tracker.py b/urx/tracker.py
index be11d89..e71168b 100644
--- a/urx/tracker.py
+++ b/urx/tracker.py
@@ -25,7 +25,7 @@ class Tracker(Process):
     def _log(self, *args):
         print(self.__class__.__name__, ": ".join([str(i) for i in args]))
 
-    def set_calibration_matrix(self, cal):
+    def set_csys(self, cal):
         if MATH3D:
             self.calibration = cal
             self.inverse = self.calibration.inverse()
diff --git a/urx/urx.py b/urx/urx.py
index ed3208e..6d12830 100644
--- a/urx/urx.py
+++ b/urx/urx.py
@@ -385,9 +385,10 @@ class Robot(URRobot):
         URRobot.__init__(self, host, useRTInterface, logLevel=logLevel, parserLogLevel=parserLogLevel)
         self.default_linear_acceleration = 0.01
         self.default_linear_velocity = 0.01
-
-        self.calibration = math3d.Transform() #identity
-        self.inverse = self.calibration.inverse()
+        self.csys_dict = {}
+        self.csys = None
+        self.csys_inv = None
+        self.set_csys("Robot", math3d.Transform()) #identity
         self.tracker = None
 
     def set_tcp(self, x=0, y=0, z=0, rx=0, ry=0, rz=0):
@@ -395,9 +396,21 @@ class Robot(URRobot):
             x = x.pose_vector
         URRobot.set_tcp(self, x, y, z, rx, ry, rz)
 
-    def set_calibration_matrix(self, matrix):
-        self.calibration = matrix
-        self.inverse = self.calibration.inverse()
+    def add_csys(self, name, matrix):
+        self.csys_dict[name] = matrix
+
+    def get_csys_list(self):
+        return self.csys_dict
+
+    def set_csys(self, name, matrix=None):
+        """
+        Set reference corrdinate system to use
+        if matrix != None then a new csys is created
+        """
+        if matrix:
+            self.add_csys(name, matrix)
+        self.csys = self.csys_dict[name]
+        self.csys_inv = self.csys.inverse()
 
     def orient(self, orient, acc=None, vel=None, wait=True):
         if type(orient) != math3d.Orientation:
@@ -423,10 +436,10 @@ class Robot(URRobot):
             acc = self.default_linear_acceleration
         if not vel: 
             vel = self.default_linear_velocity
-        t = self.calibration * trans
+        t = self.csys * trans
         pose = URRobot.movel(self, t.pose_vector, acc, vel, wait)
         if pose != None : #movel does not return anything when wait is False
-            return self.inverse * math3d.Transform(pose)
+            return self.csys_inv * math3d.Transform(pose)
 
     def add_transform_b(self, trans, acc=None, vel=None, wait=True):
         """
@@ -444,7 +457,7 @@ class Robot(URRobot):
 
     def get_transform(self, wait=False):
         pose = URRobot.getl(self, wait)
-        trans = self.inverse * math3d.Transform(pose) 
+        trans = self.csys_inv * math3d.Transform(pose) 
         return trans
 
     def get_orientation(self, wait=False):
@@ -486,7 +499,7 @@ class Robot(URRobot):
         return an object able to track robot move for logging
         """
         t = tracker.Tracker(self.host)
-        t.set_calibration_matrix(self.calibration)
+        t.set_csys(self.csys)
         return t