Equation of motion for rigid body dynamics with quaternions

105 Views Asked by At

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?