Skip to content

fix: preserve small rotations in quaternion logarithms - #237

Open
dddd-jh wants to merge 1 commit into
rai-opensource:masterfrom
dddd-jh:fix/preserve-small-quaternion-log
Open

dddd-jh wants to merge 1 commit into
rai-opensource:masterfrom
dddd-jh:fix/preserve-small-quaternion-log

Conversation

@dddd-jh

@dddd-jh dddd-jh commented Oct 6, 2026

Copy link
Copy Markdown

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.

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant