Quaternion to three hinge joints

I'm trying to drive three hinge joints each representing an euler axis of rotation using a Quaternion. I can use the euler angles, but unity's built in euler function returns a random euler form the possible ones. I wrote up this:

Code (CSharp):

1.         public static Vector3 CleanEuler(this Quaternion quaternion)
2.         {
3.             float roll = Mathf.Atan2(2.0f * (quaternion.w * quaternion.x + quaternion.y * quaternion.z),
4.                              1.0f - 2.0f * (quaternion.x * quaternion.x + quaternion.y * quaternion.y)) * Mathf.Rad2Deg;
5.             float pitch = Mathf.Asin(Mathf.Max(-1.0f, Mathf.Min(1.0f, 2.0f * (quaternion.w * quaternion.y - quaternion.z * quaternion.x)))) * Mathf.Rad2Deg;
6.             float yaw = Mathf.Atan2(2.0f * (quaternion.w * quaternion.z + quaternion.x * quaternion.y),
7.                             1.0f - 2.0f * (quaternion.y * quaternion.y + quaternion.z * quaternion.z)) * Mathf.Rad2Deg;
8.  
9.  
10.             return new Vector3(roll, pitch, yaw);
11.         }

Which is a bit more reliable a conversion, but still not very clean. I'm now looking at using a vector angle and dot products to calculate the rotation on a plane. I'm a little stuck in the math though. I can get the first rotation pretty easy by flattening the forward direction vector of the rotation on the yz plane, but I don't really know where to go from there.

I know algorithms exist for doing this with robotic arms, but I haven't found one that is simplified enough for me to both understand and apply.

Any help or pointers would be amazing!

 

It doesn't a random euler from the possible ones.

When building a quaternion from euler (or unraveling a quaternion to euler), you have to do it in SOME order. x,y,z; y,z,x; etc etc etc....

Unity does it in the order z,x,y:

https://docs.unity3d.com/ScriptReference/Quaternion-eulerAngles.html

They do this specifically because it's common in 3d games to have 'y' as up, and 'z' as forward. By building it in this order it means just changing the y allows simple rotation in place without the 'y' value going crazy (looking random). And since a character usually won't roll about the z (because 'usually' you're a bipedal person... not a lot of rolls going on). It can be all over the place.

Furthermore Unity uses a 'left-handed' coordinate system. This will also impact things as it determines your positive axis of rotation. Why it's left-handed is just because that's what Unity picked.

When you say "CleanEuler", yeah it's not exactly any cleaner. It's for a different coordinate space with a different build order. No space/order is "cleaner" than the other.

If you want to build it in your own order. Well... firstly, you need to define your order.

In the case of robotics, this will usually be dictated by how you built your physical joint in the real world.

 

You're absolutely right! I read a bunch of "why are the inspector values different then transform.eulerAngles" and got a bit lost. I'll need to dig in and do some testing so I can visualize how changing the order effects the output. I ended up calculating the first two angles on the joints in the order they are connected to each other.

Code (CSharp):

1. Vector3 direction = transform.localRotation.Forward();
2. Vector3 flattenedDirection = new Vector3(0, direction.y, direction.z);
3.  
4. float sign = Vector3.Dot(Vector3.forward, flattenedDirection) < 0.0f ? -1.0f : 1.0f;
5. float jointOneAngle = sign * Vector3.Angle(Vector3.down, flattenedDirection);
6.  
7. jointOneAngle = Mathf.Clamp(jointOneAngle, jointOneHingeLimit.x, jointOneHingeLimit.y);
8.  
9. sign = Vector3.Dot(Vector3.left, direction) < 0.0f ? -1.0f : 1.0f;
10. Vector3 jointTwoStartingDirction  = Quaternion.AngleAxis(jointOneAngle, Vector3.left) * Vector3.down;
11. float jointTwoAngle = Vector3.Angle(jointTwoStartingDirction, direction) * sign;
12.  
13. jointTwoAngle = Mathf.Clamp(jointTwoAngle, jointTwoHingeLimit.x, jointTwoHingeLimit.y);

 

The simple answer for that is that they aren't using

transform.eulerAngles

. Unity saves a separate Euler rotation value on all transforms that informs the inspector.

Code (CSharp):

1. m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}

 

When I was looking through the forums I saw that the inspector had its own values stored/calculated separately. I did not find the method used to calculate the values shown in the inspector. Would you, by chance, know what's going on under the hood?

 

There isn't really a way they're calculated.

