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_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
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
