perf(base): speed up isskewa, cache identity in trexp/trlog/rodrigues

[petercorke]

Summary

trexp/trlog are much slower than rodrigues (~20us and ~17us vs ~7us), but unlike the previous three perf PRs (#213, #214, #215) they don't have one dominant wasteful generic-NumPy call to fix - they're deep call chains (trexp -> isskewa -> vexa -> iszerovec -> unittwist_norm -> rodrigues -> skew -> rt2tr -> ishom -> isR) where cost is spread thin across many small layers, each independently re-validating/re-extracting. This PR pulls out the two safe, verified wins found in that chain; it does not close the full gap to rodrigues - see "further speedup opportunities" below.

• isskewa (the validity check trexp runs on its se(3) input) had the exact same bug ishom had before perf(base): speed up isR/ishom/ishom2 orthogonality checks #213: np.linalg.norm() on a small fixed-size matrix, plus an all(S[-1,:] == 0) array-allocation-and-compare for checking the bottom row is zero. Fixed the same way (explicit sum-of-squares residual + scalar comparisons). ~1.9x faster standalone.
• np.eye(3) was rebuilt from scratch on every call in rodrigues (1x), trexp's V-matrix construction (1x), and trlog (2x) - despite only ever being used as a read-only operand in an addition/multiplication that produces a new array. Replaced with a module-level constant _EYE3 at those call sites only. Not used at the four zero-motion/zero-rotation early-return np.eye(N) calls in rodrigues/trexp (e.g. if iszerovec(w): return np.eye(3)) - those hand the array object directly to the caller, and aliasing them to a shared constant would risk a real bug if a caller ever mutated the returned matrix in place. Left as fresh arrays.
• isskew (the plain, non-augmented so(n) check) was also tried with the same explicit-arithmetic treatment as isskewa, but did not show a reliable win under careful (min-of-repeats) benchmarking - the only cost it avoids is np.linalg.norm on an already-cheap S + S.T, not enough margin to reliably beat by hand-unrolling the indexing. Reverted to the original implementation rather than keep an unproven "fix" - same judgment call as an earlier fully-scalar isR variant that was tried and discarded for the same reason.

Net effect, measured old-vs-new in the same process, min of 9 repeats each (single-run timeit numbers were misleading here - see commit message): isskewa ~1.9x, rodrigues ~1.13x, trexp ~1.05x, trlog ~1.10x. Real, but modest compared to #213-#215.

Further speedup opportunities (not attempted here)

A bigger win on trexp/trlog would mean collapsing the redundant getvector/re-validation layers in the hot path into a more direct, fused implementation - e.g. trexp(se3_matrix) currently re-extracts and re-validates the same data through isskewa -> vexa -> iszerovec -> unittwist_norm independently, each with its own getvector call. That's a larger, riskier change: this code has clearly-deliberate handling for numerically delicate edge cases (near-zero rotation, near-pi singularity, caller-supplied theta), so a fused rewrite needs much more careful edge-case test coverage than the drop-in fixes here. Not scoped or attempted in this PR.

Two smaller, unrelated candidates surfaced but out of scope for this PR:

• np.linalg.norm is used directly (instead of the project's own norm(), which is faster for small vectors per its own docstring) in several other places - iszerovec, isunitvec, qnorm/qunit in quaternions.py, and a few spots in geom2d.py/geom3d.py/pose3d.py. Worth noting: iszerovec was checked directly as part of this investigation, and swapping it to the project's norm() did not show a reliable win either (consistent with the project's own timing script, which shows base.norm(R6) measuring slower than np.linalg.norm(R6) for a 6-vector) - so this isn't a guaranteed win either, just an inconsistency worth someone measuring properly before touching.
• np.eye(N) is rebuilt from scratch in ~20 other places across the codebase (pose2d.py, pose3d.py, transforms2d.py, transformsNd.py, and other unrelated functions in transforms3d.py). None are in the trexp/trlog/rodrigues path touched here; a broader sweep would need the same operand-vs-return-value aliasing care taken in this PR, function by function.

Test plan

• pytest tests/ - 343 passed
• isskewa verified against the prior implementation over 3000 randomized trials (skew/non-skew, near-boundary perturbations)
• rodrigues, trexp, trlog (both matrix and twist-vector return forms) verified bit-identical (max diff 0.0) against reference copies of the prior implementations over 1000-2000 randomized trials each

🤖 Generated with Claude Code

[codecov-commenter]

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

Codecov Report

❌ Patch coverage is 94.73684% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
spatialmath/base/transformsNd.py	94.11%	1 Missing ⚠️

📢 Thoughts on this report? Let us know!