Dual quaternions

Introduction

The dual quaternions are an example of "biquaternions." They can be represented equivalently either as a dual number where both both the "primal" and "tangent" part are quaternions

\[d = q_0 + q_e \epsilon = (s_0 + a_0 i + b_0 j + c_0 k) + (s_e + a_e i + b_e j + c_e k) \epsilon\]

or as a quaternion where the scalar part and three imaginary parts are all dual numbers

\[d = s + ai + bj + ck = (s_0 + s_e \epsilon) + (a_0 + a_e \epsilon) i + (b_0 + b_e \epsilon) j + (c_0 + c_e \epsilon) k.\]

Like unit quaternions can compactly representation rotations in 3D space, dual quaternions can compactly represent rigid transformations (rotation with translation).

Without any special glue code, we can construct a dual quaternion by composing ForwardDiff.Dual and Quaternion; this uses the second representation described above:

Utility functions

First let's load the packages:

using Quaternions, ForwardDiff, Random

Then we'll create some utility types/functions:

const DualQuaternion{T} = Quaternion{ForwardDiff.Dual{Nothing,T,1}}

purequat(p::AbstractVector) = quat(false, @views(p[begin:begin+2])...)

dual(x::Real, v::Real) = ForwardDiff.Dual(x, v)

function dualquat(_q0::Union{Real,Quaternion}, _qe::Union{Real,Quaternion})
    q0 = quat(_q0)
    qe = quat(_qe)
    Quaternion(
        dual(real(q0), real(qe)),
        dual.(imag_part(q0), imag_part(qe))...,
    )
end

function primal(d::DualQuaternion)
    return Quaternion(
        ForwardDiff.value(real(d)),
        ForwardDiff.value.(imag_part(d))...,
    )
end

function tangent(d::DualQuaternion)
    return Quaternion(
        ForwardDiff.partials(real(d), 1),
        ForwardDiff.partials.(imag_part(d), 1)...,
    )
end

function dualconj(d::DualQuaternion)
    de = tangent(d)
    return dualquat(conj(primal(d)), quat(-real(de), imag_part(de)...))
end

rotation_part(d::DualQuaternion) = primal(d)

translation_part(d::DualQuaternion) = dualquat(true, conj(rotation_part(d)) * tangent(d))

# first=true returns the translation performed before the rotation: R(p+t)
# first=false returns the translation performed after the rotation: R(p)+t
function translation(d::DualQuaternion; first::Bool=true)
    v = first ? primal(d)' * tangent(d) : tangent(d) * primal(d)'
    return collect(2 .* imag_part(v))
end

function transform(d::DualQuaternion, p::AbstractVector)
    dp = dualquat(true, purequat(p))
    dpnew = d * dp * dualconj(d)
    pnew_parts = imag_part(tangent(dpnew))
    pnew = similar(p, eltype(pnew_parts))
    pnew .= pnew_parts
    return pnew
end

function rotmatrix_from_quat(q::Quaternion)
    sx, sy, sz = 2q.s * q.v1, 2q.s * q.v2, 2q.s * q.v3
    xx, xy, xz = 2q.v1^2, 2q.v1 * q.v2, 2q.v1 * q.v3
    yy, yz, zz = 2q.v2^2, 2q.v2 * q.v3, 2q.v3^2
    r = [1 - (yy + zz)     xy - sz     xz + sy;
            xy + sz   1 - (xx + zz)    yz - sx;
            xz - sy      yz + sx  1 - (xx + yy)]
    return r
end

function transformationmatrix(d::DualQuaternion)
    R = rotmatrix_from_quat(rotation_part(d))
    t = translation(d; first=false)
    T = similar(R, 4, 4)
    T[1:3, 1:3] .= R
    T[1:3, 4] .= t
    T[4, 1:3] .= 0
    T[4, 4] = 1
    return T
end

randdualquat(rng::AbstractRNG,T=Float64) = dualquat(rand(rng, Quaternion{T}), rand(rng, Quaternion{T}))
randdualquat(T=Float64) = randdualquat(Random.GLOBAL_RNG,T)

