Gimbal Lock and Euler angles

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

Re: Gimbal Lock and Euler angles

Postby JoeJ » Thu Apr 14, 2016 4:20 am

No that makes no sense, i'll explain later.

Your initial code:
Code: Select all
TBEntity *ent;
ent = g_sceneManager->GetEntity(index);
dVector dvEuler1, dvEuler2;
dMatrix bMatrix, tMatrix;
NewtonBodyGetMatrix(ent->nBody, &bMatrix[0][0]);
tMatrix = bMatrix.Transpose();
tMatrix.GetEulerAngles(dvEuler1, dvEuler2, PYR);
dvEuler1 = dvEuler1.Scale(-1);
Attitude->Pitch =   dvEuler1.m_x;
Attitude->Bank =    dvEuler1.m_y;
Attitude->Heading = dvEuler1.m_z;

is correct (added the negation).

Because it still does not work i see following issues:
order PYR is wrong (need to try all 6 each time - can't trust any naming convention)
mapping Attitude to euler is wrong (verify by rotating only one axis)
Handness - this is the only thing i worry a bit, but i'm sure it's easy to fix

I suggest you do the following to reduce those cases.
Do some testing for each axis seperately, first only x, than y, than z.
Because this is order independent, angle order has no effect for those tests.
If the rotation axis don't match up, fix the mapping attitude to euler.
Note the matching of clockwise vs. counterclockwise - for how many axis does it match?
Try to fix this with negating individual angles after answering that question.
Finally, all axis should work and you test all possible 6 orders.

With some luck you may succed, otherwise we may need to change handness in the matrix befor euler calculation...
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Fri Apr 15, 2016 4:24 am

Ok - I tested all 6 modes, none of them work properly

Testing on single axis, all modes work, except on some axis I get reverse rotation, depending on the mode, as such:

PYR: X,Y reversed
PRY: Z reversed
RYP: Z reversed
RPY: X,Y reversed
YPR: Z reversed
YRP: X,Y reversed

So, I took YPR mode as a standard reference (and because my sim says that's the order it expects it in, but as you said, that can't be trusted) and I negated Attitude.Heading member:

Code: Select all
Attitude->Heading =   -dvEuler1.m_z;


I am now getting proper rotations (aligned with debug spheres) but only on SINGLE axis rotation. So - single axis rotation is now proper on 3 orders (PRY, RYP and YPR), and opposite on all 3 axis on the rest of 3 orders.

I still do not seem to have intrinsic Eulers, but I guess that's a good starting point to go from.
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby JoeJ » Fri Apr 15, 2016 6:04 am

That's expected, those results indicate it's mainly a handness issue now.

What happens if you negatge the Z axis of the matrix before euler convertion:
tMatrix[2] = tMatrix[2] .Scale(-1) ... guess it's a silly idea :oops:

A better idea is to negate z or (x&y) positions of your debug visuals and repeat the testing with that.

misho wrote:I still do not seem to have intrinsic Eulers

I don't worry about intrinsic / extrinsic eulers anymore, because you can now test both of them.
Is far as i understand the difference is rotating about local vs. global axis, and my code from yesterday proofs we can do both, i'll edd the xplantation by commenting it:

Code: Select all
    sVec3 eulers (0.2, 0.5, 1.1); // the inital eulers we want to calculate

 // rotations around every axis
          sMat3 rX = sMat3::rotationX (eulers[0]);
          sMat3 rY = sMat3::rotationY (eulers[1]);
          sMat3 rZ = sMat3::rotationZ (eulers[2]);

 // make a XYZ orderd sequence of rotations in LOCAL space
          sMat3 matrix1; matrix1.Identity();
          matrix1 = matrix1 * rX;
          matrix1 = matrix1 * rY;
          matrix1 = matrix1 * rZ;

 // make a XYZ orderd sequence of rotations in GLOBAL space
          sMat3 matrix2; matrix2.Identity();
          matrix2 = rX * matrix2;
          matrix2 = rY * matrix2;
          matrix2 = rZ * matrix2;

// at this point you would read newton body matrix,
// but we use our test matrices to see if we can calculate the initial angles

          sVec3 proof1 = matrix1.ToEuler (0x012); // this is wrong, we can not calculate local eulers
          sVec3 proof2 = matrix2.ToEuler (0x012); // this one is right, so our euler function assumes global rotations

          matrix1 = matrix1.Transposed(); // make the rotation the other way around
          proof1 = matrix1.ToEuler (0x012);
          proof1 *= -1.0f; // this one is now correct too, we can handle both local and global eulers



Here are some links with similar problems, they all seem to share the same confusion, but flipping signs of angles AND position seems to fix it:
http://forum.unity3d.com/threads/right-hand-to-left-handed-conversions.80679/
http://www.gamedev.net/topic/349800-convert-rotations-from-right-hand-to-left-hand/
http://stackoverflow.com/questions/31191752/right-handed-euler-angles-xyz-to-left-handed-euler-angles-xyz
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Fri Apr 15, 2016 2:26 pm

I had a wacky thought...

After checking the positions of the debug objects and determining that they are rock-solid as far as the orientation is concerned, I realized that they are also correcting my ECEF to NED conversion which I will have to deal with (I just checked and confirmed)... So, this begs a question:

Isn't it possible to align my sim body to the Newton body vectors constructed from those positional vectors that give debug objects their positions? As in, completely avoid using rotational matrices?

Misho
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby JoeJ » Fri Apr 15, 2016 3:45 pm

misho wrote:Isn't it possible to align my sim body to the Newton body vectors constructed from those positional vectors that give debug objects their positions? As in, completely avoid using rotational matrices?


So you want to calculate the euler angles from the positions of your debug spheres?
This is: Constructing positions from matrix, buliding orientation from positions, convert to euler.
Sorry, that just adds a unnecessary step as the matrix already contains orientation.

I'm almost sure the solution is to negate one dimension for position, and the other 2 dimensions for eulers,
or to negate 2 dimensions for position, and the remaining dimension for eulers.

I think one right spot to negate positions would be here:

Code: Select all
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);

