Skip to content

Commit a621254

Browse files
author
Rohan Budhiraja
committed
remove matrix initialization errors.
1 parent 5f34ee5 commit a621254

File tree

5 files changed

+78
-53
lines changed

5 files changed

+78
-53
lines changed

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

Lines changed: 61 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -33,46 +33,69 @@ namespace dg = dynamicgraph;
3333
/* --------------------------------------------------------------------- */
3434
namespace Eigen {
3535

36-
void pseudoInverse( dg::Matrix& _inputMatrix,
37-
dg::Matrix& _inverseMatrix,
38-
const double threshold = 1e-6)
39-
{
40-
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
41-
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
42-
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
43-
singularValues_inv.resizeLike(m_singularValues);
44-
for ( long i=0; i<m_singularValues.size(); ++i) {
45-
if ( m_singularValues(i) > threshold )
46-
singularValues_inv(i)=1.0/m_singularValues(i);
47-
else singularValues_inv(i)=0;
48-
}
49-
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
50-
}
36+
void pseudoInverse( dg::Matrix& _inputMatrix,
37+
dg::Matrix& _inverseMatrix,
38+
const double threshold = 1e-6) {
39+
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
40+
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
41+
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
42+
singularValues_inv.resizeLike(m_singularValues);
43+
for ( long i=0; i<m_singularValues.size(); ++i) {
44+
if ( m_singularValues(i) > threshold )
45+
singularValues_inv(i)=1.0/m_singularValues(i);
46+
else singularValues_inv(i)=0;
47+
}
48+
_inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
49+
}
50+
51+
void dampedInverse( dg::Matrix& _inputMatrix,
52+
dg::Matrix& _inverseMatrix,
53+
dg::Matrix& Uref,
54+
dg::Vector& Sref,
55+
dg::Matrix& Vref,
56+
const double threshold = 1e-6) {
57+
sotDEBUGIN(15);
58+
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
59+
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+
if ( m_singularValues(i) > threshold )
65+
singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
66+
else singularValues_inv(i)=0;
67+
}
68+
dg::Matrix matrix_U(svd.matrixU());
69+
dg::Matrix matrix_V(svd.matrixV());
70+
_inverseMatrix = (matrix_V*singularValues_inv.asDiagonal()*matrix_U.transpose());
71+
Uref = matrix_U; Vref = matrix_V; Sref = m_singularValues;
72+
73+
sotDEBUGOUT(15);
74+
}
75+
76+
void dampedInverse( dg::Matrix& _inputMatrix,
77+
dg::Matrix& _inverseMatrix,
78+
const double threshold = 1e-6) {
79+
sotDEBUGIN(15);
80+
sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
81+
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
82+
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
83+
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
84+
singularValues_inv.resizeLike(m_singularValues);
85+
for ( long i=0; i<m_singularValues.size(); ++i) {
86+
if ( m_singularValues(i) > threshold )
87+
singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
88+
else singularValues_inv(i)=0;
89+
}
90+
dg::Matrix Uref(svd.matrixU());
91+
dg::Matrix Vref(svd.matrixV());
92+
93+
_inverseMatrix = (Vref*singularValues_inv.asDiagonal()*Uref.transpose());
94+
95+
sotDEBUGOUT(15);
96+
}
5197

52-
void dampedInverse( dg::Matrix& _inputMatrix,
53-
dg::Matrix& _inverseMatrix,
54-
const double threshold = 1e-6,
55-
dg::Matrix* Uref = NULL,
56-
dg::Vector* Sref = NULL,
57-
dg::Matrix* Vref = NULL) {
58-
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
59-
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
60-
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
61-
singularValues_inv.resizeLike(m_singularValues);
62-
for ( long i=0; i<m_singularValues.size(); ++i) {
63-
if ( m_singularValues(i) > threshold )
64-
singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
65-
else singularValues_inv(i)=0;
66-
}
67-
MatrixXd svd_matrixV = svd.matrixV();
68-
MatrixXd svd_matrixU = svd.matrixU();
69-
70-
_inverseMatrix = (svd_matrixV*singularValues_inv.asDiagonal()*svd_matrixU.transpose());
7198

72-
if( Uref ) Uref = &svd_matrixU;
73-
if( Vref ) Vref = &svd_matrixV;
74-
if( Sref ) Sref = &singularValues_inv;
75-
}
7699

77100

78101
}

