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;
}