MBSim

Project Page
rigid_body.h
00001 /* Copyright (C) 2004-2014 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 "functions/auxiliary_functions.h"
00025 
00026 #ifdef HAVE_OPENMBVCPPINTERFACE
00027 #include "mbsim/utils/boost_parameters.h"
00028 #include "mbsim/utils/openmbv_utils.h"
00029 #endif
00030 
00031 namespace MBSim {
00032 
00033   class Frame;
00034   class Contour;
00035   class FixedRelativeFrame;
00036   class CompoundContour;
00037   class Constraint;
00038 
00053   class RigidBody : public Body {
00054     public:
00059       RigidBody(const std::string &name="");
00060 
00064       virtual ~RigidBody();
00065 
00066       void addDependency(Constraint* constraint_) {
00067         //body.push_back(body_); 
00068         constraint = constraint_;
00069       }
00070 
00071       virtual void updatedq(double t, double dt);
00072       virtual void updateqd(double t); 
00073       virtual void updateT(double t);
00074       virtual void updateh(double t, int j=0);
00075       virtual void updateh0Fromh1(double t);
00076       virtual void updateW0FromW1(double t);
00077       virtual void updateV0FromV1(double t);
00078       virtual void updatehInverseKinetics(double t, int j=0);
00079       virtual void updateStateDerivativeDependentVariables(double t);
00080       virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
00081       virtual void updateStateDependentVariables(double t) { 
00082         updateKinematicsForSelectedFrame(t); 
00083         updateKinematicsForRemainingFramesAndContours(t); 
00084       }
00085       virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
00086       void updateJacobians0(double t) { 
00087         updateJacobiansForSelectedFrame0(t); 
00088         updateJacobiansForRemainingFramesAndContours(t,0); 
00089       }
00090       void updateJacobians1(double t) { 
00091         updateJacobiansForRemainingFramesAndContours1(t); 
00092       }
00093       virtual void calcqSize();
00094       virtual void calcuSize(int j=0);
00095 
00096       /* INHERITED INTERFACE OF OBJECT */
00097       virtual void updateqRef(const fmatvec::Vec& ref);
00098       virtual void updateuRef(const fmatvec::Vec& ref);
00099       virtual void init(InitStage stage);
00100       virtual void initz();
00101       virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
00102       virtual void setUpInverseKinetics();
00103       /*****************************************************/
00104 
00105       /* INHERITED INTERFACE OF ELEMENT */
00106       virtual std::string getType() const { return "RigidBody"; }
00107       virtual void plot(double t, double dt=1);
00108       /*****************************************************/
00109 
00110       /* INTERFACE FOR DERIVED CLASSES */
00114       virtual void updateKinematicsForSelectedFrame(double t);
00118       virtual void updateJacobiansForSelectedFrame0(double t); 
00119 
00123       virtual void updateKinematicsForRemainingFramesAndContours(double t);
00124 
00128       virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
00129       virtual void updateJacobiansForRemainingFramesAndContours1(double t);
00130 
00131       /* GETTER / SETTER */
00132 
00133       // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
00138       void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
00139         fPrPK = fPrPK_;
00140         fPrPK->setParent(this);
00141         fPrPK->setName("GeneralTranslation");
00142       }
00147       void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
00148         setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
00149       }
00154       void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
00155         setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
00156       }
00157       void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
00158       void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
00159       void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
00160 
00161       // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
00166       void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
00167         fAPK = fAPK_;
00168         fAPK->setParent(this);
00169         fAPK->setName("GeneralRotation");
00170       }
00175       void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00180       void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00181       void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
00182       void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
00183       void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
00184 
00185       void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
00186       void setCoordinateTransformationForRotation(bool ct) { coordinateTransformation = ct; }
00187 
00192       Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
00197       Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
00198 
00199       void setMass(double m_) { m = m_; }
00200       double getMass() const { return m; }
00201       FixedRelativeFrame* getFrameForKinematics() { return K; };
00202       FixedRelativeFrame* getFrameC() { return C; };
00203 
00208       void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
00209 
00210       const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
00211       fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
00212 
00213       void addFrame(FixedRelativeFrame *frame); 
00214 
00215       using Body::addContour;
00216 
00220       void setFrameForKinematics(Frame *frame);
00221 
00222       void setFrameForInertiaTensor(Frame *frame);
00223 
00224 #ifdef HAVE_OPENMBVCPPINTERFACE
00225       void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
00226       void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
00227       const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
00228 
00230       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVWeight, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00231         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00232         FWeight=ombv.createOpenMBV();
00233       }
00234 
00236       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVJointForce, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00237         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00238         FArrow=ombv.createOpenMBV();
00239       }
00240 
00242       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVJointMoment, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) { 
00243         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00244         MArrow=ombv.createOpenMBV();
00245       }
00246 #endif
00247 
00248       virtual void initializeUsingXML(xercesc::DOMElement *element);
00249       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00250 
00251       virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
00252       virtual void updateAccelerations(double t, Frame *P);
00253       virtual void updateRelativeJacobians(double t, Frame *P);
00254       virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
00255       const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
00256       const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
00257       fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
00258       fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
00259       fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
00260       fmatvec::Vec& getjRel() {return jRel;}
00261       fmatvec::Vec& getqRel() {return qRel;}
00262       fmatvec::Vec& getuRel() {return uRel;}
00263       // void setqRel(const fmatvec::Vec &q) {qRel0 = q;}
00264       // void setuRel(const fmatvec::Vec &u) {uRel0 = u;}
00265       fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
00266       fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
00267 
00268       int getqRelSize() const {return nq;}
00269       int getuRelSize(int i=0) const {return nu[i];}
00270 
00271     protected:
00275       double m;
00276 
00280       fmatvec::SymMat3 SThetaS, WThetaS;
00281 
00282       FixedRelativeFrame *K;
00283 
00287       fmatvec::SymMat Mbuf;
00288 
00292       bool coordinateTransformation;
00293 
00297       fmatvec::Mat3xV PJT[2], PJR[2];
00298 
00299       fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
00300 
00304       fmatvec::SqrMat3 APK;
00305 
00309       fmatvec::Vec3 PrPK, WrPK;
00310 
00314       fmatvec::Vec3 WvPKrel, WomPK;
00315 
00316       Function<fmatvec::MatV(fmatvec::VecV)> *fTR;
00317 
00321       Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK;
00322 
00326       Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK;
00327 
00331       void (RigidBody::*updateM_)(double t, int i);
00332 
00336       void updateMConst(double t, int i=0);
00337 
00341       void updateMNotConst(double t, int i=0); 
00342 
00346       void (RigidBody::*facLLM_)(int i);
00347 
00351       void facLLMConst(int i=0) {};
00352 
00356       void facLLMNotConst(int i=0) { Object::facLLM(i); }
00357 
00358       void (RigidBody::*updateJacobians_[2])(double t); 
00359 
00361       FixedRelativeFrame *C;
00362 
00363       fmatvec::Vec aT, aR;
00364 
00365       fmatvec::Vec qRel, uRel;
00366       fmatvec::Mat JRel[2];
00367       fmatvec::Vec jRel;
00368 
00369       fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
00370       fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
00371 
00372       Constraint *constraint;
00373 
00374       int nu[2], nq;
00375 
00376       Frame *frameForJacobianOfRotation;
00377 
00378       std::vector<FixedRelativeFrame*> RBF;
00379       std::vector<CompoundContour*> RBC;
00380 
00381       Frame *frameForInertiaTensor;
00382 
00383       fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
00384 
00385       bool translationDependentRotation, constJT, constJR, constjT, constjR;
00386 
00387     private:
00388 #ifdef HAVE_OPENMBVCPPINTERFACE
00389 
00392       Frame * openMBVFrame;
00393       OpenMBV::Arrow *FWeight, *FArrow, *MArrow;
00394 #endif
00395   };
00396 
00397 }
00398 
00399 #endif
00400