Gimbal Lock and Euler angles

[Julio Jerez]

that looks pretty good!
can you show me the code fragment you are using t control the station?

Also for the record, an Gyroscope control system does no has to be located on any particular place.
That's the beauty of Gyros, that generate pure torque.

But I do agree large for an heavy short time in space stations, the mass of the rotor would have to be too large to be effective and that's a problem in payload weight, and also in free space inside the station. Thruster would be more efficient.

[misho]

that looks pretty good!

Thanks Julio! the visual system I'm using is very capable - here are a few more hi-res screenshots for you here and here

can you show me the code fragment you are using t control the station?

Do you mean force/torque code? sure - I posted it in one of the previous threads, but here it is again:

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);

...

}

cheers,
Misho

[Julio Jerez]

I mean the code that you use to apply torque generated by thruster attached at some locations.

Those code does not look right at all.

Code: Select all

dQuaternion rotation;
   NewtonBodyGetRotation(body, &rotation.m_q0);
   dTorque = rotation.RotateVector(&dTorque[0]);
   NewtonBodyAddTorque( body, &dTorque[0]);

you are getting the rotation matrix and you are rotation a torque generated at some local space,
which I assume you produce form local space and you apply the to the body.
But that is very, very wrong.

The only thing that that will do right is that it will rotate the body but that all. once the body is rotating the body will try to conserve angular momentum. so when you apply torque is the effect is very minimal. in fact you can apply a torque along X and the body will spin around Y, pf z or a combination of the two axis
Ironically that code is similar to what a Gyroscopic actuator will do.

you need to apply individual torques at individual points. each one independently do no try to use just one torque made out of three local torques in local space that will never do what you expect.

[misho]

You misunderstood me - I WANT to implement thrusters, but for now, I am using pure torques I don't have the thruster method in place (yet), but I do want to implement it properly.

As far as angular damping, I did set the coefficient to 0, to have it spin "forever"... And that works (although at very small omegas the omega slowly goes to 0)

Code: Select all

NewtonBodySetAngularDamping( eObject->nBody, &angularDampingCoefficient);

That being said, is my code correct, as far as applying pure torques on the body's principal axes? It certainly behaves correctly...

Misho

[Julio Jerez]

Believe me is not right. This is counter intuitive by apply torque is local space is what is getting you what you call the Gimbal problem.
Is not a gimbal problem, you are missing the fact the toque is rotating with the frame therefore is subject to that angular velocity derivatives. The newton engine is design to work in inertial frame.
This means that the frame cannot be rotating.

there are not expression to convert a torque form a rotating frame to an inertial frame, because torque is not really a physical quantity. you have to express torques a posit Cross force
and the rate derivative will ve will be the d ( r x f) / dt
that expression generates some estrange term that act as extra torques conpesationg for the errors.

when the body is rotating the angular momentum becomes an important part of the equations of motion. This is why is better to apply indidural forces each on acting at its own point of action.

As the body sping the point of action also sping and the force spins.

The equation of motion for a rotating body or quite tricky to derive.
https://en.wikipedia.org/wiki/Rotating_reference_frame.

but you just apply the force the way I mention you avoid those issue.

If you have still have problems after using the thrusters method, this Sunday I can write a force and torque call back for you to use.

[JoeJ]

You should also post your code on how you try to get the euler angles to feed the visuals.
(Do not hide the most interesting things with "...")

Edit: Also the code that sets the visuals position (spherical coords).
Even if it works it could be a good reference.

[misho]

You should also post your code on how you try to get the euler angles to feed the visuals.
(Do not hide the most interesting things with "...")

By all means, if it can be of use to anyone else!!

Code: Select all

void GetObjectAttitudeForSIM(structAttitude *Attitude, int index)
{
   TBEntity *ent;

   ent = g_sceneManager->GetEntity(index);

   dVector dvEuler1;
   dVector dvEuler2;

   ent->m_curRotation.GetEulerAngles(dvEuler1, dvEuler2, YRP);

   Attitude->Pitch =    dvEuler1.m_x;
   Attitude->Bank =    dvEuler1.m_y;
   Attitude->Heading = dvEuler1.m_z;
}

I have added 4 additional ( + 2 I just renamed) Euler Angle Order definitions in dMathDefines.h:

Code: Select all

enum dEulerAngleOrder
{
   m_pitchYawRoll = (0 << 8) + (1 << 4) + (2 << 0),
   m_rollYawpitch = (2 << 8) + (1 << 4) + (0 << 0),
   PYR = (0 << 8) + (1 << 4) + (2 << 0),
   PRY = (0 << 8) + (2 << 4) + (1 << 0),
   RYP = (2 << 8) + (1 << 4) + (0 << 0),
   RPY = (2 << 8) + (0 << 4) + (1 << 0),
   YPR = (1 << 8) + (0 << 4) + (2 << 0),
   YRP = (1 << 8) + (2 << 4) + (0 << 0)   
};


Edit: Also the code that sets the visuals position (spherical coords).
Even if it works it could be a good reference.

Here it is! As you can see, pretty much your code

Code: Select all

void RotateDebugObjects()
{
   TBEntity *ent;
   dMatrix bodyMatrix;
   ent = g_sceneManager->GetEntity(0); // get user object entity

   NewtonBodyGetMatrix(ent->nBody, &bodyMatrix[0][0]); // get entity body matrix

   float approxShipSize = 10.0;

   dVector center =   bodyMatrix.m_posit;
   dVector leftWing =   center + bodyMatrix.m_right.Scale(-approxShipSize);
   dVector rightWing = center + bodyMatrix.m_right.Scale(approxShipSize);
   dVector nose =      center + bodyMatrix.m_front.Scale(approxShipSize);
   dVector up =      center + bodyMatrix.m_up.Scale(approxShipSize);

   ent = g_sceneManager->GetEntity(1);
   ent->m_curPosition = rightWing;

   ent = g_sceneManager->GetEntity(2);
   ent->m_curPosition = nose;

   ent = g_sceneManager->GetEntity(3);
   ent->m_curPosition = up;
}


[JoeJ]

I see you don't use spherical coords as i have expected.
Let's ignore rotations for now and focus on position only.

My assumption (from the older thread) is:
Your renderer requires spherical coords (latidude, longitude and height) relative to earth.
Newton uses cartesian coords, so you need to convert.

Is this true?
If so, a newton object travelling a straight line through space would be visualized like orbiting earth,
and all your visuals including debug display would be wrong.

[misho]

Hi!

I see you don't use spherical coords as i have expected.
Let's ignore rotations for now and focus on position only.

I do use spherical coordinates, and the conversion happens later, when they are about to be submitted to display system. I keep all my calculations in Cartesian. All my newton bodies (in my screenshot, spacecraft and 3 spheres) only have Cartesian coords.

My assumption (from the older thread) is:
Your renderer requires spherical coords (latidude, longitude and height) relative to earth.
Newton uses cartesian coords, so you need to convert.

Is this true?
If so, a newton object travelling a straight line through space would be visualized like orbiting earth,
and all your visuals including debug display would be wrong.

Yes - I use cartesian within Newton until time comes to send them to the display engine, and then I convert them to (lat lon alt). Here's the conversion code:

Code: Select all

inline void ConvertXYZtoLatLonAlt(structXYZ *xyzPos, structPosition *latlonaltPos)
{
   latlonaltPos->SpacecraftAltitude = sqrt(xyzPos->x*xyzPos->x + xyzPos->y*xyzPos->y + xyzPos->z*xyzPos->z) - EARTH_RAD;
   latlonaltPos->SpacecraftLatitude = (M_PI/2) - atan2(sqrt(xyzPos->x*xyzPos->x + xyzPos->y*xyzPos->y), xyzPos->z);
   latlonaltPos->SpacecraftLongitude = atan2(xyzPos->x,xyzPos->y);
}

I have tested this and it's working well. I'm not sure what you mean by visuals being wrong? Just to make it clear, the orbital engine is in place - I have a force vector set up so that, wherever the Newton body is, it always points towards the centre of the Earth (Cartesian origin). Usually, in Newton examples, the gravity force vector is always in the -Z direction, since (obviously) the stage is localized to a small "game level", not to the whole earth as in my case. When I introduce newton body, I give it a precise initial orbital velocity, and the objects stay in stable orbits. I have experimented with smaller and larger velocities, and objects (as expected) start falling towards earth or go into elliptical orbits.

[JoeJ]

Ah, ok - then we can assume the positions and debug visuals are correct and continue on orientation

One thing is - you are using position over a pole because newton and visuals space seems to mach up.
How can you be sure about this?
The problem i see is, exactly over a pole the visual space orienation is undefined because we can only bulid an up vector there, but tangents are undefined and the renderer will use random tangents we don't know.
You would need to keep a bit of distance to the pole axis to avoid that.


However, i assume finaly you need to create a reference orientation from the ship position and measure euler angles in relation to that.
The reference should be constructed likely this way:

HeightAxis = (shipPos - earthCenter).Normalized()
LatitudeAxis = HeightAxis.Cross(earthPoleAxis).Normalized()
LongitudeAxis = LatitudeAxis.Cross(HeightAxis)

Dou you agree?
From those axis you can build the reference orientation matrix, calculate the rotation from ship to refernce, and get the final eulers from the rotation.

For the first step i would use your debug visuals to show those 3 reference axis instead of the ship orientation.
As the ship orbits earth, you can check if the vectors keep properly aligned to erths lat/long grid,
and the flipping cases crossing a pole should also show up.

When this works, i'd adjust the signs of the lot/long axis so they point towards increasing angles, assuming that's the proper convention.
But more important, make sure the reference system has the same handness as the ship orientation, otherwise euler angles will be garbage.


EDIT:
I'm not sure im right with my 'reference orientation relative to earth' idea.
The problem is, terms like Yaw, pitch & roll make sense in relation to an airplane,
but do they make sense in relation to earths lat/long grid?
Are there any specs from your visual system explaining the relations of the expected angles?

[misho]

Believe me is not right. This is counter intuitive by apply torque is local space is what is getting you what you call the Gimbal problem.
Is not a gimbal problem, you are missing the fact the toque is rotating with the frame therefore is subject to that angular velocity derivatives. The newton engine is design to work in inertial frame.
This means that the frame cannot be rotating.

Julio, I trust you 110% on this, you are the authority on your own Physics engine! I have a mechanical engineering background and I'm self-taught programmer, so I am doing the best I can and I certainly appreciate all the help you guys are providing, and obviously, I am open to being corrected and taught a thing or two All I see is that the method I am using (applying torques on rotated principal axis of the body) is giving me results I expect, so I assume that's correct... But definitely, if you point me to the proper method, I will apply it.

there are not expression to convert a torque form a rotating frame to an inertial frame, because torque is not really a physical quantity. you have to express torques a posit Cross force
and the rate derivative will ve will be the d ( r x f) / dt
that expression generates some estrange term that act as extra torques conpesationg for the errors.

when the body is rotating the angular momentum becomes an important part of the equations of motion. This is why is better to apply indidural forces each on acting at its own point of action.

As the body sping the point of action also sping and the force spins.

Right - however - I was applying torques as the body spins, as well. I calculated new orientation vector of the body in each cycle and applied torques to the new principal axis. I thought that was sufficient. But if, as you say, this is not the way to go, I agree

The equation of motion for a rotating body or quite tricky to derive.
https://en.wikipedia.org/wiki/Rotating_reference_frame.

but you just apply the force the way I mention you avoid those issue.

If you have still have problems after using the thrusters method, this Sunday I can write a force and torque call back for you to use.

I would be grateful if you did! Would that become a part of the Newton library, or would you write a stand alone code applicable to my case only?

grateful,
Misho

[Julio Jerez]

no that would not be part of newton. I just write a function and place of the PhysicsUtil.
it will be something like Celestial Dynamics Motion.

basically you problem is a typical vehicle dynamics when a pilot try to control a flying vehicle from the vehicle, just a little more complex than a airplane because it nee to consider the gravity with at the elevation become a variable, rather than contact.
It is actually a very simple example Generalized coordinate Langragian equations for a rigid body motion.
I assume you vehicle use as center the center of the earth? of is that wrong?

if you vehicle fry so far form the earth gravity that is get affect by the gravity of other planet then it become more complex because it need to use an sun center system. and is become a complex system of equation.
and if flight outside solar system that is even more complex because if nee to use the center of the galaxy and so on.
Beyond that is a excremental complex because it require Hamiltonian relativistic momentum based formulation with is outside Newton scope.
I believe that even outside the solar system is outside the scope of newton.

It is a long time I do no do that kind of derivation since I took analytical mechanics classes, but is is quite possible and an interesting problem to simulate.

Please tell me what is you origin of the universe, the earth or the sun.

[misho]

no that would not be part of newton. I just write a function and place of the PhysicsUtil.
it will be something like Celestial Dynamics Motion.

basically you problem is a typical vehicle dynamics when a pilot try to control a flying vehicle from the vehicle, just a little more complex than a airplane because it nee to consider the gravity with at the elevation become a variable, rather than contact.
It is actually a very simple example Generalized coordinate Langragian equations for a rigid body motion.
I assume you vehicle use as center the center of the earth? of is that wrong?

if you vehicle fry so far form the earth gravity that is get affect by the gravity of other planet then it become more complex because it need to use an sun center system. and is become a complex system of equation.
and if flight outside solar system that is even more complex because if nee to use the center of the galaxy and so on.
Beyond that is a excremental complex because it require Hamiltonian relativistic momentum based formulation with is outside Newton scope.
I believe that even outside the solar system is outside the scope of newton.

It is a long time I do no do that kind of derivation since I took analytical mechanics classes, but is is quite possible and an interesting problem to simulate.

Please tell me what is you origin of the universe, the earth or the sun.

Wow - celestial dynamics utility would be just awesome!! And you are correct, centre of my "universe" is Earth, and my visual rendering engine allows me to go out only to around a geosynchronous orbit, around 40,000km... (unfortunately). I am thinking of "fudging" a trip from Earth to Mars by calculating earth exit point and Mars entry point, and just switching Earth environment visuals with Mars visuals...(basically, telling user that the actual transit has been skipped). But, that's an entirely different topic!

FYI, here is my code snippet that sets up a gravity vector towards the cartesian centre, no matter where the body is. As you can see, I am using the proper gravity formula which is a variable, dependant on altitude (radius squared). I have this working well and I'm observing proper orbits. There are also thruster forces for translation and rotation (attitude) using torques, as we discussed before, the method you explained won't work...

Code: Select all

// callback to apply external forces to body
void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{
   dFloat Ixx;
   dFloat Iyy;
   dFloat Izz;
   dFloat mass;

   dMatrix matrix;

   // get the Body Matrix;
   NewtonBodyGetMatrix (body, &matrix[0][0]);

   NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

   //Set the gravity to always point to origin. Need to normalize this vector.


   double toOrigin[3] = {matrix.m_posit.m_x, matrix.m_posit.m_y, matrix.m_posit.m_z};
   normalize(toOrigin); // this gives a vector pointing to origin, of unit length.

   fRadius = sqrt( matrix.m_posit.m_x*matrix.m_posit.m_x + matrix.m_posit.m_y*matrix.m_posit.m_y + matrix.m_posit.m_z*matrix.m_posit.m_z );

   dFloat dGravity = (dFloat)(-M_GM / (fRadius*fRadius));
   dVector gravityForce  (mass * dGravity * toOrigin[0], mass * dGravity * toOrigin[1], mass * dGravity * toOrigin[2], 1.0f);

   // Set the gravity force
   NewtonBodySetForce(body, &gravityForce[0]);   
   
   // Attitude and position thrusters section

   // get the rotation quarternion of the body, and apply thruster torques on the rotated principal axes
   dQuaternion rotation;
   NewtonBodyGetRotation(body, &rotation.m_q0);

   dPositionalThrust = rotation.RotateVector(&dPositionalThrust[0]);
   dTorque = rotation.RotateVector(&dTorque[0]);

   NewtonBodyAddForce( body, &dPositionalThrust[0]);
   NewtonBodyAddTorque( body, &dTorque[0]);

   dPositionalThrust = dVector(0.0f, 0.0f, 0.0f, 1.0f);
   dTorque = dVector(0.0f, 0.0f, 0.0f, 1.0f);

}

cheers,
Misho

[misho]

Ah, ok - then we can assume the positions and debug visuals are correct and continue on orientation

One thing is - you are using position over a pole because newton and visuals space seems to mach up.
How can you be sure about this?
The problem i see is, exactly over a pole the visual space orienation is undefined because we can only bulid an up vector there, but tangents are undefined and the renderer will use random tangents we don't know.
You would need to keep a bit of distance to the pole axis to avoid that.

You are correct - the camera system does act a little wonky when the object it is looking at is exactly above North Pole - but I think Simulator has some code internally that takes care of complete instability. But more to the point:

At this point, I am not dealing with visual (sim) space orientation. I am not using Newton's orientation to adjust Sim orientation. I am using Newton Orientation (which is stable) to calculate Debug Objects (3 spheres) positions, that indicate Newton Body's current orientation. True, I am using a conversion utility to convert from Cartesian To Polar, but that's just a relatively simple trig. For instance, I position Newton bodies 390km above north pole, at x,y,z (0,0,(390 + EARTH_RAD) km), and the conversion to spherical Lat,Lon,Alt returns (I am paraphrasing here) something like (90,0,390km). The debug objects around the Newton body are placed along its principal axes at 10m distance, so when converted, I get (for one of the spheres, just as an example) something like Lat,Lon,Alt (89.99341, 0.000234, 390000.012). As I rotate Newton body, the spheres change their Lat,Lon,Alt position, NOT their orientation.

