/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/eigenGrasp.h
Go to the documentation of this file.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.h,v 1.14 2009/04/21 14:53:07 cmatei Exp $ 00023 // 00024 //###################################################################### 00025 #ifndef _eigengrasp_h_ 00026 #define _eigengrasp_h_ 00027 00028 #include <assert.h> 00029 00030 #include <vector> 00031 #include <QString> 00032 #include <QTextStream> 00033 00041 #define EIGENGRASP_LOOSE 00042 00043 class Matrix; 00044 class Robot; 00045 class QTextStream; 00047 00056 class EigenGrasp 00057 { 00058 private: 00060 int mSize; 00062 double *mVals; 00064 double mEigenValue; 00065 00066 public: 00068 double mMin; 00070 double mMax; 00072 bool mFixed; 00074 double fixedAmplitude; 00075 00077 EigenGrasp(int size, double e=0); 00079 EigenGrasp(const EigenGrasp *orig); 00080 ~EigenGrasp(); 00081 00082 int getSize() const {return mSize;} 00083 00085 void getEigenGrasp (double *eg) const; 00087 void setEigenGrasp(const double *eg); 00088 00090 double getEigenValue() const {return mEigenValue;} 00092 void setEigenValue(double e){mEigenValue = e;} 00093 00095 void setOnes(){for (int i=0; i<mSize; i++) mVals[i]=1.0;} 00096 00098 double getAxisValue(int i) const {assert(i<mSize&&i>=0); return mVals[i];} 00100 void setAxisValue(int i, double val){assert(i<mSize&&i>=0); mVals[i]=val;} 00101 00103 double normalize(); 00104 00106 double dot(double *d); 00107 00108 void writeToFile(FILE *fp); 00109 void readFromFile(FILE *fp); 00110 int readFromStream(QTextStream *stream); 00111 00113 void fix(double a){mFixed = true; fixedAmplitude = a;} 00115 void unfix(){mFixed = false;} 00116 }; 00117 00128 class EigenGraspInterface 00129 { 00130 private: 00132 Robot *mRobot; 00134 int dSize; 00136 int eSize; 00137 00139 std::vector<EigenGrasp*> mGrasps; 00141 EigenGrasp *mOrigin; 00142 00144 00146 EigenGrasp *mNorm; 00147 00149 QString mName; 00150 00152 Matrix *mP; 00154 Matrix *mPInv; 00155 00157 00160 mutable bool mRigid; 00161 00162 void clear(); 00163 00165 void computeProjectionMatrices(); 00166 00168 void toDOFSpace(const double *amp, double *dof, const double *origin) const; 00170 void toEigenSpace(double *amp, const double *dof, const double *origin) const; 00171 public: 00172 EigenGraspInterface(Robot *r); 00173 EigenGraspInterface(const EigenGraspInterface *orig); 00174 ~EigenGraspInterface(); 00175 00177 const EigenGrasp* getGrasp(int g){return mGrasps[g];} 00179 int getSize() const {return (int)mGrasps.size();} 00180 00182 bool isRigid() const {return mRigid;} 00184 00187 void setRigid(bool r) const {mRigid = r;} 00188 00190 int writeToFile(const char *filename); 00192 int readFromFile(QString filename); 00194 int setTrivial(); 00195 00197 void setName(QString n){mName = n;} 00199 const QString getName() const {return mName;} 00200 00202 void setOrigin(const double *dof); 00204 void setSimpleOrigin(); 00205 00207 void setMinMax(); 00209 void checkOrigin(); 00210 00212 00216 void fixEigenGrasp(int i, double fa){mGrasps[i]->fix(fa);} 00218 void unfixEigenGrasp(int i){mGrasps[i]->unfix();} 00219 00220 // These are the main functions that interface the EG: 00221 00223 void getDOF (const double *amp, double *dof) const; 00225 void getAmp(double *amp, const double *dof) const; 00226 }; 00227 00228 #endif