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 "fmatvec/fmatvec.h"
00025 #include "mbsim/kinematics.h"
00026 #include "mbsim/utils/function.h"
00027
00028 #ifdef HAVE_OPENMBVCPPINTERFACE
00029 namespace OpenMBV {
00030 class RigidBody;
00031 class Arrow;
00032 }
00033 #endif
00034
00035 namespace MBSim {
00036
00037 class Frame;
00038 class Contour;
00039 class FixedRelativeFrame;
00040 class CompoundContour;
00041 class Constraint;
00042
00057 class RigidBody : public Body {
00058 public:
00063 RigidBody(const std::string &name="");
00064
00068 virtual ~RigidBody();
00069
00070 void addDependency(Constraint* constraint_) {
00071
00072 constraint = constraint_;
00073 }
00074
00075 virtual void updateT(double t) { if(fT) TRel = (*fT)(qRel,t); }
00076 virtual void updateh(double t, int j=0);
00077 virtual void updateh0Fromh1(double t);
00078 virtual void updateW0FromW1(double t);
00079 virtual void updateV0FromV1(double t);
00080 virtual void updatehInverseKinetics(double t, int j=0);
00081 virtual void updateStateDerivativeDependentVariables(double t);
00082 virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
00083 virtual void updateStateDependentVariables(double t) {
00084 updateKinematicsForSelectedFrame(t);
00085 updateKinematicsForRemainingFramesAndContours(t);
00086 }
00087 virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
00088 void updateJacobians0(double t) {
00089 updateJacobiansForSelectedFrame0(t);
00090 updateJacobiansForRemainingFramesAndContours(t,0);
00091 }
00092 void updateJacobians1(double t) {
00093 updateJacobiansForRemainingFramesAndContours1(t);
00094 }
00095 virtual void calcqSize();
00096 virtual void calcuSize(int j=0);
00097
00098
00099 virtual void updateqRef(const fmatvec::Vec& ref);
00100 virtual void updateuRef(const fmatvec::Vec& ref);
00101 virtual void updateTRef(const fmatvec::Mat &ref);
00102 virtual void init(InitStage stage);
00103 virtual void initz();
00104 virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
00105 virtual void setUpInverseKinetics();
00106
00107
00108
00109 virtual std::string getType() const { return "RigidBody"; }
00110 virtual void plot(double t, double dt=1);
00111
00112
00113
00117 virtual void updateKinematicsForSelectedFrame(double t);
00121 virtual void updateJacobiansForSelectedFrame0(double t);
00122
00126 virtual void updateKinematicsForRemainingFramesAndContours(double t);
00127
00131 virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
00132 virtual void updateJacobiansForRemainingFramesAndContours1(double t);
00133
00134
00139 void setTranslation(Translation* fPrPK_) { fPrPK = fPrPK_; }
00144 void setRotation(Rotation* fAPK_) { fAPK = fAPK_; }
00149 Translation* getTranslation() { return fPrPK; }
00154 Rotation* getRotation() { return fAPK; }
00155 void setJacobianOfTranslation(Jacobian* fPJT_) { fPJT = fPJT_; }
00156 void setJacobianOfRotation(Jacobian* fPJR_) { fPJR = fPJR_; }
00157 void setDerivativeOfJacobianOfTranslation(DerivativeOfJacobian* fPdJT_) { fPdJT = fPdJT_;}
00158 void setDerivativeOfJacobianOfRotation(DerivativeOfJacobian* fPdJR_) { fPdJR = fPdJR_;}
00159
00161 void setGuidingVelocityOfTranslation(GuidingVelocity* fPjT_) { fPjT = fPjT_;}
00162
00164 void setGuidingVelocityOfRotation(GuidingVelocity* fPjR_) { fPjR = fPjR_;}
00165
00167 void setDerivativeOfGuidingVelocityOfTranslation(DerivativeOfGuidingVelocity* fPdjT_) { fPdjT = fPdjT_;}
00168
00170 void setDerivativeOfGuidingVelocityOfRotation(DerivativeOfGuidingVelocity* fPdjR_) { fPdjR = fPdjR_;}
00171
00172 void setMass(double m_) { m = m_; }
00173 double getMass() const { return m; }
00174 FixedRelativeFrame* getFrameForKinematics() { return K; };
00175 FixedRelativeFrame* getFrameC() { return C; };
00176 void isFrameOfBodyForRotation(bool cb_) { cb = cb_; }
00177
00182 void setInertiaTensor(const fmatvec::SymMat3& RThetaR, Frame *frame=0) {
00183 SThetaS = RThetaR;
00184 frameForInertiaTensor = frame;
00185 }
00186
00187 const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
00188 fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
00189
00190 void addFrame(FixedRelativeFrame *frame);
00191
00192 using Body::addContour;
00193
00194
00195
00196
00197
00198
00199
00200
00201
00208 void addFrame(Frame *frame, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00209
00216 void addFrame(const std::string &str, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00217
00218
00219
00220
00221
00222
00223
00224
00225
00232 void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const fmatvec::SqrMat3 &ARC, const Frame* refFrame=0);
00233
00237 void setFrameForKinematics(Frame *frame);
00238
00239 #ifdef HAVE_OPENMBVCPPINTERFACE
00240 void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
00241 void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
00242 const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
00243
00245 void setOpenMBVWeightArrow(OpenMBV::Arrow *arrow) { FWeight = arrow; }
00246
00248 void setOpenMBVJointForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00249
00251 void setOpenMBVJointMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00252 #endif
00253
00254 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00255 virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00256
00257 virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
00258 virtual void updateAccelerations(double t, Frame *P);
00259 virtual void updateRelativeJacobians(double t, Frame *P);
00260 virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
00261 const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
00262 const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
00263 fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
00264 fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
00265 fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
00266 fmatvec::Vec& getjRel() {return jRel;}
00267 fmatvec::Vec& getqRel() {return qRel;}
00268 fmatvec::Vec& getuRel() {return uRel;}
00269
00270
00271 fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
00272 fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
00273
00274 int getqRelSize() const {return nq;}
00275 int getuRelSize(int i=0) const {return nu[i];}
00276
00277 protected:
00281 double m;
00282
00286 fmatvec::SymMat3 SThetaS, WThetaS;
00287
00288 FixedRelativeFrame *K;
00289
00293 fmatvec::SymMat Mbuf;
00294
00298 bool cb;
00299
00303 fmatvec::Mat3xV PJT[2], PJR[2], PdJT, PdJR;
00304
00308 fmatvec::Vec3 PjT, PjR, PdjT, PdjR;
00309
00313 fmatvec::SqrMat3 APK;
00314
00318 fmatvec::Vec3 PrPK, WrPK;
00319
00323 fmatvec::Vec3 WvPKrel, WomPK;
00324
00328 TMatrix *fT;
00329
00333 Translation *fPrPK;
00334
00338 Rotation *fAPK;
00339
00343 Jacobian *fPJT;
00344
00348 Jacobian *fPJR;
00349
00353 DerivativeOfJacobian *fPdJT;
00354
00358 DerivativeOfJacobian *fPdJR;
00359
00363 GuidingVelocity *fPjT;
00364
00368 GuidingVelocity *fPjR;
00369
00373 DerivativeOfGuidingVelocity *fPdjT;
00374
00378 DerivativeOfGuidingVelocity *fPdjR;
00379
00383 void (RigidBody::*updateM_)(double t, int i);
00384
00388 void updateMConst(double t, int i=0);
00389
00393 void updateMNotConst(double t, int i=0);
00394
00398 void (RigidBody::*facLLM_)(int i);
00399
00403 void facLLMConst(int i=0) {};
00404
00408 void facLLMNotConst(int i=0) { Object::facLLM(i); }
00409
00410 void (RigidBody::*updateJacobians_[2])(double t);
00411
00413 FixedRelativeFrame *C;
00414
00415 fmatvec::Vec aT, aR;
00416
00417 fmatvec::Vec qRel, uRel;
00418 fmatvec::Mat JRel[2];
00419 fmatvec::Vec jRel;
00420
00421 fmatvec::Mat3xV WJTrel,WJRrel;
00422 fmatvec::Vec3 WjTrel,WjRrel;
00423
00424 fmatvec::Mat TRel;
00425
00426 Constraint *constraint;
00427
00428 int nu[2], nq;
00429
00430 Frame *frameForJacobianOfRotation;
00431
00432 std::vector<FixedRelativeFrame*> RBF;
00433 std::vector<CompoundContour*> RBC;
00434
00435 Frame *frameForInertiaTensor;
00436
00437 private:
00438 #ifdef HAVE_OPENMBVCPPINTERFACE
00439
00442 Frame * openMBVFrame;
00443 OpenMBV::Arrow *FWeight, *FArrow, *MArrow;
00444 #endif
00445 };
00446
00447 }
00448
00449 #endif
00450