Trying to understand Pose3d math

I’m trying to understand the behavior of pose transformations. The Rotation3d object in particular gives me a result I don’t understand when rotating in the Y axis.

This code behaves surprisingly to me:

        Rotation3d first = new Rotation3d(0, 0, 0);
        Rotation3d additive = new Rotation3d(0, Math.PI, 0);

        Rotation3d result = first.plus(additive);

        // All these unit tests fail
        assertEquals(result.getX(), 0);
        assertEquals(result.getY(), Math.PI);
        assertEquals(result.getZ(), 0);

The result object’s x and z values are PI, and Y is essentially 0 (minus some floating point rounding). I can’t visualize why rotating in any one axis (regardless of which) would affect the other two.

If you do the same test but rotate X or Z, the result is more intuitive to me, changing rotation in one dimension only affects that dimension. So if you rotate on X, the end result has only moved X.

        Rotation3d first = new Rotation3d(0, 0, 0);
        Rotation3d additive = new Rotation3d(Math.PI, 0, 0);

        Rotation3d result = first.plus(additive);

        // All these unit tests pass
        assertEquals(result.getX(), Math.PI);
        assertEquals(result.getY(), 0);
        assertEquals(result.getZ(), 0);

Can anyone explain if this is expected and why?

This is expected.

That’s because the get<X/Y/Z>() methods in WPI’s Rotation3d are expressed in YPR (Yaw-first, Pitch-second, Roll-third).
This causes the pitch to be bounded to [-\frac{\pi}{2},\frac{\pi}{2}], pointing straight down or straight up and never directly backward.
A rotation of \pi about the pitch axis is essentially flipping it over, and in YPR terms, it’s the same thing as rotating it about the Z axis by \pi, and then rotating it about the X axis by \pi.

You can see this in the image I have below, where the red arrow is the primary direction and the orange arrow is pointing “up”.
image

Rotations are fun that way, there’s multiple ways to express the same orientation, but you have to boil it down to one version that follows all the same rules. WPILib (and CTR) chose YPR since it’s generally the most intuitive, but there’s no perfect convention for every possibility (unless you’re dealing with the not-very-intuitive quaternions).

Edit to make it clear that the getter methods are the things expressed in YPR and not the object’s internal representation.

5 Likes

Specifically, Rotation3d has a constructor for extrinsic RPY (roll, pitch, then yaw in global frame), which is equivalent to intrinsic YPR (yaw, pitch, then roll in body frame). Rotation3d’s internal representation is a quaternion.

1 Like

OK, wow, got it.

For fun I threw in 30 degrees, within the “bounding arc” before it flips and it more intuitively stayed in just the “pitch” dimension for that calculation. 91 degrees did indeed “flip” the yaw and roll dimensions.

Thanks for the help (and expert diagrams).

3 Likes

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.