# 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 Commented Oct 7, 2019 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 Commented Oct 7, 2019 at 19:06
• I don’t see where you get r(t) ? Where is your equations of motion for the translation?
– Eli
Commented Oct 7, 2019 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. Commented Oct 7, 2019 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). Commented Oct 7, 2019 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} }$$

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.