Geometry¶

The geometry utilities contain direct pose-coordinate encodings, rotation representation conversions, and geodesic error metrics.

poses converts direct pose coordinates into homogeneous transforms.
rotations converts between quaternion, rotation-vector, matrix, and 6D rotation representations.
errors computes shortest-path orientation errors for control and tracking.

Pose Coordinates¶

soromox.utils.geometry.poses ¶

planar_pose_to_transform ¶

planar_pose_to_transform(pose: Array) -> Array

Construct an SE(2) transform from direct planar pose coordinates.

This helper treats the translation entries as pose coordinates, not as Lie-algebra twist coordinates. A pose [theta, x, y] maps directly to [[R(theta), [x, y]], [0, 0, 1]]. Use se2.exp when the input is an se(2) twist whose translational part must be integrated through the SE(2) left Jacobian.

Parameters:

Name	Type	Description	Default
`pose`	`Array`	Planar pose with shape `(3,)` or `(3, 1)` in `[theta, x, y]` order. `theta` is a right-handed rotation angle in radians about the out-of-plane z-axis. `x` and `y` are direct translation coordinates in the transform parent frame.	required

Returns:

Type	Description
`Array`	Homogeneous `SE(2)` transform with shape `(3, 3)`.

planar_pose_from_transform ¶

planar_pose_from_transform(g: Array, eps: float | Array) -> Array

Extract direct planar pose coordinates from an SE(2) transform.

This is the inverse of :func:planar_pose_to_transform for the principal angle returned by arctan2. It returns the raw transform translation, not the translational twist coordinates returned by se2.log.

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(2)` transform with shape `(3, 3)`. The rotation is read from `g[:2, :2]` and the direct translation from `g[:2, 2]`.	required
`eps`	`float \| Array`	Small positive scalar threshold. Angles with magnitude below this threshold are regularized to exactly zero.	required

Returns:

Type	Description
`Array`	Array with shape `(3,)` in direct pose-coordinate order
`Array`	`[theta, x, y]`.

quaternion_pose_to_transform ¶

quaternion_pose_to_transform(pose: Array) -> Array

Construct an SE(3) transform from quaternion pose coordinates.

This helper treats the final three entries as direct translation coordinates, not as Lie-algebra twist coordinates. The quaternion is scalar-first and is normalized by quaternion_to_rotation_matrix before constructing the rotation block. Use se3.exp when the input is an se(3) twist whose translational part must be integrated through the SE(3) left Jacobian.

Parameters:

Name	Type	Description	Default
`pose`	`Array`	Spatial pose with shape `(7,)` or `(7, 1)` in `[qw, qx, qy, qz, x, y, z]` order. The quaternion is scalar-first and the translation `[x, y, z]` is inserted directly into the homogeneous transform.	required

Returns:

Type	Description
`Array`	Homogeneous `SE(3)` transform with shape `(4, 4)`.

Rotation Representations¶

soromox.utils.geometry.rotations ¶

Rotation utilities for converting between representation encodings.

This module provides numerically stable conversions between rotation matrices, quaternions, and rotation vectors (axis-angle representation). The implementations are designed to handle edge cases such as small rotations (θ ≈ 0) and near-180-degree rotations (θ ≈ π) correctly.

All functions are JAX-compatible and can be used with jit, vmap, and grad.

Quaternion Convention

This module uses scalar-first Hamilton quaternions in [qw, qx, qy, qz] order, where qw is the scalar part and [qx, qy, qz] is the vector part. A rotation by angle θ around unit axis n is represented as: q = [cos(θ/2), sin(θ/2) * n_x, sin(θ/2) * n_y, sin(θ/2) * n_z]

Rotation Representations

This module supports multiple rotation representations via the RotationRepresentation enum. Each has different properties suitable for different use cases:

ROTATION_VECTOR (3D): Compact, axis-angle representation. The magnitude is the rotation angle and the direction is the rotation axis.
QUATERNION (4D): Unit quaternion representation. Good for interpolation and avoiding gimbal lock, but has antipodal ambiguity (q and -q represent same rotation).
ROTATION_MATRIX_6D (6D): Continuous representation using first two columns of the rotation matrix (Zhou et al. 2019). Excellent for neural networks as it has no discontinuities, but not suitable for direct error computation.

RotationRepresentation ¶

Bases: Enum


              flowchart TD
              soromox.utils.geometry.rotations.RotationRepresentation[RotationRepresentation]

              

              click soromox.utils.geometry.rotations.RotationRepresentation href "" "soromox.utils.geometry.rotations.RotationRepresentation"

Enumeration of supported rotation representations.

Each representation has different properties and is suitable for different use cases:

ROTATION_VECTOR: 3D representation where magnitude = angle, direction = axis. Compact but has discontinuity at angle = ±π.
QUATERNION: 4D unit quaternion representation. Good for interpolation but has antipodal ambiguity.
ROTATION_MATRIX_6D: 6D continuous representation (first two columns of rotation matrix). Continuous everywhere, ideal for neural networks.

Note

For computing orientation errors in control applications, use soromox.utils.geometry.errors regardless of the representation chosen for pose coordinates.

principal_axis_rotation_matrix ¶

principal_axis_rotation_matrix(angle: Array, axis: Literal['x', 'y', 'z']) -> Array

Return the active rotation matrix about a Cartesian principal axis.

Parameters:

Name	Type	Description	Default
`angle`	`Array`	Right-handed rotation angle in radians.	required
`axis`	`Literal['x', 'y', 'z']`	Static Cartesian axis identifier.	required

Returns:

Type	Description
`Array`	Active rotation matrix with shape `(3, 3)`.

principal_axis_rotation_matrix_derivative ¶

principal_axis_rotation_matrix_derivative(angle: Array, axis: Literal['x', 'y', 'z']) -> Array

Differentiate a principal-axis rotation matrix with respect to angle.

normalize_quaternion ¶

normalize_quaternion(quaternion: Array, eps: float | Array | None = None) -> Array

Normalize a scalar-first Hamilton quaternion.

Parameters:

Name	Type	Description	Default
`quaternion`	`Array`	Quaternion with shape `(4,)` in `[qw, qx, qy, qz]` order. `qw` is the scalar component and `[qx, qy, qz]` is the vector component.	required
`eps`	`float \| Array \| None`	Optional nonnegative norm threshold. If the quaternion norm is less than or equal to `eps`, the identity quaternion is returned. Defaults to machine epsilon for the quaternion dtype.	`None`

Returns:

Name	Type	Description
`Array`	`Array`	Unit quaternion with shape `(4,)` in `[qw, qx, qy, qz]`
	`Array`	order. If the input norm is numerically smaller than `eps`, the
	`Array`	identity quaternion `[1, 0, 0, 0]` is returned to avoid
	`Array`	division-by-zero NaNs in traced computations. Parameter validators
	`Array`	reject zero-norm configured poses.

rotation_matrix_to_quaternion ¶

rotation_matrix_to_quaternion(R: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Convert a rotation matrix to a unit quaternion.

Uses Shepperd's method, which is numerically stable for all rotation angles including near-180-degree rotations (θ ≈ π).

Parameters:

Name	Type	Description	Default
`R`	`Array`	Rotation matrix of shape (3, 3).	required
`eps`	`float`	Small value to avoid division by zero. Default is 1e-10.	`DEFAULT_ROTATION_EPS`

Returns:

Name	Type	Description
`q`	`Array`	Unit quaternion with shape `(4,)` in scalar-first `[qw, qx, qy, qz]` order.

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_matrix_to_quaternion

R = jnp.eye(3)
q = rotation_matrix_to_quaternion(R)  # approximately [1, 0, 0, 0]

References

Shepperd, S. W. (1978). Quaternion from rotation matrix. Journal of Guidance and Control, 1(3), 223-224.

quaternion_to_rotation_matrix ¶

quaternion_to_rotation_matrix(quat: Array) -> Array

Convert a unit quaternion to a rotation matrix.

Parameters:

Name	Type	Description	Default
`quat`	`Array`	Quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order. The quaternion is normalized before constructing the matrix.	required

Returns:

Name	Type	Description
`R`	`Array`	Rotation matrix of shape (3, 3).

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import quaternion_to_rotation_matrix

q = jnp.array([1.0, 0.0, 0.0, 0.0])
R = quaternion_to_rotation_matrix(q)  # approximately eye(3)

Note

If the quaternion norm is numerically zero, identity rotation is used to keep traced computations finite. Configured robot base poses are validated separately and reject zero-norm quaternions.

quaternion_to_rotation_vector ¶

quaternion_to_rotation_vector(quat: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Convert a unit quaternion to a rotation vector (axis-angle representation).

This correctly handles both small rotations (θ ≈ 0) and near-180-degree rotations (θ ≈ π), avoiding the singularity that affects direct rotation matrix to rotation vector conversion.

The rotation vector ω has magnitude equal to the rotation angle θ and direction equal to the rotation axis. For a rotation by angle θ around unit axis n, the rotation vector is ω = θ * n.

Parameters:

Name	Type	Description	Default
`quat`	`Array`	Unit quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order.	required
`eps`	`float`	Small value to avoid division by zero. Default is 1e-10.	`DEFAULT_ROTATION_EPS`

Returns:

Name	Type	Description
`omega`	`Array`	Rotation vector of shape (3,).

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import quaternion_to_rotation_vector

# 90-degree rotation around the z-axis
q = jnp.array([jnp.cos(jnp.pi / 4), 0, 0, jnp.sin(jnp.pi / 4)])
omega = quaternion_to_rotation_vector(q)  # approximately [0, 0, pi/2]

rotation_vector_to_quaternion ¶

rotation_vector_to_quaternion(omega: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Convert a rotation vector (axis-angle representation) to a unit quaternion.

The rotation vector ω has magnitude equal to the rotation angle θ and direction equal to the rotation axis. For a rotation by angle θ around unit axis n, the rotation vector is ω = θ * n.

Parameters:

Name	Type	Description	Default
`omega`	`Array`	Rotation vector of shape (3,).	required
`eps`	`float`	Small value for numerical stability at small angles. Default is 1e-10.	`DEFAULT_ROTATION_EPS`

Returns:

Name	Type	Description
`q`	`Array`	Unit quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order.

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_vector_to_quaternion

# 90-degree rotation around the z-axis
omega = jnp.array([0.0, 0.0, jnp.pi / 2])
q = rotation_vector_to_quaternion(omega)
# q is approximately [cos(pi/4), 0, 0, sin(pi/4)]

rotation_matrix_to_rotation_vector ¶

rotation_matrix_to_rotation_vector(R: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Convert a rotation matrix to a rotation vector (axis-angle representation).

This is a convenience wrapper around soromox.utils.lie_algebra.so3.log. It correctly handles both small rotations (θ ≈ 0) and near-180-degree rotations (θ ≈ π).

The rotation vector ω has magnitude equal to the rotation angle θ and direction equal to the rotation axis. For a rotation by angle θ around unit axis n, the rotation vector is ω = θ * n.

Parameters:

Name	Type	Description	Default
`R`	`Array`	Rotation matrix of shape (3, 3).	required
`eps`	`float`	Small value to avoid division by zero. Default is 1e-10.	`DEFAULT_ROTATION_EPS`

Returns:

Name	Type	Description
`omega`	`Array`	Rotation vector of shape (3,).

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_matrix_to_rotation_vector

# 180-degree rotation around the z-axis
R = jnp.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1.0]])
omega = rotation_matrix_to_rotation_vector(R)  # approximately [0, 0, pi]

Note

This delegates to soromox.utils.lie_algebra.so3.log so that the matrix logarithm has one canonical implementation.

rotation_vector_to_rotation_matrix ¶

rotation_vector_to_rotation_matrix(omega: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Convert a rotation vector (axis-angle representation) to a rotation matrix.

This is a convenience wrapper around soromox.utils.lie_algebra.so3.exp. It correctly handles both small rotations (θ ≈ 0) and all other rotation angles.

The rotation vector ω has magnitude equal to the rotation angle θ and direction equal to the rotation axis. For a rotation by angle θ around unit axis n, the rotation vector is ω = θ * n.

Parameters:

Name	Type	Description	Default
`omega`	`Array`	Rotation vector of shape (3,).	required
`eps`	`float`	Small value for numerical stability at small angles. Default is 1e-10.	`DEFAULT_ROTATION_EPS`

Returns:

Name	Type	Description
`R`	`Array`	Rotation matrix of shape (3, 3).

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_vector_to_rotation_matrix

# 90-degree rotation around the z-axis
omega = jnp.array([0.0, 0.0, jnp.pi / 2])
R = rotation_vector_to_rotation_matrix(omega)

Note

This delegates to soromox.utils.lie_algebra.so3.exp so that Rodrigues' formula has one canonical implementation.

rotation_matrix_to_6d ¶

rotation_matrix_to_6d(R: Array) -> Array

Convert a rotation matrix to a 6D continuous representation.

The 6D representation uses the first two columns of the rotation matrix. This representation is continuous everywhere in SO(3), making it ideal for neural network outputs.

Parameters:

Name	Type	Description	Default
`R`	`Array`	Rotation matrix of shape (3, 3).	required

Returns:

Name	Type	Description
`r6d`	`Array`	6D representation of shape (6,), containing [R[:, 0], R[:, 1]].

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_matrix_to_6d

R = jnp.eye(3)
r6d = rotation_matrix_to_6d(R)  # [1, 0, 0, 0, 1, 0]

References

Zhou, Y., Barnes, C., Lu, J., Yang, J., & Li, H. (2019). On the continuity of rotation representations in neural networks. CVPR 2019.

rotation_6d_to_rotation_matrix ¶

rotation_6d_to_rotation_matrix(r6d: Array) -> Array

Convert a 6D continuous representation to a rotation matrix.

Uses Gram-Schmidt orthonormalization to ensure the output is a valid rotation matrix. The third column is computed as the cross product of the first two orthonormalized columns.

Parameters:

Name	Type	Description	Default
`r6d`	`Array`	6D representation of shape (6,).	required

Returns:

Name	Type	Description
`R`	`Array`	Rotation matrix of shape (3, 3).

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import rotation_6d_to_rotation_matrix

r6d = jnp.array([1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
R = rotation_6d_to_rotation_matrix(r6d)  # approximately eye(3)

References

Zhou, Y., Barnes, C., Lu, J., Yang, J., & Li, H. (2019). On the continuity of rotation representations in neural networks. CVPR 2019.

quaternion_multiply ¶

quaternion_multiply(q1: Array, q2: Array) -> Array

Multiply two quaternions (Hamilton product).

The result represents the composition of rotations: first q2, then q1. That is, q1 * q2 represents rotating by q2 first, then by q1.

Parameters:

Name	Type	Description	Default
`q1`	`Array`	First quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order.	required
`q2`	`Array`	Second quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order.	required

Returns:

Name	Type	Description
`q`	`Array`	Product quaternion with shape `(4,)` in `[qw, qx, qy, qz]` order.

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import quaternion_multiply

q1 = jnp.array([1, 0, 0, 0])
q2 = jnp.array([jnp.cos(jnp.pi / 4), 0, 0, jnp.sin(jnp.pi / 4)])
q = quaternion_multiply(q1, q2)  # approximately q2

quaternion_conjugate ¶

quaternion_conjugate(q: Array) -> Array

Compute the conjugate of a quaternion.

For a unit quaternion, the conjugate is also the inverse and represents the opposite rotation.

Parameters:

Name	Type	Description	Default
`q`	`Array`	Quaternion with shape `(4,)` in scalar-first Hamilton `[qw, qx, qy, qz]` order.	required

Returns:

Name	Type	Description
`q_conj`	`Array`	Conjugate quaternion with shape `(4,)` and coordinates `[qw, -qx, -qy, -qz]`.

Example

import jax.numpy as jnp
from soromox.utils.geometry.rotations import quaternion_conjugate

q = jnp.array([jnp.cos(jnp.pi / 4), 0, 0, jnp.sin(jnp.pi / 4)])
q_conj = quaternion_conjugate(q)  # represents -90 degrees around z

Geodesic Errors¶

soromox.utils.geometry.errors ¶

Geodesic error utilities for planar and spatial rotations.

These functions compute shortest-path orientation errors on the appropriate rotation group. They are intended for control and trajectory tracking where subtracting representation coordinates directly would produce discontinuities or long-way-around errors.

wrap_angle ¶

wrap_angle(angle: Array) -> Array

Wrap an angle to the principal interval [-pi, pi).

This helper applies elementwise and is useful whenever planar angles need to be compared modulo 2*pi. The interval is half-open: both pi and -pi represent the same orientation, and this implementation maps that boundary to -pi.

Parameters:

Name	Type	Description	Default
`angle`	`Array`	Scalar or array of angles in radians.	required

Returns:

Type	Description
`Array`	Array with the same shape as `angle` containing wrapped angles in
`Array`	radians.

angle_geodesic_error ¶

angle_geodesic_error(theta_current: Array, theta_desired: Array) -> Array

Compute the shortest-path error between two planar orientations.

The result is the signed angular displacement that rotates the current orientation into the desired orientation along the shortest path on SO(2). Positive values follow the right-handed counter-clockwise convention.

Parameters:

Name	Type	Description	Default
`theta_current`	`Array`	Current planar angle in radians. May be a scalar or an array of angles.	required
`theta_desired`	`Array`	Desired planar angle in radians. Must be broadcastable with `theta_current`.	required

Returns:

Type	Description
`Array`	Scalar or array with the broadcasted shape of the inputs. Values are in
`Array`	the principal interval `[-pi, pi)`.

rotation_matrix_geodesic_error ¶

rotation_matrix_geodesic_error(R_current: Array, R_desired: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Compute the geodesic orientation error between two rotation matrices.

The error is the SO(3) logarithm of the relative rotation R_desired @ R_current.T. It is a rotation vector representing the shortest-path correction from the current orientation to the desired orientation. This is the appropriate orientation error for operational space control; naive subtraction of rotation-vector or quaternion coordinates is representation-dependent and can choose the long path.

Parameters:

Name	Type	Description	Default
`R_current`	`Array`	Current rotation matrix with shape `(3, 3)`.	required
`R_desired`	`Array`	Desired rotation matrix with shape `(3, 3)`.	required
`eps`	`float`	Small nonnegative scalar threshold passed to the `SO(3)` logarithm.	`DEFAULT_ROTATION_EPS`

Returns:

Type	Description
`Array`	Rotation vector with shape `(3,)`. Its norm is the shortest
`Array`	correction angle in radians and its direction is the correction axis.

quaternion_geodesic_error ¶

quaternion_geodesic_error(q_current: Array, q_desired: Array, eps: float = DEFAULT_ROTATION_EPS) -> Array

Compute the geodesic orientation error between two quaternions.

Quaternions use the scalar-first Hamilton convention [qw, qx, qy, qz]. The relative quaternion is q_desired * conjugate(q_current) and is flipped when needed so the scalar component is nonnegative. That resolves the antipodal ambiguity and returns the shortest-path rotation vector.

Parameters:

Name	Type	Description	Default
`q_current`	`Array`	Current quaternion with shape `(4,)` in scalar-first Hamilton order.	required
`q_desired`	`Array`	Desired quaternion with shape `(4,)` in scalar-first Hamilton order.	required
`eps`	`float`	Small nonnegative scalar threshold passed to quaternion-to-vector conversion.	`DEFAULT_ROTATION_EPS`

Returns:

Type	Description
`Array`	Rotation vector with shape `(3,)` representing the shortest-path
`Array`	correction from `q_current` to `q_desired`.