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