MBSim

Project Page
rigid_body.h
1 /* Copyright (C) 2004-2010 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  */
19 
20 #ifndef _RIGID_BODY_H_
21 #define _RIGID_BODY_H_
22 
23 #include "mbsim/body.h"
24 #include "fmatvec.h"
25 #include "mbsim/kinematics.h"
26 #include "mbsim/utils/function.h"
27 
28 #ifdef HAVE_OPENMBVCPPINTERFACE
29 namespace OpenMBV {
30  class RigidBody;
31 }
32 #endif
33 
34 namespace MBSim {
35 
36  class Frame;
37  class Constraint;
38 
53  class RigidBody : public Body {
54  public:
59  RigidBody(const std::string &name);
60 
64  virtual ~RigidBody();
65 
66  void addDependency(Constraint* constraint_) {
67  //body.push_back(body_);
68  constraint = constraint_;
69  }
70 
71  virtual void updateT(double t) { if(fT) TRel = (*fT)(qRel,t); }
72  virtual void updateh(double t, int j=0);
73  virtual void updateh0Fromh1(double t);
74  virtual void updateW0FromW1(double t);
75  virtual void updateV0FromV1(double t);
76  virtual void updatehInverseKinetics(double t, int j=0);
77  virtual void updateStateDerivativeDependentVariables(double t);
78  virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
79  virtual void updateStateDependentVariables(double t) {
82  }
83  virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
84  void updateJacobians0(double t) {
87  }
88  void updateJacobians1(double t) {
89  updateJacobiansForRemainingFramesAndContours1(t);
90  }
91  virtual void calcqSize();
92  virtual void calcuSize(int j=0);
93 
94  /* INHERITED INTERFACE OF OBJECT */
95  virtual void updateqRef(const fmatvec::Vec& ref);
96  virtual void updateuRef(const fmatvec::Vec& ref);
97  virtual void updateTRef(const fmatvec::Mat &ref);
98  virtual void init(InitStage stage);
99  virtual void initz();
100  virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
101  virtual void setUpInverseKinetics();
102  /*****************************************************/
103 
104  /* INHERITED INTERFACE OF ELEMENT */
105  virtual std::string getType() const { return "RigidBody"; }
106  virtual void plot(double t, double dt=1);
107  /*****************************************************/
108 
109  /* INTERFACE FOR DERIVED CLASSES */
113  virtual void updateKinematicsForSelectedFrame(double t);
117  virtual void updateJacobiansForSelectedFrame0(double t);
118 
122  virtual void updateKinematicsForRemainingFramesAndContours(double t);
123 
127  virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
128  virtual void updateJacobiansForRemainingFramesAndContours1(double t);
129 
130  /* GETTER / SETTER */
135  void setTranslation(Translation* fPrPK_) { fPrPK = fPrPK_; }
140  void setRotation(Rotation* fAPK_) { fAPK = fAPK_; }
150  Rotation* getRotation() { return fAPK; }
151  void setJacobianOfTranslation(Jacobian* fPJT_) { fPJT = fPJT_; }
152  void setJacobianOfRotation(Jacobian* fPJR_) { fPJR = fPJR_; }
153  void setDerivativeOfJacobianOfTranslation(Function3<fmatvec::Mat, fmatvec::Vec, fmatvec::Vec, double>* fPdJT_) { fPdJT = fPdJT_;}
154  void setDerivativeOfJacobianOfRotation(Function3<fmatvec::Mat, fmatvec::Vec, fmatvec::Vec, double>* fPdJR_) { fPdJR = fPdJR_;}
155 
158 
161 
164 
167  void setMass(double m_) { m = m_; }
168  double getMass() const { return m; }
169  Frame* getFrameForKinematics() { return frame[iKinematics]; };
171  void isFrameOfBodyForRotation(bool cb_) { cb = cb_; }
172  std::vector<fmatvec::SqrMat> getContainerForFrameOrientations() const { return ASF; }
173  std::vector<fmatvec::Vec> getContainerForFramePositions() const { return SrSF; }
174  std::vector<fmatvec::SqrMat> getContainerForContourOrientations() const { return ASC; }
175  std::vector<fmatvec::Vec> getContainerForContourPositions() const { return SrSC; }
176  /*****************************************************/
177 
182  void setInertiaTensor(const fmatvec::SymMat& RThetaR, const Frame* refFrame=0) {
183  if(refFrame)
184  iInertia = frameIndex(refFrame);
185  else
186  iInertia = 0;
187  SThetaS = RThetaR;
188  }
189 
190  const fmatvec::SymMat& getInertiaTensor() const {return SThetaS;}
191  fmatvec::SymMat& getInertiaTensor() {return SThetaS;}
192 
199  void addFrame(Frame *frame_, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const std::string& refFrameName);
200 
207  void addFrame(Frame *frame_, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const Frame* refFrame=0);
208 
215  void addFrame(const std::string &str, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const Frame* refFrame=0);
216 
223  void addContour(Contour* contour, const fmatvec::Vec &RrRC, const fmatvec::SqrMat &ARC, const std::string& refFrameName);
224 
231  void addContour(Contour* contour, const fmatvec::Vec &RrRC, const fmatvec::SqrMat &ARC, const Frame* refFrame=0);
232 
237  iKinematics = frameIndex(frame);
238  assert(iKinematics > -1);
239  }
240 
241 #ifdef HAVE_OPENMBVCPPINTERFACE
242  void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
243  void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
244 #endif
245 
246  virtual void initializeUsingXML(TiXmlElement *element);
247  virtual void updatePositionAndOrientationOfFrame(double t, unsigned int i);
248  virtual void updateAccelerations(double t, unsigned int i);
249  virtual void updateRelativeJacobians(double t, unsigned int i);
250  virtual void updateRelativeJacobians(double t, unsigned int i, fmatvec::Mat &WJTrel, fmatvec::Mat &WJRrel);
251  const fmatvec::Mat& getWJTrel() const {return WJTrel;}
252  const fmatvec::Mat& getWJRrel() const {return WJRrel;}
253  fmatvec::Mat& getWJTrel() {return WJTrel;}
254  fmatvec::Mat& getWJRrel() {return WJRrel;}
255  fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
256  fmatvec::Vec& getjRel() {return jRel;}
257  fmatvec::Vec& getqRel() {return qRel;}
258  fmatvec::Vec& getuRel() {return uRel;}
259  // void setqRel(const fmatvec::Vec &q) {qRel0 = q;}
260  // void setuRel(const fmatvec::Vec &u) {uRel0 = u;}
261  fmatvec::Mat& getPJT(int i=0) {return PJT[i];}
262  fmatvec::Mat& getPJR(int i=0) {return PJR[i];}
263 
264  protected:
268  double m;
269 
273  fmatvec::SymMat SThetaS, WThetaS;
274 
278  int iKinematics, iInertia;
279 
283  fmatvec::SymMat Mbuf;
284 
288  bool cb;
289 
293  fmatvec::Mat PJT[2], PJR[2], PdJT, PdJR;
294 
298  fmatvec::Vec PjT, PjR, PdjT, PdjR;
299 
303  fmatvec::Mat PJR0;
304 
308  fmatvec::SqrMat APK;
309 
313  fmatvec::Vec PrPK, WrPK;
314 
318  fmatvec::Vec WvPKrel, WomPK;
319 
323  std::vector<fmatvec::SqrMat> ASF;
324 
328  std::vector<fmatvec::Vec> SrSF, WrSF;
329 
330 
334  std::vector<fmatvec::SqrMat> ASC;
335 
339  std::vector<fmatvec::Vec> SrSC, WrSC;
340 
345 
350 
355 
360 
365 
370 
375 
380 
385 
390 
395 
399  void (RigidBody::*updateM_)(double t, int i);
400 
404  void updateMConst(double t, int i=0);
405 
409  void updateMNotConst(double t, int i=0);
410 
414  void (RigidBody::*facLLM_)(int i);
415 
419  void facLLMConst(int i=0) {};
420 
424  void facLLMNotConst(int i=0) { Object::facLLM(i); }
425 
426  void (RigidBody::*updateJacobians_[2])(double t);
427 
430 
431  fmatvec::Vec aT, aR;
432 
433  fmatvec::Vec qRel, uRel;
434  fmatvec::Mat JRel[2];
435  fmatvec::Vec jRel;
436 
437  fmatvec::Mat WJTrel,WJRrel;
438  fmatvec::Vec WjTrel,WjRrel;
439 
440  fmatvec::Mat TRel;
441 
442  Constraint *constraint;
443 
444  int nu[2], nq;
445 
446  //fmatvec::Vec qRel0, uRel0;
447 
448  private:
449  std::vector<std::string> saved_refFrameF, saved_refFrameC;
450  std::vector<fmatvec::Vec> saved_RrRF, saved_RrRC;
451  std::vector<fmatvec::SqrMat> saved_ARF, saved_ARC;
452 #ifdef HAVE_OPENMBVCPPINTERFACE
453 
456  Frame * openMBVFrame;
457 #endif
458  };
459 
460 }
461 
462 #endif
463