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(fmatvec::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
00139 fPrPK = fPrPK_;
00140 }
00145 void setTimeDependentTranslation(fmatvec::Function<fmatvec::Vec3(double)> *fPrPK_) {
00146 setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
00147 }
00152 void setStateDependentTranslation(fmatvec::Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
00153 setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
00154 }
00155 void setTranslation(fmatvec::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
00156 void setTranslation(fmatvec::Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
00157 void setTranslation(fmatvec::Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
00158
00159
00164 void setGeneralRotation(fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { fAPK = fAPK_; }
00169 void setTimeDependentRotation(fmatvec::Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00174 void setStateDependentRotation(fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
00175 void setRotation(fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
00176 void setRotation(fmatvec::Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
00177 void setRotation(fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
00178
00179 void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
00180 void setCoordinateTransformationForRotation(bool ct) { coordinateTransformation = ct; }
00181
00186 fmatvec::Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
00191 fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
00192
00193 void setMass(double m_) { m = m_; }
00194 double getMass() const { return m; }
00195 FixedRelativeFrame* getFrameForKinematics() { return K; };
00196 FixedRelativeFrame* getFrameC() { return C; };
00197
00202 void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
00203
00204 const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
00205 fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
00206
00207 void addFrame(FixedRelativeFrame *frame);
00208
00209 using Body::addContour;
00210
00214 void setFrameForKinematics(Frame *frame);
00215
00216 void setFrameForInertiaTensor(Frame *frame);
00217
00218 #ifdef HAVE_OPENMBVCPPINTERFACE
00219 void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
00220 void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
00221 const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
00222
00224 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))) {
00225 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00226 FWeight=ombv.createOpenMBV();
00227 }
00228
00230 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))) {
00231 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00232 FArrow=ombv.createOpenMBV();
00233 }
00234
00236 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))) {
00237 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00238 MArrow=ombv.createOpenMBV();
00239 }
00240 #endif
00241
00242 virtual void initializeUsingXML(xercesc::DOMElement *element);
00243 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00244
00245 virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
00246 virtual void updateAccelerations(double t, Frame *P);
00247 virtual void updateRelativeJacobians(double t, Frame *P);
00248 virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
00249 const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
00250 const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
00251 fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
00252 fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
00253 fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
00254 fmatvec::Vec& getjRel() {return jRel;}
00255 fmatvec::Vec& getqRel() {return qRel;}
00256 fmatvec::Vec& getuRel() {return uRel;}
00257
00258
00259 fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
00260 fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
00261
00262 int getqRelSize() const {return nq;}
00263 int getuRelSize(int i=0) const {return nu[i];}
00264
00265 protected:
00269 double m;
00270
00274 fmatvec::SymMat3 SThetaS, WThetaS;
00275
00276 FixedRelativeFrame *K;
00277
00281 fmatvec::SymMat Mbuf;
00282
00286 bool coordinateTransformation;
00287
00291 fmatvec::Mat3xV PJT[2], PJR[2];
00292
00293 fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
00294
00298 fmatvec::SqrMat3 APK;
00299
00303 fmatvec::Vec3 PrPK, WrPK;
00304
00308 fmatvec::Vec3 WvPKrel, WomPK;
00309
00310 fmatvec::Function<fmatvec::MatV(fmatvec::VecV)> *fTR;
00311
00315 fmatvec::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK;
00316
00320 fmatvec::Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK;
00321
00325 void (RigidBody::*updateM_)(double t, int i);
00326
00330 void updateMConst(double t, int i=0);
00331
00335 void updateMNotConst(double t, int i=0);
00336
00340 void (RigidBody::*facLLM_)(int i);
00341
00345 void facLLMConst(int i=0) {};
00346
00350 void facLLMNotConst(int i=0) { Object::facLLM(i); }
00351
00352 void (RigidBody::*updateJacobians_[2])(double t);
00353
00355 FixedRelativeFrame *C;
00356
00357 fmatvec::Vec aT, aR;
00358
00359 fmatvec::Vec qRel, uRel;
00360 fmatvec::Mat JRel[2];
00361 fmatvec::Vec jRel;
00362
00363 fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
00364 fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
00365
00366 Constraint *constraint;
00367
00368 int nu[2], nq;
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 private:
00382 #ifdef HAVE_OPENMBVCPPINTERFACE
00383
00386 Frame * openMBVFrame;
00387 OpenMBV::Arrow *FWeight, *FArrow, *MArrow;
00388 #endif
00389 };
00390
00391 }
00392
00393 #endif
00394