mbsim  4.0.0
MBSim Kernel
 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/objects/body.h"
24 #include "mbsim/frames/frame.h"
25 #include "mbsim/functions/time_dependent_function.h"
26 #include "mbsim/functions/state_dependent_function.h"
27 
28 namespace MBSim {
29 
30  class Frame;
31  class RigidContour;
32  class FixedRelativeFrame;
33  class Constraint;
34  class InverseKineticsJoint;
35 
50  class RigidBody : public Body {
51  public:
52  enum GeneralizedVelocityOfRotation {
53  derivativeOfGeneralizedPositionOfRotation=0,
54  coordinatesOfAngularVelocityWrtFrameOfReference,
55  coordinatesOfAngularVelocityWrtFrameForKinematics,
56  unknown
57  };
58 
63  RigidBody(const std::string &name="");
64 
68  ~RigidBody() override;
69 
70  void addDependency(Constraint* constraint_);
71 
72  void updateqd() override;
73  void updateT() override;
74  void updateh(int j=0) override;
75  void updateM() override;
76  void updateInertiaTensor();
77  void updateGeneralizedPositions() override;
78  void updateGeneralizedVelocities() override;
79  void updateDerivativeOfGeneralizedPositions() override;
80  void updateGeneralizedAccelerations() override;
81  void updateGeneralizedJacobians(int j=0);
82  void updatePositions();
83  void updateVelocities();
84  void updateJacobians() override;
85  void updateGyroscopicAccelerations();
86  void updatePositions(Frame *frame) override;
87  void updateVelocities(Frame *frame) override;
88  void updateAccelerations(Frame *frame) override;
89  void updateJacobians(Frame *frame, int j=0) override { (this->*updateJacobians_[j])(frame); }
90  void updateGyroscopicAccelerations(Frame *frame) override;
91  void updateJacobians0(Frame *frame);
92  void updateJacobians1(Frame *frame) { }
93  void updateJacobians2(Frame *frame);
94 
95  void calcSize() override;
96  void calcqSize() override;
97  void calcuSize(int j=0) override;
98  void sethSize(int hSize_, int i=0) override;
99  void sethInd(int hInd_, int i=0) override;
100 
101  /* INHERITED INTERFACE OF OBJECT */
102  void init(InitStage stage, const InitConfigSet &config) override;
103  void setUpInverseKinetics() override;
104  /*****************************************************/
105 
106  /* INHERITED INTERFACE OF ELEMENT */
107  void plot() override;
108  /*****************************************************/
109 
110  /* GETTER / SETTER */
111 
112  // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
117  void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
118  fPrPK = fPrPK_;
119  fPrPK->setParent(this);
120  fPrPK->setName("GeneralTranslation");
121  }
126  void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
128  }
133  void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
135  }
136  void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
137  void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
138  void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
139 
140  // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
145  void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
146  fAPK = fAPK_;
147  fAPK->setParent(this);
148  fAPK->setName("GeneralRotation");
149  }
154  void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
159  void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
160  void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
161  void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
162  void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
163 
164  void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
165  void setGeneralizedVelocityOfRotation(GeneralizedVelocityOfRotation generalizedVelocityOfRotation_) { generalizedVelocityOfRotation = generalizedVelocityOfRotation_; }
166 
177 
178  void setMass(double m_) { m = m_; }
179  double getMass() const { return m; }
180  Frame* getFrameForKinematics() { return &Z; };
181  FixedRelativeFrame* getFrameC() { return C; };
182 
183  const fmatvec::Vec3& evalGlobalRelativePosition() { if(updPos) updatePositions(); return WrPK; }
184  const fmatvec::Vec3& evalGlobalRelativeVelocity() { if(updVel) updateVelocities(); return WvPKrel; }
185  const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() { if(updVel) updateVelocities(); return WomPK; }
186  const fmatvec::SymMat3& evalGlobalInertiaTensor() { if(updWTS) updateInertiaTensor(); return WThetaS; }
187  const fmatvec::Mat3xV& evalPJTT() { if(updPJ) updateJacobians(); return PJTT; }
188  const fmatvec::Mat3xV& evalPJRR() { if(updPJ) updateJacobians(); return PJRR; }
189  const fmatvec::Vec3& evalPjhT() { if(updPJ) updateJacobians(); return PjhT; }
190  const fmatvec::Vec3& evalPjhR() { if(updPJ) updateJacobians(); return PjhR; }
191  const fmatvec::Vec3& evalPjbT() { if(updPjb) updateGyroscopicAccelerations(); return PjbT; }
192  const fmatvec::Vec3& evalPjbR() { if(updPjb) updateGyroscopicAccelerations(); return PjbR; }
193 
194  fmatvec::SymMat3& getGlobalInertiaTensor(bool check=true) { assert((not check) or (not updWTS)); return WThetaS; }
195  fmatvec::Vec3& getGlobalRelativeVelocity(bool check=true) { assert((not check) or (not updVel)); return WvPKrel; }
196  fmatvec::SqrMat3& getRelativeOrientation(bool check=true) { assert((not check) or (not updPos)); return APK; }
197  fmatvec::Vec3& getPjbT(bool check=true) { assert((not check) or (not updPjb)); return PjbT; }
198  fmatvec::Vec3& getPjbR(bool check=true) { assert((not check) or (not updPjb)); return PjbR; }
199 
204  void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
205 
206  const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
207  fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
208 
212  void setFrameForKinematics(Frame *frame);
213 
214  void setFrameForInertiaTensor(Frame *frame);
215 
216  void setOpenMBVRigidBody(const std::shared_ptr<OpenMBV::RigidBody> &body);
217  void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
218  const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
219 
220  void initializeUsingXML(xercesc::DOMElement *element) override;
221 
222  fmatvec::MatV& getJRel(int i=0, bool check=true) { assert((not check) or (not updGJ)); return JRel[i]; }
223  fmatvec::VecV& getjRel(bool check=true) { assert((not check) or (not updGJ)); return jRel; }
224  void setqRel(const fmatvec::VecV &q);
225  void setuRel(const fmatvec::VecV &u);
226  void setJRel(const fmatvec::MatV &J);
227  void setjRel(const fmatvec::VecV &j);
228 
229  bool transformCoordinates() const { return fTR!=nullptr; }
230 
231  void resetUpToDate() override;
232  void resetPositionsUpToDate() override;
233  void resetVelocitiesUpToDate() override;
234  void resetJacobiansUpToDate() override;
235  void resetGyroscopicAccelerationsUpToDate() override;
236  const fmatvec::VecV& evalqTRel() { if(updq) updateGeneralizedPositions(); return qTRel; }
237  const fmatvec::VecV& evalqRRel() { if(updq) updateGeneralizedPositions(); return qRRel; }
238  const fmatvec::VecV& evaluTRel() { if(updu) updateGeneralizedVelocities(); return uTRel; }
239  const fmatvec::VecV& evaluRRel() { if(updu) updateGeneralizedVelocities(); return uRRel; }
240  const fmatvec::VecV& evalqdTRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdTRel; }
241  const fmatvec::VecV& evalqdRRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdRRel; }
242  const fmatvec::MatV& evalJRel(int j=0) { if(updGJ) updateGeneralizedJacobians(); return JRel[j]; }
243  const fmatvec::VecV& evaljRel() { if(updGJ) updateGeneralizedJacobians(); return jRel; }
244 
245  fmatvec::VecV& getqTRel(bool check=true) { assert((not check) or (not updq)); return qTRel; }
246  fmatvec::VecV& getqRRel(bool check=true) { assert((not check) or (not updq)); return qRRel; }
247  fmatvec::VecV& getuTRel(bool check=true) { assert((not check) or (not updu)); return uTRel; }
248  fmatvec::VecV& getuRRel(bool check=true) { assert((not check) or (not updu)); return uRRel; }
249  fmatvec::VecV& getqdTRel(bool check=true) { assert((not check) or (not updqd)); return qdTRel; }
250  fmatvec::VecV& getqdRRel(bool check=true) { assert((not check) or (not updqd)); return qdRRel; }
251 
252  void setUpdateByReference(bool updateByReference_) { updateByReference = updateByReference_; }
253 
254  InverseKineticsJoint* getJoint() { return joint; }
255 
256  void setDynamicSystemSolver(DynamicSystemSolver* sys) override;
257 
258  protected:
262  double m{0};
263 
267  fmatvec::SymMat3 SThetaS, WThetaS;
268 
270 
271  fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
272 
276  fmatvec::SqrMat3 APK;
277 
281  fmatvec::Vec3 PrPK, WrPK;
282 
286  fmatvec::Vec3 WvPKrel, WomPK;
287 
289 
294 
299 
300 #ifndef SWIG
301  void (RigidBody::*updateJacobians_[3])(Frame *frame);
302 #endif
303 
306 
307  fmatvec::MatV JRel[2];
308  fmatvec::VecV jRel;
309 
310  fmatvec::VecV qTRel, qRRel, uTRel, uRRel, qdTRel, qdRRel;
311  fmatvec::Mat3xV PJTT, PJRR;
312 
313  Constraint *constraint{nullptr};
314 
315  Frame *frameForJacobianOfRotation{nullptr};
316 
317  Frame *frameForInertiaTensor{nullptr};
318 
319  fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
320 
321  bool translationDependentRotation{false};
322  bool constJT{false};
323  bool constJR{false};
324  bool constjT{false};
325  bool constjR{false};
326 
327  bool updPjb{true};
328  bool updGJ{true};
329  bool updWTS{true};
330  bool updateByReference{true};
331 
332  Frame Z;
333 
334  GeneralizedVelocityOfRotation generalizedVelocityOfRotation{derivativeOfGeneralizedPositionOfRotation};
335 
336  InverseKineticsJoint *joint;
337 
338  private:
343  };
344 
345 }
346 
347 #endif
void setInertiaTensor(const fmatvec::SymMat3 &RThetaR)
Definition: rigid_body.h:204
void setGeneralTranslation(Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: rigid_body.h:117
void setFrameForKinematics(Frame *frame)
Definition: rigid_body.cc:77
void init(InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: rigid_body.cc:149
Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: rigid_body.h:176
void setStateDependentRotation(Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: rigid_body.h:159
double m
mass
Definition: rigid_body.h:262
cartesian frame on rigid bodies
Definition: fixed_relative_frame.h:31
fmatvec::Vec q
positions, velocities
Definition: object.h:304
Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: rigid_body.h:171
void setStateDependentTranslation(Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: rigid_body.h:133
Definition: time_dependent_function.h:28
Frame * openMBVFrame
Frame of reference for drawing openMBVBody.
Definition: rigid_body.h:342
void setUpInverseKinetics() override
TODO.
Definition: rigid_body.cc:287
InitStage
The stages of the initialization.
Definition: element.h:62
FixedRelativeFrame * C
Definition: rigid_body.h:305
fmatvec::SymMat3 SThetaS
inertia tensor with respect to centre of gravity in centre of gravity and world Frame ...
Definition: rigid_body.h:267
void setGeneralRotation(Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: rigid_body.h:145
Definition: contact_kinematics.h:32
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: rigid_body.h:276
std::string name
name of element
Definition: element.h:260
std::vector< Frame * > frame
vector of frames and contours
Definition: body.h:169
Class for constraints between generalized coordinates of objects.
Definition: constraint.h:30
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:298
void plot() override
plots time dependent data
Definition: rigid_body.cc:300
~RigidBody() override
destructor
Definition: rigid_body.cc:71
void setDynamicSystemSolver(DynamicSystemSolver *sys) override
sets the used dynamics system solver to the element
Definition: rigid_body.cc:645
Definition: state_dependent_function.h:28
RigidBody(const std::string &name="")
constructor
Definition: rigid_body.cc:56
void setName(const std::string &str)
Definition: element.h:115
Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: rigid_body.h:293
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system ...
Definition: rigid_body.h:286
void setTimeDependentRotation(Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: rigid_body.h:154
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: rigid_body.h:281
void setTimeDependentTranslation(Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: rigid_body.h:126
base class for all mechanical bodies with mass and generalised coordinates
Definition: body.h:49
rigid bodies with arbitrary kinematics
Definition: rigid_body.h:50