def test_slerp(self):
     for r1, r2 in itertools.product(rotations_sequence(),
                                     rotations_sequence()):
         q1 = quaternion_from_rotation(r1)
         q2 = quaternion_from_rotation(r2)
         for t in [0, 0.1, 0.5, 0.75, 1]:
             a = slerp(q1, q2, t)
             b = slerp(q2, q1, 1 - t)
             assert_allclose(a, b)
 def test_slerp(self):
     for r1, r2 in itertools.product(rotations_sequence(),
                                     rotations_sequence()):
         q1 = quaternion_from_rotation(r1)
         q2 = quaternion_from_rotation(r2)
         for t in [0, 0.1, 0.5, 0.75, 1]:
             a = slerp(q1, q2, t)
             b = slerp(q2, q1, 1 - t)
             assert_allclose(a, b)
def step(dt, now, phase, orientation, seq):
    now += dt
    phase, omega, orientation = integrate(dt, phase, orientation)
    acceleration = accelerometer(orientation)
    _, _, yaw = rpy(orientation)

    header = Header(seq, rospy.Time(now), 'imu_0')
    imu = Imu(header,
              Quaternion(0, 0, 0, 1),
              np.zeros((9,)),
              Vector3(*omega),
              (np.eye(3)*1e-8).reshape((-1,)),
              Vector3(*acceleration),
              (np.eye(3)*1e-8).reshape((-1,)),
             )
    header = Header(seq, rospy.Time(now), 'gyro')
    quat = rotations.quaternion_from_axis_angle(np.r_[0, 0, 1], yaw)
    gyro = Imu(header,
               Quaternion(quat[1], quat[2], quat[3], quat[0]),
               np.diag([0, 0, 1e-10]).reshape((-1,)),
               Vector3(0,0,0),
               np.zeros((9,)),
               Vector3(0,0,0),
               np.zeros((9,)),
              )
    header = Header(seq, rospy.Time(now), 'odom')
    quat = rotations.quaternion_from_rotation(orientation)
    pose = PoseStamped(header,
            Pose(Vector3(0,0,0),
                Quaternion(quat[1], quat[2], quat[3], quat[0])))
    seq += 1
    return (imu, gyro, pose), (now, phase, orientation, seq)