Lie Algebra¶

The Lie algebra utilities are organized by semantic responsibility:

so2 and so3 contain pure rotational Lie group and Lie algebra operators.
se2 and se3 contain rigid-body Lie group and Lie algebra operators.
constant_strain contains arclength operators for rod segment kinematics.

SO(2)¶

soromox.utils.lie_algebra.so2 ¶

Lie group and Lie algebra operators for planar rotations.

This module contains pure SO(2) / so(2) operations. It does not handle translations or pose-coordinate encodings; use se2 for planar rigid-body transforms and soromox.utils.geometry.poses for direct pose coordinates.

skew ¶

skew(angle: Array) -> Array

Return the so(2) skew-symmetric matrix for a scalar angle.

The planar Lie algebra is one-dimensional. This function maps the scalar coordinate theta to theta * J where J = [[0, -1], [1, 0]]. Positive theta follows the standard right-handed convention for counter-clockwise rotation in the xy-plane.

Parameters:

Name	Type	Description	Default
`angle`	`Array`	Scalar or array-like value broadcastable to a scalar. The first flattened entry is interpreted as the angular coordinate `theta` in radians.	required

Returns:

Type	Description
`Array`	Array with shape `(2, 2)` containing the skew-symmetric matrix
`Array`	`theta * J`.

exp ¶

exp(angle: Array) -> Array

Compute the Lie-group exponential from so(2) to SO(2).

For SO(2), the exponential map is the standard planar rotation matrix R(theta). The input is an angular Lie-algebra coordinate, not a direct planar pose; translations are handled by se2.exp or by geometry pose helpers depending on the coordinate convention.

Parameters:

Name	Type	Description	Default
`angle`	`Array`	Scalar or array-like value broadcastable to a scalar. The first flattened entry is interpreted as the rotation angle `theta` in radians.	required

Returns:

Type	Description
`Array`	Rotation matrix with shape `(2, 2)`.

log ¶

log(R: Array, eps: float | Array = 1e-10) -> Array

Compute the principal Lie-group logarithm from SO(2) to so(2).

The returned scalar is the principal angle obtained from atan2 and therefore lies in the half-open wrapping convention induced by atan2. Very small angles are regularized to exact zero to keep downstream comparisons and control errors stable near identity.

Parameters:

Name	Type	Description	Default
`R`	`Array`	Rotation matrix with shape `(2, 2)`. The angle is read from `atan2(R[1, 0], R[0, 0])`.	required
`eps`	`float \| Array`	Small nonnegative scalar threshold. If the recovered angle has magnitude less than `eps`, the function returns exact zero.	`1e-10`

Returns:

Type	Description
`Array`	Scalar array containing the principal rotation angle in radians.

SO(3)¶

soromox.utils.lie_algebra.so3 ¶

Lie group and Lie algebra operators for spatial rotations.

This module contains pure SO(3) / so(3) operations. Rotation vectors use axis-angle coordinates: the vector direction is the rotation axis and its Euclidean norm is the rotation angle in radians.

vee ¶

vee(matrix: Array) -> Array

Extract the vector coordinate of the skew part of a matrix.

This is the inverse of :func:skew for matrices in so(3). For a general matrix, it first projects onto so(3) using 0.5 * (matrix - matrix.T). In particular, vee(skew(omega)) == omega.

Parameters:

Name	Type	Description	Default
`matrix`	`Array`	Matrix with shape `(3, 3)`.	required

Returns:

Type	Description
`Array`	Vector with shape `(3,)` in `[x, y, z]` order.

skew ¶

skew(vec: Array) -> Array

Return the so(3) skew-symmetric cross-product matrix.

For a vector a = [x, y, z], the returned matrix a_hat satisfies a_hat @ b == cross(a, b) for any Cartesian 3-vector b. This is the matrix representation used by SO(3) and the rotational blocks of spatial SE(3) operators.

Parameters:

Name	Type	Description	Default
`vec`	`Array`	Vector with shape `(3,)` or `(3, 1)` in Cartesian `[x, y, z]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` containing the skew-symmetric
`Array`	cross-product matrix.

exp ¶

exp(omega: Array, eps: float | Array = 1e-10) -> Array

Compute the Lie-group exponential from so(3) to SO(3).

The input is an axis-angle rotation vector. The function evaluates Rodrigues' formula and switches to a Taylor series near zero rotation to avoid singular divisions.

Parameters:

