fix: preserve small rotations in quaternion logarithms

[dddd-jh]

UnitQuaternion.Rx(1e-9).log().v currently returns [0, 0, 0] instead of [5e-10, 0, 0]. acos(s / norm) loses the angle when the quotient rounds to one; it also loses precision for larger small rotations. Both singleton and sequence logarithms are affected, including non-unit quaternions.

Use atan2(norm(v), s) in both paths. This is the same principal angle for a nonzero vector part, retains small-angle information, and preserves the branch for negative scalar parts. The existing zero-vector handling and scalar log(norm(q)) are unchanged.

Regression tests cover signed small rotations from 1e-12 to 1e-4 radians, an ordinary rotation, singleton/sequence agreement, non-unit scales, exp(log(q)), input preservation, and the branch near the negative real axis. The original implementation fails eight scalar subcases, both sequence subcases, and the negative-real precision check.

Validation on Windows / Python 3.12.14:

• Complete suite: 350 passed, 3 skipped, with all 12 new subtests passing, on both NumPy 2.5.3 / SciPy 1.18.1 and NumPy 1.26.4 / SciPy 1.15.3 (MPLBACKEND=Agg, the CI timeout options).
• Independently checked 100 deterministic rotation vectors against scipy.spatial.transform.Rotation, through singleton unit, singleton scaled, and sequence unit quaternion logarithms: all 300 comparisons pass after the fix; 270 fail a 1e-13 relative-error check before it. Maximum relative error after the fix is about 3.6e-16 in both dependency environments.
• Black 23.10.0, flake8's syntax/undefined-name checks (E9,F63,F7,F82), and git diff --check pass.
• Isolated sdist and wheel builds pass.

This validates the quaternion math and repository tests; no ROS nodes or robot hardware were run.

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

✅ All modified and coverable lines are covered by tests.

📢 Thoughts on this report? Let us know!

[petercorke]

Thank you for your interest in SMTB, and for a very clear analysis. Replacing acos(s / ‖q‖) with atan2(‖v‖, s) is the right fix. The two give the same principal angle for a nonzero vector part, including the negative-real branch, but acos has conditioning that grows like 1/θ² as the angle shrinks, whereas atan2 is bounded by 2. I checked the result against exact rational arithmetic and it is at double-precision rounding level down to angles of about 1e-14. I've approved this.

Two suggestions, neither of which is a blocker:

1. The zero-vector guard. smb.iszerovec(...) just before the changed line is an absolute test, ‖v‖ < 20·eps ≈ 4.4e-15, and it ignores the size of q. That means a quaternion with a tiny norm still loses its rotation entirely. For example, a 90° rotation scaled by 1e-16 returns a vector part of 0 instead of π/4, and a rotation of 1e-9 rad at scale 1e-6 also returns 0. This is pre-existing, but it sits on the lines you are already changing, and your tests only use scales 1 to 5. Since atan2 handles tiny inputs fine, the guard could become an exact zero test, which also avoids underflow in the squared terms:

n = math.hypot(*self._A[1:4])
v = np.zeros(3) if n == 0 else math.atan2(n, self._A[0]) * self._A[1:4] / n

A regression case with a small scale factor, such as 1e-6 * [cos(a/2), sin(a/2)*axis], would cover it.

2. A note in the docstring. The docstring still presents the formula with cos⁻¹(s / ‖q‖), which is mathematically equivalent. It would help future readers to add a short note saying that the implementation deliberately uses atan2(‖v‖, s) rather than acos, because acos loses precision for small angles (it returns exactly zero below about 2e-8 rad).

You may see the sphinx / sphinx check fail. That is not caused by this change: for pull requests from a fork, GitHub gives the docs job a read-only token, so its final push to gh-pages gets a 403. The fix is already open as #222 and is waiting on review. All of the unit-test jobs are the real signal here.

Merging is up to the RAI maintainers, so it may take a little while. Thanks again for the contribution!