geometry

wrenfold.geometry.inverse_left_jacobian_of_so3(w: wrenfold.sym.MatrixExpr, epsilon: wrenfold.sym.Expr | None) → wrenfold.sym.MatrixExpr

Compute the inverse of the left jacobian of SO(3). Given a rotation vector w, this method computes the 3x3 derivative:

\[\mathbf{J}_l^{-1} = \frac{ \partial \left(\text{log}\left( \text{exp}\left(\delta\mathbf{x}\right) \cdot \text{exp}\left(\mathbf{w}\right)\right) - \mathbf{w}\right) } {\partial \delta\mathbf{x}} \biggr\rvert_{\delta\mathbf{x} = 0}\]

Where exp(...) maps from a rotation vector to a quaternion, and log(...) maps from a quaternion to a rotation vector.

Parameters:

• w – 3x1 rotation vector, in radians.

• epsilon – If provided, epsilon specifies the threshold below which a small-angle approximation is used. A conditional will be inserted using wrenfold.sym.where() to switch between the normal and small-angle code paths. This value should be positive.

Returns:

The 3x3 inverse left jacobian of SO(3).

References

• A micro Lie theory for state estimation in robotics

• Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems

wrenfold.geometry.left_jacobian_of_so3(w: wrenfold.sym.MatrixExpr, epsilon: wrenfold.sym.Expr | None) → wrenfold.sym.MatrixExpr

Compute the left jacobian of SO(3). Given a rotation vector w, this method computes the 3x3 derivative:

\[\mathbf{J}_l = \frac{ \partial \text{log}\left( \text{exp}\left(\mathbf{w} + \delta\mathbf{w}\right) \cdot \text{exp}\left(\mathbf{w}\right)^T\right) } {\partial \delta\mathbf{w}} \biggr\rvert_{\delta\mathbf{w} = 0}\]

Where exp(...) maps from a rotation vector to a quaternion, and log(...) maps from a quaternion to a rotation vector.

This matrix has a secondary interpretation, as the integral:

\[\mathbf{J}_l = \int_{0}^{1} \text{exp}\left(\alpha \cdot \mathbf{w}\right) \,d\alpha\]

Tip

The right jacobian can be obtained by transposing the left jacobian.

Parameters:

• w – 3x1 rotation vector, in radians.

• epsilon – If provided, epsilon specifies the threshold below which a small-angle approximation is used. A conditional will be inserted using wrenfold.sym.where() to switch between the normal and small-angle code paths. This value should be positive.

Returns:

The 3x3 left jacobian of SO(3).

References

• A micro Lie theory for state estimation in robotics

• Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems

class wrenfold.geometry.Quaternion

A quaternion class used to represent 3D rotations and orientations.

__eq__(self: wrenfold.geometry.Quaternion, other: wrenfold.geometry.Quaternion) → bool

Check for strict equality. This is not the same as mathematical equivalence.

__hash__(self: wrenfold.geometry.Quaternion) → int

Compute hash.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: wrenfold.geometry.Quaternion, w: wrenfold.sym.Expr, x: wrenfold.sym.Expr, y: wrenfold.sym.Expr, z: wrenfold.sym.Expr) -> None

Construct a quaternion from scalar expressions in [w,x,y,z] order.

Parameters:

• w – The scalar component of the quaternion.

• x – Vector element multiplied by basis vector \(\mathbf{i}\).

• y – Vector element multiplied by basis vector \(\mathbf{j}\).

• z – Vector element multiplied by basis vector \(\mathbf{k}\).

Caution

Quaternion will not normalize after construction. It is expected that the user will pass normalized components, or invoke wrenfold.sym.Quaternion.normalized().

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> q = geometry.Quaternion(w, x, y, z)
>>> q
Quaternion(w, x, y, z)
>>> q.x
x

2. __init__(self: wrenfold.geometry.Quaternion) -> None

Construct the identity quaternion.

Examples

>>> geometry.Quaternion()
Quaternion(1, 0, 0, 0)

