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

+#!/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()

myLabTransform.py

+#!/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()

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
 

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