mbsimflexiblebody  4.0.0
MBSim Flexible Body Module
generic_flexible_ffr_body.h
1 /* Copyright (C) 2004-2015 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 _GENERIC_FLEXIBLE_FFR_BODY_H_
21 #define _GENERIC_FLEXIBLE_FFR_BODY_H_
22 
23 #include "mbsimFlexibleBody/node_based_body.h"
24 #include "mbsimFlexibleBody/utils/openmbv_utils.h"
25 #include "mbsim/functions/time_dependent_function.h"
26 #include "mbsim/functions/state_dependent_function.h"
27 #include "mbsim/utils/index.h"
28 
29 namespace MBSim {
30  class Frame;
31 }
32 
33 namespace MBSimFlexibleBody {
34 
35  template<typename Dep>
36  struct BaseType;
37 
38  template<int N>
39  struct BaseType<fmatvec::Vector<fmatvec::Fixed<N>, double>> {
40  using type = fmatvec::VecV;
41  };
42 
43  template<>
44  struct BaseType<fmatvec::SqrMat3> {
45  using type = fmatvec::MatVx3;
46  };
47 
48  template<>
49  struct BaseType<fmatvec::SqrMatV> {
50  using type = fmatvec::MatV;
51  };
52 
53  template<int N>
54  struct BaseType<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<N>, fmatvec::Var, double>> {
55  using type = fmatvec::MatV;
56  };
57 
63 
64  public:
65  enum GeneralizedVelocityOfRotation {
66  derivativeOfGeneralizedPositionOfRotation=0,
67  coordinatesOfAngularVelocityWrtFrameOfReference,
68  coordinatesOfAngularVelocityWrtFrameForKinematics,
69  unknown
70  };
71 
72  GenericFlexibleFfrBody(const std::string &name="");
76  ~GenericFlexibleFfrBody() override;
77 
78  void updateqd() override;
79  void updateT() override;
80  void updateh(int j=0) override;
81  void updateM() override;
82  void updateGeneralizedPositions() override;
83  void updateGeneralizedVelocities() override;
84  void updateDerivativeOfGeneralizedPositions() override;
85  void updateGeneralizedAccelerations() override;
86  void updatePositions();
87  void updateVelocities();
88  void updateAccelerations();
89  void updateJacobians() override;
90  void updateGyroscopicAccelerations();
91  void updatePositions(MBSim::Frame *frame) override;
92  void updateVelocities(MBSim::Frame *frame) override;
93  void updateAccelerations(MBSim::Frame *frame) override;
94  void updateJacobians(MBSim::Frame *frame, int j=0) override { (this->*updateJacobians_[j])(frame); }
95  void updateGyroscopicAccelerations(MBSim::Frame *frame) override;
96  void updateJacobians0(MBSim::Frame *frame);
97  void updateJacobians1(MBSim::Frame *frame) { }
98  void updateMb();
99  void updatehb();
100  void updateKJ(int j=0) { (this->*updateKJ_[j])(); }
101  void updateKJ0();
102  void updateKJ1();
103  void (GenericFlexibleFfrBody::*updateKJ_[2])();
104  void calcSize() override;
105  void calcqSize() override;
106  void calcuSize(int j=0) override;
107 
108  /* INHERITED INTERFACE OF OBJECT */
109  void init(InitStage stage, const MBSim::InitConfigSet &config) override;
110  void setUpInverseKinetics() override;
111  /*****************************************************/
112 
113  /* INHERITED INTERFACE OF ELEMENT */
114  void plot() override;
115  /*****************************************************/
116 
117  /* GETTER / SETTER */
118  int getNumberOfModeShapes() { if(updSize) calcSize(); return ne; }
119 
120  // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
125  void setGeneralTranslation(MBSim::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
126  fPrPK = fPrPK_;
127  fPrPK->setParent(this);
128  fPrPK->setName("GeneralTranslation");
129  }
134  void setTimeDependentTranslation(MBSim::Function<fmatvec::Vec3(double)> *fPrPK_) {
136  }
141  void setStateDependentTranslation(MBSim::Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
143  }
144  void setTranslation(MBSim::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
145  void setTranslation(MBSim::Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
146  void setTranslation(MBSim::Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
147 
148  // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
153  void setGeneralRotation(MBSim::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
154  fAPK = fAPK_;
155  fAPK->setParent(this);
156  fAPK->setName("GeneralRotation");
157  }
168  void setRotation(MBSim::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
169  void setRotation(MBSim::Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
170  void setRotation(MBSim::Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
171 
172  void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
173  void setGeneralizedVelocityOfRotation(GeneralizedVelocityOfRotation generalizedVelocityOfRotation_) { generalizedVelocityOfRotation = generalizedVelocityOfRotation_; }
174 
179  MBSim::Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
184  MBSim::Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
185 
186  MBSim::Frame* getFrameK() { return K; };
187 
188  const fmatvec::Vec3& getNodalRelativePosition(int i) const { return KrKP[i]; }
189  const fmatvec::SqrMat3& getNodalRelativeOrientation(int i) const { return ARP[i]; }
190  const fmatvec::Mat3xV& getNodalShapeMatrixOfTranslation(int i) const { return Phi[i]; }
191  const fmatvec::Mat3xV& getNodalShapeMatrixOfRotation(int i) const { return Psi[i]; }
192 
195 
196  void initializeUsingXML(xercesc::DOMElement *element) override;
197 
198  void setqRel(const fmatvec::VecV &q);
199  void setuRel(const fmatvec::VecV &u);
200 
201  bool transformCoordinates() const {return fTR!=nullptr;}
202 
203  void resetUpToDate() override;
204  const fmatvec::VecV& evalqTRel() { if(updq) updateGeneralizedPositions(); return qTRel; }
205  const fmatvec::VecV& evalqRRel() { if(updq) updateGeneralizedPositions(); return qRRel; }
206  const fmatvec::VecV& evalqERel() { if(updq) updateGeneralizedPositions(); return qERel; }
207  const fmatvec::VecV& evaluTRel() { if(updu) updateGeneralizedVelocities(); return uTRel; }
208  const fmatvec::VecV& evaluRRel() { if(updu) updateGeneralizedVelocities(); return uRRel; }
209  const fmatvec::VecV& evaluERel() { if(updu) updateGeneralizedVelocities(); return uERel; }
210  const fmatvec::VecV& evalqdTRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdTRel; }
211  const fmatvec::VecV& evalqdRRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdRRel; }
212  const fmatvec::VecV& evalqdERel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdERel; }
213  const fmatvec::VecV& evaludERel() { if(updud) updateGeneralizedAccelerations(); return udERel; }
214  const fmatvec::Vec3& evalGlobalRelativePosition() { if(updPos) updatePositions(); return WrPK; }
215  const fmatvec::Vec3& evalGlobalRelativeVelocity() { if(updVel) updateVelocities(); return WvPKrel; }
216  const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() { if(updVel) updateVelocities(); return WomPK; }
217  const fmatvec::Mat3xV& evalPJTT() { if(updPJ) updateJacobians(); return PJTT; }
218  const fmatvec::Mat3xV& evalPJRR() { if(updPJ) updateJacobians(); return PJRR; }
219  const fmatvec::Vec3& evalPjhT() { if(updPJ) updateJacobians(); return PjhT; }
220  const fmatvec::Vec3& evalPjhR() { if(updPJ) updateJacobians(); return PjhR; }
221  const fmatvec::Vec3& evalPjbT() { if(updPjb) updateGyroscopicAccelerations(); return PjbT; }
222  const fmatvec::Vec3& evalPjbR() { if(updPjb) updateGyroscopicAccelerations(); return PjbR; }
223  const fmatvec::SymMatV& evalMb() { if(updMb) updateMb(); return M_; }
224  const fmatvec::VecV& evalhb() { if(updMb) updateMb(); return h_; }
225  const fmatvec::MatV& evalKJ(int j=0) { if(updKJ[j]) updateKJ(j); return KJ[j]; }
226  const fmatvec::VecV& evalKi() { if(updKJ[0]) updateKJ(0); return Ki; }
227  const fmatvec::Vec3& evalGlobalRelativePosition(int i) { if(updNodalPos[i]) updatePositions(i); return WrRP[i]; }
228  const fmatvec::Vec3& evalGlobalRelativeAngularVelocity(int i) { if(updNodalVel[i]) updateVelocities(i); return Womrel[i]; }
229 
230  fmatvec::Vec3& getGlobalRelativeVelocity(bool check=true) { assert((not check) or (not updVel)); return WvPKrel; }
231  fmatvec::SqrMat3& getRelativeOrientation(bool check=true) { assert((not check) or (not updPos)); return APK; }
232  fmatvec::Vec3& getPjbT(bool check=true) { assert((not check) or (not updPjb)); return PjbT; }
233  fmatvec::Vec3& getPjbR(bool check=true) { assert((not check) or (not updPjb)); return PjbR; }
234 
235  fmatvec::VecV& getqTRel(bool check=true) { assert((not check) or (not updq)); return qTRel; }
236  fmatvec::VecV& getqRRel(bool check=true) { assert((not check) or (not updq)); return qRRel; }
237  fmatvec::VecV& getqERel(bool check=true) { assert((not check) or (not updq)); return qERel; }
238  fmatvec::VecV& getuTRel(bool check=true) { assert((not check) or (not updu)); return uTRel; }
239  fmatvec::VecV& getuRRel(bool check=true) { assert((not check) or (not updu)); return uRRel; }
240  fmatvec::VecV& getuERel(bool check=true) { assert((not check) or (not updu)); return uERel; }
241  fmatvec::VecV& getqdTRel(bool check=true) { assert((not check) or (not updqd)); return qdTRel; }
242  fmatvec::VecV& getqdRRel(bool check=true) { assert((not check) or (not updqd)); return qdRRel; }
243  fmatvec::VecV& getqdERel(bool check=true) { assert((not check) or (not updqd)); return qdERel; }
244  fmatvec::VecV& getudERel(bool check=true) { assert((not check) or (not updud)); return udERel; }
245 
246  fmatvec::Vec3& getNodalRelativeVelocity(int i, bool check=true) { assert((not check) or (not updNodalVel[i])); return Wvrel[i]; }
247 
248  void updateStresses(int i) override;
249  void updatePositions(int i) override;
250  void updateVelocities(int i) override;
251  void updateAccelerations(int i) override;
252  void updateJacobians(int i, int j=0) override;
253  void updateGyroscopicAccelerations(int i) override;
254 
255  template <class T>
256  static std::vector<T> getCellArray1D(xercesc::DOMElement *element) {
257  std::vector<T> array;
258  xercesc::DOMElement* e=element->getFirstElementChild();
259  if(MBXMLUtils::E(e)->getTagName()==MBSIMFLEX%"ele") {
260  while(e) {
261  array.push_back(MBXMLUtils::E(e)->getText<T>());
262  e=e->getNextElementSibling();
263  }
264  }
265  return array;
266  }
267 
268  template <class T>
269  static std::vector<T> getCellArray1D(int m, const typename BaseType<T>::type &A) {
270  std::vector<T> array;
271  int M = A.rows()/m;
272  int n = A.cols();
273  for(int i=0; i<M; i++)
274  array.push_back(T(A(fmatvec::RangeV(i*m,i*m+m-1),fmatvec::RangeV(0,n-1))));
275  return array;
276  }
277 
278  template <class T>
279  static std::vector<std::vector<T>> getCellArray2D(xercesc::DOMElement *element) {
280  std::vector<std::vector<T>> array;
281  xercesc::DOMElement *e=element->getFirstElementChild();
282  if(MBXMLUtils::E(e)->getTagName()==MBSIMFLEX%"row") {
283  while(e) {
284  array.push_back(std::vector<T>());
285  xercesc::DOMElement *ee=e->getFirstElementChild();
286  while(ee) {
287  array[array.size()-1].push_back(MBXMLUtils::E(ee)->getText<T>());
288  ee=ee->getNextElementSibling();
289  }
290  e=e->getNextElementSibling();
291  }
292  }
293  return array;
294  }
295 
296  template <class T>
297  static std::vector<std::vector<T>> getCellArray2D(int m, int N, const typename BaseType<T>::type &A) {
298  int M = A.rows()/(m*N);
299  int n = A.cols();
300  std::vector<std::vector<T>> array(M);
301  for(int i=0, k=0; i<M; i++) {
302  for(int j=0; j<N; j++) {
303  array[i].push_back(T(A(fmatvec::RangeV(k*m,k*m+m-1),fmatvec::RangeV(0,n-1))));
304  k++;
305  }
306  }
307  return array;
308  }
309 
310  protected:
311  double m{0};
312  fmatvec::Vec3 rdm;
313  fmatvec::SymMat3 rrdm, mmi0;
314  fmatvec::Mat3xV Pdm;
315  std::vector<std::vector<fmatvec::SqrMatV>> PPdm, Knl2;
316  std::vector<std::vector<fmatvec::SqrMatV>> Ke2;
317  std::vector<fmatvec::Mat3xV> rPdm;
318  std::vector<std::vector<fmatvec::SqrMat3>> mmi2, Gr1;
319  std::vector<fmatvec::SqrMatV> Knl1, K0t, K0r, K0om, Ct1, Cr1, Ge, Oe1, Ke1, De1;
320  fmatvec::VecV ksigma0;
321  fmatvec::SqrMatV ksigma1;
322  std::vector<fmatvec::SymMat3> mmi1;
323  fmatvec::MatVx3 Ct0, Cr0;
324  fmatvec::SymMatV Me, Ke0, De0;
325  std::vector<fmatvec::SqrMat3> Gr0;
326  fmatvec::Matrix<fmatvec::General,fmatvec::Var,fmatvec::Fixed<6>,double> Oe0;
327 
328  fmatvec::SqrMat3 Id;
329  std::vector<fmatvec::Vec3> KrKP, WrRP, Wvrel, Womrel;
330  std::vector<fmatvec::SqrMat3> ARP;
331  std::vector<fmatvec::Mat3xV> Phi, Psi;
332  std::vector<std::vector<fmatvec::SqrMatV>> K0F, K0M;
333  std::vector<fmatvec::Vector<fmatvec::Fixed<6>, double>> sigma0;
334  std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double>> sigmahel;
335  std::vector<std::vector<fmatvec::Matrix<fmatvec::General, fmatvec::Fixed<6>, fmatvec::Var, double>> > sigmahen;
336 
337  // Number of mode shapes
338  int ne{0};
339 
340  MBSim::Frame *K;
341 
345  fmatvec::SymMat Mbuf;
346 
347  fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
348 
352  fmatvec::SqrMat3 APK;
353 
357  fmatvec::Vec3 PrPK, WrPK;
358 
362  fmatvec::Vec3 WvPKrel, WomPK;
363 
364  MBSim::Function<fmatvec::MatV(fmatvec::VecV)> *fTR{nullptr};
365 
369  MBSim::Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK{nullptr};
370 
374  MBSim::Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK{nullptr};
375 
376  void (GenericFlexibleFfrBody::*updateJacobians_[2])(MBSim::Frame *frame);
377 
378  fmatvec::Vec aT, aR;
379 
380  fmatvec::VecV qTRel, qRRel, qERel, uTRel, uRRel, uERel, qdTRel, qdRRel, qdERel, udERel;
381  fmatvec::Mat3xV WJTrel, WJRrel, PJTT, PJRR;
382 
383  MBSim::Frame *frameForJacobianOfRotation{nullptr};
384 
385  fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iqE, iuT, iuR, iuE;
386 
387  bool translationDependentRotation{false}, constJT{false}, constJR{false}, constjT{false}, constjR{false};
388 
389  bool updPjb{true}, updGC{true}, updMb{true}, updKJ[2];
390 
391  fmatvec::SymMatV M_;
392  fmatvec::VecV h_;
393  fmatvec::MatV KJ[2];
394  fmatvec::VecV Ki;
395 
396  void assemble();
397  void prefillMassMatrix();
398 
399  GeneralizedVelocityOfRotation generalizedVelocityOfRotation{derivativeOfGeneralizedPositionOfRotation};
400 
401  OpenMBVFlexibleBody::ColorRepresentation ombvColorRepresentation{OpenMBVFlexibleBody::none};
402 
403  std::vector<int> plotNodes, visuNodes;
404 
405  private:
406  double (GenericFlexibleFfrBody::*evalOMBVColorRepresentation[12])(int i);
407  double evalNone(int i) { return 0; }
408  double evalXDisplacement(int i) { return evalNodalDisplacement(i)(0); }
409  double evalYDisplacement(int i) { return evalNodalDisplacement(i)(1); }
410  double evalZDisplacement(int i) { return evalNodalDisplacement(i)(2); }
411  double evalTotalDisplacement(int i) { return fmatvec::nrm2(evalNodalDisplacement(i)); }
412  double evalXXStress(int i) { return evalNodalStress(i)(0); }
413  double evalYYStress(int i) { return evalNodalStress(i)(1); }
414  double evalZZStress(int i) { return evalNodalStress(i)(2); }
415  double evalXYStress(int i) { return evalNodalStress(i)(3); }
416  double evalYZStress(int i) { return evalNodalStress(i)(4); }
417  double evalZXStress(int i) { return evalNodalStress(i)(5); }
418  double evalEquivalentStress(int i);
419  };
420 
421 }
422 
423 #endif
MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> * getTranslation()
get Kinematic for translational motion
Definition: generic_flexible_ffr_body.h:179
void setStateDependentTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV)> *fPrPK_)
set Kinematic for only state dependent translational motion
Definition: generic_flexible_ffr_body.h:141
void setStateDependentRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV)> *fAPK_)
set Kinematic for only state dependent rotational motion
Definition: generic_flexible_ffr_body.h:167
Definition: generic_flexible_ffr_body.h:36
fmatvec::SymMat Mbuf
TODO.
Definition: generic_flexible_ffr_body.h:345
fmatvec::Mat T
fmatvec::Vec3 PrPK
translation from parent to kinematic Frame in parent and world system
Definition: generic_flexible_ffr_body.h:357
fmatvec::Vec3 WvPKrel
translational and angular velocity from parent to kinematic Frame in world system
Definition: generic_flexible_ffr_body.h:362
fmatvec::Vec q
~GenericFlexibleFfrBody() override
destructor
Definition: generic_flexible_ffr_body.cc:88
void setGeneralRotation(MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK_)
set Kinematic for general rotational motion
Definition: generic_flexible_ffr_body.h:153
void setGeneralTranslation(MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_)
set Kinematic for genral translational motion
Definition: generic_flexible_ffr_body.h:125
void addFrame(NodeBasedFrame *frame)
Definition: node_based_body.cc:70
fmatvec::SqrMat3 APK
rotation matrix from kinematic Frame to parent Frame
Definition: generic_flexible_ffr_body.h:352
std::string name
std::vector< Frame * > frame
Generic flexible body using a floating frame of reference formulation.
Definition: generic_flexible_ffr_body.h:62
MBSim::Function< fmatvec::Vec3(fmatvec::VecV, double)> * fPrPK
translation from parent Frame to kinematic Frame in parent system
Definition: generic_flexible_ffr_body.h:369
void setTimeDependentTranslation(MBSim::Function< fmatvec::Vec3(double)> *fPrPK_)
set Kinematic for only time dependent translational motion
Definition: generic_flexible_ffr_body.h:134
virtual void addContour(Contour *contour)
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * fAPK
rotation from kinematic Frame to parent Frame
Definition: generic_flexible_ffr_body.h:374
void setName(const std::string &str)
Definition: node_based_body.h:30
void setTimeDependentRotation(MBSim::Function< fmatvec::RotMat3(double)> *fAPK_)
set Kinematic for only time dependent rotational motion
Definition: generic_flexible_ffr_body.h:162
MBSim::Function< fmatvec::RotMat3(fmatvec::VecV, double)> * getRotation()
get Kinematic for rotational motion
Definition: generic_flexible_ffr_body.h:184
fmatvec::SymMat M