Example: transforming a point

Now we'll create a unit dual quaternion.

julia> x = sign(randdualquat())Quaternion{ForwardDiff.Dual{Nothing, Float64, 1}}(Dual{Nothing}(0.4454363371369335,0.2056669329647805), Dual{Nothing}(0.6577102070830146,0.1573916285918871), Dual{Nothing}(0.26097252993898523,-0.07883925020629244), Dual{Nothing}(0.5485408751169738,-0.31821644522346726))

sign(q) == q / abs(q) both normalizes the primal part of the dual quaternion and makes the tangent part perpendicular to it.

julia> abs(primal(x)) ≈ 1true
julia> isapprox(real(primal(x)' * tangent(x)), 0; atol=1e-10)true

Here's how we use dual quaternions to transform a point:

julia> p = randn(3)3-element Vector{Float64}:
 -0.8087918010269345
 -0.9643674222157581
 -0.22610640560965936

julia> transform(x, p)3-element Vector{Float64}:
 -0.49732553454711226
  0.2588562973446884
 -1.9313871599945913

Example: homomorphism from unit dual quaternions to the transformation matrices

Each unit dual quaternion can be mapped to an affine transformation matrix $T$. $T$ can be used to transform a vector $p$ like this:

\[T \begin{pmatrix} p \\ 1\end{pmatrix} = \begin{pmatrix} R & t \\ 0^\mathrm{T} & 1\end{pmatrix} \begin{pmatrix} p \\ 1\end{pmatrix} = \begin{pmatrix} Rp + t \\ 1\end{pmatrix},\]

where $R$ is a rotation matrix, and $t$ is a translation vector. Our helper function transformationmatrix maps from a unit dual quaternion to such an affine matrix.

julia> y = sign(randdualquat())Quaternion{ForwardDiff.Dual{Nothing, Float64, 1}}(Dual{Nothing}(0.8480030838934948,0.09587609764106952), Dual{Nothing}(0.04515789272924243,0.5964620644952602), Dual{Nothing}(0.5273604831399743,-0.22394026201935663), Dual{Nothing}(0.027248032107833945,0.3618260701236408))

julia> X = transformationmatrix(x)4×4 Matrix{Float64}:
 0.261992  -0.145391   0.954055    -0.209921
 0.831969  -0.46696   -0.299628     0.413677
 0.489069   0.872244  -0.00137876  -0.69498
 0.0        0.0        0.0          1.0
julia> Y = transformationmatrix(y)4×4 Matrix{Float64}:
  0.442297   0.00141615   0.896868   1.39677
  0.0938418  0.994437    -0.047849  -0.4811
 -0.891946   0.105327     0.439703  -0.040892
  0.0        0.0          0.0        1.0
julia> XY = transformationmatrix(x*y)4×4 Matrix{Float64}:
 -0.748731  -0.0437237  0.661431   0.186958
  0.591409  -0.494742   0.636762   1.81266
  0.299396   0.867939   0.396287  -0.431443
  0.0        0.0        0.0        1.0
julia> X*Y ≈ XYtrue

We can check that our transformation using the unit dual quaternion gives the same result as transforming with an affine transformation matrix:

julia> transform(x, p) ≈ (X * vcat(p, 1))[1:3]true

Example: motion planning

For unit quaternions, spherical linear interpolation with slerp can be used to interpolate between two rotations with unit quaternions, which can be used to plan motion between two orientations. Similarly, we can interpolate between unit dual quaternions to plan motion between two rigid poses. Conveniently, we can do this using the exact same slerp implementation.

julia> slerp(x, y, 0) ≈ xtrue

julia> slerp(x, y, 1) ≈ ytrue

julia> slerp(x, y, 0.3)Quaternion{ForwardDiff.Dual{Nothing, Float64, 1}}(Dual{Nothing}(0.6350753755076702,0.05673219963056468), Dual{Nothing}(0.5170637329346645,0.24512514140084532), Dual{Nothing}(0.3827195392212891,-0.21921909360820657), Dual{Nothing}(0.42760977281360874,-0.1844558243357899))