All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
rigid_body.h
1 /* Copyright (C) 2004-2014 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 "mbsim/functions/auxiliary_functions.h"
25 
26 #ifdef HAVE_OPENMBVCPPINTERFACE
27 #include "mbsim/utils/boost_parameters.h"
28 #include "mbsim/utils/openmbv_utils.h"
29 #endif
30 
31 namespace MBSim {
32 
33  class Frame;
34  class Contour;
35  class FixedRelativeFrame;
36  class CompoundContour;
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 updatedq(double t, double dt);
72  virtual void updateqd(double t);
73  virtual void updateT(double t);
74  virtual void updateh(double t, int j=0);
75  virtual void updatehInverseKinetics(double t, int j=0);
76  virtual void updateStateDerivativeDependentVariables(double t);
77  virtual void updateM(double t, int i=0) { (this->*updateM_)(t,i); }
78  virtual void updateStateDependentVariables(double t) {
81  }
82  virtual void updateJacobians(double t, int j=0) { (this->*updateJacobians_[j])(t); }
83  void updateJacobians0(double t) {
86  }
87  void updateJacobians1(double t) {
88  updateJacobiansForRemainingFramesAndContours1(t);
89  }
90  virtual void calcqSize();
91  virtual void calcuSize(int j=0);
92 
93  /* INHERITED INTERFACE OF OBJECT */
94  virtual void updateqRef(const fmatvec::Vec& ref);
95  virtual void updateuRef(const fmatvec::Vec& ref);
96  virtual void init(InitStage stage);
97  virtual void initz();
98  virtual void facLLM(int i=0) { (this->*facLLM_)(i); }
99  virtual void setUpInverseKinetics();
100  /*****************************************************/
101 
102  /* INHERITED INTERFACE OF ELEMENT */
103  virtual std::string getType() const { return "RigidBody"; }
104  virtual void plot(double t, double dt=1);
105  /*****************************************************/
106 
107  /* INTERFACE FOR DERIVED CLASSES */
111  virtual void updateKinematicsForSelectedFrame(double t);
115  virtual void updateJacobiansForSelectedFrame0(double t);
116 
120  virtual void updateKinematicsForRemainingFramesAndContours(double t);
121 
125  virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0);
126  virtual void updateJacobiansForRemainingFramesAndContours1(double t);
127 
128  /* GETTER / SETTER */
129 
130  // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
135  void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
136  fPrPK = fPrPK_;
137  fPrPK->setParent(this);
138  fPrPK->setName("GeneralTranslation");
139  }
144  void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
145  setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
146  }
151  void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
152  setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
153  }
154  void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
155  void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
156  void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
157 
158  // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
163  void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
164  fAPK = fAPK_;
165  fAPK->setParent(this);
166  fAPK->setName("GeneralRotation");
167  }
172  void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
177  void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
178  void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
179  void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
180  void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
181 
182  void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
183  void setCoordinateTransformationForRotation(bool ct) { coordinateTransformation = ct; }
184 
189  Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
194  Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
195 
196  void setMass(double m_) { m = m_; }
197  double getMass() const { return m; }
198  FixedRelativeFrame* getFrameForKinematics() { return K; };
199  FixedRelativeFrame* getFrameC() { return C; };
200 
205  void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
206 
207  const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
208  fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
209 
210  void addFrame(FixedRelativeFrame *frame);
211 
212  using Body::addContour;
213 
217  void setFrameForKinematics(Frame *frame);
218 
219  void setFrameForInertiaTensor(Frame *frame);
220 
221 #ifdef HAVE_OPENMBVCPPINTERFACE
222  void setOpenMBVRigidBody(const boost::shared_ptr<OpenMBV::RigidBody> &body);
223  void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
224  const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
225 
227  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))) {
228  OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
229  FWeight=ombv.createOpenMBV();
230  }
231 
233  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))) {
234  OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
235  FArrow=ombv.createOpenMBV();
236  }
237 
239  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))) {
240  OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
241  MArrow=ombv.createOpenMBV();
242  }
243 #endif
244 
245  virtual void initializeUsingXML(xercesc::DOMElement *element);
246  virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
247 
248  virtual void updatePositionAndOrientationOfFrame(double t, Frame *P);
249  virtual void updateAccelerations(double t, Frame *P);
250  virtual void updateRelativeJacobians(double t, Frame *P);
251  virtual void updateRelativeJacobians(double t, Frame *P, fmatvec::Mat3xV &WJTrel, fmatvec::Mat3xV &WJRrel);
252  const fmatvec::Mat3xV& getWJTrel() const {return WJTrel;}
253  const fmatvec::Mat3xV& getWJRrel() const {return WJRrel;}
254  fmatvec::Mat3xV& getWJTrel() {return WJTrel;}
255  fmatvec::Mat3xV& getWJRrel() {return WJRrel;}
256  fmatvec::Mat& getJRel(int i=0) {return JRel[i];}
257  fmatvec::Vec& getjRel() {return jRel;}
258  fmatvec::Vec& getqRel() {return qRel;}
259  fmatvec::Vec& getuRel() {return uRel;}
260  fmatvec::Mat& getTRel() {return TRel;}
261  // void setqRel(const fmatvec::Vec &q) {qRel0 = q;}
262  // void setuRel(const fmatvec::Vec &u) {uRel0 = u;}
263  fmatvec::Mat3xV& getPJT(int i=0) {return PJT[i];}
264  fmatvec::Mat3xV& getPJR(int i=0) {return PJR[i];}
265 
266  int getqRelSize() const {return nq;}
267  int getuRelSize(int i=0) const {return nu[i];}
268 
269  bool transformCoordinates() const {return fTR!=NULL;}
270 
271  protected:
275  double m;
276 
280  fmatvec::SymMat3 SThetaS, WThetaS;
281 
283 
288 
293 
294  fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
295 
299  fmatvec::SqrMat3 APK;
300 
304  fmatvec::Vec3 PrPK, WrPK;
305 
309  fmatvec::Vec3 WvPKrel, WomPK;
310 
311  Function<fmatvec::MatV(fmatvec::VecV)> *fTR;
312 
316  Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK;
317 
321  Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK;
322 
326  void (RigidBody::*updateM_)(double t, int i);
327 
331  void updateMConst(double t, int i=0);
332 
336  void updateMNotConst(double t, int i=0);
337 
341  void (RigidBody::*facLLM_)(int i);
342 
346  void facLLMConst(int i=0) {};
347 
351  void facLLMNotConst(int i=0) { Object::facLLM(i); }
352 
353  void (RigidBody::*updateJacobians_[2])(double t);
354 
357 
358  fmatvec::Vec aT, aR;
359 
360  fmatvec::Vec qRel, uRel;
361  fmatvec::Mat JRel[2];
362  fmatvec::Vec jRel;
363  fmatvec::Mat TRel;
364 
365  fmatvec::VecV qTRel, qRRel, uTRel, uRRel;
366  fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
367 
368  Constraint *constraint;
369 
370  Frame *frameForJacobianOfRotation;
371 
372  std::vector<FixedRelativeFrame*> RBF;
373  std::vector<CompoundContour*> RBC;
374 
375  Frame *frameForInertiaTensor;
376 
378 
379  bool translationDependentRotation, constJT, constJR, constjT, constjR;
380 
381  fmatvec::Vec3 WF, WM;
382 
383  private:
384 #ifdef HAVE_OPENMBVCPPINTERFACE
385 
388  Frame * openMBVFrame;
389  boost::shared_ptr<OpenMBV::Arrow> FWeight, FArrow, MArrow;
390 #endif
391  };
392 
393 }
394 
395 #endif
396 
void setInertiaTensor(const fmatvec::SymMat3 &RThetaR)
Definition: rigid_body.h:205
void facLLMConst(int i=0)
Cholesky decomposition of constant mass matrix.
Definition: rigid_body.h:346
void(RigidBody::* updateM_)(double t, int i)
function pointer to update mass matrix
Definition: rigid_body.h:326
void facLLMNotConst(int i=0)
Cholesky decomposition of time dependent mass matrix.
Definition: rigid_body.h:351
void setGeneralTranslation(Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: rigid_body.h:135
void setFrameForKinematics(Frame *frame)
Definition: rigid_body.cc:69
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: rigid_body.h:194
void setStateDependentRotation(Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: rigid_body.h:177
virtual void facLLM(int i=0)
perform Cholesky decomposition of mass martix
Definition: rigid_body.h:98
void(RigidBody::* facLLM_)(int i)
function pointer for Cholesky decomposition of mass matrix
Definition: rigid_body.h:341
virtual void updateuRef(const fmatvec::Vec &ref)
references to velocities of dynamic system parent
Definition: rigid_body.cc:544
void updateMConst(double t, int i=0)
update constant mass matrix
Definition: rigid_body.cc:559
double m
mass
Definition: rigid_body.h:275
cartesian frame on rigid bodies
Definition: frame.h:187
virtual void plot(double t, double dt=1)
plots time dependent data
Definition: rigid_body.cc:355
void updateMNotConst(double t, int i=0)
update time dependend mass matrix
Definition: rigid_body.cc:563
fmatvec::Mat3xV PJT[2]
Definition: body.h:173
virtual std::string getType() const
Definition: rigid_body.h:103
bool coordinateTransformation
boolean to use body fixed Frame for rotation
Definition: rigid_body.h:292
virtual void setUpInverseKinetics()
TODO.
Definition: rigid_body.cc:342
Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: rigid_body.h:189
void setStateDependentTranslation(Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: rigid_body.h:151
virtual void updateqRef(const fmatvec::Vec &ref)
references to positions of dynamic system parent
Definition: rigid_body.cc:539
InitStage
The stages of the initialization.
Definition: element.h:97
virtual ~RigidBody()
destructor
Definition: rigid_body.cc:63
FixedRelativeFrame * C
Definition: rigid_body.h:356
fmatvec::SymMat3 SThetaS
inertia tensor with respect to centre of gravity in centre of gravity and world Frame ...
Definition: rigid_body.h:280
void setGeneralRotation(Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: rigid_body.h:163
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: rigid_body.h:299
std::string name
name of element
Definition: element.h:290
std::vector< Frame * > frame
vector of frames and contours
Definition: body.h:162
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:39
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:39
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: rigid_body.h:321
virtual void updateJacobiansForRemainingFramesAndContours(double t, int j=0)
updates remaining JACOBIANS for kinematics starting with and from cog Frame
Definition: rigid_body.cc:518
virtual void updateKinematicsForRemainingFramesAndContours(double t)
updates kinematics for remaining Frames starting with and from cog Frame
Definition: rigid_body.cc:504
virtual void initz()
Definition: rigid_body.cc:333
virtual void updateKinematicsForSelectedFrame(double t)
updates kinematics of kinematic Frame starting from reference Frame
Definition: rigid_body.cc:424
virtual void addContour(Contour *contour)
Definition: body.cc:140
RigidBody(const std::string &name="")
constructor
Definition: rigid_body.cc:50
virtual void updateJacobiansForSelectedFrame0(double t)
updates JACOBIAN for kinematics starting from reference Frame
Definition: rigid_body.cc:466
Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: rigid_body.h:316
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system ...
Definition: rigid_body.h:309
fmatvec::SymMat Mbuf
TODO.
Definition: rigid_body.h:287
void setTimeDependentRotation(Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: rigid_body.h:172
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: rigid_body.h:304
void setTimeDependentTranslation(Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: rigid_body.h:144
virtual void init(InitStage stage)
initialize object at start of simulation with respect to contours and frames
Definition: rigid_body.cc:107
base class for all mechanical bodies with mass and generalised coordinates
Definition: body.h:52
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:53
virtual void facLLM(int i=0)
perform Cholesky decomposition of mass martix
Definition: object.h:217

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML