MatrixSolvers.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
10 #include <iostream>
11 #include <vector>
12 #include "MatrixSolvers.h"
13 
14 using namespace std;
15 
16 #ifdef USE_CLAPACK_INTERFACE
17 extern "C" {
18 #include <cblas.h>
19 #include <clapack.h>
20 };
21 #else
22 extern "C" void dgesvx_(char *fact, char *trans, int *n,
23  int *nrhs, double *a, int *lda,
24  double *af, int *ldaf, int *ipiv,
25  char *equed, double *r, double *c,
26  double *b, int *ldb, double *x,
27  int *ldx, double *rcond, double *ferr,
28  double *berr, double *work, int *iwork,
29  int *info);
30 
31 extern "C" void dgesv_(const int* n, const int* nrhs,
32  double* a, int* lda, int* ipiv,
33  double* b, int* ldb, int* info);
34 
35 extern "C" void dgetrf_( int *m, int *n, double *a, int *lda, int *ipiv, int *info);
36 #endif
37 
38 extern "C" void dgesvd_(char const* jobu, char const* jobvt,
39  int const* m, int const* n, double* a, int const* lda,
40  double* s, double* u, int const* ldu,
41  double* vt, int const* ldvt,
42  double* work, int const* lwork, int* info);
43 
44 extern "C" void dgeev_(char const*jobvl, char const*jobvr, int *n, double *A,
45  int *lda, double *wr,double *wi, double *vl,
46  int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info);
47 
48 
49 // originally in hrpCLAPACK.{cpp,h}
50 // solveLinearEquation()
51 // b = a * x, x = b^(-1) * a
52 namespace hrp {
53 
54  static inline int max(int a, int b) { return (a >= b) ? a : b; }
55  static inline int min(int a, int b) { return (a <= b) ? a : b; }
56 
57 
58  int solveLinearEquationLU(dmatrix a, const dmatrix &b, dmatrix &out_x)
59  {
60  assert(a.rows() == a.cols() && a.cols() == b.rows() );
61 
62  out_x = b;
63 
64  const int n = a.rows();
65  const int nrhs = b.cols();
66  int info;
67  std::vector<int> ipiv(n);
68 
69 #ifndef USE_CLAPACK_INTERFACE
70 
71  int lda = n;
72  int ldb = n;
73  dgesv_(&n, &nrhs, &(a(0,0)), &lda, &(ipiv[0]), &(out_x(0,0)), &ldb, &info);
74 #else
75  info = clapack_dgesv(CblasColMajor,
76  n, nrhs, &(a(0,0)), n,
77  &(ipiv[0]),
78  &(out_x(0,0)),
79  n);
80 #endif
81  assert(info == 0);
82 
83  return info;
84  }
85 
86 
91  int solveLinearEquationLU(const dmatrix &_a, const dvector &_b, dvector &_x)
92  {
93  assert(_a.cols() == _a.rows() && _a.cols() == _b.size() );
94 
95  int n = (int)_a.cols();
96  int nrhs = 1;
97 
98  int lda = n;
99 
100  std::vector<int> ipiv(n);
101 
102  int ldb = n;
103 
104  int info;
105 
106  // compute the solution
107 #ifndef USE_CLAPACK_INTERFACE
108  char fact = 'N';
109  char transpose = 'N';
110 
111  double *af = new double[n*n];
112 
113  int ldaf = n;
114 
115  char equed = 'N';
116 
117  double *r = new double[n];
118  double *c = new double[n];
119 
120  int ldx = n;
121 
122  double rcond;
123 
124  double *ferr = new double[nrhs];
125  double *berr = new double[nrhs];
126  double *work = new double[4*n];
127 
128  int *iwork = new int[n];
129 
130  _x.resize(n); // memory allocation for the return vector
131  dgesvx_(&fact, &transpose, &n, &nrhs, const_cast<double *>(&(_a(0,0))), &lda, af, &ldaf, &(ipiv[0]),
132  &equed, r, c, const_cast<double *>(&(_b(0))), &ldb, &(_x(0)), &ldx, &rcond,
133  ferr, berr, work, iwork, &info);
134 
135  delete [] iwork;
136  delete [] work;
137  delete [] berr;
138  delete [] ferr;
139  delete [] c;
140  delete [] r;
141 
142  delete [] af;
143 #else
144  _x = _b;
145  info = clapack_dgesv(CblasColMajor,
146  n, nrhs, const_cast<double *>(&(a(0,0))), lda, &(ipiv[0]),
147  &(_x(0)), ldb);
148 #endif
149 
150  return info;
151  }
152 
157  int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
158  {
159  const int m = _a.rows();
160  const int n = _a.cols();
161  assert( m == static_cast<int>(_b.size()) );
162  _x.resize(n);
163 
164  int i, j;
165  char jobu = 'A';
166  char jobvt = 'A';
167 
168  int max_mn = max(m,n);
169  int min_mn = min(m,n);
170 
171  dmatrix a(m,n);
172  a = _a;
173 
174  int lda = m;
175  double *s = new double[max_mn]; // singular values
176  int ldu = m;
177  double *u = new double[ldu*m];
178  int ldvt = n;
179  double *vt = new double[ldvt*n];
180 
181  int lwork = max(3*min_mn+max_mn, 5*min_mn); // for CLAPACK ver.2 & ver.3
182  double *work = new double[lwork];
183  int info;
184 
185  for(i = 0; i < max_mn; i++) s[i] = 0.0;
186 
187  dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
188  &lwork, &info);
189 
190  double tmp;
191 
192  double smin, smax=0.0;
193  for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
194  smin = smax*_sv_ratio; // 1.0e-3;
195  for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
196 
197  double *utb = new double[m]; // U^T*b
198 
199  for (j = 0; j < m; j++){
200  tmp = 0;
201  if (s[j]){
202  for (i = 0; i < m; i++) tmp += u[j*m+i] * _b(i);
203  tmp /= s[j];
204  }
205  utb[j] = tmp;
206  }
207 
208  // v*utb
209  for (j = 0; j < n; j++){
210  tmp = 0;
211  for (i = 0; i < n; i++){
212  if(s[i]) tmp += utb[i] * vt[j*n+i];
213  }
214  _x(j) = tmp;
215  }
216 
217  delete [] utb;
218  delete [] work;
219  delete [] vt;
220  delete [] s;
221  delete [] u;
222 
223  return info;
224  }
225 
226  //======================= Unified interface to solve a linear equation =============================
230  int solveLinearEquation(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
231  {
232  if(_a.cols() == _a.rows())
233  return solveLinearEquationLU(_a, _b, _x);
234  else
235  return solveLinearEquationSVD(_a, _b, _x, _sv_ratio);
236  }
237 
238  //======================= Calculate pseudo-inverse =================================================
243  int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
244  {
245  int i, j, k;
246  char jobu = 'A';
247  char jobvt = 'A';
248  int m = (int)_a.rows();
249  int n = (int)_a.cols();
250  int max_mn = max(m,n);
251  int min_mn = min(m,n);
252 
253  dmatrix a(m,n);
254  a = _a;
255 
256  int lda = m;
257  double *s = new double[max_mn];
258  int ldu = m;
259  double *u = new double[ldu*m];
260  int ldvt = n;
261  double *vt = new double[ldvt*n];
262  int lwork = max(3*min_mn+max_mn, 5*min_mn); // for CLAPACK ver.2 & ver.3
263  double *work = new double[lwork];
264  int info;
265 
266  for(i = 0; i < max_mn; i++) s[i] = 0.0;
267 
268  dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
269  &lwork, &info);
270 
271 
272  double smin, smax=0.0;
273  for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
274  smin = smax*_sv_ratio; // default _sv_ratio is 1.0e-3
275  for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
276 
277  //------------ calculate pseudo inverse pinv(A) = V*S^(-1)*U^(T)
278  // S^(-1)*U^(T)
279  for (j = 0; j < m; j++){
280  if (s[j]){
281  for (i = 0; i < m; i++) u[j*m+i] /= s[j];
282  }
283  else {
284  for (i = 0; i < m; i++) u[j*m+i] = 0.0;
285  }
286  }
287 
288  // V * (S^(-1)*U^(T))
289  _a_pseu.resize(n,m);
290  for(j = 0; j < n; j++){
291  for(i = 0; i < m; i++){
292  _a_pseu(j,i) = 0.0;
293  for(k = 0; k < min_mn; k++){
294  if(s[k]) _a_pseu(j,i) += vt[j*n+k] * u[k*m+i];
295  }
296  }
297  }
298 
299  delete [] work;
300  delete [] vt;
301  delete [] s;
302  delete [] u;
303 
304  return info;
305  }
306 
307  //----- Calculation of eigen vectors and eigen values -----
308  int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval)
309  {
310  assert( _a.cols() == _a.rows() );
311 
312  typedef dmatrix mlapack;
313  typedef dvector vlapack;
314 
315  mlapack a = _a; // <-
316  mlapack evec = _evec;
317  vlapack eval = _eval;
318 
319  int n = (int)_a.cols();
320 
321  double *wi = new double[n];
322  double *vl = new double[n*n];
323  double *work = new double[4*n];
324 
325  int lwork = 4*n;
326  int info;
327 
328  dgeev_("N","V", &n, &(a(0,0)), &n, &(eval(0)), wi, vl, &n, &(evec(0,0)), &n, work, &lwork, &info);
329 
330  _evec = evec.transpose();
331  _eval = eval;
332 
333  delete [] wi;
334  delete [] vl;
335  delete [] work;
336 
337  return info;
338  }
339 
340 
341  //--- Calculation of SR-Inverse ---
342  int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w) {
343  // J# = W Jt(J W Jt + kI)-1 (Weighted SR-Inverse)
344  // SR-inverse :
345  // Y. Nakamura and H. Hanafusa : "Inverse Kinematic Solutions With
346  // Singularity Robustness for Robot Manipulator Control"
347  // J. Dyn. Sys., Meas., Control 1986. vol 108, Issue 3, pp. 163--172.
348 
349  const int c = _a.rows(); // 6
350  const int n = _a.cols(); // n
351 
352  if ( _w.cols() != n || _w.rows() != n ) {
353  _w = dmatrix::Identity(n, n);
354  }
355 
356  dmatrix at = _a.transpose();
357  dmatrix a1(c, c);
358  a1 = (_a * _w * at + _sr_ratio * dmatrix::Identity(c,c)).inverse();
359 
360  //if (DEBUG) { dmatrix aat = _a * at; std::cerr << " a*at :" << std::endl << aat; }
361 
362  _a_sr = _w * at * a1;
363  //if (DEBUG) { dmatrix ii = _a * _a_sr; std::cerr << " i :" << std::endl << ii; }
364  return 0;
365  }
366 
367  //--- Calculation of determinamt ---
368  double det(const dmatrix &_a)
369  {
370  assert( _a.cols() == _a.rows() );
371 
372  typedef dmatrix mlapack;
373  mlapack a = _a; // <-
374 
375  int info;
376  int n = a.cols();
377  int lda = n;
378  std::vector<int> ipiv(n);
379 
380 #ifdef USE_CLAPACK_INTERFACE
381  info = clapack_dgetrf(CblasColMajor,
382  n, n, &(a(0,0)), lda, &(ipiv[0]));
383 #else
384  dgetrf_(&n, &n, &a(0,0), &lda, &(ipiv[0]), &info);
385 #endif
386 
387  double det=1.0;
388 
389  for(int i=0; i < n-1; i++)
390  if(ipiv[i] != i+1) det = -det;
391 
392  for(int i=0; i < n; i++) det *= a(i,i);
393 
394  assert(info == 0);
395 
396  return det;
397  }
398 
399 } // namespace hrp
hrp::solveLinearEquationLU
int solveLinearEquationLU(const dmatrix &_a, const dvector &_b, dvector &_x)
Definition: MatrixSolvers.cpp:91
autoplay.c
int c
Definition: autoplay.py:16
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
x
* x
Definition: IceUtils.h:98
hrp::min
static int min(int a, int b)
Definition: MatrixSolvers.cpp:55
hrp::solveLinearEquationSVD
int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
Definition: MatrixSolvers.cpp:157
dgetrf_
void dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info)
dgesvd_
void dgesvd_(char const *jobu, char const *jobvt, int const *m, int const *n, double *a, int const *lda, double *s, double *u, int const *ldu, double *vt, int const *ldvt, double *work, int const *lwork, int *info)
i
png_uint_32 i
Definition: png.h:2735
hrp::dvector
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
hrp::inverse
dmatrix inverse(const dmatrix &M)
Definition: MatrixSolvers.h:37
b
long b
Definition: jpegint.h:371
trans
png_infop png_bytep * trans
Definition: png.h:2435
hrp::det
double det(const dmatrix &_a)
Definition: MatrixSolvers.cpp:368
j
def j(str, encoding="cp932")
hrp::calcSRInverse
int calcSRInverse(const dmatrix &_a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w)
Definition: MatrixSolvers.cpp:342
a
string a
hrp::calcPseudoInverse
int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
Definition: MatrixSolvers.cpp:243
dgeev_
void dgeev_(char const *jobvl, char const *jobvr, int *n, double *A, int *lda, double *wr, double *wi, double *vl, int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info)
int
typedef int
Definition: png.h:1113
info
backing_store_ptr info
Definition: jmemsys.h:181
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int *info)
hrp::solveLinearEquation
int solveLinearEquation(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
Definition: MatrixSolvers.cpp:230
dgesvx_
void dgesvx_(char *fact, char *trans, int *n, int *nrhs, double *a, int *lda, double *af, int *ldaf, int *ipiv, char *equed, double *r, double *c, double *b, int *ldb, double *x, int *ldx, double *rcond, double *ferr, double *berr, double *work, int *iwork, int *info)
hrp::calcEigenVectors
int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval)
Definition: MatrixSolvers.cpp:308
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:39