From 8c9f35f3a00d54b574786e8e20ce5717ac22f4b6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 20 Aug 2026 17:16:45 +1000 Subject: [PATCH] test: add regression coverage for trlog near-identity division trlog's general-case branch divides by sin(theta), computed from trace(R) via acos. The only existing test used R = np.eye(3) exactly, never a numerically near-identity matrix as produced by real computation -- the case #63 (unmerged) reported hitting a divide-by-zero on. Confirmed against the pre-a9fc08a code (rework code to be more robust to nearly identity rotation matrix) that this exact input raised FloatingPointError there; current code's crisp `st == 0` guard handles it cleanly. --- tests/base/test_transforms3d.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index 8b2fb080..e9b9af14 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -319,6 +319,34 @@ def test_trlog(self): T = transl(1, 2, 3) @ rpy2tr(0.1, 0.2, 0.3) nt.assert_array_almost_equal(logm(T), trlog(T)) + def test_trlog_near_identity(self): + # Regression test for #63: trlog's general-case branch divides by + # sin(theta), computed from trace(R) via acos. A near-identity R + # that isn't *exactly* np.eye(3) (as produced by real computation, + # not hand-constructed) must not raise or blow up. + + # theta small enough that cos(theta) underflows to exactly 1.0 in + # float64, so trace(R) rounds to exactly 3 and acos(1.0) == 0.0 + # exactly: this is the case that used to divide by sin(theta) == 0 + # with no guard. R itself is not bitwise np.eye(3) (it still has + # sin(theta) noise off the diagonal), so this also exercises the + # "is this actually the identity" branch on a matrix that isn't one. + R = rotx(1e-9) + assert not np.array_equal(R, np.eye(3)) + nt.assert_array_almost_equal(trlog(R, twist=True), [0, 0, 0]) + nt.assert_array_almost_equal(trlog(R), skew([0, 0, 0])) + + # theta small enough to be well within the near-identity regime, but + # not small enough to clamp: the general-case formula must stay + # accurate here rather than falling back to a coarse zero. This is + # the case a fuzzy identity-tolerance (checking R against I before + # computing theta, as originally proposed in #63) risks getting + # wrong in the other direction, by discarding a real small rotation. + theta = 1e-7 + for R in (rotx(theta), roty(theta), rotz(theta)): + v = trlog(R, twist=True) + nt.assert_almost_equal(np.linalg.norm(v), theta, decimal=12) + def test_trexp(self): R = trexp(skew([0.5, 0, 0])) nt.assert_array_almost_equal(R, rotx(0.5))