What is suppose to happen when aircraft reach 90 degree of pitch

[ShirelJosef]

I am initializing aircraft with 0,0,0 pitch roll yaw and gives it 1 in throttle and -1 in elevator.
the aircraft pitch rise until it reach 90 and then it changes continuously making a very fast non physical roll (I visualize it using opengl).
Is it expected?
Although from the graph it seems immediate, the change is not immediate
gifs:

import jsbsim

import numpy as np
import matplotlib.pyplot as plt

def print_values(simulation):
    print("attitude/phi-deg "+str( simulation.get_property_value("attitude/phi-deg")))
    print("attitude/theta-deg "+str( simulation.get_property_value("attitude/theta-deg")))
    print("attitude/psi-deg "+str( simulation.get_property_value("attitude/psi-deg")))
    print("position/h-sl-ft "+str( simulation.get_property_value("position/h-sl-ft")))
    print("\n")

roll_list = []
pitch_list = []
yaw_list = []

# Short example script to create and run the environment with
# constant action for 1 simulation second.
if __name__ == "__main__":
    from time import sleep
    root='.'
    # Initialize JSBSim
    simulation = jsbsim.FGFDMExec(root, None)
    simulation.set_debug_level(0)
    simulation.set_dt(0.01)

    initial_long_deg = 0
    intial_lat_deg = 0
    # Load F-16 model and set initial conditions
    simulation.load_model('f16')
    simulation.set_property_value('propulsion/set-running', -1)
    simulation.set_property_value('ic/h-sl-ft', 10000)
    simulation.set_property_value('ic/long-gc-deg', initial_long_deg)
    simulation.set_property_value('ic/lat-geod-deg', intial_lat_deg)
    simulation.set_property_value('ic/u-fps', 1000.)

    simulation.run_ic()
    print_values(simulation)
    
    t = np.arange(6000)
    for i in t:
        simulation.set_property_value("propulsion/tank/contents-lbs", 1000)
        simulation.set_property_value("propulsion/tank[1]/contents-lbs", 1000)

        # Set gear up
        simulation.set_property_value("gear/gear-cmd-norm", 0.0)
        simulation.set_property_value("gear/gear-pos-norm", 0.0)
        simulation.set_property_value("fcs/aileron-cmd-norm", 0)
        simulation.set_property_value("fcs/elevator-cmd-norm", -1)
        simulation.set_property_value("fcs/rudder-cmd-norm", 0)
        simulation.set_property_value("fcs/throttle-cmd-norm", 1)
        simulation.run()

        pitch = simulation.get_property_value("attitude/pitch-rad")
        roll = simulation.get_property_value("attitude/roll-rad")
        yaw =  simulation.get_property_value("attitude/heading-true-rad")

        roll_list.append(roll)
        pitch_list.append(pitch)
        yaw_list.append(yaw)
        '''if pitch > np.pi/2 - 0.01:
            print("break")
            print("pitch"+str((pitch)))
            print("roll"+str((roll)))
            print("yaw"+str((yaw)))
            print("\n")'''

    fig, axs = plt.subplots(3 ,sharex=True)
    axs[0].plot(t, roll_list)
    axs[1].plot(t, pitch_list)
    axs[2].plot(t, yaw_list)
    axs[0].set_title('roll')
    axs[1].set_title('pitch')
    axs[2].set_title('yaw')
    plt.show()

[seanmcleod]

Hmm, so the roll angle toggles between 0 and pi when passing through +-90 degrees of pitch.

Possibly due to 'gimbal lock' during conversion from quaternions to Euler angles.

jsbsim/src/math/FGMatrix33.cpp

Lines 156 to 190 in 6c1abbe

	// Compute the Euler-angles
	// Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8..
	FGColumnVector3 FGMatrix33::GetEuler(void) const
	{
	FGColumnVector3 mEulerAngles;
	bool GimbalLock = false;
	if (data[6] <= -1.0) {
	mEulerAngles(2) = 0.5*M_PI;
	GimbalLock = true;
	}
	else if (1.0 <= data[6]) {
	mEulerAngles(2) = -0.5*M_PI;
	GimbalLock = true;
	}
	else
	mEulerAngles(2) = asin(-data[6]);
	if (GimbalLock)
	mEulerAngles(1) = atan2(-data[5], data[4]);
	else
	mEulerAngles(1) = atan2(data[7], data[8]);
	if (GimbalLock)
	mEulerAngles(3) = 0.0;
	else {
	double psi = atan2(data[3], data[0]);
	if (psi < 0.0)
	psi += 2*M_PI;
	mEulerAngles(3) = psi;
	}
	return mEulerAngles;
	}

[seanmcleod]

So just double-checking, I logged the roll rate velocities/p-rad_sec, displayed as deg/s in the graph.

So with a roll rate so small there is definitely no way that the aircraft is actually rolling through 180 degrees in a couple of simulation frames. So I'm pretty sure the issue is related to converting between quaternions and Euler angles.

Notice the same issue with the yaw, also switches virtually instantly between 0/360 (north) and 180 (south) each time the pitch attitude passes through +-90 degrees.

[ShirelJosef]

Yes, this is an issue in the conversion
I used to following code:

from scipy.spatial.transform import Rotation as R
m_rot = R.from_matrix(propagate.get_Tl2b().T)
f16_roll, f16_pitch, f16_yaw = r = m_rot.as_euler('zyx')

And I get smooth transition.
Isn't this a bug?

Also, I need to use angular velocities in the body frame ( p,q,r I assume), but now I think it might suffer from similar issues.

I tried to get the angular velocities from computing relative rotations with quaternions from the previous and current R.from_matrix(propagate.get_Tl2b().T) but it doesn't seem correct
(red is from jsbsim)

using this code from https://mariogc.com/post/angular-velocity-quaternions/

def angular_velocities(q1, q2, dt):
    return (2 / dt) * np.array([
        q1[0]*q2[1] - q1[1]*q2[0] - q1[2]*q2[3] + q1[3]*q2[2],
        q1[0]*q2[2] + q1[1]*q2[3] - q1[2]*q2[0] - q1[3]*q2[1],
        q1[0]*q2[3] - q1[1]*q2[2] + q1[2]*q2[1] - q1[3]*q2[0]])

[seanmcleod]

And I get smooth transition.

So what does your graph output look like now?

Notice the same issue with the yaw,....

Ignore that comment, yaw should be switching between north and south at the top and bottom of the loop.

[ShirelJosef]

the roll and psi cancel each other near gimbal lock values for example:
before reaching 90 pitch
f16_roll -0.19745271797018438
f16_pitch 1.5658956076812902
f16_yaw 0.19770115798908572
after reaching 90 pitch
f16_roll -3.038253346978119
f16_pitch 1.561270932793192
f16_yaw 3.0385035131695073

[seanmcleod]

In terms of the yaw, as shown in the graphs above do you agree that the output is correct? Just take into account the wrap-around 0/360 deg.

[agodemar]

It's a well-known problem. The moment you use Euler angles for visualization you get this. If you use a visualization function that directly handles the aircraft attitude quaternion, the motion should be correctly displayed