Go to the documentation of this file.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 ''' Coordinate frames for testing ROS-interface (copied from Cody).
00029 '''
00030
00031 import numpy as np, math
00032 import copy
00033 import hrl_lib.transforms as tr
00034
00035
00036
00037 _globalT = {
00038 'torso' : None,
00039 'thok0' : None,
00040 'utm0' : None,
00041 'utmcam0': None,
00042 'mecanum': None
00043 }
00044
00045 def create_globalTDict():
00046 """ call the create functions for all the coord frames
00047 """
00048 createTorsoTransform()
00049 createThok0Transform()
00050 createUtm0Transform()
00051 createMecanumTransform()
00052
00053 def createTorsoTransform():
00054 ''' torso frame -> global frame
00055 '''
00056 disp = np.matrix([0.,0.,0.]).T
00057 rot = np.matrix(np.eye(3))
00058 t = tr.composeHomogeneousTransform(rot,disp)
00059 _globalT['torso'] = t
00060
00061 def createThok0Transform():
00062 ''' thok0 frame -> global frame
00063 '''
00064 disp = np.matrix([0.,0.,0.09]).T
00065 rot = np.matrix(np.eye(3))
00066 t = tr.composeHomogeneousTransform(rot,disp)
00067 _globalT['thok0'] = t
00068
00069 def createUtm0Transform():
00070 ''' utm0 frame -> global frame
00071 '''
00072 disp = copy.copy(tr.getDispSubMat(_globalT['thok0']))
00073 disp[2,0] += 0.055
00074 rot = np.matrix(np.eye(3))
00075 t = tr.composeHomogeneousTransform(rot,disp)
00076 _globalT['utm0'] = t
00077
00078 def createMecanumTransform():
00079 ''' mecanum frame -> global frame (ignores the zenither)
00080 '''
00081 disp = np.matrix([-0.25,0.,0.0]).T
00082 rot = np.matrix(np.eye(3))
00083 t = tr.composeHomogeneousTransform(rot,disp)
00084 _globalT['mecanum'] = t
00085
00086 create_globalTDict()
00087
00088
00089 def globalTmecanum(p,floating_vector=False):
00090 ''' 3x1 vector from mecanum to global.
00091 '''
00092 p_hom = tr.xyzToHomogenous(p, floating_vector)
00093 p_gl = _globalT['mecanum'] * p_hom
00094 if floating_vector == False:
00095 return p_gl[0:3]/p_gl[3]
00096 else:
00097 return p_gl[0:3]
00098
00099 def mecanumTglobal(p,floating_vector=False):
00100 ''' 3x1 vector from global to mecanum.
00101 '''
00102 p_hom = tr.xyzToHomogenous(p, floating_vector)
00103 p_gl = tr.invertHomogeneousTransform(_globalT['mecanum']) * p_hom
00104 if floating_vector == False:
00105 return p_gl[0:3]/p_gl[3]
00106 else:
00107 return p_gl[0:3]
00108
00109 def globalTtorso(p,floating_vector=False):
00110 ''' 3x1 vector from torso to global.
00111 '''
00112 p_hom = tr.xyzToHomogenous(p, floating_vector)
00113 p_gl = _globalT['torso'] * p_hom
00114 if floating_vector == False:
00115 return p_gl[0:3]/p_gl[3]
00116 else:
00117 return p_gl[0:3]
00118
00119 def torsoTglobal(p,floating_vector=False):
00120 ''' 3x1 vector from global to torso.
00121 '''
00122 p_hom = tr.xyzToHomogenous(p, floating_vector)
00123 p_gl = tr.invertHomogeneousTransform(_globalT['torso']) * p_hom
00124 if floating_vector == False:
00125 return p_gl[0:3]/p_gl[3]
00126 else:
00127 return p_gl[0:3]
00128
00129 def globalTthok0(p,floating_vector=False):
00130 ''' 3x1 vector from thok0 to global.
00131 '''
00132 p_hom = tr.xyzToHomogenous(p, floating_vector)
00133 p_gl = _globalT['thok0'] * p_hom
00134 if floating_vector == False:
00135 return p_gl[0:3]/p_gl[3]
00136 else:
00137 return p_gl[0:3]
00138
00139 def thok0Tglobal(p,floating_vector=False):
00140 ''' 3x1 vector from global to thok0.
00141 '''
00142 p_hom = tr.xyzToHomogenous(p, floating_vector)
00143 p_gl = tr.invertHomogeneousTransform(_globalT['thok0']) * p_hom
00144 if floating_vector == False:
00145 return p_gl[0:3]/p_gl[3]
00146 else:
00147 return p_gl[0:3]
00148
00149 def globalTutm0(p,floating_vector=False):
00150 ''' 3x1 vector from utm0 to global.
00151 '''
00152 p_hom = tr.xyzToHomogenous(p, floating_vector)
00153 p_gl = _globalT['utm0'] * p_hom
00154 if floating_vector == False:
00155 return p_gl[0:3]/p_gl[3]
00156 else:
00157 return p_gl[0:3]
00158
00159 def utm0Tglobal(p,floating_vector=False):
00160 ''' 3x1 vector from global to utm0.
00161 '''
00162 p_hom = tr.xyzToHomogenous(p, floating_vector)
00163 p_gl = tr.invertHomogeneousTransform(_globalT['utm0']) * p_hom
00164 if floating_vector == False:
00165 return p_gl[0:3]/p_gl[3]
00166 else:
00167 return p_gl[0:3]
00168
00169
00170
00171
00172 def utmcam0Tglobal_mat(ang):
00173 thok0Tglobal_mat = tr.invertHomogeneousTransform(_globalT['thok0'])
00174
00175 disp = np.matrix([0.,0.,0.]).T
00176 tmat = tr.composeHomogeneousTransform(tr.Ry(ang),disp)*thok0Tglobal_mat
00177
00178
00179 x = 0.012
00180 y = -0.056
00181 z = 0.035
00182 r1 = 0.
00183 r2 = 0.
00184 r3 = -0.7
00185 disp = np.matrix([-x,-y,-z]).T
00186 r = tr.Rz(math.radians(-90))*tr.Ry(math.radians(90.))
00187 disp = r*disp
00188 r = r*tr.Rx(math.radians(r1))
00189 r = r*tr.Ry(math.radians(r2))
00190 r = r*tr.Rz(math.radians(r3))
00191
00192 t = tr.composeHomogeneousTransform(r, disp)
00193 tmat = t*tmat
00194 return tmat
00195
00196
00197
00198
00199
00200
00201 def utmcam0Tglobal(p,ang,floating_vector=False):
00202 t = utmcam0Tglobal_mat(ang)
00203 p_hom = tr.xyzToHomogenous(p, floating_vector)
00204 p_c = t * p_hom
00205 if floating_vector == False:
00206 pt = p_c[0:3]/p_c[3]
00207 else:
00208 pt = p_c[0:3]
00209
00210 return pt
00211
00212
00213
00214
00215
00216
00217 def globalTutmcam0(p,ang,floating_vector=False):
00218 t = utmcam0Tglobal_mat(ang)
00219 t = tr.invertHomogeneousTransform(t)
00220 p_hom = tr.xyzToHomogenous(p, floating_vector)
00221 p_c = t * p_hom
00222 if floating_vector == False:
00223 pt = p_c[0:3]/p_c[3]
00224 else:
00225 pt = p_c[0:3]
00226
00227 return pt
00228