//************************************************************************************************************************* 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 }