MBSim

Project Page
kinematics.h
1 /* Copyright (C) 2004-2009 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 _KINEMATICS_H_
21 #define _KINEMATICS_H_
22 
23 #include "fmatvec.h"
24 #include "mbsim/utils/function.h"
25 
26 namespace MBSim {
27 
34  class Translation : public Function2<fmatvec::Vec,fmatvec::Vec,double> {
35  public:
40 
44  virtual ~Translation() {}
45 
46  /* INTERFACE FOR DERIVED CLASSES */
50  virtual int getqSize() const = 0;
51 
57  virtual fmatvec::Vec operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
58 
59  virtual void initializeUsingXML(TiXmlElement *element) {}
60  /***************************************************/
61  };
62 
69  class LinearTranslation : public Translation {
70  public:
75 
80  LinearTranslation(const fmatvec::Mat &PJT_) { PJT = PJT_; }
81 
82  /* INTERFACE OF TRANSLATION */
83  virtual int getqSize() const { throw; return 0; }
84  virtual fmatvec::Vec operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return PJT*q(0,PJT.cols()-1); }
85  virtual void initializeUsingXML(TiXmlElement *element);
86  /***************************************************/
87 
88  /* GETTER / SETTER */
89  const fmatvec::Mat& getTranslationVectors() const { return PJT; }
90 
95  void setTranslationVectors(const fmatvec::Mat &PJT_) { PJT = PJT_; }
96  /***************************************************/
97 
98  private:
102  fmatvec::Mat PJT;
103  };
104 
112  public:
117 
123 
127  virtual ~TimeDependentTranslation() { delete pos; pos = 0; }
128 
129  /* INTERFACE OF TRANSLATION */
130  virtual int getqSize() const { return 0; }
131  virtual fmatvec::Vec operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return (*pos)(t); }
132  virtual void initializeUsingXML(TiXmlElement *element);
133  /***************************************************/
134 
135  /* GETTER / SETTER */
140  /***************************************************/
141 
142  private:
147  };
148 
155  class Rotation : public Function2<fmatvec::SqrMat,fmatvec::Vec,double> {
156  public:
160  Rotation() {}
161 
165  virtual ~Rotation() {}
166 
167  /* INTERFACE FOR DERIVED CLASSES */
171  virtual int getqSize() const = 0;
172 
178  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
179 
180  virtual void initializeUsingXML(TiXmlElement *element) {}
181  /***************************************************/
182  };
183 
185  public:
190 
191  virtual int getqSize() const { throw; return 0; }
192 
193  protected:
197  fmatvec::SqrMat APK;
198  };
199 
205  public:
210 
211  /* INTERFACE OF ROTATION */
212  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
213  virtual void initializeUsingXML(TiXmlElement *element) {}
214  };
215 
221  public:
226 
227  /* INTERFACE OF ROTATION */
228  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
229  virtual void initializeUsingXML(TiXmlElement *element) {}
230  };
231 
238  public:
243 
244  /* INTERFACE OF ROTATION */
245  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
246  virtual void initializeUsingXML(TiXmlElement *element) {}
247  };
248 
256  public:
261 
266  RotationAboutFixedAxis(const fmatvec::Vec &a_) : RotationAboutOneAxis() { a = a_; }
267 
268  /* INTERFACE OF ROTATION */
269  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
270  virtual void initializeUsingXML(TiXmlElement *element);
271  /***************************************************/
272 
273  /* GETTER / SETTER */
274  const fmatvec::Vec& getAxisOfRotation() const { return a; }
275  void setAxisOfRotation(const fmatvec::Vec& a_) { a = a_; }
276  /***************************************************/
277 
278  protected:
282  fmatvec::Vec a;
283  };
284 
293  public:
298 
305 
309  virtual ~TimeDependentRotationAboutFixedAxis() { delete rot; rot = 0; delete angle; angle = 0; }
310 
311 
312  /* INTERFACE OF ROTATION */
313  virtual int getqSize() const { return 0; }
314  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
315  virtual void initializeUsingXML(TiXmlElement *element);
316  /***************************************************/
317 
318  /* GETTER / SETTER */
319  void setRotationalFunction(Function1<double, double> *angle_) { angle = angle_; }
320  const fmatvec::Vec& getAxisOfRotation() const { return rot->getAxisOfRotation(); }
321  void setAxisOfRotation(const fmatvec::Vec& a_) { rot->setAxisOfRotation(a_); }
322  /***************************************************/
323 
324  private:
329 
334  };
335 
337  public:
342 
343  virtual int getqSize() const { throw; return 0; }
344 
345  protected:
349  fmatvec::SqrMat APK;
350  };
351 
359  public:
364 
365  /* INTERFACE OF ROTATION */
366  virtual int getqSize() const { return 2; }
367  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
368  virtual void initializeUsingXML(TiXmlElement *element) {};
369  /***************************************************/
370  };
371 
379  public:
384 
385  /* INTERFACE OF ROTATION */
386  virtual int getqSize() const { return 2; }
387  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
388  virtual void initializeUsingXML(TiXmlElement *element) {};
389  /***************************************************/
390  };
391 
393  public:
398 
399  virtual int getqSize() const { throw; return 0; }
400 
401  protected:
405  fmatvec::SqrMat APK;
406  };
407 
415  public:
420 
421  /* INTERFACE OF ROTATION */
422  virtual int getqSize() const { return 3; }
423  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
424  virtual void initializeUsingXML(TiXmlElement *element) {};
425  /***************************************************/
426  };
427 
434  public:
439 
440  /* INTERFACE OF ROTATION */
441  virtual int getqSize() const { return 3; }
442  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
443  virtual void initializeUsingXML(TiXmlElement *element) {};
444  /***************************************************/
445  };
446 
455  public:
460 
466 
470  virtual ~TimeDependentCardanAngles() { delete rot; rot = 0; delete angle; angle = 0; }
471 
472  /* INTERFACE OF ROTATION */
473  virtual int getqSize() const { return 0; }
474  virtual fmatvec::SqrMat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
475  virtual void initializeUsingXML(TiXmlElement *element);
476  /***************************************************/
477 
478  private:
483 
488  };
489 
497  class Jacobian : public Function2<fmatvec::Mat,fmatvec::Vec,double> {
498  public:
502  Jacobian() {}
503 
507  virtual ~Jacobian() {}
508 
509  /* INTERFACE FOR DERIVED CLASSES */
513  virtual int getuSize() const = 0;
514 
521  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) = 0;
522 
523  virtual void initializeUsingXML(TiXmlElement *element) {};
524  /***************************************************/
525  };
526 
533  class ConstantJacobian : public Jacobian {
534  public:
538  ConstantJacobian(const fmatvec::Mat &J_) { J = J_; }
539 
540  ConstantJacobian() {}
541 
545  virtual ~ConstantJacobian() {}
546 
547  /* INTERFACE OF JACOBIAN */
548  virtual int getuSize() const { return J.cols(); }
549  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL) { return J; }
550  virtual void initializeUsingXML(TiXmlElement *element);
551  /***************************************************/
552 
553  private:
557  fmatvec::Mat J;
558  };
559 
566  public:
571  JRotationAboutAxesXY(int uSize_) : uSize(uSize_), J(3,uSize) {}
572 
573  /* INTERFACE OF JACOBIAN */
574  int getuSize() const { return uSize; }
575  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
576  /***************************************************/
577 
578  private:
582  int uSize;
583 
587  fmatvec::Mat J;
588  };
589 
596  public:
601  JRotationAboutAxesYZ(int uSize_) : uSize(uSize_), J(3,uSize) {}
602 
603  /* INTERFACE OF JACOBIAN */
604  int getuSize() const { return uSize; }
605  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
606  /***************************************************/
607 
608  private:
612  int uSize;
613 
617  fmatvec::Mat J;
618  };
619 
626  class TCardanAngles : public Jacobian {
627  public:
633  TCardanAngles(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {}
634 
635  /* INTERFACE OF JACOBIAN */
636  int getuSize() const { return uSize; }
637  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
638  /***************************************************/
639 
640  private:
644  int qSize, uSize;
645 
649  fmatvec::Mat T;
650  };
651 
657  class TEulerAngles : public Jacobian {
658  public:
664  TEulerAngles(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {
665  int iq = qSize-1;
666  int iu = uSize-1;
667  T(iq-2,iu) = 1;
668  T(iq,iu) = 0;
669  }
670 
671  /* INTERFACE OF JACOBIAN */
672  int getuSize() const { return uSize; }
673  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
674  /***************************************************/
675 
676  private:
680  int qSize, uSize;
681 
685  fmatvec::Mat T;
686  };
687 
694  class TCardanAngles2 : public Jacobian {
695  public:
701  TCardanAngles2(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {}
702 
703  /* INTERFACE OF JACOBIAN */
704  int getuSize() const { return uSize; }
705  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
706  /***************************************************/
707 
708  private:
712  int qSize, uSize;
713 
717  fmatvec::Mat T;
718  };
719 
725  class TEulerAngles2 : public Jacobian {
726  public:
732  TEulerAngles2(int qSize_, int uSize_) : qSize(qSize_), uSize(uSize_), T(qSize,uSize,fmatvec::EYE) {
733  }
734 
735  /* INTERFACE OF JACOBIAN */
736  int getuSize() const { return uSize; }
737  virtual fmatvec::Mat operator()(const fmatvec::Vec &q, const double &t, const void * =NULL);
738  /***************************************************/
739 
740  private:
744  int qSize, uSize;
745 
749  fmatvec::Mat T;
750  };
751 
756  class JdRotationAboutAxesXY : public Function3<fmatvec::Mat,fmatvec::Vec,fmatvec::Vec,double> {
757  public:
762  JdRotationAboutAxesXY(int uSize_) : uSize(uSize_), Jd(3,uSize) {}
763 
764  virtual fmatvec::Mat operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void*);
765 
766  private:
770  int uSize;
771 
775  fmatvec::Mat Jd;
776  };
777 
782  class JdRotationAboutAxesYZ : public Function3<fmatvec::Mat,fmatvec::Vec,fmatvec::Vec,double> {
783  public:
788  JdRotationAboutAxesYZ(int uSize_) : uSize(uSize_), Jd(3,uSize) {}
789 
790  virtual fmatvec::Mat operator()(const fmatvec::Vec &qd, const fmatvec::Vec& q, const double& t, const void*);
791 
792  private:
796  int uSize;
797 
801  fmatvec::Mat Jd;
802  };
803 
1019 
1020 }
1021 
1022 #endif /* _KINEMATICS_H_ */
1023