unilab.envs.common.rotation

Functions

np_matrix_first_two_cols_from_quat(q)

Return flattened first two rotation-matrix columns for quaternions (..., 4).

np_matrix_from_quat(q)

Convert quaternion(s) to rotation matrix (N, 3, 3) or (3, 3), w-first.

np_quat_angular_velocity(q, dt)

Estimate angular velocity from a quaternion time sequence using shortest-arc diffs.

np_quat_apply(q, v)

Rotate vector(s) by quaternion(s), supports batched/scalar inputs.

np_quat_apply_batched(q, v)

Rotate broadcast-compatible vector arrays by quaternions.

np_quat_apply_inverse(q, v)

Rotate vector(s) by inverse quaternion(s).

np_quat_canonicalize(q)

Flip quaternion signs so the real part is non-negative.

np_quat_conjugate(q)

Conjugate of unit quaternions (N, 4) or (4,), w-first.

np_quat_conjugate_batched(q)

Conjugate quaternions with shape (..., 4), w-first.

np_quat_ensure_continuity(q)

Flip quaternion signs in a time sequence to keep adjacent dots non-negative.

np_quat_error_magnitude(q1, q2)

Angular error magnitude between quaternions (N,) or scalar.

np_quat_error_magnitude_batched(q1, q2)

Angular error magnitude for broadcast-compatible quaternions (..., 4).

np_quat_error_magnitude_squared_batched(q1, q2)

Squared angular error for broadcast-compatible quaternions (..., 4).

np_quat_from_euler_xyz(roll, pitch, yaw)

Convert Euler angles (XYZ) to quaternions (N, 4) or (4,), w-first.

np_quat_inv(q)

Inverse of unit quaternions (N, 4) or (4,), w-first.

np_quat_mul(q1, q2)

Multiply quaternions in NumPy, supports (N, 4) and (4,) inputs.

np_quat_mul_batched(q1, q2)

Multiply broadcast-compatible quaternion arrays with shape (..., 4).

np_quat_to_axis_angle(q)

Convert unit quaternion batch (N, 4), w-first, to axis-angle vectors (N, 3).

np_subtract_anchor_frame_transforms(...)

Compute body transforms relative to per-env anchors.

np_subtract_frame_transforms(pos1, quat1, ...)

Compute relative transform from frame 1 to frame 2 in frame-1 coordinates.

np_wrap_to_pi(angle)

Wrap angle(s) into (-pi, pi].

np_yaw_from_quat(quat)

Yaw angle (N,) from quaternion batch (N, 4), w-first.

np_yaw_quat(q)

Extract yaw-only quaternion from full quaternion(s), w-first.

np_yaw_to_quat(yaw)

Convert yaw batch (N,) to quaternion batch (N, 4) in NumPy.
unilab.envs.common.rotation.np_quat_mul(q1, q2)[source]

Multiply quaternions in NumPy, supports (N, 4) and (4,) inputs.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_conjugate_batched(q)[source]

Conjugate quaternions with shape (…, 4), w-first.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_mul_batched(q1, q2)[source]

Multiply broadcast-compatible quaternion arrays with shape (…, 4).

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_conjugate(q)[source]

Conjugate of unit quaternions (N, 4) or (4,), w-first.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_canonicalize(q)[source]

Flip quaternion signs so the real part is non-negative.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_ensure_continuity(q)[source]

Flip quaternion signs in a time sequence to keep adjacent dots non-negative.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_to_axis_angle(q)[source]

Convert unit quaternion batch (N, 4), w-first, to axis-angle vectors (N, 3).

Adapted from PyTorch3D. Uses atan2 + Taylor expansion for numerical stability near zero rotation.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_angular_velocity(q, dt)[source]

Estimate angular velocity from a quaternion time sequence using shortest-arc diffs.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_yaw_to_quat(yaw)[source]

Convert yaw batch (N,) to quaternion batch (N, 4) in NumPy.

Parameters:

yaw (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_yaw_from_quat(quat)[source]

Yaw angle (N,) from quaternion batch (N, 4), w-first.

Parameters:

quat (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_wrap_to_pi(angle)[source]

Wrap angle(s) into (-pi, pi].

Parameters:

angle (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_inv(q)[source]

Inverse of unit quaternions (N, 4) or (4,), w-first.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_quat_apply(q, v)[source]

Rotate vector(s) by quaternion(s), supports batched/scalar inputs.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_apply_batched(q, v)[source]

Rotate broadcast-compatible vector arrays by quaternions.

q has shape (…, 4), v has shape (…, 3), and leading dimensions are broadcast. This avoids flattening/tile allocations in hot env paths.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_apply_inverse(q, v)[source]

Rotate vector(s) by inverse quaternion(s).

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_error_magnitude(q1, q2)[source]

Angular error magnitude between quaternions (N,) or scalar.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_error_magnitude_batched(q1, q2)[source]

Angular error magnitude for broadcast-compatible quaternions (…, 4).

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_error_magnitude_squared_batched(q1, q2)[source]

Squared angular error for broadcast-compatible quaternions (…, 4).

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_quat_from_euler_xyz(roll, pitch, yaw)[source]

Convert Euler angles (XYZ) to quaternions (N, 4) or (4,), w-first.

Parameters:
Return type:

ndarray

unilab.envs.common.rotation.np_yaw_quat(q)[source]

Extract yaw-only quaternion from full quaternion(s), w-first.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_matrix_from_quat(q)[source]

Convert quaternion(s) to rotation matrix (N, 3, 3) or (3, 3), w-first.

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_matrix_first_two_cols_from_quat(q)[source]

Return flattened first two rotation-matrix columns for quaternions (…, 4).

The output order matches np_matrix_from_quat(q)[:, :, :2].reshape(..., 6).

Parameters:

q (ndarray)

Return type:

ndarray

unilab.envs.common.rotation.np_subtract_frame_transforms(pos1, quat1, pos2, quat2)[source]

Compute relative transform from frame 1 to frame 2 in frame-1 coordinates.

Parameters:
Return type:

tuple[ndarray, ndarray]

unilab.envs.common.rotation.np_subtract_anchor_frame_transforms(anchor_pos, anchor_quat, body_pos, body_quat)[source]

Compute body transforms relative to per-env anchors.

anchor_pos/anchor_quat are shaped (N, 3)/(N, 4), while body_pos/body_quat are shaped (N, B, 3)/(N, B, 4).

Parameters:
Return type:

tuple[ndarray, ndarray]