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