Convert homogeneous transform or rotation matrix to roll-pitch-yaw angles
RPY = rts_c_tr2rpy(TR, OPTIONS)
compute angles in degrees (radians default)
RPY = RTS_C_TR2RPY(TR, OPTIONS) are the RPY angles expressed as a row vector corresponding to the rotational part of a homogeneous transform TR. The 3 angles RPY=[ROLL, PITCH, YAW] correspond to sequential rotations about the X, Y and Z fixed axes respectively.
RPY = RTS_C_TR2RPY(R, OPTIONS) are the RPY angles expressed as a row vector corresponding to the orthonormal rotation matrix R.
If R has (3*n) columns or T has (4*n) columns, they are interpreted as a trajectory of n elements. Each row of RPY corresponds to a step of the trajectory.
There is a singularity for the case where PITCH=pi/2 in which case ROLL is arbitrarily set to zero and YAW is the sum (ROLL+YAW).