00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 import numpy as np, math
00029 import hrl_lib.transforms as tr
00030
00031
00032
00033 def residualXform( residuals ):
00034 '''
00035 residuals are np.array([ Rz2, Rx, Rz1, dx, dy, dz ])
00036 returns rotResid, dispResid
00037 '''
00038 rotResid = tr.Rz( residuals[0] ) * tr.Rx( residuals[1] ) * tr.Rz( residuals[2] )
00039 dispResid = np.matrix([ residuals[3], residuals[4], residuals[5] ]).T
00040 return rotResid, dispResid
00041
00042 def camTlaser( res = np.zeros(7) ):
00043
00044 rot = tr.Ry( math.radians( 0.0 + res[0] )) * tr.Rz( math.radians( 0.0 + res[1] )) * tr.Rx( math.radians( -90.0 + res[2] )) * tr.Rz( math.radians( -90.0 + res[3]))
00045 disp = np.matrix([ res[4], res[5], res[6] ]).T + np.matrix([ 0.0, 0.0, 0.0 ]).T
00046 return tr.composeHomogeneousTransform(rot, disp)
00047
00048 def rollTtool_pointer( residuals = np.zeros(6) ):
00049 rotResid, dispResid = residualXform( residuals )
00050 rot = rotResid * tr.Rz( math.radians( -10.0 ))
00051 disp = dispResid + np.matrix([ 0.008, 0.0, 0.0 ]).T
00052 return tr.composeHomogeneousTransform(rot, disp)
00053
00054 def rollTtool_MA( residuals = np.zeros(6) ):
00055 rotResid, dispResid = residualXform( residuals )
00056 rot = rotResid * tr.Ry( math.radians( -90.0 ))
00057 disp = dispResid + np.matrix([ 0.0476, 0.0, 0.0 ]).T
00058 return tr.composeHomogeneousTransform(rot, disp)
00059
00060
00061 def panTroll(rollAng, residuals = np.zeros(6) ):
00062 rotResid, dispResid = residualXform( residuals )
00063 rot = rotResid * tr.Rx( -1.0 * rollAng )
00064 disp = dispResid + np.matrix([0.02021, 0.0, 0.04236 ]).T
00065 return tr.composeHomogeneousTransform(rot, disp)
00066
00067 def tiltTpan(panAng, residuals = np.zeros(6) ):
00068 rotResid, dispResid = residualXform( residuals )
00069 rot = rotResid * tr.Rz( -1.0 * panAng )
00070 disp = dispResid + np.matrix([ 0.07124, 0.0, 0.02243 ]).T
00071 return tr.composeHomogeneousTransform(rot, disp)
00072
00073 def laserTtilt(tiltAng, residuals = np.zeros(6) ):
00074 rotResid, dispResid = residualXform( residuals )
00075 rot = rotResid * tr.Ry( +1.0 * tiltAng )
00076 disp = dispResid + np.matrix([ 0.03354, 0.0, 0.23669 ]).T
00077 return tr.composeHomogeneousTransform(rot, disp)
00078
00079 def laserTtool_pointer(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
00080 '''
00081 This is specifically for the off-axis laser pointer! Tool coordinate frame will change for each tool.
00082 Here, residuals are 4x6 array where:
00083 res[0] = rollTtool
00084 res[1] = panTroll
00085 res[2] = tiltTpan
00086 res[3] = laserTtilt
00087 '''
00088 res = residuals
00089 return laserTtilt(tiltAng, res[3] ) * tiltTpan(panAng, res[2] ) * panTroll(rollAng, res[1] ) * rollTtool_pointer(res[0])
00090
00091 def tool_pointerTlaser(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
00092 return tr.invertHomogeneousTransform( laserTtool_pointer(rollAng, panAng, tiltAng, residuals) )
00093
00094
00095 def laserTtool_MA(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
00096 '''
00097 This is specifically for the multi-antenna (MA) tool attachment! Tool coordinate frame will change for each tool.
00098 Here, residuals are 4x6 array where:
00099 res[0] = rollTtool
00100 res[1] = panTroll
00101 res[2] = tiltTpan
00102 res[3] = laserTtilt
00103 '''
00104 res = residuals
00105 return laserTtilt(tiltAng, res[3] ) * tiltTpan(panAng, res[2] ) * panTroll(rollAng, res[1] ) * rollTtool_MA(res[0])
00106
00107 def tool_MATlaser(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
00108 return tr.invertHomogeneousTransform( laserTtool_MA(rollAng, panAng, tiltAng, residuals) )
00109