Unified the QR and pseudo inversion.

python/dynreduced.py

@@ -169,7 +169,7 @@ featureCom.sdes.value = 'featureComDes'
 taskCom = TaskDynPD('taskCom')
 taskCom.add('featureCom')
 plug(dyn.velocity,taskCom.qdot)
-taskCom.dt.value = 1e-3
+taskCom.dt.value = dt
 
 gCom = GainAdaptive('gCom')
 plug(taskCom.error,gCom.error)
@@ -185,11 +185,13 @@ gCom.set(1050,45,125e3)
 contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
 contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
 contactLF.feature.frame('desired')
+contactLF.task.dt.value=dt
 
 # Right foot contact
 contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
 contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
 contactRF.feature.frame('desired')
+contactLF.task.dt.value=dt
 
 # --- SOT Dyn OpSpaceH --------------------------------------
 
@@ -237,6 +239,7 @@ robot.after.addSignal('sot.acceleration')
 tr.add('taskCom.error','com')
 tr.add('taskCom.task','')
 tr.add(contactLF.task.name+'.error','lf')
+tr.add(contactRF.task.name+'.error','rf')
 
 if not rs: tr.add('sot.solution','')
 
@@ -287,10 +290,9 @@ attime(500,stop,'stop')
 
 
 
-go()
+#go()
 
 # --- DEBUG ----------------------------------------------
