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 testEuler():
'''
Test some euler conversions
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
mat = transformUtils.getNumpyFromTransform(frame)
rpy = transformUtils.rollPitchYawFromTransform(frame)
rpy2 = transformations.euler_from_matrix(mat)
print rpy
print rpy2
assert np.allclose(rpy, rpy2)
def rotation_only(homogeneous):
euler = transformations.euler_from_matrix(homogeneous)
return transformations.euler_matrix(euler[0], euler[1], euler[2])
def rotation_only(homogeneous): euler = transformations.euler_from_matrix(homogeneous) return transformations.euler_matrix(euler[0], euler[1], euler[2])