Skip to content

Commit 324062c

Browse files
jmirabeljmirabel
committed
Remove some dynamic allocation.
1 parent ac4295e commit 324062c

File tree

4 files changed

+22
-27
lines changed

4 files changed

+22
-27
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ namespace dynamicgraph {
4747
dg::Matrix Jact; //( nJ,mJ ); // Activated part
4848
dg::Matrix JK; //(nJ,mJ);
4949

50-
dg::Matrix V;
50+
dg::Matrix Proj;
5151

5252
typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
5353
SVD_t svd;

‎include/sot/core/sot.hh

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -112,8 +112,6 @@ namespace dynamicgraph {
112112
/*! \brief Store a pointer to compute the gradient */
113113
TaskAbstract* taskGradient;
114114

115-
/*! Projection used to compute the control law. */
116-
dg::Matrix Proj;
117115
//Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
118116
/*! Force the recomputation at each step. */
119117
bool recomputeEachTime;

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

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,6 @@ initMemory( const Matrix::Index nJ,const Matrix::Index mJ,const Matrix::Index ff
6363
Jact.resize( nJ,mJ );
6464

6565
JK.resize( nJ,mJ );
66-
V.resize( mJ,mJ );
6766

6867
svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
6968

@@ -75,7 +74,6 @@ initMemory( const Matrix::Index nJ,const Matrix::Index mJ,const Matrix::Index ff
7574
Jff.setZero();
7675
Jact.setZero();
7776
JK.setZero();
78-
V.setZero();
7977
} else {
8078
Eigen::pseudoInverse(Jt,Jp);
8179
}

‎src/sot/sot.cpp

Lines changed: 21 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -484,6 +484,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
484484

485485
sotDEBUGF(5, " --- Time %d -------------------", iterTime );
486486
unsigned int iterTask = 0;
487+
const Matrix* PrevProj = NULL;
487488
for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
488489
{
489490
sotDEBUGF(5,"Rank %d.",iterTask);
@@ -504,17 +505,16 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
504505
task.memoryInternal = mem;
505506
}
506507

507-
dynamicgraph::Matrix &Jp = mem->Jp;
508-
dynamicgraph::Matrix &V = mem->V;
509-
dynamicgraph::Matrix &JK = mem->JK;
510-
dynamicgraph::Matrix &Jt = mem->Jt;
508+
Matrix &Jp = mem->Jp;
509+
Matrix &JK = mem->JK;
510+
Matrix &Jt = mem->Jt;
511+
Matrix &Proj = mem->Proj;
511512
MemoryTaskSOT::SVD_t& svd = mem->svd;
512513

513514
taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
514515
const dynamicgraph::Vector &err = mem->err;
515516

516517
Jp.resize( mJ,nJ );
517-
V.resize( mJ,mJ );
518518
Jt.resize( nJ,mJ );
519519
JK.resize( nJ,mJ );
520520

@@ -551,7 +551,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
551551
/***/sotCOUNTER(2,3); // compute JK
552552

553553
/* --- COMPUTE Jt --- */
554-
if( 0<iterTask ) Jt.noalias() = JK*Proj; else { Jt = JK; }
554+
if( 0<iterTask ) Jt.noalias() = JK*(*PrevProj); else { Jt = JK; }
555555
/***/sotCOUNTER(3,4); // compute Jt
556556

557557
/* --- COMPUTE S --- */
@@ -561,9 +561,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
561561
/* --- PINV --- */
562562
svd.compute (Jt);
563563
Eigen::dampedInverse (svd, Jp, th);
564-
V.noalias() = svd.matrixV();
565564
/***/sotCOUNTER(5,6); // PINV
566-
sotDEBUG(2) << "V after dampedInverse." << V <<endl;
565+
sotDEBUG(20) << "V after dampedInverse." << svd.matrixV() <<endl;
567566
/* --- RANK --- */
568567
{
569568
rankJ = 0;
@@ -581,7 +580,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
581580
sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
582581
//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
583582
sotDEBUG(45) << "S"<<iterTask<<" = "<< svd.singularValues()<<endl;
584-
sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
583+
sotDEBUG(45) << "V"<<iterTask<<" = "<< svd.matrixV()<<endl;
585584
sotDEBUG(45) << "U"<<iterTask<<" = "<< svd.matrixU()<<endl;
586585

587586
mem->jacobianInvSINOUT = Jp;
@@ -590,7 +589,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
590589
mem->jacobianConstrainedSINOUT.setTime( iterTime );
591590
mem->jacobianProjectedSINOUT = Jt;
592591
mem->jacobianProjectedSINOUT.setTime( iterTime );
593-
mem->singularBaseImageSINOUT = V;
592+
mem->singularBaseImageSINOUT = svd.matrixV().leftCols(rankJ);
594593
mem->singularBaseImageSINOUT.setTime( iterTime );
595594
mem->rankSINOUT = rankJ;
596595
mem->rankSINOUT.setTime( iterTime );
@@ -600,27 +599,23 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
600599
sotDEBUG(2)<<"Inverse not recomputed."<<endl;
601600
rankJ = mem->rankSINOUT.accessCopy();
602601
Jp = mem->jacobianInvSINOUT.accessCopy();
603-
V = mem->singularBaseImageSINOUT.accessCopy();
604602
JK = mem->jacobianConstrainedSINOUT.accessCopy ();
605603
Jt = mem->jacobianProjectedSINOUT.accessCopy ();
606604
}
607605
/***/sotCOUNTER(6,7); // TRACE
608606

609-
if (rankJ == 0) break;
610-
611607
/* --- COMPUTE QDOT AND P --- */
612608
/*DEBUG: normally, the first iter (ie the test below)
613609
* is the same than the other, starting with control_0 = q0SIN. */
614610
if( iterTask==0 ) control.noalias() += Jp*err;
615-
else control += Proj * (Jp*(err - JK*control));
611+
else control += *PrevProj * (Jp*(err - JK*control));
616612
/***/sotCOUNTER(7,8); // QDOT
617613

618614
/* --- OPTIMAL FORM: To debug. --- */
619615
if( 0==iterTask ) {
620-
Proj.resize( mJ,mJ ); Proj.setIdentity();
616+
Proj.noalias() = svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
621617
} else {
622-
// Proj.noalias() -= svd.matrixV().leftCols(rankJ) * svd.matrixV().leftCols(rankJ).adjoint();
623-
Proj = Proj * svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
618+
Proj.noalias() = *PrevProj * svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
624619
}
625620

626621
/* --- OLIVIER START --- */
@@ -629,16 +624,17 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
629624
sotDEBUG(2) << "Proj non optimal (rankJ= " <<rankJ
630625
<< ", iterTask =" << iterTask
631626
<< ")";
632-
sotDEBUG(2) << "V = " << V;
633-
sotDEBUG(2) << "Jt = " << Jt;
634-
sotDEBUG(2) << "JpxJt = " << Jp*Jt;
627+
sotDEBUG(20) << "V = " << svd.matrixV();
628+
sotDEBUG(20) << "Jt = " << Jt;
629+
sotDEBUG(20) << "JpxJt = " << Jp*Jt;
635630
sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
636631

637632
/* --- OLIVIER END --- */
638633

639634
sotDEBUG(15) << "q"<<iterTask<<" = "<<control<<std::endl;
640635
sotDEBUG(25) << "P"<<iterTask<<" = "<< Proj <<endl;
641636
iterTask++;
637+
PrevProj = &Proj;
642638

643639
sotPRINTCOUNTER(1); sotPRINTCOUNTER(2); sotPRINTCOUNTER(3);
644640
sotPRINTCOUNTER(4); sotPRINTCOUNTER(5); sotPRINTCOUNTER(6);
@@ -695,7 +691,10 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
695691
svd.compute (Jt);
696692
// TODO the two next lines could be replaced by
697693
Eigen::dampedInverse( svd, Jp,th );
698-
PJp.noalias() = Proj*Jp;
694+
if (PrevProj != NULL)
695+
PJp.noalias() = (*PrevProj)*Jp;
696+
else
697+
PJp.noalias() = Jp;
699698

700699
/* --- COMPUTE ERR --- */
701700
const dynamicgraph::Vector& Herr( err );
@@ -705,7 +704,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
705704

706705
/* --- TRACE --- */
707706
sotDEBUG(45) << "Pgrad = " << (PJp*Herr) <<endl;
708-
sotDEBUG(45) << "P = " << Proj <<endl;
707+
if (PrevProj != NULL) { sotDEBUG(45) << "P = " << *PrevProj <<endl; }
709708
sotDEBUG(45) << "Jp = " << Jp <<endl;
710709
sotDEBUG(45) << "PJp = " << PJp <<endl;
711710
}

0 commit comments

Comments
 (0)