def mecanumTglobal(p,floating_vector=False):
''' 3x1 vector from global to mecanum.
'''
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_gl = tr.invertHomogeneousTransform(_globalT['mecanum']) * p_hom
if floating_vector == False:
return p_gl[0:3]/p_gl[3]
else:
return p_gl[0:3]
def globalTmecanum(p,floating_vector=False):
''' 3x1 vector from mecanum to global.
'''
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_gl = _globalT['mecanum'] * p_hom
if floating_vector == False:
return p_gl[0:3]/p_gl[3]
else:
return p_gl[0:3]
def globalTutm0(p,floating_vector=False):
''' 3x1 vector from utm0 to global.
'''
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_gl = _globalT['utm0'] * p_hom
if floating_vector == False:
return p_gl[0:3]/p_gl[3]
else:
return p_gl[0:3]
def utmcam0Tglobal(p,ang,floating_vector=False):
t = utmcam0Tglobal_mat(ang)
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_c = t * p_hom
if floating_vector == False:
pt = p_c[0:3]/p_c[3]
else:
pt = p_c[0:3]
return pt
def globalTutmcam0(p,ang,floating_vector=False):
t = utmcam0Tglobal_mat(ang)
t = tr.invertHomogeneousTransform(t)
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_c = t * p_hom
if floating_vector == False:
pt = p_c[0:3]/p_c[3]
else:
pt = p_c[0:3]
return pt
