Skip to content

Commit 136552f

Browse files
author
Olivier Stasse
authored
Merge pull request #50 from jmirabel/master
Avoid some dynamic allocations
2 parents 4699068 + 7f45756 commit 136552f

File tree

5 files changed

+62
-111
lines changed

5 files changed

+62
-111
lines changed

‎include/sot/core/matrix-svd.hh

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,19 @@ void pseudoInverse( dg::Matrix& _inputMatrix,
4848
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
4949
}
5050

51-
void dampedInverse( dg::Matrix& _inputMatrix,
51+
void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
52+
dg::Matrix& _inverseMatrix,
53+
const double threshold = 1e-6) {
54+
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
55+
ArrayWrapper<const SV_t> sigmas (svd.singularValues());
56+
57+
SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
58+
59+
_inverseMatrix.noalias() =
60+
( svd.matrixV() * sv_inv.asDiagonal() * svd.matrixU().transpose());
61+
}
62+
63+
void dampedInverse( const dg::Matrix& _inputMatrix,
5264
dg::Matrix& _inverseMatrix,
5365
dg::Matrix& Uref,
5466
dg::Vector& Sref,
@@ -57,37 +69,25 @@ void dampedInverse( dg::Matrix& _inputMatrix,
5769
sotDEBUGIN(15);
5870
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
5971
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
60-
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
61-
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
62-
singularValues_inv.resizeLike(m_singularValues);
63-
for ( long i=0; i<m_singularValues.size(); ++i) {
64-
singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
65-
}
66-
dg::Matrix matrix_U(svd.matrixU());
67-
dg::Matrix matrix_V(svd.matrixV());
68-
_inverseMatrix = (matrix_V*singularValues_inv.asDiagonal()*matrix_U.transpose());
69-
Uref = matrix_U; Vref = matrix_V; Sref = m_singularValues;
72+
73+
dampedInverse (svd, _inverseMatrix, threshold);
74+
75+
Uref = svd.matrixU();
76+
Vref = svd.matrixV();
77+
Sref = svd.singularValues();
7078

7179
sotDEBUGOUT(15);
7280
}
7381

74-
void dampedInverse( dg::Matrix& _inputMatrix,
82+
void dampedInverse( const dg::Matrix& _inputMatrix,
7583
dg::Matrix& _inverseMatrix,
7684
const double threshold = 1e-6) {
7785
sotDEBUGIN(15);
7886
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
87+
7988
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
80-
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
81-
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
82-
singularValues_inv.resizeLike(m_singularValues);
83-
for ( long i=0; i<m_singularValues.size(); ++i) {
84-
singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
85-
}
86-
dg::Matrix Uref(svd.matrixU());
87-
dg::Matrix Vref(svd.matrixV());
88-
89-
_inverseMatrix = (Vref*singularValues_inv.asDiagonal()*Uref.transpose());
90-
89+
dampedInverse (svd, _inverseMatrix, threshold);
90+
9191
sotDEBUGOUT(15);
9292
}
9393

‎include/sot/core/memory-task-sot.hh

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,10 @@ namespace dynamicgraph {
4646
dg::Matrix Jact; //( nJ,mJ ); // Activated part
4747
dg::Matrix JK; //(nJ,mJ);
4848

49-
dg::Matrix U,V;
50-
dg::Vector S;
49+
dg::Matrix V;
50+
51+
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
52+
SVD_t svd;
5153

5254
public:
5355
/* mJ is the number of actuated joints, nJ the number of feature in the task,

‎src/sot/memory-task-sot.cpp

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -63,9 +63,9 @@ initMemory( const unsigned int nJ,const unsigned int mJ,const unsigned int ffsiz
6363
Jact.resize( nJ,mJ );
6464

6565
JK.resize( nJ,mJ );
66-
U.resize( nJ,nJ );
6766
V.resize( mJ,mJ );
68-
S.resize( std::min( nJ,mJ ) );
67+
68+
svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeThinV);
6969

7070
JK.fill(0);
7171
if (atConstruction) {
@@ -75,9 +75,7 @@ initMemory( const unsigned int nJ,const unsigned int mJ,const unsigned int ffsiz
7575
Jff.setZero();
7676
Jact.setZero();
7777
JK.setZero();
78-
U.setZero();
7978
V.setZero();
80-
S.setZero();
8179
} else {
8280
Eigen::pseudoInverse(Jt,Jp);
8381
}

‎src/sot/sot.cpp

Lines changed: 27 additions & 81 deletions
Original file line numberDiff line numberDiff line change
@@ -383,7 +383,7 @@ static void computeJacobianActivated( Task* taskSpec,
383383
for( int i=0;i<Jt.cols();++i )
384384
{
385385
if(! controlSelec(i) )
386-
{for( int j=0;j<Jt.rows();++j ) { Jt(j,i)=0.; }}
386+
{ Jt.col (i).setZero(); }
387387
}
388388
}
389389
else
@@ -514,6 +514,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
514514
dynamicgraph::Matrix &V = mem->V;
515515
dynamicgraph::Matrix &JK = mem->JK;
516516
dynamicgraph::Matrix &Jt = mem->Jt;
517+
MemoryTaskSOT::SVD_t& svd = mem->svd;
517518

518519
Jp.resize( mJ,nJ );
519520
V.resize( mJ,mJ );
@@ -531,22 +532,19 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
531532
sotDEBUG(2) <<"Recompute inverse."<<endl;
532533

533534
/* --- FIRST ALLOCS --- */
534-
dynamicgraph::Vector &S = mem->S;
535-
536535
sotDEBUG(1) << "Size = "
537-
<< S.size() + mem->Jff.cols()*mem->Jff.rows()
536+
<< std::min(nJ, mJ) + mem->Jff.cols()*mem->Jff.rows()
538537
+ mem->Jact.cols()*mem->Jact.rows() << std::endl;
539538

540539
sotDEBUG(1) << std::endl;
541-
S.resize( std::min(nJ,mJ) );
542540
sotDEBUG(1) << "nJ=" << nJ << " " << "Jac.cols()=" << Jac.cols()
543541
<<" "<< "mJ=" << mJ<<std::endl;
544542
mem->Jff.resize( nJ,Jac.cols()-mJ ); // number dofs, number constraints
545543
sotDEBUG(1) << std::endl;
546544
mem->Jact.resize( nJ,mJ );
547545
sotDEBUG(1) << std::endl;
548546
sotDEBUG(1) << "Size = "
549-
<< S.size() + mem->Jff.cols()*mem->Jff.rows()
547+
<< std::min(nJ, mJ) + mem->Jff.cols()*mem->Jff.rows()
550548
+ mem->Jact.cols()*mem->Jact.rows() << std::endl;
551549

552550
/***/sotCOUNTER(1,2); // first allocs
@@ -556,22 +554,25 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
556554
/***/sotCOUNTER(2,3); // compute JK
557555

558556
/* --- COMPUTE Jt --- */
559-
if( 0<iterTask ) Jt = JK*Proj; else { Jt = JK; }
557+
if( 0<iterTask ) Jt.noalias() = JK*Proj; else { Jt = JK; }
560558
/***/sotCOUNTER(3,4); // compute Jt
561559

562560
/* --- COMPUTE S --- */
563561
computeJacobianActivated( dynamic_cast<Task*>( &task ),Jt,iterTime );
564562
/***/sotCOUNTER(4,5); // Jt*S
565563

566564
/* --- PINV --- */
567-
Eigen::MatrixXd EMPTY(0,0);
568-
Eigen::dampedInverse(Jt,Jp,EMPTY,S,V,th);
565+
svd.compute (Jt);
566+
Eigen::dampedInverse (svd, Jp, th);
567+
V.noalias() = svd.matrixV();
569568
/***/sotCOUNTER(5,6); // PINV
570569
sotDEBUG(2) << "V after dampedInverse." << V <<endl;
571570
/* --- RANK --- */
572571
{
573-
const unsigned int Jmax = S.size(); rankJ=0;
574-
for( unsigned i=0;i<Jmax;++i ) { if( S(i)>th ) rankJ++; }
572+
rankJ = 0;
573+
while ( rankJ < svd.singularValues().size()
574+
&& th < svd.singularValues()[rankJ])
575+
{ ++rankJ; }
575576
}
576577

577578
sotDEBUG(45) << "control"<<iterTask<<" = "<<control<<endl;
@@ -582,9 +583,9 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
582583
sotDEBUG(15) << "e"<<iterTask<<" = "<<err<<endl;
583584
sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
584585
//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
585-
sotDEBUG(45) << "S"<<iterTask<<" = "<< S<<endl;
586+
sotDEBUG(45) << "S"<<iterTask<<" = "<< svd.singularValues()<<endl;
586587
sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
587-
sotDEBUG(45) << "U"<<iterTask<<" = "<< EMPTY<<endl;
588+
sotDEBUG(45) << "U"<<iterTask<<" = "<< svd.matrixU()<<endl;
588589

589590
mem->jacobianInvSINOUT = Jp;
590591
mem->jacobianInvSINOUT.setTime( iterTime );
@@ -612,43 +613,17 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
612613
/* --- COMPUTE QDOT AND P --- */
613614
/*DEBUG: normally, the first iter (ie the test below)
614615
* is the same than the other, starting with control_0 = q0SIN. */
615-
if( iterTask==0 ) control += Jp*err; else
616-
control += Jp*(err - JK*control);
616+
if( iterTask==0 ) control.noalias() += Jp*err;
617+
else control += Jp*(err - JK*control);
617618
/***/sotCOUNTER(7,8); // QDOT
618619

619620
/* --- OPTIMAL FORM: To debug. --- */
620621
if( 0==iterTask )
621622
{ Proj.resize( mJ,mJ ); Proj.setIdentity(); }
622623

623-
// {
624-
// double *p,*v1,*v2,*vtmp1,*vtmp2;
625-
// p = traits::matrix_storage(Proj.matrix);
626-
// v1 = traits::matrix_storage(V.matrix);
627-
// v2 = traits::matrix_storage(V.matrix);
628-
// vtmp1 = traits::matrix_storage(V.matrix);
629-
// /***/sotCOUNTER(6,7); // Ppre
630-
631-
// for( unsigned int i=0;i<mJ;++i )
632-
// {
633-
// vtmp2 = traits::matrix_storage(V.matrix);
634-
// for( unsigned int j=0;j<mJ;++j )
635-
// {
636-
// v1 = vtmp1; v2 = vtmp2;
637-
// for( unsigned int k=0;k<rankJ;++k )
638-
// {
639-
// (*p) -=( *v1) * (*v2);
640-
// v2++;v1++;
641-
// }
642-
// p++; vtmp2 += mJ;
643-
// }
644-
// vtmp1 += mJ;
645-
// /***/sotCOUNTER(7,8); // P
646-
// }
647-
// }
648-
/* NON OPTIMAL FORM: to be replaced after debug. */
649-
// Proj-=Jp*Jt;
650-
651624
/* --- OLIVIER START --- */
625+
// Update by Joseph Mirabel to match Eigen API
626+
652627
sotDEBUG(2) << "Proj non optimal (rankJ= " <<rankJ
653628
<< ", iterTask =" << iterTask
654629
<< ")";
@@ -657,36 +632,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
657632
sotDEBUG(2) << "JpxJt = " << Jp*Jt;
658633
sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
659634

660-
/* NON OPTIMAL FORM: to be replaced after debug. */
661-
if (1)
662-
{
663-
double *p,*v1,*v2,*vtmp1,*vtmp2;
664-
p = MRAWDATA(Proj);
665-
v1 = MRAWDATA(V);
666-
v2 = MRAWDATA(V);
667-
vtmp1 = MRAWDATA(V);
668-
/***/sotCOUNTER(6,7); // Ppre
669-
670-
for( int i=0;i<mJ;++i )
671-
{
672-
vtmp2 = MRAWDATA(V);
673-
for( int j=0;j<mJ;++j )
674-
{
675-
v1 = vtmp1; v2 =vtmp2;
676-
for(unsigned int k=0;k<rankJ;++k )
677-
//for( unsigned int k=0;k<mJ;++k )
678-
{
679-
(*p) -=( *v1) * (*v2);
680-
v2+=mJ;v1+=mJ;
681-
}
682-
p++; vtmp2 ++;
683-
}
684-
vtmp1++;
685-
}
686-
/***/sotCOUNTER(7,8); // P
687-
}
688-
else
689-
{ Proj-=Jp*Jt;}
635+
Proj.noalias() -= svd.matrixV().leftCols(rankJ) * svd.matrixV().leftCols(rankJ).adjoint();
690636

691637
/* --- OLIVIER END --- */
692638

@@ -727,6 +673,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
727673
dynamicgraph::Matrix &Jp = mem->Jp;
728674
dynamicgraph::Matrix &PJp = mem->PJp;
729675
dynamicgraph::Matrix &Jt = mem->Jt;
676+
MemoryTaskSOT::SVD_t& svd = mem->svd;
730677

731678
mem->JK.resize( nJ,mJ );
732679
mem->Jt.resize( nJ,mJ );
@@ -743,18 +690,17 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
743690
sotDEBUG(35) << "Jgrad = " << JK <<endl;
744691

745692
// Use optimized-memory Jt to do the p-inverse.
746-
Jt=JK; Eigen::dampedInverse( Jt, Jp,th );
747-
PJp = Proj*Jp;
693+
Jt=JK;
694+
svd.compute (Jt);
695+
// TODO the two next lines could be replaced by
696+
Eigen::dampedInverse( svd, Jp,th );
697+
PJp.noalias() = Proj*Jp;
748698

749699
/* --- COMPUTE ERR --- */
750-
dynamicgraph::Vector Herr( err.size() );
751-
for( int i=0;i<err.size(); ++i )
752-
{
753-
Herr(i) = err(i);
754-
}
700+
const dynamicgraph::Vector& Herr( err );
755701

756702
/* --- COMPUTE CONTROL --- */
757-
control += PJp*Herr;
703+
control.noalias() += PJp*Herr;
758704

759705
/* --- TRACE --- */
760706
sotDEBUG(45) << "Pgrad = " << (PJp*Herr) <<endl;

‎src/task/task.cpp

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ Task( const std::string& n )
5050
,withDerivative(false)
5151
,controlGainSIN( NULL,"sotTask("+n+")::input(double)::controlGain" )
5252
,dampingGainSINOUT( NULL,"sotTask("+n+")::in/output(double)::damping" )
53+
// TODO As far as I understand, this is not used in this class.
5354
,controlSelectionSIN( NULL,"sotTask("+n+")::input(flag)::controlSelec" )
5455
,errorSOUT( boost::bind(&Task::computeError,this,_1,_2),
5556
sotNOSIGNAL,
@@ -250,7 +251,8 @@ computeErrorTimeDerivative( dynamicgraph::Vector & res, int time)
250251

251252
const dynamicgraph::Vector& partialErrorDot = feature.getErrorDot()(time);
252253
const int dim = partialErrorDot.size();
253-
for( int k=0;k<dim;++k ){ res(cursor++) = partialErrorDot(k); }
254+
res.segment (cursor, dim) = partialErrorDot;
255+
cursor += dim;
254256
}
255257

256258
return res;
@@ -315,6 +317,9 @@ computeJacobian( dynamicgraph::Matrix& J,int time )
315317

316318
while( cursorJ+nbr>=dimJ )
317319
{ dimJ *= 2; J.conservativeResize(dimJ,nbc); }
320+
// TODO If controlSelectionSIN is really to be removed,
321+
// then the following loop is equivalent to:
322+
// J.middleRows (cursorJ, nbr) = partialJacobian;
318323
for( int kc=0;kc<nbc;++kc )
319324
{
320325
// if( selection(kc) )

0 commit comments

Comments
 (0)