From d3fb85788536f1008a62df8170361ed5655655fa Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 29 Aug 2011 16:41:34 +0200
Subject: [PATCH] Unified the QR and pseudo inversion.
---
python/dynreduced.py | 9 ++---
src/col-piv-qr-solve-in-place.h | 16 +++++++--
src/solver-dyn-reduced.cpp | 63 +++++++++++++++++++++------------
src/solver-dyn-reduced.h | 5 ++-
4 files changed, 60 insertions(+), 33 deletions(-)
diff --git a/python/dynreduced.py b/python/dynreduced.py
index 176d9a7..dda7edf 100644
--- a/python/dynreduced.py
+++ b/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
diff --git a/src/col-piv-qr-solve-in-place.h b/src/col-piv-qr-solve-in-place.h
index 2779c07..77773de 100644
--- a/src/col-piv-qr-solve-in-place.h
+++ b/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));
}
diff --git a/src/solver-dyn-reduced.cpp b/src/solver-dyn-reduced.cpp
index 17ae8cf..55e366b 100644
--- a/src/solver-dyn-reduced.cpp
+++ b/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::
diff --git a/src/solver-dyn-reduced.h b/src/solver-dyn-reduced.h
index 7112f0f..a4a9969 100644
--- a/src/solver-dyn-reduced.h
+++ b/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;
--
GitLab