Ejemplo n.º 1
0
def frameInterpolate(trans_a, trans_b, weight_b):
    '''
    Interpolate two frames where weight_b=[0,1]
    '''
    [pos_a, quat_a] = poseFromTransform(trans_a)
    [pos_b, quat_b] = poseFromTransform(trans_b)
    pos_c = pos_a *(1-weight_b) + pos_b * weight_b;
    quat_c = botpy.quat_interpolate(quat_a,quat_b, weight_b)
    return transformFromPose(pos_c, quat_c)
Ejemplo n.º 2
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)
Ejemplo n.º 3
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)