__mul__(self: wrenfold.geometry.Quaternion, arg0: wrenfold.geometry.Quaternion) → wrenfold.geometry.Quaternion

conjugate(self: wrenfold.geometry.Quaternion) → wrenfold.geometry.Quaternion

Compute the quaternion conjugate. Given the quaternion:

\[\mathbf{q} = q_w + q_x\cdot\mathbf{i} + q_y\cdot\mathbf{j} + q_z\cdot\mathbf{k}\]

The conjugate is defined as:

\[\bar{\mathbf{q}} = q_w - q_x\cdot\mathbf{i} - q_y\cdot\mathbf{j} - q_z\cdot\mathbf{k}\]

If \(\mathbf{q}\) is normalized, then \(\mathbf{q}\cdot\bar{\mathbf{q}}\) is the identity element.

Returns:

Conjugated quaternion, where the signs of the [x, y, z] components have been flipped.

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> q = geometry.Quaternion(w, x, y, z)
>>> q * q.conjugate()
Quaternion(w**2 + x**2 + y**2 + z**2, 0, 0, 0)
>>> q = geometry.Quaternion(0, 1, 1, 0).normalized()
>>> q.conjugate() * q
Quaternion(1, 0, 0, 0)

eval(self: wrenfold.geometry.Quaternion) → numpy.ndarray

Invoke wrenfold.sym.Expr.eval() on every element of the quaternion, and return a numpy array.

Returns:

4-element numpy array in [w, x, y, z] (scalar-first) order.

Examples

>>> geometry.Quaternion().eval()
array([1., 0., 0., 0.])

static from_angle_axis(*args, **kwargs)

Overloaded function.

1. from_angle_axis(angle: wrenfold.sym.Expr, vx: wrenfold.sym.Expr, vy: wrenfold.sym.Expr, vz: wrenfold.sym.Expr) -> wrenfold.geometry.Quaternion

Construct quaternion from an angle and normalized axis. This function constructs the quaternion:

\[\mathbf{q} = \cos{\frac{\theta}{2}} + v_x \cdot \sin{\frac{\theta}{2}} \cdot \mathbf{i} + v_y \cdot \sin{\frac{\theta}{2}} \cdot \mathbf{j} + v_z \cdot \sin{\frac{\theta}{2}} \cdot \mathbf{k}\]

Caution

The resulting quaternion only represents a rotation if the axis [vx, vy, vz] is normalized.

Parameters:

• angle – Rotation angle in radians.

• vx – X component of the rotation axis.

• vy – Y component of the rotation axis.

• vz – Z component of the rotation axis.

Returns:

A unit-norm quaternion representing the rotation about vector [vx, vy, vz] by angle radians.

Examples

>>> angle, x, y, z = sym.symbols('angle, x, y, z')
>>> geometry.Quaternion.from_angle_axis(angle, x, y, z)
Quaternion(cos(angle/2), x*sin(angle/2), y*sin(angle/2), z*sin(angle/2))

2. from_angle_axis(angle: wrenfold.sym.Expr, axis: wrenfold.sym.MatrixExpr) -> wrenfold.geometry.Quaternion

Overload of from_angle_axis that accepts sym.MatrixExpr for the axis.

static from_rotation_matrix(R: wrenfold.sym.MatrixExpr) → wrenfold.geometry.Quaternion

Construct a quaternion from a rotation matrix. This function uses Sheppard’s method, as documented in section 3.3 of A Survey on the Computation of Quaternions from Rotation Matrices, Sarabandi and Thomas.

Caution

This operation is only valid if R is a valid member of SO(3).

Parameters:

R – 3x3 rotation matrix.

Returns:

A unit-norm quaternion corresponding to the rotation matrix.

static from_rotation_vector(*args, **kwargs)

Overloaded function.

1. from_rotation_vector(x: wrenfold.sym.Expr, y: wrenfold.sym.Expr, z: wrenfold.sym.Expr, epsilon: Optional[wrenfold.sym.Expr]) -> wrenfold.geometry.Quaternion

