pymomentum.quaternion_np

Quaternion Utilities (NumPy)

NumPy implementation of quaternion utilities for PyMomentum.

This module mirrors quaternion but uses NumPy arrays instead of PyTorch tensors. See quaternion for full documentation on the quaternion format and operations.

Quaternion Format

This module uses the (x, y, z, w) format where:

(x, y, z): Vector part representing the rotation axis scaled by sin(θ/2)
w: Scalar part representing cos(θ/2), where θ is the rotation angle

The identity quaternion is (0, 0, 0, 1), representing no rotation.

pymomentum.quaternion_np.align_z_with(direction: NDArray) → NDArray

Return an xyzw quaternion that rotates the +Z axis onto direction.

The returned quaternion represents the shortest-arc rotation from [0, 0, 1] to the normalized direction vector. Useful for orienting Z-aligned primitives (capsules, cylinders) along an arbitrary world-space direction.

Parameters:: direction – A 3D direction vector. Normalized internally; a near-zero vector returns the identity quaternion.
Returns:: A normalized xyzw quaternion.

pymomentum.quaternion_np.angular_distance(q0: NDArray, q1: NDArray) → NDArray

Compute the geodesic angle, in radians, between two orientations.

Returns the rotation angle of the relative rotation q0^{-1} * q1 along the shorter arc, so the result lies in [0, pi]. Uses the numerically robust 2 * atan2(|vec|, |w|) form rather than arccos, which loses precision for small angles. Inputs are assumed normalized. Convert to degrees with numpy.degrees() if needed.

Vectorized over leading dimensions: for inputs of shape [..., 4] the result has shape [...].

Parameters:

q0 – A normalized quaternion ((x, y, z), w)).
q1 – A normalized quaternion ((x, y, z), w)).

Returns:

The angle between q0 and q1 in radians.

pymomentum.quaternion_np.blend(quaternions: NDArray, weights_in: NDArray | None = None) → NDArray

Blend multiple quaternions together using the method described in https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions and http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf.

Parameters:

quaternions – An array of shape (…, k, 4) representing the quaternions to blend.
weights_in – An optional array of shape (…, k) representing the weights for each quaternion. If not provided, all quaternions will be weighted equally.

Returns:

An array of shape (…, 4) representing the blended quaternion.

pymomentum.quaternion_np.check(q: NDArray) → None

Check if an array represents a quaternion.

Parameters:: q – An array representing a quaternion.

pymomentum.quaternion_np.check_and_normalize_weights(quaternions: NDArray, weights_in: NDArray | None = None) → NDArray

Check and normalize the weights for blending quaternions.

Parameters:

quaternions – An array of shape (…, k, 4) representing the quaternions to blend.
weights_in – An optional array of shape (…, k) representing the weights for each quaternion. If not provided, all quaternions will be weighted equally.

Returns:

An array of shape (…, k) representing the normalized weights.

pymomentum.quaternion_np.conjugate(q: NDArray) → NDArray

Conjugate a quaternion.

Parameters:: q – A quaternion ((x, y, z), w)).
Returns:: The conjugate.

pymomentum.quaternion_np.euler_xyz_to_quaternion(euler_xyz: NDArray) → NDArray

Convert Euler XYZ angles to a quaternion.

This function converts XYZ Euler angles to quaternions. The rotation order is X-Y-Z, meaning first rotate around X-axis, then Y-axis, then Z-axis.

Parameters:: euler_xyz – An array of shape (…, 3) representing the Euler XYZ angles in order [roll, pitch, yaw].
Returns:: An array of shape (…, 4) representing the quaternion in ((x, y, z), w) format.

pymomentum.quaternion_np.euler_zyx_to_quaternion(euler_zyx: NDArray) → NDArray

Convert Euler ZYX angles to a quaternion.

This function converts ZYX Euler angles (yaw-pitch-roll convention) to quaternions. The rotation order is Z-Y-X, meaning first rotate around Z-axis (yaw), then Y-axis (pitch), then X-axis (roll).

Parameters:: euler_zyx – An array of shape (…, 3) representing the Euler ZYX angles in order [yaw, pitch, roll].
Returns:: An array of shape (…, 4) representing the quaternion in ((x, y, z), w) format.

pymomentum.quaternion_np.from_axis_angle(axis_angle: NDArray) → NDArray

Convert an axis-angle array to a quaternion.

Parameters:: axis_angle – An array of shape (…, 3) representing the axis-angle.
Returns:: An array of shape (…, 4) representing the quaternion in ((x, y, z), w) format.

pymomentum.quaternion_np.from_rotation_matrix(matrices: NDArray) → NDArray

Convert a rotation matrix to a quaternion using numerically stable method.

This implementation uses the robust algorithm that computes all four quaternion component candidates and selects the best-conditioned one, ensuring numerical stability across all rotation matrix configurations.

Parameters:: matrices – An array of shape (…, 3, 3) representing the rotation matrices.
Returns:: An array of shape (…, 4) representing the quaternions in ((x, y, z), w) format.

pymomentum.quaternion_np.from_two_vectors(v1: NDArray, v2: NDArray) → NDArray

Construct a quaternion that rotates one vector into another.

Parameters:

v1 – The initial vector.
v2 – The target vector.

Returns:

A quaternion representing the rotation from v1 to v2.

pymomentum.quaternion_np.identity(size: ~typing.Sequence[int] | None = None, dtype: ~numpy.dtype = <class 'numpy.float32'>) → NDArray

Create a quaternion identity array.

Parameters:

size – A tuple of integers representing the size of the quaternion array.
dtype – The data type for the array.

Returns:

A quaternion identity array with the specified size and dtype.

pymomentum.quaternion_np.inverse(q: NDArray) → NDArray

Compute the inverse of a quaternion.

Uses numerical clamping to avoid division by very small numbers, improving numerical stability for near-zero quaternions.

Parameters:: q – A quaternion ((x, y, z), w)).
Returns:: The inverse.

pymomentum.quaternion_np.multiply(q1: NDArray, q2: NDArray) → NDArray

Multiply two quaternions together.

Normalizes input quaternions before multiplication for numerical stability. For performance-critical code where quaternions are guaranteed to be normalized, use multiply_assume_normalized().

Parameters:

q1 – A quaternion ((x, y, z), w)).
q2 – A quaternion ((x, y, z), w)).

Returns:

The normalized product q1*q2.

pymomentum.quaternion_np.multiply_assume_normalized(q1: NDArray, q2: NDArray) → NDArray

Multiply two quaternions together, assuming they are already normalized.

This is a performance-optimized version of multiply() that skips normalization of the input quaternions. Use this only when you are certain both quaternions are already normalized.

Parameters:

q1 – A normalized quaternion ((x, y, z), w)).
q2 – A normalized quaternion ((x, y, z), w)).

Returns:

The product q1*q2.

pymomentum.quaternion_np.normalize(q: NDArray) → NDArray

Normalize a quaternion.

Parameters:: q – A quaternion ((x, y, z), w)).
Returns:: The normalized quaternion.

pymomentum.quaternion_np.quaternion_to_xyz_euler(q: NDArray, eps: float = 1e-06) → NDArray

Parameters:: eps – a small number to avoid calling asin(1) or asin(-1). Should not be smaller than 1e-6 as this can cause numerical issues.

pymomentum.quaternion_np.rotate_vector(q: NDArray, v: NDArray) → NDArray

Rotate a vector by a quaternion.

Normalizes the input quaternion before rotation for numerical stability. For performance-critical code where quaternions are guaranteed to be normalized, use rotate_vector_assume_normalized().

Parameters:

q – (nBatch x k x 4) array with the quaternions in ((x, y, z), w) format.
v – (nBatch x k x 3) vector.

Returns:

(nBatch x k x 3) rotated vectors.

pymomentum.quaternion_np.rotate_vector_assume_normalized(q: NDArray, v: NDArray) → NDArray

Rotate a vector by a quaternion, assuming the quaternion is already normalized.

This is a performance-optimized version of rotate_vector() that skips normalization of the input quaternion. Use this only when you are certain the quaternion is already normalized.

Parameters:

q – (nBatch x k x 4) array with normalized quaternions in ((x, y, z), w) format.
v – (nBatch x k x 3) vector.

Returns:

(nBatch x k x 3) rotated vectors.

pymomentum.quaternion_np.slerp(q0: NDArray, q1: NDArray, t: NDArray | float) → NDArray

Perform spherical linear interpolation (slerp) between two quaternions.

Parameters:

q0 – The starting quaternion.
q1 – The ending quaternion.
t – The interpolation parameter, where 0 <= t <= 1. t=0 corresponds to q0, t=1 corresponds to q1.

Returns:

The interpolated quaternion.

pymomentum.quaternion_np.split(q: NDArray) → tuple[NDArray, NDArray]

Split a quaternion into its scalar and vector parts.

Parameters:: q – An array representing a quaternion.
Returns:: The scalar and vector parts of the quaternion.

pymomentum.quaternion_np.to_rotation_matrix(q: NDArray) → NDArray

Convert quaternions to 3x3 rotation matrices.

Parameters:: q – (nBatch x k x 4) array with the quaternions in ((x, y, z), w) format.
Returns:: (nBatch x k x 3 x 3) array with 3x3 rotation matrices.

pymomentum.quaternion_np.to_rotation_matrix_assume_normalized(q: NDArray) → NDArray

Convert quaternions to 3x3 rotation matrices.

Parameters:: q – (nBatch x k x 4) array with the quaternions in ((x, y, z), w) format.
Returns:: (nBatch x k x 3 x 3) array with 3x3 rotation matrices.