From 2418566d0873aa0b3e7a3c939bac69f90abb3f79 Mon Sep 17 00:00:00 2001
From: Kevin Peterson <kmpeters@anl.gov>
Date: Mon, 5 Mar 2012 22:21:50 +0000
Subject: [PATCH] Added the actual transform that Ross is using and my
development copy of it.
git-svn-id: https://subversion.xray.aps.anl.gov/kmp/transform/trunk@213 f095f811-4e88-6c84-d33e-e382bdb4531c
---
labTransform.py | 60 +++++++++++++++++++++++++++++++++++++++++++++++
myLabTransform.py | 60 +++++++++++++++++++++++++++++++++++++++++++++++
myTransform.py | 4 ++--
transform.py | 10 +++++++-
4 files changed, 131 insertions(+), 3 deletions(-)
create mode 100644 labTransform.py
create mode 100644 myLabTransform.py
diff --git a/labTransform.py b/labTransform.py
new file mode 100644
index 0000000..da2cc96
--- /dev/null
+++ b/labTransform.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import transform
+from numpy import matrix, sin, cos, degrees, radians
+
+class myTransform(transform.transform):
+ ### The transform functions should be overloaded for each application
+
+ #
+ # Note: transform functions must gracefully handle transform over
+ # the position ranges available to the real motors.
+ #
+
+ # Transform real coordinates into soft coordinates
+ def fwdTrans(self, m_positions, t_positions):
+ """ """
+ m_matrix = matrix(m_positions)
+ m_matrix.shape=3,1
+ th, phi, chi = [radians(x) for x in t_positions]
+ Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
+ Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
+ Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])
+
+ R = Rth * Rchi * Rphi
+
+ retval = R * m_matrix
+
+ #!print retval
+ retval.shape=1,3
+
+ return retval.tolist()[0]
+
+ # Transform soft coordinates into real coordinates
+ def invTrans(self, tm_positions, t_positions):
+ """ """
+ tm_matrix = matrix(tm_positions)
+ tm_matrix.shape=3,1
+ th, phi, chi = [radians(x) for x in t_positions]
+ Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
+ Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
+ Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])
+
+ R = Rth * Rchi * Rphi
+
+ # Inverse (R.I) is the same as the transpose (R.T) for rotations
+
+ retval = R.T * tm_matrix
+ retval.shape=1,3
+ print "retval",retval.tolist()
+
+ return retval.tolist()[0]
+
+if __name__ == "__main__":
+ prefix = '34idc:'
+ realMotors = ["%s%s" % (prefix, x) for x in ('m58:c3:m5', 'm58:c3:m6', 'm58:c0:m8') ]
+ transformMotors = ["%s%s" % (prefix, x) for x in ('aero:c0:m1', 'm58:c3:m1', 'm58:c3:m2') ]
+ softMotors = ["%s%s" % (prefix, x) for x in ('lab:m1', 'lab:m2', 'lab:m3') ]
+ print realMotors, transformMotors, softMotors
+ MyTrans = myTransform(realMotors, transformMotors, softMotors)
+ MyTrans.main()
diff --git a/myLabTransform.py b/myLabTransform.py
new file mode 100644
index 0000000..1ff32af
--- /dev/null
+++ b/myLabTransform.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import transform
+from numpy import matrix, sin, cos, degrees, radians
+
+class myTransform(transform.transform):
+ ### The transform functions should be overloaded for each application
+
+ #
+ # Note: transform functions must gracefully handle transform over
+ # the position ranges available to the real motors.
+ #
+
+ # Transform real coordinates into soft coordinates
+ def fwdTrans(self, m_positions, t_positions):
+ """ """
+ m_matrix = matrix(m_positions)
+ m_matrix.shape=3,1
+ th, phi, chi = [radians(x) for x in t_positions]
+ Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
+ Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
+ Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])
+
+ R = Rth * Rchi * Rphi
+
+ retval = R * m_matrix
+
+ #!print retval
+ retval.shape=1,3
+
+ return retval.tolist()[0]
+
+ # Transform soft coordinates into real coordinates
+ def invTrans(self, tm_positions, t_positions):
+ """ """
+ tm_matrix = matrix(tm_positions)
+ tm_matrix.shape=3,1
+ th, phi, chi = [radians(x) for x in t_positions]
+ Rth = matrix([[cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1]])
+ Rphi = matrix([[1, 0, 0],[0, cos(phi), -sin(phi)],[0, sin(phi), cos(phi)]])
+ Rchi = matrix([[sin(chi), 0, -cos(chi)], [0, 1, 0], [cos(chi), 0, sin(chi)]])
+
+ R = Rth * Rchi * Rphi
+
+ # Inverse (R.I) is the same as the transpose (R.T) for rotations
+
+ retval = R.T * tm_matrix
+ retval.shape=1,3
+ print "retval",retval.tolist()
+
+ return retval.tolist()[0]
+
+if __name__ == "__main__":
+ prefix = 'kmp2:'
+ realMotors = ["%s%s" % (prefix, x) for x in ('m1', 'm2', 'm3') ]
+ transformMotors = ["%s%s" % (prefix, x) for x in ('m4', 'm5', 'm6') ]
+ softMotors = ["%s%s" % (prefix, x) for x in ('lab:m1', 'lab:m2', 'lab:m3') ]
+ print realMotors, transformMotors, softMotors
+ MyTrans = myTransform(realMotors, transformMotors, softMotors)
+ MyTrans.main()
diff --git a/myTransform.py b/myTransform.py
index fd53d84..b7c1cef 100644
--- a/myTransform.py
+++ b/myTransform.py
@@ -24,7 +24,7 @@ class myTransform(transform.transform):
# Inverse is R.I
- retval = m_matrix * R
+ retval = R* m_matrix
#!print retval
@@ -41,7 +41,7 @@ class myTransform(transform.transform):
R = Rx * Ry * Rz
- retval = tm_matrix * R.I
+ retval = R.I * tm_matrix
#!print retval
diff --git a/transform.py b/transform.py
index 08cd4a1..bfc3092 100644
--- a/transform.py
+++ b/transform.py
@@ -162,12 +162,20 @@ class transform:
if self.softMotorInitiatedMove == False:
# The following sleep is required to prevent problems when making small moves
sleep(0.05)
- tm_positions = [x.get() for x in self.tm_rbv]
+ #tm_positions = [x.get() for x in self.tm_rbv]
+ ### MAYBE SHOULD RECALCULATE BASED ON REAL MOTORS INSTEAD
+ m_positions = [x.get() for x in self.m_val]
+ t_positions = [x.get() for x in self.t_rbv]
+
+ # Do the forward transform
+ tm_positions = self.fwdTrans(m_positions, t_positions)
#!print "syncing these target positions:", tm_positions
for i in range(len(self.tm_rbv)):
self.tm_val[i].put(tm_positions[i])
+
+
# For soft-motor-initiate moves don't do anything
else:
# reset soft-motor-initiated-move indicator
--
GitLab