rightWing[0] *= -1; // e.g. negating pos x & y
rightWing[1] *= -1;

nose [0] *= -1;
nose [1] *= -1;

up [0] *= -1;
up [1] *= -1;

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


I'd hope this, and YPR, and negating euler.z should work.
Did you try this already?
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Fri Apr 15, 2016 4:55 pm

JoeJ wrote:I'd hope this, and YPR, and negating euler.z should work.
Did you try this already?


Yes, I did - and sadly, it does not work. :roll: I have only implemented negating debug object positions, as in the code. YPR and Attitude negation I had in place since last night. One odd thing I noticed, while both X and Y objects flip to the other side, the Z object does not flip position no matter what I do, it keeps pointing up.

I have a readout of the Euler angles of pitch, yaw and roll that is calculated and is getting passed into the sim, and as soon as I see 2 angles changing while I'm rotating abut a single axis (which I am, since my debug objects are rotating properly), I know I have rotations applied on world axis, not object axis, which is exactly what Extrinsic and Intrinsic are. I can show you that in the screenshots...
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby JoeJ » Sat Apr 16, 2016 1:33 am

JoeJ wrote:I have a readout of the Euler angles of pitch, yaw and roll that is calculated and is getting passed into the sim, and as soon as I see 2 angles changing while I'm rotating abut a single axis (which I am, since my debug objects are rotating properly), I know I have rotations applied on world axis, not object axis, which is exactly what Extrinsic and Intrinsic are. I can show you that in the screenshots...


Yes, show the screenshot, but i think those 2 angle changes expected no matter of global or local.
Usually there is only ona axis that causing only one angle change.

Maybe you should try now to create the matrix from eulers as i suggested earlier:
Code: Select all
dVector eulers = read out from rederer
// do negations on eulers here

 // rotations around every axis
dMatirx r[3];
r[0] = dPitchMatrix (eulers[0]); // x
r[1] = dYawMatrix (eulers[1]); // y
r[2] = dRollMatrix (eulers[2]); // z

int eulerOrder = 0x102; // your YPR, where we can't be sure
dMatirx visMatrix = dGetIdentityMatrix();
// make euler sequence of rotations

if (usingGlobalVsLocal)
{
          visMatrix = visMatrix * r[(eulerOrder>>8)&3];
          visMatrix = visMatrix * r[(eulerOrder>>4)&3];
          visMatrix = visMatrix * r[(eulerOrder>>0)&3];
}
else
{
          visMatrix = r[(eulerOrder>>8)&3] * visMatrix;
          visMatrix = r[(eulerOrder>>4)&3] * visMatrix;
          visMatrix = r[(eulerOrder>>0)&3] * visMatrix;
}

// visualize the debug spheres using visMatrix and potential negations on position...


After some trial and error you really should be able to match ship and debug rotation.
If so, we now know anything to invert the process.


EDIT:
Idea: Make a spaceship model that contains the debug spheres, to immideately see the necessary sign changes on position without any rotation.
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Tue Apr 19, 2016 5:30 pm

Hi!

Took a break from this over the weekend. Looking at your latest suggested code, I'm not following:

What are the dPitchMatrix, dYaw... ? How to construct them?

Also, what are they doing with euler values? Pitch matrix rotated about x axis by euler[0] and so on?

thanks,
Misho
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby JoeJ » Wed Apr 20, 2016 1:18 am

misho wrote:What are the dPitchMatrix, dYaw... ? How to construct them?

