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