00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
00106 virtual std::string getType() const { return "RigidBody"; }
00107 virtual void plot(double t, double dt=1);
00108
00109
00110
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
00132
00133
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
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
00264
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