removed cout and added new CalcPinv cleaned

src/common_fc.cpp

@@ -8,51 +8,10 @@ CCommon_Fc::~CCommon_Fc(){}
 
 Eigen::MatrixXd CCommon_Fc::CalcPinv(const Eigen::MatrixXd& a, double epsilon)
 {
-  // Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  // double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
-  // Eigen::MatrixXd mpinv = svdd.matrixV() *  (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
-  // return mpinv;
-
-// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
-
-const Eigen::MatrixXd* m;
-Eigen::MatrixXd t;
-Eigen::MatrixXd m_pinv;
-
-// transpose so SVD decomp can work...
-if ( a.rows()<a.cols() )
-{
-  t = a.transpose();
-  m = &t;
-} 
-else 
-  m = &a;
-
-// SVD
-//JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
-Eigen::JacobiSVD<Eigen::MatrixXd> svd = m->jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
-Eigen::MatrixXd vSingular = svd.singularValues();
-// Build a diagonal matrix with the Inverted Singular values
-// The pseudo inverted singular matrix is easy to compute :
-// is formed by replacing every nonzero entry by its reciprocal (inversing).
-Eigen::MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
-for (int iRow =0; iRow<vSingular.rows(); iRow++) 
-{
-  if ( fabs(vSingular(iRow))<=epsilon )
-    vPseudoInvertedSingular(iRow,0)=0.;
-  else 
-    vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
-}
-// A little optimization here
-Eigen::MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
-// Pseudo-Inversion : V * S * U'
-m_pinv = (svd.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU  ;
-
-// transpose back if necessary
-if ( a.rows()<a.cols() )
-  return m_pinv.transpose();
-
-return m_pinv;
+  Eigen::JacobiSVD<Eigen::MatrixXd> svdd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  double tolerance = epsilon * std::max(a.cols(), a.rows()) *svdd.singularValues().array().abs()(0);
+  Eigen::MatrixXd mpinv = svdd.matrixV() *  (svdd.singularValues().array().abs() > tolerance).select(svdd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svdd.matrixU().adjoint();
+  return mpinv;
 }
 
 

src/kinematics.cpp

@@ -24,8 +24,6 @@ void CKinematics::UAMKine(const UAM::CQuad& quad, UAM::CArm& arm, UAM::CHT& HT,
   // Quadrotor Jacobian body to inertial frames
   Eigen::MatrixXd Jb2i = QuadJac(quad.pos);
 
-std::cout << __LINE__ << " [" << quad.pos.transpose() << "]" << std::endl;
-
   // Inertial to body frames
   Eigen::MatrixXd Ji2b = UAM::CCommon_Fc::CalcPinv(Jb2i);