-'''
 for i in range(50):    inc()
 
 print sot.velocity.m
@@ -331,7 +333,6 @@ else:
     print "u=x(1:24); psi=x(25:end); "
 
     print "ddq2= ddq; phi2=phi; f2=f; fn2=f(3:3:end);"
-'''
 
 '''
 if 0: # double check

src/col-piv-qr-solve-in-place.h

@@ -18,6 +18,7 @@
 #define __sot_dyninv_ColPivQRSolveInPlace_H__
 
 #include <Eigen/QR>
+#include <iostream>
 
 namespace Eigen
 {
@@ -43,7 +44,7 @@ namespace Eigen
       assert( r==m );
       assert( Gp.rows() == m ); // TODO: if not proper size, resize.
 
-      VectorXd workspace( n );
+      VectorXd workspace( Gp.rows() ); // Size of Gtp number of rows.
       /* P2*P1*P0 ... */
       for (int k = r-1; k >= 0 ; --k)
 	{
@@ -96,11 +97,20 @@ namespace Eigen
 
       /* Compute Q R^+T E'*X. */
       /* Q = P1*P2* ... *Pn */
-      VectorXd workspace( n );
+      VectorXd workspace( Gtp.cols() ); // size of Gtp number of cols.
       for (int k = r-1; k >= 0 ; --k)
 	{
 	  int remainingSize = m-k;
-	  Gtp.bottomRows( Gtp.rows()-k )
+	  /* std::cout << " ---------------------------" << std::endl; */
+	  /* std::cout << remainingSize << std::endl; */
+	  /* std::cout << " ---------------------------" << std::endl; */
+	  /* std::cout << Gtp.bottomRows( Gtp.rows()-k ) << std::endl; */
+	  /* std::cout << " ---------------------------" << std::endl; */
+	  /* std::cout << matrixQR().col(k).transpose() << std::endl; */
+	  /* std::cout << " ---------------------------" << std::endl; */
+	  /* std::cout << matrixQR().col(k).tail(remainingSize-1).transpose() << std::endl; */
+	  /* std::cout << " ---------------------------" << std::endl; */
+	  Gtp.bottomRows( remainingSize )
 	    .applyHouseholderOnTheLeft(matrixQR().col(k).tail(remainingSize-1),
 				       hCoeffs().coeff(k), &workspace.coeffRef(0));
 	}

src/solver-dyn-reduced.cpp

@@ -744,7 +744,7 @@ namespace dynamicgraph
 	  V = Q [ 0 ; I ].
 	 */
 	EIGEN_MATRIX_FROM_MATRIX(V,mlV,nq,freeRank);
-	assert( freeRank == J.rows() );
+	assert( G_rank == J.rows() );
 	assert( Gt_qr.householderQ().cols()==nq && Gt_qr.householderQ().rows()==nq );
 	V.topRows(nq-freeRank).fill(0);
 	V.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
@@ -764,17 +764,21 @@ namespace dynamicgraph
 	const int & nf = sizeForcePointSOUT(t);
 	assert( X.rows()==nf );
 
-	X_qr.compute( (MatrixXd)((X*Jc).leftCols(6)));
+	/* J[:,1:6] = [ R_1  X_1 ; ... ; R_N X_N ]
+	 * with R_i the rotation of the frame where contact point i is expressed
+	 * and X_i the skew of the vector from waist to contact point. */
+	X_qr.compute( X*(Jc.leftCols(6)) );
 	X_qr.setThreshold(1e-3);
 	const unsigned int freeRank = nf-X_qr.rank();
 	assert( X_qr.rank()==6 );
-	//assert( X_qr.rank()==X.cols() );
 
 	sotDEBUG(5) << "JSb = " << (MATLAB)( (MatrixXd)((X*Jc).leftCols(6)) ) << endl;
-	sotDEBUG(5) << "Q = " << (MATLAB)X_qr.matrixQ()  << endl;
+	sotDEBUG(5) << "Q = " << (MATLAB)(MatrixXd)X_qr.householderQ()  << endl;
 
 	EIGEN_MATRIX_FROM_MATRIX(K,mlK,nf,freeRank);
-	K = X_qr.matrixQ().rightCols(freeRank);
+	K.topRows(X_qr.rank()).fill(0);
+	K.bottomRows(freeRank) = MatrixXd::Identity(freeRank,freeRank);
+	K.applyOnTheLeft( X_qr.householderQ() );
 
 	return mlK;
       }
@@ -945,27 +949,30 @@ namespace dynamicgraph
 
 	/* -1- */
 	{
-	  MatrixXd XJS = Xc*Jc.leftCols(6);
-	  sotDEBUG(15) << "XJS = "     << (MATLAB)XJS << std::endl;
-	  HouseholderQR<MatrixXd> qr(XJS);
-
-	  MatrixXd XJSp = qr.solve( MatrixXd::Identity(nf,nf) );
-	  MatrixXd XJSptSBV = -XJSp.transpose()
-	    * sBi.transpose().topLeftCorner(6,6).triangularView<Lower>() * V.ROWS_FF;
-	  VectorXd ref = XJSp.transpose()
-	    * (b.ROWS_FF
-	       + sBi.transpose().topLeftCorner(6,6).triangularView<Lower>()*drift.ROWS_FF);
-
-	  sotDEBUG(15) << "XJSp = "     << (MATLAB)XJSp << std::endl;
-	  sotDEBUG(15) << "XJSptSBV = "     << (MATLAB)XJSptSBV << std::endl;
-	  sotDEBUG(15) << "ref = "     << (MATLAB)ref << std::endl;
-
-	  assert( XJSptSBV.rows()==nbForce*3 && K.rows()==nbForce*3 && ref.size()==nbForce*3 );
+	  MatrixXd XJSptSBV2( nf,nu );
+	  XJSptSBV2.ROWS_FF
+	    = - (sBi.transpose().topLeftCorner(6,6).triangularView<Lower>() * V.ROWS_FF);
+	  XJSptSBV2.bottomRows( nf-6 ).setZero();
+	  sotDEBUG(15) << "SBitV = " << (MATLAB)XJSptSBV2 << std::endl;
+	  X_qr.solveTransposeInPlace( XJSptSBV2 );
+	  sotDEBUG(15) << "XJSptSBitV = " << (MATLAB)XJSptSBV2 << std::endl;
+
+	  VectorXd XJSptdelta( nf );
+	  XJSptdelta.ROWS_FF
+	    = b.ROWS_FF
+	    + sBi.transpose().topLeftCorner(6,6).triangularView<Lower>()*drift.ROWS_FF;
+	  XJSptdelta.bottomRows( nf-6 ).setZero();
+	  sotDEBUG(15) << "SBitdelta = " << (MATLAB)XJSptdelta << std::endl;
+	  X_qr.solveTransposeInPlace( XJSptdelta );
+	  sotDEBUG(15) << "XJSptSBitdelta = " << (MATLAB)XJSptdelta << std::endl;
+
+	  assert( XJSptSBV2.rows()==nbForce*3 && K.rows()==nbForce*3
+		  && XJSptdelta.size()==nbForce*3 );
 	  for( int i=0;i<nbForce;++i )
 	    {
-	      Cforce.COLS_U.row(i) = XJSptSBV.row(3*i+2);
+	      Cforce.COLS_U.row(i) = XJSptSBV2.row(3*i+2);
 	      Cforce.COLS_F.row(i) = K.row(3*i+2);
-	      bforce[i] = Bound( ref[3*i+2], Bound::BOUND_SUP );
+	      bforce[i] = Bound( XJSptdelta[3*i+2], Bound::BOUND_SUP );
 	    }
 	}
 	sotDEBUG(15) << "Cforce = "     << (MATLAB)Cforce << std::endl;
@@ -1093,6 +1100,16 @@ namespace dynamicgraph
 	sotDEBUG(40) << "B = " << (MATLAB)sB << std::endl;
 	sotDEBUG(40) << "u = " << (MATLAB)u << std::endl;
 	ddq = BV*u + B*drift;
+
+	/* --- verif --- */
+	{
+	  EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,JcSOUT(t));
+	  sotDEBUG(40) << "fdrift = " << (MATLAB)forceDrift << std::endl;
+	  sotDEBUG(40) << "Jqdd = " << (MATLAB)(MatrixXd)(Jc*ddq)  << std::endl;
+	  sotDEBUG(40) << "diff = " << (MATLAB)(MatrixXd)(Jc*ddq-forceDrift)  << std::endl;
+	  sotDEBUG(40) << "ndiff = " << (Jc*ddq-forceDrift).norm() << std::endl;
+	}
+
 	return mlddq;
       }
       ml::Vector& SolverDynReduced::

src/solver-dyn-reduced.h

@@ -162,14 +162,13 @@ namespace dynamicgraph {
 	  void refreshTaskTime( int time );
 	  void resizeSolver( void );
 	  void computeSizesForce( int t );
- 
+
 	private:
 	  typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
 	  hcod_ptr_t hsolver;
 
 	  int G_rank;
-	  Eigen::FullPivHouseholderQR<Eigen::MatrixXd> X_qr;
-	  Eigen::ColPivQRSolveInPlace Gt_qr;
+	  Eigen::ColPivQRSolveInPlace X_qr,Gt_qr;
 
 
 	  Eigen::MatrixXd Cforce,Czero;