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 #ifndef _HandObjectState_h_
00027 #define _HandObjectState_h_
00028
00029 #include <vector>
00030 #include <list>
00031 #include <QString>
00032
00033 #include "search.h"
00034 #include "matvec3D.h"
00035
00036 class Hand;
00037 class Body;
00038 class GraspableBody;
00039 class QualityMeasure;
00040
00041 class SoSeparator;
00042 class SoTransform;
00043 class SoMaterial;
00044
00052 class SearchVariable
00053 {
00054 private:
00055 double mValue;
00057
00059 double mConfidence;
00061 QString mName;
00063 bool mFixed;
00065
00066 bool mCircular;
00067
00068 public:
00069 double mMinVal, mMaxVal, mMaxJump;
00070
00071 SearchVariable(QString name, double min, double max, double maxJump, bool circular = false);
00072 SearchVariable(const SearchVariable *v);
00073
00074 void setValue(double v){mValue = v;}
00075 double getValue() const {return mValue;}
00076 void setFixed(bool f){mFixed = f;}
00077 bool isFixed() const {return mFixed;}
00078 void setCircular(bool c){mCircular = c;}
00079 bool isCircular() const {return mCircular;}
00080 void setRange(double min, double max){mMinVal = min; mMaxVal = max;}
00081 double getRange() const { return fabs(mMaxVal - mMinVal); }
00082 void setMaxJump(double mj){mMaxJump = mj;}
00083 void setJump(double j){mMaxJump = getRange() * j;}
00084 double getConfidence() const {return mConfidence;}
00085 void setConfidence(double c) {assert(c>=0 && c<=1);mConfidence=c;}
00086 QString getName() const {return mName;}
00087 };
00088
00096 class SearchParameter {
00097 private:
00098 QString mName;
00099 double mValue;
00100 public:
00101 SearchParameter(QString name, double val=0) : mName(name), mValue(val) {}
00102 SearchParameter(const SearchParameter &p) {
00103 mName = p.mName;
00104 mValue = p.mValue;
00105 }
00106 void set(double v){mValue = v;}
00107 double get() const {return mValue;}
00108 QString name() const {return mName;}
00109 };
00110
00114 class VariableSet
00115 {
00116 protected:
00118 std::vector<SearchVariable*> mVariables;
00120 std::vector<SearchParameter> mParameters;
00122
00123 const Hand *mHand;
00124
00125 void clearVariables();
00126
00128 virtual void createVariables() = 0;
00129 public:
00130 VariableSet(const Hand *h){mHand = h;}
00131 VariableSet(const VariableSet &vs);
00132 virtual ~VariableSet();
00133 virtual StateType getType() const = 0;
00134
00135 inline void copyValuesFrom(const VariableSet *s);
00136 inline void reset();
00137 virtual void changeHand(const Hand *h, bool sticky = false);
00138
00139 int getNumVariables() const {return mVariables.size();}
00140 int getNumUsedVariables() const;
00141 SearchVariable* getVariable(int i){assert(i<(int)mVariables.size()); return mVariables[i];}
00142 const SearchVariable* getVariable(int i) const {assert(i<(int)mVariables.size()); return mVariables[i];}
00143 SearchVariable* getVariable (QString name);
00144 const SearchVariable* getConstVariable (QString name) const;
00145 double readVariable(int i) const {return mVariables[i]->getValue();}
00146 double readVariable(QString name) const;
00147 virtual double distance(const VariableSet *s) const;
00148
00149 void setParameter(QString name, double value);
00150 double getParameter (QString name) const;
00151
00152 void addParameter(QString name, double value);
00153 void removeParameter(QString name);
00154
00155 void setAllConfidences(double c);
00156 void setAllFixed(bool f);
00157
00158 void writeToFile(FILE *fp) const;
00159 bool readFromFile(FILE *fp);
00160 bool readFromArray(std::vector<double> array);
00161 void print() const;
00162 };
00163
00165 class AttributeSet : public VariableSet
00166 {
00167 private:
00169 virtual void createVariables(){}
00170 public:
00171 AttributeSet(const Hand *h) : VariableSet(h){}
00172 AttributeSet(const AttributeSet &as) : VariableSet(as){}
00173 virtual StateType getType() const {return ATTRIBUTES;}
00174 };
00175
00181 class PostureState : public VariableSet
00182 {
00183 public:
00184 PostureState(const Hand *h) : VariableSet(h) {}
00185 ~PostureState(){}
00187 virtual void getHandDOF(double *dof) const = 0;
00189
00190 virtual void storeHandDOF(const double *dof) = 0;
00191 static PostureState* createInstance(StateType type, const Hand *h);
00192 };
00193
00200 class PositionState : public VariableSet
00201 {
00202 public:
00203 PositionState(const Hand *h) : VariableSet(h){}
00204 ~PositionState(){}
00206 virtual transf getCoreTran() const = 0;
00208
00209 virtual void setTran(const transf &t) = 0;
00210 static PositionState* createInstance(StateType type, const Hand *h);
00211 };
00212
00246 class HandObjectState
00247 {
00248 private:
00249 void init();
00251 PostureState *mPosture;
00253 PositionState *mPosition;
00255
00259 AttributeSet* mAttributes;
00260
00262 Hand *mHand;
00263
00265 GraspableBody *mTargetObject;
00266
00268 transf mRefTran;
00269
00271 SoSeparator *IVRoot;
00273 SoTransform *IVTran;
00275 SoMaterial *IVMat;
00276
00277 public:
00278 HandObjectState(Hand *h);
00279 HandObjectState(const HandObjectState *s){init();copyFrom(s);}
00280 virtual ~HandObjectState();
00281 inline void copyFrom(const HandObjectState *s);
00283 inline void reset();
00284
00286 void saveCurrentHandState();
00287
00288 void changeHand(Hand *h, bool sticky = false);
00289 Hand *getHand() const {return mHand;}
00290 void setObject(GraspableBody *b){mTargetObject = b;}
00291 GraspableBody *getObject() const {return mTargetObject;}
00292
00294 void setPositionType(StateType type, bool sticky = false);
00296 void setPostureType(StateType type, bool sticky = false);
00298 void setRefTran(transf t, bool sticky = false);
00299
00301 bool execute(Hand *h = NULL) const;
00302
00304 transf getTotalTran() const {return mPosition->getCoreTran() * mRefTran;}
00305
00306 PostureState* getPosture(){return mPosture;}
00307 const PostureState* readPosture() const {return mPosture;}
00308 PositionState* getPosition(){return mPosition;}
00309 const PositionState* readPosition() const {return mPosition;}
00310
00311 void writeToFile(FILE *fp) const;
00312 bool readFromFile(FILE *fp);
00313
00315 void printState () const {mPosture->print(); mPosition->print();}
00316
00317 double distance(const HandObjectState *s) const;
00318
00319 int getNumVariables() const {return mPosture->getNumVariables() + mPosition->getNumVariables();}
00320 int getNumUsedVariables() const {return mPosture->getNumUsedVariables() +
00321 mPosition->getNumUsedVariables();}
00322
00323 SearchVariable* getVariable(int i);
00324 const SearchVariable* getVariable(int i) const;
00325 SearchVariable* getVariable (QString name);
00326 const SearchVariable* getConstVariable (QString name) const;
00327
00328 void setAttribute(QString name, double value){mAttributes->setParameter(name, value);}
00329 double getAttribute(QString name) const {return mAttributes->getParameter(name);}
00330 void addAttribute(QString name, double value) {mAttributes->addParameter(name, value);}
00331 void removeAttribute(QString name){mAttributes->removeParameter(name);}
00332
00334 SoSeparator *getIVRoot();
00335 void setIVMarkerColor(double r, double g, double b) const;
00336
00338 void showVisualMarker();
00340 void hideVisualMarker();
00341 };
00342
00344
00350 class GraspPlanningState : public HandObjectState
00351 {
00352 private:
00354 double mEnergy;
00356 bool mLegal;
00358 int mItNumber;
00360 double mDistance;
00362 double mQualEpsilon;
00364 double mQualVolume;
00366 int mIndex;
00367
00369 std::list<position> mContacts;
00370
00371 public:
00372 GraspPlanningState(Hand* hand) : HandObjectState(hand) {}
00373 GraspPlanningState(const GraspPlanningState *gps) : HandObjectState(gps){
00374 copyFrom(gps);
00375 }
00376
00377 void copyFrom(const GraspPlanningState *t){
00378 HandObjectState::copyFrom(t);
00379
00380 mEnergy=t->getEnergy();
00381 mLegal=t->isLegal();
00382 mDistance = t->getDistance();
00383 mItNumber = t->getItNumber();
00384
00385 mQualEpsilon = t->getEpsilonQuality();
00386 mQualVolume = t->getVolume();
00387 mIndex = t->getIndex();
00388 }
00389
00390 double getEnergy() const {return mEnergy;} void setEnergy(double e){mEnergy = e;}
00391 bool isLegal() const {return mLegal;} void setLegal(bool l){mLegal = l;}
00392 int getItNumber() const {return mItNumber;} void setItNumber(int n){mItNumber=n;}
00393 double getDistance() const {return mDistance;} void setDistance(double d){mDistance = d;}
00394
00395 double getEpsilonQuality() const {return mQualEpsilon;} void setEpsilonQuality(double q){mQualEpsilon = q;}
00396 double getVolume() const {return mQualVolume;} void setVolume(double v){mQualVolume = v;}
00397 int getIndex() const {return mIndex;} void setIndex(double i){mIndex = i;}
00398 std::list<position> *getContacts(){return &mContacts;}
00399 const std::list<position> *getContacts() const {return &mContacts;}
00400
00402 static bool compareStates(const GraspPlanningState *s1, const GraspPlanningState *s2);
00404 static bool compareStatesDistances(const GraspPlanningState *s1, const GraspPlanningState *s2);
00405
00406 };
00407
00408 void VariableSet::copyValuesFrom(const VariableSet *s)
00409 {
00410 for (int i=0; i<(int)s->mVariables.size(); i++) {
00411 mVariables[i]->setValue( s->mVariables[i]->getValue());
00412 mVariables[i]->setFixed( s->mVariables[i]->isFixed() );
00413 }
00414 for (int i=0; i<(int)s->mParameters.size(); i++) {
00415 mParameters[i].set( s->mParameters[i].get() );
00416 }
00417 }
00418
00423 void VariableSet::reset()
00424 {
00425 for (int i=0; i<(int)mVariables.size(); i++) {
00426 mVariables[i]->setValue(0);
00427 mVariables[i]->setFixed(false);
00428 mVariables[i]->setConfidence(0.0);
00429 }
00430 }
00431
00432 void HandObjectState::copyFrom(const HandObjectState *s)
00433 {
00434 mHand = s->mHand;
00435 mTargetObject = s->mTargetObject;
00436 mRefTran = s->mRefTran;
00437
00438 if (mPosture) delete mPosture;
00439 if (mPosition) delete mPosition;
00440 mPosture = PostureState::createInstance(s->mPosture->getType(), mHand);
00441 mPosture->copyValuesFrom(s->mPosture);
00442 mPosition = PositionState::createInstance(s->mPosition->getType(), mHand);
00443 mPosition->copyValuesFrom(s->mPosition);
00444
00445 if (mAttributes) delete mAttributes;
00446 mAttributes = new AttributeSet(*s->mAttributes);
00447 }
00448
00449 void HandObjectState::reset()
00450 {
00451 mPosture->reset();
00452 mPosition->reset();
00453 }
00454
00459 void createPositionSpaceSampling(const GraspPlanningState &seed,
00460 std::list<GraspPlanningState*> *sampling, int samples);
00461
00462 #endif
