@mytec: pushed back before 1.1
This commit is contained in:
@@ -0,0 +1,31 @@
|
||||
"""
|
||||
Spatial Transformations (:mod:`scipy.spatial.transform`)
|
||||
========================================================
|
||||
|
||||
.. currentmodule:: scipy.spatial.transform
|
||||
|
||||
This package implements various spatial transformations. For now, rotations
|
||||
and rigid transforms (rotations and translations) are supported.
|
||||
|
||||
Rotations in 3 dimensions
|
||||
-------------------------
|
||||
.. autosummary::
|
||||
:toctree: generated/
|
||||
|
||||
RigidTransform
|
||||
Rotation
|
||||
Slerp
|
||||
RotationSpline
|
||||
"""
|
||||
from ._rotation import Rotation, Slerp
|
||||
from ._rigid_transform import RigidTransform
|
||||
from ._rotation_spline import RotationSpline
|
||||
|
||||
# Deprecated namespaces, to be removed in v2.0.0
|
||||
from . import rotation
|
||||
|
||||
__all__ = ['Rotation', 'Slerp', 'RotationSpline', 'RigidTransform']
|
||||
|
||||
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -0,0 +1,448 @@
|
||||
from types import EllipsisType
|
||||
|
||||
from scipy._lib._array_api import (
|
||||
array_namespace,
|
||||
Array,
|
||||
ArrayLike,
|
||||
is_lazy_array,
|
||||
xp_vector_norm,
|
||||
xp_result_type,
|
||||
xp_promote,
|
||||
is_array_api_obj,
|
||||
)
|
||||
import scipy._lib.array_api_extra as xpx
|
||||
from scipy.spatial.transform._rotation_xp import (
|
||||
as_matrix as quat_as_matrix,
|
||||
from_matrix as quat_from_matrix,
|
||||
_from_matrix_orthogonal as quat_from_matrix_orthogonal,
|
||||
from_rotvec as quat_from_rotvec,
|
||||
as_rotvec as quat_as_rotvec,
|
||||
compose_quat,
|
||||
from_quat,
|
||||
inv as quat_inv,
|
||||
mean as quat_mean,
|
||||
)
|
||||
from scipy._lib.array_api_compat import device as xp_device
|
||||
from scipy._lib._util import broadcastable
|
||||
|
||||
|
||||
def from_matrix(matrix: Array, normalize: bool = True, copy: bool = True) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
# Shape check should be done before calling this function
|
||||
if normalize or copy:
|
||||
matrix = xp.asarray(matrix, copy=True)
|
||||
|
||||
last_row_ok = xp.all(
|
||||
matrix[..., 3, :] == xp.asarray([0, 0, 0, 1.0], device=xp_device(matrix)),
|
||||
axis=-1,
|
||||
)
|
||||
lazy = is_lazy_array(matrix)
|
||||
# We delay lazy branch checks until after normalization to avoid overwriting nans
|
||||
# with the rotation matrix
|
||||
if not lazy and xp.any(~last_row_ok):
|
||||
if last_row_ok.shape == ():
|
||||
idx = ()
|
||||
else:
|
||||
idx = tuple(int(i[0]) for i in xp.nonzero(~last_row_ok))
|
||||
vals = matrix[idx + (3, ...)]
|
||||
raise ValueError(
|
||||
f"Expected last row of transformation matrix {idx} to be "
|
||||
f"exactly [0, 0, 0, 1], got {vals}"
|
||||
)
|
||||
|
||||
# The quat_from_matrix() method orthogonalizes the rotation
|
||||
# component of the transformation matrix. While this does have some
|
||||
# overhead in converting a rotation matrix to a quaternion and back, it
|
||||
# allows for skipping singular value decomposition for near-orthogonal
|
||||
# matrices, which is a computationally expensive operation.
|
||||
if normalize:
|
||||
rotmat = quat_as_matrix(quat_from_matrix(matrix[..., :3, :3]))
|
||||
matrix = xpx.at(matrix)[..., :3, :3].set(rotmat)
|
||||
# Lazy branch matrix invalidation
|
||||
if lazy:
|
||||
matrix = xp.where(last_row_ok[..., None, None], matrix, xp.nan)
|
||||
return matrix
|
||||
|
||||
|
||||
def from_rotation(quat: Array) -> Array:
|
||||
xp = array_namespace(quat)
|
||||
rotmat = quat_as_matrix(quat)
|
||||
matrix = xp.zeros(
|
||||
(*rotmat.shape[:-2], 4, 4), dtype=quat.dtype, device=xp_device(quat)
|
||||
)
|
||||
matrix = xpx.at(matrix)[..., :3, :3].set(rotmat)
|
||||
matrix = xpx.at(matrix)[..., 3, 3].set(1)
|
||||
return matrix
|
||||
|
||||
|
||||
def from_translation(translation: Array) -> Array:
|
||||
xp = array_namespace(translation)
|
||||
|
||||
if translation.shape[-1] != 3:
|
||||
raise ValueError(
|
||||
f"Expected `translation` to have shape (..., 3), got {translation.shape}."
|
||||
)
|
||||
device = xp_device(translation)
|
||||
dtype = xp_result_type(translation, force_floating=True, xp=xp)
|
||||
eye = xp.eye(4, dtype=dtype, device=device)
|
||||
|
||||
matrix = xpx.atleast_nd(eye, ndim=translation.ndim + 1, xp=xp)
|
||||
matrix = xp.zeros(
|
||||
(*translation.shape[:-1], 4, 4),
|
||||
dtype=dtype,
|
||||
device=device,
|
||||
)
|
||||
matrix = xpx.at(matrix)[...].set(xp.eye(4, dtype=dtype, device=device))
|
||||
matrix = xpx.at(matrix)[..., :3, 3].set(
|
||||
xp_promote(translation, force_floating=True, xp=xp)
|
||||
)
|
||||
return matrix
|
||||
|
||||
|
||||
def from_components(translation: Array, quat: Array) -> Array:
|
||||
return compose_transforms(from_translation(translation), from_rotation(quat))
|
||||
|
||||
|
||||
def from_exp_coords(exp_coords: Array) -> Array:
|
||||
rot_vec = exp_coords[..., :3]
|
||||
rot_matrix = quat_as_matrix(quat_from_rotvec(rot_vec))
|
||||
translation_transform = _compute_se3_exp_translation_transform(rot_vec)
|
||||
translations = (translation_transform @ exp_coords[..., 3:, None])[..., 0]
|
||||
return _create_transformation_matrix(translations, rot_matrix)
|
||||
|
||||
|
||||
def from_dual_quat(dual_quat: Array, *, scalar_first: bool = False) -> Array:
|
||||
xp = array_namespace(dual_quat)
|
||||
|
||||
if dual_quat.shape[-1] != 8:
|
||||
raise ValueError(
|
||||
f"Expected `dual_quat` to have shape (..., 8), got {dual_quat.shape}."
|
||||
)
|
||||
|
||||
real_part = dual_quat[..., :4]
|
||||
dual_part = dual_quat[..., 4:]
|
||||
if scalar_first:
|
||||
real_part = xp.roll(real_part, -1, axis=-1)
|
||||
dual_part = xp.roll(dual_part, -1, axis=-1)
|
||||
|
||||
real_part, dual_part = _normalize_dual_quaternion(real_part, dual_part)
|
||||
|
||||
rot_quat = from_quat(real_part)
|
||||
|
||||
translation = 2.0 * compose_quat(dual_part, quat_inv(rot_quat))[..., :3]
|
||||
matrix = _create_transformation_matrix(translation, quat_as_matrix(rot_quat))
|
||||
|
||||
return matrix
|
||||
|
||||
|
||||
def as_exp_coords(matrix: Array) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
rot_vec = quat_as_rotvec(quat_from_matrix_orthogonal(matrix[..., :3, :3]))
|
||||
translation_transform = _compute_se3_log_translation_transform(rot_vec)
|
||||
translations = (translation_transform @ matrix[..., :3, 3][..., None])[..., 0]
|
||||
exp_coords = xp.concat([rot_vec, translations], axis=-1)
|
||||
return exp_coords
|
||||
|
||||
|
||||
def as_dual_quat(matrix: Array, *, scalar_first: bool = False) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
real_parts = quat_from_matrix_orthogonal(matrix[..., :3, :3])
|
||||
|
||||
pure_translation_quats = xp.empty(
|
||||
(*matrix.shape[:-2], 4), dtype=matrix.dtype, device=xp_device(matrix)
|
||||
)
|
||||
pure_translation_quats = xpx.at(pure_translation_quats)[..., :3].set(
|
||||
matrix[..., :3, 3]
|
||||
)
|
||||
pure_translation_quats = xpx.at(pure_translation_quats)[..., 3].set(0.0)
|
||||
|
||||
dual_parts = 0.5 * compose_quat(pure_translation_quats, real_parts)
|
||||
|
||||
if scalar_first:
|
||||
real_parts = xp.roll(real_parts, 1, axis=-1)
|
||||
dual_parts = xp.roll(dual_parts, 1, axis=-1)
|
||||
|
||||
dual_quats = xp.concat([real_parts, dual_parts], axis=-1)
|
||||
return dual_quats
|
||||
|
||||
|
||||
def compose_transforms(tf_matrix: Array, other_tf_matrix: Array) -> Array:
|
||||
if not broadcastable(tf_matrix.shape, other_tf_matrix.shape):
|
||||
len_a = tf_matrix.shape[0] if tf_matrix.ndim == 3 else 1
|
||||
len_b = other_tf_matrix.shape[0] if other_tf_matrix.ndim == 3 else 1
|
||||
raise ValueError(
|
||||
"Expected equal number of transforms in both or a "
|
||||
"single transform in either object, got "
|
||||
f"{len_a} transforms in first and {len_b}"
|
||||
"transforms in second object."
|
||||
)
|
||||
return tf_matrix @ other_tf_matrix
|
||||
|
||||
|
||||
def inv(matrix: Array) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
r_inv = xp.matrix_transpose(matrix[..., :3, :3])
|
||||
# Matrix multiplication of r_inv and translation vector
|
||||
t_inv = -(r_inv @ matrix[..., :3, 3][..., None])[..., 0]
|
||||
matrix = xp.zeros(
|
||||
(*matrix.shape[:-2], 4, 4), dtype=matrix.dtype, device=xp_device(matrix)
|
||||
)
|
||||
matrix = xpx.at(matrix)[..., :3, :3].set(r_inv)
|
||||
matrix = xpx.at(matrix)[..., :3, 3].set(t_inv)
|
||||
matrix = xpx.at(matrix)[..., 3, 3].set(1)
|
||||
return matrix
|
||||
|
||||
|
||||
def apply(matrix: Array, vector: Array, inverse: bool = False) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
if vector.shape[-1] != 3:
|
||||
raise ValueError(f"Expected vector to have shape (..., 3), got {vector.shape}.")
|
||||
vec = xp.empty(
|
||||
(*vector.shape[:-1], 4), dtype=vector.dtype, device=xp_device(vector)
|
||||
)
|
||||
vec = xpx.at(vec)[..., :3].set(vector)
|
||||
vec = xpx.at(vec)[..., 3].set(1)
|
||||
vec = vec[..., None]
|
||||
|
||||
if inverse:
|
||||
matrix = inv(matrix)
|
||||
|
||||
# We raise a ValueError manually here because letting the function run its course
|
||||
# would raise heterogeneous error types and messages for different frameworks.
|
||||
# However, the error only mimics numpy's error message and does not provide the
|
||||
# same amount of context.
|
||||
if not broadcastable(matrix.shape, vec.shape):
|
||||
raise ValueError("operands could not be broadcast together")
|
||||
return (matrix @ vec)[..., :3, 0]
|
||||
|
||||
|
||||
def pow(matrix: Array, n: float | Array) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
device = xp_device(matrix)
|
||||
# If n is an array, we sanitize it to a scalar and promote quat and n to
|
||||
# the same dtype.
|
||||
if is_array_api_obj(n):
|
||||
if n.shape == (1,):
|
||||
n = n[0]
|
||||
elif n.ndim != 0:
|
||||
raise ValueError("Array exponent must be a scalar")
|
||||
matrix, n = xp_promote(matrix, n, force_floating=True, xp=xp)
|
||||
|
||||
# If n is a lazy array, we cannot take fast paths for special cases.
|
||||
if is_lazy_array(n):
|
||||
# Lazy execution. We compute all special cases and the general case
|
||||
result = from_exp_coords(as_exp_coords(matrix) * n)
|
||||
identity = xp.eye(4, dtype=matrix.dtype, device=device)
|
||||
result = xp.where(n == 0, identity, result)
|
||||
result = xp.where(n == -1, inv(matrix), result)
|
||||
result = xp.where(n == 1, matrix, result)
|
||||
return result
|
||||
if n == 0:
|
||||
identity = xp.eye(4, dtype=matrix.dtype, device=device)
|
||||
identity = xpx.atleast_nd(identity, ndim=matrix.ndim, xp=xp)
|
||||
return xp.tile(identity, (*matrix.shape[:-2], 1, 1))
|
||||
elif n == -1:
|
||||
return inv(matrix)
|
||||
elif n == 1:
|
||||
return matrix
|
||||
return from_exp_coords(as_exp_coords(matrix) * n)
|
||||
|
||||
|
||||
def mean(
|
||||
matrix: Array,
|
||||
weights: ArrayLike | None = None,
|
||||
axis: None | int | tuple[int, ...] = None
|
||||
) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
if matrix.shape[0] == 0:
|
||||
raise ValueError("Mean of an empty rotation set is undefined.")
|
||||
# Axis logic: For None, we reduce over all axes. For int, we only reduce over that
|
||||
# axis. For tuple, we reduce over all specified axes.
|
||||
all_axes = tuple(range(matrix.ndim - 2))
|
||||
if axis is None:
|
||||
axis = all_axes
|
||||
elif isinstance(axis, int):
|
||||
axis = (axis,)
|
||||
if not isinstance(axis, tuple):
|
||||
raise ValueError("`axis` must be None, int, or tuple of ints.")
|
||||
# Ensure all axes are within bounds
|
||||
if (axis != () and
|
||||
(min(axis) < -(matrix.ndim - 2) or max(axis) > (matrix.ndim - 3))
|
||||
):
|
||||
raise ValueError(
|
||||
f"axis {axis} is out of bounds for transform with shape "
|
||||
f"{matrix.shape[:-2]}."
|
||||
)
|
||||
# Ensure all axes are positive and unique
|
||||
axis = tuple(sorted(set(x % (matrix.ndim - 2) for x in axis)))
|
||||
|
||||
lazy = is_lazy_array(matrix)
|
||||
quats = quat_from_matrix_orthogonal(matrix[..., :3, :3])
|
||||
if weights is None:
|
||||
quats_mean = quat_mean(quats, axis=axis)
|
||||
else:
|
||||
neg_weights = weights < 0
|
||||
any_neg_weights = xp.any(neg_weights)
|
||||
if not lazy and any_neg_weights:
|
||||
raise ValueError("`weights` must be non-negative.")
|
||||
if weights.shape != matrix.shape[:-2]:
|
||||
raise ValueError(
|
||||
f"Expected `weights` to match transform shape, got shape "
|
||||
f"{weights.shape} for {matrix.shape[:-2]} transformations."
|
||||
)
|
||||
quats_mean = quat_mean(quats, weights=weights, axis=axis)
|
||||
r_mean = quat_as_matrix(quats_mean)
|
||||
|
||||
t = matrix[..., :3, 3]
|
||||
if weights is None:
|
||||
t_mean = xp.mean(t, axis=axis)
|
||||
else:
|
||||
norm = xp.sum(weights[..., None], axis=axis)
|
||||
wsum = xp.sum(t * weights[..., None], axis=axis)
|
||||
t_mean = wsum / norm
|
||||
|
||||
tf = _create_transformation_matrix(t_mean, r_mean)
|
||||
if weights is not None and lazy:
|
||||
# We cannot raise on negative weights because jit code needs to be
|
||||
# non-branching. We return NaN instead
|
||||
mask = xp.where(any_neg_weights, xp.nan, 1.0)
|
||||
tf = mask * tf
|
||||
return tf
|
||||
|
||||
|
||||
def setitem(
|
||||
matrix: Array,
|
||||
indexer: Array | int | tuple | slice | EllipsisType | None,
|
||||
value: Array,
|
||||
) -> Array:
|
||||
xp = array_namespace(matrix)
|
||||
if isinstance(indexer, EllipsisType):
|
||||
return xpx.at(matrix)[indexer].set(value)
|
||||
if is_array_api_obj(indexer) and indexer.dtype == xp.bool:
|
||||
return xpx.at(matrix)[indexer].set(value)
|
||||
return xpx.at(matrix)[indexer, ...].set(value)
|
||||
|
||||
|
||||
def normalize_dual_quaternion(dual_quat: Array) -> Array:
|
||||
"""Normalize dual quaternion."""
|
||||
xp = array_namespace(dual_quat)
|
||||
real, dual = _normalize_dual_quaternion(dual_quat[..., :4], dual_quat[..., 4:])
|
||||
return xp.concat((real, dual), axis=-1)
|
||||
|
||||
|
||||
def _create_transformation_matrix(
|
||||
translations: Array, rotation_matrices: Array
|
||||
) -> Array:
|
||||
if not translations.shape[:-1] == rotation_matrices.shape[:-2]:
|
||||
raise ValueError(
|
||||
"The number of rotation matrices and translations must be the same."
|
||||
)
|
||||
xp = array_namespace(translations)
|
||||
matrix = xp.empty(
|
||||
(*translations.shape[:-1], 4, 4),
|
||||
dtype=translations.dtype,
|
||||
device=xp_device(translations),
|
||||
)
|
||||
matrix = xpx.at(matrix)[..., :3, :3].set(rotation_matrices)
|
||||
matrix = xpx.at(matrix)[..., :3, 3].set(translations)
|
||||
matrix = xpx.at(matrix)[..., 3, :3].set(0)
|
||||
matrix = xpx.at(matrix)[..., 3, 3].set(1)
|
||||
return matrix
|
||||
|
||||
|
||||
def _compute_se3_exp_translation_transform(rot_vec: Array) -> Array:
|
||||
"""Compute the transformation matrix from the se3 translation part to SE3
|
||||
translation.
|
||||
|
||||
The transformation matrix depends on the rotation vector.
|
||||
"""
|
||||
xp = array_namespace(rot_vec)
|
||||
device = xp_device(rot_vec)
|
||||
dtype = rot_vec.dtype
|
||||
angle = xp_vector_norm(rot_vec, axis=-1, keepdims=True, xp=xp)
|
||||
small_scale = angle < 1e-3
|
||||
|
||||
k1_small = 0.5 - angle**2 / 24 + angle**4 / 720
|
||||
# Avoid division by zero for non-branching computations. The value will get
|
||||
# discarded in the xp.where selection.
|
||||
safe_angle = angle + xp.asarray(small_scale, dtype=dtype, device=device)
|
||||
k1 = (1.0 - xp.cos(angle)) / safe_angle**2
|
||||
k1 = xp.where(small_scale, k1_small, k1)
|
||||
|
||||
k2_small = 1 / 6 - angle**2 / 120 + angle**4 / 5040
|
||||
# Again, avoid division by zero by adding one to all near-zero angles.
|
||||
safe_angle = angle + xp.asarray(small_scale, dtype=dtype, device=device)
|
||||
k2 = (angle - xp.sin(angle)) / safe_angle**3
|
||||
k2 = xp.where(small_scale, k2_small, k2)
|
||||
|
||||
s = _create_skew_matrix(rot_vec)
|
||||
eye = xp.eye(3, dtype=dtype, device=device)
|
||||
|
||||
return eye + k1[..., None] * s + k2[..., None] * s @ s
|
||||
|
||||
|
||||
def _compute_se3_log_translation_transform(rot_vec: Array) -> Array:
|
||||
"""Compute the transformation matrix from SE3 translation to the se3
|
||||
translation part.
|
||||
|
||||
It is the inverse of `_compute_se3_exp_translation_transform` in a closed
|
||||
analytical form.
|
||||
"""
|
||||
xp = array_namespace(rot_vec)
|
||||
dtype = rot_vec.dtype
|
||||
device = xp_device(rot_vec)
|
||||
angle = xp_vector_norm(rot_vec, axis=-1, keepdims=True, xp=xp)
|
||||
mask = angle < 1e-3
|
||||
|
||||
k_small = 1 / 12 + angle**2 / 720 + angle**4 / 30240
|
||||
safe_angle = angle + xp.asarray(mask, dtype=dtype, device=device)
|
||||
k = (1 - 0.5 * angle / xp.tan(0.5 * safe_angle)) / safe_angle**2
|
||||
k = xp.where(mask, k_small, k)
|
||||
|
||||
s = _create_skew_matrix(rot_vec)
|
||||
|
||||
return xp.eye(3, dtype=dtype, device=device) - 0.5 * s + k[..., None] * s @ s
|
||||
|
||||
|
||||
def _create_skew_matrix(vec: Array) -> Array:
|
||||
"""Create skew-symmetric (aka cross-product) matrix for stack of vectors."""
|
||||
xp = array_namespace(vec)
|
||||
result = xp.zeros((*vec.shape[:-1], 3, 3), dtype=vec.dtype, device=xp_device(vec))
|
||||
result = xpx.at(result)[..., 0, 1].set(-vec[..., 2])
|
||||
result = xpx.at(result)[..., 0, 2].set(vec[..., 1])
|
||||
result = xpx.at(result)[..., 1, 0].set(vec[..., 2])
|
||||
result = xpx.at(result)[..., 1, 2].set(-vec[..., 0])
|
||||
result = xpx.at(result)[..., 2, 0].set(-vec[..., 1])
|
||||
result = xpx.at(result)[..., 2, 1].set(vec[..., 0])
|
||||
return result
|
||||
|
||||
|
||||
def _normalize_dual_quaternion(
|
||||
real_part: Array, dual_part: Array
|
||||
) -> tuple[Array, Array]:
|
||||
"""Ensure that the dual quaternion has unit norm.
|
||||
|
||||
The norm is a dual number and must be 1 + 0 * epsilon, which means that
|
||||
the real quaternion must have unit norm and the dual quaternion must be
|
||||
orthogonal to the real quaternion.
|
||||
"""
|
||||
xp = array_namespace(real_part)
|
||||
|
||||
real_norm = xp_vector_norm(real_part, axis=-1, keepdims=True, xp=xp)
|
||||
|
||||
# special case: real quaternion is 0, we set it to identity
|
||||
zero_real_mask = real_norm == 0.0
|
||||
unit_quat = xp.asarray(
|
||||
[0.0, 0.0, 0.0, 1.0], dtype=real_part.dtype, device=xp_device(real_part)
|
||||
)
|
||||
real_part = xp.where(zero_real_mask, unit_quat, real_part)
|
||||
real_norm = xp.where(zero_real_mask, 1.0, real_norm)
|
||||
|
||||
# 1. ensure unit real quaternion
|
||||
real_part = real_part / real_norm
|
||||
dual_part = dual_part / real_norm
|
||||
|
||||
# 2. ensure orthogonality of real and dual quaternion
|
||||
dual_part -= xp.sum(real_part * dual_part, axis=-1, keepdims=True) * real_part
|
||||
|
||||
return real_part, dual_part
|
||||
File diff suppressed because it is too large
Load Diff
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,465 @@
|
||||
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:
|
||||
"""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
|
||||
>>> import numpy as np
|
||||
|
||||
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 rotations.single:
|
||||
raise ValueError("`rotations` must be a sequence of rotations.")
|
||||
|
||||
if rotations.as_quat().ndim > 2:
|
||||
raise ValueError(
|
||||
"Rotations with more than 1 leading dimension are not supported."
|
||||
)
|
||||
|
||||
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, "
|
||||
f"got {len(rotations)} rotations "
|
||||
f"and {len(times)} timestamps.")
|
||||
|
||||
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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,21 @@
|
||||
# This file is not meant for public use and will be removed in SciPy v2.0.0.
|
||||
# Use the `scipy.spatial` namespace for importing the functions
|
||||
# included below.
|
||||
|
||||
from scipy._lib.deprecation import _sub_module_deprecation
|
||||
|
||||
|
||||
__all__ = [ # noqa: F822
|
||||
'Rotation',
|
||||
'Slerp',
|
||||
]
|
||||
|
||||
|
||||
def __dir__():
|
||||
return __all__
|
||||
|
||||
|
||||
def __getattr__(name):
|
||||
return _sub_module_deprecation(sub_package="spatial.transform", module="rotation",
|
||||
private_modules=["_rotation"], all=__all__,
|
||||
attribute=name)
|
||||
Binary file not shown.
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
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"] + [f"C{n}" for n in NS] + [f"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(f"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(f"C{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,188 @@
|
||||
from itertools import product
|
||||
import numpy as np
|
||||
from numpy.testing import assert_allclose
|
||||
import pytest
|
||||
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)
|
||||
|
||||
|
||||
pytestmark = pytest.mark.skip_xp_backends(np_only=True)
|
||||
|
||||
|
||||
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()
|
||||
# rtol bumped from 1e-15 to 1.5e-15 in gh18414 for linux 32 bit
|
||||
assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1.5e-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():
|
||||
with pytest.raises(ValueError):
|
||||
RotationSpline([1.0], Rotation.random())
|
||||
|
||||
r = Rotation.random(10)
|
||||
t = np.arange(10).reshape(5, 2)
|
||||
with pytest.raises(ValueError):
|
||||
RotationSpline(t, r)
|
||||
|
||||
t = np.arange(9)
|
||||
with pytest.raises(ValueError):
|
||||
RotationSpline(t, r)
|
||||
|
||||
t = np.arange(10)
|
||||
t[5] = 0
|
||||
with pytest.raises(ValueError):
|
||||
RotationSpline(t, r)
|
||||
|
||||
t = np.arange(10)
|
||||
|
||||
s = RotationSpline(t, r)
|
||||
with pytest.raises(ValueError):
|
||||
s(10, -1)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
s(np.arange(10).reshape(5, 2))
|
||||
|
||||
r = Rotation.from_quat(np.array([[[0.0, 0, 0, 1], [1, 0, 0 ,0]]]))
|
||||
t = np.arange(2)
|
||||
with pytest.raises(ValueError):
|
||||
RotationSpline(t, r) # More than 2 dimensions
|
||||
|
||||
|
||||
@pytest.mark.skip_xp_backends("numpy")
|
||||
def test_xp_errors(xp):
|
||||
times = xp.asarray([0, 10])
|
||||
r = Rotation.random(2)
|
||||
r = Rotation.from_quat(xp.asarray(r.as_quat()))
|
||||
s = RotationSpline(times, r)
|
||||
t = xp.asarray([0.5, 1.5])
|
||||
# RotationSpline does not have native Array API support, so we check that it
|
||||
# converts any array to NumPy and outputs NumPy arrays.
|
||||
assert isinstance(s(t).as_quat(), np.ndarray)
|
||||
Reference in New Issue
Block a user