Fixed database typo and removed unnecessary class identifier.
This commit is contained in:
parent
00ad49a143
commit
45fb349a7d
5098 changed files with 952558 additions and 85 deletions
26
venv/Lib/site-packages/scipy/spatial/transform/__init__.py
Normal file
26
venv/Lib/site-packages/scipy/spatial/transform/__init__.py
Normal file
|
@ -0,0 +1,26 @@
|
|||
"""
|
||||
Spatial Transformations (:mod:`scipy.spatial.transform`)
|
||||
========================================================
|
||||
|
||||
.. currentmodule:: scipy.spatial.transform
|
||||
|
||||
This package implements various spatial transformations. For now,
|
||||
only rotations are supported.
|
||||
|
||||
Rotations in 3 dimensions
|
||||
-------------------------
|
||||
.. autosummary::
|
||||
:toctree: generated/
|
||||
|
||||
Rotation
|
||||
Slerp
|
||||
RotationSpline
|
||||
"""
|
||||
from .rotation import Rotation, Slerp
|
||||
from ._rotation_spline import RotationSpline
|
||||
|
||||
__all__ = ['Rotation', 'Slerp', 'RotationSpline']
|
||||
|
||||
from scipy._lib._testutils import PytestTester
|
||||
test = PytestTester(__name__)
|
||||
del PytestTester
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -0,0 +1,140 @@
|
|||
import numpy as np
|
||||
from scipy.constants import golden as phi
|
||||
|
||||
|
||||
def icosahedral(cls):
|
||||
g1 = tetrahedral(cls).as_quat()
|
||||
a = 0.5
|
||||
b = 0.5 / phi
|
||||
c = phi / 2
|
||||
g2 = np.array([[+a, +b, +c, 0],
|
||||
[+a, +b, -c, 0],
|
||||
[+a, +c, 0, +b],
|
||||
[+a, +c, 0, -b],
|
||||
[+a, -b, +c, 0],
|
||||
[+a, -b, -c, 0],
|
||||
[+a, -c, 0, +b],
|
||||
[+a, -c, 0, -b],
|
||||
[+a, 0, +b, +c],
|
||||
[+a, 0, +b, -c],
|
||||
[+a, 0, -b, +c],
|
||||
[+a, 0, -b, -c],
|
||||
[+b, +a, 0, +c],
|
||||
[+b, +a, 0, -c],
|
||||
[+b, +c, +a, 0],
|
||||
[+b, +c, -a, 0],
|
||||
[+b, -a, 0, +c],
|
||||
[+b, -a, 0, -c],
|
||||
[+b, -c, +a, 0],
|
||||
[+b, -c, -a, 0],
|
||||
[+b, 0, +c, +a],
|
||||
[+b, 0, +c, -a],
|
||||
[+b, 0, -c, +a],
|
||||
[+b, 0, -c, -a],
|
||||
[+c, +a, +b, 0],
|
||||
[+c, +a, -b, 0],
|
||||
[+c, +b, 0, +a],
|
||||
[+c, +b, 0, -a],
|
||||
[+c, -a, +b, 0],
|
||||
[+c, -a, -b, 0],
|
||||
[+c, -b, 0, +a],
|
||||
[+c, -b, 0, -a],
|
||||
[+c, 0, +a, +b],
|
||||
[+c, 0, +a, -b],
|
||||
[+c, 0, -a, +b],
|
||||
[+c, 0, -a, -b],
|
||||
[0, +a, +c, +b],
|
||||
[0, +a, +c, -b],
|
||||
[0, +a, -c, +b],
|
||||
[0, +a, -c, -b],
|
||||
[0, +b, +a, +c],
|
||||
[0, +b, +a, -c],
|
||||
[0, +b, -a, +c],
|
||||
[0, +b, -a, -c],
|
||||
[0, +c, +b, +a],
|
||||
[0, +c, +b, -a],
|
||||
[0, +c, -b, +a],
|
||||
[0, +c, -b, -a]])
|
||||
return cls.from_quat(np.concatenate((g1, g2)))
|
||||
|
||||
|
||||
def octahedral(cls):
|
||||
g1 = tetrahedral(cls).as_quat()
|
||||
c = np.sqrt(2) / 2
|
||||
g2 = np.array([[+c, 0, 0, +c],
|
||||
[0, +c, 0, +c],
|
||||
[0, 0, +c, +c],
|
||||
[0, 0, -c, +c],
|
||||
[0, -c, 0, +c],
|
||||
[-c, 0, 0, +c],
|
||||
[0, +c, +c, 0],
|
||||
[0, -c, +c, 0],
|
||||
[+c, 0, +c, 0],
|
||||
[-c, 0, +c, 0],
|
||||
[+c, +c, 0, 0],
|
||||
[-c, +c, 0, 0]])
|
||||
return cls.from_quat(np.concatenate((g1, g2)))
|
||||
|
||||
|
||||
def tetrahedral(cls):
|
||||
g1 = np.eye(4)
|
||||
c = 0.5
|
||||
g2 = np.array([[c, -c, -c, +c],
|
||||
[c, -c, +c, +c],
|
||||
[c, +c, -c, +c],
|
||||
[c, +c, +c, +c],
|
||||
[c, -c, -c, -c],
|
||||
[c, -c, +c, -c],
|
||||
[c, +c, -c, -c],
|
||||
[c, +c, +c, -c]])
|
||||
return cls.from_quat(np.concatenate((g1, g2)))
|
||||
|
||||
|
||||
def dicyclic(cls, n, axis=2):
|
||||
g1 = cyclic(cls, n, axis).as_rotvec()
|
||||
|
||||
thetas = np.linspace(0, np.pi, n, endpoint=False)
|
||||
rv = np.pi * np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
|
||||
g2 = np.roll(rv, axis, axis=1)
|
||||
return cls.from_rotvec(np.concatenate((g1, g2)))
|
||||
|
||||
|
||||
def cyclic(cls, n, axis=2):
|
||||
thetas = np.linspace(0, 2 * np.pi, n, endpoint=False)
|
||||
rv = np.vstack([thetas, np.zeros(n), np.zeros(n)]).T
|
||||
return cls.from_rotvec(np.roll(rv, axis, axis=1))
|
||||
|
||||
|
||||
def create_group(cls, group, axis='Z'):
|
||||
if not isinstance(group, str):
|
||||
raise ValueError("`group` argument must be a string")
|
||||
|
||||
permitted_axes = ['x', 'y', 'z', 'X', 'Y', 'Z']
|
||||
if axis not in permitted_axes:
|
||||
raise ValueError("`axis` must be one of " + ", ".join(permitted_axes))
|
||||
|
||||
if group in ['I', 'O', 'T']:
|
||||
symbol = group
|
||||
order = 1
|
||||
elif group[:1] in ['C', 'D'] and group[1:].isdigit():
|
||||
symbol = group[:1]
|
||||
order = int(group[1:])
|
||||
else:
|
||||
raise ValueError("`group` must be one of 'I', 'O', 'T', 'Dn', 'Cn'")
|
||||
|
||||
if order < 1:
|
||||
raise ValueError("Group order must be positive")
|
||||
|
||||
axis = 'xyz'.index(axis.lower())
|
||||
if symbol == 'I':
|
||||
return icosahedral(cls)
|
||||
elif symbol == 'O':
|
||||
return octahedral(cls)
|
||||
elif symbol == 'T':
|
||||
return tetrahedral(cls)
|
||||
elif symbol == 'D':
|
||||
return dicyclic(cls, order, axis=axis)
|
||||
elif symbol == 'C':
|
||||
return cyclic(cls, order, axis=axis)
|
||||
else:
|
||||
assert False
|
|
@ -0,0 +1,456 @@
|
|||
import numpy as np
|
||||
from scipy.linalg import solve_banded
|
||||
from .rotation import Rotation
|
||||
|
||||
|
||||
def _create_skew_matrix(x):
|
||||
"""Create skew-symmetric matrices corresponding to vectors.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
x : ndarray, shape (n, 3)
|
||||
Set of vectors.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3, 3)
|
||||
"""
|
||||
result = np.zeros((len(x), 3, 3))
|
||||
result[:, 0, 1] = -x[:, 2]
|
||||
result[:, 0, 2] = x[:, 1]
|
||||
result[:, 1, 0] = x[:, 2]
|
||||
result[:, 1, 2] = -x[:, 0]
|
||||
result[:, 2, 0] = -x[:, 1]
|
||||
result[:, 2, 1] = x[:, 0]
|
||||
return result
|
||||
|
||||
|
||||
def _matrix_vector_product_of_stacks(A, b):
|
||||
"""Compute the product of stack of matrices and vectors."""
|
||||
return np.einsum("ijk,ik->ij", A, b)
|
||||
|
||||
|
||||
def _angular_rate_to_rotvec_dot_matrix(rotvecs):
|
||||
"""Compute matrices to transform angular rates to rot. vector derivatives.
|
||||
|
||||
The matrices depend on the current attitude represented as a rotation
|
||||
vector.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
rotvecs : ndarray, shape (n, 3)
|
||||
Set of rotation vectors.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3, 3)
|
||||
"""
|
||||
norm = np.linalg.norm(rotvecs, axis=1)
|
||||
k = np.empty_like(norm)
|
||||
|
||||
mask = norm > 1e-4
|
||||
nm = norm[mask]
|
||||
k[mask] = (1 - 0.5 * nm / np.tan(0.5 * nm)) / nm**2
|
||||
mask = ~mask
|
||||
nm = norm[mask]
|
||||
k[mask] = 1/12 + 1/720 * nm**2
|
||||
|
||||
skew = _create_skew_matrix(rotvecs)
|
||||
|
||||
result = np.empty((len(rotvecs), 3, 3))
|
||||
result[:] = np.identity(3)
|
||||
result[:] += 0.5 * skew
|
||||
result[:] += k[:, None, None] * np.matmul(skew, skew)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def _rotvec_dot_to_angular_rate_matrix(rotvecs):
|
||||
"""Compute matrices to transform rot. vector derivatives to angular rates.
|
||||
|
||||
The matrices depend on the current attitude represented as a rotation
|
||||
vector.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
rotvecs : ndarray, shape (n, 3)
|
||||
Set of rotation vectors.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3, 3)
|
||||
"""
|
||||
norm = np.linalg.norm(rotvecs, axis=1)
|
||||
k1 = np.empty_like(norm)
|
||||
k2 = np.empty_like(norm)
|
||||
|
||||
mask = norm > 1e-4
|
||||
nm = norm[mask]
|
||||
k1[mask] = (1 - np.cos(nm)) / nm ** 2
|
||||
k2[mask] = (nm - np.sin(nm)) / nm ** 3
|
||||
|
||||
mask = ~mask
|
||||
nm = norm[mask]
|
||||
k1[mask] = 0.5 - nm ** 2 / 24
|
||||
k2[mask] = 1 / 6 - nm ** 2 / 120
|
||||
|
||||
skew = _create_skew_matrix(rotvecs)
|
||||
|
||||
result = np.empty((len(rotvecs), 3, 3))
|
||||
result[:] = np.identity(3)
|
||||
result[:] -= k1[:, None, None] * skew
|
||||
result[:] += k2[:, None, None] * np.matmul(skew, skew)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def _angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot):
|
||||
"""Compute the non-linear term in angular acceleration.
|
||||
|
||||
The angular acceleration contains a quadratic term with respect to
|
||||
the derivative of the rotation vector. This function computes that.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
rotvecs : ndarray, shape (n, 3)
|
||||
Set of rotation vectors.
|
||||
rotvecs_dot: ndarray, shape (n, 3)
|
||||
Set of rotation vector derivatives.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3)
|
||||
"""
|
||||
norm = np.linalg.norm(rotvecs, axis=1)
|
||||
dp = np.sum(rotvecs * rotvecs_dot, axis=1)
|
||||
cp = np.cross(rotvecs, rotvecs_dot)
|
||||
ccp = np.cross(rotvecs, cp)
|
||||
dccp = np.cross(rotvecs_dot, cp)
|
||||
|
||||
k1 = np.empty_like(norm)
|
||||
k2 = np.empty_like(norm)
|
||||
k3 = np.empty_like(norm)
|
||||
|
||||
mask = norm > 1e-4
|
||||
nm = norm[mask]
|
||||
k1[mask] = (-nm * np.sin(nm) - 2 * (np.cos(nm) - 1)) / nm ** 4
|
||||
k2[mask] = (-2 * nm + 3 * np.sin(nm) - nm * np.cos(nm)) / nm ** 5
|
||||
k3[mask] = (nm - np.sin(nm)) / nm ** 3
|
||||
|
||||
mask = ~mask
|
||||
nm = norm[mask]
|
||||
k1[mask] = 1/12 - nm ** 2 / 180
|
||||
k2[mask] = -1/60 + nm ** 2 / 12604
|
||||
k3[mask] = 1/6 - nm ** 2 / 120
|
||||
|
||||
dp = dp[:, None]
|
||||
k1 = k1[:, None]
|
||||
k2 = k2[:, None]
|
||||
k3 = k3[:, None]
|
||||
|
||||
return dp * (k1 * cp + k2 * ccp) + k3 * dccp
|
||||
|
||||
|
||||
def _compute_angular_rate(rotvecs, rotvecs_dot):
|
||||
"""Compute angular rates given rotation vectors and its derivatives.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
rotvecs : ndarray, shape (n, 3)
|
||||
Set of rotation vectors.
|
||||
rotvecs_dot : ndarray, shape (n, 3)
|
||||
Set of rotation vector derivatives.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3)
|
||||
"""
|
||||
return _matrix_vector_product_of_stacks(
|
||||
_rotvec_dot_to_angular_rate_matrix(rotvecs), rotvecs_dot)
|
||||
|
||||
|
||||
def _compute_angular_acceleration(rotvecs, rotvecs_dot, rotvecs_dot_dot):
|
||||
"""Compute angular acceleration given rotation vector and its derivatives.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
rotvecs : ndarray, shape (n, 3)
|
||||
Set of rotation vectors.
|
||||
rotvecs_dot : ndarray, shape (n, 3)
|
||||
Set of rotation vector derivatives.
|
||||
rotvecs_dot_dot : ndarray, shape (n, 3)
|
||||
Set of rotation vector second derivatives.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (n, 3)
|
||||
"""
|
||||
return (_compute_angular_rate(rotvecs, rotvecs_dot_dot) +
|
||||
_angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot))
|
||||
|
||||
|
||||
def _create_block_3_diagonal_matrix(A, B, d):
|
||||
"""Create a 3-diagonal block matrix as banded.
|
||||
|
||||
The matrix has the following structure:
|
||||
|
||||
DB...
|
||||
ADB..
|
||||
.ADB.
|
||||
..ADB
|
||||
...AD
|
||||
|
||||
The blocks A, B and D are 3-by-3 matrices. The D matrices has the form
|
||||
d * I.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
A : ndarray, shape (n, 3, 3)
|
||||
Stack of A blocks.
|
||||
B : ndarray, shape (n, 3, 3)
|
||||
Stack of B blocks.
|
||||
d : ndarray, shape (n + 1,)
|
||||
Values for diagonal blocks.
|
||||
|
||||
Returns
|
||||
-------
|
||||
ndarray, shape (11, 3 * (n + 1))
|
||||
Matrix in the banded form as used by `scipy.linalg.solve_banded`.
|
||||
"""
|
||||
ind = np.arange(3)
|
||||
ind_blocks = np.arange(len(A))
|
||||
|
||||
A_i = np.empty_like(A, dtype=int)
|
||||
A_i[:] = ind[:, None]
|
||||
A_i += 3 * (1 + ind_blocks[:, None, None])
|
||||
|
||||
A_j = np.empty_like(A, dtype=int)
|
||||
A_j[:] = ind
|
||||
A_j += 3 * ind_blocks[:, None, None]
|
||||
|
||||
B_i = np.empty_like(B, dtype=int)
|
||||
B_i[:] = ind[:, None]
|
||||
B_i += 3 * ind_blocks[:, None, None]
|
||||
|
||||
B_j = np.empty_like(B, dtype=int)
|
||||
B_j[:] = ind
|
||||
B_j += 3 * (1 + ind_blocks[:, None, None])
|
||||
|
||||
diag_i = diag_j = np.arange(3 * len(d))
|
||||
i = np.hstack((A_i.ravel(), B_i.ravel(), diag_i))
|
||||
j = np.hstack((A_j.ravel(), B_j.ravel(), diag_j))
|
||||
values = np.hstack((A.ravel(), B.ravel(), np.repeat(d, 3)))
|
||||
|
||||
u = 5
|
||||
l = 5
|
||||
result = np.zeros((u + l + 1, 3 * len(d)))
|
||||
result[u + i - j, j] = values
|
||||
return result
|
||||
|
||||
|
||||
class RotationSpline(object):
|
||||
"""Interpolate rotations with continuous angular rate and acceleration.
|
||||
|
||||
The rotation vectors between each consecutive orientation are cubic
|
||||
functions of time and it is guaranteed that angular rate and acceleration
|
||||
are continuous. Such interpolation are analogous to cubic spline
|
||||
interpolation.
|
||||
|
||||
Refer to [1]_ for math and implementation details.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
times : array_like, shape (N,)
|
||||
Times of the known rotations. At least 2 times must be specified.
|
||||
rotations : `Rotation` instance
|
||||
Rotations to perform the interpolation between. Must contain N
|
||||
rotations.
|
||||
|
||||
Methods
|
||||
-------
|
||||
__call__
|
||||
|
||||
References
|
||||
----------
|
||||
.. [1] `Smooth Attitude Interpolation
|
||||
<https://github.com/scipy/scipy/files/2932755/attitude_interpolation.pdf>`_
|
||||
|
||||
Examples
|
||||
--------
|
||||
>>> from scipy.spatial.transform import Rotation, RotationSpline
|
||||
|
||||
Define the sequence of times and rotations from the Euler angles:
|
||||
|
||||
>>> times = [0, 10, 20, 40]
|
||||
>>> angles = [[-10, 20, 30], [0, 15, 40], [-30, 45, 30], [20, 45, 90]]
|
||||
>>> rotations = Rotation.from_euler('XYZ', angles, degrees=True)
|
||||
|
||||
Create the interpolator object:
|
||||
|
||||
>>> spline = RotationSpline(times, rotations)
|
||||
|
||||
Interpolate the Euler angles, angular rate and acceleration:
|
||||
|
||||
>>> angular_rate = np.rad2deg(spline(times, 1))
|
||||
>>> angular_acceleration = np.rad2deg(spline(times, 2))
|
||||
>>> times_plot = np.linspace(times[0], times[-1], 100)
|
||||
>>> angles_plot = spline(times_plot).as_euler('XYZ', degrees=True)
|
||||
>>> angular_rate_plot = np.rad2deg(spline(times_plot, 1))
|
||||
>>> angular_acceleration_plot = np.rad2deg(spline(times_plot, 2))
|
||||
|
||||
On this plot you see that Euler angles are continuous and smooth:
|
||||
|
||||
>>> import matplotlib.pyplot as plt
|
||||
>>> plt.plot(times_plot, angles_plot)
|
||||
>>> plt.plot(times, angles, 'x')
|
||||
>>> plt.title("Euler angles")
|
||||
>>> plt.show()
|
||||
|
||||
The angular rate is also smooth:
|
||||
|
||||
>>> plt.plot(times_plot, angular_rate_plot)
|
||||
>>> plt.plot(times, angular_rate, 'x')
|
||||
>>> plt.title("Angular rate")
|
||||
>>> plt.show()
|
||||
|
||||
The angular acceleration is continuous, but not smooth. Also note that
|
||||
the angular acceleration is not a piecewise-linear function, because
|
||||
it is different from the second derivative of the rotation vector (which
|
||||
is a piecewise-linear function as in the cubic spline).
|
||||
|
||||
>>> plt.plot(times_plot, angular_acceleration_plot)
|
||||
>>> plt.plot(times, angular_acceleration, 'x')
|
||||
>>> plt.title("Angular acceleration")
|
||||
>>> plt.show()
|
||||
"""
|
||||
# Parameters for the solver for angular rate.
|
||||
MAX_ITER = 10
|
||||
TOL = 1e-9
|
||||
|
||||
def _solve_for_angular_rates(self, dt, angular_rates, rotvecs):
|
||||
angular_rate_first = angular_rates[0].copy()
|
||||
|
||||
A = _angular_rate_to_rotvec_dot_matrix(rotvecs)
|
||||
A_inv = _rotvec_dot_to_angular_rate_matrix(rotvecs)
|
||||
M = _create_block_3_diagonal_matrix(
|
||||
2 * A_inv[1:-1] / dt[1:-1, None, None],
|
||||
2 * A[1:-1] / dt[1:-1, None, None],
|
||||
4 * (1 / dt[:-1] + 1 / dt[1:]))
|
||||
|
||||
b0 = 6 * (rotvecs[:-1] * dt[:-1, None] ** -2 +
|
||||
rotvecs[1:] * dt[1:, None] ** -2)
|
||||
b0[0] -= 2 / dt[0] * A_inv[0].dot(angular_rate_first)
|
||||
b0[-1] -= 2 / dt[-1] * A[-1].dot(angular_rates[-1])
|
||||
|
||||
for iteration in range(self.MAX_ITER):
|
||||
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
|
||||
delta_beta = _angular_acceleration_nonlinear_term(
|
||||
rotvecs[:-1], rotvecs_dot[:-1])
|
||||
b = b0 - delta_beta
|
||||
angular_rates_new = solve_banded((5, 5), M, b.ravel())
|
||||
angular_rates_new = angular_rates_new.reshape((-1, 3))
|
||||
|
||||
delta = np.abs(angular_rates_new - angular_rates[:-1])
|
||||
angular_rates[:-1] = angular_rates_new
|
||||
if np.all(delta < self.TOL * (1 + np.abs(angular_rates_new))):
|
||||
break
|
||||
|
||||
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
|
||||
angular_rates = np.vstack((angular_rate_first, angular_rates[:-1]))
|
||||
|
||||
return angular_rates, rotvecs_dot
|
||||
|
||||
def __init__(self, times, rotations):
|
||||
from scipy.interpolate import PPoly
|
||||
|
||||
if len(rotations) == 1:
|
||||
raise ValueError("`rotations` must contain at least 2 rotations.")
|
||||
|
||||
times = np.asarray(times, dtype=float)
|
||||
if times.ndim != 1:
|
||||
raise ValueError("`times` must be 1-dimensional.")
|
||||
|
||||
if len(times) != len(rotations):
|
||||
raise ValueError("Expected number of rotations to be equal to "
|
||||
"number of timestamps given, got {} rotations "
|
||||
"and {} timestamps."
|
||||
.format(len(rotations), len(times)))
|
||||
|
||||
dt = np.diff(times)
|
||||
if np.any(dt <= 0):
|
||||
raise ValueError("Values in `times` must be in a strictly "
|
||||
"increasing order.")
|
||||
|
||||
rotvecs = (rotations[:-1].inv() * rotations[1:]).as_rotvec()
|
||||
angular_rates = rotvecs / dt[:, None]
|
||||
|
||||
if len(rotations) == 2:
|
||||
rotvecs_dot = angular_rates
|
||||
else:
|
||||
angular_rates, rotvecs_dot = self._solve_for_angular_rates(
|
||||
dt, angular_rates, rotvecs)
|
||||
|
||||
dt = dt[:, None]
|
||||
coeff = np.empty((4, len(times) - 1, 3))
|
||||
coeff[0] = (-2 * rotvecs + dt * angular_rates
|
||||
+ dt * rotvecs_dot) / dt ** 3
|
||||
coeff[1] = (3 * rotvecs - 2 * dt * angular_rates
|
||||
- dt * rotvecs_dot) / dt ** 2
|
||||
coeff[2] = angular_rates
|
||||
coeff[3] = 0
|
||||
|
||||
self.times = times
|
||||
self.rotations = rotations
|
||||
self.interpolator = PPoly(coeff, times)
|
||||
|
||||
def __call__(self, times, order=0):
|
||||
"""Compute interpolated values.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
times : float or array_like
|
||||
Times of interest.
|
||||
order : {0, 1, 2}, optional
|
||||
Order of differentiation:
|
||||
|
||||
* 0 (default) : return Rotation
|
||||
* 1 : return the angular rate in rad/sec
|
||||
* 2 : return the angular acceleration in rad/sec/sec
|
||||
|
||||
Returns
|
||||
-------
|
||||
Interpolated Rotation, angular rate or acceleration.
|
||||
"""
|
||||
if order not in [0, 1, 2]:
|
||||
raise ValueError("`order` must be 0, 1 or 2.")
|
||||
|
||||
times = np.asarray(times, dtype=float)
|
||||
if times.ndim > 1:
|
||||
raise ValueError("`times` must be at most 1-dimensional.")
|
||||
|
||||
singe_time = times.ndim == 0
|
||||
times = np.atleast_1d(times)
|
||||
|
||||
rotvecs = self.interpolator(times)
|
||||
if order == 0:
|
||||
index = np.searchsorted(self.times, times, side='right')
|
||||
index -= 1
|
||||
index[index < 0] = 0
|
||||
n_segments = len(self.times) - 1
|
||||
index[index > n_segments - 1] = n_segments - 1
|
||||
result = self.rotations[index] * Rotation.from_rotvec(rotvecs)
|
||||
elif order == 1:
|
||||
rotvecs_dot = self.interpolator(times, 1)
|
||||
result = _compute_angular_rate(rotvecs, rotvecs_dot)
|
||||
elif order == 2:
|
||||
rotvecs_dot = self.interpolator(times, 1)
|
||||
rotvecs_dot_dot = self.interpolator(times, 2)
|
||||
result = _compute_angular_acceleration(rotvecs, rotvecs_dot,
|
||||
rotvecs_dot_dot)
|
||||
else:
|
||||
assert False
|
||||
|
||||
if singe_time:
|
||||
result = result[0]
|
||||
|
||||
return result
|
2128
venv/Lib/site-packages/scipy/spatial/transform/rotation.py
Normal file
2128
venv/Lib/site-packages/scipy/spatial/transform/rotation.py
Normal file
File diff suppressed because it is too large
Load diff
9
venv/Lib/site-packages/scipy/spatial/transform/setup.py
Normal file
9
venv/Lib/site-packages/scipy/spatial/transform/setup.py
Normal file
|
@ -0,0 +1,9 @@
|
|||
|
||||
def configuration(parent_package='', top_path=None):
|
||||
from numpy.distutils.misc_util import Configuration
|
||||
|
||||
config = Configuration('transform', parent_package, top_path)
|
||||
|
||||
config.add_data_dir('tests')
|
||||
|
||||
return config
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load diff
|
@ -0,0 +1,169 @@
|
|||
import pytest
|
||||
|
||||
import numpy as np
|
||||
from numpy.testing import assert_array_almost_equal
|
||||
from scipy.spatial.transform import Rotation
|
||||
from scipy.optimize import linear_sum_assignment
|
||||
from scipy.spatial.distance import cdist
|
||||
from scipy.constants import golden as phi
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
|
||||
TOL = 1E-12
|
||||
NS = range(1, 13)
|
||||
NAMES = ["I", "O", "T"] + ["C%d" % n for n in NS] + ["D%d" % n for n in NS]
|
||||
SIZES = [60, 24, 12] + list(NS) + [2 * n for n in NS]
|
||||
|
||||
|
||||
def _calculate_rmsd(P, Q):
|
||||
"""Calculates the root-mean-square distance between the points of P and Q.
|
||||
The distance is taken as the minimum over all possible matchings. It is
|
||||
zero if P and Q are identical and non-zero if not.
|
||||
"""
|
||||
distance_matrix = cdist(P, Q, metric='sqeuclidean')
|
||||
matching = linear_sum_assignment(distance_matrix)
|
||||
return np.sqrt(distance_matrix[matching].sum())
|
||||
|
||||
|
||||
def _generate_pyramid(n, axis):
|
||||
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
|
||||
P = np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
|
||||
P = np.concatenate((P, [[1, 0, 0]]))
|
||||
return np.roll(P, axis, axis=1)
|
||||
|
||||
|
||||
def _generate_prism(n, axis):
|
||||
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
|
||||
bottom = np.vstack([-np.ones(n), np.cos(thetas), np.sin(thetas)]).T
|
||||
top = np.vstack([+np.ones(n), np.cos(thetas), np.sin(thetas)]).T
|
||||
P = np.concatenate((bottom, top))
|
||||
return np.roll(P, axis, axis=1)
|
||||
|
||||
|
||||
def _generate_icosahedron():
|
||||
x = np.array([[0, -1, -phi],
|
||||
[0, -1, +phi],
|
||||
[0, +1, -phi],
|
||||
[0, +1, +phi]])
|
||||
return np.concatenate([np.roll(x, i, axis=1) for i in range(3)])
|
||||
|
||||
|
||||
def _generate_octahedron():
|
||||
return np.array([[-1, 0, 0], [+1, 0, 0], [0, -1, 0],
|
||||
[0, +1, 0], [0, 0, -1], [0, 0, +1]])
|
||||
|
||||
|
||||
def _generate_tetrahedron():
|
||||
return np.array([[1, 1, 1], [1, -1, -1], [-1, 1, -1], [-1, -1, 1]])
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name", [-1, None, True, np.array(['C3'])])
|
||||
def test_group_type(name):
|
||||
with pytest.raises(ValueError,
|
||||
match="must be a string"):
|
||||
Rotation.create_group(name)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name", ["Q", " ", "CA", "C ", "DA", "D ", "I2", ""])
|
||||
def test_group_name(name):
|
||||
with pytest.raises(ValueError,
|
||||
match="must be one of 'I', 'O', 'T', 'Dn', 'Cn'"):
|
||||
Rotation.create_group(name)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name", ["C0", "D0"])
|
||||
def test_group_order_positive(name):
|
||||
with pytest.raises(ValueError,
|
||||
match="Group order must be positive"):
|
||||
Rotation.create_group(name)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("axis", ['A', 'b', 0, 1, 2, 4, False, None])
|
||||
def test_axis_valid(axis):
|
||||
with pytest.raises(ValueError,
|
||||
match="`axis` must be one of"):
|
||||
Rotation.create_group("C1", axis)
|
||||
|
||||
|
||||
def test_icosahedral():
|
||||
"""The icosahedral group fixes the rotations of an icosahedron. Here we
|
||||
test that the icosahedron is invariant after application of the elements
|
||||
of the rotation group."""
|
||||
P = _generate_icosahedron()
|
||||
for g in Rotation.create_group("I"):
|
||||
g = Rotation.from_quat(g.as_quat())
|
||||
assert _calculate_rmsd(P, g.apply(P)) < TOL
|
||||
|
||||
|
||||
def test_octahedral():
|
||||
"""Test that the octahedral group correctly fixes the rotations of an
|
||||
octahedron."""
|
||||
P = _generate_octahedron()
|
||||
for g in Rotation.create_group("O"):
|
||||
assert _calculate_rmsd(P, g.apply(P)) < TOL
|
||||
|
||||
|
||||
def test_tetrahedral():
|
||||
"""Test that the tetrahedral group correctly fixes the rotations of a
|
||||
tetrahedron."""
|
||||
P = _generate_tetrahedron()
|
||||
for g in Rotation.create_group("T"):
|
||||
assert _calculate_rmsd(P, g.apply(P)) < TOL
|
||||
|
||||
|
||||
@pytest.mark.parametrize("n", NS)
|
||||
@pytest.mark.parametrize("axis", 'XYZ')
|
||||
def test_dicyclic(n, axis):
|
||||
"""Test that the dicyclic group correctly fixes the rotations of a
|
||||
prism."""
|
||||
P = _generate_prism(n, axis='XYZ'.index(axis))
|
||||
for g in Rotation.create_group("D%d" % n, axis=axis):
|
||||
assert _calculate_rmsd(P, g.apply(P)) < TOL
|
||||
|
||||
|
||||
@pytest.mark.parametrize("n", NS)
|
||||
@pytest.mark.parametrize("axis", 'XYZ')
|
||||
def test_cyclic(n, axis):
|
||||
"""Test that the cyclic group correctly fixes the rotations of a
|
||||
pyramid."""
|
||||
P = _generate_pyramid(n, axis='XYZ'.index(axis))
|
||||
for g in Rotation.create_group("C%d" % n, axis=axis):
|
||||
assert _calculate_rmsd(P, g.apply(P)) < TOL
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
|
||||
def test_group_sizes(name, size):
|
||||
assert len(Rotation.create_group(name)) == size
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
|
||||
def test_group_no_duplicates(name, size):
|
||||
g = Rotation.create_group(name)
|
||||
kdtree = cKDTree(g.as_quat())
|
||||
assert len(kdtree.query_pairs(1E-3)) == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
|
||||
def test_group_symmetry(name, size):
|
||||
g = Rotation.create_group(name)
|
||||
q = np.concatenate((-g.as_quat(), g.as_quat()))
|
||||
distance = np.sort(cdist(q, q))
|
||||
deltas = np.max(distance, axis=0) - np.min(distance, axis=0)
|
||||
assert (deltas < TOL).all()
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name", NAMES)
|
||||
def test_reduction(name):
|
||||
"""Test that the elements of the rotation group are correctly
|
||||
mapped onto the identity rotation."""
|
||||
g = Rotation.create_group(name)
|
||||
f = g.reduce(g)
|
||||
assert_array_almost_equal(f.magnitude(), np.zeros(len(g)))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("name", NAMES)
|
||||
def test_single_reduction(name):
|
||||
g = Rotation.create_group(name)
|
||||
f = g[-1].reduce(g)
|
||||
assert_array_almost_equal(f.magnitude(), 0)
|
||||
assert f.as_quat().shape == (4,)
|
|
@ -0,0 +1,161 @@
|
|||
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-19)
|
||||
assert_allclose(spline(times_check, 2), 0, atol=1e-19)
|
||||
|
||||
assert_allclose(spline(5.5).as_rotvec(), 1, rtol=1e-15)
|
||||
assert_allclose(spline(5.5, 1), 0, atol=1e-19)
|
||||
assert_allclose(spline(5.5, 2), 0, atol=1e-19)
|
||||
|
||||
|
||||
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))
|
Loading…
Add table
Add a link
Reference in a new issue