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: world.h,v 1.45 2010/08/11 02:45:37 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00029 #ifndef WORLD_HXX
00030 
00031 #include <list>
00032 #include <vector>
00033 #include <utility>
00034 #include <set>
00035 #include <QString>
00036 #include <QObject>
00037 
00038 #include "material.h"
00039 
00040 #include <Inventor/SoType.h>
00041 
00042 class vec3;
00043 class position;
00044 class mat3;
00045 class Quaternion;
00046 class transf;
00047 
00048 class IVmgr;
00049 class Hand;
00050 class Robot;
00051 class HumanHand;
00052 class Body;
00053 class DynamicBody;
00054 class Link;
00055 class GraspableBody;
00056 class WorldElement;
00057 class Tendon;
00058 
00059 typedef std::vector<position> Neighborhood;
00060 
00061 struct ContactDataS;
00062 typedef struct ContactDataS ContactData;
00063 typedef std::vector<ContactData> ContactReport;
00064 
00065 struct CollisionDataS;
00066 typedef struct CollisionDataS CollisionData;
00067 typedef std::vector<CollisionData> CollisionReport;
00068 
00069 class CollisionInterface;
00070 class BoundingBox;
00071 class TiXmlElement; 
00072 class SoGroup;
00073 class SoSeparator;
00074 class SoIdleSensor;
00075 class SoSensor;
00076 
00078 
00087 class World : public QObject {
00088   Q_OBJECT ;
00089 
00090 protected:
00092   IVmgr *myIVmgr;
00093 
00095   double worldTime;
00096 
00098   std::vector<Body *> bodyVec;
00099 
00101   std::vector<GraspableBody *> GBVec;
00102 
00104   std::vector<Robot *> robotVec;        
00105 
00107   std::vector<Hand *> handVec;
00108 
00110   int numBodies;
00111 
00113   int numGB;
00114 
00116   int numRobots;
00117 
00119   int numHands;
00120 
00122   int numSelectedElements;
00123 
00125   int numSelectedBodyElements;
00126 
00128   int numSelectedRobotElements;
00129 
00131   int numSelectedBodies;
00132 
00134   bool modified;
00135 
00137   Hand *currentHand;
00138 
00140   std::list<WorldElement *> selectedElementList;
00141 
00143   std::vector<Body *> selectedBodyVec;
00144   
00146   bool isTendonSelected;
00147 
00149   Tendon* selectedTendon;
00150 
00152   bool allCollisionsOFF;
00153 
00155   bool softContactsON;
00156 
00158   CollisionInterface *mCollisionInterface;
00159 
00161   SoSeparator *IVRoot;
00162 
00164   int numMaterials;
00165   
00167   std::vector<QString> materialNames;
00168 
00170   double **cofTable;
00171 
00173   double **kcofTable;
00174 
00176   bool dynamicsOn;
00177 
00179   SoIdleSensor *idleSensor;
00180 
00182   double dynamicsTimeStep;  
00183 
00185   void readSettings();
00186 
00188   void saveSettings();
00189 
00191   static void dynamicsCB(void *data,SoSensor *sensor);
00192   
00193   friend class Body;
00194   friend class DynamicBody;
00195   friend class MainWindow;
00196 
00197 signals:
00199   void dynamicStepTaken();
00200 
00202   void dynamicsError(const char *errMsg);
00203 
00205   void selectionsChanged();
00206 
00208   void numElementsChanged();
00209 
00211   void handRemoved();
00212 
00214   void graspsUpdated();
00215 
00217   void handSelectionChanged();
00218 
00220   void tendonSelectionChanged();
00221 
00223   void tendonDetailsChanged();
00224 
00225 public: 
00227   World(QObject *parent=0,const char *name=0, IVmgr *mgr=NULL);
00228 
00230   ~World();
00231   
00233   int getNumBodies() const {return numBodies;}
00234 
00236   int getNumGB() const {return numGB;}
00237 
00239   int getNumRobots() const {return numRobots;}
00240 
00242   int getNumHands() const {return numHands;}
00243 
00245   int getNumMaterials() const {return numMaterials;}
00246 
00248   int getMaterialIdx(const QString &matName) const; 
00249 
00251   QString getMaterialName(int i) const {return materialNames[i];}
00252 
00254   int getNumSelectedBodyElements() const {return numSelectedBodyElements;}
00255   
00257   int getNumSelectedRobotElements() const {return numSelectedRobotElements;}
00258 
00260   int getNumSelectedElements() const {return numSelectedElements;}
00261 
00263   int getNumSelectedBodies() const {return numSelectedBodies;}
00264 
00266   Body *getSelectedBody(int i) const {return selectedBodyVec[i];}
00267 
00269   int getCurrentHandNumberTendons();
00270 
00272   Tendon* getSelectedTendon(){return selectedTendon;}
00273 
00275   QString getSelectedHandTendonName(int i);
00276 
00278   bool queryTendonSelected(){return isTendonSelected;}
00279 
00281   void selectTendon(Tendon *t);
00282 
00284   void selectTendon(int i);
00285 
00287   void deselectTendon();
00288 
00290   void tendonChange(){emit tendonDetailsChanged();}
00291 
00293   double getCOF(int mat1,int mat2) {return cofTable[mat1][mat2];}
00294 
00296   double getKCOF(int mat1,int mat2) {return kcofTable[mat1][mat2];}
00297 
00299   double getWorldTime() const {return worldTime;}
00300 
00302   double getTimeStep() const {return dynamicsTimeStep;}
00303   
00305   bool wasModified() const {return modified;}
00306 
00308   SoSeparator *getIVRoot() const {return IVRoot;}
00309 
00311   Body *getBody(int i) const {return bodyVec[i];}
00312 
00314   GraspableBody *getGB(int i) const {return GBVec[i];}
00315 
00317   Hand *getHand(int i) const {return handVec[i];}
00318 
00320   Robot *getRobot(int i) const {return robotVec[i];}
00321 
00323   bool isSelected(WorldElement *e) const;
00324 
00326   const std::list<WorldElement *>& getSelectedElementList() const {return selectedElementList;}
00327 
00329   Hand *getCurrentHand() const { return currentHand;}
00330 
00332   bool dynamicsAreOn() const {return dynamicsOn;}
00333 
00335   bool softContactsAreOn() { return softContactsON; }
00336 
00338   void setDefaults();
00339 
00341   void setModified() {modified = true;}
00342 
00344   int load(const QString &filename);
00345 
00347   int loadFromXml(const TiXmlElement* root,QString rootPath);
00348 
00350   int save(const QString &filename);
00351   
00353   Body *importBody(QString bodyType,QString filename);
00354 
00356   Body *importBodyFromXml(QString bodyType, const TiXmlElement* child, QString rootPath);
00357 
00359   void addBody(Body *body);
00360 
00362   void addLink(Link *newLink);
00363 
00365   void setCurrentHand(Hand *hand) {currentHand = hand; emit handSelectionChanged();}
00366 
00368   Robot *importRobot(QString filename);
00369         
00371   void addRobot(Robot *robot, bool addToScene = true);
00372 
00374   void removeRobot(Robot *robot);
00375 
00377   DynamicBody *makeBodyDynamic(Body *b, double mass);
00378 
00380   void deselectAll();
00381 
00383   void selectElement(WorldElement *e);
00384 
00386   void deselectElement(WorldElement *e);
00387 
00389   void destroyElement(WorldElement *e, bool deleteElement = true);
00390   
00392   void toggleAllCollisions(bool on);
00393 
00395   void toggleCollisions(bool on, WorldElement *e1,WorldElement *e2=NULL);
00396 
00398   bool collisionsAreOff(WorldElement *e1=NULL, WorldElement *e2=NULL);
00399 
00401   bool robotCollisionsAreOff(Robot *r, WorldElement *e);
00402 
00404   bool noCollision(WorldElement *e=NULL);
00405 
00407   int getCollisionReport(CollisionReport *colReport, const std::vector<Body*> *interestList = NULL);
00408 
00410   // double getDist(Body *b1,Body *b2);
00411 
00413   double getDist(WorldElement *e1, WorldElement *e2);
00414 
00416   double getDist(Body *b1,Body *b2, position &p1, position &p2);
00417 
00419   void findContacts(CollisionReport &colReport);
00420 
00422   void findContacts(Body *b);
00423 
00425   void findAllContacts();
00426 
00428   void findVirtualContacts(Hand *hand, Body *object);
00429 
00431   ContactData findVirtualContact(Link *link, Body* object);
00432 
00434   void getBvs(Body *b, int depth, std::vector<BoundingBox> *bvs);
00435 
00437   void updateGrasps();
00438 
00440   void resetDynamicWrenches();
00441 
00443   void turnOnDynamics();
00444 
00446   void turnOffDynamics();
00447 
00449   void resetDynamics();
00450 
00452   void stepDynamics();
00453 
00455   double moveDynamicBodies(double timeStep);
00456 
00458   int computeNewVelocities(double timeStep);
00459 
00461   void pushDynamicState();
00462 
00464   void popDynamicState();
00465 
00467   void FindRegion( const Body *body, position point, vec3 normal, double radius,
00468                                    Neighborhood *neighborhood);
00469 
00471   CollisionInterface *getCollisionInterface() {return mCollisionInterface;}
00472 
00474   vec3 pointDistanceToBody(position p, Body *b, vec3* normal = NULL);
00475 
00477   void addElementToSceneGraph(WorldElement *e);
00479   void removeElementFromSceneGraph(WorldElement *e);
00480 
00482   void emitGraspsUpdated(){emit graspsUpdated();}
00483 };
00484 
00485 #define WORLD_HXX
00486 #endif
00487 
00488
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/world.h
