mbsim  4.0.0
MBSim Kernel
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 
171  Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
176  Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
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 
209  void addFrame(FixedRelativeFrame *frame);
210  void addContour(RigidContour *contour);
211 
215  void setFrameForKinematics(Frame *frame);
216 
217  void setFrameForInertiaTensor(Frame *frame);
218 
219  void setOpenMBVRigidBody(const std::shared_ptr<OpenMBV::RigidBody> &body);
220  void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
221  const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
222 
223  void initializeUsingXML(xercesc::DOMElement *element) override;
224 
225  fmatvec::MatV& getJRel(int i=0, bool check=true) { assert((not check) or (not updGJ)); return JRel[i]; }
226  fmatvec::VecV& getjRel(bool check=true) { assert((not check) or (not updGJ)); return jRel; }
227  void setqRel(const fmatvec::VecV &q);
228  void setuRel(const fmatvec::VecV &u);
229  void setJRel(const fmatvec::MatV &J);
230  void setjRel(const fmatvec::VecV &j);
231 
232  bool transformCoordinates() const { return fTR!=nullptr; }
233 
234  void resetUpToDate() override;
235  void resetPositionsUpToDate() override;
236  void resetVelocitiesUpToDate() override;
237  void resetJacobiansUpToDate() override;
238  void resetGyroscopicAccelerationsUpToDate() override;
239  const fmatvec::VecV& evalqTRel() { if(updq) updateGeneralizedPositions(); return qTRel; }
240  const fmatvec::VecV& evalqRRel() { if(updq) updateGeneralizedPositions(); return qRRel; }
241  const fmatvec::VecV& evaluTRel() { if(updu) updateGeneralizedVelocities(); return uTRel; }
242  const fmatvec::VecV& evaluRRel() { if(updu) updateGeneralizedVelocities(); return uRRel; }
243  const fmatvec::VecV& evalqdTRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdTRel; }
244  const fmatvec::VecV& evalqdRRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdRRel; }
245  const fmatvec::MatV& evalJRel(int j=0) { if(updGJ) updateGeneralizedJacobians(); return JRel[j]; }
246  const fmatvec::VecV& evaljRel() { if(updGJ) updateGeneralizedJacobians(); return jRel; }
247 
248  fmatvec::VecV& getqTRel(bool check=true) { assert((not check) or (not updq)); return qTRel; }
249  fmatvec::VecV& getqRRel(bool check=true) { assert((not check) or (not updq)); return qRRel; }
250  fmatvec::VecV& getuTRel(bool check=true) { assert((not check) or (not updu)); return uTRel; }
251  fmatvec::VecV& getuRRel(bool check=true) { assert((not check) or (not updu)); return uRRel; }
252  fmatvec::VecV& getqdTRel(bool check=true) { assert((not check) or (not updqd)); return qdTRel; }
253  fmatvec::VecV& getqdRRel(bool check=true) { assert((not check) or (not updqd)); return qdRRel; }
254 
255  void setUpdateByReference(bool updateByReference_) { updateByReference = updateByReference_; }
256 
257  InverseKineticsJoint* getJoint() { return joint; }
258 
259  protected:
263  double m{0};
264 
268  fmatvec::SymMat3 SThetaS, WThetaS;
269 
271 
272  fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
273 
277  fmatvec::SqrMat3 APK;
278 
282  fmatvec::Vec3 PrPK, WrPK;
283 
287  fmatvec::Vec3 WvPKrel, WomPK;
288 
289  Function<fmatvec::MatV(fmatvec::VecV)> *fTR{nullptr};
290 
294  Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK{nullptr};
295 
299  Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK{nullptr};
300 
301 #ifndef SWIG
302  void (RigidBody::*updateJacobians_[3])(Frame *frame);
303 #endif
304 
307 
308  fmatvec::MatV JRel[2];
309  fmatvec::VecV jRel;
310 
311  fmatvec::VecV qTRel, qRRel, uTRel, uRRel, qdTRel, qdRRel;
312  fmatvec::Mat3xV PJTT, PJRR;
313 
314  Constraint *constraint{nullptr};
315 
316  Frame *frameForJacobianOfRotation{nullptr};
317 
318  Frame *frameForInertiaTensor{nullptr};
319 
320  fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
321 
322  bool translationDependentRotation{false};
323  bool constJT{false};
324  bool constJR{false};
325  bool constjT{false};
326  bool constjR{false};
327 
328  bool updPjb{true};
329  bool updGJ{true};
330  bool updWTS{true};
331  bool updateByReference{true};
332 
333  Frame Z;
334 
335  GeneralizedVelocityOfRotation generalizedVelocityOfRotation{derivativeOfGeneralizedPositionOfRotation};
336 
337  InverseKineticsJoint *joint;
338 
339  private:
344  };
345 
346 }
347 
348 #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:263
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:343
void setUpInverseKinetics() override
TODO.
Definition: rigid_body.cc:285
InitStage
The stages of the initialization.
Definition: element.h:64
FixedRelativeFrame * C
Definition: rigid_body.h:306
fmatvec::SymMat3 SThetaS
inertia tensor with respect to centre of gravity in centre of gravity and world Frame
Definition: rigid_body.h:268
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:277
std::string name
name of element
Definition: element.h:251
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:299
void plot() override
plots time dependent data
Definition: rigid_body.cc:298
~RigidBody() override
destructor
Definition: rigid_body.cc:71
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:117
Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: rigid_body.h:294
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system
Definition: rigid_body.h:287
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:282
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
namespace MBSim
Definition: bilateral_constraint.cc:29