Rotational motion integration (Rigid body dynamics)

Question

I am trying to integrate the rotational motion of a rigid body (a set of N point masses) $\textbf{in the inertial frame}$, but my results seem totally wrong. What of the following steps could be wrong?

1) Assuming only an inertial frame, we can write:

$$ \frac{d\vec{L}}{dt} = \vec{\tau} \Rightarrow \frac{d(I\vec{\omega})}{dt} = \vec{\tau} \Rightarrow \frac{dI}{dt}\vec{\omega} + I\frac{d\vec{\omega}}{dt} = \vec{\tau} \Rightarrow \boxed{\frac{d\vec{\omega}}{dt} = I^{-1}(\vec{\tau} - \frac{dI}{dt}\vec{\omega})} \hspace{0.2cm} (1) $$

2) In the inertial frame we have:

$$ \vec{r}_i(t) = x_i(t)\hat{x} + y_i(t)\hat{y} + z_i(t)\hat{z} $$ $$ \vec{v}_i(t) = \dot{\vec{r}}_i(t) = \dot{x}_i(t)\hat{x} + \dot{y}_i(t)\hat{y} + \dot{z}_i(t)\hat{z} $$ $$ \vec{\omega}(t) = \omega_x(t)\hat{x} + \omega_y(t)\hat{y} + \omega_z(t)\hat{z} $$ $$ \dot{\vec{r}}_i(t) = \vec{\omega}\times \vec{r}_i $$

3) Since I have assumed only an inertial frame, the inertia tensor $I$ will be a function of time and will be updated at each time step $t$.

$$I(t) = \begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{yx} & I_{yy} & I_{yz} \\ I_{zx} & I_{zy} & I_{zz} \\ \end{bmatrix} $$

where

$$I_{xx} = \sum m_i(y_i^2+z_i^2)$$

$$I_{yy} = \sum m_i(x_i^2+z_i^2)$$

$$I_{zz} = \sum m_i(x_i^2+y_i^2)$$

$$I_{xy} = I_{yx} = -\sum m_ix_iy_i$$

$$I_{xz} = I_{zx} = -\sum m_ix_iz_i$$

$$I_{yz} = I_{zy} = -\sum m_iy_iz_i$$

I have calculated the derivative of $I$ to be:

$$ \dot{I} = \begin{bmatrix} \dot{I}_{xx} & \dot{I}_{xy} & \dot{I}_{xz} \\ \dot{I}_{yx} & \dot{I}_{yy} & \dot{I}_{yz} \\ \dot{I}_{zx} & \dot{I}_{zy} & \dot{I}_{zz} \\ \end{bmatrix} $$

where

$$\dot{I}_{xx} = \sum m_i(2y_i\dot{y}_i + 2z_i\dot{z}_i)$$

$$\dot{I}_{yy} = \sum m_i(2x_i\dot{x}_i + 2z_i\dot{z}_i)$$

$$\dot{I}_{zz} = \sum m_i(2x_i\dot{x}_i + 2y_i\dot{y}_i)$$

$$\dot{I}_{xy} = \dot{I}_{yx} = -\sum m_i(\dot{x}_iy_i + x_i\dot{y}_i)$$

$$\dot{I}_{xz} = \dot{I}_{zx} = -\sum m_i(\dot{x}_iz_i + x_i\dot{z}_i)$$

$$\dot{I}_{yz} = \dot{I}_{zy} = -\sum m_i(\dot{y}_iz_i + y_i\dot{z}_i)$$

4) I integrate the differential equation $(1)$ using a simple Runge-Kutta 4 scheme like this:

$$t_{i+1} = t_i + h$$ $$\vec{\omega}_{i+1} = \vec{\omega}_i + \frac{h}{6}(\vec{k}_1+2\vec{k}_2+2\vec{k}_3+\vec{k}_4)$$

where $h$ is the integration time step and

$$\vec{k}_1 = \vec{f}(\vec{\omega}_i)$$ $$\vec{k}_2 = \vec{f}(\vec{\omega}_i + \frac{\vec{k}_1h}{2})$$ $$\vec{k}_3 = \vec{f}(\vec{\omega}_i + \frac{\vec{k}_2h}{2})$$ $$\vec{k}_4 = \vec{f}(\vec{\omega}_i + \vec{k}_3h)$$

I begin the simulation by initializing the system with an angular velocity $\vec{\omega}_0$. After that, at each time step I rotate all $N$ points of the rigid body around the current vector $\vec{\omega}$ by an angle $|\vec{\omega}|h$ using a rotation matrix calculated through Rodrigues formula

$$ R = J + \sin(\omega h)W + [1-\cos(\omega h)]W^2 $$

where $J$ is the $3\times 3$ identity matrix and $W = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \\ \end{bmatrix} \hspace{0.2cm} \text{with} \hspace{0.2cm} \vec{u} = \dfrac{\vec{\omega}}{|\vec{\omega}|}$

After the rotation/update of all $N$ points, I recalculate the inertia tensor $I$ (and thus $\dot{I}$ and $I^{-1}$) and then, through equation $(1)$ I update the angular velocity $\vec{\omega}$. The cycle goes on from $t = 0$ up to some $t_{max}$ with step $h$. The problem is that at first, the results are correct (angular momentum and energy are constant), but after some time iterations, the numbers grow quickly too big and I get full of NaNs. Even for the simplest case were the external torque is $\vec{\tau} = \vec{0}$, the same happens. I checked if there is a problem with the determinant of $I$ (and thus cannot be inversed), but the determinant remains nonzero. Is there anything wrong with any of the equations? Must I perform some kind of normalization to a quantity during the time loop? There must be way in which you can simulate the rigid body rotation in the inertial frame. Thank you.

