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)
