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 "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
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
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
00103 virtual std::string getType() const { return "RigidBody"; }
00104 virtual void plot(double t, double dt=1);
00105
00106
00107
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
00129
00130
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
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
00262
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