C++ dmatrix::cols方法代码示例

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

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