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):  Andrew T. Miller and Matei T. Ciocarlie 
00021 //
00022 // $Id: body.h,v 1.46 2010/08/10 17:23:59 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00029 #ifndef BODY_HXX
00030 
00031 #include <QTextStream>
00032 #include <QString>
00033 #include <list>
00034 #include <vector>
00035 
00036 #include "worldElement.h"
00037 #include "contact.h"
00038 
00039 #ifdef CGDB_ENABLED
00040 #include "DBase/graspit_db_model.h"
00041 #endif
00042 
00043 class SoCoordinate3;
00044 class SoGroup;
00045 class SoIndexedFaceSet;
00046 class SoMaterial;
00047 class SoScale;
00048 class SoSeparator;
00049 class SoSwitch;
00050 class SoTranslation;
00051 class SoTransform;
00052 
00053 class Contact;
00054 class Robot;
00055 class DynJoint;
00056 class World;
00057 class BoundingBox;
00058 class Triangle;
00059 class TiXmlElement;
00060 
00062 
00065 typedef struct {
00066   double norm[3];
00067   double w;
00068   double *verts[3];
00069 } FACE;
00070 
00072 
00076 class Body : public WorldElement {
00077   Q_OBJECT
00078 public:
00080   static const float CONE_HEIGHT;
00081 
00082 protected:
00084   int material; 
00085 
00087   bool mIsElastic;
00088         
00090   double youngMod;
00091 
00093   QString mGeometryFilename;
00094         
00096   QString mGeometryFileType;
00097 
00099   transf Tran;  
00100 
00102   bool mRenderGeometry;
00103 
00105   int numContacts;
00106 
00108   std::list<Contact *> contactList;
00109 
00111   std::list<Contact *> prevContactList;
00112 
00114   std::list<Contact *> virtualContactList;
00115 
00117   bool showFC;
00118 
00120   bool showVC;
00121 
00123   SoSeparator *IVGeomRoot;
00124 
00126   SoSeparator *IVBVRoot;
00127 
00128 #ifdef GEOMETRY_LIB
00129 
00130   SoSeparator *IVPrimitiveRoot;
00131 #endif
00132 
00134   SoTransform *IVTran;  
00135 
00137   SoMaterial *IVMat;
00138 
00140   SoSeparator *IVContactIndicators;
00141 
00143   bool mUsesFlock;
00144 
00146   int mBirdNumber;
00147 
00149   FlockTransf mFlockTran;
00150 
00152   SoSwitch *IVAxes;
00153 
00155   SoSeparator *IVWorstCase;
00156 
00158   SoTranslation *axesTranToCOG;
00159 
00161   SoScale *axesScale;
00162 
00164   Body (const Body &b);
00165 
00167   void createAxesGeometry();
00168 
00170   void initializeIV();
00171 
00173   virtual void getBodyList(std::vector<Body*> *bodies) {bodies->push_back(this);}
00174 
00176 public:
00178   Body(World *w,const char *name=0);
00179 
00181   virtual ~Body();
00182 
00184   virtual int load(const QString &filename);
00185   
00187   int convert2xml(QString filename);
00188   
00190   virtual int loadFromXml(const TiXmlElement *root, QString rootPath);
00191 
00193   int loadGeometryIV(const QString &filename);
00194 
00196   int loadGeometryOFF(const QString &filename);
00197 
00199   int loadGeometryPLY(const QString &filename);
00200 
00202   int loadGeometryMemory(const std::vector<position> &vertices, const std::vector<int> &triangles);
00203 
00205  virtual int saveToXml(QTextStream& xml);
00206 
00208   virtual void cloneFrom(const Body* original);
00209 
00211   void addIVMat(bool clone = false);
00212 
00214   virtual void addToIvc(bool ExpectEmpty = false);
00215 
00217   virtual void cloneToIvc(const Body* original);
00218 
00220   virtual int setTran(transf const& newTr);
00221 
00223   virtual void setDefaultViewingParameters();
00224 
00226   void setRenderGeometry(bool s);
00227 
00229   bool getRenderGeometry() const {return mRenderGeometry;}
00230 
00232   bool usesFlock(){return mUsesFlock;}
00233 
00235   int getBirdNumber(){return mBirdNumber;}
00236 
00238   FlockTransf *getFlockTran(){return &mFlockTran;}
00239 
00241   virtual WorldElement *getOwner() {return (WorldElement *)this;}
00242 
00244   virtual bool isDynamic() const {return false;}
00245 
00247   bool isElastic(){return mIsElastic;}
00248 
00252   int getMaterial() const {return material;}
00254   void setMaterial(int mat);
00255 
00257   SoMaterial *getIVMat() const {return IVMat;}
00258 
00260   double getYoungs() { return youngMod; }
00261 
00263   SoSeparator *getIVGeomRoot() const {return IVGeomRoot;}
00264 
00265 #ifdef GEOMETRY_LIB
00266 
00267   SoSeparator *getIVPrimitiveRoot() const {return IVPrimitiveRoot;}
00268 #endif
00269 
00271   SoTransform *getIVTran() const {return IVTran;}
00272 
00276   SoSeparator *getIVContactIndicators() const {return IVContactIndicators;}
00277 
00281   void setBVGeometry(const std::vector<BoundingBox> &bvs);
00282 
00284   transf const& getTran() const {return Tran;}
00285 
00287   int getNumContacts(Body *b = NULL) const;
00288 
00290   int getNumVirtualContacts() const {return (int)virtualContactList.size();}
00291 
00293   std::list<Contact *>getContacts(Body *b = NULL) const;
00294 
00296   std::list<Contact *>getVirtualContacts() const {return virtualContactList;}
00297 
00301   bool frictionConesShown() const {return showFC;}
00302 
00304   float getTransparency() const;
00306   void setTransparency(float t);
00307 
00309   void showFrictionCones(bool on, int vc=0);
00311   virtual void redrawFrictionCones();
00312 
00314   virtual void addContact(Contact *c);
00316   virtual void addVirtualContact(Contact *c);
00318   virtual void removeContact(Contact *c);
00320   virtual void removePrevContact(Contact *c);
00322   virtual void breakContacts();
00324   virtual void breakVirtualContacts();
00326   virtual int loadContactData(QString fn);
00327 
00329   virtual void resetContactList();
00331   Contact* checkContactInheritance(Contact *c);
00332 
00334   virtual bool contactsPreventMotion(const transf& motion) const;
00335 
00337   friend QTextStream& operator<<(QTextStream &os, const Body &b);
00338 
00340   void getGeometryTriangles(std::vector<Triangle> *triangles) const;
00341 
00343   void getGeometryVertices(std::vector<position> *vertices) const;
00344 };
00345 
00347 
00351 class DynamicBody : public Body {
00352   Q_OBJECT
00353 
00355   bool useDynamics;
00356 
00358   double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
00359 
00361   double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
00362 
00363  protected:
00365   position CoG;
00366 
00368   double maxRadius;
00369 
00371   double mass;
00372 
00374   bool fixed;
00375 
00377   transf fixedTran;
00378 
00380   DynJoint *dynJoint;
00381 
00383   vec3 bbox_max, bbox_min;
00384 
00386   bool showAx;
00387 
00389   bool showDynCF;
00390 
00392   double a[6];
00394   double v[6];
00396   double q[7];
00398   std::list<double*> vStack;
00400   std::list<double*> qStack;
00402   double markedV[6];
00404   double markedQ[7];
00405 
00407   double I[9];
00408   
00410   bool dynamicsComputedFlag;
00411 
00413   double extWrenchAcc[6];
00414 
00416   void compProjectionIntegrals(FACE &f,int A,int B);
00417 
00419   void compFaceIntegrals(FACE &f,int A,int B,int C);
00420 
00422   int computeDefaultInertiaMatrix(std::vector<Triangle> &triangles, double *defaultI);
00423 
00425   void init();
00426 
00427 public:
00429   DynamicBody(World *w, const char *name=0);
00430 
00432   DynamicBody(const Body &b, double m);
00433 
00434   virtual ~DynamicBody();
00435 
00437   void resetDynamics();
00438 
00440   void cloneFrom(const DynamicBody *newBody);
00441 
00443   virtual int loadFromXml(const TiXmlElement *root, QString rootPath);
00444 
00446   virtual int saveToXml(QTextStream& xml);
00447 
00449   virtual int setTran(transf const& newTr);
00450 
00452   void setCoG(const position& newCoG);
00453   
00455   void setInertiaMatrix(const double *newI);
00456 
00458   void setDefaultDynamicParameters();
00459 
00461   void setMaxRadius(double maxRad);
00462 
00464   void computeDefaultMassProp(position &cog, double *I);
00465 
00467   double computeDefaultMaxRadius();
00468 
00470   virtual void setDefaultViewingParameters();
00471 
00473   position const& getCoG() const {return CoG;}
00474 
00476   double getMaxRadius() const {return maxRadius;}
00477 
00481   double getMass() const {return mass;}
00482 
00484   const double *getInertia() const {return I;} 
00485 
00487   const double *getVelocity() {return v;}
00488 
00492   const double *getAccel() {return a;}
00493 
00497   const double *getPos() {return q;} 
00498 
00501   SoSeparator *getIVWorstCase() const {return IVWorstCase;}
00502 
00504   bool isFixed() const {return fixed;}
00505 
00509   bool isDynamic() const {return useDynamics;}
00510 
00512   const transf& getFixedTran() const {return fixedTran;}
00513 
00517   virtual DynJoint *getDynJoint() {return dynJoint;}
00518 
00520   bool axesShown() const {return showAx;}
00521 
00523   bool dynContactForcesShown() const {return showDynCF;}
00524 
00530   void setUseDynamics(bool dyn) {useDynamics = dyn;}
00531 
00537   bool dynamicsComputed() const {return dynamicsComputedFlag;}  
00538 
00542   void resetDynamicsFlag() {dynamicsComputedFlag = false;}
00543   
00547   void setDynamicsFlag() {dynamicsComputedFlag = true;}
00548 
00550   void showAxes(bool on);
00551 
00553   void showDynContactForces(bool on);
00554 
00556   bool setPos(const double *new_q);
00557 
00559   void setVelocity(double *new_v)  {memcpy(v,new_v,6*sizeof(double));}
00560 
00562   void setAccel(double *new_a)  {memcpy(a,new_a,6*sizeof(double));}
00563 
00565   void setBounds(vec3 minBounds,vec3 maxBounds)
00566     {bbox_min = minBounds; bbox_max = maxBounds;}
00567   
00571   void setMass(double m) {mass = m;}
00572 
00574   double *getExtWrenchAcc() {return extWrenchAcc;}
00575 
00577   void pushState();
00579   bool popState();
00581   void clearState();
00583   void markState();
00585   void returnToMarkedState();
00586 
00588   void resetExtWrenchAcc();
00590   void addExtWrench(double *extW);
00592   void addForce(vec3 force);
00594   void addTorque(vec3 torque);
00596   void addRelTorque(vec3 torque);
00598   void addForceAtPos(vec3 force,position pos);
00600   void addForceAtRelPos(vec3 force,position pos);
00601   
00603   void fix();
00605   void unfix();
00607   virtual void setDynJoint(DynJoint *dj);
00608 
00612   static double defaultMass;
00613 };
00614 
00616 
00619 class Link : public DynamicBody {
00620   Q_OBJECT
00621 
00622   friend class Robot;
00623   friend class KinematicChain;
00624 
00625  protected:
00626 
00628   Robot *owner;
00629 
00631   int chainNum,linkNum;
00632 
00633  public:
00634   Link(Robot *r,int c, int l,World *w,const char *name=0);
00635   virtual ~Link();
00636 
00638   virtual WorldElement *getOwner() {return (WorldElement *)owner;}
00639 
00641   int getChainNum() const {return chainNum;}
00642 
00644   int getLinkNum() const {return linkNum;}
00645   
00647   bool externalContactsPreventMotion(const transf& motion);
00648 
00650   virtual void setContactsChanged();
00651 
00653   position getDistalJointLocation();
00654 
00656   position getProximalJointLocation();
00657 
00659   vec3 getProximalJointAxis();
00660 };
00661 
00662 
00663 
00664 #ifdef CGDB_ENABLED
00665 class GraspitDBModel;
00666 #endif
00667 
00669 
00672 class GraspableBody : public DynamicBody {
00673   Q_OBJECT
00674 
00676   SoSeparator *IVGeomPrimitives;
00677 
00679 #ifdef CGDB_ENABLED
00680   GraspitDBModel* mGraspitDBModel;
00681 #endif
00682 
00683  public:
00684   GraspableBody(World *w, const char *name=0);
00685   virtual ~GraspableBody();
00686 
00687   virtual void setDefaultViewingParameters();
00688 
00689   virtual void cloneFrom(const GraspableBody *newBody);
00690 
00694   SoSeparator *getIVGeomPrimitives() const {return IVGeomPrimitives;}
00695 
00696   friend QTextStream& operator<<(QTextStream &os, const GraspableBody &gb);
00697 
00698 #ifdef CGDB_ENABLED
00699 
00700   GraspitDBModel *getDBModel(){
00701     return mGraspitDBModel;
00702   }
00704   void setDBModel(GraspitDBModel* model) {mGraspitDBModel = model;}
00705 #endif
00706 
00707 };
00708 
00709 
00710 #define BODY_HXX
00711 #endif
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/body.h
