def homogeneous2translation_rpy(homogeneous): """ Translation: [x, y, z] RPY: [sx, sy, sz] """ translation = transformations.translation_from_matrix(homogeneous) rpy = transformations.euler_from_matrix(homogeneous) return translation, rpy
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