Construct quaternion from a rotation vector. A rotation vector (sometimes referred to as “Rodrigues parameters”) is parallel to the axis of rotation, and has a norm equal to the angle of rotation in radians.

Caution

This method encounters a singularity when the norm of [x, y, z] is zero. Refer to the note for argument epsilon on how this is handled.

Parameters:

• x – X component of the vector, in radians.

• y – Y component of the vector, in radians.

• z – Z component of the vector, in radians.

• epsilon – If provided, epsilon specifies the threshold below which a small-angle approximation is used. A conditional will be inserted using wrenfold.sym.where() to switch between the normal and small-angle code paths. This value should be positive.

Returns:

A unit-norm quaternion representing the rotation vector [x, y, z].

Examples

>>> geometry.Quaternion.from_rotation_vector(sym.pi, 0, 0, epsilon=None)
Quaternion(0, 1, 0, 0)

2. from_rotation_vector(v: wrenfold.sym.MatrixExpr, epsilon: Optional[wrenfold.sym.Expr]) -> wrenfold.geometry.Quaternion

Overload of from_rotation_vector that accepts sym.MatrixExpr.

static from_wxyz(*args, **kwargs)

Overloaded function.

1. from_wxyz(wxyz: wrenfold.sym.MatrixExpr) -> wrenfold.geometry.Quaternion

Create a quaternion from a 4-element column vector in order [w, x, y, z] (scalar first).

Parameters:

wxyz – 4x1 sym.MatrixExpr

Returns:

Quaternion instance.

2. from_wxyz(wxyz: Iterable) -> wrenfold.geometry.Quaternion

Overload of wrenfold.geometry.Quaternion.from_wxyz() that accepts Iterable[sym.Expr].

static from_x_angle(angle: wrenfold.sym.Expr) → wrenfold.geometry.Quaternion

Construct quaternion that rotates about the x-axis by the specified angle.

Parameters:

angle – Angle in radians.

Returns:

A unit-norm quaternion representing a rotation about the x-axis.

static from_xyzw(*args, **kwargs)

Overloaded function.

1. from_xyzw(xyzw: wrenfold.sym.MatrixExpr) -> wrenfold.geometry.Quaternion

Create a quaternion from a 4-element column vector in order [x, y, z, w] (scalar last).

Parameters:

xyzw – 4x1 sym.MatrixExpr

Returns:

Quaternion instance.

2. from_xyzw(xyzw: Iterable) -> wrenfold.geometry.Quaternion

Overload of wrenfold.geometry.Quaternion.from_xyzw() that accepts Iterable[sym.Expr].

static from_y_angle(angle: wrenfold.sym.Expr) → wrenfold.geometry.Quaternion

Construct quaternion that rotates about the y-axis by the specified angle.

Parameters:

angle – Angle in radians.

Returns:

A unit-norm quaternion representing a rotation about the y-axis.

static from_z_angle(angle: wrenfold.sym.Expr) → wrenfold.geometry.Quaternion

Construct quaternion that rotates about the z-axis by the specified angle.

Parameters:

angle – Angle in radians.

Returns:

A unit-norm quaternion representing a rotation about the z-axis.

inverse(self: wrenfold.geometry.Quaternion) → wrenfold.geometry.Quaternion

Compute the quaternion inverse. Given the quaternion:

\[\mathbf{q} = q_w + q_x\cdot\mathbf{i} + q_y\cdot\mathbf{j} + q_z\cdot\mathbf{k}\]

The inverse is defined as:

\[\mathbf{q}^{-1} = \frac{q_w}{\lvert q \rvert^2} - \frac{q_x}{\lvert q \rvert^2}\cdot\mathbf{i} - \frac{q_y}{\lvert q \rvert^2}\cdot\mathbf{j} - \frac{q_z}{\lvert q \rvert^2}\cdot\mathbf{k}\]

And \(\mathbf{q}\cdot\mathbf{q}^{-1}\) is the identity element.

Returns:

Inverted quaternion, where the signs of the [x, y, z] components have been flipped and the all components are divided by the squared norm.

Examples

>>> q = geometry.Quaternion(4, 2, 1, -3)
>>> q * q.inverse()
Quaternion(1, 0, 0, 0)

is_identical_to(*args, **kwargs)

Overloaded function.

1. is_identical_to(self: wrenfold.geometry.Quaternion, other: wrenfold.geometry.Quaternion) -> bool

Check for strict equality. This is not the same as mathematical equivalence.

2. is_identical_to(self: wrenfold.geometry.Quaternion, other: wrenfold.geometry.Quaternion) -> bool

Test if two quaternions have identical expression trees.

norm(self: wrenfold.geometry.Quaternion) → wrenfold.sym.Expr

The L2 norm of self.

Returns:

Scalar-valued expression sqrt(w**2 + x**2 + y**2 + z**2).

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> geometry.Quaternion(w, x, y, z).norm()
(w**2 + x**2 + y**2 + z**2)**(1/2)

normalized(self: wrenfold.geometry.Quaternion) → wrenfold.geometry.Quaternion

Create a new quaternion by dividing every element of self by self.norm().

Returns:

Quaternion with unit norm.

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> q = geometry.Quaternion(w, x, y, z)
>>> norm = q.norm()
>>> q.normalized().subs(norm, sym.symbols('q_n'))
Quaternion(w/q_n, x/q_n, y/q_n, z/q_n)
>>> geometry.Quaternion(0, 1, 1, 0).normalized()
Quaternion(0, 2**(1/2)/2, 2**(1/2)/2, 0)

right_local_coordinates_derivative(self: wrenfold.geometry.Quaternion) → wrenfold.sym.MatrixExpr

Compute the 3x4 derivative:

\[\frac{\partial \text{log}\left(\bar{\mathbf{q}} \cdot \left(\mathbf{q} + \delta\mathbf{q}\right) \right)}{\partial \delta\mathbf{q}}\biggr\rvert_{\delta\mathbf{q} = 0}\]

Where log(...) converts a quaternion to a rotation vector, and dq is an additive perturbation to self. The derivative of the rotation vector is taken with respect to the four quaternion elements (ordered [w, x, y, z]).

Returns:

3x4 jacobian of d(log(self^T * (self + dq)) / dq evaluated at dq = 0.

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> geometry.Quaternion(w, x, y, z).right_retract_derivative()
[[-2*x, 2*w, 2*z, -2*y], [-2*y, -2*z, 2*w, 2*x], [-2*z, 2*y, -2*x, 2*w]]

right_retract_derivative(self: wrenfold.geometry.Quaternion) → wrenfold.sym.MatrixExpr

Compute the 4x3 derivative of this quaternion with respect to a right-multiplied tangent-space perturbation. This is the derivative:

\[\frac{\partial \left[\mathbf{q} \cdot \text{exp}\left(\mathbf{v}\right)\right]} {\partial \mathbf{v}} \biggr\rvert_{\mathbf{v} = 0}\]

Where exp(...) maps from a rotation vector to a quaternion. The derivatives of the four quaternion elements (ordered [w, x, y, z]) are computed with respect to the three elements of v, linearized about v = 0.

Returns:

4x3 jacobian d(self * exp(v)) / dv evaluated at v = 0.

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> geometry.Quaternion(w, x, y, z).right_retract_derivative()
[[-x/2, -y/2, -z/2], [w/2, -z/2, y/2], [z/2, w/2, -x/2], [-y/2, x/2, w/2]]

squared_norm(self: wrenfold.geometry.Quaternion) → wrenfold.sym.Expr

The sum of squared elements of self, or the squared L2 norm.

Returns:

Scalar-valued expression w**2 + x**2 + y**2 + z**2.

Examples

>>> w, x, y, z = sym.symbols('w, x, y, z')
>>> geometry.Quaternion(w, x, y, z).squared_norm()
w**2 + x**2 + y**2 + z**2

subs(self: wrenfold.geometry.Quaternion, target: wrenfold.sym.Expr, replacement: wrenfold.sym.Expr) → wrenfold.geometry.Quaternion

Invoke wrenfold.sym.Expr.subs() on every element of the quaternion.

to_angle_axis(self: wrenfold.geometry.Quaternion, epsilon: wrenfold.sym.Expr | None = 0) → Tuple[wrenfold.sym.Expr, wrenfold.sym.MatrixExpr]

Recover a rotation angle and axis from a unit-norm quaternion. The method used is documented in Quaternion Computation, Neil Dantum, equation 19.

If the quaternion is near identity, recovering the normalized axis of rotation entails a division by zero. If the norm of the vector component of the quaternion falls below epsilon, the division is skipped and a constant value of [0, 0, 0] is used instead.

Caution

This operation is only valid if self is normalized.

Parameters:

epsilon – If specified, a conditional branch is inserted to handle the singularity for quaternions near identity. This value should be positive.

Returns:

Angle in radians, converted into range [0, pi]. wrenfold.sym.MatrixExpr: Normalized axis of rotation.

Return type:

wrenfold.sym.Expr

to_list(self: wrenfold.geometry.Quaternion) → list

Convert to list in [w, x, y, z] (scalar first) order.

to_rotation_matrix(self: wrenfold.geometry.Quaternion) → wrenfold.sym.MatrixExpr

Convert quaternion to an element of SO(3), the group of rotation matrices.

Caution

The result of this operation is only well defined if self is a normalized quaternion.

Returns:

A 3x3 rotation matrix.

Return type:

wrenfold.sym.MatrixExpr

Examples

>>> geometry.Quaternion().to_rotation_matrix()
[[1, 0, 0], [0, 1, 0], [0, 0, 1]]
>>> geometry.Quaternion(0, 1, 1, 0).normalized().to_rotation_matrix()
[[0, 1, 0], [1, 0, 0], [0, 0, -1]]

References

• Quaternions and rotations.

to_rotation_vector(self: wrenfold.geometry.Quaternion, epsilon: wrenfold.sym.Expr | None = 0, use_atan2: bool = True) → wrenfold.sym.MatrixExpr

Recover a rotation vector from a unit-norm quaternion. The following formula is used:

\[\mathbf{v} = \frac{2}{\lvert \mathbf{q}_v \rvert} \cdot \text{atan2}\left(\lvert \mathbf{q}_v \rvert, \text{abs}\left(q_w\right)\right) \cdot \text{sign}\left(q_w\right) \cdot \mathbf{q}_v\]

Where \(\mathbf{q}_v\) is the vector component of the quaternion.

Caution

This operation is only valid if self is normalized.

Parameters:

• epsilon – If provided, epsilon specifies the threshold below which a small-angle approximation is used. A conditional will be inserted using wrenfold.sym.where() to switch between the normal and small-angle code paths. This value should be positive.

• use_atan2 – Defaults to true. If false, an implementation that uses arccos is used instead. This may produce faster code.

Returns:

3x1 matrix expression for the rotation vector.

to_vector_wxyz(self: wrenfold.geometry.Quaternion) → wrenfold.sym.MatrixExpr

Convert to a 4x1 vector in [w, x, y, z] (scalar-first) order.

to_vector_xyzw(self: wrenfold.geometry.Quaternion) → wrenfold.sym.MatrixExpr

Convert to a 4x1 vector in [x, y, z, w] (scalar-last) order.

property w

Access scalar element of quaternion.

static with_name(name: str) → wrenfold.geometry.Quaternion

Create a quaternion filled with variable expressions, prefixed with the provided name.

Parameters:

name – Name prefix to prepend to the quaternion elements.

Returns:

Quaternion instance.

Examples

>>> geometry.Quaternion.with_name('foo')
Quaternion(foo_w, foo_x, foo_y, foo_z)

property x

Access i coefficient of quaternion.

property y

Access j coefficient of quaternion.

property z

Access k coefficient of quaternion.