Get pitch and roll from matrix without singularities

Question

I'm working on motion simulator with 2 DOF (pitch & roll). I'm reading transformation matrix from game and I need to get the angles and send to hardware to drive motors. Since Euler angles have singularities, I can't really use them. It behaves like this:

when it should like this:

I prepared online example to better show the issue:

// Get euler angles from model matrix
var mat = model.matrix;
mat.transpose();

var e = new THREE.Euler();
e.setFromRotationMatrix(mat, 'XZY');
var v = e.toVector3();

var pitch = -v.z;
var roll  = -v.x;

http://jsfiddle.net/qajro0ny/3/

As far as I understand, there are two problems here.

1. There is no yaw axis on simulator.
2. Even if there were yaw axis, motors just don't behave like computer graphics, i.e. they need time to get to target position.

I've read about gimbal lock and even implemented euler filter, but that didn't work as expected. Most advices about gimbal lock were to use quaternions, but I can't drive physical motor with quaternion (or can I?).

Axis order doesn't really matter here, because changing it will only move singularity from one axis to another.

I need to handle this some other way.

I tried multiplying axis vectors by matrix and then using cross and dot product to get angles, but that failed too. I think there should be also axis reprojection involved to get this right, but I couldn't figure it out. But something tells me that this is the right way to do this. It was something like this: http://jsfiddle.net/qajro0ny/53/

Then I came up with different idea. I know previous position, so maybe do the following:

1. Convert matrix to quaternion
2. Compute difference between current and previous quaternion
3. Convert resulting quaternion to euler angles
4. Add those angles to static pitch, roll and yaw variables.

So I tried that and... it worked! No singularities in any of the directions, perfect 360 degree rotation in pitch, roll and yaw. The perfect solution! Except... it is not. The frames did not synchronize, so after a while angles were way off from what they should be. I've been thinking about some sort of synchronization mechanism, but I figured this is not the right way.

It looked like this: http://jsfiddle.net/qajro0ny/52/

And the same logic, but directly with matrices: http://jsfiddle.net/qajro0ny/54/

I've searched web high and low, I've read dozens of papers and other questions/posts and I just can't believe nothing really works for my case.

I might not understand or miss something, so here is everything I found and tried:

Links: http://pastebin.com/3G0dYLvu

Code: http://pastebin.com/PiZKwE2t (I've put it all together so it's messy)

I must be missing something, or I'm looking at this from the wrong angle.

Answer 1

If you know that the matrix contains only the two rotations (in the given order T = RZ * RX), then you can do something like the following:

The local x-axis is not affected by the second rotation. So you can calculate the first angle with only the local x-axis. Then you can remove this rotation from the matrix and calculate the remaining angle from any of the two other axes:

function calculateAngles() {
    var mat = model.matrix;

    //calculates the angle from the local x-axis
    var pitch = Math.atan2(mat.elements[1], mat.elements[0]);

    //this matrix is used to remove the first rotation
    var invPitch = new THREE.Matrix4();
    invPitch.makeRotationZ(-pitch);

    //this matrix will only contain the roll rotation
    //  rollRot = RZ^-1 * T
    //          = RZ^-1 * RZ * RX
    //          = RX
    var rollRot = new THREE.Matrix4();
    rollRot.multiplyMatrices(invPitch, mat);

    //calculate the remaining angle from the local y-axis
    var roll  = Math.atan2(rollRot.elements[6], rollRot.elements[5]);

    updateSimAngles(pitch, roll);
}

This does of course only work if the given matrix matches the requirements. It must not contain a third rotation. Otherwise, you'll probably need to find a non-linear least-squares solution.

Answer 2

Your idea about using the deltas of the rotation sounds quite promising actually.

I am not quite sure what you mean with the "frames did not synchronize" though. I could imagine that your calculations with quaternions might be not 100% exact (do you use floating points?) which results in small errors which stack up and finally result in asynchronous movements.

The point is that you are supposed to use unit-quaternions for the representation of rotations. You can do that in a theoretical model, but if you are representing quaternions by 4 floating point numbers, your quaternions won't be unit quaternions in most cases, but only be really close (their norm is 1+e for some small - and possibly negative - value e). This is okay since you won't notice these small differences, however if you are throwing tons of operations at your quaternions (which you are doing by constantly roating your model and calculating the deltas), these small errors will stack up. Thus you need to renormalize your quaternions constantly to keep them as close to unit quaternions as possible so your calculations - especially the conversion back to Euler angles - stay (almost) exact.

Answer 3

I added my two cents in http://jsfiddle.net/qajro0ny/58/, basically applying the works of http://blogs.msdn.com/b/mikepelton/archive/2004/10/29/249501.aspx which i stumbled upon a few years ago. It basically boils down to

var a = THREE.Math.clamp(-mat.elements[6], -1, 1);
var xPitch = Math.asin(a);
var yYaw = 0;
var zRoll = 0;
if (Math.cos(xPitch) > 0.0001)
{
  zRoll = -Math.atan2(mat.elements[4], mat.elements[5]);
  yYaw = -Math.atan2(mat.elements[2], mat.elements[10]);
}
else
{
  zRoll = -Math.atan2(-mat.elements[1], mat.elements[0]);
}

I noticed that the assumed mapping yaw, pitch, roll to axes (y,x,z) as used in DirectX left-handed coordinate system differs the from one you are using OpenGL/WebGL, so maybe this one needs to be finally sorted out.

I hope this helps.

Answer 4

I imagine that you actually could drive physical motors with quaternions. Remember that quaternion represents a rotation axis (x, y, z) and an angle. I suppose that you have three motors (for each axis), rotation velocity of which you can scale. Now, by scaling rotation speed of these three motors you can set a specific rotation axis, and by carefully measuring time you can set a specific rotation angle. It should be easier than converting to Euler angles deltas.