You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
162 lines
4.9 KiB
Python
162 lines
4.9 KiB
Python
from itertools import product
|
|
import numpy as np
|
|
from numpy.testing import assert_allclose
|
|
from pytest import raises
|
|
from scipy.spatial.transform import Rotation, RotationSpline
|
|
from scipy.spatial.transform._rotation_spline import (
|
|
_angular_rate_to_rotvec_dot_matrix,
|
|
_rotvec_dot_to_angular_rate_matrix,
|
|
_matrix_vector_product_of_stacks,
|
|
_angular_acceleration_nonlinear_term,
|
|
_create_block_3_diagonal_matrix)
|
|
|
|
|
|
def test_angular_rate_to_rotvec_conversions():
|
|
np.random.seed(0)
|
|
rv = np.random.randn(4, 3)
|
|
A = _angular_rate_to_rotvec_dot_matrix(rv)
|
|
A_inv = _rotvec_dot_to_angular_rate_matrix(rv)
|
|
|
|
# When the rotation vector is aligned with the angular rate, then
|
|
# the rotation vector rate and angular rate are the same.
|
|
assert_allclose(_matrix_vector_product_of_stacks(A, rv), rv)
|
|
assert_allclose(_matrix_vector_product_of_stacks(A_inv, rv), rv)
|
|
|
|
# A and A_inv must be reciprocal to each other.
|
|
I_stack = np.empty((4, 3, 3))
|
|
I_stack[:] = np.eye(3)
|
|
assert_allclose(np.matmul(A, A_inv), I_stack, atol=1e-15)
|
|
|
|
|
|
def test_angular_rate_nonlinear_term():
|
|
# The only simple test is to check that the term is zero when
|
|
# the rotation vector
|
|
np.random.seed(0)
|
|
rv = np.random.rand(4, 3)
|
|
assert_allclose(_angular_acceleration_nonlinear_term(rv, rv), 0,
|
|
atol=1e-19)
|
|
|
|
|
|
def test_create_block_3_diagonal_matrix():
|
|
np.random.seed(0)
|
|
A = np.empty((4, 3, 3))
|
|
A[:] = np.arange(1, 5)[:, None, None]
|
|
|
|
B = np.empty((4, 3, 3))
|
|
B[:] = -np.arange(1, 5)[:, None, None]
|
|
d = 10 * np.arange(10, 15)
|
|
|
|
banded = _create_block_3_diagonal_matrix(A, B, d)
|
|
|
|
# Convert the banded matrix to the full matrix.
|
|
k, l = list(zip(*product(np.arange(banded.shape[0]),
|
|
np.arange(banded.shape[1]))))
|
|
k = np.asarray(k)
|
|
l = np.asarray(l)
|
|
|
|
i = k - 5 + l
|
|
j = l
|
|
values = banded.ravel()
|
|
mask = (i >= 0) & (i < 15)
|
|
i = i[mask]
|
|
j = j[mask]
|
|
values = values[mask]
|
|
full = np.zeros((15, 15))
|
|
full[i, j] = values
|
|
|
|
zero = np.zeros((3, 3))
|
|
eye = np.eye(3)
|
|
|
|
# Create the reference full matrix in the most straightforward manner.
|
|
ref = np.block([
|
|
[d[0] * eye, B[0], zero, zero, zero],
|
|
[A[0], d[1] * eye, B[1], zero, zero],
|
|
[zero, A[1], d[2] * eye, B[2], zero],
|
|
[zero, zero, A[2], d[3] * eye, B[3]],
|
|
[zero, zero, zero, A[3], d[4] * eye],
|
|
])
|
|
|
|
assert_allclose(full, ref, atol=1e-19)
|
|
|
|
|
|
def test_spline_2_rotations():
|
|
times = [0, 10]
|
|
rotations = Rotation.from_euler('xyz', [[0, 0, 0], [10, -20, 30]],
|
|
degrees=True)
|
|
spline = RotationSpline(times, rotations)
|
|
|
|
rv = (rotations[0].inv() * rotations[1]).as_rotvec()
|
|
rate = rv / (times[1] - times[0])
|
|
times_check = np.array([-1, 5, 12])
|
|
dt = times_check - times[0]
|
|
rv_ref = rate * dt[:, None]
|
|
|
|
assert_allclose(spline(times_check).as_rotvec(), rv_ref)
|
|
assert_allclose(spline(times_check, 1), np.resize(rate, (3, 3)))
|
|
assert_allclose(spline(times_check, 2), 0, atol=1e-16)
|
|
|
|
|
|
def test_constant_attitude():
|
|
times = np.arange(10)
|
|
rotations = Rotation.from_rotvec(np.ones((10, 3)))
|
|
spline = RotationSpline(times, rotations)
|
|
|
|
times_check = np.linspace(-1, 11)
|
|
assert_allclose(spline(times_check).as_rotvec(), 1, rtol=1e-15)
|
|
assert_allclose(spline(times_check, 1), 0, atol=1e-17)
|
|
assert_allclose(spline(times_check, 2), 0, atol=1e-17)
|
|
|
|
assert_allclose(spline(5.5).as_rotvec(), 1, rtol=1e-15)
|
|
assert_allclose(spline(5.5, 1), 0, atol=1e-17)
|
|
assert_allclose(spline(5.5, 2), 0, atol=1e-17)
|
|
|
|
|
|
def test_spline_properties():
|
|
times = np.array([0, 5, 15, 27])
|
|
angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]
|
|
|
|
rotations = Rotation.from_euler('xyz', angles, degrees=True)
|
|
spline = RotationSpline(times, rotations)
|
|
|
|
assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
|
|
assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])
|
|
|
|
h = 1e-8
|
|
rv0 = spline(times).as_rotvec()
|
|
rvm = spline(times - h).as_rotvec()
|
|
rvp = spline(times + h).as_rotvec()
|
|
assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1e-15)
|
|
|
|
r0 = spline(times, 1)
|
|
rm = spline(times - h, 1)
|
|
rp = spline(times + h, 1)
|
|
assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)
|
|
|
|
a0 = spline(times, 2)
|
|
am = spline(times - h, 2)
|
|
ap = spline(times + h, 2)
|
|
assert_allclose(a0, am, rtol=1e-7)
|
|
assert_allclose(a0, ap, rtol=1e-7)
|
|
|
|
|
|
def test_error_handling():
|
|
raises(ValueError, RotationSpline, [1.0], Rotation.random())
|
|
|
|
r = Rotation.random(10)
|
|
t = np.arange(10).reshape(5, 2)
|
|
raises(ValueError, RotationSpline, t, r)
|
|
|
|
t = np.arange(9)
|
|
raises(ValueError, RotationSpline, t, r)
|
|
|
|
t = np.arange(10)
|
|
t[5] = 0
|
|
raises(ValueError, RotationSpline, t, r)
|
|
|
|
t = np.arange(10)
|
|
|
|
s = RotationSpline(t, r)
|
|
raises(ValueError, s, 10, -1)
|
|
|
|
raises(ValueError, s, np.arange(10).reshape(5, 2))
|