Equation of motion for rigid body dynamics with quaternions

Question

I'm trying to understand the equation of motion for rigid body dynamics in the presence of a quaternion joint for the root of a humanoid robot. But the dimensionality inconsistency issue is confusing me now.

Let $\mathbf{q}\in \mathbb{R}^{m}$ be a configuration vector of the humanoid body such that the first three entry represents the global $xyz$ position of the root of the humanoid. The second three entry represents the $xyz$ root orientation of the humanoid. The remaining represents each joint angle. Let's say the humanoid has $n$ joints, then the dimentionality of $\mathbf{q}$ will be $m=3+3+n$ ($xyz$ root translation+xyz root orientation + joint angles). I also denote the velocity and acceleration of $\mathbf{q}$ as $\mathbf{\dot{q}}\in \mathbb{R}^{m}$ and $\mathbf{\ddot{q}}\in \mathbb{R}^{m}$, respectively.

In general the equation of motion is written:

\begin{equation} \label{eq:eom} \mathbf{M} \ddot{\mathbf{q}} = \boldsymbol{\tau} + \boldsymbol{\tau}_{ext}, \end{equation} where $\boldsymbol{\tau}\in \mathbb{R}^{m}$ is the force vector generated on joints, $\boldsymbol{\tau}_{ext}\in \mathbb{R}^{m}$ is the applied bias forces in the generalized coordinate, e.g. ground reaction force, gravity and centrifugel forces. $\mathbf{M}\in \mathbb{R}^{m\times m}$ is joint-space inertia matrix.

Now, I use a quaternion joint to represents the 3D angle of the humanoid root orientation. Since quaternion joint has a dimensionality of 4, the dimensionality of $\mathbf{q}\in \mathbb{R}^{m_{quat}}$ is now $m_{quat} = 7 + n$. Accordingly the dimensionality of $\mathbf{M}$ will be lifted up from $m\times m$ to $m_{quat}\times m_{quat}$. This confusing for me. In my understanding, the dimensionality of $\boldsymbol{\tau}$ and $\ddot{\mathbf{q}}$ remain the same ($m$) even if we use a quaternion joint to represent the root orientation. Then, isn't the equation not solvable anymore because of the inconsistency of the dimensionalities? Or am I misunderstanding something?

Answer 1

Although the quaternion has 4 parameters, it really has 3 degrees of freedom, since it must obey the unity condition $\sqrt{x^2+y^2+z^2+w^2}=1$.

Also note that $\boldsymbol{\omega} \neq \boldsymbol{\dot q}$ and the same applied for their derivatives also.

Baseline Method

So the process goes like this

1. Given known orientation $\boldsymbol{q}$ calculate the 3×3 mass mass matrix $\mathbf{M}$, and external torque vector $\boldsymbol{\tau}_{\rm ext}$.
2. Given known motion $\boldsymbol{\omega}$ calculate the velocity related force vector $\boldsymbol{b} = \boldsymbol{\omega} \times \mathbf{M} \boldsymbol{\omega} $
3. Calculate the rate of change of the orientation $\boldsymbol{\dot q} = \tfrac{1}{2} \pmatrix{\boldsymbol{\omega} \\0} \otimes \boldsymbol{q}$ (to be used later)
4. From the above initial conditions apply the equations of motion $$\mathbf{M}\, \boldsymbol{\dot \omega} = \boldsymbol{\tau}_{\rm ext} + \boldsymbol{b}$$ and solve for rotational accelerations $\boldsymbol{\dot \omega}$
5. Take an integration step (or sub-step in a RK4 scheme) with time step $h$
   • $t \leftarrow t + h$
   • $\boldsymbol{q} \leftarrow {\rm unit}(\boldsymbol{q} + \boldsymbol{\dot q} \,h)$
   • $\boldsymbol{\omega} \leftarrow \boldsymbol{\omega} + \boldsymbol{\dot \omega} \, h$

So you only using the 4 vector of the quaternion for orientation, and all the motion related quantities are the usual 3 vectors rotational velocity $\boldsymbol{\omega}$ and acceleration $\boldsymbol{\dot \omega}$.

Alternative 1

If the time step is too large, then the orientation is going to drift away slowly because the quantity $\boldsymbol{q} + \boldsymbol{\dot q} \,h$ isn't an orientation until it is re-normalized with the $\mathrm{unit}(\boldsymbol{q})$ function. Also the desired accuracy of the integration may be comprised by the baseline scheme. We expect $\mathcal{O}(h^4)$ error with RK4, but the four repeated normalizations might make the error worse. Each normalization introduces an error in angle equal to $\tfrac{\theta}{2} - \tan^{-1}( \tfrac{\theta}{2} )$ where $\theta$ is amount of rotation in the time step $h$.

If the rotational velocity $\boldsymbol{\omega}$ and time step $h$ are combined then we can represent a finite rotation (rotation constant assumed during the substep). This finite rotation has angle $\theta = h\,\|\boldsymbol{\omega}\|$ and axis $\boldsymbol{\hat{u}} = \boldsymbol{\omega} / \| \boldsymbol{\omega} \|$. We create a unit quaternion for this finite rotation $$ \boldsymbol{q}_\theta = {\rm rotation}(\boldsymbol{\hat u}, \, \theta)$$ and then use the integration substep $$ \boldsymbol{q} \leftarrow \boldsymbol{q} \otimes \boldsymbol{q}_\theta $$

The result is guaranteed to be a unit quaternion. The problem is when the rotation speed is small, the axis of rotation isn't defined properly, and you have to default back to the baseline method, or introduce more errors.

Alternative 2

This can be use together with the alternative one, and basically instead of tracking $\boldsymbol{\omega}$ and integrating motions, you track angular momentum $\boldsymbol{H} = \mathbf{M}\,\boldsymbol{\omega}$ making the equations of motion $$ \boldsymbol{\dot H} = \boldsymbol{\tau}_{\rm ext} + \boldsymbol{\omega}\times\boldsymbol{H} $$ and integrating with $$ \boldsymbol{H} \leftarrow \boldsymbol{H} + h\,\boldsymbol{\dot H}$$ and calculating $\boldsymbol{\omega}$ from $\boldsymbol{H}$ to be used in the quaternion integration step as $$\boldsymbol{\omega} = \mathbf{M}^{-1} \boldsymbol{H}$$

Alternative 3

Use $\boldsymbol{\dot q} = \tfrac{1}{2}\pmatrix{ \boldsymbol{\omega} \\0} \otimes \boldsymbol{q}$ to establish that the 4 vector $$ \pmatrix{ \boldsymbol{\omega} \\ 0} = 2 \boldsymbol{\dot q} \otimes \boldsymbol{q}^{-1}$$ and its derivatives

$$ \pmatrix{ \boldsymbol{\dot \omega} \\ 0} = 2 \boldsymbol{\ddot q} \otimes \boldsymbol{q}^{-1} + 2 \boldsymbol{\dot q} \otimes \boldsymbol{\dot q}^{-1}$$

or

$$ \pmatrix{ \boldsymbol{\dot \omega} \\ 0} = \left( 2 \boldsymbol{\ddot q} + \pmatrix{ \boldsymbol{\omega} \\0} \otimes \boldsymbol{q} \right) \otimes \boldsymbol{\dot q}^{-1}$$

for use in forward dynamics

$$\boldsymbol{\tau}_{\rm ext} =\mathbf{M}\, \boldsymbol{\dot \omega} -\boldsymbol{b}$$