Exemplo n.º 1
0
def testQuaternionInterpolate():
    '''
    Test quaternion interpolation in botpy
    '''

    q1 = transformations.random_quaternion()
    q2 = transformations.random_quaternion()

    print q1
    print q2

    for weight in np.linspace(0, 1, 10):
        qi = botpy.quat_interpolate(q1, q2, weight)
        qi2 = transformations.quaternion_slerp(q1, q2, weight)

        print weight, qi, qi2
        assert isQuatEqual(qi, qi2)

        if weight == 0.0:
            assert isQuatEqual(qi, q1)

        if weight == 1.0:
            assert isQuatEqual(qi, q2)
Exemplo n.º 2
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.º 3
0
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)
Exemplo n.º 4
0
def testTransform():
    '''
    test transformFromPose --> getAxesFromTransform is same as quat --> matrix
    '''

    quat = transformations.random_quaternion()
    pos = np.random.rand(3)

    frame = transformUtils.transformFromPose(pos, quat)
    axes = transformUtils.getAxesFromTransform(frame)
    mat = transformUtils.getNumpyFromTransform(frame)

    assert np.allclose(mat[:3, :3], np.array(axes).transpose())

    mat2 = transformations.quaternion_matrix(quat)
    mat2[:3, 3] = pos

    print mat
    print mat2
    assert np.allclose(mat, mat2)
Exemplo n.º 5
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)