Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
#!/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 = 'kmp3:'
prefix = 'kmp:'
realMotors = ["%s%s" % (prefix, x) for x in ('sm1', 'sm2', 'sm3') ]
transformMotors = ["%s%s" % (prefix, x) for x in ('m1', 'm2', 'm3') ]
softMotors = ["%s%s:%s" % (prefix, labStr, x) for x in ('m1', 'm2', 'm3') ]
enablePv = "%s%s:enable" % (prefix, labStr)
print realMotors, transformMotors, softMotors, enablePv
MyTrans = myTransform(realMotors, transformMotors, softMotors, enablePv)
MyTrans.main()