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}^{mtimes 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 $mtimes 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?

JAlex · Answer

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

Given known orientation $boldsymbol{q}$ calculate the 3×3 mass mass matrix $mathbf{M}$, and external torque vector $boldsymbol{tau}_{rm ext}$.
Given known motion $boldsymbol{omega}$ calculate the velocity related force vector $boldsymbol{b} = boldsymbol{omega} times mathbf{M} boldsymbol{omega} $
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)
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}$
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}timesboldsymbol{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}$$

Equation of motion for rigid body dynamics with quaternions

One Answer

Baseline Method

Alternative 1

Alternative 2

Alternative 3

Add your own answers!

Ask a Question