Solve the IK velocity step via JJ^T eigendecomposition, not JacobiSVD - #3804
Solve the IK velocity step via JJ^T eigendecomposition, not JacobiSVD#3804hugogo1998 wants to merge 2 commits into
Conversation
ChainIkSolverVelMimicSVD::CartToJnt runs once per Newton iteration of
the IK solve, and each call was ~94% JacobiSVD of the 6xN Jacobian
(rows <= 6, cols = number of active joints).
The min-norm least-squares solution for a wide Jacobian J (rows <=
cols), x = J^T (JJ^T)^# vin, only needs the eigendecomposition of the
small rows x rows matrix JJ^T instead of a full SVD of the 6xN J.
Same thresholded pseudo-inverse: singular values are sqrt(eigenvalue),
kept or zeroed against the same relative threshold
(svd_.threshold() * largest singular value) svd_.solve() already
uses internally, so truncation behaves identically at the same
condition-number cutoff.
svd_ itself is kept (still used for rows()/cols()/threshold() and
isPositionOnly()), just no longer computed or solved in the hot path.
Verified against svd_.solve() over 200k random Jacobians (rows in
{3,6}, cols in {6,7,9}), using the class's actual default threshold
(0.001, not Eigen's near-epsilon default -- tried that first, got a
large false-alarm diff before realizing my test wasn't configured to
match the real solver): max |qdot| difference 3.3e-8, matching the
tool's own reported 7.3e-9 order of magnitude, four-plus orders of
magnitude below the solver's 1e-5 convergence tolerance.
rhaschke
left a comment
rhaschke
left a comment
There was a problem hiding this comment.
Did you try computing a thin JacobiSVD alternatively?
JacobiSVD<MatrixXf, ComputeThinU | ComputeThinV>
Manually computing the pseudoinverse seems to be awkward.
Codecov Report❌ Patch coverage is
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. 🚀 New features to boost your workflow:
|
|
The existing solver is already configured with You’re right that having the pseudoinverse calculation inline made I also reran the benchmark. For 6x7 matrices, thin JacobiSVD took 5.33–5.38 µs and the eigendecomposition about 1.85 µs. For 3x7, the numbers were 1.24–1.27 µs and about 0.76 µs. Both implementations selected the same effective rank in all tested cases. |
You are right.
I cannot find those recent changes. Did you push them already? |
|
Sorry for overlooking that. They are on the PR now in f7cd23f. I moved the pseudoinverse calculation into a small internal helper and added comparisons against Eigen’s thin JacobiSVD for 6x7 and 3x7 matrices, including full-rank, rank-deficient, ill-conditioned, and threshold-boundary cases. |
Signed-off-by: hugogo1998 <295481948+hugogo1998@users.noreply.github.com>
fd34f83 to
f7cd23f
Compare
|
Hi @rhaschke, could you pls take a look again when you get time? Please let me know if I overlook something, and any feedback is appreciated, thanks |
2 participants
Description
ChainIkSolverVelMimicSVD::CartToJnt()runs once per Newton iteration, and profiling showed that most of its runtime was spent computing a thinJacobiSVDof the 6xN Jacobian.This replaces that decomposition with the equivalent minimum-norm solve
using a self-adjoint eigendecomposition of the smaller
rows x rowsmatrixJJ^T. The helper applies the same relative singular-value threshold as the existing SVD solve.Results
Measured on a 7-DOF Panda chain:
Validation
Added tests comparing the helper against Eigen's thin
JacobiSVDfor 6x7 and 3x7 Jacobians. They cover full-rank, rank-deficient, ill-conditioned, and threshold-boundary cases.clang-formatrun on all changed C++ files-Wall -Wextra -WerrorChecklist
clang-formatMIGRATION.mdentry is needed