Quaternion constraints

This post is mainly a distillation of the information given by Claude Lacoursière in his thesis and in a chapter from the Game Physics Pearls book. It is mostly the result of trying to implement these quaternion based constraints into a game physics engine. I will limit myself to only two examples that seemed most practical and are also part of the understanding process: the totally locked rotational constraint and the hinge joint. I will leave it to you to investigate more complex rotational joint types. My only goal here is to simplify a bit the original texts and give some direct results, ready for implementation.

Let's start from a completely locked quaternion-based rotational constraint:
\begin{equation}
g(q,p)=\mathbb{P}p^*q = G(p)q = 0,
\end{equation}
where $p$ represents the desired relative orientation and $q=p_1^*p_2$ is the relative quaternion between the two bodies. By $p_i$ we mean the constraint frame orientation quaternion in world space relative to body $i$. The projection matrix $\mathbb{P}$ extracts the vector part from a quaternion: $q_v = \mathbb{P}q$ and $q=(q_s, q_v)$.

$G$ is a helper matrix used to convert quaternion operations to matrix operations. We will be using two such matrices:
\begin{equation}
E(q) =
\begin{bmatrix}
-q_v && S(q)
\end{bmatrix},
\end{equation}
\begin{equation}
G(q) =
\begin{bmatrix}
-q_v && T(q)
\end{bmatrix}.
\end{equation}
The 3 by 3 matrices $S$ and $T$ are the transpose of each other so we can only use the latter:
\begin{equation}
\label{tmatrix}
T(q) = q_s I_3 + [q_v]^{\times},
\end{equation}
where the skew-symmetric matrix of a vector $v=(x,y,z)$ is:
\begin{equation}
    [v]^{\times}=
    \begin{bmatrix}
    0 && v.z && -v.y \\
    -v.z && 0 && v.x \\
    v.y && -v.x && 0
    \end{bmatrix}.
\end{equation}
Note that we are using a convention different than the one used by Lacoursiere, i.e. the transpose of his matrix (or with a flipped sign as for any skew-symmetric matrix $A$ we have $A^T=-A$).

In general, the time derivative of the constraint function (i.e. the velocity error) is of the form:
\begin{equation}
\label{velconstr}
\dot{g}=J_1 \omega_1 + J_2 \omega_2,
\end{equation}
where $J_1$ and $J_2$ are the angular Jacobians corresponding to the two bodies and we denote by $\omega$ their angular velocities. Note that I have considered that the constraint function is not time dependent; so make sure the constraint frame is not changing in time, otherwise you will have to include the time derivative of $g$ too.

I will now give the formula for the Jacobian. Keeping in mind that $J_2 = -J_1$ we have:
\begin{equation}
    J_1 = -\tfrac{1}{2}G(p_1 p) E^T(p_2).
\end{equation}
Using the desired target relative quaternion $p$ is a good thing when you want to set up a joint motor or to set up a locked constraint without actually providing a constraint frame (by setting $p$ to the initial relative orientation between the bodies, i.e. $p=q_1^*q_2$ using the body orientations at the time the constraint is set up). However, for most practical reasons (like implementing a hinge) you will require a constraint frame to be defined and then all is required is that $p$ is the identity quaternion (and thus $q$ too):
\begin{equation}
g(p)=\mathbb{P}q=q_v=0.
\end{equation}
That simply means that the two constraint frames expressed in the two body local frames should overlap (for a locked joint at least). We only need three constraint equations because there are only three rotational degrees of freedom. This is also why we use unit quaternions (as the $q_s=1$ constraint becomes implicit).

After getting rid of $p$, we can further simplify the Jacobian so that it  only consists of 3 by 3 matrices:

\begin{equation}
    J_1 = -\tfrac{1}{2}[q_{1,v} q_{2,v}^T + T(q_1)T(q_2)].
\end{equation}

A hinge constraint is defined as following:

\begin{equation}
    g(q) =
    \begin{bmatrix}
    a^T \\
    b^T
    \end{bmatrix}
    \mathbb{P}q = P_{hinge}q=
    \begin{bmatrix}
    a \cdot q_v \\
    b \cdot q_v
    \end{bmatrix},
\end{equation}
where $a$ and $b$ are two perpendicular unit vectors in the constraint frame - their cross product will be the hinge axis. Most of the time we will choose them to be two of the coordinate system axes.

For solving the constraint we define two vectors $u$ and $v$ as the columns of $J_1^T$ corresponding to the coordinate axes chosen as $a$ and $b$. Then the velocity error becomes:
\begin{equation}
    \eta=
    \begin{bmatrix}
    u^T \\
    v^T
    \end{bmatrix}
    (\omega_1 - \omega_2).
\end{equation}
If we define the symmetric matrix:
\begin{equation}
    \tilde{A}=
    \begin{bmatrix}
    u^TAu && u^TAv \\
    v^TAu && v^TAv
    \end{bmatrix},
\end{equation}
then the hinge constraint reduces to solving the 2 by 2 linear system:
\begin{equation}
    \tilde{A}\lambda + \eta + \tfrac{\beta}{h}g(q)=0.
\end{equation}