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.
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.