Rotational motion integration (Rigid body dynamics)

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.

• a few suggestions: prefer implicit methods over explicits as they are typically more numerically stable. Also remember that RK doesn't conserve energy, you might want to apply some constraints/Lagrange multipliers to make sure the variations of energy remain zero to acceptable order Oct 7 '19 at 18:39
• If you want to make the results more accurate, you can also try to shift the axis to the principal axis where the moment of inertia tensor is diagonal. See en.wikipedia.org/wiki/Moment_of_inertia#Principal_axes Oct 7 '19 at 19:06
• I don’t see where you get r(t) ? Where is your equations of motion for the translation?
– Eli
Oct 7 '19 at 19:21
• @Eli I have made proper edits to the post, explaining exactly how I update r(t) of each point. Also there is only rotation here. No translation of the center of mass. Oct 7 '19 at 21:52
• @RishabhJain The body is oriented from the beginning in such a way so that the x,y,z axes coincide with principal axes of inertia (Inertia tensor is diagonal). Oct 7 '19 at 21:54

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} }$$

But often still people get this step wrong, because integrating the above $$\boldsymbol{\hat{q}} \rightarrow \boldsymbol{\hat{q}} + h \dot{\boldsymbol{\hat{q}}}$$ still brakes the rotation representation.

The proper way to take an integration step with quaternions is as follows. 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 like other formulations do.