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 "mbsim/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 updatehInverseKinetics(double t, int j=0);
00076       virtual void updateStateDerivativeDependentVariables(double t);
00077       virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
00078       virtual void updateStateDependentVariables(double t) { 
00079         updateKinematicsForSelectedFrame(t); 
00080         updateKinematicsForRemainingFramesAndContours(t); 
00081       }
00082       virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
00083       void updateJacobians0(double t) { 
00084         updateJacobiansForSelectedFrame0(t); 
00085         updateJacobiansForRemainingFramesAndContours(t,0); 
00086       }
00087       void updateJacobians1(double t) { 
00088         updateJacobiansForRemainingFramesAndContours1(t); 
00089       }
00090       virtual void calcqSize();
00091       virtual void calcuSize(int j=0);
00092 
00093       /* INHERITED INTERFACE OF OBJECT */
00094       virtual void updateqRef(const fmatvec::Vec& ref);
00095       virtual void updateuRef(const fmatvec::Vec& ref);
00096       virtual void init(InitStage stage);
00097       virtual void initz();
00098       virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
00099       virtual void setUpInverseKinetics();
00100       /*****************************************************/
00101 
00102       /* INHERITED INTERFACE OF ELEMENT */
00103       virtual std::string getType() const { return "RigidBody"; }
00104       virtual void plot(double t, double dt=1);
00105       /*****************************************************/
00106 
00107       /* INTERFACE FOR DERIVED CLASSES */
00111       virtual void updateKinematicsForSelectedFrame(double t);
00115       virtual void updateJacobiansForSelectedFrame0(double t); 
00116 
00120       virtual void updateKinematicsForRemainingFramesAndContours(double t);
00121 
00125       virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
00126       virtual void updateJacobiansForRemainingFramesAndContours1(double t);
00127 
00128       /* GETTER / SETTER */
00129 
00130       // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
00135       void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
00136         fPrPK = fPrPK_;
00137         fPrPK->setParent(this);
00138         fPrPK->setName("GeneralTranslation");
00139       }
00144       void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
00145         setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
00146       }
00151       void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
00152         setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
00153       }
00154       void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
00155       void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
00156       void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
00157 
00158       // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
00163       void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
00164         fAPK = fAPK_;
00165         fAPK->setParent(this);
00166         fAPK->setName("GeneralRotation");
00167       }
00172       void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00177       void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00178       void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
00179       void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
00180       void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
00181 
00182       void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
00183       void setCoordinateTransformationForRotation(bool ct) { coordinateTransformation = ct; }
00184 
00189       Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
00194       Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
00195 
00196       void setMass(double m_) { m = m_; }
00197       double getMass() const { return m; }
00198       FixedRelativeFrame* getFrameForKinematics() { return K; };
00199       FixedRelativeFrame* getFrameC() { return C; };
00200 
00205       void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
00206 
00207       const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
00208       fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
00209 
00210       void addFrame(FixedRelativeFrame *frame); 
00211 
00212       using Body::addContour;
00213 
00217       void setFrameForKinematics(Frame *frame);
00218 
00219       void setFrameForInertiaTensor(Frame *frame);
00220 
00221 #ifdef HAVE_OPENMBVCPPINTERFACE
00222       void setOpenMBVRigidBody(const boost::shared_ptr<OpenMBV::RigidBody> &body);
00223       void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
00224       const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
00225 
00227       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))) { 
00228         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00229         FWeight=ombv.createOpenMBV();
00230       }
00231 
00233       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))) { 
00234         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00235         FArrow=ombv.createOpenMBV();
00236       }
00237 
00239       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))) { 
00240         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00241         MArrow=ombv.createOpenMBV();
00242       }
00243 #endif
00244 
00245       virtual void initializeUsingXML(xercesc::DOMElement *element);
00246       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00247 
00248       virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
00249       virtual void updateAccelerations(double t, Frame *P);
00250       virtual void updateRelativeJacobians(double t, Frame *P);
00251       virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
00252       const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
00253       const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
00254       fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
00255       fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
00256       fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
00257       fmatvec::Vec& getjRel() {return jRel;}
00258       fmatvec::Vec& getqRel() {return qRel;}
00259       fmatvec::Vec& getuRel() {return uRel;}
00260       fmatvec::Mat& getTRel() {return TRel;}
00261       // void setqRel(const fmatvec::Vec &q) {qRel0 = q;}
00262       // void setuRel(const fmatvec::Vec &u) {uRel0 = u;}
00263       fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
00264       fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
00265 
00266       int getqRelSize() const {return nq;}
00267       int getuRelSize(int i=0) const {return nu[i];}
00268 
00269       bool transformCoordinates() const {return fTR!=NULL;}
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 
00294       fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
00295 
00299       fmatvec::SqrMat3 APK;
00300 
00304       fmatvec::Vec3 PrPK, WrPK;
00305 
00309       fmatvec::Vec3 WvPKrel, WomPK;
00310 
00311       Function<fmatvec::MatV(fmatvec::VecV)> *fTR;
00312 
00316       Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK;
00317 
00321       Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK;
00322 
00326       void (RigidBody::*updateM_)(double t, int i);
00327 
00331       void updateMConst(double t, int i=0);
00332 
00336       void updateMNotConst(double t, int i=0); 
00337 
00341       void (RigidBody::*facLLM_)(int i);
00342 
00346       void facLLMConst(int i=0) {};
00347 
00351       void facLLMNotConst(int i=0) { Object::facLLM(i); }
00352 
00353       void (RigidBody::*updateJacobians_[2])(double t); 
00354 
00356       FixedRelativeFrame *C;
00357 
00358       fmatvec::Vec aT, aR;
00359 
00360       fmatvec::Vec qRel, uRel;
00361       fmatvec::Mat JRel[2];
00362       fmatvec::Vec jRel;
00363       fmatvec::Mat TRel;
00364 
00365       fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
00366       fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
00367 
00368       Constraint *constraint;
00369 
00370       Frame *frameForJacobianOfRotation;
00371 
00372       std::vector<FixedRelativeFrame*> RBF;
00373       std::vector<CompoundContour*> RBC;
00374 
00375       Frame *frameForInertiaTensor;
00376 
00377       fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
00378 
00379       bool translationDependentRotation, constJT, constJR, constjT, constjR;
00380 
00381       fmatvec::Vec3 WF, WM;
00382 
00383     private:
00384 #ifdef HAVE_OPENMBVCPPINTERFACE
00385 
00388       Frame * openMBVFrame;
00389       boost::shared_ptr<OpenMBV::Arrow> FWeight, FArrow, MArrow;
00390 #endif
00391   };
00392 
00393 }
00394 
00395 #endif
00396