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)
