Sot: Use basis of null-space from Eigen::JacobiSVD

Author: jmirabel

src/sot/memory-task-sot.cpp

@@ -65,7 +65,7 @@ initMemory( const Matrix::Index nJ,const Matrix::Index mJ,const Matrix::Index ff
    JK.resize( nJ,mJ );
    V.resize( mJ,mJ );
 
-   svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeThinV);
+   svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
 
    JK.fill(0);
    if (atConstruction) {

src/sot/sot.cpp

@@ -615,8 +615,12 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       /***/sotCOUNTER(7,8); // QDOT
 
       /* --- OPTIMAL FORM: To debug. --- */
-      if( 0==iterTask )
-	{ Proj.resize( mJ,mJ ); Proj.setIdentity(); }
+      if( 0==iterTask ) {
+        Proj.resize( mJ,mJ ); Proj.setIdentity();
+      } else {
+        // Proj.noalias() -= svd.matrixV().leftCols(rankJ) * svd.matrixV().leftCols(rankJ).adjoint();
+        Proj = Proj * svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
+      }
 
       /* --- OLIVIER START  --- */
       // Update by Joseph Mirabel to match Eigen API
@@ -629,8 +633,6 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       sotDEBUG(2) << "JpxJt = " << Jp*Jt;
       sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
 
-      Proj.noalias() -= svd.matrixV().leftCols(rankJ) * svd.matrixV().leftCols(rankJ).adjoint();
-
        /* --- OLIVIER END --- */
 
        sotDEBUG(15) << "q"<<iterTask<<" = "<<control<<std::endl;