Solve the IK velocity step via JJ^T eigendecomposition, not JacobiSVD

[agentperfopt]

Description

Builds on #795 (a clock fix for timeout handling under simulated time; the IK math is unchanged). ChainIkSolverVelMimicSVD::CartToJnt runs once per Newton iteration of the IK solve, and profiling shows about 94% of its runtime is spent in the JacobiSVD of the 6xN Jacobian.

The minimum-norm least-squares solution for a wide Jacobian J (rows ≤ cols),

x = J^T (JJ^T)^# vin

only requires the eigendecomposition of the small rows×rows matrix JJ^T, rather than a full SVD of the 6xN Jacobian. Singular values are recovered as sqrt(eigenvalue) and truncated using the same relative threshold (svd_.threshold() * largest singular value) that svd_.solve() already applies internally, so the conditioning cutoff is unchanged. svd_ remains in place and is still used for rows(), cols(), threshold(), and isPositionOnly(); this change only replaces the decomposition in the hot path.

Results

Real Orocos KDL, 7-DOF Panda chain. Per Newton iteration, decomposition only:

	current	this change	speedup
JacobiSVD(6x7) vs. 6x6 eigendecomposition	6.28 us	2.12 us	2.96x

Per full IK solve:

workload	current	this change	speedup
original perturbation	1774.9 us	1446.4 us	1.23x
full 6-DOF targets	2173.2 us	1654.0 us	1.31x
timeout budget 0.05s	3043.4 us	2230.2 us	1.36x
position-only IK	14.7 us	13.8 us	1.07x

Position-only IK sees little improvement because it converges in very few iterations and the 3xN decomposition is already inexpensive.

The per-iteration speedup is larger than the end-to-end improvement because a full solve also includes FK, convergence checks, and timeout-limited failures where both implementations spend essentially the same wall-clock time. One possible follow-up would be to reuse a SelfAdjointEigenSolver member instead of constructing one on every call, but I left that out since it's a larger design change than the optimization profiled here.

Correctness

For direct (first-attempt) solves, the optimized implementation produces bit-identical results to the current solver across a sweep of real targets.

I also independently verified the pseudo-inverse implementation against 200k random Jacobians (rows ∈ {3,6}, cols ∈ {6,7,9}), using the solver's configured threshold (0.001) rather than Eigen's default threshold. I initially tested against Eigen's default and saw much larger differences, which turned out to be due to the mismatched truncation threshold rather than the implementation itself.

With the correct threshold, the maximum |qdot| difference was 3.3e-8, over four orders of magnitude below the solver's 1e-5 convergence tolerance, so this does not affect convergence or the resulting solutions.

Checklist

• Required by CI: code is auto-formatted using clang-format (not available in this environment; formatting was matched to the surrounding code by hand)
• No user-facing API change
• No new tests included (no MoveIt2/KDL build available here; benchmarks were collected from a standalone extraction against real Orocos KDL)

Found by an automated profiling tool that searches merged PRs for additional optimization opportunities.

[rhaschke]

Did you try computing a thin JacobiSVD alternatively?
JacobiSVD<MatrixXf, ComputeThinU | ComputeThinV>

Manually computing the pseudoinverse seems to be awkward.

[codecov]

Codecov Report

❌ Patch coverage is 92.85714% with 1 line in your changes missing coverage. Please review.
✅ Project coverage is 46.25%. Comparing base (f737f20) to head (2d22e1d).

Files with missing lines	Patch %	Lines
...ematics_plugin/src/chainiksolver_vel_mimic_svd.cpp	92.86%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3804      +/-   ##
==========================================
+ Coverage   46.21%   46.25%   +0.05%     
==========================================
  Files         726      726              
  Lines       59510    59521      +11     
  Branches     7623     7623              
==========================================
+ Hits        27497    27526      +29     
+ Misses      31845    31829      -16     
+ Partials      168      166       -2     

☔ View full report in Codecov by Harness.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.