Those functions ar in dMatrix.cpp
Code: Select all
dMatrix dPitchMatrix(dFloat ang)
{
   dFloat cosAng;
   dFloat sinAng;
   sinAng = dSin (ang);
   cosAng = dCos (ang);
   return dMatrix (dVector (1.0f,    0.0f,    0.0f, 0.0f),
               dVector (0.0f,  cosAng,  sinAng, 0.0f),
               dVector (0.0f, -sinAng,  cosAng, 0.0f),
               dVector (0.0f,    0.0f,    0.0f, 1.0f));

}

Looking at the code you see y&z axis are constructed from angle, thus we know the 'Pitch' matrix is a rotation around x axis.

misho wrote:Also, what are they doing with euler values? Pitch matrix rotated about x axis by euler[0] and so on?


Yes, they make a rotation around x,y&z axis about the corresponding euler angles.
visMatrix is finally made by multiplying those 3 rotations in given order.
That's the way to make the resulting orientation from euler angles.
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Sun Apr 24, 2016 5:02 pm

Thanks, as always.

JoeJ wrote:Those functions ar in dMatrix.cpp


Duh - I should have checked the class members :)

JoeJ wrote:
misho wrote:Also, what are they doing with euler values? Pitch matrix rotated about x axis by euler[0] and so on?


Yes, they make a rotation around x,y&z axis about the corresponding euler angles.
visMatrix is finally made by multiplying those 3 rotations in given order.
That's the way to make the resulting orientation from euler angles.



I am still struggling with this. I'll have another go at it this evening and I'll report findings.

Misho
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby misho » Mon Apr 25, 2016 3:24 am

Hi Julio,

Julio Jerez wrote: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.


Is it possible for you to write the force and torque callback you were mentioning? I am still working on the proper rotation order for my space simulator, and I just want to make sure I am not observing any undesirable effects from applying torques on principal local axis.

Just to recap: I am pitching/rolling/yawing my spacecraft by applying torques on local principal axis of the body in space, but instead, I'd like to implement a "force at a point method", as used in spacecraft (thrusters)

thanks,
Misho
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby Julio Jerez » Mon Apr 25, 2016 1:34 pm

Ok, I have being busy but I can spend some time and add that function.

Basically what you want is a force and torque call back to apply force an torque to a body that is moving on a non inertial rotating frame. does that describe the problem?

misho wrote:Just to recap: I am pitching/rolling/yawing my spacecraft by applying torques on local principal axis of the body in space, but instead, I'd like to implement a "force at a point method", as used in spacecraft (thrusters)

The part that got me confused is that why did not get good results by applying thrusters at some fix point. That method always works, and you do not have to worry about angle order.
did you try that?

why do you want to control the attitude with one torques as if you were using a Gyroscopic.
how are those torque being produced is where you problem relies. adding torques is not a linear operation.
Torques are the product of two quantities, line of axiom and force vector, so when you add them the operation is non linear, plus also adding a torque to a rotation body, does not always do the effect you expect because of conservation of momentum, a spinning body wants to conserve its momentum, so when you add a torque the result is the sum of the angular momentum the body has plus the torque time the time step.
usually the product torque time step is smaller compared to the angular momentum so the torque does not do what seems intuitive to you.

this is why torque control is complex and counter intuitive, on the other hand using thrusters is always intuitive because if when you apply a push at one point you expect the body to start rotating on the perpendicular axis, if does not you can always relate the magnitude of the push force to the rotation.
Julio Jerez
Moderator
Moderator
 
Posts: 12426
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Gimbal Lock and Euler angles

Postby misho » Mon Apr 25, 2016 3:13 pm

Hi Julio!

Julio Jerez wrote:Basically what you want is a force and torque call back to apply force an torque to a body that is moving on a non inertial rotating frame. does that describe the problem?


Yes, that is it in a nutshell. As far as non-inertial frame, I am unsure what precisely you refer to, so let me describe my setup:

  • my bodies are moving in an orbit around earth, in a gravitational field, and wherever the body is, a gravitational force vector that acts on the body points to the centre of Earth (and centre of coordinate system).
  • At the start of the simulation, I give the body an initial velocity which I calculate to be exactly an orbital velocity at that altitude.
  • The centripetal force produced by the angular velocity cancels out the gravitational force, and this puts the body in a perfect circular and stable orbit.
  • If I give it less velocity, the object falls back to the earth, and if I give it more, it goes into elliptical orbit (or, even more, it escapes out of orbit, so called escape velocity) Unless I am missing something, this is a pure case of rigid-body dynamics :roll:

Now, I need to change the attitude of my rigid body (spacecraft) to point it in different directions. To do that, I wanted to use a thruster setup, where force is being applied at different points on the spacecraft. However, I didn't find a method to apply a force at some distance (there is only a method to apply force at the centre of gravity of the object). So, instead, I:

  • Use 6 keys on the keyboard to apply +/- yaw, +/- pitch and +/- roll. I populate these values into a torque vector, for instance dTorque(-0.34, 2.72, 0.031)
  • Use body's current rotation to rotate this vector and align it with body's principal axis
  • Use NewtonBodyAddTorque to apply this torque in the body's axis (rotated)
  • I then clear the dTorque vector for another cycle of thruster inputs.
  • I do this within ApplyForceAndTorqueCallback

The part that got me confused is that why did not get good results by applying thrusters at some fix point. That method always works, and you do not have to worry about angle order.
did you try that?


I didn't try that because I didn't know HOW to do that in Newton :oops: - Newton library has no method for that (unless i am missing something). Only methods available (NewtonBodyAddForce and NewtonBodySetForce) are applying a force at body's centre of gravity. So, since torque is produced when applying a force at some distance (line of axiom) from CG, I decided to try the torque approach, which seemed to work, but, as you warned me, this method is not the way do this.

So, yes, absolutely, I agree that applying torques is not the correct way to do it. I wanted to use thrusters, but I just don't know how to set up the physics. Using basic physics, I was going to:

  • Define a point on the body (away from CG), apply a force vector on it and calculate how much torque is produced by the force.
  • The resultant of this force is a torque AND force, both acting on the body's CG
  • Since a spacecraft has a pair of equal but opposite thrusters for every axis, I would apply the same but opposite force on an opposite end, which would result in another torque and force acting on CG, except the force would be of same magnitude, but opposite.
  • This would cancel out the forces, leaving only torque on CG... Which is equivalent as applying pure torques. Which is what I did, from the outset :mrgreen:

So, to sum up, I absolutely agree with your reasoning of applying force vectors on line axioms - I just don't know HOW to do it :roll:

One idea I had was to add actual Newton "child" bodies to various parts of my "parent" spacecraft body using a "hard" joint (CustomHinge, with all limits set to 0?), and apply pure forces on those bodies. They would be then acting as actual thruster boxes, locations away from the body CG at which the forces would act. However, this is probably a bit of an overkill...

Thanks,
Misho
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

Re: Gimbal Lock and Euler angles

Postby JoeJ » Mon Apr 25, 2016 3:41 pm

doing a thruster is very easy, contuneing from this thread: http://newtondynamics.com/forum/viewtopic.php?f=9&t=8912

Code: Select all
AddGlobalForce (Force, Point)
{
   R = Point - BodyMatrix.Position;
   Torque = CrossProduct (R, Force);
   NewtonAddForce (Force)
   NewtonAddTorque (Torque)
}


AddLocalForce (LocalForce, localPoint)
{
   GlobaForce = BodyMatrixRotate (LocalForce)
   GlobalPoint = BodyMatrixTranform (localPoint)
   AddGlobalForce (GlobaForce, GlobalPoint);
}


The code for a thruster would be:

Code: Select all
dVector localThrusterPosition (1,0,0);
dVector localThrusterForce (0,0.2,0);
AddLocalForce (localThrusterForce , localThrusterPosition )


What's the problem here, just syntax issues from pseudo code?
Do you understand how it's supposed to work?


Note that instead setting force / torque you also can change the linear / angular velocity or even the body orientation / position directly inside Force Tourque Callback.
(That's useful to test the euler angle conversion)
User avatar
JoeJ
 
Posts: 1489
Joined: Tue Dec 21, 2010 6:18 pm

Re: Gimbal Lock and Euler angles

Postby misho » Mon Apr 25, 2016 5:09 pm

Hehe thanks JoeJ! I should have researched it a bit more! I KNOW I asked that question before!

Ok, diagram time! :lol:

Image

Second line (F1 and F2 forces) is the set up I'd like to achieve... This is obviously only one axis, but my spacecraft will have thrusters set up so that they rotate the body around CG by applying equal but opposite forces at opposing points on the body. This set up results in a force-torque system, where the forces on CG cancel each other out.

However - this still requires applying resultant torques. I WAS going to do this, and, as I was saying in my previous thread, when I apply an opposite thruster force, the 2 forces on CG will cancel each other, and all I have left is a pure torque on a principal axis - which is precisely what I did, and Julio says that's not the way to go!

All I am saying is, if I do it the way you suggested in your pseudo code, I'll just end up applying torques on principal axis because the opposing forces will cancel each other out... and this is exactly what I did already, except I didn't bother with forces at all. :roll:

Thoughts?
Misho Katulic
CTO, FSX SpacePort
TerraBuilder
www.terrabuilder.com
misho
 
Posts: 675
Joined: Tue May 04, 2010 10:13 am

PreviousNext

Return to General Discussion

Who is online

Users browsing this forum: No registered users and 5 guests

cron