Exemplo n.º 1
0
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)
Exemplo n.º 2
0
def testEulerBotpy():
    '''
    Test some quaternion and euler conversions with botpy
    '''

    quat = transformations.random_quaternion()
    rpy = transformations.euler_from_quaternion(quat)

    rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
    quat2 = botpy.roll_pitch_yaw_to_quat(rpy)

    mat = transformations.quaternion_matrix(quat)
    frame = transformUtils.getTransformFromNumpy(mat)
    rpy3 = transformUtils.rollPitchYawFromTransform(frame)

    print quat, quat2
    print rpy, rpy2, rpy3

    assert isQuatEqual(quat, quat2)
    assert np.allclose(rpy, rpy2)
    assert np.allclose(rpy2, rpy3)