Name	Type	Description	Default
`omega`	`Array`	Rotation vector with shape `(3,)` or `(3, 1)`. Its direction is the rotation axis and its Euclidean norm is the angle in radians.	required
`eps`	`float \| Array`	Small nonnegative scalar threshold. If `norm(omega) <= eps`, the small-angle series branch is used.	`1e-10`

Returns:

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

log ¶

log(R: Array, eps: float | Array = 1e-10) -> Array

Compute the principal Lie-group logarithm from SO(3) to so(3).

The returned vector is the principal axis-angle coordinate of R. Its norm is in [0, pi] and its direction is the corresponding rotation axis. The implementation uses the skew part of R in the regular case, a first-order series near identity, and a diagonal-axis reconstruction near pi where the skew part becomes singular.

Parameters:

Name	Type	Description	Default
`R`	`Array`	Rotation matrix with shape `(3, 3)`.	required
`eps`	`float \| Array`	Small nonnegative scalar threshold used for normalization and small-angle handling.	`1e-10`

Returns:

Type	Description
`Array`	Rotation vector with shape `(3,)` whose Euclidean norm is the
`Array`	principal rotation angle in radians.

SE(2)¶

soromox.utils.lie_algebra.se2 ¶

hat ¶

hat(xi: Array) -> Array

Return the homogeneous matrix representation of an se(2) twist.

Twists use angular-first coordinates xi = [theta, v_x, v_y]. The first entry is the scalar angular component and the last two entries are the translational twist coordinates. The resulting matrix is [[so2.skew(theta), v], [0, 0, 0]] and is suitable for use in the matrix exponential.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Planar twist with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` representing the same algebra element in
`Array`	homogeneous matrix form.

exp ¶

exp(xi: Array, eps: float | Array) -> Array

Compute the Lie-group exponential from se(2) to SE(2).

This is the matrix exponential of :func:hat. The translational part is integrated through the SE(2) left Jacobian, so the input translation is a twist coordinate, not a direct pose translation. Use poses.planar_pose_to_transform when the input is a direct [theta, x, y] pose.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Planar twist with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required
`eps`	`float \| Array`	Small positive scalar threshold. For `abs(theta) <= eps` the function uses a truncated Taylor series to avoid singular divisions and keep autodiff finite near zero rotation.	required

Returns:

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

log ¶

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

Compute the Lie-group logarithm from SE(2) to se(2).

This function is the inverse of :func:exp for regular planar transforms: it returns twist coordinates, not direct pose coordinates. In particular, the returned translational coordinates are obtained by applying the inverse SE(2) left Jacobian to the transform translation. Use poses.planar_pose_from_transform when the desired output is direct pose coordinates [theta, x, y].

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 translation from `g[:2, 2]`.	required
`eps`	`float \| Array`	Small positive scalar threshold. Angles with magnitude below this threshold use the small-angle inverse-Jacobian series.	required

Returns:

Type	Description
`Array`	Array with shape `(3,)` containing twist coordinates
`Array`	`[theta, v_x, v_y]`.

small_adjoint ¶

small_adjoint(xi: Array) -> Array

Return the Lie algebra adjoint matrix ad_xi.

The returned matrix implements the planar Lie bracket in angular-first coordinates: small_adjoint(xi) @ eta == [xi, eta] for twists xi and eta written as [theta, v_x, v_y].

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Planar twist with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` representing `ad_xi`.

coadjoint ¶

coadjoint(xi: Array) -> Array

Return the planar coadjoint matrix used for dual wrench dynamics.

Vectors are angular-first, so dual quantities are ordered as [moment_z, force_x, force_y]. With xi = [theta, v_x, v_y] this function returns the package's planar coadjoint convention: [[0, v_y, -v_x], [0, 0, -theta], [0, theta, 0]]. This matches the existing dynamics convention for planar mass and force terms.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Planar twist with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` acting on angular-first dual vectors.

adjoint ¶

adjoint(g: Array) -> Array

Return the group adjoint matrix Ad_g for an SE(2) transform.

The adjoint maps planar twists between frames according to the homogeneous transform g and the angular-first twist convention. It is constructed so that hat(adjoint(g) @ xi) == g @ hat(xi) @ inverse(g).

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(2)` transform with shape `(3, 3)`. The rotation block is `g[:2, :2]` and the translation is `g[:2, 2]`.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` representing `Ad_g` in
`Array`	`[theta, v_x, v_y]` coordinates.

adjoint_inverse ¶

adjoint_inverse(g: Array) -> Array

Return the inverse group adjoint matrix Ad_g^{-1}.

This is equivalent to adjoint(inverse(g)) but avoids explicitly constructing the inverse homogeneous transform. The result maps twists in the opposite direction from :func:adjoint.

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(2)` transform with shape `(3, 3)`.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` representing the inverse adjoint in
`Array`	angular-first coordinates.

SE(3)¶

soromox.utils.lie_algebra.se3 ¶

hat ¶

hat(xi: Array) -> Array

Return the homogeneous matrix representation of an se(3) twist.

Spatial twists use angular-first coordinates xi = [omega_x, omega_y, omega_z, v_x, v_y, v_z]. The returned matrix is [[so3.skew(omega), v], [0, 0, 0, 0]] and is suitable for use in the matrix exponential.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Spatial twist with shape `(6,)` or `(6, 1)`. The first three entries are the angular component `omega` and the final three entries are the translational component `v`.	required

Returns:

Type	Description
`Array`	Array with shape `(4, 4)` representing the same algebra element in
`Array`	homogeneous matrix form.

log ¶

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

Compute the Lie-group logarithm from SE(3) to se(3).

The returned vector is a spatial twist in angular-first coordinates. The translational component is not the raw transform translation; it is the result of applying the inverse SE(3) left Jacobian to the translation. This makes the function the inverse of :func:exp for regular transforms.

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(3)` transform with shape `(4, 4)`. The rotation is read from `g[:3, :3]` and the translation from `g[:3, 3]`.	required
`eps`	`float \| Array`	Small positive scalar threshold passed to `so3.log` for rotation extraction. The inverse Jacobian uses a conservatively scaled threshold to avoid cancellation in `1 - cos(theta)`.	required

Returns:

Type	Description
`Array`	Array with shape `(6,)` in
`Array`	`[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.

exp ¶

exp(xi: Array, eps: float | Array) -> Array

Compute the Lie-group exponential from se(3) to SE(3).

This is the matrix exponential of :func:hat. The translational component of xi is integrated through the SE(3) left Jacobian, so it is a twist coordinate rather than a direct pose translation. Use poses.quaternion_pose_to_transform for direct quaternion-plus-position pose coordinates.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Spatial twist with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required
`eps`	`float \| Array`	Small positive scalar threshold. If `norm(omega) <= eps`, a truncated Taylor series is used to avoid singular divisions and keep derivatives finite near zero rotation.	required

Returns:

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

small_adjoint ¶

small_adjoint(xi: Array) -> Array

Return the Lie algebra adjoint matrix ad_xi.

The returned matrix implements the spatial Lie bracket in angular-first coordinates: small_adjoint(xi) @ eta == [xi, eta] for twists xi and eta written as [omega, v].

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Spatial twist with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` representing `ad_xi`.

coadjoint ¶

coadjoint(xi: Array) -> Array

Return the spatial coadjoint matrix -ad_xi.T.

Dual vectors are ordered consistently with angular-first twists, i.e. [moment_x, moment_y, moment_z, force_x, force_y, force_z]. The returned matrix acts on those dual vectors and is used in the dynamics terms where spatial inertia and wrench quantities are expressed in the same convention.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Spatial twist with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` representing the coadjoint action on
`Array`	angular-first dual vectors.

adjoint ¶

adjoint(g: Array) -> Array

Return the group adjoint matrix Ad_g for an SE(3) transform.

The adjoint maps spatial twists between frames according to the homogeneous transform g and the angular-first twist convention. It is constructed so that hat(adjoint(g) @ xi) == g @ hat(xi) @ inverse(g).

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(3)` transform with shape `(4, 4)`. The rotation block is `g[:3, :3]` and the translation is `g[:3, 3]`.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` representing `Ad_g` in
`Array`	`[omega, v]` coordinates.

adjoint_inverse ¶

adjoint_inverse(g: Array) -> Array

Return the inverse group adjoint matrix Ad_g^{-1}.

This is equivalent to adjoint(inverse(g)) but avoids explicitly constructing the inverse homogeneous transform. The result maps spatial twists in the opposite direction from :func:adjoint.

Parameters:

Name	Type	Description	Default
`g`	`Array`	Homogeneous `SE(3)` transform with shape `(4, 4)`.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` representing the inverse adjoint in
`Array`	angular-first coordinates.

Constant Strain¶

soromox.utils.lie_algebra.constant_strain ¶

adjoint_se2 ¶

adjoint_se2(xi: Array, s: Array, eps: float | Array) -> Array

Return the adjoint accumulated along a constant planar strain segment.

This is a rod-kinematics helper, not a generic transform adjoint. For a segment with constant planar strain xi evaluated at arclength s, the returned matrix represents the closed-form adjoint associated with the segment transform exp(s * xi) under the angular-first se(2) convention.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant planar strain with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used to select the small-angle series branch when `abs(theta) <= eps`.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` mapping planar twists through the segment
`Array`	adjoint at arclength `s`.

adjoint_inverse_se2 ¶

adjoint_inverse_se2(xi: Array, s: Array, eps: float | Array) -> Array

Return the inverse adjoint for a constant planar strain segment.

This computes the inverse of :func:adjoint_se2 without explicitly inverting the full homogeneous segment transform. It is used when propagating planar body-frame Jacobians backward through a constant-strain segment.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant planar strain with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold for the same small-angle branch used by :func:`adjoint_se2`.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` representing the inverse segment adjoint.

tangent_se2 ¶

tangent_se2(xi: Array, s: Array, eps: float | Array) -> Array

Return the tangent operator for a constant planar strain segment.

The tangent operator integrates the local strain perturbation over arclength for a segment whose strain is constant. In PCS/GVS recurrences it maps strain-basis contributions into body-frame Jacobian contributions at arclength s.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant planar strain with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used for the small-angle Taylor series when `abs(theta) <= eps`.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` containing the planar constant-strain
`Array`	tangent operator at arclength `s`.

tangent_derivative_se2 ¶

tangent_derivative_se2(xi: Array, xid: Array, s: Array, eps: float | Array) -> Array

Return the time derivative of the planar tangent operator.

This differentiates :func:tangent_se2 with respect to time through the strain xi and strain rate xid while holding arclength s fixed. The function uses the same angular-first planar convention as se2.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant planar strain with shape `(3,)` or `(3, 1)` in `[theta, v_x, v_y]` order.	required
`xid`	`Array`	Time derivative of `xi` with the same shape and coordinate order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used for the small-angle series branch.	required

Returns:

Type	Description
`Array`	Array with shape `(3, 3)` containing `d/dt tangent_se2(xi, s)`.

adjoint_se3 ¶

adjoint_se3(xi: Array, s: Array, eps: float | Array) -> Array

Return the adjoint accumulated along a constant spatial strain segment.

This is a rod-kinematics helper for spatial PCS/GVS segments. For constant strain xi evaluated at arclength s, the returned matrix represents the closed-form adjoint associated with the segment transform exp(s * xi) under the angular-first se(3) convention.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant spatial strain with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used to select the small-angle branch when `norm(omega) <= eps`.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` mapping spatial twists through the segment
`Array`	adjoint at arclength `s`.

adjoint_inverse_se3 ¶

adjoint_inverse_se3(xi: Array, s: Array, eps: float | Array) -> Array

Return the inverse adjoint for a constant spatial strain segment.

This computes the inverse of :func:adjoint_se3 without explicitly constructing and inverting the homogeneous segment transform. It is used when propagating body-frame Jacobians backward through spatial constant-strain segments.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant spatial strain with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold for the same small-angle branch used by :func:`adjoint_se3`.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` representing the inverse segment adjoint.

tangent_se3 ¶

tangent_se3(xi: Array, s: Array, eps: float | Array) -> Array

Return the tangent operator for a constant spatial strain segment.

The tangent operator integrates local spatial strain perturbations over arclength for a segment whose strain is constant. In PCS/GVS recurrences it maps strain-basis contributions into body-frame Jacobian contributions at arclength s.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant spatial strain with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used for the small-angle Taylor series when `norm(omega) <= eps`.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` containing the spatial constant-strain
`Array`	tangent operator at arclength `s`.

tangent_derivative_se3 ¶

tangent_derivative_se3(xi: Array, xid: Array, s: Array, eps: float | Array) -> Array

Return the time derivative of the spatial tangent operator.

This differentiates :func:tangent_se3 with respect to time through the strain xi and strain rate xid while holding arclength s fixed. The rotational magnitude derivative is regularized in the zero-rotation branch to keep autodiff finite.

Parameters:

Name	Type	Description	Default
`xi`	`Array`	Constant spatial strain with shape `(6,)` or `(6, 1)` in `[omega_x, omega_y, omega_z, v_x, v_y, v_z]` order.	required
`xid`	`Array`	Time derivative of `xi` with the same shape and coordinate order.	required
`s`	`Array`	Scalar arclength position measured from the base of the segment.	required
`eps`	`float \| Array`	Small positive scalar threshold used for the small-angle series branch.	required

Returns:

Type	Description
`Array`	Array with shape `(6, 6)` containing `d/dt tangent_se3(xi, s)`.