SO3 .rpy() method

[julienissuts]

Hello,
I noticed that the method .rpy(order='xyz') from spatialmath outputs the same as .as_euler('zyx') from scipy.spatial.transform.Rotation and .rpy(order='zyx') the same as .as_euler('xyz').

Is there something wrong with the rpy() method or did I misunderstand how the order is interpreted by the function?

Best regards
Julien

[jbarnett-bdai]

I think the documentation on SO3.rpy describes the order (reformatted slightly and condensed):

rot.rpy is the roll-pitch-yaw angle representation of the rotation. The angles are
a 3-vector (r, p, y) which correspond to successive rotations about the axes
specified by order:

• 'zyx' (default), rotate by yaw about the z-axis, then by pitch about the new y-axis,
  then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
  and y-axis sideways.

A couple things to note, focusing on zyx:

• order is specifying the order of applying the corresponding intrinsic rotations, but while the order is read left-to-right, the sequence of rotations is multiplied right-to-left. (Flip the order of multiplication for extrinsic rotations.)
• complicating matters, the order in which the angles are returned is based on the traditional meaning of roll, pitch, and yaw, as being rotations about the (intrinsic) x axis, (intrinsic) y axis, and (extrinsic) z axis -- this is the opposite of the order specified in order, and probably the source of confusion.

scpiy.spatial.transfrom.Rotation.as_euler:

• interprets seq as the order of rotations (again read left-to-right, but multiplied right-to-left) of either intrinsic or extrinsic rotations (as specified)
• but the output order corresponds to the order in seq, since there is no need to preserve meaning of "roll,pitch,yaw".

So with spatialmath, the following is valid; I'm using desctructuring assignment to align names, and reversing the order of multiplication to use extrinsic rotations:

In [1]: from spatialmath import SO3

In [2]: r = SO3.Rand()

In [3]: r.A
Out[3]: 
array([[-0.18110268, -0.95298081, -0.24295965],
       [ 0.83993087, -0.02136809, -0.54227256],
       [ 0.51158376, -0.30227633,  0.80430783]])

In [4]: (roll, pitch, yaw) = r.rpy()

In [5]: r1 = SO3.Rz(yaw) * SO3.Ry(pitch) * SO3.Rx(roll)

In [6]: r1.A
Out[6]: 
array([[-0.18110268, -0.95298081, -0.24295965],
       [ 0.83993087, -0.02136809, -0.54227256],
       [ 0.51158376, -0.30227633,  0.80430783]])

In [7]: # with different order

In [8]: rpy_xyz = r.rpy(order='xyz')

In [9]: r2 = SO3.Rx(rpy_xyz[2]) * SO3.Ry(rpy_xyz[1]) * SO3.Rz(rpy_xyz[0])

In [10]: r2.A
Out[10]: 
array([[-0.18110268, -0.95298081, -0.24295965],
       [ 0.83993087, -0.02136809, -0.54227256],
       [ 0.51158376, -0.30227633,  0.80430783]])

I hope this helps.

[petercorke]

Thanks @jbarnett-bdai, there is so much confusion around this topic online, and a lot of sources are just plain wrong. There are two approaches that go by many different names intrinsic/extrinsic rotation, pre-/post-multiplication, fixed/current axes etc. I'll update the documentation with equations to make it really explicit what these functions are doing, though it's pretty clear in the code

    if order in ("xyz", "arm"):
        R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
    elif order in ("zyx", "vehicle"):
        R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
    elif order in ("yxz", "camera"):
        R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])

There's an extra level of confusion because this function takes the angles in the reverse order to which they are used, ie. yaw is the last argument, but the first used (in a left to right sense). The similar MATLAB function eul2rotm takes (yaw, pitch, roll) but I think this is confusing because we speak of "roll-pitch-yaw angles" rather than "yaw-pitch-roll angles".

I'll make a PR for this.