tf_ext.py

Go to the documentation of this file.

# tf_ext.py
# several extention function for tf
import tf
from tf.transformations import *

def transformToMatrix(transform):
    # transform = (pos, quaternion)
    mat = quaternion_matrix(transform[1])
    mat[:3, 3] = transform[0]
    return mat

def xyzxyzwToMatrix(xyzxyzw):
    # convert [x, y, z, xx, yy, zz, ww] to matrix
    # where xx, yy, zz and ww mean orientation parameter.
    return transformToMatrix((xyzxyzw[:3], xyzxyzw[3:]))

def decomposeMatrix(mat):
    # convert matrix into (pos, quaternion) tuple
    pos = mat[:3, 3]
    q = quaternion_from_matrix(mat)
    return (pos, q)