Gimbal Lock and Euler angles

[misho]

I am somewhat stuck on the Euler angles and a problem of gimbal lock. I am working with an existing flight simulator platform as a display engine, and Newton Game dynamics as a driving physics engine. First, to stipulate the problem:

My display engine platform accepts Euler angles (yaw, roll, pitch) as the ONLY means of specifying object's attitude.

Since Newton Dynamics works with quaternions which are a lot more stable and avoid gimbal lock, I was hoping that would eliminate the gimbal lock problem. However, I am still seeing it... and I'm guessing, that's because I have to convert from Quaternion to Euler angles, which then re-introduces the gimbal lock problem.

I noticed that in dVector dQuaternion::GetXYZ_EulerAngles(), code exists which tries to minimize gimbal lock. Is gimbal lock happening because of the conversion from quaternion to Euler angles? Am I stuck with having to deal with a certain amount of gimbal lock because I HAVE to use Euler angles to give my objects attitude values?

Thanks,
Misho

[JoeJ]

Euler angles (yaw, roll, pitch)

if your renderer assumes euler angles in that order, you need to add this to dMathDefines.h:

enum dEulerAngleOrder
{
m_pitchYawRoll = (0 << 8 ) + (1 << 4) + (2 << 0),
m_rollYawpitch = (2 << 8 ) + (1 << 4) + (0 << 0),
m_yawRollPitch = (1 << 8 ) + (2 << 4) + (0 << 0), // <- add your order
};

Then you get them by:
dVector euler, unused;
myQuat.GetEulerAngles(euler, unused, m_yawRollPitch);

See the code from actual Newton (GetXYZ_EulerAngles seems replaced by that).

works with quaternions which are a lot more stable and avoid gimbal lock, I was hoping that would eliminate the gimbal lock problem

Gimbal lock is a result of the fact that euler angles are a series of 3 rotations in specified order.
You must be sure that you use the same order that your library expects, otherwise it may look ok only as long as one or more angles are close to zero.

There is support for 6 possible orders in GetEulerAngles() - only exotic stuff like XYX does not work.
You may want to try them all before expecting the problem elsewhere.

Hexcodes:
0x012 // = XYZ
0x120 // = YZX = (1 << 8 ) + (2 << 4) + (0 << 0)
0x201
0x102
0x210
0x021

Edit:
If it still does not help, it may be a left / right handed issue too.
GetEulerAngles converts quat to matrix.
negate one axis of that matrix (matrix.m_front = matrix.m_front.scale(-1.0f)) to change handness and try all 6 again.
Hope that works - it may also work to negate some of the resulting euler angles.

[misho]

Thanks JoeJ!

Couple of questions:

I am (still) using Newton 2.24 since it contained GetXYZ_EulerAngles. I didn't find any Euler conversion functions in the newer versions (Last one I have is 3.11) so I kept working with 2.24. What version has myQuat.GetEulerAngles(euler, unused, m_yawRollPitch) that you mentioned? I should be bringing my code up to the latest spec.

I understand the order of rotation, and in fact, I found and implemented code that has a much more extensive set of options for conversion, from here:

http://bediyap.com/programming/convert-quaternion-to-euler-rotations/

This code, however, doesn't seem to address gimbal lock at all. My flight simulator specifies that it expects the euler angles in (heading,pitch,roll) order. I implemented the rotations using this order, and it still does not fix the gimbal lock. In fact, from my research on line, it seems that gimbal lock cannot be fully fixed if Euler angles are used. Newton's dQuaternion::GetXYZ_EulerAngles() function seems to be fixing it by checking a special case when the angles are close to the gimbal lock conditions. The newer graphics systems allow for rotations by specifying them using quaternions, but I have a misfortune of using an older system that does not allow for quaternion specification. Does the latest Newton code address gimbal lock?

Another question I need to address is the range of angles, particularly pitch, which I need in range of -180/180 degrees. It looks like both approaches I tried (Newton and the above link) return pitch in the range of -90/90 degrees.

Thanks!
Misho

[JoeJ]

You do not need to update, just download actual version and see / copy code from dMath stuff.
Relevant Matrix to Euler function below.

