Prevent rotational drift

Question

I am making a rigid-body simulation in 3D. And right now I have a precision problem with rotations. Eventually the orientation of a body (free of external forces) converges to be around the axis with smallest moment of inertia. Lowering dt helps but not too much. Is there a way to minimize this drift?

Here is my current algorithm:

Given:

Angular momentum L
Moment of inertia about principal axes in local space I
Simulation time step dt
Initial orientation matrix O

Steps:

Calculate angular velocity W from angular momentum. L = I * W => W = Inverse(I) * L. Because only the local moment of inertia is known the actual formula is W = O * Inverse(I) * Inverse(O) * L.
Calculate and apply change in rotation dO - rotation matrix about normalized W with angle Length(W) * dt.

To prevent orthogonality problems orientation is represented with a quaternion instead, but the algorithm is otherwise unchanged.

My thought was may be there is a way to leverage conservation of energy. While analyzing one of my test examples I found that the rotational energy drifts (increases) as the body rotates. Since I am using conservation of momentum to calculate angular velocity, using the conservation of energy could push all the numerical errors into some other "dimension". I presume that would be time, which I suppose is less painful to watch. But i do not even know where to begin.

user_9911440 · Answer

I can share my algorithm. It is a symplectic algorithm, based on a splitting of the inertia matrix into a sum two inertia matrices, two equal axes each, which leads to splitting the original system of differential equations for the angular momentum (the so called Euler's equations) into two systems of differential equations of bodies with two equal inertial axes. Each system can be solved explicitly and then the phase flows of the two systems are combined in a leap-frog manner. As a result, this algorithm preserve angular momenutm just like the original system and it quasi-conserves energy, i.e. it almost conserves energy and the latter does not dissipate, so there is no angular velocity drift. This is due to the fact that the algorithm preserves the so called symplectic structure on the sphere of the angular momentum, which geometrically means the algorithm preserves the area on the said sphere.

import math
import numpy as np

def Rot_3(m):
    cs = math.cos(m)
    sn = math.sin(m)
    return np.array([
            [cs, -sn, 0],
            [sn,  cs, 0],
            [ 0,   0, 1]])

def Rot_1(m):
    cs = math.cos(m)
    sn = math.sin(m)
    return np.array([
            [1,  0,   0],
            [0, cs, -sn],
            [0, sn,  cs]])
        
def vector_to_matrix(Vector):
    Matrix = np.array([ 0, - Vector[2],  Vector[1]],
                      [ 0,           0, -Vector[0]],
                      [ 0,           0,         0 ])
    return Matrix - Matrix.T

def Angular_Momentum_step(M_input, k_23, k_21, t_step):
    M_step = Rot_3(t_step*k_23 * M_input[2]/2).dot(M_input)
    M_step = Rot_1(t_step*k_21 * M_step[0]).dot(M_step)
    M_step = Rot_3(t_step*k_23 * M_step[2]/2).dot(M_step)
    return M_step

def Rotation_step(M, I_inv, t_step):
    O = I_inv*M
    angle = math.sqrt(O.dot(O))
    O = O / angle
    angle = t_step*angle
    O = vector_to_matrix(O)
    U = np.array([[1,0,0],[0,1,0],[0,0,1]])
    U = U + math.sin(angle)*O + (1-math.cos(angle))*(O.dot(O))
    return U 

def Propagate_Angular_Momentum(M_initial, Inertia, n_iterations, t_step):
    I_inv = np.array([1/Inertia[0], 1/Inertia[1], 1/Inertia[2]])
    k_23 = I_inv[1]-I_inv[2]
    k_21 = I_inv[1]-I_inv[0]
    M_evolution = np.empty((3, n_iterations), dtype=float)
    M_evolution[:,0] = M_initial.copy()
    for i in range((n_iterations-1)):
        M_evolution[:,i+1] = Angular_Momentum_step(M_evolution[:,i], k_23, k_21, t_step)
    return M_evolution

def Propagate_Rotation(Body_initial, Moment_initial, Inertia, n_iterations, t_step):
    I_inv = np.array([1/Inertia[0], 1/Inertia[1], 1/Inertia[2]])
    k_23 = I_inv[1]-I_inv[2]
    k_21 = I_inv[1]-I_inv[0]
    Moment_evolution = np.empty((3, n_iterations), dtype=float)
    Moment_evolution[:,0] = Moment_initial.copy()
    Body_evolution = np.empty((3, n_iterations+1), dtype=float)
    Body_evolution[:,0] = Body_initial.dopy()
    for i in range((n_iterations-1)):
        Moment_evolution[:,i+1] = Angular_Momentum_step(Moment_evolution[:,i], k_23, k_21, t_step)
        Body_evolution[:,i+1] = Rotation_step(Moment_evolution[:,i], I_inv, t_step)
        Body_evolution[:,i+1] = Body_evolution[:,i+1].dot(Body_evolution[:,i])
    return Body_evolution, Moment_evolution

# a test example, set up the initial angular velocity and the 
#  
I1 = 2.35
I2 = 2.0
I3 = 1.0
I = np.array([I1, I2, I3])
O = np.array([0, 2, 0.95])
O = Rot_3(-math.pi*(30)/180).dot(O)
M = I*O
dt = 0.3
n_iter=500

# propagate the system
Momenta = Propagate_Angular_Momentum(M, I, n_iter, dt)

# plot
fig = plt.figure()
ax = fig.add_subplot(1,2,1,projection='3d')
ax.set_xlim((-4, 4))
ax.set_ylim((-4, 4))
ax.set_zlim((-4, 4))

ax.plot(Momenta[0,:], Momenta[1,:], Momenta[2,:], 'r-')


plt.show()

Observe that the angular momentum always traverses the red curve which is always on a 2D sphere. The curve looks closed, there is no spiraling ootwards any of the coordinate axes, which are aligned with the inertial axes in the body fixed frame.

Prevent rotational drift

1 Answers