Example #1
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)
Example #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)
Example #3
0
def rollPitchYawToQuaternion(rpy):
    return botpy.roll_pitch_yaw_to_quat(rpy)