rigid_body.h

/* Copyright (C) 2004-2014 MBSim Development Team
 *
 * This library is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation; either 
 * version 2.1 of the License, or (at your option) any later version. 
 *  
 * This library is distributed in the hope that it will be useful, 
 * but WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
 * Lesser General Public License for more details. 
 *  
 * You should have received a copy of the GNU Lesser General Public 
 * License along with this library; if not, write to the Free Software 
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
 *
 * Contact: martin.o.foerg@googlemail.com
 */
 
#ifndef _RIGID_BODY_H_
#define _RIGID_BODY_H_
 
#include "mbsim/objects/body.h"
#include "mbsim/frames/frame.h"
#include "mbsim/functions/time_dependent_function.h"
#include "mbsim/functions/state_dependent_function.h"
 
namespace MBSim {
 
  class Frame;
  class RigidContour;
  class FixedRelativeFrame;
  class Constraint;
  class InverseKineticsJoint;
 
  class RigidBody : public Body {
    public:
      enum GeneralizedVelocityOfRotation {
        derivativeOfGeneralizedPositionOfRotation=0,
        coordinatesOfAngularVelocityWrtFrameOfReference,
        coordinatesOfAngularVelocityWrtFrameForKinematics,
        unknown
      };
 
      RigidBody(const std::string &name="");
 
      ~RigidBody() override;
 
      void addDependency(Constraint* constraint_);
 
      void updateqd() override;
      void updateT() override;
      void updateh(int j=0) override;
      void updateM() override;
      void updateInertiaTensor();
      void updateGeneralizedPositions() override;
      void updateGeneralizedVelocities() override;
      void updateDerivativeOfGeneralizedPositions() override;
      void updateGeneralizedAccelerations() override;
      void updateGeneralizedJacobians(int j=0);
      void updatePositions();
      void updateVelocities();
      void updateJacobians() override;
      void updateGyroscopicAccelerations();
      void updatePositions(Frame *frame) override;
      void updateVelocities(Frame *frame) override;
      void updateAccelerations(Frame *frame) override;
      void updateJacobians(Frame *frame, int j=0) override { (this->*updateJacobians_[j])(frame); }
      void updateGyroscopicAccelerations(Frame *frame) override;
      void updateJacobians0(Frame *frame);
      void updateJacobians1(Frame *frame) { }
      void updateJacobians2(Frame *frame);
 
      void calcSize() override;
      void calcqSize() override;
      void calcuSize(int j=0) override;
      void sethSize(int hSize_, int i=0) override;
      void sethInd(int hInd_, int i=0) override;
 
      /* INHERITED INTERFACE OF OBJECT */
      void init(InitStage stage, const InitConfigSet &config) override;
      void setUpInverseKinetics() override;
      /*****************************************************/
 
      /* INHERITED INTERFACE OF ELEMENT */
      void plot() override;
      /*****************************************************/
 
      /* GETTER / SETTER */
 
