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)