Fix rank computation

7fb0e1cc · Joseph Mirabel · olivier stasse · d985c54c · 7fb0e1cc
Commit 7fb0e1cc authored Jan 31, 2020 by Joseph Mirabel Committed by olivier stasse Apr 09, 2020
--- a/src/sot/sot.cpp
+++ b/src/sot/sot.cpp
@@ -531,8 +531,10 @@ dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
        svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeThinV);
      else
        svd.compute(*Jt, Eigen::ComputeThinU | Eigen::ComputeFullV);
-      svd.setThreshold(th);
-      rankJ = svd.rank();
+      rankJ = 0;
+      while (rankJ < svd.singularValues().size() &&
+             th < svd.singularValues()[rankJ])
+        ++rankJ;
      /***/ sotCOUNTER(4, 5); // SVD and rank

      /* --- COMPUTE QDOT AND P --- */