MBSim

Project Page
rigid_body.h
00001 /* Copyright (C) 2004-2010 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  */
00019 
00020 #ifndef _RIGID_BODY_H_
00021 #define _RIGID_BODY_H_
00022 
00023 #include "mbsim/body.h"
00024 #include "fmatvec/fmatvec.h"
00025 #include "mbsim/kinematics.h"
00026 #include "mbsim/utils/function.h"
00027 
00028 #ifdef HAVE_OPENMBVCPPINTERFACE
00029 namespace OpenMBV {
00030   class RigidBody;
00031   class Arrow;
00032 }
00033 #endif
00034 
00035 namespace MBSim {
00036 
00037   class Frame;
00038   class Contour;
00039   class FixedRelativeFrame;
00040   class CompoundContour;
00041   class Constraint;
00042 
00057   class RigidBody : public Body {
00058     public:
00063       RigidBody(const std::string &name="");
00064 
00068       virtual ~RigidBody();
00069 
00070       void addDependency(Constraint* constraint_) {
00071         //body.push_back(body_); 
00072         constraint = constraint_;
00073       }
00074 
00075       virtual void updateT(double t) { if(fT) TRel = (*fT)(qRel,t); }
00076       virtual void updateh(double t, int j=0);
00077       virtual void updateh0Fromh1(double t);
00078       virtual void updateW0FromW1(double t);
00079       virtual void updateV0FromV1(double t);
00080       virtual void updatehInverseKinetics(double t, int j=0);
00081       virtual void updateStateDerivativeDependentVariables(double t);
00082       virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
00083       virtual void updateStateDependentVariables(double t) { 
00084         updateKinematicsForSelectedFrame(t); 
00085         updateKinematicsForRemainingFramesAndContours(t); 
00086       }
00087       virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
00088       void updateJacobians0(double t) { 
00089         updateJacobiansForSelectedFrame0(t); 
00090         updateJacobiansForRemainingFramesAndContours(t,0); 
00091       }
00092       void updateJacobians1(double t) { 
00093         updateJacobiansForRemainingFramesAndContours1(t); 
00094       }
00095       virtual void calcqSize();
00096       virtual void calcuSize(int j=0);
00097 
00098       /* INHERITED INTERFACE OF OBJECT */
00099       virtual void updateqRef(const fmatvec::Vec& ref);
00100       virtual void updateuRef(const fmatvec::Vec& ref);
00101       virtual void updateTRef(const fmatvec::Mat &ref);
00102       virtual void init(InitStage stage);
00103       virtual void initz();
00104       virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
00105       virtual void setUpInverseKinetics();
00106       /*****************************************************/
00107 
00108       /* INHERITED INTERFACE OF ELEMENT */
00109       virtual std::string getType() const { return "RigidBody"; }
00110       virtual void plot(double t, double dt=1);
00111       /*****************************************************/
00112 
00113       /* INTERFACE FOR DERIVED CLASSES */
00117       virtual void updateKinematicsForSelectedFrame(double t);
00121       virtual void updateJacobiansForSelectedFrame0(double t); 
00122 
00126       virtual void updateKinematicsForRemainingFramesAndContours(double t);
00127 
00131       virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
00132       virtual void updateJacobiansForRemainingFramesAndContours1(double t);
00133 
00134       /* GETTER / SETTER */
00139       void setTranslation(Translation* fPrPK_) { fPrPK = fPrPK_; }
00144       void setRotation(Rotation* fAPK_)        { fAPK  = fAPK_;  }
00149       Translation* getTranslation()            { return fPrPK;   }
00154       Rotation*    getRotation()               { return fAPK;    }
00155       void setJacobianOfTranslation(Jacobian* fPJT_) { fPJT = fPJT_; }
00156       void setJacobianOfRotation(Jacobian* fPJR_)    { fPJR = fPJR_; }
00157       void setDerivativeOfJacobianOfTranslation(DerivativeOfJacobian* fPdJT_) { fPdJT = fPdJT_;}
00158       void setDerivativeOfJacobianOfRotation(DerivativeOfJacobian* fPdJR_) { fPdJR = fPdJR_;}
00159 
00161       void setGuidingVelocityOfTranslation(GuidingVelocity* fPjT_) { fPjT = fPjT_;}
00162 
00164       void setGuidingVelocityOfRotation(GuidingVelocity* fPjR_) { fPjR = fPjR_;}
00165 
00167       void setDerivativeOfGuidingVelocityOfTranslation(DerivativeOfGuidingVelocity* fPdjT_) { fPdjT = fPdjT_;}
00168 
00170       void setDerivativeOfGuidingVelocityOfRotation(DerivativeOfGuidingVelocity* fPdjR_) { fPdjR = fPdjR_;}
00171 
00172       void setMass(double m_) { m = m_; }
00173       double getMass() const { return m; }
00174       FixedRelativeFrame* getFrameForKinematics() { return K; };
00175       FixedRelativeFrame* getFrameC() { return C; };
00176       void isFrameOfBodyForRotation(bool cb_) { cb = cb_; }
00177 
00182       void setInertiaTensor(const fmatvec::SymMat3& RThetaR, Frame *frame=0) {
00183         SThetaS = RThetaR;
00184         frameForInertiaTensor = frame;
00185       }
00186 
00187       const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
00188       fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
00189 
00190       void addFrame(FixedRelativeFrame *frame); 
00191 
00192       using Body::addContour;
00193 
00194 //      /**
00195 //       * \param frame        specific Frame to add
00196 //       * \param RrRF         constant relative vector from reference Frame to specific Frame in reference system
00197 //       * \param ARF          constant relative rotation from specific Frame to reference Frame
00198 //       * \param refFrameName reference Frame name
00199 //       */
00200 //      void addFrame(Frame *frame, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const std::string& refFrameName); 
00201 
00208       void addFrame(Frame *frame, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00209 
00216       void addFrame(const std::string &str, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00217 
00218 //      /**
00219 //       * \param contour      specific contour to add
00220 //       * \param RrRC         constant relative vector from reference Frame to specific contour in reference system
00221 //       * \param ARC          constant relative rotation from specific contour to reference Frame
00222 //       * \param refFrameName reference Frame name
00223 //       */
00224 //      void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const fmatvec::SqrMat3 &ARC, const std::string& refFrameName);
00225 
00232       void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const fmatvec::SqrMat3 &ARC, const Frame* refFrame=0);
00233 
00237       void setFrameForKinematics(Frame *frame);
00238 
00239 #ifdef HAVE_OPENMBVCPPINTERFACE
00240       void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
00241       void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
00242       const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
00243 
00245       void setOpenMBVWeightArrow(OpenMBV::Arrow *arrow) { FWeight = arrow; }
00246 
00248       void setOpenMBVJointForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00249 
00251       void setOpenMBVJointMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00252 #endif
00253 
00254       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00255       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00256 
00257       virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
00258       virtual void updateAccelerations(double t, Frame *P);
00259       virtual void updateRelativeJacobians(double t, Frame *P);
00260       virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
00261       const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
00262       const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
00263       fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
00264       fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
00265       fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
00266       fmatvec::Vec& getjRel() {return jRel;}
00267       fmatvec::Vec& getqRel() {return qRel;}
00268       fmatvec::Vec& getuRel() {return uRel;}
00269       // void setqRel(const fmatvec::Vec &q) {qRel0 = q;}
00270       // void setuRel(const fmatvec::Vec &u) {uRel0 = u;}
00271       fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
00272       fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
00273 
00274       int getqRelSize() const {return nq;}
00275       int getuRelSize(int i=0) const {return nu[i];}
00276 
00277     protected:
00281       double m;
00282 
00286       fmatvec::SymMat3 SThetaS, WThetaS;
00287 
00288       FixedRelativeFrame *K;
00289 
00293       fmatvec::SymMat Mbuf;
00294 
00298       bool cb;
00299 
00303       fmatvec::Mat3xV PJT[2], PJR[2], PdJT, PdJR;
00304 
00308       fmatvec::Vec3 PjT, PjR, PdjT, PdjR;
00309 
00313       fmatvec::SqrMat3 APK;
00314 
00318       fmatvec::Vec3 PrPK, WrPK;
00319 
00323       fmatvec::Vec3 WvPKrel, WomPK;
00324 
00328       TMatrix *fT;
00329 
00333       Translation *fPrPK;
00334 
00338       Rotation *fAPK;
00339 
00343       Jacobian *fPJT;
00344 
00348       Jacobian *fPJR;
00349 
00353       DerivativeOfJacobian *fPdJT;
00354 
00358       DerivativeOfJacobian *fPdJR;
00359 
00363       GuidingVelocity *fPjT;
00364 
00368       GuidingVelocity *fPjR;
00369 
00373       DerivativeOfGuidingVelocity *fPdjT;
00374 
00378       DerivativeOfGuidingVelocity *fPdjR;
00379 
00383       void (RigidBody::*updateM_)(double t, int i);
00384 
00388       void updateMConst(double t, int i=0);
00389 
00393       void updateMNotConst(double t, int i=0); 
00394 
00398       void (RigidBody::*facLLM_)(int i);
00399 
00403       void facLLMConst(int i=0) {};
00404 
00408       void facLLMNotConst(int i=0) { Object::facLLM(i); }
00409 
00410       void (RigidBody::*updateJacobians_[2])(double t); 
00411 
00413       FixedRelativeFrame *C;
00414 
00415       fmatvec::Vec aT, aR;
00416 
00417       fmatvec::Vec qRel, uRel;
00418       fmatvec::Mat JRel[2];
00419       fmatvec::Vec jRel;
00420 
00421       fmatvec::Mat3xV WJTrel,WJRrel;
00422       fmatvec::Vec3 WjTrel,WjRrel;
00423 
00424       fmatvec::Mat TRel;
00425 
00426       Constraint *constraint;
00427 
00428       int nu[2], nq;
00429 
00430       Frame *frameForJacobianOfRotation;
00431 
00432       std::vector<FixedRelativeFrame*> RBF;
00433       std::vector<CompoundContour*> RBC;
00434 
00435       Frame *frameForInertiaTensor;
00436 
00437     private:
00438 #ifdef HAVE_OPENMBVCPPINTERFACE
00439 
00442       Frame * openMBVFrame;
00443       OpenMBV::Arrow *FWeight, *FArrow, *MArrow;
00444 #endif
00445   };
00446 
00447 }
00448 
00449 #endif
00450