Really what it is is that they store the euler values separate from the quat and always increment/decrement/modify those. So that way the value always reflects something similar to what was previously showing.

If it says <35, 45, 90> and you set the y to 180, it becomes <35, 180, 90> and then it sets the quaternion to that using the same algorithm as Quaternion.Euler. Even though the quat might return something different from eulerAngles, unity ignores that. It maintains the <35, 180, 90> because that's the last thing you put in there.

This is opposed to constantly reading and writing to the quat itself.

It's just a way to make it reader friendly.

This doesn't really work in code though. The in-game actual Quaternion is the way rotation is stored to keep the benefits of the quat over euler. It'd require more memory to store both a quat AND an euler at the same time.

 

It wouldn't be any help to you. Their solution is to completely avoid the problem. As long as you are using user-input values, it defers to those saved Euler angles hint value. If the rotation gets changed via script, then it will use the automatic eulerAngles converted values until the user manually changes them again.

If possible, I'd suggest you try and do the same: use Euler angles as much as possible. Try and avoid going there and back, as it will thrash your euler values.

 

By the way after the rotation changed the LocalEulerAnglesHint is recalculated based on the values it hat before, to make it more human readable. The caluculation looks something like this:

Code (CSharp):

1. private void SyncLocalEulerAnglesHint()
2. {
3.     var newEuler = transform.localEulerAngles;
4.  
5.     newEuler.x = RepeatWorking(newEuler.x - localEulerAnglesHint.x + 180.0F, 360.0F) + localEulerAnglesHint.x - 180.0F;
6.     newEuler.y = RepeatWorking(newEuler.y - localEulerAnglesHint.y + 180.0F, 360.0F) + localEulerAnglesHint.y - 180.0F;
7.     newEuler.z = RepeatWorking(newEuler.z - localEulerAnglesHint.z + 180.0F, 360.0F) + localEulerAnglesHint.z - 180.0F;
8.  
9.     localEulerAnglesHint = newEuler;
10.     Debug.Log(localEulerAnglesHint);
11. }
12.  
13. private float RepeatWorking (float t, float length)
14. {
15.     return (t - (Mathf.Floor(t / length) * length));
16. }

SyncLocalEulerAnglesHint() is called after any change to the rotation.

 

what you want there is a simple modulo operation, that wraps any given number to a fixed range [0..360).
think of clock hands. hour 19 has absolutely the same meaning of hour 7 (though a different value), but it's "name" is different. still, if you cared for the difference between 7 and 19 (and many do), if you take a nap at 19 and wake up 16 hours later, it wouldn't be hour 35, but hour 11 of the following day.

similarly, you can think of polygons, for example, having vertices named in the increasing order, just like clocks do, and you want to move N vertices away from a given vertex. how would you guess where you land?

or think of a "warping" chess board where your pieces can move forward, until they hit an edge, then they have to wrap around and go back to their starting square. sure you can do this one with an

if(piece == 8) piece = 0

or

if(piece >= 8) piece -=8;

but there is a much more elegant solution to these problems.

this is known as modular arithmetic, and we can represent this behavior by introducing a MOD (or modulo) operator.

i.e.
(19+16) MOD 24 = 11
(2+4) MOD 5 = 1
etc.

in general, MOD will always return a value that is greater than 0 and less than the right hand operand.
so if you do

23 MOD 24 = 23, but
24 MOD 24 = 0
25 MOD 24 = 1

in fact, 1238735 MOD 24 = 23

when you look at this closely, it becomes apparent that what you need is, in fact, a division remainder because
X * 24 + 23 = 1238735
where X represents a division 1238735 / 24 (= 51613.958333)
however truncated (floored) to an integer (= 51613)

so there are 51613 whole revolutions until we can finish this modularity with an offset of 23
from the clock example alone you can see how we can use this with angles just as well
angles are a prime example of modular arithmetic

i.e. 150 + 215 = 365 and we all know that 365 degs is the same as 5 degs
and indeed (150 + 215) MOD 360 = 5

that said, C# doesn't have modulo operator at its disposal, but has a division remainder one (%).
the difference is subtle, but critical. however if used strictly with positive numbers, there is no difference whatsoever.

for example, these two expressions work the same
8 MOD 6 = 2
8 % 6 = 2

but these two don't, so be wary of this behavior
-1 MOD 6 = 4
-1 % 6 = -1

note that we can work with decimal values just as well
8.65 MOD 6 = 2.65
8.65 % 6 = 2.65