      // NOTE: we can not use a overloaded setTranslation here due to restrictions in XML but define them for convinience in c++
      void setGeneralTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) {
        fPrPK = fPrPK_;
        fPrPK->setParent(this);
        fPrPK->setName("GeneralTranslation");
      }
      void setTimeDependentTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) {
        setGeneralTranslation(new TimeDependentFunction<fmatvec::Vec3>(fPrPK_));
      }
      void setStateDependentTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) {
        setGeneralTranslation(new StateDependentFunction<fmatvec::Vec3>(fPrPK_));
      }
      void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK_) { setGeneralTranslation(fPrPK_); }
      void setTranslation(Function<fmatvec::Vec3(double)> *fPrPK_) { setTimeDependentTranslation(fPrPK_); }
      void setTranslation(Function<fmatvec::Vec3(fmatvec::VecV)> *fPrPK_) { setStateDependentTranslation(fPrPK_); }
 
      // NOTE: we can not use a overloaded setRotation here due to restrictions in XML but define them for convinience in c++
      void setGeneralRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) {
        fAPK = fAPK_;
        fAPK->setParent(this);
        fAPK->setName("GeneralRotation");
      }
      void setTimeDependentRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setGeneralRotation(new TimeDependentFunction<fmatvec::RotMat3>(fAPK_)); }
      void setStateDependentRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setGeneralRotation(new StateDependentFunction<fmatvec::RotMat3>(fAPK_)); }
      void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV, double)>* fAPK_) { setGeneralRotation(fAPK_); }
      void setRotation(Function<fmatvec::RotMat3(double)>* fAPK_) { setTimeDependentRotation(fAPK_); }
      void setRotation(Function<fmatvec::RotMat3(fmatvec::VecV)>* fAPK_) { setStateDependentRotation(fAPK_); }
 
      void setTranslationDependentRotation(bool dep) { translationDependentRotation = dep; }
      void setGeneralizedVelocityOfRotation(GeneralizedVelocityOfRotation generalizedVelocityOfRotation_) { generalizedVelocityOfRotation = generalizedVelocityOfRotation_; }
 
      Function<fmatvec::Vec3(fmatvec::VecV, double)>* getTranslation() { return fPrPK; }
      Function<fmatvec::RotMat3(fmatvec::VecV, double)>* getRotation() { return fAPK; }
 
      void setMass(double m_) { m = m_; }
      double getMass() const { return m; }
      Frame* getFrameForKinematics() { return &Z; };
      FixedRelativeFrame* getFrameC() { return C; };
 
      const fmatvec::Vec3& evalGlobalRelativePosition() { if(updPos) updatePositions(); return WrPK; }
      const fmatvec::Vec3& evalGlobalRelativeVelocity() { if(updVel) updateVelocities(); return WvPKrel; }
      const fmatvec::Vec3& evalGlobalRelativeAngularVelocity() { if(updVel) updateVelocities(); return WomPK; }
      const fmatvec::SymMat3& evalGlobalInertiaTensor() { if(updWTS) updateInertiaTensor(); return WThetaS; }
      const fmatvec::Mat3xV& evalPJTT() { if(updPJ) updateJacobians(); return PJTT; }
      const fmatvec::Mat3xV& evalPJRR() { if(updPJ) updateJacobians(); return PJRR; }
      const fmatvec::Vec3& evalPjhT() { if(updPJ) updateJacobians(); return PjhT; }
      const fmatvec::Vec3& evalPjhR() { if(updPJ) updateJacobians(); return PjhR; }
      const fmatvec::Vec3& evalPjbT() { if(updPjb) updateGyroscopicAccelerations(); return PjbT; }
      const fmatvec::Vec3& evalPjbR() { if(updPjb) updateGyroscopicAccelerations(); return PjbR; }
 
      fmatvec::SymMat3& getGlobalInertiaTensor(bool check=true) {  assert((not check) or (not updWTS)); return WThetaS; }
      fmatvec::Vec3& getGlobalRelativeVelocity(bool check=true) { assert((not check) or (not updVel)); return WvPKrel; }
      fmatvec::SqrMat3& getRelativeOrientation(bool check=true) { assert((not check) or (not updPos)); return APK; }
      fmatvec::Vec3& getPjbT(bool check=true) { assert((not check) or (not updPjb)); return PjbT; }
      fmatvec::Vec3& getPjbR(bool check=true) { assert((not check) or (not updPjb)); return PjbR; }
 
      void setInertiaTensor(const fmatvec::SymMat3& RThetaR) { SThetaS = RThetaR; }
 
      const fmatvec::SymMat3& getInertiaTensor() const {return SThetaS;}
      fmatvec::SymMat3& getInertiaTensor() {return SThetaS;}
 
      void setFrameForKinematics(Frame *frame);
 
      void setFrameForInertiaTensor(Frame *frame);
 
      void setOpenMBVRigidBody(const std::shared_ptr<OpenMBV::RigidBody> &body);
      void setOpenMBVFrameOfReference(Frame * frame) {openMBVFrame=frame; }
      const Frame* getOpenMBVFrameOfReference() const {return openMBVFrame; }
 
       void initializeUsingXML(xercesc::DOMElement *element) override;
 
      fmatvec::MatV& getJRel(int i=0, bool check=true) { assert((not check) or (not updGJ)); return JRel[i]; }
      fmatvec::VecV& getjRel(bool check=true) { assert((not check) or (not updGJ)); return jRel; }
      void setqRel(const fmatvec::VecV &q);
      void setuRel(const fmatvec::VecV &u);
      void setJRel(const fmatvec::MatV &J);
      void setjRel(const fmatvec::VecV &j);
 
      bool transformCoordinates() const { return fTR!=nullptr; }
 
      void resetUpToDate() override;
      void resetPositionsUpToDate() override;
      void resetVelocitiesUpToDate() override;
      void resetJacobiansUpToDate() override;
      void resetGyroscopicAccelerationsUpToDate() override;
      const fmatvec::VecV& evalqTRel() { if(updq) updateGeneralizedPositions(); return qTRel; }
      const fmatvec::VecV& evalqRRel() { if(updq) updateGeneralizedPositions(); return qRRel; }
      const fmatvec::VecV& evaluTRel() { if(updu) updateGeneralizedVelocities(); return uTRel; }
      const fmatvec::VecV& evaluRRel() { if(updu) updateGeneralizedVelocities(); return uRRel; }
      const fmatvec::VecV& evalqdTRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdTRel; }
      const fmatvec::VecV& evalqdRRel() { if(updqd) updateDerivativeOfGeneralizedPositions(); return qdRRel; }
      const fmatvec::MatV& evalJRel(int j=0) { if(updGJ) updateGeneralizedJacobians(); return JRel[j]; }
      const fmatvec::VecV& evaljRel() { if(updGJ) updateGeneralizedJacobians(); return jRel; }
 
      fmatvec::VecV& getqTRel(bool check=true) { assert((not check) or (not updq)); return qTRel; }
      fmatvec::VecV& getqRRel(bool check=true) { assert((not check) or (not updq)); return qRRel; }
      fmatvec::VecV& getuTRel(bool check=true) { assert((not check) or (not updu)); return uTRel; }
      fmatvec::VecV& getuRRel(bool check=true) { assert((not check) or (not updu)); return uRRel; }
      fmatvec::VecV& getqdTRel(bool check=true) { assert((not check) or (not updqd)); return qdTRel; }
      fmatvec::VecV& getqdRRel(bool check=true) { assert((not check) or (not updqd)); return qdRRel; }
 
      void setUpdateByReference(bool updateByReference_) { updateByReference = updateByReference_; }
 
      InverseKineticsJoint* getJoint() { return joint; }
 
      void setDynamicSystemSolver(DynamicSystemSolver* sys) override;
 
    protected:
      double m{0};
 
      fmatvec::SymMat3 SThetaS, WThetaS;
 
      FixedRelativeFrame *K;
 
      fmatvec::Vec3 PjhT, PjhR, PjbT, PjbR;
 
      fmatvec::SqrMat3 APK;
 
      fmatvec::Vec3 PrPK, WrPK;
 
      fmatvec::Vec3 WvPKrel, WomPK;
 
      Function<fmatvec::MatV(fmatvec::VecV)> *fTR{nullptr};
 
      Function<fmatvec::Vec3(fmatvec::VecV, double)> *fPrPK{nullptr};
 
      Function<fmatvec::RotMat3(fmatvec::VecV, double)> *fAPK{nullptr};
 
#ifndef SWIG
      void (RigidBody::*updateJacobians_[3])(Frame *frame);
#endif
 
      FixedRelativeFrame *C;
 
      fmatvec::MatV JRel[2];
      fmatvec::VecV jRel;
 
      fmatvec::VecV qTRel, qRRel, uTRel, uRRel, qdTRel, qdRRel;
      fmatvec::Mat3xV PJTT, PJRR;
 
      Constraint *constraint{nullptr};
 
      Frame *frameForJacobianOfRotation{nullptr};
 
      Frame *frameForInertiaTensor{nullptr};
 
      fmatvec::Range<fmatvec::Var,fmatvec::Var> iqT, iqR, iuT, iuR;
 
      bool translationDependentRotation{false};
      bool constJT{false};
      bool constJR{false};
      bool constjT{false};
      bool constjR{false};
 
      bool updPjb{true};
      bool updGJ{true};
      bool updWTS{true};
      bool updateByReference{true};
 
      Frame Z;
 
      GeneralizedVelocityOfRotation generalizedVelocityOfRotation{derivativeOfGeneralizedPositionOfRotation};
      
      InverseKineticsJoint *joint;
 
    private:
      Frame * openMBVFrame;
  };
 
}
 
#endif