def true_acc(t):
q = trajectory.orientation(t)
acc_world = trajectory.acceleration(t)
R = quat_to_rotation_matrix(q)
gravity = np.array([0, 0, 9.80665])
acc = R.T @ (acc_world - gravity)
return acc
def test_from_trajectory(sensor):
q_ct, p_ct = sensor.relative_pose
R_ct = quat_to_rotation_matrix(q_ct)
X_trajectory = np.random.uniform(-3, 3, size=3)
X_camera_true = R_ct @ X_trajectory + p_ct
X_camera_actual = sensor.from_trajectory(X_trajectory)
assert_almost_equal(X_camera_actual, X_camera_true)
def test_to_trajectory(sensor):
q_ct, p_ct = sensor.relative_pose
print(q_ct, p_ct)
R_ct = quat_to_rotation_matrix(q_ct)
X_camera = np.random.uniform(-3, 3, size=3)
X_trajectory_true = R_ct.T @ (X_camera - p_ct)
X_trajectory_actual = sensor.to_trajectory(X_camera)
assert_almost_equal(X_trajectory_actual, X_trajectory_true)
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_se3_require_se3_elements():
traj = UniformSE3SplineTrajectory()
q = random_quaternion()
cp_good = np.block(
[[quat_to_rotation_matrix(q),
np.random.uniform(-1, 1, size=(3, 1))], [np.array([0, 0, 0, 1])]])
traj.append_knot(cp_good) # Should not fail
cp_bad_determinant = np.copy(cp_good)
cp_bad_determinant[:3, :3] = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
np.testing.assert_almost_equal(np.linalg.det(cp_bad_determinant[:3, :3]),
-1)
with pytest.raises(ValueError):
traj.append_knot(cp_bad_determinant)
cp_bad_last_row = np.copy(cp_good)
cp_bad_last_row[3] = np.random.uniform(-1, 1, size=4)
cp_bad_last_row[3] /= np.linalg.norm(cp_bad_last_row[3])
with pytest.raises(ValueError):
traj.append_knot(cp_bad_last_row)
def trajectory(request):
"Handcrafted 'simple' trajectory which is at least 5 seconds long"
cls = request.param
if cls == UniformR3SplineTrajectory:
dt = 2.3
t0 = 1.22
control_points = [
np.array([1, 1, 2]),
np.array([1, 2, 1.4]),
np.array([1, 4, 0]),
np.array([-2, 2, 2]),
np.array([-3, -2, 1]),
np.array([-4, -2, 0]),
np.array([-1, 2, 0]),
np.array([-2, -1.5, 1.2])
]
instance = cls(dt, t0)
for cp in control_points:
instance.append_knot(cp)
return instance
elif cls == UniformSO3SplineTrajectory:
dt = 0.6
t0 = 1.22
N = int(np.ceil(5. / dt)) + 3
times = t0 + np.arange(-3, N - 3) * dt
w, axis = np.deg2rad(10), np.array([1., 0, 1])
axis /= np.linalg.norm(axis)
control_points = []
for t in times:
theta = w * t
q = np.empty(4)
q[0] = np.cos(theta / 2)
q[1:] = np.sin(theta / 2) * axis
control_points.append(q)
# # Make unit quaternions and make sure they are not flipped
# control_points /= np.linalg.norm(control_points, axis=1).reshape(-1, 1)
# for i in range(1, len(control_points)):
# q1 = control_points[i-1]
# q2 = control_points[i]
# if np.dot(q1, q2) < 0:
# q2[:] = -q2
instance = cls(dt, t0)
for cp in control_points:
instance.append_knot(cp)
return instance
elif cls == UniformSE3SplineTrajectory:
dt = 2.3
t0 = 1.22
instance = cls(dt, t0)
control_points = [
([1, 0, 2, 3], [1, 4, 6]),
([3, 1, 2, 3], [-1, 2, 3]),
([1, 0, 1, 3], [2, 3, 2]),
([2, 1, 4, 1], [1, 4, 7]),
([1, 0, 2, 3], [1, 4, 6]),
([1, 1, 3, 1], [2, -1, 2]),
]
for q, p in control_points:
q = np.array(q) / np.linalg.norm(q)
p = np.array(p).reshape(3, 1)
R = quat_to_rotation_matrix(q)
T = np.block([[R, p], [np.array([[0., 0., 0., 1.]])]])
instance.append_knot(T)
return instance
elif cls == SplitTrajectory:
# Get examples for R3 and SO3
r3_traj = trajectory(DummyRequest(UniformR3SplineTrajectory))
so3_traj = trajectory(DummyRequest(UniformSO3SplineTrajectory))
instance = SplitTrajectory(r3_traj, so3_traj)
return instance
else:
raise ValueError(f"Fixture simple_trajectory not available for {cls}")
def true_gyro(t):
q = trajectory.orientation(t)
R = quat_to_rotation_matrix(q)
w_world = trajectory.angular_velocity(t)
w_body = R.T @ w_world
return w_body