def frameFromPositionAndRPY(position, rpy):
'''
rpy specified in degrees
'''
rpy = np.radians(rpy)
mat = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
mat[:3,3] = position
return getTransformFromNumpy(mat)
def testEulerToFrame():
'''
Test some euler converions
'''
rpy = transformations.euler_from_quaternion(transformations.random_quaternion())
frame = transformUtils.frameFromPositionAndRPY([0,0,0], np.degrees(rpy))
mat = transformUtils.getNumpyFromTransform(frame)
mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
print mat
print mat2
assert np.allclose(mat, mat2)
def testEulerToFrame():
'''
Test some euler converions
'''
rpy = transformations.euler_from_quaternion(
transformations.random_quaternion())
frame = transformUtils.frameFromPositionAndRPY([0, 0, 0], np.degrees(rpy))
mat = transformUtils.getNumpyFromTransform(frame)
mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
print(mat)
print(mat2)
assert np.allclose(mat, mat2)
def rotation_only(homogeneous): euler = transformations.euler_from_matrix(homogeneous) return transformations.euler_matrix(euler[0], euler[1], euler[2])
