def homogeneous2translation_quaternion(homogeneous): """ Translation: [x, y, z] Quaternion: [x, y, z, w] """ translation = transformations.translation_from_matrix(homogeneous) quaternion = transformations.quaternion_from_matrix(homogeneous) return translation, quaternion
def poseFromTransform(transform): ''' Returns position, quaternion ''' mat = getNumpyFromTransform(transform) return np.array(mat[:3, 3]), transformations.quaternion_from_matrix( mat, isprecise=True)
def poseFromTransform(transform): """ Returns position, quaternion mfallon: I do not trust this function, test case of a transform of rpy [0,0,140] does match other tests (esp ROS) """ mat = getNumpyFromTransform(transform) return ( np.array(mat[:3, 3]), transformations.quaternion_from_matrix(mat, isprecise=True), )
def serialize_transform(tform): return { "translation": list(transformations.translation_from_matrix(tform)), "quaternion": list(transformations.quaternion_from_matrix(tform)) }
def poseFromTransform(transform): ''' Returns position, quaternion ''' mat = getNumpyFromTransform(transform) return np.array(mat[:3,3]), transformations.quaternion_from_matrix(mat, isprecise=True)