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: dof.h,v 1.16 2009/07/27 17:00:27 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #ifndef _dof_h_
00027 #define _dof_h_
00028 
00029 #include <vector>
00030 #include <list>
00031 #include <map>
00032 
00033 #include "mytools.h"
00034 
00035 class Robot;
00036 class Joint;
00037 class Body;
00038 class Link;
00039 
00040 class QTextStream;
00041 
00043 
00071 class DOF {
00072   friend class Robot;
00073 
00074 public:
00075   enum Type{RIGID, BREAKAWAY, COMPLIANT};
00076 
00077 protected:
00079   Robot *owner;
00080 
00082   int dofNum;
00083 
00085   double q;
00086   
00088   double maxq;
00089 
00091   double minq;
00092 
00094   double desiredq;
00095 
00097   double setPoint;
00098 
00099   double desiredVelocity;
00100   double defaultVelocity;
00101   double actualVelocity;
00102   double maxAccel;
00103 
00105   double force;
00106 
00108   double maxForce;
00109 
00111   double extForce;
00112 
00114   double Kv;
00115 
00117   double Kp;
00118 
00120   std::list<double> mErrorHistory;
00121 
00123   std::list<double> mPositionHistory;
00124 
00126   std::list<double> mVelocityHistory;
00127 
00129   std::list<double> mForceHistory;
00130 
00132   int mHistoryMaxSize;
00133 
00135   double mDynStartTime;
00136 
00138   std::vector<double> trajectory;
00139 
00141   int currTrajPt;
00142 
00144   std::list<Joint *> jointList;
00145 
00147   double draggerScale;
00148 
00149 public:
00150 
00152   DOF();
00154   DOF(const DOF *original);
00156   virtual ~DOF() {}
00158   virtual void initDOF(Robot *myRobot,const std::list<Joint *>& jList);
00160   void updateMinMax();
00161 
00163   virtual Type getType() const = 0;
00165   virtual void reset() = 0;
00166 
00167   //------------------- STATICS ----------------------
00174   virtual bool accumulateMove(double q1, double *jointVals, int *stoppedJoints) = 0;
00178   virtual void updateFromJointValues(const double *jointVals = NULL) = 0;
00181   virtual void updateVal(double q1) = 0;
00184   virtual void getJointValues(double *jointVals) const = 0;
00189   virtual bool computeStaticJointTorques(double *jointTorques, double dofForce = -1) = 0;
00191   virtual double getStaticRatio(Joint *j) const = 0;
00192 
00193   //------------------- DYNAMICS ---------------------
00195   virtual int getNumLimitConstraints() = 0;
00197   virtual void buildDynamicLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
00198                                             double* H, double *g, int &hcn) = 0;
00200   virtual int getNumCouplingConstraints() = 0;
00202   virtual void buildDynamicCouplingConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
00203                                                double* Nu, double *eps, int &ncn) = 0;
00205   virtual void setForce(double f) = 0;
00206 
00208   virtual void callController(double timeStep);
00209 
00211   virtual double PDPositionController(double timeStep);
00212 
00214   virtual bool dynamicsProgress();
00215 
00217   virtual bool writeToStream(QTextStream &stream);
00219   virtual bool readFromStream(QTextStream &stream);
00221         virtual bool readParametersFromXml(const TiXmlElement* root);
00223   double getVal() const {return q;}
00225 
00226   virtual double getSaveVal() const {return q;}
00228   int getDOFNum() const {return dofNum;}
00230   double getMax() const { return maxq;}
00232   double getMin() const { return minq;}
00233 
00235   double getDesiredVelocity() const {return desiredVelocity;}
00237   double getDefaultVelocity() const {return defaultVelocity;}
00239   double getActualVelocity() const {return actualVelocity;}
00241   double getMaxAccel() const {return maxAccel;}
00243   double getForce() const {return force;}
00245   double getDesiredForce() const {return force;}
00247   double getMaxForce() const {return maxForce;}
00249   double getExtForce() const {return extForce;}
00251   float getDraggerScale() const {return draggerScale;}
00252 
00254   double getSetPoint() const {return setPoint;}
00255   double getDesiredPos() const {return desiredq;}
00256   void updateSetPoint();
00257   void setDesiredPos(double p) {desiredq = MIN(maxq,MAX(minq,p));}
00258   void setDesiredVelocity(double v) {desiredVelocity = v;}
00259   void setDefaultVelocity(double v) {defaultVelocity = v;}
00260   void setTrajectory(double *traj,int numPts);
00261   void addToTrajectory(double *traj,int numPts);
00262 };
00263 
00267 class RigidDOF : public DOF 
00268 {
00269 private:
00271         double getClosestJointLimit(int *direction);
00272 public:
00273         RigidDOF() : DOF() {}
00274         RigidDOF(RigidDOF *original) : DOF(original) {}
00275         Type getType() const {return RIGID;}
00276 
00277     //------------------- STATICS ---------------------
00279         virtual bool accumulateMove(double q1, double *jointVals, int *stoppedJoints);
00281         virtual void updateFromJointValues(const double *jointVals = NULL);
00283         virtual void updateVal(double q1){q = q1;}
00285         virtual void reset(){}
00287         virtual double getStaticRatio(Joint *j) const;
00289         virtual void getJointValues(double *jointVals) const;
00291 
00294         virtual bool computeStaticJointTorques(double*,double){return true;}
00295 
00296     //------------------- DYNAMICS ---------------------
00298         virtual int getNumLimitConstraints();
00300         virtual void buildDynamicLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
00301                                                   double* H, double *g, int &hcn);
00303         virtual int getNumCouplingConstraints(){return jointList.size() - 1;}
00305         virtual void buildDynamicCouplingConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
00306                                                      double* Nu, double *eps, int &ncn);
00308         virtual void setForce(double f);
00309 };
00310 
00328 class BreakAwayDOF : public RigidDOF
00329 {
00330 private:
00332         int *mInBreakAway;
00334         double *mBreakAwayValues;
00336         double mBreakAwayTorque;
00337 
00338 public:
00339         Type getType()const {return BREAKAWAY;}
00340         BreakAwayDOF() : RigidDOF(), mInBreakAway(NULL), mBreakAwayValues(NULL),mBreakAwayTorque(0.0) {}
00341         BreakAwayDOF(BreakAwayDOF *original) : RigidDOF(original), mInBreakAway(NULL), 
00342                                                mBreakAwayValues(NULL), 
00343                                                mBreakAwayTorque(original->mBreakAwayTorque) {}
00344         ~BreakAwayDOF();
00345 
00347         void initDOF(Robot *myRobot,const std::list<Joint *>& jList);
00349         virtual bool accumulateMove(double q1, double *jointVals, int *stoppedJoints);
00351         virtual void updateFromJointValues(const double *jointVals = NULL);
00353         virtual void updateVal(double q1);
00355         virtual void getJointValues(double *jointVals) const;
00357         virtual void reset();
00359         virtual bool computeStaticJointTorques(double *jointTorques, double dofForce = -1);
00361         virtual double getSaveVal() const;
00362 
00364         virtual bool writeToStream(QTextStream &stream);
00366         virtual bool readFromStream(QTextStream &stream);
00368         virtual bool readParametersFromXml(const TiXmlElement* root);
00369 };
00370 
00377 class CompliantDOF : public DOF
00378 {
00379 private:
00381         double smoothProfileController(double timeStep);
00382 public:
00383         CompliantDOF() : DOF() {}
00384         CompliantDOF(CompliantDOF *original) : DOF(original) {}
00385         Type getType() const {return COMPLIANT;}
00386 
00387     //------------------- STATICS ---------------------
00389         virtual void initDOF(Robot *myRobot,const std::list<Joint *>& jList);
00391         virtual double getStaticRatio(Joint *j) const;
00393         virtual void getJointValues(double* jointVals) const;
00395         virtual void updateVal(double q1){q = q1;}
00397         virtual void updateFromJointValues(const double* jointVals= NULL);
00399         virtual bool accumulateMove(double q1, double *jointVals, int *stoppedJoints);
00401         virtual bool computeStaticJointTorques(double* jointTorques, double dofForce = -1);
00403         virtual void reset(){}
00404 
00405         //------------------- DYNAMICS ---------------------
00407         virtual int getNumCouplingConstraints(){return 0;}
00409         virtual void buildDynamicCouplingConstraints(std::map<Body*,int>&, int, 
00410                 double*, double*, int&){}
00412         virtual int getNumLimitConstraints();
00414         virtual void buildDynamicLimitConstraints(std::map<Body*,int> &islandIndices, int numBodies, 
00415                                                   double* H, double *g, int &hcn);
00417         virtual void setForce(double f);
00419         virtual double PDPositionController(double timeStep);
00421         virtual bool dynamicsProgress();
00422 };
00423 
00424 #endif
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/dof.h
