graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id: eigenGrasp.cpp,v 1.15 2009/08/14 18:09:33 cmatei Exp $
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 
00036 #include "math/matrix.h"
00037 
00038 #include "debug.h"
00039 
00041 EigenGrasp::EigenGrasp(int size, double e)
00042 {
00043         if (size <= 0 ) {
00044                 fprintf(stderr,"Wrong size of eigen grasp\n");
00045                 return;
00046         }
00047         mVals = new double[size];
00048         for (int i=0; i < size; i++)
00049                 mVals[i] = 0;
00050         mSize = size;
00051         mEigenValue = e;
00052         mFixed = false;
00053 }
00054 
00055 EigenGrasp::EigenGrasp(const EigenGrasp *orig)
00056 {
00057         mSize = orig->mSize;
00058         mVals = new double[mSize];
00059         memcpy(mVals, orig->mVals, mSize*sizeof(double));
00060         mEigenValue = orig->mEigenValue;
00061         mMin = orig->mMin; mMax = orig->mMax;
00062         mFixed = orig->mFixed;
00063         fixedAmplitude = orig->fixedAmplitude;
00064 }
00065 
00066 EigenGrasp::~EigenGrasp()
00067 {
00068         delete [] mVals;
00069 }
00070 
00071 void
00072 EigenGrasp::getEigenGrasp(double *eg) const
00073 {
00074         for (int i=0; i<mSize; i++)
00075                 eg[i] = mVals[i];
00076 }
00077 
00078 void
00079 EigenGrasp::setEigenGrasp(const double *eg)
00080 {
00081         for (int i=0; i<mSize; i++)
00082                 mVals[i] = eg[i];
00083 }
00084 
00085 double
00086 EigenGrasp::normalize()
00087 {
00088         int i;
00089         double norm = 0;
00090         for (i=0; i<mSize; i++) {
00091                 norm += mVals[i] * mVals[i];
00092         }
00093         norm = sqrt(norm);
00094         for (i=0; i<mSize; i++) {
00095                 mVals[i] = mVals[i] / norm;
00096         }
00097         return norm;
00098 }
00099 
00100 void
00101 EigenGrasp::writeToFile(FILE *fp)
00102 {
00103         fprintf(fp,"%f\n",mEigenValue);
00104         for (int i=0; i<mSize; i++) {
00105                 fprintf(fp,"%f ", mVals[i]);
00106         }
00107         fprintf(fp,"\n");
00108 }
00109 
00110 void 
00111 EigenGrasp::readFromFile(FILE *fp)
00112 {
00113         float v;
00114         fscanf(fp,"%f",&v);
00115         mEigenValue = v;
00116         for (int i=0; i<mSize; i++) {
00117                 fscanf(fp,"%f",&v);
00118                 mVals[i] = v;
00119         }
00120 }
00121 
00122 int
00123 EigenGrasp::readFromStream(QTextStream *stream)
00124 {
00125         if (stream->atEnd()) {fprintf(stderr,"Unable to read EG, end of file\n");return 0;}
00126         QString line = stream->readLine();
00127         mEigenValue = line.toDouble();
00128         if (stream->atEnd()) {fprintf(stderr,"Unable to read EG, end of file\n");return 0;}
00129         line = stream->readLine();
00130         for (int i=0; i<mSize; i++) {
00131                 QString val = line.section(' ',i,i,QString::SectionSkipEmpty);
00132                 if ( val.isNull() || val.isEmpty() ) {fprintf(stderr,"Unable to read EG value #%d\n",i);return 0;}
00133                 mVals[i] = val.toDouble();
00134         }
00135         return 1;
00136 }
00137 
00138 double
00139 EigenGrasp::dot(double *d)
00140 {
00141         double dot = 0;
00142         for (int i=0; i<mSize; i++) {
00143                 dot += mVals[i] * d[i];
00144         }
00145         return dot;
00146 }
00147 
00148 //---------------------------------------------------- EigenGraspInterface --------------------------------------
00149 
00150 EigenGraspInterface::EigenGraspInterface(Robot *r)
00151 {
00152         mRobot = r;
00153         dSize = mRobot->getNumDOF();
00154         eSize = mGrasps.size();
00155         mOrigin = NULL;
00156         mNorm = NULL;
00157         mRigid = false;
00158         mP = mPInv = NULL;
00159 }
00160 
00161 EigenGraspInterface::EigenGraspInterface(const EigenGraspInterface *orig)
00162 {
00163         mRobot = orig->mRobot;
00164         dSize = mRobot->getNumDOF();
00165         eSize = orig->eSize;
00166 
00167         for (int i=0; i<eSize; i++) {
00168                 mGrasps.push_back( new EigenGrasp(orig->mGrasps[i]) );
00169         }
00170 
00171         mOrigin = new EigenGrasp( orig->mOrigin );
00172         mNorm = new EigenGrasp( orig->mNorm);
00173         mName = orig->mName;
00174         mRigid = orig->mRigid;
00175         mP = mPInv = NULL;
00176         if (orig->mP) {
00177                 mP = new Matrix(*(orig->mP));
00178         }
00179         if (orig->mPInv) {
00180                 mPInv = new Matrix(*(orig->mPInv));
00181         }
00182 }
00183 
00184 EigenGraspInterface::~EigenGraspInterface()
00185 {
00186         clear();
00187 }
00188 
00189 void
00190 EigenGraspInterface::clear()
00191 {
00192         for (int i=0; i < eSize; i++) {
00193                 delete mGrasps[i];
00194         }
00195         mGrasps.clear();
00196         eSize = 0;
00197         if (mOrigin){
00198                 delete mOrigin;
00199                 mOrigin = NULL;
00200         }
00201         if (mNorm) {
00202                 delete mNorm;
00203                 mNorm = NULL;
00204         }
00205         if (mP) {delete mP; mP = NULL;}
00206         if (mPInv) {delete mPInv; mPInv = NULL;}
00207         mRigid = false;
00208 }
00209 
00218 int
00219 EigenGraspInterface::writeToFile(const char *filename)
00220 {
00221         FILE *fp = fopen(filename,"w");
00222         if (!fp) return 0;
00223 
00224         fprintf(fp,"%d\n",eSize);
00225         fprintf(fp,"%d\n",mRobot->getNumDOF());
00226         for (int i=0; i<eSize; i++) {
00227                 mGrasps[i]->writeToFile(fp);
00228         }
00229         mOrigin->writeToFile(fp);
00230         fclose(fp);
00231         return 1;
00232 }
00233 
00234 int
00235 EigenGraspInterface::setTrivial()
00236 {
00237         if (dSize != mRobot->getNumDOF() ) {
00238                 fprintf(stderr,"ERROR setting trivial EG's\n");
00239                 return 0;
00240         }
00241         clear();
00242         eSize = mRobot->getNumDOF();
00243         double *eg = new double[eSize];
00244         for (int i=0; i<eSize; i++) {
00245                 eg[i] = 0;
00246         }
00247         for (int i=0; i<eSize; i++) {
00248                 EigenGrasp *newGrasp = new EigenGrasp(eSize);
00249                 eg[i]=1;
00250                 newGrasp->setEigenGrasp(eg);
00251                 eg[i]=0;
00252                 mGrasps.push_back(newGrasp);
00253         }
00254         mNorm = new EigenGrasp(dSize);
00255         mNorm->setOnes();
00256         mOrigin = new EigenGrasp(dSize);
00257         setSimpleOrigin();
00258         computeProjectionMatrices();
00259         setName("Identity");
00260         delete [] eg;
00261         return 1;
00262 }
00263 
00273 int
00274 EigenGraspInterface::readFromFile(QString filename)
00275 {
00276         QFile file(filename);
00277         if (!file.open(QIODevice::ReadOnly)) {
00278                 DBGA("Failed to open EG file: " << filename.latin1());
00279                 return 0;
00280         }
00281         QTextStream stream( &file );
00282 
00283         clear();
00284         int numDims = 0;
00285         QString line,id;
00286         bool error = false;
00287         while (!error && !stream.atEnd() ) {
00288                 line = stream.readLine();
00289                 line.stripWhiteSpace();
00290                 id = line.section(' ',0,0);
00291                 if (id == "DIMENSIONS") {
00292                         numDims = line.section(' ',1,1).toInt();
00293                         if (numDims != mRobot->getNumDOF()) {
00294                                 DBGA("ERROR reading eigengrasp: appears to be constructed for another robot.");
00295                         }
00296                 } else if ( id == "EG" ) {
00297                         if (!numDims) {DBGA("Number of dimensions not specified!"); error = true; continue;}
00298                         EigenGrasp *newGrasp = new EigenGrasp(numDims);
00299                         if (!newGrasp->readFromStream(&stream)) {error = true; continue;}
00300                         newGrasp->normalize();
00301                         mGrasps.push_back(newGrasp);
00302                 } else if ( id == "ORIGIN" ) {
00303                         if (!numDims) {DBGA("Number of dimensions not specified!"); error = true; continue;}
00304                         mOrigin = new EigenGrasp(numDims);
00305                         if (!mOrigin->readFromStream(&stream)){error = true; continue;}
00306                         checkOrigin();
00307                 } else if ( id == "NORM" ) {
00308                         if (!numDims) {DBGA("Number of dimensions not specified!"); error = true; continue;}
00309                         mNorm = new EigenGrasp(numDims);
00310                         if (!mNorm->readFromStream(&stream)){error = true; continue;}
00311                         DBGA("EG Normalization data loaded from file");
00312                 }
00313         }
00314         if (error) {
00315                 DBGA("Error reading EG file");
00316                 return 0;
00317         }
00318 
00319         eSize = mGrasps.size();
00320         DBGA("Read " << eSize << " eigengrasps from EG file");
00321         if (!mNorm) {
00322                 DBGA("No normalization data found; using factors of 1.0");
00323                 mNorm = new EigenGrasp(dSize);
00324                 mNorm->setOnes();
00325         }
00326         if (!mOrigin) {
00327                 DBGA("No EG origin found; using automatic origin");
00328                 mOrigin = new EigenGrasp(dSize);
00329                 setSimpleOrigin();
00330         }
00331         computeProjectionMatrices();
00332         setMinMax();
00333         return 1;
00334 }
00335 
00340 void
00341 EigenGraspInterface::checkOrigin()
00342 {
00343         for (int d=0; d < dSize; d++) {
00344                 if (mOrigin->getAxisValue(d) < mRobot->getDOF(d)->getMin()) {
00345                         fprintf(stderr,"WARNING: Eigengrasp origin lower than DOF %d range\n",d);
00346                         mOrigin->setAxisValue( d, mRobot->getDOF(d)->getMin() );
00347 //                      mOrigin->setAxisValue( d, 0.5 * (mRobot->getDOF(d)->getMin() + mRobot->getDOF(d)->getMax()) );
00348                 }
00349                 if (mOrigin->getAxisValue(d) > mRobot->getDOF(d)->getMax()) {
00350                         fprintf(stderr,"WARNING: Eigengrasp origin greater than DOF %d range\n",d);
00351                         mOrigin->setAxisValue( d, mRobot->getDOF(d)->getMax() );
00352 //                      mOrigin->setAxisValue( d, 0.5 * (mRobot->getDOF(d)->getMin() + mRobot->getDOF(d)->getMax()) );
00353                 }
00354         }
00355 }
00356 void
00357 EigenGraspInterface::setOrigin(const double *dof)
00358 {
00359         mOrigin->setEigenGrasp(dof);
00360         checkOrigin();
00361 }
00362 
00366 void
00367 EigenGraspInterface::setSimpleOrigin()
00368 {
00369         double mmin, mmax;
00370         double* o = new double[dSize];
00371         for (int d=0; d < dSize; d++) {
00372                 mmin = mRobot->getDOF(d)->getMin();
00373                 mmax = mRobot->getDOF(d)->getMax();
00374                 o[d] = (mmax + mmin) / 2.0;
00375         }
00376         setOrigin(o);
00377         delete [] o;
00378 }
00379 
00392 void
00393 EigenGraspInterface::computeProjectionMatrices()
00394 {
00395         if (mP) delete mP;
00396         if (mPInv) delete mPInv;
00397 
00398         //first build the E matrix that just contains the (potentially non-orthonormal) bases
00399         Matrix E(eSize, dSize);
00400         for (int e=0; e<eSize; e++) {
00401                 for (int d=0; d<dSize; d++) {
00402                         E.elem(e,d) = mGrasps[e]->getAxisValue(d);
00403                 }
00404         }
00405 
00406         //the trivial case (assumes ortho-normal E)
00407         //mP = new Matrix(E);
00408         //mPInv = new Matrix(E.transposed());
00409 
00410         //general case
00411         //remember: P(PInv(a)) = a (always)
00412         //                  PInv(P(x)) != x (usually)
00413         // P = (E*ET)'*E
00414         // P' = ET
00415         Matrix ET(E.transposed());
00416         Matrix EET(eSize, eSize);
00417         matrixMultiply(E, ET, EET);
00418         Matrix EETInv(eSize, eSize);
00419         int result = matrixInverse(EET, EETInv);
00420         if (result) {
00421                 DBGA("Projection matrix is rank deficient!");
00422                 mP = new Matrix(Matrix::ZEROES<Matrix>(eSize, dSize));
00423                 mPInv = new Matrix(Matrix::ZEROES<Matrix>(dSize, eSize));
00424                 return;
00425         }
00426         mP = new Matrix(eSize, dSize);
00427         matrixMultiply(EETInv, E, *mP);
00428         mPInv = new Matrix(ET);
00429 }
00430 
00446 void
00447 EigenGraspInterface::setMinMax()
00448 {
00449         double m,M,mmin,mmax;
00450         double* eg = new double[dSize];
00451         double* currentDOF = new double[dSize];
00452         double* currentAmps  = new double[eSize];
00453 
00454         mRobot->getDOFVals(currentDOF);
00455         getAmp(currentAmps, currentDOF);
00456 
00457         for (int e = 0; e < eSize; e++)
00458         {
00459                 int mind, maxd;
00460                 //fprintf(stderr,"\n------\nEG %d\n",e);
00461 #ifdef EIGENGRASP_LOOSE
00462                 mmin = +1.0e5;
00463                 mmax = -1.0e5;
00464 #else
00465                 mmin = -1.0e5;
00466                 mmax = +1.0e5;
00467 #endif
00468                 mGrasps[e]->getEigenGrasp(eg);
00469                 for (int d=0; d < dSize; d++) {
00470                         if (eg[d]==0) continue;
00471                         m = ( mRobot->getDOF(d)->getMin() - currentDOF[d] ) / ( eg[d] * mNorm->getAxisValue(d) );
00472                         M = ( mRobot->getDOF(d)->getMax() - currentDOF[d] ) / ( eg[d] * mNorm->getAxisValue(d) );
00473                         if ( m > M) {std::swap(m,M);} //swap m and M if needed
00474 #ifdef EIGENGRASP_LOOSE
00475                         if ( m < mmin ) {mmin = m; mind = d;}
00476                         if ( M > mmax ) {mmax = M; maxd = d;}
00477 #else
00478                         if ( m > mmin ) {mmin = m; mind = d;}
00479                         if ( M < mmax ) {mmax = M; maxd = d;}
00480 #endif
00481                 }
00482                 mGrasps[e]->mMin = currentAmps[e] + mmin;
00483                 mGrasps[e]->mMax = currentAmps[e] + mmax;
00484 
00485                 //fprintf(stderr,"Current: %f; range: %f(%d) -- %f(%d) \n",currentAmps[e],
00486                 //              mGrasps[e]->mMin, mind, mGrasps[e]->mMax, maxd);
00487         }
00488 
00489         delete [] eg;
00490         delete [] currentDOF;
00491         delete [] currentAmps;
00492 }
00493 
00494 void
00495 EigenGraspInterface::toDOFSpace(const double *amp, double *dof, const double *origin) const
00496 {
00497         Matrix a(amp, eSize, 1, true);
00498         Matrix x(dSize, 1);
00499         matrixMultiply(*mPInv, a, x);
00500         for(int d=0; d<dSize; d++) {
00501                 dof[d] = x.elem(d,0) * mNorm->getAxisValue(d) + origin[d];
00502         }
00503 }
00504 
00505 void
00506 EigenGraspInterface::toEigenSpace(double *amp, const double *dof, const double *origin) const
00507 {
00508         Matrix x(dSize, 1);
00509         for (int d=0; d < dSize; d++) {
00510                 x.elem(d,0) = (dof[d] - origin[d]) / mNorm->getAxisValue(d);
00511         }
00512         Matrix a(eSize, 1);
00513         matrixMultiply(*mP, x, a);
00514         for (int e=0; e<eSize; e++) {
00515                 amp[e] = a.elem(e,0);
00516         }
00517 }
00518 
00531 void 
00532 EigenGraspInterface::getDOF(const double *amp, double *dof) const 
00533 {
00534         double *origin = new double[dSize];
00535         double *rigidAmp = new double[eSize];
00536         
00537         for (int e=0; e < eSize; e++) {
00538                 if ( !mGrasps[e]->mFixed )
00539                         rigidAmp[e] = amp[e];
00540                 else {
00541                         rigidAmp[e] = mGrasps[e]->fixedAmplitude;
00542                         DBGA(e << " fixed at " << mGrasps[e]->fixedAmplitude);
00543                 }
00544         }
00545 
00546         if (mRigid) {
00547                 //if the interface is rigid, we add the amplitudes to the pre-specified eigen space origin
00548                 //it means we DISCARD whatever component was in the pose that was not from eigenspace
00549                 mOrigin->getEigenGrasp(origin);
00550                 toDOFSpace(rigidAmp, dof, origin);
00551         } else {
00552                 //if it is not, we just add the change in amplitudes to the current position of the robot
00553                 //this means we KEEP the position component that was not in eigen space
00554                 double *currentAmp = new double[eSize];
00555                 double *relativeAmp = new double[eSize];
00556                 mRobot->getDOFVals(origin);
00557                 getAmp(currentAmp, origin);
00558                 for (int e=0; e < eSize; e++) {
00559                         relativeAmp[e] = rigidAmp[e] - currentAmp[e];
00560                 }
00561                 toDOFSpace(relativeAmp, dof, origin);
00562                 delete [] currentAmp;
00563                 delete [] relativeAmp;          
00564         }
00565 
00566         delete [] rigidAmp;
00567         delete [] origin;
00568 }
00569 
00575 void 
00576 EigenGraspInterface::getAmp(double *amp, const double *dof) const
00577 {
00578         double *origin = new double[dSize];
00579         mOrigin->getEigenGrasp(origin);
00580         toEigenSpace(amp, dof, origin);
00581         delete [] origin;
00582 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/eigenGrasp.cpp
