graspit: body.h Source 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):  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   SoTransform* IVScaleTran;
00127   
00129   SoTransform *IVOffsetTran;
00130 
00132   SoSeparator *IVBVRoot;
00133 
00134 #ifdef GEOMETRY_LIB
00135 
00136   SoSeparator *IVPrimitiveRoot;
00137 #endif
00138 
00140   SoTransform *IVTran;  
00141 
00143   SoMaterial *IVMat;
00144 
00146   SoSeparator *IVContactIndicators;
00147 
00149   bool mUsesFlock;
00150 
00152   int mBirdNumber;
00153 
00155   FlockTransf mFlockTran;
00156 
00158   SoSwitch *IVAxes;
00159 
00161   SoSeparator *IVWorstCase;
00162 
00164   SoTranslation *axesTranToCOG;
00165 
00167   SoScale *axesScale;
00168 
00170   Body (const Body &b);
00171 
00173   void createAxesGeometry();
00174 
00176   void initializeIV();
00177 
00179   virtual void getBodyList(std::vector<Body*> *bodies) {bodies->push_back(this);}
00180 
00182 public:
00184   Body(World *w,const char *name=0);
00185 
00187   virtual ~Body();
00188 
00190   virtual int load(const QString &filename);
00191   
00193   int convert2xml(QString filename);
00194   
00196   virtual int loadFromXml(const TiXmlElement *root, QString rootPath);
00197 
00199   int loadGeometryIV(const QString &filename);
00200 
00202   int loadGeometryOFF(const QString &filename);
00203 
00205   int loadGeometryPLY(const QString &filename);
00206 
00208   int loadGeometryMemory(const std::vector<position> &vertices, const std::vector<int> &triangles);
00209 
00211  virtual int saveToXml(QTextStream& xml);
00212 
00214   virtual void cloneFrom(const Body* original);
00215 
00217   void addIVMat(bool clone = false);
00218 
00220   virtual void addToIvc(bool ExpectEmpty = false);
00221 
00223   virtual void cloneToIvc(const Body* original);
00224 
00226   virtual void setGeometryScaling(double x, double y, double z);
00227 
00229   virtual void setGeometryOffset(transf tr);
00230 
00232   virtual int setTran(transf const& newTr);
00233 
00235   virtual void setDefaultViewingParameters();
00236 
00238   void setRenderGeometry(bool s);
00239 
00241   bool getRenderGeometry() const {return mRenderGeometry;}
00242 
00244   bool usesFlock(){return mUsesFlock;}
00245 
00247   int getBirdNumber(){return mBirdNumber;}
00248 
00250   FlockTransf *getFlockTran(){return &mFlockTran;}
00251 
00253   virtual WorldElement *getOwner() {return (WorldElement *)this;}
00254 
00256   virtual bool isDynamic() const {return false;}
00257 
00259   bool isElastic(){return mIsElastic;}
00260 
00264   int getMaterial() const {return material;}
00266   void setMaterial(int mat);
00267 
00269   SoMaterial *getIVMat() const {return IVMat;}
00270 
00272   double getYoungs() { return youngMod; }
00273 
00275   SoSeparator *getIVGeomRoot() const {return IVGeomRoot;}
00276 
00278   SoTransform* getIVScaleTran() {return IVScaleTran;}
00279 
00280 #ifdef GEOMETRY_LIB
00281 
00282   SoSeparator *getIVPrimitiveRoot() const {return IVPrimitiveRoot;}
00283 #endif
00284 
00286   SoTransform *getIVTran() const {return IVTran;}
00287 
00291   SoSeparator *getIVContactIndicators() const {return IVContactIndicators;}
00292 
00296   void setBVGeometry(const std::vector<BoundingBox> &bvs);
00297 
00299   transf const& getTran() const {return Tran;}
00300 
00302   int getNumContacts(Body *b = NULL) const;
00303 
00305   int getNumVirtualContacts() const {return (int)virtualContactList.size();}
00306 
00308   std::list<Contact *>getContacts(Body *b = NULL) const;
00309 
00311   std::list<Contact *>getVirtualContacts() const {return virtualContactList;}
00312 
00316   bool frictionConesShown() const {return showFC;}
00317 
00319   float getTransparency() const;
00321   void setTransparency(float t);
00322 
00324   void showFrictionCones(bool on, int vc=0);
00326   virtual void redrawFrictionCones();
00327 
00329   virtual void addContact(Contact *c);
00331   virtual void addVirtualContact(Contact *c);
00333   virtual void removeContact(Contact *c);
00335   virtual void removePrevContact(Contact *c);
00337   virtual void breakContacts();
00339   virtual void breakVirtualContacts();
00341   virtual int loadContactData(QString fn);
00342 
00344   virtual void resetContactList();
00346   Contact* checkContactInheritance(Contact *c);
00347 
00349   virtual bool contactsPreventMotion(const transf& motion) const;
00350 
00352   friend QTextStream& operator<<(QTextStream &os, const Body &b);
00353 
00355   void getGeometryTriangles(std::vector<Triangle> *triangles) const;
00356 
00358   void getGeometryVertices(std::vector<position> *vertices) const;
00359 };
00360 
00362 
00366 class DynamicBody : public Body {
00367   Q_OBJECT
00368 
00370   bool useDynamics;
00371 
00373   double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
00374 
00376   double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
00377 
00378  protected:
00380   position CoG;
00381 
00383   double maxRadius;
00384 
00386   double mass;
00387 
00389   bool fixed;
00390 
00392   transf fixedTran;
00393 
00395   DynJoint *dynJoint;
00396 
00398   vec3 bbox_max, bbox_min;
00399 
00401   bool showAx;
00402 
00404   bool showDynCF;
00405 
00407   double a[6];
00409   double v[6];
00411   double q[7];
00413   std::list<double*> vStack;
00415   std::list<double*> qStack;
00417   double markedV[6];
00419   double markedQ[7];
00420 
00422   double I[9];
00423   
00425   bool dynamicsComputedFlag;
00426 
00428   double extWrenchAcc[6];
00429 
00431   void compProjectionIntegrals(FACE &f,int A,int B);
00432 
00434   void compFaceIntegrals(FACE &f,int A,int B,int C);
00435 
00437   int computeDefaultInertiaMatrix(std::vector<Triangle> &triangles, double *defaultI);
00438 
00440   void init();
00441 
00442 public:
00444   DynamicBody(World *w, const char *name=0);
00445 
00447   DynamicBody(const Body &b, double m);
00448 
00449   virtual ~DynamicBody();
00450 
00452   void resetDynamics();
00453 
00455   void cloneFrom(const DynamicBody *newBody);
00456 
00458   virtual int loadFromXml(const TiXmlElement *root, QString rootPath);
00459 
00461   virtual int saveToXml(QTextStream& xml);
00462 
00464   virtual int setTran(transf const& newTr);
00465 
00467   void setCoG(const position& newCoG);
00468   
00470   void setInertiaMatrix(const double *newI);
00471 
00473   void setDefaultDynamicParameters();
00474 
00476   void setMaxRadius(double maxRad);
00477 
00479   void computeDefaultMassProp(position &cog, double *I);
00480 
00482   double computeDefaultMaxRadius();
00483 
00485   virtual void setDefaultViewingParameters();
00486 
00488   position const& getCoG() const {return CoG;}
00489 
00491   double getMaxRadius() const {return maxRadius;}
00492 
00496   double getMass() const {return mass;}
00497 
00499   const double *getInertia() const {return I;} 
00500 
00502   const double *getVelocity() {return v;}
00503 
00507   const double *getAccel() {return a;}
00508 
00512   const double *getPos() {return q;} 
00513 
00516   SoSeparator *getIVWorstCase() const {return IVWorstCase;}
00517 
00519   bool isFixed() const {return fixed;}
00520 
00524   bool isDynamic() const {return useDynamics;}
00525 
00527   const transf& getFixedTran() const {return fixedTran;}
00528 
00532   virtual DynJoint *getDynJoint() {return dynJoint;}
00533 
00535   bool axesShown() const {return showAx;}
00536 
00538   bool dynContactForcesShown() const {return showDynCF;}
00539 
00545   void setUseDynamics(bool dyn) {useDynamics = dyn;}
00546 
00552   bool dynamicsComputed() const {return dynamicsComputedFlag;}  
00553 
00557   void resetDynamicsFlag() {dynamicsComputedFlag = false;}
00558   
00562   void setDynamicsFlag() {dynamicsComputedFlag = true;}
00563 
00565   void showAxes(bool on);
00566 
00568   void showDynContactForces(bool on);
00569 
00571   bool setPos(const double *new_q);
00572 
00574   void setVelocity(double *new_v)  {memcpy(v,new_v,6*sizeof(double));}
00575 
00577   void setAccel(double *new_a)  {memcpy(a,new_a,6*sizeof(double));}
00578 
00580   void setBounds(vec3 minBounds,vec3 maxBounds)
00581     {bbox_min = minBounds; bbox_max = maxBounds;}
00582   
00586   void setMass(double m) {mass = m;}
00587 
00589   double *getExtWrenchAcc() {return extWrenchAcc;}
00590 
00592   void pushState();
00594   bool popState();
00596   void clearState();
00598   void markState();
00600   void returnToMarkedState();
00601 
00603   void resetExtWrenchAcc();
00605   void addExtWrench(double *extW);
00607   void addForce(vec3 force);
00609   void addTorque(vec3 torque);
00611   void addRelTorque(vec3 torque);
00613   void addForceAtPos(vec3 force,position pos);
00615   void addForceAtRelPos(vec3 force,position pos);
00616   
00618   void fix();
00620   void unfix();
00622   virtual void setDynJoint(DynJoint *dj);
00623 
00627   static double defaultMass;
00628 };
00629 
00631 
00634 class Link : public DynamicBody {
00635   Q_OBJECT
00636 
00637   friend class Robot;
00638   friend class KinematicChain;
00639 
00640  protected:
00641 
00643   Robot *owner;
00644 
00646   int chainNum,linkNum;
00647 
00648  public:
00649   Link(Robot *r,int c, int l,World *w,const char *name=0);
00650   virtual ~Link();
00651 
00653   virtual WorldElement *getOwner() {return (WorldElement *)owner;}
00654 
00656   int getChainNum() const {return chainNum;}
00657 
00659   int getLinkNum() const {return linkNum;}
00660   
00662   bool externalContactsPreventMotion(const transf& motion);
00663 
00665   virtual void setContactsChanged();
00666 
00668   position getDistalJointLocation();
00669 
00671   position getProximalJointLocation();
00672 
00674   vec3 getProximalJointAxis();
00675 };
00676 
00677 
00678 
00679 #ifdef CGDB_ENABLED
00680 class GraspitDBModel;
00681 #endif
00682 
00684 
00687 class GraspableBody : public DynamicBody {
00688   Q_OBJECT
00689 
00691   SoSeparator *IVGeomPrimitives;
00692 
00694 #ifdef CGDB_ENABLED
00695   GraspitDBModel* mGraspitDBModel;
00696 #endif
00697 
00698  public:
00699   GraspableBody(World *w, const char *name=0);
00700   virtual ~GraspableBody();
00701 
00702   virtual void setDefaultViewingParameters();
00703 
00704   virtual void cloneFrom(const GraspableBody *newBody);
00705 
00709   SoSeparator *getIVGeomPrimitives() const {return IVGeomPrimitives;}
00710 
00711   friend QTextStream& operator<<(QTextStream &os, const GraspableBody &gb);
00712 
00713 #ifdef CGDB_ENABLED
00714 
00715   GraspitDBModel *getDBModel(){
00716     return mGraspitDBModel;
00717   }
00719   void setDBModel(GraspitDBModel* model) {mGraspitDBModel = model;}
00720 #endif
00721 
00722 };
00723 
00724 
00725 #define BODY_HXX
00726 #endif