def test_velocity_numerical(trajectory):
import scipy.misc
t = safe_time(trajectory)
dt = 1e-3
vel_num = scipy.misc.derivative(trajectory.position, t, dx=dt, n=1)
vel_actual = trajectory.velocity(t)
np.testing.assert_almost_equal(vel_actual, vel_num, decimal=3)
def test_orientation_return_type(trajectory):
t = safe_time(trajectory)
q = trajectory.orientation(t)
assert q.shape == (4,)
np.testing.assert_almost_equal(np.linalg.norm(q), 1)
w = trajectory.angular_velocity(t)
assert w.shape == (3,)
def test_translation_return_type(trajectory):
t = safe_time(trajectory)
p = trajectory.position(t)
assert p.shape == (3,)
v = trajectory.velocity(t)
assert v.shape == (3,)
a = trajectory.acceleration(t)
assert a.shape == (3,)
def test_to_world(trajectory):
t = safe_time(trajectory)
Rwt = quat_to_rotation_matrix(trajectory.orientation(t))
pwt = trajectory.position(t)
X_traj = np.random.uniform(-10, 10, size=3)
X_world_expected = Rwt @ X_traj + pwt
X_world_actual = trajectory.to_world(X_traj, t)
np.testing.assert_almost_equal(X_world_actual, X_world_expected)
def test_acceleration_numerical(trajectory):
import scipy.misc
if type(trajectory) == UniformSE3SplineTrajectory:
pytest.xfail("SE3 fails because second order derivative is not the same as body acceleration")
t = safe_time(trajectory)
dt = 1e-3
acc_num = scipy.misc.derivative(trajectory.velocity, t, dx=dt, n=1)
acc_actual = trajectory.acceleration(t)
np.testing.assert_almost_equal(acc_actual, acc_num, decimal=3)
def test_imu_measurement_time_offset(mcls, imu, split_trajectory):
t = safe_time(split_trajectory)
d = np.random.uniform(-imu.max_time_offset, imu.max_time_offset)
v = np.random.uniform(-1, 1, size=3)
m1 = mcls(imu, t, v)
y1 = m1.measure(split_trajectory)
imu.time_offset = d
m2 = mcls(imu, t - d, v)
y2 = m2.measure(split_trajectory)
np.testing.assert_equal(y1, y2)
def test_angular_velocity_numerical(trajectory):
import scipy.misc
from kontiki.rotations import quat_mult, quat_conj
t = safe_time(trajectory)
dt = 1e-3
q = trajectory.orientation(t)
dq_num = scipy.misc.derivative(trajectory.orientation, t, dx=dt, n=1)
w_num = (2 * quat_mult(dq_num, quat_conj(q)))[1:]
print()
print(f'Python: q={q}')
print(f'Python: dq={dq_num}')
w_actual = trajectory.angular_velocity(t)
np.testing.assert_almost_equal(w_actual, w_num, decimal=4)
def test_locks_effective(trajectory):
imu = ConstantBiasImu()
imu.accelerometer_bias_locked = True
imu.gyroscope_bias_locked = True
t = safe_time(trajectory)
ma = AccelerometerMeasurement(imu, t, np.array([5, 6, 2]))
mg = GyroscopeMeasurement(imu, t, np.array([1, 2, 3]))
estimator_locked = TrajectoryEstimator(trajectory)
estimator_locked.add_measurement(ma)
estimator_locked.add_measurement(mg)
summary_locked = estimator_locked.solve(max_iterations=2)
imu.accelerometer_bias_locked = False
imu.gyroscope_bias_locked = False
estimator_unlocked = TrajectoryEstimator(trajectory)
estimator_unlocked.add_measurement(ma)
estimator_unlocked.add_measurement(mg)
summary_unlocked = estimator_unlocked.solve()
assert summary_unlocked.num_parameters_reduced == summary_locked.num_parameters_reduced + (2 * 3)
def test_has_gyroscope(imu, trajectory):
t = safe_time(trajectory)
w = imu.gyroscope(trajectory, t)
def test_has_acceleration(imu, trajectory):
t = safe_time(trajectory)
acc = imu.accelerometer(trajectory, t)