Inconsistency in quaternion to roll, pitch, yaw conversion and vice versa in InterpolatedMovingPlatform

[zoltantorok]

The problem
I noticed that roll and pitch angles are swapped in the trajectory output compared to the trajectory input converted by using RotationSpec::ARINC_705 in InterpolatedMovingPlatform::InterpolatedMovingPlatform

How to reproduce
Add the following log lines to InterpolatedMovingPlatform::InterpolatedMovingPlatform() and use an interpolated trajectory.

        case InterpolationScope::POSITION_AND_ATTITUDE:
            doStepUpdates = [&] (double const t) -> void{
                arma::Col<double> const x = tf->eval(t); //roll,pitch,yaw,x,y,z

                auto q = calcAttitude(x);
                double r, p, y;
                _getRollPitchYaw(r, p, y, q);
                stringstream ss;
                ss << "t = " << t << "\nroll, pitch, yaw = " << x[0] * 180 / M_PI << ", " << x[1] * 180 / M_PI << ", " << x[2] * 180 / M_PI << "\n" <<
                    "q = " << q << "\n" <<
                    "roll, pitch, yaw = " << r * 180 / M_PI << ", " << p * 180 / M_PI << ", " << y * 180 / M_PI;
                logging::INFO(ss.str());


                setPosition(glm::dvec3(x[3], x[4], x[5]));
                setAttitude(calcAttitude(x));
            };

This produces the following log lines in my case:
roll, pitch, yaw = -0.551372, -4.36025, 91.3625
q = [0.698005, 0.0300172, -0.0238576, 0.715066]
roll, pitch, yaw = -4.36025, -0.551372, 91.3625

If necessary, provide simulation XML files, XML code snippets or pyhelios code snippets.
I assume that the bug can be reproduced using any configuration. Just make sure to use an external trajectory.

Your environment (please complete the following information):

• OS: Windows 11
• HELIOS++/pyhelios version (or Git revision) that exhibits the issue 1.3.0

Ideas for possible solutions (Optional)
It seems that the roll, pitch, yaw angle conversion to quaternion using RotationSpec::ARINC_705 is incosistent with the quaternion to Euler ZYX conversion.

[albertoesmp]

Hi @zoltantorok, thank you for sharing your findings. We have updated the implementation to use the same format for input and output trajectories when using the ARINC 705 specification. We will include the fix in a future release.

In the meantime, you can find the update available in this PR: #455

I hope this solves the issue. If not, please let us know so we can work on further fixes.

[zoltantorok]

Hi Alberto, thank your the quick response. Your changes indeed fix the inconsistency.
May I suggest you to implement a unit test which makes sure that the forward and backward computation of the angles result in the same angles?

[albertoesmp]

Hi Alberto, thank your the quick response. Your changes indeed fix the inconsistency. May I suggest you to implement a unit test which makes sure that the forward and backward computation of the angles result in the same angles?

Indeed, that is an excellent idea. We will update the tests after the new release because we are currently preparing a major update.

[zoltantorok]

Hi @albertoesmp,
I'm verfying the simulated point cloud with your changes. For this, I'm entering the following trajectory to interpolated trajectory:

#HEADER: "t", "x", "y", "z", "roll", "pitch", "yaw"
1691589166.01,6000.552158877952,4125.025641061366,99.99462950136513,-0.5513724297999998,-4.360254827899994,91.3624969843
1691589166.02,6001.105148244358,4125.049764296971,99.9893055697903,-0.5638788007999999,-4.366566129299997,91.37006893870002
1691589166.03,6001.657148337923,4125.07468111068,99.98460558243096,-0.5677190979999995,-4.3792503633999935,91.38001748879998
1691589166.04,6002.209151129646,4125.098919056356,99.97917184233665,-0.5503749543000008,-4.3787357822000015,91.39198127840001
1691589166.05,6002.762296588975,4125.124445407651,99.97391144186258,-0.5642748178000002,-4.414036033799994,91.40258991510001
1691589166.06,6003.314299382793,4125.14868335519,99.96847779676318,-0.5522718768000007,-4.409630028599999,91.4121149046
1691589166.07,6003.867286057619,4125.1734854588285,99.96388790849596,-0.5694027671000005,-4.4334761785999985,91.42005404180003

Using the following leg:

        <leg>
            <platformSettings 
                trajectory="R66_trajectory_helios.csv"
                tIndex="0" xIndex="1" yIndex="2" zIndex="3" rollIndex="4" pitchIndex="5" yawIndex="6"
                slopeFilterThreshold="0.0" toRadians="true" syncGPSTime="true"
                interpolationDomain="position_and_attitude" trajectory_separator=","
            />
            <scannerSettings template="set" trajectoryTimeInterval_s="0.01"/>
        </leg>

This roll, pitch, yaw angle -0.5513724297999998,-4.360254827899994,91.3624969843 corresponds to the following rotation matrix:

[[-0.02370905,  0.99682376,  0.07602816],
 [-0.99968837, -0.02304531, -0.00959574],
 [-0.00781317, -0.07623197,  0.9970595 ]]

However, when I print the rotation matrix using your changes in InterpolatedMovingPlatform::InterpolatedMovingPlatform, then I get this rotation matrix:

[[-0.023777  , -0.996841  ,  0.0757779 ],
 [ 0.999671  , -0.0229778 ,  0.0114008 ],
 [-0.00962355,  0.0760241 ,  0.99706   ]]

This means that internally in Helios the roll and pitch angles are swapped and negated.
I checked the simulated point cloud and it looks accordingly. The point cloud is visibly rotated around the roll axis by abour 4 degrees.

[albertoesmp]

Hi again @zoltantorok

I've swapped and negated the roll and pitch angles. The new update should be available here: #455

Could you confirm that this works as expected for you?

[zoltantorok]

Thanks @albertoesmp for your latest fix. I can confirm that the rotation matrix is now as expected.
I'll run further tests, to verify my full simulation environment. Should anything else pop up, I'll report it here.