But, Let me step back. Here is the list of the steps I am going through to address this problem:

1: Establish Newton Cartesian and Simulator Spherical coordinate system parity
2: Establish Newton rotation to Simulator Euler angle relationship
3: Convert attitudes from Newton (ECEF) frame of reference to Simulator (NED) frame of reference

I thought step #1 was established and that object placement (but NOT orientation) was at parity between Newton and my Simulator... however, if you think there might be a problem with my reasoning, please let me know if there is a test case I can use to verify.

However, i assume finaly you need to create a reference orientation from the ship position and measure euler angles in relation to that.
The reference should be constructed likely this way:

HeightAxis = (shipPos - earthCenter).Normalized()
LatitudeAxis = HeightAxis.Cross(earthPoleAxis).Normalized()
LongitudeAxis = LatitudeAxis.Cross(HeightAxis)

Dou you agree?
From those axis you can build the reference orientation matrix, calculate the rotation from ship to refernce, and get the final eulers from the rotation.

For the first step i would use your debug visuals to show those 3 reference axis instead of the ship orientation.
As the ship orbits earth, you can check if the vectors keep properly aligned to erths lat/long grid,
and the flipping cases crossing a pole should also show up.

When this works, i'd adjust the signs of the lot/long axis so they point towards increasing angles, assuming that's the proper convention.
But more important, make sure the reference system has the same handness as the ship orientation, otherwise euler angles will be garbage.


EDIT:
I'm not sure im right with my 'reference orientation relative to earth' idea.
The problem is, terms like Yaw, pitch & roll make sense in relation to an airplane,
but do they make sense in relation to earths lat/long grid?
Are there any specs from your visual system explaining the relations of the expected angles?

Well, yes, I think I know what you're getting at, and this is the gist of the whole problem, and has to do with step #3. Newton deals with a pure cartesian coordinate system. If you take Earth, make z-axis go through its poles, and have positive Y axis connect Earth center and intersection of equator and the prime meridian, that is called Earth-Centered, Earth-Fixed (ECEF) system. If you look at 2 airplanes, both with roll,pitch,yaw at 0,0,0 flying above the Earth surface, one above North pole, and another one flying above south pole, the airplane at south pole will look upside down.

However, to each pilot, airplane is in perfectly wing-level, nose level attitude. That's because they each have their own coordinate reference frame, which is oriented depending on where on hearth they are positioned. This coordinate reference frame is called NED (North-East-Down).

My Simulator is taking the attitude parameters ONLY in NED format. Newton, being a pure 3D cartesian coordinate system, is in ECEF format. My task is to convert from ECEF to NED, and feed NED values to my Simulator.

I am at stage #2, so I am completely staying away from worrying about converting the orientations from Newton to Sim (display engine). I actually have code for this conversion: I found a scientific paper on this here, (PDF download).

This is, in reality, a conversion commonly used in space dynamics applications. There is an explanation of the problem, with diagrams, of the conversion process. The two coordinate systems are, again, referred to as ECEF (earth centered, earth fixed) and NED (North-East-Down). I have taken the method described in this paper and wrote a procedure to convert from ECEF to NED (which I will need) and also, the other way around, from NED to ECEF. However, to test it, I need to make sure that my coordinate systems and orientations are set up as expected.

I also have a quite simple, bullet proof verification approach to this problem: Assuming a spacecraft with fixed attitude (that is, non rotating), is orbiting around the Earth:

NED mode (the mode Simulator is in): horizon (and Earth itself) will be fixed in relation to the spacecraft

ECEF mode: Stars will be fixed in relation to the spacecraft.

When I see stars not moving through the porthole of my spacecraft, whatever the spacecraft orbit is, I'll know I've performed ECEF->NED conversion correctly

Misho

[JoeJ]

Makes sense.

Code: Select all

ent->m_curRotation.GetEulerAngles(dvEuler1, dvEuler2, YRP);

   Attitude->Pitch =    dvEuler1.m_x;
   Attitude->Bank =    dvEuler1.m_y;
   Attitude->Heading = dvEuler1.m_z;


So did it work with any of the 2*6 possible combinations at this code?
If it does not, i suggest making a transposed copy of ent->m_curRotation and try again.

If it still does not work, you could try starting changing the signs of the axis in the matrix.

But if it still does not work, maybe the reason is a Intrinsic / Extrinsic mismatch,
see there: https://en.wikipedia.org/wiki/Euler_angles
I would not know how to deal with this