//*************************************************************************************************************************
void wholeBodyReach::assertEqual(const MatrixRXd &A, const MatrixRXd &B, string testName, double tol)
{
if(A.cols() != B.cols() || A.rows()!=B.rows())
{
cout<< testName<< ": dim(A) != dim(B): " << A.rows()<< "x"<<A.cols()<<" != "<< B.rows()<< "x"<<B.cols()<<endl;
return testFailed(testName);
}
for(int r=0; r<A.rows(); r++)
for(int c=0; c<A.cols(); c++)
if(abs(A(r,c)-B(r,c))>tol)
{
printf("%s: element %d,%d is different, absolute difference is %f\n", testName.c_str(), r, c, abs(A(r,c)-B(r,c)));
return testFailed(testName);
}
}
//*************************************************************************************************************************
//************************************************* OLD STUFF *************************************************************
//*************************************************************************************************************************
void wholeBodyReach::pinvTrunc(const MatrixRXd &A, double tol, MatrixRXd &Apinv, MatrixRXd &Spinv, VectorXd &sv)
{
// allocate memory
int m = A.rows(), n = A.cols(), k = m<n?m:n;
Spinv.setZero(k,k);
// compute decomposition
JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD
sv = svd.singularValues();
// compute pseudoinverse of singular value matrix
for (int c=0;c<k; c++)
if ( sv(c)> tol)
Spinv(c,c) = 1/sv(c);
// compute pseudoinverse
Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose();
}
//*************************************************************************************************************************
void wholeBodyReach::pinvDampTrunc(const MatrixRXd &A, double tol, double damp, MatrixRXd &Apinv, MatrixRXd &ApinvDamp)
{
// allocate memory
int m = A.rows(), n = A.cols(), k = m<n?m:n;
MatrixRXd Spinv = MatrixRXd::Zero(k,k);
MatrixRXd SpinvD = MatrixRXd::Zero(k,k);
// compute decomposition
JacobiSVD<MatrixRXd> svd(A, ComputeThinU | ComputeThinV); // default Eigen SVD
VectorXd sv = svd.singularValues();
// compute pseudoinverses of singular value matrix
double damp2 = damp*damp;
for (int c=0;c<k; c++)
{
SpinvD(c,c) = sv(c) / (sv(c)*sv(c) + damp2);
if ( sv(c)> tol)
Spinv(c,c) = 1/sv(c);
}
// compute pseudoinverses
Apinv = svd.matrixV() * Spinv * svd.matrixU().transpose(); // truncated pseudoinverse
ApinvDamp = svd.matrixV() * SpinvD * svd.matrixU().transpose(); // damped pseudoinverse
}
