8. 3D Rotations¶

Quaternions provide a convention for storing and composing rotations in three dimensions. wrenfold provides the symbolic Quaternion type to represent 3D rotations.

Quaternions can be constructed from:

Rotation vectors using from_rotation_vector().

Angle-axis representation using from_angle_axis().

Rotation matrices (members of \(SO(3)\)) using from_rotation_matrix().

Angles about the principal axes using from_x_angle(), from_y_angle(), and from_z_angle().

Quaternions can be converted to:

Rotation matrices using to_rotation_matrix().

Angle-axis representation using to_angle_axis().

Rotation vectors using to_rotation_vector().

Internally, wrenfold quaternions are stored in [w, x, y, z] (scalar-first) order. Conversion to scalar-last representation can be effectuated using to_vector_xyzw() and from_xyzw().

Warning

When converting rotation representations, some care is required to handle singularities. Many of the methods listed above accept an epsilon parameter that governs the behavior near angle=0. When epsilon is specified, a conditional statement and small-angle approximation are inserted to safely handle cases where \(\lvert\theta\rvert < \epsilon\).

8.1. Tangent-space Jacobians¶

Jacobians for rotation groups are typically computed with respect to a tangent-space perturbation. One of the conceptually simplest ways to calculate the derivative is to first compute the Jacobian with respect to the quaternion elements \(\mathbf{q} = \left[q_w, q_x, q_y, q_z\right]\), and then chain rule this with the Jacobian of the tangent-space perturbation itself:

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

Where \(\text{exp}\left(\mathbf{v}\right)\) maps from a rotation vector into a unit quaternion. The term \(\mathbf{q} \oplus \mathbf{\delta v} = \mathbf{q}\cdot\text{exp}\left(\mathbf{\delta v}\right)\) is sometimes referred to as the retraction operation [1] [2] - it maps the perturbation \(\mathbf{\delta v}\) onto the group of quaternions about \(\mathbf{q}\).

The Jacobian \(\mathbf{J}_r = \frac{\partial\mathbf{q}\cdot\text{exp}\left(\mathbf{\delta v}\right)}{\partial\mathbf{\delta v}}\) (evaluated about \(\mathbf{\delta v} = 0\)) is available in wrenfold using right_retract_derivative().

Alternatively, one can also replace the retraction operation with an additive first-order Taylor series approximation. The series is substituted into the original function \(\mathbf{f}\left(\mathbf{q}\right)\) and then evaluated about zero: [3]

\[\mathbf{J} = \frac{\partial \mathbf{f}\left(\mathbf{q} + \mathbf{J}_r\left(\mathbf{q}\right)\mathbf{\delta v}\right)} {\partial \mathbf{\delta v}}\biggr\rvert_{\mathbf{\delta v} = 0}\]

Where \(\mathbf{J}_r\) is the right retraction Jacobian. Because we are evaluating about \(\mathbf{\delta v} = 0\), the two methods yield equivalent results. However, the second method can sometimes produce lower operation counts when evaluated symbolically.

The following snippet illustrates both methods:

from wrenfold import sym
from wrenfold.geometry import Quaternion

q = Quaternion.with_name("q")
p = sym.vector(*sym.symbols("p_x, p_y, p_z"))

# Rotate `p` by the Quaternion `q`:
p_rot = q.to_rotation_matrix() * p

# Compute the jacobian wrt `q` (method 1):
J1 = sym.jacobian(p_rot, q.to_vector_wxyz()) * q.right_retract_derivative()

# Compute the jacobian wrt `q` (method 2):
dv = sym.vector(*sym.symbols("v_x, v_y, v_z"))
p_rot = (
    Quaternion.from_wxyz(
        q.to_vector_wxyz() + q.right_retract_derivative() * dv
    ).to_rotation_matrix()
    * p
)
J2 = sym.subs(sym.jacobian(p_rot, dv), [(x, 0) for x in dv])

print(J1.distribute() - J2.distribute())  # prints: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]

8.1.1. Local-coordinates Jacobian¶

In the previous example it is assumed that the function \(\mathbf{f}\) returned a vector-space that we could directly differentiate. What if it returns a quaternion?

\[\mathbf{q}^\prime = \mathbf{f}\left(\mathbf{q}\right)\]

In this instance, we may want the 3x3 Jacobian mapping perturbations from the tangent-space of \(\mathbf{q}\) to the tangent-space of \(\mathbf{q}^\prime\). This Jacobian can be expressed as:

\[\mathbf{J} = \frac{\partial \text{log}\left(\bar{\mathbf{q}^\prime} \cdot \mathbf{f}\left(\mathbf{q} \oplus \mathbf{\delta v}\right) \right)}{\partial \mathbf{\delta v}}\biggr\rvert_{\mathbf{\delta v} = 0}\]

Where \(\text{log}\left(\mathbf{x}\right)\) converts the quaternion \(\mathbf{x}\) to a rotation vector (the inverse of \(\text{exp}\), as we defined it above), and \(\bar{\mathbf{q}^\prime}\) is the conjugate of \(\mathbf{q}^\prime\).

By applying the chain rule, we can rewrite this Jacobian as:

\[\mathbf{J} = \frac{\partial \text{log}\left(\bar{\mathbf{q}^\prime} \cdot \left(\mathbf{q}^\prime + \mathbf{\delta q}^\prime\right) \right)}{\partial \mathbf{\delta q}^\prime}\biggr\rvert_{\mathbf{\delta q}^\prime = 0} \frac{\partial \mathbf{f}\left(\mathbf{q} \oplus \mathbf{\delta v}\right)}{\partial \mathbf{\delta v}}\biggr\rvert_{\mathbf{\delta v} = 0}\]

wrenfold refers to the first term in \(\mathbf{J}\) as the local-coordinates Jacobian. It has dimensions (3, 4) and can be obtained by invoking right_local_coordinates_derivative().

Tip

Refer to the quaternion_interpolation example to see the above ideas implemented in practice.