orocos_kdl: orocos_kdl: constraints.cpp Source File

00001 #include <kdl/constraints.hpp>
00002 #include <kdl/jacobianframe.hpp>
00003 #include <fstream>
00004 #include <iomanip>
00005 
00006 using namespace KDL;
00007 
00012 ConstraintMatrix::ConstraintMatrix(int _maxrows,int _maxcols) :
00013       maxrows(_maxrows),
00014       maxcols(_maxcols),
00015       cmat(new double[_maxrows*_maxcols]),
00016       cmatstride(_maxcols),
00017       desval(new double[_maxrows]),
00018       weight(new double[_maxrows]),   /* square root of the constraint weight matrix */
00019       jsweight(new double[_maxcols]), /* square root of the joint space weight matrix */
00020       pinvtmps( _maxrows>_maxcols?_maxrows:_maxcols), 
00021       /*pinvtmps(
00022         _maxrows>_maxcols?_maxrows:_maxcols, 
00023         _maxrows>_maxcols?_maxrows:_maxcols,
00024         100,
00025         5E-2), */
00026       cmati(new double[_maxrows*_maxcols]),
00027       cmatistride(_maxrows),
00028       nrOfConstraints(0),      
00029       eps(0.05),
00030       nrofits(100)
00031     {}
00032 
00033   void ConstraintMatrix::dump(std::ostream& os) {
00034       int p = os.precision(7);
00035           int w = 16;
00036           std::ios_base::fmtflags fl = os.setf(std::ios::scientific);
00037       int i,j;
00038       os << "Constraint Matrix and desired velocity(last column)" << std::endl;
00039       for (i=0;i<nrOfConstraints;++i) {
00040           for (j=0;j<nrofjoints;++j) {
00041               os << std::setw(w) << cmat[i*cmatstride+j] << "\t";
00042           }
00043           os << desval[i];
00044           os << std::endl;
00045       }
00046       os.flags(fl);
00047           os.precision(p);
00048   }
00049 
00050   void ConstraintMatrix::dumpsvd(std::ostream& os) {
00051       int p = os.precision(7);
00052           int w = 16;
00053           std::ios_base::fmtflags fl = os.setf(std::ios::scientific);
00054       int i;
00055       for (i=0;i<nrofjoints;++i) {
00056           os << std::setw(w) << pinvtmps.S[pinvtmps.ndx[i]] << " ";
00057       }
00058       os << std::endl;
00059       os.flags(fl);
00060           os.precision(p);
00061   }
00062 
00063         void ConstraintMatrix::resetConstraints(int _nrofjoints) {
00064         int i;
00065         nrofjoints = _nrofjoints;
00066         nrOfConstraints=0;        
00067         for (i=0;i<nrofjoints;++i) {
00068             jsweight[i]=1.0;
00069         }
00070     }
00071     void ConstraintMatrix::addConstraint(const Jacobian<double>& c,double K,double _desval,double _desvald,double _weight) {
00072         int i;        
00073         assert((0<=nrOfConstraints)&&(nrOfConstraints<maxrows));
00074         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00075         desval[nrOfConstraints] = K*(_desval - c.value()) + _desvald;
00076         weight[nrOfConstraints] = ::sqrt(_weight);
00077         if (c.hasDerivs()) {
00078             for (i=0;i<nrofjoints;++i) {
00079                                 if (c.hasDeriv(i)) {
00080                                         cmat[cmatstride*nrOfConstraints+i] = c.deriv(i);
00081                                 } else {
00082                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00083                                 }
00084                         }
00085         } else {
00086             for (i=0;i<nrofjoints;++i) {
00087                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00088             }
00089         }
00090         nrOfConstraints++;
00091     }
00092 
00093     void ConstraintMatrix::addConstraintJoint(int jointnr,double value,double K,double _desval,double _desvald,double _weight) {
00094         int i;        
00095         assert((0<=nrOfConstraints)&&(nrOfConstraints<maxrows));
00096         desval[nrOfConstraints] = K*(_desval - value) + _desvald;
00097         weight[nrOfConstraints] = ::sqrt(_weight);
00098         for (i=0;i<nrofjoints;++i) {
00099             cmat[cmatstride*nrOfConstraints+i] = (i==jointnr)?1.0:0.0;
00100         }
00101         nrOfConstraints++;
00102     }
00103 
00104 
00105         void ConstraintMatrix::addConstraint(
00106                 const Jacobian<Vector>& c,
00107                 double K,
00108                 const Vector& _desval,
00109                 const Vector& _desvald,
00110                 double _weight
00111         ) {
00112         assert((0<=nrOfConstraints)&&(nrOfConstraints+2<maxrows));
00113         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00114                 int r,i;
00115                 Vector d = K*(_desval- c.value()) + _desvald;
00116         _weight = ::sqrt(_weight);
00117                 for (r=0;r<3;r++) {     
00118                         desval[nrOfConstraints] = d(r);
00119                         weight[nrOfConstraints] = _weight;
00120                         if (c.hasDerivs()) {
00121                                 for (i=0;i<nrofjoints;++i) {
00122                                         if (c.hasDeriv(i)) {
00123                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00124                                         } else {
00125                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00126                                         }
00127                                 }
00128                         } else {
00129                                 for (i=0;i<nrofjoints;++i) {
00130                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00131                                 }
00132                         }
00133                         nrOfConstraints++;
00134                 }        
00135         }
00139         void ConstraintMatrix::addConstraint(
00140                 const Jacobian<Frame>& c,
00141                 const std::vector<double>& K,
00142                 const Frame& _desval,
00143                 const Twist& _desvald,
00144                 const std::vector<double>& _weight
00145         ) {
00146         assert((0<=nrOfConstraints)&&(nrOfConstraints+5<maxrows));
00147         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00148                 int r,i;
00149                 Twist d = diff(c.value(),_desval,1.0);
00150                 for (r=0;r<6;r++) {     
00151                         desval[nrOfConstraints] = K[r]*d(r)+_desvald(r);
00152                         weight[nrOfConstraints] = ::sqrt(_weight[r]);
00153                         if (c.hasDerivs()) {
00154                                 for (i=0;i<nrofjoints;++i) {
00155                                         if (c.hasDeriv(i)) {
00156                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00157                                         } else {
00158                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00159                                         }
00160                                 }
00161                         } else {
00162                                 for (i=0;i<nrofjoints;++i) {
00163                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00164                                 }
00165                         }
00166                         nrOfConstraints++;
00167                 }        
00168     }
00169 
00173         void ConstraintMatrix::addConstraintWithoutControl(
00174                 const Jacobian<Frame>& c,
00175                 const Twist& desired,
00176                 const std::vector<double>& _weight
00177         ) {
00178         assert((0<=nrOfConstraints)&&(nrOfConstraints+5<maxrows));
00179         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00180                 int r,i;
00181                 for (r=0;r<6;r++) {     
00182                         desval[nrOfConstraints] = desired(r);
00183                         weight[nrOfConstraints] = ::sqrt(_weight[r]);
00184                         if (c.hasDerivs()) {
00185                                 for (i=0;i<nrofjoints;++i) {
00186                                         if (c.hasDeriv(i)) {
00187                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00188                                         } else {
00189                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00190                                         }
00191                                 }
00192                         } else {
00193                                 for (i=0;i<nrofjoints;++i) {
00194                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00195                                 }
00196                         }
00197                         nrOfConstraints++;
00198                 }   
00199     }
00200 
00201 
00202 
00203         void ConstraintMatrix::addConstraint(
00204                 const Jacobian<Frame>& c,
00205                 double Kpos,
00206                 double Krot,
00207                 const Frame& _desval,
00208                 const Twist& _desvald,
00209                 double _weightpos,
00210                 double _weightrot
00211         ) {
00212         assert((0<=nrOfConstraints)&&(nrOfConstraints+5<maxrows));
00213         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00214                 int r,i;
00215                 Twist d = diff(c.value(),_desval,1.0);
00216         _weightpos = ::sqrt(_weightpos);
00217         _weightrot = ::sqrt(_weightrot);
00218                 for (r=0;r<6;r++) {     
00219                         if (r<3) {
00220                                 desval[nrOfConstraints] = Kpos*d(r)+_desvald(r);
00221                                 weight[nrOfConstraints] = _weightpos;
00222                         } else {
00223                                 desval[nrOfConstraints] = Krot*d(r)+_desvald(r);
00224                                 weight[nrOfConstraints] = _weightrot;
00225                         }
00226                         if (c.hasDerivs()) {
00227                                 for (i=0;i<nrofjoints;++i) {
00228                                         if (c.hasDeriv(i)) {
00229                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00230                                         } else {
00231                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00232                                         }
00233                                 }
00234                         } else {
00235                                 for (i=0;i<nrofjoints;++i) {
00236                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00237                                 }
00238                         }
00239                         nrOfConstraints++;
00240                 }        
00241         }
00242 
00243 
00244 
00245         void ConstraintMatrix::addConstraint(
00246                 const Jacobian<Twist>& c,
00247                 double Kpos,
00248                 double Krot,
00249                 const Twist& _desval,
00250                 const Twist& _desvald,
00251                 double _weightpos,
00252                 double _weightrot
00253         ) {
00254         assert((0<=nrOfConstraints)&&(nrOfConstraints+5<maxrows));
00255         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00256                 int r,i;
00257                 Twist d = _desval - c.value();
00258         _weightpos = ::sqrt(_weightpos);
00259         _weightrot = ::sqrt(_weightrot);
00260                 for (r=0;r<6;r++) {     
00261                         if (r<3) {
00262                                 desval[nrOfConstraints] = Kpos*d(r)+_desvald(r);
00263                                 weight[nrOfConstraints] = _weightpos;
00264                         } else {
00265                                 desval[nrOfConstraints] = Krot*d(r)+_desvald(r);
00266                                 weight[nrOfConstraints] = _weightrot;
00267                         }
00268                         if (c.hasDerivs()) {
00269                                 for (i=0;i<nrofjoints;++i) {
00270                                         if (c.hasDeriv(i)) {
00271                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00272                                         } else {
00273                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00274                                         }
00275                                 }
00276                         } else {
00277                                 for (i=0;i<nrofjoints;++i) {
00278                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00279                                 }
00280                         }
00281                         nrOfConstraints++;
00282                 }        
00283         }
00284 
00285 
00286 
00287 
00288         void ConstraintMatrix::addConstraint(
00289                 const Jacobian<Wrench>& c,
00290                 double Kpos,
00291                 double Krot,
00292                 const Wrench& _desval,
00293                 const Wrench& _desvald,
00294                 double _weightpos,
00295                 double _weightrot
00296         ) {
00297 
00298         assert((0<=nrOfConstraints)&&(nrOfConstraints+5<maxrows));
00299         assert((!c.hasDerivs())||(c.nrOfDeriv()==nrofjoints));
00300                 int r,i;
00301         _weightpos = ::sqrt(_weightpos);
00302         _weightrot = ::sqrt(_weightrot);
00303                 Wrench d = diff(c.value(),_desval,1.0);
00304                 for (r=0;r<6;r++) {     
00305                         if (r<3) {
00306                                 desval[nrOfConstraints] = Kpos*d(r)+_desvald(r);
00307                                 weight[nrOfConstraints] = _weightpos;
00308                         } else {
00309                                 desval[nrOfConstraints] = Krot*d(r)+_desvald(r);
00310                                 weight[nrOfConstraints] = _weightrot;
00311                         }
00312                         if (c.hasDerivs()) {
00313                                 for (i=0;i<nrofjoints;++i) {
00314                                         if (c.hasDeriv(i)) {
00315                                                 cmat[cmatstride*nrOfConstraints+i] = c.deriv(i)(r);
00316                                         } else {
00317                                                 cmat[cmatstride*nrOfConstraints+i] = 0.0;
00318                                         }
00319                                 }
00320                         } else {
00321                                 for (i=0;i<nrofjoints;++i) {
00322                                         cmat[cmatstride*nrOfConstraints+i] = 0.0;
00323                                 }
00324                         }
00325                         nrOfConstraints++;
00326                 }        
00327         }
00334     void ConstraintMatrix::calculateOutput(std::vector<double>& output) {
00335         int i;
00336         if (nrOfConstraints==0) {
00337             for (i=0;i<nrofjoints;++i) output[i]=0.0;
00338             return;
00339         }
00340         /*
00341         int i,j;
00342         double sum;
00343         // temporary implementation..
00344         //MatrixOutput(std::cout,cmat,cmatstride,maxrows,maxcols);
00345         //std::cout << std::endl;
00346         pinvtmps.pinv(cmat,cmatstride,nrOfConstraints,maxcols,cmati,cmatistride);
00347         //MatrixOutput(std::cout,cmati,cmatistride,maxcols,maxrows);
00348         // std::cout << std::endl;
00349         for (i=0;i<maxcols;++i) {
00350             sum = 0.0;
00351             for (j=0;j<nrOfConstraints;++j) {
00352                 sum+= cmati[ cmatistride* i + j]  * desval[j];
00353             }
00354             output[i] = sum;
00355         }*/
00356                 /*
00357         if (lowerprior) {
00358             lowerprior->calculateOutput(output2);
00359             pinvtmps.inverseWithNullSpace(
00360                 cmat,cmatstride,nrOfConstraints,maxcols,
00361                 desval, output2,
00362                 weight,jsweight,
00363                 output);
00364         } else {
00365                 */
00366         /*pinvtmps.inverse(
00367             cmat,cmatstride,nrOfConstraints,nrofjoints,
00368             desval,
00369             weight,jsweight,
00370             output);*/
00371         pinvtmps.prepare(cmat,cmatstride,nrOfConstraints,nrofjoints,weight,jsweight,nrofits);
00372                 // WARNING : this makes some assumptions on the implementation of vector<double> :
00373         pinvtmps.inverse(desval,&output[0],eps);
00374     }
00375 
00376     ConstraintMatrix::~ConstraintMatrix() {
00377         delete [] cmat;
00378         delete [] desval;
00379         delete [] weight;
00380                 delete [] jsweight;
00381         delete [] cmati;
00382     }
00383 
00384         void ConstraintMatrix::setJointSpaceWeights(const std::vector<double>& jsw) {
00385                 int i;
00386                 for (i=0;i<nrofjoints;++i) {
00387             jsweight[i]=::sqrt(jsw[i]);
00388         }
00389         }
00394         void ConstraintMatrix::calculateOutput(std::vector<double>& output,const std::vector<double>& nullspace) {
00395         int i;
00396         if (nrOfConstraints==0) {
00397             for (i=0;i<nrofjoints;++i) output[i]=nullspace[i];
00398             return;
00399         }
00400         
00401         /*pinvtmps.inverseWithNullSpace(
00402             cmat,cmatstride,nrOfConstraints,nrofjoints,
00403             desval,nullspace,
00404             weight,jsweight,
00405             output);*/
00406         
00407         pinvtmps.prepare(cmat,cmatstride,nrOfConstraints,nrofjoints,weight,jsweight,nrofits);
00408         pinvtmps.inverseWithNullSpace(desval,&output[0],&nullspace[0],eps); 
00409         //dumpsvd(std::cout);
00410     }
00411 
00412 
00413     void ConstraintMatrix::reduce(const std::vector<bool>& list,const std::vector<double> value) {
00414         int newcol =0;   // index in the reduced vector (newcol <= col)
00415         for (int col=0;col<nrofjoints;++col) {
00416             // PRE COND : 0..col-1 is reduced
00417            int row;
00418            if (!list[col]) {
00419                 // if not fixed
00420                 // adapt col :
00421                 for (row=0;row<nrOfConstraints;++row)
00422                     cmat[cmatstride*row+newcol] = cmat[cmatstride*row+col];
00423                 // adapt joint space weights :
00424                 jsweight[newcol]=jsweight[col];
00425                 // adapt desired values and indices.
00426                 newcol++;
00427            } else {
00428                for (row=0;row<nrOfConstraints;++row) {
00429                    desval[row] -= cmat[cmatstride*row+col]*value[row]; 
00430                }
00431            }
00432            // POST COND 0..col is reduced into a vector of 0..newcol-1
00433         }
00434         // reduce nrofjoints
00435         nrofjoints= newcol;
00436     }
00437 
00438     void ConstraintMatrix::reducejv(const std::vector<bool>& list,std::vector<double>& vec)
00439         {
00440         int newcol =0;
00441         for (int col=0;col<nrofjoints;++col) {
00442            if (!list[col]) {
00443                 vec[newcol] = vec[col];
00444                 newcol++;
00445            }
00446         }
00447     }
00448 
00449     void ConstraintMatrix::expandjv(int nrofalljoints,const std::vector<bool>& list,const std::vector<double>& values,std::vector<double>& vec)
00450     {
00451         int oldcol =nrofjoints-1;
00452         for (int col=nrofalljoints-1;col>=0;--col) {
00453            // PRE : vec is expanded for col+1<i<nrofalljoints
00454            if (!list[col]) {
00455                 vec[col] = vec[oldcol];
00456                 oldcol--;
00457            } else {
00458                 vec[col] = values[col];
00459            }
00460            // POST : vec is expanded for col<i<nrofalljoints
00461         }
00462     }
00463 
00464 
00465 
00466     void ConstraintMatrix::calculateOutput(const std::vector<bool>& list, const std::vector<double>& value, std::vector<double>& output) {
00467         int nrofalljoints = nrofjoints;
00468         reduce(list,value);
00469         calculateOutput(output);  
00470         expandjv(nrofalljoints,list,value,output);
00471     }
00472 
00473 
00474     void ConstraintMatrix::calculateOutput(
00475                 const std::vector<bool>& list, 
00476                 const std::vector<double>& values, 
00477                 std::vector<double>& output,
00478                 std::vector<double>& nullspace) 
00479         {
00480         int nrofalljoints = nrofjoints;
00481         reduce(list,values);
00482         reducejv(list,nullspace);
00483         calculateOutput(output,nullspace);
00484         expandjv(nrofalljoints,list,values,output);
00485     }
00486 
00487