Answer 1

I did not follow your derivation of $\frac{{\rm d}\mathbf{I}}{{\rm d}t}$. In most textbooks it is evaluated as follows $$\frac{{\rm d}\mathbf{I}}{{\rm d}t} =\boldsymbol{ \omega } \times \mathbf{I} = \begin{vmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{vmatrix} \begin{vmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{xy} & I_{yy} & I_{yz} \\ I_{xz} & I_{yz} & I_{zz} \end{vmatrix} $$

with the added caveat that $\mathbf{I}$ depends on the orientation of the body. The orientation may be tracked using Euler angles, Quaternions or just the 3×3 rotation matrix $\mathbf{R}$. Either way the end result is that the mass moment of inertia tensor needs to be calculated at every instant from the MMOI in the body frame

$$ \mathbf{I} = \mathbf{R}\,\mathbf{I}_{\rm body} \,\mathbf{R}^\top $$

In the end you have the equations of motion

$$ \left. \boldsymbol{\tau} = \mathbf{I}\, \boldsymbol{\dot{\omega}} + \boldsymbol{\omega} \times \mathbf{I} \boldsymbol{\omega}\;\; \right\} \;\; \boldsymbol{\dot{\omega}} = \mathbf{I}^{-1}\left(\boldsymbol{\tau} - \boldsymbol{\omega} \times \mathbf{I} \boldsymbol{\omega} \right) $$

It is also common to express the above in terms of angular momentum in the following algorithm. Each integration step is given the rotation matrix $\mathbf{R}$ and momentum vector $\boldsymbol{L}$

$$ \begin{array}{c|cc} \text{Step} & \text{Calculation} & \text{Notes}\\ \hline 0 & \mathbf{I}=\mathbf{R}\mathbf{I}_{{\rm body}}\mathbf{R}^{\top} & \text{MMOI in world coorinates}\\ 1 & \boldsymbol{\omega}=\mathbf{I}^{-1}\boldsymbol{L} & \text{Extract rotational vector}\\ 2 & \dot{\mathbf{R}}=\boldsymbol{\omega}\times\mathbf{R} & \text{Change in rotation}^\star\\ 3 & \dot{\boldsymbol{L}}=\boldsymbol{\tau}(t,\mathbf{R},\boldsymbol{\omega}) & \text{Change in momentum due to torque }\boldsymbol{\tau} \end{array} $$

Note : When integrating the rotation matrix $\mathbf{R}$ using Runge-Kutta the result of $\mathbf{R} \rightarrow \mathbf{R} + h \dot{\mathbf{R}}$ is no longer a rotation matrix and the solution will quickly degrade in accuracy.

So instead, people often use quaternions $\boldsymbol{\hat{q}} = \pmatrix{ \boldsymbol{q}_{\rm v} & q_{\rm s}} $ that describe the rotation as $$ \mathbf{R} = \mathbf{1} + 2 q_{\rm s} [ \boldsymbol{q}_{\rm v}\times] + 2 [ \boldsymbol{q}_{\rm v} \times][ \boldsymbol{q}_{\rm v} \times] $$ _{where $[ \boldsymbol{q}_{\rm v} \times] = \begin{vmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{vmatrix}$ is the 3×3 cross product matrix operator of the vector part of the quaternion $\boldsymbol{q}_{\rm v}$.}

The derivative of the quaternion is defined as $$ \dot{\boldsymbol{\hat{q}}} = \frac{1}{2} \pmatrix{ -\boldsymbol{\omega}^\top \boldsymbol{q}_{\rm v} \\ q_{\rm s} \boldsymbol{\omega} + \boldsymbol{\omega} \times \boldsymbol{q}_{\rm v} }$$

There are two ways to integrate the quaternion

• use $\boldsymbol{\hat{q}} \rightarrow \boldsymbol{\hat{q}} + h \dot{\boldsymbol{\hat{q}}}$ which requires re-normalization after each substep.

• Given $\boldsymbol{\hat{q}} = \pmatrix{\boldsymbol{q}_{\rm v} & q_{\rm s}}$ and $\boldsymbol{\omega}$ vector

  $$ \pmatrix{\boldsymbol{q}_{\rm v} \\ q_{\rm s}} \rightarrow \begin{vmatrix} \cos(\tfrac{\theta}{2} ) & -\sin(\tfrac{\theta}{2} ) \boldsymbol{z}^\top \\ \sin(\tfrac{\theta}{2} ) \boldsymbol{z} & \cos(\tfrac{\theta}{2} ) + \sin(\tfrac{\theta}{2} ) [\boldsymbol{z}\times] \end{vmatrix} \pmatrix{\boldsymbol{q}_{\rm v} \\ q_{\rm s}} $$

  where $\theta = h \| \boldsymbol{\omega} \|$ is the step angle and $\boldsymbol{z} = \boldsymbol{\omega}/\|\boldsymbol{\omega}\|$ is the step rotation axis.

  The resulting quaternion still represents rotations always and does not drift away as other formulations do, but is unstable for low rotation values, as you can see from the $\omega$ in the denominator.