00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "eigenGrasp.h"
00027 #include "robot.h"
00028
00029 #include <math.h>
00030 #include <stdio.h>
00031
00032 #include <QFile>
00033 #include <QString>
00034 #include <QTextStream>
00035 #include "tinyxml.h"
00036
00037 #include "math/matrix.h"
00038
00039 #include "debug.h"
00040
00042 EigenGrasp::EigenGrasp(int size, double e)
00043 {
00044 if (size <= 0 ) {
00045 fprintf(stderr,"Wrong size of eigen grasp\n");
00046 return;
00047 }
00048 mVals = new double[size];
00049 for (int i=0; i < size; i++)
00050 mVals[i] = 0;
00051 mSize = size;
00052 mEigenValue = e;
00053 mFixed = false;
00054 mPredefinedLimits=false;
00055 }
00056
00057 EigenGrasp::EigenGrasp(const EigenGrasp *orig)
00058 {
00059 mSize = orig->mSize;
00060 mVals = new double[mSize];
00061 memcpy(mVals, orig->mVals, mSize*sizeof(double));
00062 mEigenValue = orig->mEigenValue;
00063 mMin = orig->mMin; mMax = orig->mMax;
00064 mFixed = orig->mFixed;
00065 fixedAmplitude = orig->fixedAmplitude;
00066 mPredefinedLimits=orig->mPredefinedLimits;
00067 }
00068
00069 EigenGrasp::~EigenGrasp()
00070 {
00071 delete [] mVals;
00072 }
00073
00074 void
00075 EigenGrasp::getEigenGrasp(double *eg) const
00076 {
00077 for (int i=0; i<mSize; i++)
00078 eg[i] = mVals[i];
00079 }
00080
00081 void
00082 EigenGrasp::setEigenGrasp(const double *eg)
00083 {
00084 for (int i=0; i<mSize; i++)
00085 mVals[i] = eg[i];
00086 }
00087
00088 double
00089 EigenGrasp::normalize()
00090 {
00091 int i;
00092 double norm = 0;
00093 for (i=0; i<mSize; i++) {
00094 norm += mVals[i] * mVals[i];
00095 }
00096 norm = sqrt(norm);
00097 for (i=0; i<mSize; i++) {
00098 mVals[i] = mVals[i] / norm;
00099 }
00100 return norm;
00101 }
00102
00103 void
00104 EigenGrasp::writeToFile(TiXmlElement *ep)
00105 {
00106 TiXmlElement * EigenValue = new TiXmlElement( "EigenValue" );
00107 EigenValue->SetDoubleAttribute("value",mEigenValue);
00108 ep->LinkEndChild( EigenValue );
00109
00110 if(mPredefinedLimits)
00111 {
00112 TiXmlElement * Limits = new TiXmlElement( "Limits" );
00113 Limits->SetDoubleAttribute("min",mMin);
00114 Limits->SetDoubleAttribute("max",mMax);
00115 ep->LinkEndChild( Limits );
00116 }
00117
00118 TiXmlElement * DimVals = new TiXmlElement( "DimVals" );
00119 QString varStr;
00120 for (int i=0; i<mSize; i++)
00121 {
00122 varStr.setNum(i);
00123 varStr="d"+varStr;
00124 DimVals->SetDoubleAttribute(varStr,mVals[i]);
00125 }
00126 ep->LinkEndChild( DimVals );
00127
00128 }
00129
00130 void
00131 EigenGrasp::writeToFile(FILE *fp)
00132 {
00133 fprintf(fp,"%f\n",mEigenValue);
00134 for (int i=0; i<mSize; i++) {
00135 fprintf(fp,"%f ", mVals[i]);
00136 }
00137 fprintf(fp,"\n");
00138 }
00139
00140 void
00141 EigenGrasp::readFromFile(FILE *fp)
00142 {
00143 float v;
00144 if (fscanf(fp,"%f",&v) <= 0) {
00145 DBGA("EigenGrasp::readFromFile - failed to read eigenvalue");
00146 return;
00147 }
00148 mEigenValue = v;
00149 for (int i=0; i<mSize; i++) {
00150 if(fscanf(fp,"%f",&v) <= 0) {
00151 DBGA("EigenGrasp::readFromFile - failed to read eigenvector");
00152 return;
00153 }
00154 mVals[i] = v;
00155 }
00156 }
00157
00158 int
00159 EigenGrasp::readFromStream(QTextStream *stream)
00160 {
00161 if (stream->atEnd()) {fprintf(stderr,"Unable to read EG, end of file\n");return 0;}
00162 QString line = stream->readLine();
00163 bool ok;
00164 mEigenValue = line.toDouble(&ok); if (!ok) {DBGA("ERROR: EigenValue should be a number.");return 0;}
00165 if (stream->atEnd()) {fprintf(stderr,"Unable to read EG, end of file\n");return 0;}
00166 line = stream->readLine();
00167 for (int i=0; i<mSize; i++) {
00168 QString val = line.section(' ',i,i,QString::SectionSkipEmpty);
00169 if ( val.isNull() || val.isEmpty() ) {fprintf(stderr,"Unable to read EG value #%d\n",i);return 0;}
00170 mVals[i] = val.toDouble(&ok); if (!ok) {DBGA("ERROR: Entries should be numbers.");return 0;}
00171 }
00172 return 1;
00173 }
00174
00175 int
00176 EigenGrasp::readFromXml(const TiXmlElement *element)
00177 {
00178 bool ok;
00179 QString valueStr;
00180 std::list<const TiXmlElement*> EVList = findAllXmlElements(element, "EigenValue");
00181 if(!countXmlElements(element, "EigenValue"))
00182 {
00183 DBGA("WARNING: EigenValue tag missing from file defaulting EigenValue to 0.5!");
00184 mEigenValue=0.5;
00185 }
00186 else
00187 {
00188 valueStr=(*EVList.begin())->Attribute("value");
00189 if(valueStr.isNull()){
00190 QTWARNING("DOF Type not found");
00191 return 0;
00192 }
00193 mEigenValue=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: EigenValue entries should only contain numbers.");return 0;}
00194 }
00195
00196 EVList = findAllXmlElements(element, "DimVals");
00197 if(!countXmlElements(element, "DimVals")) {DBGA("DimVals tag missing from file.");return 0;}
00198 for (int i=0; i<mSize; i++) {
00199 valueStr=(*EVList.begin())->Attribute(QString("d") + QString::number(i));
00200 if ( valueStr.isNull() || valueStr.isEmpty() ) mVals[i]=0.0;
00201 else {
00202 mVals[i] = valueStr.toDouble(&ok);
00203 if (!ok) {DBGA("ERROR: DimVals entries should only contain numbers.");return 0;}
00204 }
00205 }
00206
00207 EVList = findAllXmlElements(element, "Limits");
00208 if(countXmlElements(element, "Limits"))
00209 {
00210 mPredefinedLimits=true;
00211 valueStr=(*EVList.begin())->Attribute("min");
00212 mMin=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: min entries should only contain numbers.");return 0;}
00213 if ( false ) mPredefinedLimits=false;
00214 valueStr=(*EVList.begin())->Attribute("max");
00215 mMax=valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: max entries should only contain numbers.");return 0;}
00216 if ( false ) mPredefinedLimits=false;
00217 }
00218 return 1;
00219 }
00220
00221 double
00222 EigenGrasp::dot(double *d)
00223 {
00224 double dot = 0;
00225 for (int i=0; i<mSize; i++) {
00226 dot += mVals[i] * d[i];
00227 }
00228 return dot;
00229 }
00230
00231
00232
00233 EigenGraspInterface::EigenGraspInterface(Robot *r)
00234 {
00235 mRobot = r;
00236 dSize = mRobot->getNumDOF();
00237 eSize = mGrasps.size();
00238 mOrigin = NULL;
00239 mNorm = NULL;
00240 mRigid = false;
00241 mP = mPInv = NULL;
00242 }
00243
00244 EigenGraspInterface::EigenGraspInterface(const EigenGraspInterface *orig)
00245 {
00246 mRobot = orig->mRobot;
00247 dSize = mRobot->getNumDOF();
00248 eSize = orig->eSize;
00249
00250 for (int i=0; i<eSize; i++) {
00251 mGrasps.push_back( new EigenGrasp(orig->mGrasps[i]) );
00252 }
00253
00254 mOrigin = new EigenGrasp( orig->mOrigin );
00255 mNorm = new EigenGrasp( orig->mNorm);
00256 mName = orig->mName;
00257 mRigid = orig->mRigid;
00258 mP = mPInv = NULL;
00259 if (orig->mP) {
00260 mP = new Matrix(*(orig->mP));
00261 }
00262 if (orig->mPInv) {
00263 mPInv = new Matrix(*(orig->mPInv));
00264 }
00265 }
00266
00267 EigenGraspInterface::~EigenGraspInterface()
00268 {
00269 clear();
00270 }
00271
00272 void
00273 EigenGraspInterface::clear()
00274 {
00275 for (int i=0; i < eSize; i++) {
00276 delete mGrasps[i];
00277 }
00278 mGrasps.clear();
00279 eSize = 0;
00280 if (mOrigin){
00281 delete mOrigin;
00282 mOrigin = NULL;
00283 }
00284 if (mNorm) {
00285 delete mNorm;
00286 mNorm = NULL;
00287 }
00288 if (mP) {delete mP; mP = NULL;}
00289 if (mPInv) {delete mPInv; mPInv = NULL;}
00290 mRigid = false;
00291 }
00292
00301 int
00302 EigenGraspInterface::writeToFile(const char *filename)
00303 {
00304 TiXmlDocument doc;
00305 TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
00306 doc.LinkEndChild( decl );
00307
00308 TiXmlElement * root = new TiXmlElement( "EigenGrasps" );
00309 root->SetAttribute("dimensions", mRobot->getNumDOF());
00310 doc.LinkEndChild( root );
00311
00312 TiXmlElement * ep;
00313 for (int i=0; i<eSize; i++) {
00314 ep= new TiXmlElement("EG");
00315 mGrasps[i]->writeToFile(ep);
00316 root->LinkEndChild(ep);
00317
00318 }
00319
00320 ep = new TiXmlElement("ORIGIN");
00321 mOrigin->writeToFile(ep);
00322 root->LinkEndChild(ep);
00323
00324 doc.SaveFile(filename);
00325
00326 return 1;
00327 }
00328
00329 int
00330 EigenGraspInterface::setTrivial()
00331 {
00332 if (dSize != mRobot->getNumDOF() ) {
00333 fprintf(stderr,"ERROR setting trivial EG's\n");
00334 return 0;
00335 }
00336 clear();
00337 eSize = mRobot->getNumDOF();
00338 double *eg = new double[eSize];
00339 for (int i=0; i<eSize; i++) {
00340 eg[i] = 0;
00341 }
00342 for (int i=0; i<eSize; i++) {
00343 EigenGrasp *newGrasp = new EigenGrasp(eSize);
00344 eg[i]=1;
00345 newGrasp->setEigenGrasp(eg);
00346 eg[i]=0;
00347 mGrasps.push_back(newGrasp);
00348 }
00349 mNorm = new EigenGrasp(dSize);
00350 mNorm->setOnes();
00351 mOrigin = new EigenGrasp(dSize);
00352 setSimpleOrigin();
00353 computeProjectionMatrices();
00354 setName("Identity");
00355 delete [] eg;
00356 return 1;
00357 }
00358
00368 int
00369 EigenGraspInterface::readFromFile(QString filename)
00370 {
00371
00372 bool ok;
00373 QString fileType = filename.section('.',-1,-1);
00374 QString xmlFilename;
00375 if (fileType == "xml"){
00376
00377 xmlFilename = filename;
00378 } else {
00379 QTWARNING("Could not open " + xmlFilename);
00380 DBGA("Old non-xml file format is no longer supported");
00381 return 0;
00382 }
00383
00384
00385 TiXmlDocument doc(xmlFilename);
00386 if(doc.LoadFile()==false){
00387 DBGA("Failed to open EG file: " << filename.latin1());
00388 QTWARNING("Could not open " + xmlFilename);
00389 return 0;
00390 }
00391
00392 int numDims = 0;
00393 QString valueStr;
00394 clear();
00395 TiXmlElement* root = doc.RootElement();
00396 if(root == NULL){
00397 DBGA("The "<<filename.toStdString()<<" file must contain a root tag named EigenGrasps.");
00398 return 0;
00399 } else{
00400 valueStr = root->Attribute("dimensions");
00401 numDims = valueStr.toDouble(&ok); if (!ok) {DBGA("ERROR: Dimension should contain a number.");return 0;}
00402 if (numDims <= 0) {
00403 DBGA("invalid number of dimensions in EigenGrasps tag in file: "<<filename.toStdString());
00404 return 0;
00405 }
00406 }
00407
00408
00409 std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "EG");
00410 int numEG = countXmlElements(root, "EG");
00411 if (numEG < 1) {
00412 DBGA("Number of Eigengrasps specified: " << numEG);
00413 return 0;
00414 }
00415 std::list<const TiXmlElement*>::iterator p = elementList.begin();
00416 while(p!=elementList.end()){
00417 EigenGrasp *newGrasp = new EigenGrasp(numDims);
00418 if (!newGrasp->readFromXml(*p++)) return 0;
00419 newGrasp->normalize();
00420 mGrasps.push_back(newGrasp);
00421 }
00422
00423
00424
00425 elementList = findAllXmlElements(root, "ORIGIN");
00426 int numORG = countXmlElements(root, "ORIGIN");
00427 if (!numORG) {
00428 DBGA("No EG origin found; using automatic origin");
00429 mOrigin = new EigenGrasp(numDims);
00430 setSimpleOrigin();
00431 }
00432 else if(numORG==1)
00433 {
00434 mOrigin = new EigenGrasp(numDims);
00435 if (!mOrigin->readFromXml(*(elementList.begin()))){ return 0;}
00436 checkOrigin();
00437 }
00438 else
00439 {
00440 DBGA("Multiple Origins specified in Eigen Grasp file.");
00441 return 0;
00442 }
00443
00444
00445 elementList = findAllXmlElements(root, "NORM");
00446 int numNORM = countXmlElements(root, "NORM");
00447 if (!numNORM) {
00448 DBGA("No normalization data found; using factors of 1.0");
00449 mNorm = new EigenGrasp(numDims);
00450 mNorm->setOnes();
00451 }
00452 else if(numNORM==1)
00453 {
00454 mNorm = new EigenGrasp(numDims);
00455 if (!mNorm->readFromXml(*(elementList.begin()))){ return 0;}
00456 DBGA("EG Normalization data loaded from file");
00457 }
00458 else
00459 {
00460 DBGA("Multiple Normals specified in Eigen Grasp file.");
00461 return 0;
00462 }
00463
00464 eSize = mGrasps.size();
00465 DBGA("Read " << eSize << " eigengrasps from EG file");
00466
00467 computeProjectionMatrices();
00468 setMinMax();
00469 return 1;
00470 }
00471
00476 void
00477 EigenGraspInterface::checkOrigin()
00478 {
00479 for (int d=0; d < dSize; d++) {
00480 if (mOrigin->getAxisValue(d) < mRobot->getDOF(d)->getMin()) {
00481 fprintf(stderr,"WARNING: Eigengrasp origin lower than DOF %d range\n",d);
00482 mOrigin->setAxisValue( d, mRobot->getDOF(d)->getMin() );
00483
00484 }
00485 if (mOrigin->getAxisValue(d) > mRobot->getDOF(d)->getMax()) {
00486 fprintf(stderr,"WARNING: Eigengrasp origin greater than DOF %d range\n",d);
00487 mOrigin->setAxisValue( d, mRobot->getDOF(d)->getMax() );
00488
00489 }
00490 }
00491 }
00492 void
00493 EigenGraspInterface::setOrigin(const double *dof)
00494 {
00495 mOrigin->setEigenGrasp(dof);
00496 checkOrigin();
00497 }
00498
00502 void
00503 EigenGraspInterface::setSimpleOrigin()
00504 {
00505 double mmin, mmax;
00506 double* o = new double[dSize];
00507 for (int d=0; d < dSize; d++) {
00508 mmin = mRobot->getDOF(d)->getMin();
00509 mmax = mRobot->getDOF(d)->getMax();
00510 o[d] = (mmax + mmin) / 2.0;
00511 }
00512 setOrigin(o);
00513 delete [] o;
00514 }
00515
00528 void
00529 EigenGraspInterface::computeProjectionMatrices()
00530 {
00531 if (mP) delete mP;
00532 if (mPInv) delete mPInv;
00533
00534
00535 Matrix E(eSize, dSize);
00536 for (int e=0; e<eSize; e++) {
00537 for (int d=0; d<dSize; d++) {
00538 E.elem(e,d) = mGrasps[e]->getAxisValue(d);
00539 }
00540 }
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551 Matrix ET(E.transposed());
00552 Matrix EET(eSize, eSize);
00553 matrixMultiply(E, ET, EET);
00554 Matrix EETInv(eSize, eSize);
00555 int result = matrixInverse(EET, EETInv);
00556 if (result) {
00557 DBGA("Projection matrix is rank deficient!");
00558 mP = new Matrix(Matrix::ZEROES<Matrix>(eSize, dSize));
00559 mPInv = new Matrix(Matrix::ZEROES<Matrix>(dSize, eSize));
00560 return;
00561 }
00562 mP = new Matrix(eSize, dSize);
00563 matrixMultiply(EETInv, E, *mP);
00564 mPInv = new Matrix(ET);
00565 }
00566
00582 void
00583 EigenGraspInterface::setMinMax()
00584 {
00585 double m,M,mmin,mmax;
00586 double* eg = new double[dSize];
00587 double* currentDOF = new double[dSize];
00588 double* currentAmps = new double[eSize];
00589
00590 mRobot->getDOFVals(currentDOF);
00591 getAmp(currentAmps, currentDOF);
00592
00593 for (int e = 0; e < eSize; e++)
00594 {
00595 int mind, maxd;
00596
00597 #ifdef EIGENGRASP_LOOSE
00598 mmin = +1.0e5;
00599 mmax = -1.0e5;
00600 #else
00601 mmin = -1.0e5;
00602 mmax = +1.0e5;
00603 #endif
00604 mGrasps[e]->getEigenGrasp(eg);
00605 for (int d=0; d < dSize; d++) {
00606 if (eg[d]==0) continue;
00607 m = ( mRobot->getDOF(d)->getMin() - currentDOF[d] ) / ( eg[d] * mNorm->getAxisValue(d) );
00608 M = ( mRobot->getDOF(d)->getMax() - currentDOF[d] ) / ( eg[d] * mNorm->getAxisValue(d) );
00609 if ( m > M) {std::swap(m,M);}
00610 #ifdef EIGENGRASP_LOOSE
00611 if ( m < mmin ) {mmin = m; mind = d;}
00612 if ( M > mmax ) {mmax = M; maxd = d;}
00613 #else
00614 if ( m > mmin ) {mmin = m; mind = d;}
00615 if ( M < mmax ) {mmax = M; maxd = d;}
00616 #endif
00617 }
00618 if(!mGrasps[e]->mPredefinedLimits){
00619 mGrasps[e]->mMin = currentAmps[e] + mmin;
00620 mGrasps[e]->mMax = currentAmps[e] + mmax;
00621 }
00622
00623
00624 }
00625
00626 delete [] eg;
00627 delete [] currentDOF;
00628 delete [] currentAmps;
00629 }
00630
00631 void
00632 EigenGraspInterface::toDOFSpace(const double *amp, double *dof, const double *origin) const
00633 {
00634 Matrix a(amp, eSize, 1, true);
00635 Matrix x(dSize, 1);
00636 matrixMultiply(*mPInv, a, x);
00637 for(int d=0; d<dSize; d++) {
00638 dof[d] = x.elem(d,0) * mNorm->getAxisValue(d) + origin[d];
00639 }
00640 }
00641
00642 void
00643 EigenGraspInterface::toEigenSpace(double *amp, const double *dof, const double *origin) const
00644 {
00645 Matrix x(dSize, 1);
00646 for (int d=0; d < dSize; d++) {
00647 x.elem(d,0) = (dof[d] - origin[d]) / mNorm->getAxisValue(d);
00648 }
00649 Matrix a(eSize, 1);
00650 matrixMultiply(*mP, x, a);
00651 for (int e=0; e<eSize; e++) {
00652 amp[e] = a.elem(e,0);
00653 }
00654 }
00655
00668 void
00669 EigenGraspInterface::getDOF(const double *amp, double *dof) const
00670 {
00671 double *origin = new double[dSize];
00672 double *rigidAmp = new double[eSize];
00673
00674 for (int e=0; e < eSize; e++) {
00675 if ( !mGrasps[e]->mFixed )
00676 rigidAmp[e] = amp[e];
00677 else {
00678 rigidAmp[e] = mGrasps[e]->fixedAmplitude;
00679 DBGA(e << " fixed at " << mGrasps[e]->fixedAmplitude);
00680 }
00681 }
00682
00683 if (mRigid) {
00684
00685
00686 mOrigin->getEigenGrasp(origin);
00687 toDOFSpace(rigidAmp, dof, origin);
00688 } else {
00689
00690
00691 double *currentAmp = new double[eSize];
00692 double *relativeAmp = new double[eSize];
00693 mRobot->getDOFVals(origin);
00694 getAmp(currentAmp, origin);
00695 for (int e=0; e < eSize; e++) {
00696 relativeAmp[e] = rigidAmp[e] - currentAmp[e];
00697 }
00698 toDOFSpace(relativeAmp, dof, origin);
00699 delete [] currentAmp;
00700 delete [] relativeAmp;
00701 }
00702
00703 delete [] rigidAmp;
00704 delete [] origin;
00705 }
00706
00712 void
00713 EigenGraspInterface::getAmp(double *amp, const double *dof) const
00714 {
00715 double *origin = new double[dSize];
00716 mOrigin->getEigenGrasp(origin);
00717 toEigenSpace(amp, dof, origin);
00718 delete [] origin;
00719 }