8 MOD 6.5 = 1.5
8 % 6.5 = 1.5

in your case, you want to fix the euler angles of the rotation to a more human-readable format, and I'm guessing you want to constrain the angles to a fixed [0..360) interval without changing the rotation itself.

the simplest solution would be

Code (csharp):

1. while(angle < 0f) angle += 360f; // to prevent working with negative numbers
2. angle %= 360f; // to get the offset within the range

here is how you can implement a more robust modulo that works with negative numbers

Code (csharp):

1. int Mod(float value, float modulus) {
2.   if(modulus <= 0f) throw new System.ArgumentOutOfRangeException("Invalid modulus.");
3.   if((value %= modulus) < 0f) value += modulus;
4.   return value;
5. }

and so

Code (csharp):

1. var newEuler = transform.localEulerAngles;
2. newEuler = new Vector3(
3.   Mod(newEuler.x, 360f),
4.   Mod(newEuler.y, 360f),
5.   Mod(newEuler.z, 360f),
6. );

now, if you really want just to hinge on the user-driven values instead, to make them appear more stable, I'd recommend keeping the actual euler angles and deriving the quaternions from that, instead of losing it and having to reconstruct this information from a quaternion. the two systems have nothing in common (except both being about rotation), and share no sympathy for each other's data.

 

This method is not working when wou rotate an object around x axis in world coordinate space. Maybe somebody have an idea how to fix it or how to implement calculation of euler angles exactly as in unity editor.

 

I noticed that, too. Unfortunatelly the method is not perfect. It is not possible to get fully continuity as its the euler representation. You can also take a look at how its calculated in Blender, wich is probably similar to unity. The function is called "compatible_eul" in math_rotation.c. It trys to find the closest euler angle to an old euler angle (see this code).
It should directly transfer to unitys euler angles, but keep in mind that Blender uses radians for euler angles instead of degree.
Something like this would be the equivalent to how Blender calculates it...

Code (CSharp):

1. public static Vector3 FindClosestEuler(Vector3 eul, Vector3 hint)
2. {
3.     /* we could use M_PI as pi_thresh: which is correct but 5.1 gives better results.
4.      * Checked with baking actions to fcurves - campbell */
5.     eul *= Mathf.Deg2Rad;
6.     hint *= Mathf.Deg2Rad;
7.  
8.     const float pi_thresh = (5.1f);
9.     const float pi_x2 = (2.0f * (float)Math.PI);
10.  
11.     var dif = Vector3.zero;
12.     /* correct differences of about 360 degrees first */
13.     for (int i = 0; i < 3; i++) {
14.         dif[i] = eul[i] - hint[i];
15.         if (dif[i] > pi_thresh) {
16.             eul[i] -= Mathf.Floor((dif[i] / pi_x2) + 0.5f) * pi_x2;
17.             dif[i] = eul[i] - hint[i];
18.         }
19.         else if (dif[i] < -pi_thresh) {
20.             eul[i] += Mathf.Floor((-dif[i] / pi_x2) + 0.5f) * pi_x2;
21.             dif[i] = eul[i] - hint[i];
22.         }
23.     }
24.  
25.     /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */
26.     if (Mathf.Abs(dif[0]) > 3.2f && Mathf.Abs(dif[1]) < 1.6f && Mathf.Abs(dif[2]) < 1.6f) {
27.         if (dif[0] > 0.0f) {
28.             eul[0] -= pi_x2;
29.         }
30.         else {
31.             eul[0] += pi_x2;
32.         }
33.     }
34.     if (Mathf.Abs(dif[1]) > 3.2f && Mathf.Abs(dif[2]) < 1.6f && Mathf.Abs(dif[0]) < 1.6f) {
35.         if (dif[1] > 0.0f) {
36.             eul[1] -= pi_x2;
37.         }
38.         else {
39.             eul[1] += pi_x2;
40.         }
41.     }
42.     if (Mathf.Abs(dif[2]) > 3.2f && Mathf.Abs(dif[0]) < 1.6f && Mathf.Abs(dif[1]) < 1.6f) {
43.         if (dif[2] > 0.0f) {
44.             eul[2] -= pi_x2;
45.         }
46.         else {
47.             eul[2] += pi_x2;
48.         }
49.     }
50.     return new Vector3(eul.x, eul.y, eul.z) * Mathf.Rad2Deg;
51. }
52.  

Keep in mind that this code snippet is licenced under GNU GPL, as it comes from the Blender source code!