The link you posted is nice, can also handle XYX ... but in your case that can't be and it should be the same otherwise.

Another question I need to address is the range of angles, particularly pitch, which I need in range of -180/180 degrees. It looks like both approaches I tried (Newton and the above link) return pitch in the range of -90/90 degrees.

* Maybe the second set of euler angles coming out of dMatrix::GetEulerAngles are better for you.
I don't know the reason julio added them, seems to handle handness issues.
* Because euler angles give a final result from 3 rotations, there are infinite possibilities to get the same result including sign issues (-10 == 350) and overspin. But what you say indicates it's an order issue most probably.


Those things can be frustrating, it's a lot of trial and error because even if they tell you the order, you can't be sure that you both has the same conventions for questions like: Is Roll about x or y axis, coordinate system handness and so on...

You'd need to post a video with some frailure cases.
Gimbal lock is nothing that needs a fix because there is nothing wrong about it.
I remember you are the guy who alse has to use spherical coordinate system (lat, long, alt) for position?

What you really need would ba a way to verify conventions match, then you could try out all possibilities until it matches for rotations about all angles.
Can you hack your graphics lid code? It must use matrices at some point internally.

Code: Select all

void dMatrix::GetEulerAngles(dVector& euler0, dVector& euler1, dEulerAngleOrder order) const
{
   int a0 = (order>>8)&3;
   int a1 = (order>>4)&3;
   int a2 = (order>>0)&3;
   const dMatrix& matrix = *this;

   // Assuming the angles are in radians.
   if (matrix[a0][a2] > 0.99995f) {
      dFloat picth0 = 0.0f;
      dFloat yaw0 = -3.141592f * 0.5f;
      dFloat roll0 = - dAtan2(matrix[a2][a1], matrix[a1][a1]);
      euler0[a0] = picth0;
      euler0[a1] = yaw0;
      euler0[a2] = roll0;

      euler1[a0] = picth0;
      euler1[a1] = yaw0;
      euler1[a2] = roll0;

   } else if (matrix[a0][a2] < -0.99995f) {
      dFloat picth0 = 0.0f;
      dFloat yaw0 = 3.141592f * 0.5f;
      dFloat roll0 = dAtan2(matrix[a2][a1], matrix[a1][a1]);
      euler0[a0] = picth0;
      euler0[a1] = yaw0;
      euler0[a2] = roll0;

      euler1[a0] = picth0;
      euler1[a1] = yaw0;
      euler1[a2] = roll0;
   } else {
      //euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
      //euler[a1] = -dAsin ( matrix[a0][a2]);
      //euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);

      dFloat yaw0 = -dAsin ( matrix[a0][a2]);
      dFloat yaw1 = 3.141592f - yaw0;
      dFloat sign0 = dSign(dCos (yaw0));
      dFloat sign1 = dSign(dCos (yaw1));

      dFloat picth0 = dAtan2(matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
      dFloat picth1 = dAtan2(matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);

      dFloat roll0 = dAtan2(matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
      dFloat roll1 = dAtan2(matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);

      if (yaw1 > 3.141592f) {
         yaw1 -= 2.0f * 3.141592f;
      }

      euler0[a0] = picth0;
      euler0[a1] = yaw0;
      euler0[a2] = roll0;

      euler1[a0] = picth1;
      euler1[a1] = yaw1;
      euler1[a2] = roll1;
   }
   euler0[3] = dFloat(0.0f);
   euler1[3] = dFloat(0.0f);

   #ifdef _DEBUG
      if (order == m_pitchYawRoll) {
         dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix(euler0[1]) * dRollMatrix(euler0[2]));
         dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix(euler1[1]) * dRollMatrix(euler1[2]));
         for (int i = 0; i < 3; i ++) {
            for (int j = 0; j < 3; j ++) {
               dFloat error = dAbs (m0[i][j] - matrix[i][j]);
               dAssert (error < 5.0e-2f);
               error = dAbs (m1[i][j] - matrix[i][j]);
               dAssert (error < 5.0e-2f);
            }
         }
      }
   #endif
}


[misho]

Thanks again! Is there a dQuaternion::GetEulerAngles version? I think you mentioned it in the previous post.

