int solveLinearEquationLU(dmatrix a, const dmatrix &b, dmatrix &out_x) { assert(a.rows() == a.cols() && a.cols() == b.rows() ); out_x = b; const int n = (int)a.rows(); const int nrhs = (int)b.cols(); int info; std::vector<int> ipiv(n); #ifndef USE_CLAPACK_INTERFACE int lda = n; int ldb = n; dgesv_(&n, &nrhs, &(a(0,0)), &lda, &(ipiv[0]), &(out_x(0,0)), &ldb, &info); #else info = clapack_dgesv(CblasColMajor, n, nrhs, &(a(0,0)), n, &(ipiv[0]), &(out_x(0,0)), n); #endif assert(info == 0); return info; }
//----- Calculation of eigen vectors and eigen values ----- int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval) { assert( _a.cols() == _a.rows() ); typedef dmatrix mlapack; typedef dvector vlapack; mlapack a = _a; // <- mlapack evec = _evec; vlapack eval = _eval; int n = (int)_a.cols(); double *wi = new double[n]; double *vl = new double[n*n]; double *work = new double[4*n]; int lwork = 4*n; int info; dgeev_("N","V", &n, &(a(0,0)), &n, &(eval(0)), wi, vl, &n, &(evec(0,0)), &n, work, &lwork, &info); _evec = evec.transpose(); _eval = eval; delete [] wi; delete [] vl; delete [] work; return info; }
//--- Calculation of determinamt --- double det(const dmatrix &_a) { assert( _a.cols() == _a.rows() ); typedef dmatrix mlapack; mlapack a = _a; // <- int info; int n = (int)a.cols(); int lda = n; std::vector<int> ipiv(n); #ifdef USE_CLAPACK_INTERFACE info = clapack_dgetrf(CblasColMajor, n, n, &(a(0,0)), lda, &(ipiv[0])); #else dgetrf_(&n, &n, &a(0,0), &lda, &(ipiv[0]), &info); #endif double det=1.0; for(int i=0; i < n-1; i++) if(ipiv[i] != i+1) det = -det; for(int i=0; i < n; i++) det *= a(i,i); assert(info == 0); return det; }
/** Unified interface to solve a linear equation */ int solveLinearEquation(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio) { if(_a.cols() == _a.rows()) return solveLinearEquationLU(_a, _b, _x); else return solveLinearEquationSVD(_a, _b, _x, _sv_ratio); }
int hrp::calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w) { // J# = W Jt(J W Jt + kI)-1 (Weighted SR-Inverse) // SR-inverse : // Y. Nakamura and H. Hanafusa : "Inverse Kinematic Solutions With // Singularity Robustness for Robot Manipulator Control" // J. Dyn. Sys., Meas., Control 1986. vol 108, Issue 3, pp. 163--172. const int c = _a.rows(); // 6 const int n = _a.cols(); // n if ( _w.cols() != n || _w.rows() != n ) { _w = dmatrix::Identity(n, n); } dmatrix at = _a.transpose(); dmatrix a1(c, c); a1 = (_a * _w * at + _sr_ratio * dmatrix::Identity(c,c)).inverse(); //if (DEBUG) { dmatrix aat = _a * at; std::cerr << " a*at :" << std::endl << aat; } _a_sr = _w * at * a1; //if (DEBUG) { dmatrix ii = _a * _a_sr; std::cerr << " i :" << std::endl << ii; } }
/** calculate the mass matrix using the unit vector method \todo replace the unit vector method here with a more efficient method that only requires O(n) computation time The motion equation (dv != dvo) | | | dv | | | | fext | | out_M | * | dw | + | b1 | = | tauext | | | |ddq | | | | u | */ void Body::calcMassMatrix(dmatrix& out_M) { // buffers for the unit vector method dmatrix b1; dvector ddqorg; dvector uorg; Vector3 dvoorg; Vector3 dworg; Vector3 root_w_x_v; Vector3 g(0, 0, 9.8); uint nJ = numJoints(); int totaldof = nJ; if( !isStaticModel_ ) totaldof += 6; out_M.resize(totaldof,totaldof); b1.resize(totaldof, 1); // preserve and clear the joint accelerations ddqorg.resize(nJ); uorg.resize(nJ); for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ddqorg[i] = ptr->ddq; uorg [i] = ptr->u; ptr->ddq = 0.0; } // preserve and clear the root link acceleration dvoorg = rootLink_->dvo; dworg = rootLink_->dw; root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p)); rootLink_->dvo = g - root_w_x_v; // dv = g, dw = 0 rootLink_->dw.setZero(); setColumnOfMassMatrix(b1, 0); if( !isStaticModel_ ){ for(int i=0; i < 3; ++i){ rootLink_->dvo[i] += 1.0; setColumnOfMassMatrix(out_M, i); rootLink_->dvo[i] -= 1.0; } for(int i=0; i < 3; ++i){ rootLink_->dw[i] = 1.0; Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p); // spatial acceleration caused by ang. acc. rootLink_->dvo -= dw_x_p; setColumnOfMassMatrix(out_M, i + 3); rootLink_->dvo += dw_x_p; rootLink_->dw[i] = 0.0; } } for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ptr->ddq = 1.0; int j = i + 6; setColumnOfMassMatrix(out_M, j); out_M(j, j) += ptr->Jm2; // motor inertia ptr->ddq = 0.0; } // subtract the constant term for(size_t i = 0; i < (size_t)out_M.cols(); ++i){ out_M.col(i) -= b1; } // recover state for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ptr->ddq = ddqorg[i]; ptr->u = uorg [i]; } rootLink_->dvo = dvoorg; rootLink_->dw = dworg; }
/** solve linear equation using LU decomposition by lapack library DGESVX (_a must be square matrix) */ int solveLinearEquationLU(const dmatrix &_a, const dvector &_b, dvector &_x) { assert(_a.cols() == _a.rows() && _a.cols() == _b.size() ); int n = (int)_a.cols(); int nrhs = 1; int lda = n; std::vector<int> ipiv(n); int ldb = n; int info; // compute the solution #ifndef USE_CLAPACK_INTERFACE char fact = 'N'; char transpose = 'N'; double *af = new double[n*n]; int ldaf = n; char equed = 'N'; double *r = new double[n]; double *c = new double[n]; int ldx = n; double rcond; double *ferr = new double[nrhs]; double *berr = new double[nrhs]; double *work = new double[4*n]; int *iwork = new int[n]; _x.resize(n); // memory allocation for the return vector dgesvx_(&fact, &transpose, &n, &nrhs, const_cast<double *>(&(_a(0,0))), &lda, af, &ldaf, &(ipiv[0]), &equed, r, c, const_cast<double *>(&(_b(0))), &ldb, &(_x(0)), &ldx, &rcond, ferr, berr, work, iwork, &info); delete [] iwork; delete [] work; delete [] berr; delete [] ferr; delete [] c; delete [] r; delete [] af; #else _x = _b; info = clapack_dgesv(CblasColMajor, n, nrhs, const_cast<double *>(&(a(0,0))), lda, &(ipiv[0]), &(_x(0)), ldb); #endif return info; }
/** calculate Pseudo-Inverse using SVD(Singular Value Decomposition) by lapack library DGESVD (_a can be non-square matrix) */ int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio) { int i, j, k; char jobu = 'A'; char jobvt = 'A'; int m = (int)_a.rows(); int n = (int)_a.cols(); int max_mn = max(m,n); int min_mn = min(m,n); dmatrix a(m,n); a = _a; int lda = m; double *s = new double[max_mn]; int ldu = m; double *u = new double[ldu*m]; int ldvt = n; double *vt = new double[ldvt*n]; int lwork = max(3*min_mn+max_mn, 5*min_mn); // for CLAPACK ver.2 & ver.3 double *work = new double[lwork]; int info; for(i = 0; i < max_mn; i++) s[i] = 0.0; dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work, &lwork, &info); double smin, smax=0.0; for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j]; smin = smax*_sv_ratio; // default _sv_ratio is 1.0e-3 for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0; //------------ calculate pseudo inverse pinv(A) = V*S^(-1)*U^(T) // S^(-1)*U^(T) for (j = 0; j < m; j++){ if (s[j]){ for (i = 0; i < m; i++) u[j*m+i] /= s[j]; } else { for (i = 0; i < m; i++) u[j*m+i] = 0.0; } } // V * (S^(-1)*U^(T)) _a_pseu.resize(n,m); for(j = 0; j < n; j++){ for(i = 0; i < m; i++){ _a_pseu(j,i) = 0.0; for(k = 0; k < min_mn; k++){ if(s[k]) _a_pseu(j,i) += vt[j*n+k] * u[k*m+i]; } } } delete [] work; delete [] vt; delete [] s; delete [] u; return info; }
/** solve linear equation using SVD(Singular Value Decomposition) by lapack library DGESVD (_a can be non-square matrix) */ int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio) { const int m = _a.rows(); const int n = _a.cols(); assert( m == static_cast<int>(_b.size()) ); _x.resize(n); int i, j; char jobu = 'A'; char jobvt = 'A'; int max_mn = max(m,n); int min_mn = min(m,n); dmatrix a(m,n); a = _a; int lda = m; double *s = new double[max_mn]; // singular values int ldu = m; double *u = new double[ldu*m]; int ldvt = n; double *vt = new double[ldvt*n]; int lwork = max(3*min_mn+max_mn, 5*min_mn); // for CLAPACK ver.2 & ver.3 double *work = new double[lwork]; int info; for(i = 0; i < max_mn; i++) s[i] = 0.0; dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work, &lwork, &info); double tmp; double smin, smax=0.0; for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j]; smin = smax*_sv_ratio; // 1.0e-3; for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0; double *utb = new double[m]; // U^T*b for (j = 0; j < m; j++){ tmp = 0; if (s[j]){ for (i = 0; i < m; i++) tmp += u[j*m+i] * _b(i); tmp /= s[j]; } utb[j] = tmp; } // v*utb for (j = 0; j < n; j++){ tmp = 0; for (i = 0; i < n; i++){ if(s[i]) tmp += utb[i] * vt[j*n+i]; } _x(j) = tmp; } delete [] utb; delete [] work; delete [] vt; delete [] s; delete [] u; return info; }