diff --git a/sot-core3/Makefile b/sot-core3/Makefile
index a69be5158c4930b1875f731025c7c895a1311eac..84a885d370356dbc5268a0ae0f41820abb2d8b96 100644
--- a/sot-core3/Makefile
+++ b/sot-core3/Makefile
@@ -5,7 +5,7 @@
PKGNAME= sot-core-${VERSION}
DISTNAME= sot-core-${VERSION}
VERSION= 3.0.0
-PKGREVISION= 2
+PKGREVISION= 3
MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=sot-core/}
MASTER_REPOSITORY= ${MASTER_REPOSITORY_GITHUB}/proyan/sot-core
diff --git a/sot-core3/distinfo b/sot-core3/distinfo
index 77392a7b8c7877f25e8a7359ee7afb073154bf15..15286023dc839061ebb6a8765a67fb13b1d0680f 100644
--- a/sot-core3/distinfo
+++ b/sot-core3/distinfo
@@ -3,3 +3,4 @@ RMD160 (sot-core-3.0.0.tar.gz) = 9b7dc959a0550ab765b9f1ae5e8088c85653c0e3
Size (sot-core-3.0.0.tar.gz) = 365510 bytes
SHA1 (patch-aa) = 857c36de0e50f578ecd216577c2fc7afea68c7f2
SHA1 (patch-ab) = 85be5cdd6cbda1fd903085b59640ce11f854daea
+SHA1 (patch-ac) = 54b72948c5a379434655fe5b19dbf25e0341d7f5
diff --git a/sot-core3/patches/patch-ac b/sot-core3/patches/patch-ac
new file mode 100644
index 0000000000000000000000000000000000000000..ef185e9a6c448a6d4f46bd792c40d1f2eeef418a
--- /dev/null
+++ b/sot-core3/patches/patch-ac
@@ -0,0 +1,211 @@
+[PATCH] remove matrix initialization errors
+
+diff --git include/sot/core/matrix-svd.hh include/sot/core/matrix-svd.hh
+index b6e5cff..1830415 100644
+--- include/sot/core/matrix-svd.hh
++++ include/sot/core/matrix-svd.hh
+@@ -33,10 +33,9 @@ namespace dg = dynamicgraph;
+ /* --------------------------------------------------------------------- */
+ namespace Eigen {
+
+- void pseudoInverse( dg::Matrix& _inputMatrix,
++void pseudoInverse( dg::Matrix& _inputMatrix,
+ dg::Matrix& _inverseMatrix,
+- const double threshold = 1e-6)
+- {
++ const double threshold = 1e-6) {
+ JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
+ JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
+ JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
+@@ -47,14 +46,16 @@ namespace Eigen {
+ else singularValues_inv(i)=0;
+ }
+ _inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
+- }
++}
+
+- void dampedInverse( dg::Matrix& _inputMatrix,
++void dampedInverse( dg::Matrix& _inputMatrix,
+ dg::Matrix& _inverseMatrix,
+- const double threshold = 1e-6,
+- dg::Matrix* Uref = NULL,
+- dg::Vector* Sref = NULL,
+- dg::Matrix* Vref = NULL) {
++ dg::Matrix& Uref,
++ dg::Vector& Sref,
++ dg::Matrix& Vref,
++ const double threshold = 1e-6) {
++ sotDEBUGIN(15);
++ sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
+ JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
+ JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
+ JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
+@@ -64,15 +65,37 @@ namespace Eigen {
+ singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
+ else singularValues_inv(i)=0;
+ }
+- MatrixXd svd_matrixV = svd.matrixV();
+- MatrixXd svd_matrixU = svd.matrixU();
++ dg::Matrix matrix_U(svd.matrixU());
++ dg::Matrix matrix_V(svd.matrixV());
++ _inverseMatrix = (matrix_V*singularValues_inv.asDiagonal()*matrix_U.transpose());
++ Uref = matrix_U; Vref = matrix_V; Sref = m_singularValues;
+
+- _inverseMatrix = (svd_matrixV*singularValues_inv.asDiagonal()*svd_matrixU.transpose());
++ sotDEBUGOUT(15);
++}
+
+- if( Uref ) Uref = &svd_matrixU;
+- if( Vref ) Vref = &svd_matrixV;
+- if( Sref ) Sref = &singularValues_inv;
++void dampedInverse( dg::Matrix& _inputMatrix,
++ dg::Matrix& _inverseMatrix,
++ const double threshold = 1e-6) {
++ sotDEBUGIN(15);
++ sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
++ JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
++ JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
++ JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
++ singularValues_inv.resizeLike(m_singularValues);
++ for ( long i=0; i<m_singularValues.size(); ++i) {
++ if ( m_singularValues(i) > threshold )
++ singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
++ else singularValues_inv(i)=0;
+ }
++ dg::Matrix Uref(svd.matrixU());
++ dg::Matrix Vref(svd.matrixV());
++
++ _inverseMatrix = (Vref*singularValues_inv.asDiagonal()*Uref.transpose());
++
++ sotDEBUGOUT(15);
++}
++
++
+
+
+ }
+diff --git include/sot/core/sot.hh include/sot/core/sot.hh
+index d46d6de..8da9cd1 100644
+--- include/sot/core/sot.hh
++++ include/sot/core/sot.hh
+@@ -115,7 +115,7 @@ namespace dynamicgraph {
+
+ /*! Projection used to compute the control law. */
+ dg::Matrix Proj;
+-
++ //Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
+ /*! Force the recomputation at each step. */
+ bool recomputeEachTime;
+
+diff --git src/dynamic_graph/sot/core/meta_tasks.py src/dynamic_graph/sot/core/meta_tasks.py
+index d0e247c..b259e96 100644
+--- src/dynamic_graph/sot/core/meta_tasks.py
++++ src/dynamic_graph/sot/core/meta_tasks.py
+@@ -8,7 +8,7 @@ class MetaTaskCom(object):
+ def __init__(self,dyn,name="com"):
+ self.dyn=dyn
+ self.name=name
+- dyn.setProperty('ComputeCoM','true')
++ #dyn.setProperty('ComputeCoM','true')
+
+ self.feature = FeatureGeneric('feature'+name)
+ self.featureDes = FeatureGeneric('featureDes'+name)
+diff --git src/sot/sot.cpp src/sot/sot.cpp
+index bb27109..e8a052d 100644
+--- src/sot/sot.cpp
++++ src/sot/sot.cpp
+@@ -447,10 +447,11 @@ static void computeJacobianActivated( Task* taskSpec,
+ dynamicgraph::Vector Sot::
+ taskVectorToMlVector( const VectorMultiBound& taskVector )
+ {
+- dynamicgraph::Vector res(taskVector.size()); unsigned int i=0;
++ dynamicgraph::Vector res(taskVector.size()); res.setZero();
++ unsigned int i=0;
++
+ for( VectorMultiBound::const_iterator iter=taskVector.begin();
+- iter!=taskVector.end();++iter,++i )
+- {
++ iter!=taskVector.end();++iter,++i ) {
+ res(i)=iter->getSingleBound();
+ }
+ return res;
+@@ -475,11 +476,11 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
+ try {
+ control = q0SIN( iterTime );
+ sotDEBUG(15) << "initial velocity q0 = " << control << endl;
+- if( mJ!=control.size() ) { control.resize( mJ ); control.fill(.0); }
++ if( mJ!=control.size() ) { control.resize( mJ ); control.setZero(); }
+ }
+ catch (...)
+ {
+- if( mJ!=control.size() ) { control.resize( mJ ); }
++ if( mJ!=control.size() ) { control.resize( mJ );}
+ control.setZero();
+ sotDEBUG(25) << "No initial velocity." <<endl;
+ }
+@@ -561,7 +562,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
+ /***/sotCOUNTER(4,5); // Jt*S
+
+ /* --- PINV --- */
+- Eigen::dampedInverse(Jt,Jp,th,NULL,&S,&V);
++ Eigen::MatrixXd EMPTY(0,0);
++ Eigen::dampedInverse(Jt,Jp,EMPTY,S,V,th);
+ /***/sotCOUNTER(5,6); // PINV
+ sotDEBUG(2) << "V after dampedInverse." << V <<endl;
+ /* --- RANK --- */
+@@ -580,6 +582,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
+ //sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
+ sotDEBUG(45) << "S"<<iterTask<<" = "<< S<<endl;
+ sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
++ sotDEBUG(45) << "U"<<iterTask<<" = "<< EMPTY<<endl;
+
+ mem->jacobianInvSINOUT = Jp;
+ mem->jacobianInvSINOUT.setTime( iterTime );
+@@ -611,7 +614,6 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
+ control += Jp*(err - JK*control);
+ /***/sotCOUNTER(7,8); // QDOT
+
+-
+ /* --- OPTIMAL FORM: To debug. --- */
+ if( 0==iterTask )
+ { Proj.resize( mJ,mJ ); Proj.setIdentity(); }
+@@ -651,7 +653,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
+ sotDEBUG(2) << "V = " << V;
+ sotDEBUG(2) << "Jt = " << Jt;
+ sotDEBUG(2) << "JpxJt = " << Jp*Jt;
+- sotDEBUG(2) << "Ptmp" << iterTask <<" = " << Jp*Jt;
++ sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
+
+ /* NON OPTIMAL FORM: to be replaced after debug. */
+ if (1)
+diff --git src/task/task.cpp src/task/task.cpp
+index 2186bb3..6dcd926 100644
+--- src/task/task.cpp
++++ src/task/task.cpp
+@@ -197,7 +197,7 @@ computeError( dynamicgraph::Vector& error,int time )
+ /* First assumption: vector dimensions have not changed. If 0, they are
+ * initialized to dim 1.*/
+ int dimError = error .size();
+- if( 0==dimError ){ dimError = 1; error.resize(dimError); }
++ if( 0==dimError ){ dimError = 1; error.resize(dimError); error.setZero();}
+
+ dynamicgraph::Vector vectTmp;
+ int cursorError = 0;
+@@ -214,7 +214,7 @@ computeError( dynamicgraph::Vector& error,int time )
+
+ const int dim = partialError.size();
+ while( cursorError+dim>dimError ) // DEBUG It was >=
+- { dimError *= 2; error.resize(dimError); }
++ { dimError *= 2; error.resize(dimError); error.setZero(); }
+
+ for( int k=0;k<dim;++k ){ error(cursorError++) = partialError(k); }
+ sotDEBUG(35) << "feature: "<< partialError << std::endl;
+@@ -222,7 +222,7 @@ computeError( dynamicgraph::Vector& error,int time )
+ }
+
+ /* If too much memory has been allocated, resize. */
+- error .resize(cursorError);
++ error.conservativeResize(cursorError);
+ } catch SOT_RETHROW;
+
+ sotDEBUG(35) << "error_final: "<< error << std::endl;