gimbal lock condition computing jacobian from quaternion to rpy

[giafranchini]

In rpy_and_jacobian the implementation of the jacobian components when gimbal lock occurs appears to be wrong to me. For example, in the case of pitch=90°, if I symbolically derived the yaw expression yaw = -2 * atan2(x(), r()) wrt the 4 quaternion components, I obtain the following:

• dyaw/dx() = -2*r() / (r()^2 + x()^2)
• dyaw/dr() = 2*x() / (r()^2 + x()^2)

while the other two components should be zeros. Instead, in the current implementation I found:

• dyaw/dr() = +2 / x()
• dyaw/dy() = -2 * r() / (x() * x())

and the other two components equal to zero. This leads to NaN results when trying to convert quaternions when x() goes to zero.

[jlblancoc]

Wow, can't find an explanation for it! Thanks a lot for investigating it and reporting. It's now covered by unit tests and fixed in #1290