dynamic_system.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 _DYNAMIC_SYSTEM_H_
#define _DYNAMIC_SYSTEM_H_
 
#include "mbsim/element.h"
#include "mbsim/mbsim_event.h"
 
namespace H5 {
  class Group;
}
 
namespace MBSim {
  class Frame;
  class FixedRelativeFrame;
  class Contour;
  class RigidContour;
  class Object;
  class Link;
  class Constraint;
  class ModellingInterface;
  class Contact;
  class Observer;
 
  class DynamicSystem : public Element {
    public:
      DynamicSystem(const std::string &name);
 
      ~DynamicSystem() override;
 
      /* INTERFACE FOR DERIVED CLASSES */
      virtual void updateT();
      virtual void updateh(int k=0);
      virtual void updateM();
      virtual void updateLLM();
      virtual void updatedq();
      virtual void updatedu();
      virtual void updatedx();
      virtual void updateqd();
      virtual void updateud();
      virtual void updatexd();
      virtual void updatezd();
      virtual void updatewb();
      virtual void updateW(int j=0);
      virtual void updateV(int j=0);
      virtual void updater(int j=0);
      virtual void updateJrla(int j=0);
      virtual void updateg();
      virtual void updategd();
      virtual void updateStopVector();
      virtual void updateStopVectorParameters();
      virtual void updateLinkStatus();
      virtual void updateLinkStatusReg();
      virtual void updateWInverseKinetics();
      virtual void updatebInverseKinetics();
      virtual void sethSize(int hSize_, int j=0);
      virtual int gethSize(int i=0) const { return hSize[i]; }
      virtual int getqSize() const { return qSize; }
      virtual int getuSize(int i=0) const { return uSize[i]; }
      virtual void calcqSize();
      virtual void calcuSize(int j=0);
      virtual int getuInd(int i=0) { return uInd[i]; }
      virtual void setqInd(int qInd_);
      virtual void setuInd(int uInd_, int j=0);
      virtual void sethInd(int hInd_, int j=0);
      virtual void setxInd(int xInd_);
      virtual const fmatvec::Vec& getq() const { return q; };
      virtual fmatvec::Vec& getq() { return q; };
      virtual const fmatvec::Vec& getu() const { return u; };
      virtual fmatvec::Vec& getu() { return u; };
      void setq(const fmatvec::Vec& q_) { q = q_; }
      void setu(const fmatvec::Vec& u_) { u = u_; }
      void setx(const fmatvec::Vec& x_) { x = x_; }
      void setjsv(const fmatvec::VecInt& jsv_) { jsv = jsv_; }
      void setInternalState(const fmatvec::Vec& internalState) { curis = internalState; nextis = internalState; }
      virtual H5::GroupBase *getPlotGroup() { return plotGroup; }
      std::shared_ptr<OpenMBV::Group> getOpenMBVGrp() override { return openMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getFramesOpenMBVGrp() override { return framesOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getContoursOpenMBVGrp() override { return contoursOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getGroupsOpenMBVGrp() override { return groupsOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getObjectsOpenMBVGrp() override { return objectsOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getLinksOpenMBVGrp() override { return linksOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getConstraintsOpenMBVGrp() override { return constraintsOpenMBVGrp; }
      std::shared_ptr<OpenMBV::Group> getObserversOpenMBVGrp() override { return observersOpenMBVGrp; }
 
      virtual void calcxSize();
      const fmatvec::Vec& getx() const { return x; };
      fmatvec::Vec& getx() { return x; };
      int getxSize() const { return xSize; }
      void updatexRef(fmatvec::Vec &xParent);
      void updatexdRef(fmatvec::Vec &xdParent);
      void updatedxRef(fmatvec::Vec &dxParent);
      void init(InitStage stage, const InitConfigSet &config) override;
      virtual void initz();
      virtual void writez(H5::GroupBase *parent);
      virtual void readz0(H5::GroupBase *parent);
      /*****************************************************/
 
      /* INHERITED INTERFACE OF ELEMENT */
      void setDynamicSystemSolver(DynamicSystemSolver* sys) override;
      void plot() override;
      void plotAtSpecialEvent() override;
      /*****************************************************/
 
      virtual int solveConstraintsFixpointSingle();
 
      virtual int solveImpactsFixpointSingle();
 
      virtual int solveConstraintsGaussSeidel();
 
      virtual int solveImpactsGaussSeidel();
 
      virtual int solveConstraintsRootFinding();
 
      virtual int solveImpactsRootFinding();
 
      virtual int jacobianConstraints();
 
      virtual int jacobianImpacts();
 
      virtual void checkConstraintsForTermination();
 
      virtual void checkImpactsForTermination();
 
      virtual void updaterFactors();
 
      virtual Frame* getFrame(const std::string &name, bool check=true) const;
 
      virtual Contour* getContour(const std::string &name, bool check=true) const;
      /*****************************************************/
 
      /* GETTER / SETTER */
      void setFrameOfReference(Frame *frame) { R = frame; };
      const Frame* getFrameOfReference() const { return R; };
 
      const fmatvec::Mat& getT(bool check=true) const;
      const fmatvec::Vec& geth(int i=0, bool check=true) const;
      const fmatvec::SymMat& getM(bool check=true) const;
      const fmatvec::SymMat& getLLM(bool check=true) const;
      const fmatvec::Vec& getdq(bool check=true) const;
      const fmatvec::Vec& getdu(bool check=true) const;
      const fmatvec::Vec& getdx(bool check=true) const;
      const fmatvec::Vec& getqd(bool check=true) const;
      const fmatvec::Vec& getud(bool check=true) const;
      const fmatvec::Vec& getxd(bool check=true) const;
      const fmatvec::Mat& getW(int i=0, bool check=true) const;
      const fmatvec::Mat& getV(int i=0, bool check=true) const;
      const fmatvec::Vec& getwb(bool check=true) const;
      const fmatvec::Vec& getla(bool check=true) const;
      const fmatvec::Vec& getLa(bool check=true) const;
      const fmatvec::Vec& getg(bool check=true) const;
      const fmatvec::Vec& getgd(bool check=true) const;
      fmatvec::Vec& getla(bool check=true);
      fmatvec::Vec& getLa(bool check=true);
      void setla(const fmatvec::Vec &la_) { la = la_; }
      void setLa(const fmatvec::Vec &La_) { La = La_; }
      fmatvec::VecInt& getjsv() { return jsv; }
      const fmatvec::VecInt& getjsv() const { return jsv; }
      fmatvec::Mat& getW(int i=0, bool check=true);
      fmatvec::SymMat& getLLM(bool check=true);
      fmatvec::Vec& getdq(bool check=true);
      fmatvec::Vec& getdu(bool check=true);
      fmatvec::Vec& getdx(bool check=true);
 
      fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
      fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
      const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
      const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
 
      const fmatvec::Mat& evalT();
      const fmatvec::Vec& evalh(int i=0);
      const fmatvec::SymMat& evalM();
      const fmatvec::SymMat& evalLLM();
      const fmatvec::Mat& evalW(int i=0);
      const fmatvec::Mat& evalV(int i=0);
      const fmatvec::Vec& evalwb();
      const fmatvec::Vec& evalr(int i=0);
      const fmatvec::Mat& evalJrla(int i=0);
      const fmatvec::Vec& evalrdt();
      const fmatvec::Vec& evalg();
      const fmatvec::Vec& evalgd();
      const fmatvec::Mat& evalWInverseKinetics();
      const fmatvec::Mat& evalbInverseKinetics();
 
      void setqd(const fmatvec::Vec& qd_) { qd = qd_; }
      void setud(const fmatvec::Vec& ud_) { ud = ud_; }
      void setxd(const fmatvec::Vec& xd_) { xd = xd_; }
 
      int getxInd() { return xInd; }
      int getlaInd() const { return laInd; } 
 
      int gethInd(int i=0) { return hInd[i]; }
      void setlaInd(int ind) { laInd = ind; }
      void setisInd(int ind) { isInd = ind; }
      void setgInd(int ind) { gInd = ind; }
      void setgdInd(int ind) { gdInd = ind; }
      void setrFactorInd(int ind) { rFactorInd = ind; }
      virtual void setsvInd(int svInd_);
      void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};      
      void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
 
      void setqSize(int qSize_) { qSize = qSize_; }
      void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
      void setxSize(int xSize_) { xSize = xSize_; }
 
      int getisSize() const { return isSize; } 
      int getlaSize() const { return laSize; } 
      int getgSize() const { return gSize; } 
      int getgdSize() const { return gdSize; } 
      int getrFactorSize() const { return rFactorSize; } 
      int getsvSize() const { return svSize; }
      int getLinkStatusSize() const { return LinkStatusSize; }
      int getLinkStatusRegSize() const { return LinkStatusRegSize; }
      /*****************************************************/
 
      const std::vector<Object*>& getObjects() const { return object; }
      const std::vector<Link*>& getLinks() const { return link; }
      const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
      const std::vector<Frame*>& getFrames() const { return frame; }
      const std::vector<Contour*>& getContours() const { return contour; }
      const std::vector<Link*>& getSetValuedLinks() const { return linkSetValued; }
 
      void updateqRef(fmatvec::Vec &qParent);
 
      void updateqdRef(fmatvec::Vec &qdParent);
 
      void updatedqRef(fmatvec::Vec &dqParent);
 
      void updateuRef(fmatvec::Vec &uParent);
 
      void updateuallRef(fmatvec::Vec &uParent);
 
      void updateudRef(fmatvec::Vec &udParent);
 
      void updateduRef(fmatvec::Vec &duParent);
 
      void updateudallRef(fmatvec::Vec &udParent);
 
      void updatehRef(fmatvec::Vec &hParent, int j=0);
 
      void updaterRef(fmatvec::Vec &rParent, int j=0);
 
      void updateJrlaRef(fmatvec::Mat &rParent, int j=0);
 
      void updaterdtRef(fmatvec::Vec &rdtParent);
 
      void updateTRef(fmatvec::Mat &TParent);
 
      void updateMRef(fmatvec::SymMat &MParent);
 
      void updateLLMRef(fmatvec::SymMat &LLMParent);
 
      virtual void updateInternalStateRef(fmatvec::Vec &curisParent, fmatvec::Vec &nextisParent);
 
      virtual void updategRef(fmatvec::Vec &gParent);
 
      virtual void updategdRef(fmatvec::Vec &gdParent);
 
      void updatelaRef(fmatvec::Vec &laParent);
 
      void updateLaRef(fmatvec::Vec &LaParent);
 
      void updatelaInverseKineticsRef(fmatvec::Vec &laParent);
      void updatebInverseKineticsRef(fmatvec::Mat &bParent);
 
      virtual void updatewbRef(fmatvec::Vec &wbParent);
 
      virtual void updateWRef(fmatvec::Mat &WParent, int j=0);
 
      void updateWInverseKineticsRef(fmatvec::Mat &WParent);
 
      virtual void updateVRef(fmatvec::Mat &VParent, int j=0);
 
      void updatesvRef(fmatvec::Vec& svParent);
 
      void updatejsvRef(fmatvec::VecInt &jsvParent);
 
      void updateLinkStatusRef(fmatvec::VecInt &LinkStatusParent);
 
      void updateLinkStatusRegRef(fmatvec::VecInt &LinkStatusRegParent);
            
      void updateresRef(fmatvec::Vec &resParent);
 
      void updaterFactorRef(fmatvec::Vec &rFactorParent);
 
      void clearElementLists();
 
      void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
 
      void buildListOfObjects(std::vector<Object*> &obj);
 
      void buildListOfLinks(std::vector<Link*> &lnk);
 
      void buildListOfConstraints(std::vector<Constraint*> &crt);
 
      void buildListOfFrames(std::vector<Frame*> &frm);
 
      void buildListOfContours(std::vector<Contour*> &cnt);
 
      void buildListOfModels(std::vector<ModellingInterface*> &modelList);
 
      void buildListOfInverseKineticsLinks(std::vector<Link*> &iklnk);
 
      void buildListOfObservers(std::vector<Observer*> &obsrv);
 
      void setUpInverseKinetics();
 
      void setUpLinks();
 
      bool gActiveChanged();
      
      bool gActiveChangedReg();
      
      bool detectImpact();
 
      void calcsvSize();
 
      void calclaSize(int j);
 
      void calcLinkStatusSize();
 
      void calcLinkStatusRegSize();
 
      void calclaInverseKineticsSize();
 
      void calcbInverseKineticsSize();
 
      void calcisSize();
 
      virtual void aboutToUpdateInternalState();
 
      virtual void postprocessing();
 
      void calcgSize(int j);
 
      void calcgdSize(int j);
 
      void calcrFactorSize(int j);
 
      void setUpObjectsWithNonConstantMassMatrix();
 
      void setUpActiveLinks();
 
      void checkActive(int j);
 
      virtual void setGeneralizedRelativePositionTolerance(double tol);
 
      virtual void setGeneralizedRelativeVelocityTolerance(double tol);
 
      virtual void setGeneralizedRelativeAccelerationTolerance(double tol);
 
      virtual void setGeneralizedForceTolerance(double tol);
 
      virtual void setGeneralizedImpulseTolerance(double tol);
 
      virtual void setGeneralizedRelativePositionCorrectionValue(double corr);
 
      virtual void setGeneralizedRelativeVelocityCorrectionValue(double corr);
 
      void setrMax(double rMax);
 
      int frameIndex(const Frame *frame_) const;
 
      void addFrame(FixedRelativeFrame *frame);
 
      void addContour(RigidContour *contour);
 
      void addGroup(DynamicSystem *sys);
 
      DynamicSystem* getGroup(const std::string &name,bool check=true) const;
 
      void addObject(Object *obj);
 
      Object* getObject(const std::string &name,bool check=true) const;
 
      void addLink(Link *lnk);
 
      void addConstraint(Constraint *crt);
 
      void addInverseKineticsLink(Link *lnk);
 
      Observer* getObserver(const std::string &name,bool check=true) const;
      void addObserver(Observer *ele);
 
      Link* getLink(const std::string &name,bool check=true) const;
 
      Constraint* getConstraint(const std::string &name,bool check=true) const;
 
      void addModel(ModellingInterface *model_);
 
      ModellingInterface* getModel(const std::string &name, bool check=true) const;
 
      Frame *getFrameI() { return I; }
 
      Element *getChildByContainerAndName(const std::string &container, const std::string &name) const override;
 
      virtual void updatecorr(int j);
      void updatecorrRef(fmatvec::Vec &ref);
      void calccorrSize(int j);
 
      void checkRoot();
 
      void resetUpToDate() override;
 
      void updateStateTable();
 
      H5::GroupBase *getFramesPlotGroup() override { return framesPlotGroup; }
      H5::GroupBase *getContoursPlotGroup() override { return contoursPlotGroup; }
      H5::GroupBase *getGroupsPlotGroup() override { return groupsPlotGroup; }
      H5::GroupBase *getObjectsPlotGroup() override { return objectsPlotGroup; }
      H5::GroupBase *getLinksPlotGroup() override { return linksPlotGroup; }
      H5::GroupBase *getConstraintsPlotGroup() override { return constraintsPlotGroup; }
      H5::GroupBase *getObserversPlotGroup() override { return observersPlotGroup; }
 
    private:
      friend class DynamicSystemSolver;
      void addFrame(Frame *frame_);
      void addContour(Contour *contour_);
 
    protected:
      Frame *R;
 
      std::vector<Object*> object;
      std::vector<Object*> objectWithNonConstantMassMatrix;
      std::vector<Link*> link;
      std::vector<Link*> linkSingleValued;
      std::vector<Link*> linkSetValued;
      std::vector<Link*> linkSetValuedActive;
      std::vector<Link*> linkWithStopVector;
      std::vector<ModellingInterface*> model;
      std::vector<DynamicSystem*> dynamicsystem;
      std::vector<Link*> inverseKineticsLink;
      std::vector<Observer*> observer;
      std::vector<Constraint*> constraint;
 
      fmatvec::Mat T;
 
      fmatvec::SymMat M;
 
      fmatvec::SymMat LLM;
 
      fmatvec::Vec q, qd, dq;
 
      fmatvec::Vec u, ud, du;
 
      fmatvec::Vec x, xd, dx;
 
      fmatvec::Vec h[2], r[2], rdt;
 
      fmatvec::Mat Jrla[2];
 
      fmatvec::Mat W[2], V[2];
 
      fmatvec::Vec la, La;
 
      fmatvec::Vec curis, nextis;
 
      fmatvec::Vec g, gd;
 
      fmatvec::Vec wb;
 
      fmatvec::Vec res;
 
      fmatvec::Vec rFactor;
 
      fmatvec::Vec sv;
 
      fmatvec::VecInt jsv;
 
      fmatvec::VecInt LinkStatus;
 
      fmatvec::VecInt LinkStatusReg;
 
      int qSize, qInd;
 
      int uSize[2], uInd[2];
 
      int xSize, xInd;
 
      int hSize[2], hInd[2];
 
      int isSize, isInd;
 
      int gSize, gInd;
 
      int gdSize, gdInd;
 
      int laSize, laInd;
 
      int rFactorSize, rFactorInd;
 
      int svSize, svInd;
 
      int LinkStatusSize, LinkStatusInd;
      
      int LinkStatusRegSize, LinkStatusRegInd;
      
      std::vector<Frame*> frame;
      std::vector<Contour*> contour;
 
      std::shared_ptr<OpenMBV::Group> openMBVGrp;
      std::shared_ptr<OpenMBV::Group> framesOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> contoursOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> groupsOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> objectsOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> linksOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> constraintsOpenMBVGrp;
      std::shared_ptr<OpenMBV::Group> observersOpenMBVGrp;
      std::shared_ptr<H5::File> hdf5File;
 
      Frame *I;
 
      int laInverseKineticsSize, bInverseKineticsSize;
 
      fmatvec::Mat WInverseKinetics, bInverseKinetics;
      fmatvec::Vec laInverseKinetics;
 
      int corrSize, corrInd;
      fmatvec::Vec corr;
 
      std::string saved_frameOfReference;
 
      H5::GroupBase *framesPlotGroup;
      H5::GroupBase *contoursPlotGroup;
      H5::GroupBase *groupsPlotGroup;
      H5::GroupBase *objectsPlotGroup;
      H5::GroupBase *linksPlotGroup;
      H5::GroupBase *constraintsPlotGroup;
      H5::GroupBase *observersPlotGroup;
  };
}
 
#endif