‎include/sot/core/sot.hh

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,7 @@ namespace dynamicgraph {
115115

116116
/*! Projection used to compute the control law. */
117117
dg::Matrix Proj;
118-
118+
//Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
119119
/*! Force the recomputation at each step. */
120120
bool recomputeEachTime;
121121

‎src/dynamic_graph/sot/core/meta_tasks.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ class MetaTaskCom(object):
88
def __init__(self,dyn,name="com"):
99
self.dyn=dyn
1010
self.name=name
11-
dyn.setProperty('ComputeCoM','true')
11+
#dyn.setProperty('ComputeCoM','true')
1212

1313
self.feature = FeatureGeneric('feature'+name)
1414
self.featureDes = FeatureGeneric('featureDes'+name)

‎src/sot/sot.cpp

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -447,12 +447,13 @@ static void computeJacobianActivated( Task* taskSpec,
447447
dynamicgraph::Vector Sot::
448448
taskVectorToMlVector( const VectorMultiBound& taskVector )
449449
{
450-
dynamicgraph::Vector res(taskVector.size()); unsigned int i=0;
450+
dynamicgraph::Vector res(taskVector.size()); res.setZero();
451+
unsigned int i=0;
452+
451453
for( VectorMultiBound::const_iterator iter=taskVector.begin();
452-
iter!=taskVector.end();++iter,++i )
453-
{
454-
res(i)=iter->getSingleBound();
455-
}
454+
iter!=taskVector.end();++iter,++i ) {
455+
res(i)=iter->getSingleBound();
456+
}
456457
return res;
457458
}
458459

@@ -475,11 +476,11 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
475476
try {
476477
control = q0SIN( iterTime );
477478
sotDEBUG(15) << "initial velocity q0 = " << control << endl;
478-
if( mJ!=control.size() ) { control.resize( mJ ); control.fill(.0); }
479+
if( mJ!=control.size() ) { control.resize( mJ ); control.setZero(); }
479480
}
480481
catch (...)
481482
{
482-
if( mJ!=control.size() ) { control.resize( mJ ); }
483+
if( mJ!=control.size() ) { control.resize( mJ );}
483484
control.setZero();
484485
sotDEBUG(25) << "No initial velocity." <<endl;
485486
}
@@ -561,7 +562,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
561562
/***/sotCOUNTER(4,5); // Jt*S
562563

563564
/* --- PINV --- */
564-
Eigen::dampedInverse(Jt,Jp,th,NULL,&S,&V);
565+
Eigen::MatrixXd EMPTY(0,0);
566+
Eigen::dampedInverse(Jt,Jp,EMPTY,S,V,th);
565567
/***/sotCOUNTER(5,6); // PINV
566568
sotDEBUG(2) << "V after dampedInverse." << V <<endl;
567569
/* --- RANK --- */
@@ -580,6 +582,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
580582
//sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
581583
sotDEBUG(45) << "S"<<iterTask<<" = "<< S<<endl;
582584
sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
585+
sotDEBUG(45) << "U"<<iterTask<<" = "<< EMPTY<<endl;
583586

584587
mem->jacobianInvSINOUT = Jp;
585588
mem->jacobianInvSINOUT.setTime( iterTime );
@@ -611,7 +614,6 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
611614
control += Jp*(err - JK*control);
612615
/***/sotCOUNTER(7,8); // QDOT
613616

614-
615617
/* --- OPTIMAL FORM: To debug. --- */
616618
if( 0==iterTask )
617619
{ Proj.resize( mJ,mJ ); Proj.setIdentity(); }
@@ -651,7 +653,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
651653
sotDEBUG(2) << "V = " << V;
652654
sotDEBUG(2) << "Jt = " << Jt;
653655
sotDEBUG(2) << "JpxJt = " << Jp*Jt;
654-
sotDEBUG(2) << "Ptmp" << iterTask <<" = " << Jp*Jt;
656+
sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
655657

656658
/* NON OPTIMAL FORM: to be replaced after debug. */
657659
if (1)

‎src/task/task.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -197,7 +197,7 @@ computeError( dynamicgraph::Vector& error,int time )
197197
/* First assumption: vector dimensions have not changed. If 0, they are
198198
* initialized to dim 1.*/
199199
int dimError = error .size();
200-
if( 0==dimError ){ dimError = 1; error.resize(dimError); }
200+
if( 0==dimError ){ dimError = 1; error.resize(dimError); error.setZero();}
201201

202202
dynamicgraph::Vector vectTmp;
203203
int cursorError = 0;
@@ -214,15 +214,15 @@ computeError( dynamicgraph::Vector& error,int time )
214214

215215
const int dim = partialError.size();
216216
while( cursorError+dim>dimError ) // DEBUG It was >=
217-
{ dimError *= 2; error.resize(dimError); }
217+
{ dimError *= 2; error.resize(dimError); error.setZero(); }
218218

219219
for( int k=0;k<dim;++k ){ error(cursorError++) = partialError(k); }
220220
sotDEBUG(35) << "feature: "<< partialError << std::endl;
221221
sotDEBUG(35) << "error: "<< error << std::endl;
222222
}
223223

224224
/* If too much memory has been allocated, resize. */
225-
error .resize(cursorError);
225+
error.conservativeResize(cursorError);
226226
} catch SOT_RETHROW;
227227

228228
sotDEBUG(35) << "error_final: "<< error << std::endl;

0 commit comments

Comments
 (0)