MatrixSolvers.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00010 #include <iostream>
00011 #include <vector>
00012 #include "MatrixSolvers.h"
00013 
00014 using namespace std;
00015 
00016 #ifdef USE_CLAPACK_INTERFACE
00017 extern "C" {
00018 #include <cblas.h>
00019 #include <clapack.h>
00020 };
00021 #else
00022 extern "C" void dgesvx_(char *fact, char *trans, int *n,
00023                                                 int *nrhs, double *a, int *lda,
00024                                                 double *af, int *ldaf, int *ipiv,
00025                                                 char *equed, double *r, double *c,
00026                                                 double *b, int *ldb, double *x,
00027                                                 int *ldx, double *rcond, double *ferr,
00028                                                 double *berr, double *work, int *iwork,
00029                                                 int *info);
00030 
00031 extern "C" void dgesv_(const int* n, const int* nrhs, 
00032                                            double* a, int* lda, int* ipiv, 
00033                                            double* b, int* ldb, int* info);
00034 
00035 extern "C" void dgetrf_( int *m, int *n, double *a, int *lda, int *ipiv, int *info);
00036 #endif
00037 
00038 extern "C" void dgesvd_(char const* jobu, char const* jobvt, 
00039                                                 int const* m, int const* n, double* a, int const* lda, 
00040                                                 double* s, double* u, int const* ldu, 
00041                                                 double* vt, int const* ldvt,
00042                                                 double* work, int const* lwork, int* info);
00043 
00044 extern "C" void dgeev_(char const*jobvl, char const*jobvr, int *n, double *A, 
00045                                           int *lda, double *wr,double *wi, double *vl, 
00046                                           int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info);
00047 
00048 
00049 // originally in hrpCLAPACK.{cpp,h}
00050 // solveLinearEquation()
00051 // b = a * x, x = b^(-1) * a
00052 namespace hrp {
00053                 
00054                 static inline int max(int a, int b) { return (a >= b) ? a : b; }
00055                 static inline int min(int a, int b) { return (a <= b) ? a : b; }
00056                 
00057 
00058                 int solveLinearEquationLU(dmatrix a, const dmatrix &b, dmatrix &out_x)
00059                 {
00060                                 assert(a.rows() == a.cols() && a.cols() == b.rows() );
00061 
00062                                 out_x = b;
00063 
00064                                 const int n = a.rows();
00065                                 const int nrhs = b.cols();
00066                                 int info;
00067                                 std::vector<int> ipiv(n);
00068 
00069 #ifndef USE_CLAPACK_INTERFACE
00070 
00071                                 int lda = n;
00072                                 int ldb = n;
00073                                 dgesv_(&n, &nrhs, &(a(0,0)), &lda, &(ipiv[0]), &(out_x(0,0)), &ldb, &info);
00074 #else
00075                                 info = clapack_dgesv(CblasColMajor,
00076                                                                          n, nrhs, &(a(0,0)), n, 
00077                                                                          &(ipiv[0]),
00078                                                                          &(out_x(0,0)),
00079                                                                          n);
00080 #endif
00081                                 assert(info == 0);
00082                                 
00083                                 return info;
00084                 }
00085                                 
00086                 
00091                 int solveLinearEquationLU(const dmatrix &_a, const dvector &_b, dvector &_x)
00092                 {
00093                                 assert(_a.cols() == _a.rows() && _a.cols() == _b.size() );
00094 
00095                                 int n = (int)_a.cols();
00096                                 int nrhs = 1;
00097 
00098                                 int lda = n;
00099 
00100                                 std::vector<int> ipiv(n);
00101 
00102                                 int ldb = n;
00103 
00104                                 int info;
00105 
00106                                 // compute the solution
00107 #ifndef USE_CLAPACK_INTERFACE
00108                                 char fact      = 'N';
00109                                 char transpose = 'N';
00110 
00111                                 double *af = new double[n*n];
00112 
00113                                 int ldaf = n;
00114 
00115                                 char equed = 'N';
00116 
00117                                 double *r = new double[n];
00118                                 double *c = new double[n];
00119 
00120                                 int ldx = n;
00121 
00122                                 double rcond;
00123 
00124                                 double *ferr = new double[nrhs];
00125                                 double *berr = new double[nrhs];
00126                                 double *work = new double[4*n];
00127 
00128                                 int *iwork = new int[n];
00129 
00130                             _x.resize(n);                       // memory allocation for the return vector
00131                                 dgesvx_(&fact, &transpose, &n, &nrhs, const_cast<double *>(&(_a(0,0))), &lda, af, &ldaf, &(ipiv[0]),
00132                                                 &equed, r, c, const_cast<double *>(&(_b(0))), &ldb, &(_x(0)), &ldx, &rcond,
00133                                                 ferr, berr, work, iwork, &info);
00134 
00135                                 delete [] iwork;
00136                                 delete [] work;
00137                                 delete [] berr;
00138                                 delete [] ferr;
00139                                 delete [] c;
00140                                 delete [] r;
00141 
00142                                 delete [] af;
00143 #else
00144                                 _x = _b;
00145                                 info = clapack_dgesv(CblasColMajor,
00146                                                                          n, nrhs, const_cast<double *>(&(a(0,0))), lda, &(ipiv[0]),
00147                                                                          &(_x(0)), ldb);
00148 #endif
00149 
00150                                 return info;
00151                 }
00152 
00157                 int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
00158                 {
00159                                 const int m = _a.rows();
00160                                 const int n = _a.cols();
00161                                 assert( m == static_cast<int>(_b.size()) );
00162                                 _x.resize(n);
00163 
00164                                 int i, j;
00165                                 char jobu  = 'A';
00166                                 char jobvt = 'A';
00167         
00168                                 int max_mn = max(m,n);
00169                                 int min_mn = min(m,n);
00170 
00171                                 dmatrix a(m,n);
00172                                 a = _a;
00173 
00174                                 int lda = m;
00175                                 double *s = new double[max_mn];         // singular values
00176                                 int ldu = m;
00177                                 double *u = new double[ldu*m];
00178                                 int ldvt = n;
00179                                 double *vt = new double[ldvt*n];
00180 
00181                                 int lwork = max(3*min_mn+max_mn, 5*min_mn);     // for CLAPACK ver.2 & ver.3
00182                                 double *work = new double[lwork];
00183                                 int info;
00184 
00185                                 for(i = 0; i < max_mn; i++) s[i] = 0.0;
00186 
00187                                 dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
00188                                                 &lwork, &info);
00189 
00190                                 double tmp;
00191 
00192                                 double smin, smax=0.0;
00193                                 for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
00194                                 smin = smax*_sv_ratio; // 1.0e-3;
00195                                 for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
00196         
00197                                 double *utb = new double[m];            // U^T*b
00198 
00199                                 for (j = 0; j < m; j++){
00200                                                 tmp = 0;
00201                                                 if (s[j]){
00202                                                                 for (i = 0; i < m; i++) tmp += u[j*m+i] * _b(i);
00203                                                                 tmp /= s[j];
00204                                                 }
00205                                                 utb[j] = tmp;
00206                                 }
00207 
00208                                 // v*utb
00209                                 for (j = 0; j < n; j++){
00210                                                 tmp = 0;
00211                                                 for (i = 0; i < n; i++){
00212                                                                 if(s[i]) tmp += utb[i] * vt[j*n+i];
00213                                                 }
00214                                                 _x(j) = tmp;
00215                                 }
00216 
00217                                 delete [] utb;
00218                                 delete [] work;
00219                                 delete [] vt;
00220                                 delete [] s;
00221                                 delete [] u;
00222         
00223                                 return info;
00224                 }
00225 
00226                 //======================= Unified interface to solve a linear equation =============================
00230                 int solveLinearEquation(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
00231                 {
00232                                 if(_a.cols() == _a.rows())
00233                                                 return solveLinearEquationLU(_a, _b, _x);
00234                                 else
00235                                                 return solveLinearEquationSVD(_a, _b, _x,  _sv_ratio);
00236                 }
00237                 
00238                 //======================= Calculate pseudo-inverse =================================================
00243                 int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
00244                 {
00245                                 int i, j, k;
00246                                 char jobu  = 'A';
00247                                 char jobvt = 'A';
00248                                 int m = (int)_a.rows();
00249                                 int n = (int)_a.cols();
00250                                 int max_mn = max(m,n);
00251                                 int min_mn = min(m,n);
00252 
00253                                 dmatrix a(m,n);
00254                                 a = _a;
00255 
00256                                 int lda = m;
00257                                 double *s = new double[max_mn];
00258                                 int ldu = m;
00259                                 double *u = new double[ldu*m];
00260                                 int ldvt = n;
00261                                 double *vt = new double[ldvt*n];
00262                                 int lwork = max(3*min_mn+max_mn, 5*min_mn);     // for CLAPACK ver.2 & ver.3
00263                                 double *work = new double[lwork];
00264                                 int info;
00265 
00266                                 for(i = 0; i < max_mn; i++) s[i] = 0.0;
00267                    
00268                                 dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
00269                                                 &lwork, &info);
00270 
00271 
00272                                 double smin, smax=0.0;
00273                                 for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
00274                                 smin = smax*_sv_ratio;                  // default _sv_ratio is 1.0e-3
00275                                 for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
00276 
00277                                 //------------ calculate pseudo inverse   pinv(A) = V*S^(-1)*U^(T)
00278                                 // S^(-1)*U^(T)
00279                                 for (j = 0; j < m; j++){
00280                                                 if (s[j]){
00281                                                                 for (i = 0; i < m; i++) u[j*m+i] /= s[j];
00282                                                 }
00283                                                 else {
00284                                                                 for (i = 0; i < m; i++) u[j*m+i] = 0.0;
00285                                                 }
00286                                 }
00287 
00288                                 // V * (S^(-1)*U^(T)) 
00289                                 _a_pseu.resize(n,m);
00290                                 for(j = 0; j < n; j++){
00291                                                 for(i = 0; i < m; i++){
00292                                                                 _a_pseu(j,i) = 0.0;
00293                                                                 for(k = 0; k < min_mn; k++){
00294                                                                                 if(s[k]) _a_pseu(j,i) += vt[j*n+k] * u[k*m+i];
00295                                                                 }
00296                                                 }
00297                                 }
00298 
00299                                 delete [] work;
00300                                 delete [] vt;
00301                                 delete [] s;
00302                                 delete [] u;
00303 
00304                                 return info;
00305                 }
00306 
00307                 //----- Calculation of eigen vectors and eigen values -----
00308                 int calcEigenVectors(const dmatrix &_a, dmatrix  &_evec, dvector &_eval)
00309                 {
00310                                 assert( _a.cols() == _a.rows() );
00311 
00312                                 typedef dmatrix mlapack;
00313                                 typedef dvector vlapack;
00314                                 
00315                                 mlapack a    = _a; // <-
00316                                 mlapack evec = _evec;
00317                                 vlapack eval = _eval;
00318                                 
00319                                 int n = (int)_a.cols();
00320                                 
00321                                 double *wi = new double[n];
00322                                 double *vl = new double[n*n];
00323                                 double *work = new double[4*n];
00324 
00325                                 int lwork = 4*n;
00326                                 int info;
00327                                 
00328                                 dgeev_("N","V", &n, &(a(0,0)), &n, &(eval(0)), wi, vl, &n, &(evec(0,0)), &n, work, &lwork, &info);
00329                                 
00330                                 _evec = evec.transpose();
00331                                 _eval = eval;
00332                                 
00333                                 delete [] wi;
00334                                 delete [] vl;
00335                                 delete [] work;
00336                                 
00337                                 return info;
00338                 }
00339 
00340 
00341                 //--- Calculation of SR-Inverse ---
00342                 int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w) {
00343                     // J# = W Jt(J W Jt + kI)-1 (Weighted SR-Inverse)
00344                     // SR-inverse :
00345                     // Y. Nakamura and H. Hanafusa : "Inverse Kinematic Solutions With
00346                     // Singularity Robustness for Robot Manipulator Control"
00347                     // J. Dyn. Sys., Meas., Control  1986. vol 108, Issue 3, pp. 163--172.
00348                 
00349                     const int c = _a.rows(); // 6
00350                     const int n = _a.cols(); // n
00351                 
00352                     if ( _w.cols() != n || _w.rows() != n ) {
00353                         _w = dmatrix::Identity(n, n);
00354                     }
00355                 
00356                     dmatrix at = _a.transpose();
00357                     dmatrix a1(c, c);
00358                     a1 = (_a * _w * at +  _sr_ratio * dmatrix::Identity(c,c)).inverse();
00359                 
00360                     //if (DEBUG) { dmatrix aat = _a * at; std::cerr << " a*at :" << std::endl << aat; }
00361                 
00362                     _a_sr  = _w * at * a1;
00363                     //if (DEBUG) { dmatrix ii = _a * _a_sr; std::cerr << "    i :" << std::endl << ii; }
00364                     return 0;
00365                 }
00366 
00367                 //--- Calculation of determinamt ---
00368                 double det(const dmatrix &_a)
00369                 {
00370                                 assert( _a.cols() == _a.rows() );
00371 
00372                                 typedef dmatrix mlapack;
00373                                 mlapack a = _a; // <-
00374 
00375                                 int info;
00376                                 int n = a.cols();
00377                                 int lda = n;
00378                                 std::vector<int> ipiv(n);
00379 
00380 #ifdef USE_CLAPACK_INTERFACE
00381                                 info = clapack_dgetrf(CblasColMajor,
00382                                                                           n, n, &(a(0,0)), lda, &(ipiv[0]));
00383 #else
00384                                 dgetrf_(&n, &n, &a(0,0), &lda, &(ipiv[0]), &info);
00385 #endif
00386 
00387                                 double det=1.0;
00388         
00389                                 for(int i=0; i < n-1; i++)
00390                                                 if(ipiv[i] != i+1)  det = -det;
00391                                 
00392                                 for(int i=0; i < n; i++)  det *= a(i,i);
00393 
00394                                 assert(info == 0);
00395                                 
00396                                 return det;
00397                 }
00398 
00399 } // namespace hrp


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55