00001 # tf_ext.py 00002 # several extention function for tf 00003 import tf 00004 from tf.transformations import * 00005 00006 def transformToMatrix(transform): 00007 # transform = (pos, quaternion) 00008 mat = quaternion_matrix(transform[1]) 00009 mat[:3, 3] = transform[0] 00010 return mat 00011 00012 def xyzxyzwToMatrix(xyzxyzw): 00013 # convert [x, y, z, xx, yy, zz, ww] to matrix 00014 # where xx, yy, zz and ww mean orientation parameter. 00015 return transformToMatrix((xyzxyzw[:3], xyzxyzw[3:])) 00016 00017 def decomposeMatrix(mat): 00018 # convert matrix into (pos, quaternion) tuple 00019 pos = mat[:3, 3] 00020 q = quaternion_from_matrix(mat) 00021 return (pos, q)