コード例 #1
0
void Body::calcCMJacobian(Link *base, dmatrix &J)
{
    // prepare subm, submwc
    JointPathPtr jp;
    if (base){
        jp = getJointPath(rootLink(), base);
        Link *skip = jp->joint(0);
        skip->subm = rootLink()->m;
        skip->submwc = rootLink()->m*rootLink()->wc;
        Link *l = rootLink()->child;
        if (l){
            if (l != skip) {
                l->calcSubMassCM();
                skip->subm += l->subm;
                skip->submwc += l->submwc;
            }
            l = l->sibling;
            while(l){
                if (l != skip){
                    l->calcSubMassCM();
                    skip->subm += l->subm;
                    skip->submwc += l->submwc;
                }
                l = l->sibling;
            }
        }
        
        // assuming there is no branch between base and root
        for (int i=1; i<jp->numJoints(); i++){
            l = jp->joint(i);
            l->subm = l->parent->m + l->parent->subm;
            l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
        }
        
        J.resize(3, numJoints());
    }else{
        rootLink()->calcSubMassCM();
        J.resize(3, numJoints()+6);
    }
    
    // compute Jacobian
    std::vector<int> sgn(numJoints(), 1);
    if (jp) {
        for (int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
    }
    
    for (int i=0; i<numJoints(); i++){
        Link *j = joint(i);
        switch(j->jointType){
        case Link::ROTATIONAL_JOINT:
        {
            Vector3 omega(sgn[j->jointId]*j->R*j->a);
            Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
            Vector3 dp(omega.cross(arm));
            J.col(j->jointId) = dp;
            break;
        }
        default:
            std::cerr << "calcCMJacobian() : unsupported jointType("
                      << j->jointType << std::endl;
        }
    }
    if (!base){
        int c = numJoints();
        J(0, c  ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
        J(1, c  ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
        J(2, c  ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;

        Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
        J(0, c+3) =    0.0; J(0, c+4) =  dp(2); J(0, c+5) = -dp(1);
        J(1, c+3) = -dp(2); J(1, c+4) =    0.0; J(1, c+5) =  dp(0);
        J(2, c+3) =  dp(1); J(2, c+4) = -dp(0); J(2, c+5) =    0.0;
    }
}
コード例 #2
0
/**
   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;
}
コード例 #3
0
  bool sffs::apply(const dmatrix& src,const ivector& srcIds, 
                   dmatrix& dest) const {
    bool ok=true;
    dest.clear();
    parameters param=getParameters();
    // initialize cross validator

    costFunction *cF;
    cF = param.usedCostFunction;
    cF->setSrc(src,srcIds);

    int featureToInsert(0),featureToDelete(0),i;
    double oldRate,newRate;
    bool doInclude=true;
    bool terminate=false;
    int nbFeatures=src.columns();
    std::list<int> in,out;
    std::list<int>::iterator it;
    std::map<double,int> values;
    double value;
    for (i=0; i<nbFeatures; i++) {
      out.push_back(i);
    }
    ivector posInSrc(nbFeatures,-1);//saves the position in src of the inserted
    // feature to mark it as not used if this feature is deleted later
    dvector regRate(nbFeatures);  // the recognition rates after the insertion 
                                  // of a new feature
    if (param.nbFeatures<2) {
      setStatusString("You will have to choose at least two features. Set nbFeatures=2");
      return false;
    }

    // add the first best two features; do 2 steps sfs
    for (i=0; i<2; i++ ) {
      if (dest.columns()<src.columns() && !terminate) {
        // add space for one extra feature
        for (it=out.begin(); it!=out.end(); it++) {
          in.push_back(*it);
          cF->apply(in,value);
          values[value]=*it;
          in.pop_back();
        }
        // search for maximum in regRate; all possibilities not tested are -1
        in.push_back((--values.end())->second);
        out.remove((--values.end())->second);
      }
    }
    cF->apply(in,oldRate);
    while (!terminate) {
      // STEP 1: include the best possible feature
      if (static_cast<int>(in.size())<src.columns() && 
          !terminate && doInclude) {
        values.clear();
        for (it=out.begin(); it!=out.end(); it++) {
          in.push_back(*it);
          cF->apply(in,value);
          values[value]=*it;
          in.pop_back();
        }
        featureToInsert=(--values.end())->second;
        in.push_back(featureToInsert);
        out.remove(featureToInsert);
      }
      // STEP 2: conditional exclusion
      if (in.size()>0 && !terminate) {
        values.clear();
        for (it=in.begin(); it!=in.end(); it++) {
          int tmp=*it;
          it=in.erase(it);
          cF->apply(in,value);
          values[value]=tmp;
          in.insert(it,tmp);
          it--;
        }
        featureToDelete=(--values.end())->second;

        // if the least significant feature is equal to the most significant
        // feature that was included in step 1, leave feature and 
        // include the next one
        if (featureToDelete==featureToInsert) {
          doInclude=true;
        } else {    // delete this feature and compute new recognition rate

          // if the feature to delete is not the last feature in dest,
          // change the feature against the last feature in dest and delete
          // the last column in dest, otherwise if the feature to delete 
          // is equal to the last feature in dest nothing will be done, 
          // because this is already the lacking feature in temp
          cF->apply(in,newRate);
          // if recognition rate without least significant feature is better 
          // than with this feature delete it
          if (newRate>oldRate) { 

            in.remove(featureToDelete);
            out.push_back(featureToDelete);
            // search for another least significant feature before 
            // including the next one
            doInclude=false;
            oldRate=newRate;
          } else {
            doInclude=true;
          }
          // if only two features left, include the next one
          if (dest.columns()<=2) {
            doInclude=true;    
          }
        }          
      } // end of exclusion
      // test if the predetermined number of features is reached
      terminate=(param.nbFeatures==static_cast<int>(in.size()));
    } // while (!terminate)

    // Now fill dest
    const int sz = static_cast<int>(in.size());
    dest.resize(src.rows(), sz, 0., false, false);
    ivector idvec(false, sz);
    std::list<int>::const_iterator lit = in.begin();
    for (i=0; i < sz; ++i) {
      idvec.at(i)=*lit;
      ++lit;
    }
    for (i=0; i < src.rows(); ++i) {
      const dvector& svec = src.getRow(i);
      dvector& dvec = dest.getRow(i);
      for (int j=0; j < sz; ++j) {
        dvec.at(j) = svec.at(idvec.at(j));
      }
    }

    return ok;
  };
コード例 #4
0
		/**
		   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;
		}
コード例 #5
0
ファイル: Body.cpp プロジェクト: fkanehiro/openhrp3
void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
{
    // prepare subm, submwc
    JointPathPtr jp;

    dmatrix M;
    calcCMJacobian(base, M);
    M.conservativeResize(3, numJoints());
    M *= totalMass();

    if (base){
        jp = getJointPath(rootLink(), base);
        Link *skip = jp->joint(0);
        skip->subm = rootLink()->m;
        skip->submwc = rootLink()->m*rootLink()->wc;
        Link *l = rootLink()->child;
        if (l){
            if (l != skip) {
                l->calcSubMassCM();
                skip->subm += l->subm;
                skip->submwc += l->submwc;
            }
            l = l->sibling;
            while(l){
                if (l != skip){
                    l->calcSubMassCM();
                    skip->subm += l->subm;
                    skip->submwc += l->submwc;
                }
                l = l->sibling;
            }
        }
        
        // assuming there is no branch between base and root
        for (unsigned int i=1; i<jp->numJoints(); i++){
            l = jp->joint(i);
            l->subm = l->parent->m + l->parent->subm;
            l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
        }
        
        H.resize(3, numJoints());
    }else{
        rootLink()->calcSubMassCM();
        H.resize(3, numJoints()+6);
    }
    
    // compute Jacobian
    std::vector<int> sgn(numJoints(), 1);
    if (jp) {
        for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
    }
    
    for (unsigned int i=0; i<numJoints(); i++){
        Link *j = joint(i);
        switch(j->jointType){
        case Link::ROTATIONAL_JOINT:
        {
            Vector3 omega(sgn[j->jointId]*j->R*j->a);
            Vector3 Mcol = M.col(j->jointId);
            Matrix33 jsubIw;
            j->calcSubMassInertia(jsubIw);
            Vector3 dp = jsubIw*omega;
            if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
            H.col(j->jointId) = dp;
            break;
        }
        case Link::SLIDE_JOINT:
        {
          if(j->subm!=0){
            Vector3 Mcol =M.col(j->jointId);
            Vector3 dp = (j->submwc/j->subm).cross(Mcol);
            H.col(j->jointId) = dp;
          }
          break;
        }
        default:
            std::cerr << "calcCMJacobian() : unsupported jointType("
                      << j->jointType << ")" << std::endl;
        }
    }
    if (!base){
        int c = numJoints();
        H.block(0, c, 3, 3).setZero();
        Matrix33 Iw;
        rootLink_->calcSubMassInertia(Iw);
        H.block(0, c+3, 3, 3) = Iw;
        Vector3 cm = calcCM();
        Matrix33 cm_cross;
        cm_cross <<
          0.0, -cm(2), cm(1),
          cm(2), 0.0, -cm(0),
          -cm(1), cm(0), 0.0;
        H.block(0,0,3,c) -= cm_cross * M;
    }
}