examples: use geometry utils

[GuillaumeLaine]

Changes

• Use geometry utils in examples
• Clarify usage of RPY extrinsic Tait-Bryan euler angles (YPR intrinsic).
  • This is consistent with PX4 matrix lib, which uses 3-2-1 intrinsic euler angles.
• Moved quaternionFromEuler in manual example to eulerRpyToQuaternion in geometry utils
  • added unit tests
• Rename RPY -> Rpy

Side-node on euler angles

Rotation conventions

Some euler angle convention choices (details here):

• Classic vs Tait-Bryan:
  • Classic: the three elemental rotations are performed around only two axes (e.g. X-Y-X, Z-X-Z...)
  • Tait-Bryan: the three elemental rotations are performed around the three axes of the cartesian coordinate frame (e.g. X-Y-Z)
• Intrinsic vs Extrinsic:
  • Intrinsic: each rotation is applied in the local coordinate system, the axes of rotation change after each rotation.
  • Extrinsic: each rotation is applied in the global coordinate system, the axes of rotation do not change during the sequence of rotations.
• Order:
  • Rotation order matters in all conventions (e.g. X-Y-Z vs Y-Z-X)

Interesting result

@bkueng you were correct. Given a rotation R s.t.:

R = R_z * R_y * R_x 

with R_x, R_y, R_z rotations along X, Y, Z axes respectively. Then R corresponds to euler angles respecting both of the following rotation conventions:

• XYZ / RPY extrinsic Tait-Bryan (i.e. right to left product)
• ZYX / YPR intrinsic Tait-Bryan (i.e. left to right product) I didn't read everything but explained nicely here.

[GuillaumeLaine]

@bkueng I double checked, the code works as documented and is consistent with the PX4 matrix lib (we're using 3-2-1 intrinsic euler angles).

My only concern now is that the naming quaternionToEulerRpy is confusing, seeing as rpy <-> ypr depends on the intrinsic/extrinsic convention used. Do you think we should remove the Rpy from the function name and have people refer to the docstring? I'm not sure quaternionToEulerRpyExtrinsic is helpful, I wonder if people generally know about these conventions.

[bkueng]

I think it still adds some value, as it describes the order of the vector elements.