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 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])