And yes - I don't have to tell you how many permutations and combinations of ordering I tried! Trial and error seems to consume most of my dev time. I'm slowly getting there.

As for the order issue - I found through my extensive testing that the ordering does NOT matter to flight simulator itself when I pass the (roll,pitch,yaw) values through to my simulator call - they are passed all together, in a struct of 3 doubles. Where the order does manifest is, when I manually rotate my object in flight simulator (I have keys set up that control values of roll, pitch and yaw). I get the proper attitude only when I first rotate in Heading (yaw), then in pitch, then in roll (which is the order specified in flight simulator's documentation). Any other order of rotation and the object's rotation gets out of whack. The (Heading,pitch,roll) sequence is the only one that correctly moves the principal axes along with rotations and rotates the object correctly, but again, that is an internal simulator operation over which I don't have any control whatsoever. All I need is to make sure I pass the correct RPY values into the simulator.

Which is where I am at now. Basically, to test the handedness and angle limits, I have set up my code to set RPY angles to a Newton body (and it's quaternion rotation), which then gets transferred back to euler angles and applied to my simulator object. This is what my code looks like:

Code: Select all

   // for testing purposes, PBH values are set through keyboard inputs
   dQuaternion QX(dVector(1.0f, 0.0f, 0.0f), (float)Attitude->Pitch);
   dQuaternion QY(dVector(0.0f, 1.0f, 0.0f), (float)Attitude->Bank);
   dQuaternion QZ(dVector(0.0f, 0.0f, 1.0f), (float)Attitude->Heading);

   // Zero out the quaternion
   ent->m_curRotation = dQuaternion();

   ent->m_curRotation = ent->m_curRotation*QX;
   ent->m_curRotation = ent->m_curRotation*QY;
   ent->m_curRotation = ent->m_curRotation*QZ;

and then, I convert back to Euler, in order to get values usable by my flight simulator client:

Code: Select all

   dVector dvEuler;
   dvEuler =  ent->m_curRotation.GetXYZ_EulerAngles(); // returns pitch, roll (bank), and yaw (heading)

   // And set it to the return structure:
   Attitude->Pitch =   dvEuler.m_x;
   Attitude->Bank =   dvEuler.m_y;
   Attitude->Heading =   dvEuler.m_z;

The above code works well as far as getting back the same RPY values I entered, except for the problems of gimbal lock and angle ranges, which I'm hoping to solve through this discussion . Important to note here is that the "old" GetXYZ_EulerAngles() method expects the one and only one rotation sequence order, which I applied using QX,QY and QZ quaternion rotation sequence. If I use quaternion2Euler() method from that link I sent you, I can apply QX,QY and QZ in any order, and then specify the reverse order "zyx" as the order parameter in quaternion2Euler() call, and I would get the correct result. There are , as you mentioned, 6 ways of specifying the rotation order, and they all yield correct results if they are "unwrapped" in reverse of the order applied.

It is important to note that the first half of the code above is for testing purposes only. Newton body will be acted upon only by external forces (and not by directly applying the values of pitch, bank and heading), and the resulting attitude changes will be converted into Euler angles for input into the simulator display layer.

Yes, you are correct, I am the guy that is using spherical coordinate system That part (position conversion between spherical and cartesian) was relatively easy to implement, and that's working without any problems. The part I was stuck on (back in April, I think) was the attitude conversion between Earth based and Celestial based reference frames. Since then, I've figured that part out and coded functions that convert attitude angles between the two reference frames (called ECEF and NED, but that's a whole another topic ). However, to properly test these, I need my attitude algorithms to be bulletproof and work absolutely correctly, and that's where I'm at now. Other things that complicate matters a bit (but not insurmountable) are that Newton is a Left-Handed coordinate system, and my flight simulator client is Right-Handed. Also, to answer your question, no, I have no way to hack into my graphics code, there is no source code to recompile the libs, and hex hacking would be way beyond my abilities, if possible at all. All I have is one lousy function call to change object attitude, and all it takes are RPY values.

Sorry for this being so verbose, but I feel I'm getting ahead with this project and I sure do appreciate your attention and help on it!

Misho

[JoeJ]

Is there a dQuaternion::GetEulerAngles version?

Yes, but it converts quat to matrix first, so you can use BodyGetMatrix() directly instead.
Orientation from body quaternion and matrix is always equal.

Code: Select all

void dQuaternion::GetEulerAngles(dVector& euler1, dVector& euler2, dEulerAngleOrder order) const
{
   dMatrix matrix (*this, dVector (0.0f,0.0f,0.0f,1.0f));
    matrix.GetEulerAngles (euler1, euler2, order);
}


The above code works well as far as getting back the same RPY values I entered, except for the problems of gimbal lock and angle ranges

So you see the gimbal lock problem only, because you manually enter angles by keyboard.
This is normal because it's difficult for a human to get a desiered rotation from editing euler angles.

But your goal is to get the orientation from a newton body and display the same orientation for the graphics, right? (important question to be sure i do not misunderstand you).
I assume what you want is:

Code: Select all

void RenderShip (ship)
{
NewtonBody newtonBody = ship.body;
vec angles = GetEulerAngles (newtonBody, referenceFrame);
vec pos = GetSphericalPos (newtonBody, referenceFrame);
Renderer.SetTransform (angles, pos);
Renderer.RenderModel (ship);
}


I have no way to hack into my graphics code

A workaround would be to setup a way to do debug rendering. Something like this:

Code: Select all

void RenderPoint (vec pos)
{
pos = GetSphericalPos (pos, referenceFrame);
Renderer.RenderPoint (pos);
// or, if it has no point rendering:
Renderer.SetTransform (zeroAngles, pos);
Renderer.RenderModel (tinySphereModel);
}

// then, when rendering a spaceship as usual...

RenderShip (myTestShip);

// ...add some debug visuals:

Matrix m = newtonBodyGetMatrix (myTestShip.body);
float approxShipSize = 50;
vec center = m.position;
vec leftWing = center + m.left.scale(approxShipSize);
vec rightWing = center + m.left.scale(-approxShipSize);
vec nose = center + m.front.scale(approxShipSize);
vec up = center + m.up.scale(approxShipSize);

RenderPoint (center );
RenderPoint (leftWing);
RenderPoint (rightWing);
RenderPoint (nose);
RenderPoint (up);



Then when the spaceship flys, the debug visuals must follow exactly its orientation.
If they drift appart, you know something is wrong.
It should be easier to find the reason when you have visual feedback.

The trick is that debug visuals do not need to have euler angles conversation working because they get their position from the newton body alone and thus show the real orientation of the simulation.
They do anly need the spherical coords conversation working correctly.

[misho]

To answer your question, yes, your RenderShip pseudocode is correct. I am getting rotations stored in the newton bodies and applying them to my space objects (be it a user-controlled spaceship or a space station or whatever else). Newton bodies will NOT be controlled by direct attitude input, instead I will set up force points (attitude manoeuvring thrusters) and let newton take care of attitude changes.

Your graphics `hack`mode sounds like a great idea, thanks! I have no way of rendering pixels and lines, but I can certainly insert small 3D spheres onto locations and see how they behave compared to the spaceship. That`s a great way of checking the logic.

thanks for the Euler equation, I`ll implement it first, then I`ll start inserting my NED to ECEF computations to see what I`m getting out of it. I`ll report back as soon as I have results.

thanks
Misho

[misho]

Hi JoeJ!

Ok - I have upgraded my project to work with the latest Newton library, including the double precision support. Last time we have chatted about this, I was going to implement debug objects to help me figure out what is going on with my rotations... which I have now done so. I tested it - and my rotations are all over the place! To clarify, the debug objects (colored spheres) seem to be behaving correctly, but the spaceship is not following them, and this, I presume has to do with euler angle determination. I can post a few screenshots, or even a movie, but I have a few questions first.

I am rotating my spaceship by introducing torque on principal axes. To do this, I use keyboard keys to introduce torque force on each axis (positive or negative, so 6 key combinations), and store this in dTorque vector. I apply this torque inside my ApplyForceAndTorqueCallback, as such:

Code: Select all

void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{

...

// get the rotation quarternion of the body
// and apply thruster torques on the rotated principal axes
   dQuaternion rotation;
   NewtonBodyGetRotation(body, &rotation.m_q0);
   dTorque = rotation.RotateVector(&dTorque[0]);
   NewtonBodyAddTorque( body, &dTorque[0]);

...

}


Am I doing this correctly? I can see the spheres behaving somewhat correctly, except sometimes there is a small wobble at the beginning. I was under assumption that the torque applied would be on the principal axis of the body, no matter how rotated it is. Instead, the NewtonBodyAddTorque adds torques on the world axis, so I have to perform the rotation myself (and it seems to be working).

Thanks,
Misho

[Julio Jerez]

applying torque to the principals axis is not the correct thing to do.

why don't you use the joint CustomKinematicController
you just attach that joint to your body, and then you make a pitch Yea Roll Matrix for the desire angle of your station, and the joint will calculate the precise torques that need to apply.

you control will only nee to control the Pitch Yaw and Roll angle as three independent variables an you will ne get gimbal.

not only you can control the orientation by you can also control the position as well.

[JoeJ]

The code seems correct assuming dTorque initially contains a copy of your local torque,
something like:

Code: Select all

void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{
...
dVector dTorque = rotation.RotateVector(&mySpaceship->controlTorque[0]); // make a copy to ensure original data is not corrupted
...


applying torque to the principals axis is not the correct thing to do.

Why not? Multiple thrusters can be controlled so that their linear aceleration cancels out each other and only angular acceleration remains.

why don't you use the joint CustomKinematicController

For a spaceship? I'd use this for Pacman motion when we want constant velocity,
but for a spaciship using single torque and force would be the first thing coming to my mind.

I'm confused

[Julio Jerez]

I agree 100% with joe.
the most correct, easier and even the most realistic way is to attach thrusters to some fix point on the space station.

them you stop the rotation you can use inertial dampers. this could be something as simple as:
1-mutioplying the angular velocity by a value small than 1.0 in force and torque callback.
2-adding a more correct viscous damper torque, T = - omega.Scale (k)
3-appling a dry friction joint that apply a friction torque that remove angular momentum linearly, and stops the station dead below some threshold.

[misho]

hi guys, and thanks for your input!

The code seems correct assuming dTorque initially contains a copy of your local torque

Actually - I completely CLEAR OUT the dTorque from its values: (I should have included this in my original snippet)

Code: Select all

void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{

...

// get the rotation quarternion of the body
// and apply thruster torques on the rotated principal axes
   dQuaternion rotation;
   NewtonBodyGetRotation(body, &rotation.m_q0);
   dTorque = rotation.RotateVector(&dTorque[0]);
   NewtonBodyAddTorque( body, &dTorque[0]);
   dTorque = dVector(0.0f, 0.0f, 0.0f, 1.0f);

...

}

The reason is, if I didn't clear it, the torque would continue to get applied even after I let the thruster key go... which is not the behaviour I want. The dTorque vector, containing axis torques, contains value(s) as long as I hold the thruster key(s), therefore, it needs to be cleared every cycle, and re-populated if the thruster key(s) are active. The torque amount is constant (not additive), but the effect on the rotation is cumulative: the longer I apply a constant torque, the faster the ship spins, which is the effect I want. The omega (rotation) of course, stays constant if I let go of the thruster. Makes sense?

As for the other comments - the CORRECT thing to do (and, of course, that's what spacecraft do) is to implement a thruster system consisting of multiple thrusters arranged so that firing them in different combinations would produce rotations in all of the principal axis. That's what Apollo and Space Shuttle (among others) had. The PROBLEM I have is, Newton does not provide functionality to apply force to a body on any other point except its centre of mass. (Unless this is what Julio was refering to when he suggested the use of CustomKinematicController, with which I am unfamiliar).

In physics, applying a force on object at a point other than its centre of mass is the same as applying force AT the centre of mass, PLUS, additional torque resultant from a moment gained from applying force at a certain distance from centre of mass. I could set up a procedure that would calculate this "offset force" (and I still may opt to do this) However, each thruster on a real spacecraft has its opposite thruster whose purpose is to cancel out the force of the other thruster, and result in pure rotation around its principal axis: The Yaw-Pitch-Roll control system. So - in practice, I am ok with applying pure torques on principal axis, for now.

Misho

[Julio Jerez]

The PROBLEM I have is, Newton does not provide functionality to apply force to a body on any other point except its centre of mass

yes it does, ther are ton of examples ho wo do that.

you set the point and the direction where the thruster apply the force.
In you call back you do.

Code: Select all

force = CalculateGravitationalForce
torque = vector (0)
the for each thruster i
  F = thrusterDir(I)Scale (thrustForce(i))
  r = VehicleMatrix.Rotate (trusterPivot)
  T = crossPruduct (r, f)
  force += F
  torque += T

// here you can add the inertia dampening, in the force of torque,
turque -= vehicleOmega.Scale (someFrag);
 
NewtonAddForce(force)
NewtonAddTorque(torque)

The reason I mentioned the CustomKinematicController is because in space controlling a space craft with torques is done the way you say describe, but is harder than you think.
It was actually one of the harder problem the Apollo mission designed had. The computer were not fast enough to control the space craft attitude. because in space there is not drag.

These days the way satellite control the attitude is by having an array of Gyroscopes that apply pure torque, but that is a harder to simulate problem.
I can help you to do that control style if you like, and you get a remarkably stable control, because by using Gyros you can get direct conservative torques.
I would try the thruster first. But that would be 70's and 80's technology.

[misho]

yes it does, ther are ton of examples ho wo do that.

thanks Julio - what I meant to say is, there is no direct way Newton does that, for example:

Code: Select all

NewtonBodyAddForceAtPoint(body ..., force ... , point ...)

So, I would, as I was mentioning, write my own code to implement "offset force" (force-torque) system on body, which is precisely what you did - thanks!

The reason I mentioned the CustomKinematicController is because in space controlling a space craft with torques is doe eh way you say describe, but is harder that you think.

That was actually one of the harder problem the Apollo mention had, the computer were no fast enough to control the space craft attitude. because in space there is not drag.

These day they the way satellite control the attitude is by having an array of Gyroscopes that apply pure torque, but that is a harder to solve problem.

you can ado that way if you like, and you get a reparably stable object, because by use Gyros you can get conservative torque.
I would try the thruster first. Bu that would be 70 and 80 technology.

Glad to see you're familiar with spacecraft dynamics! Yes indeed, the satellite gyro setup is called CMG (control moment gyro) and the setup I have right now mimics CMG functionality. However - the thrusters are still used today, especially on multi-module configurations. The spacecraft I am modelling here is NASA Orion, and it consists of 2 parts - capsule Orion and Service module. in this case, CMG (in order to function efficiently) would have to be located at the centre of mass of the whole assembly, which is approximately where the Orion thermal shield is. Also for Apollo, if they used CMG, it would have to be located around where the docking tunnel was between LEM and command module - also impractical. Then, when LEM separated, it would have to use its own CMG. CMG works for a single unit satellites, but multi-module craft still have to rely on thrusters

Misho

[misho]

So - just to illustrate the results which I am getting with my "debug" setup... and, to be clear, I am now seeing correct results, PLUS, I have not observed gimbal lock . This is a sequence of rotations around principal axis of the newton body (disregard the attitude of the actual spaceship, it is all over the place ):

• The spacecraft is exactly above the north pole - this is where Newton and Visual system's coordinate systems are coincident, so for now I don't have to worry about conversion between the two.
• The spheres represent the axis in the newton body: +X is yellow, +Y is red, and +Z is blue.
• The sphere's POSITION is calculated according to the newton body attitude, in other words, I am using spheres to see if newton body attitude is behaving correctly.
• The spaceship is a separate object, attached to the newton body. Its attitude is set by newton body, but the visual system accepts attitude parameters which are earth locale-based, and I am trying to solve for the conversion

The attitude of the spacecraft is all over the place, but the first step, (to check if newton body is responding correctly to torque inputs) is behaving correctly and is validated. The spacecraft's attitude is possible to set only using Euler angles (not quaternions), and now, the next step is to determine what's going on with that, and the right sequence of Euler angles.

Misho