spatialmath-python
spatialmath-python copied to clipboard
Fix trlog exceptional condition
For the rotation part of trlog, the iseye condition does not catch all conditions where the rotation value is near-zero. I actually got divided-by-zero exception in the general case part while doing some simulation.
There are many ways to avoid divided-by-zero exception when we calculate skw = ... / math.sin(theta). But, I recommend you to fix iseye routine. Instead of iseye routine, we can simply check if trace(R) = 1 + 2cos(theta=0) = 3
thanks for this, it's elegant to use the trace twice. Can you give me the numeric example where iseye() failed. iseye() applies the tolerance threshold to the norm of the matrix, rather than the trace, and the latter is bigger than the former so the tolerances are not equivalent.
On the typing branch I've updated iseye and added a unit test for trlog for this case.
Perhaps there should be a test on sin(theta) in the "general case" to truly ensure that you're example can't happen again.