Summary

• trnorm (Gram-Schmidt re-orthonormalization used by e.g. SE3.__matmul__/@) and tr2adjoint (adjoint matrix, used by SE3.Ad()/Twist3.Ad()) both built small, fixed-size results (3x3/4x4/6x6) via generic NumPy helpers - np.cross/unitvec/np.stack in trnorm, np.block in tr2adjoint - whose dispatch overhead (built for arbitrary shapes/broadcasting) dominates cost at this size. Same pattern as the isR/ishom speedup in perf(base): speed up isR/ishom/ishom2 orthogonality checks #213.
• trnorm: replaced with explicit scalar cross-product/normalization arithmetic and direct in-place array construction.
• tr2adjoint: replaced np.block([[...]]) with a pre-allocated np.zeros((6,6), dtype=T.dtype) and direct slice assignment. dtype=T.dtype is preserved specifically so the function's documented :SymPy: supported contract still holds for symbolic (dtype=object) input.
• Net effect (isolated timeit, trnorm/tr2adjoint called directly): trnorm ~12x faster (27.4us -> 2.3us), tr2adjoint ~2.7x faster (8.8us -> 3.3us). End to end, SE3 @ SE3 (which normalizes via trnorm) drops from ~30us to ~4us. SE3.Ad()/base.tr2adjoint() calls on an already-known SE3/rotation matrix land around ~3.3us.
• Note: Twist3.Ad() itself is not meaningfully faster from this change - its cost is dominated by the separate self.SE3() -> trexp() conversion step (matrix exponential), not by tr2adjoint. That's a different, inherent cost (same class as Twist3 * Twist3's "product of exponentials"), not addressed here.

Test plan

• pytest tests/ - 343 passed
• Verified bit-for-bit numeric equivalence against the prior implementation over 200 random SO(3)/SE(3) trials (both trnorm and tr2adjoint, both 3x3 and 4x4 forms)
• Verified symbolic (SymPy, dtype=object) equivalence for tr2adjoint specifically, since it's the one with a documented :SymPy: supported contract

🤖 Generated with Claude Code