Lie Algebra¶

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

Return the left Jacobian of the :math:SE(2) exponential.

For an accumulated planar twist xi, this evaluates

.. math::

J_l(\xi) = \int_0^1
\exp(\sigma\,\operatorname{ad}_\xi)\,d\sigma.

The operation is intrinsic to SE(2) and does not assume that xi came from a constant-strain rod segment.

The angular-first convention makes the first coordinate the accumulated rotation and the remaining coordinates the accumulated translation. The result is a coordinate Jacobian in the same basis as xi; it is not an arclength-scaled constant-strain tangent operator.

Parameters:

Returns:

Return D J_l(xi)[xid] for the :math:SE(2) left Jacobian.

xid is an arbitrary direction in the Lie algebra; it need not be a physical time derivative or a constant-strain rate.

This is the Fréchet derivative of the full matrix-valued function left_jacobian along xid. Both the rotational and translational components of xid contribute to the result.

Parameters:

Returns:

Evaluate J_l(xi) and D J_l(xi)[xid] with shared work.

This bundle is numerically equivalent to calling :func:left_jacobian and :func:left_jacobian_directional_derivative separately, but reuses the scalar coefficients needed by both evaluations. Use it when both the Jacobian and its directional derivative are required at the same point.

Parameters:

Returns:

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:

Returns:

Apply the planar coadjoint without materializing its matrix.

Parameters:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

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:

Returns:

Apply the Lie algebra adjoint without materializing its matrix.

Parameters:

Returns:

Return the left Jacobian of the :math:SE(3) exponential.

For an accumulated spatial twist xi, this evaluates

.. math::

J_l(\xi) = \int_0^1 \exp(\sigma\,\operatorname{ad}_\xi)\,d\sigma.

The operation is intrinsic to SE(3) and does not assume that xi came from a constant-strain rod segment.

The angular-first convention makes the first three coordinates the accumulated rotation and the final three coordinates the accumulated translation. The result is a coordinate Jacobian in the same basis as xi; it is not an arclength-scaled constant-strain tangent operator.

Parameters:

Returns:

Return D J_l(xi)[xid] for the :math:SE(3) left Jacobian.

xid is an arbitrary direction in the Lie algebra; it need not be a physical time derivative or a constant-strain rate.

This is the Fréchet derivative of the full matrix-valued function left_jacobian along xid. Both the rotational and translational components of xid contribute through the spatial Lie-algebra polynomial.

Parameters:

Returns:

Evaluate J_l(xi) and D J_l(xi)[xid] with shared work.

This bundle is numerically equivalent to calling :func:left_jacobian and :func:left_jacobian_directional_derivative separately, but reuses the Lie-algebra powers and scalar coefficients needed by both evaluations. Use it when both the Jacobian and its directional derivative are required at the same point.

Parameters:

Returns:

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:

Returns:

Apply the spatial coadjoint without materializing its matrix.

Parameters:

Returns:

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:

Returns:

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:

Returns:

Return the dtype-aware cutoff for forward left-Jacobian coefficients.

For an input dtype with machine epsilon u, the cutoff is u**(1/12). The subtraction-based forward closed forms have Hessian roundoff of order O(u / theta**4) near zero. The series retained in this module have Hessian truncation error O(theta**8); balancing the two errors gives theta**12 ~= u.

Parameters:

Returns:

Return the dtype-aware cutoff for inverse left-Jacobian coefficients.

For an input dtype with machine epsilon u, the cutoff is u**(1/8). It balances the O(u / theta**2) roundoff amplification in the closed-form Hessian of (theta / 2) cot(theta / 2) against the O(theta**6) Hessian truncation error of the retained inverse series.

Parameters:

Returns:

Evaluate the analytic sinc coefficient with stable derivatives.

The coefficient is

.. math::

\operatorname{sinc}(\theta) = \frac{\sin\theta}{\theta}.

Near zero it is evaluated with the even Taylor polynomial

.. math::

1 - \frac{\theta^2}{3!} + \frac{\theta^4}{5!}
- \frac{\theta^6}{7!} + \frac{\theta^8}{9!}.

Parameters:

Returns:

Evaluate the analytic cosc coefficient with stable derivatives.

The coefficient is

.. math::

\operatorname{cosc}(\theta)
= \frac{1 - \cos\theta}{\theta^2},

with Taylor polynomial

.. math::

\frac{1}{2!} - \frac{\theta^2}{4!}
+ \frac{\theta^4}{6!} - \frac{\theta^6}{8!}
+ \frac{\theta^8}{10!}

in the small-angle region. This coefficient multiplies the squared skew matrix in Rodrigues' formula and the first skew term of the SE(3) left Jacobian.

Parameters:

Returns:

Evaluate the cubic forward-Jacobian coefficient stably.

The coefficient is

.. math::

\operatorname{tanc}(\theta)
= \frac{\theta - \sin\theta}{\theta^3},

with Taylor polynomial

.. math::

\frac{1}{3!} - \frac{\theta^2}{5!}
+ \frac{\theta^4}{7!} - \frac{\theta^6}{9!}
+ \frac{\theta^8}{11!}.

It is used as the quadratic-skew coefficient of the SE(3) forward left Jacobian.

Parameters:

Returns:

Evaluate the inverse-Jacobian half-angle coefficient stably.

The analytic coefficient

.. math::

h(\theta) = \frac{\theta}{2}\cot\!\left(\frac{\theta}{2}\right)

is evaluated near zero as

.. math::

1 - \frac{\theta^2}{12} - \frac{\theta^4}{720}
- \frac{\theta^6}{30240}.

Parameters:

Returns:

Evaluate the quadratic coefficient of the inverse SE(3) Jacobian.

For Omega = skew(omega) and theta = norm(omega), the inverse left Jacobian is

.. math::

J_l^{-1}(\omega) = I - \frac{1}{2}\Omega + q(\theta)\Omega^2,

where

.. math::

q(\theta) =
\frac{1 - (\theta/2)\cot(\theta/2)}{\theta^2}.

The small-angle branch uses

.. math::

\frac{1}{12} + \frac{\theta^2}{720}
+ \frac{\theta^4}{30240} + \frac{\theta^6}{1209600}.

Parameters:

Returns: