MBSim

Project Page
dynamic_system.h
00001 /* Copyright (C) 2004-2009 MBSim Development Team
00002  * This library is free software; you can redistribute it and/or 
00003  * modify it under the terms of the GNU Lesser General Public 
00004  * License as published by the Free Software Foundation; either 
00005  * version 2.1 of the License, or (at your option) any later version. 
00006  *  
00007  * This library is distributed in the hope that it will be useful, 
00008  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00009  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00010  * Lesser General Public License for more details. 
00011  *  
00012  * You should have received a copy of the GNU Lesser General Public 
00013  * License along with this library; if not, write to the Free Software 
00014  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00015  *
00016  * Contact: martin.o.foerg@googlemail.com
00017  */
00018 
00019 #ifndef _DYNAMIC_SYSTEM_H_
00020 #define _DYNAMIC_SYSTEM_H_
00021 
00022 #include "mbsim/element.h"
00023 #include "mbsim/mbsim_event.h"
00024 
00025 namespace H5 {
00026   class Group;
00027 }
00028 
00029 namespace MBSim {
00030   class Frame;
00031   class FixedRelativeFrame;
00032   class Contour;
00033   class Object;
00034   class Link;
00035   class ModellingInterface;
00036   class Contact;
00037   class InverseKineticsJoint;
00038   class Observer;
00039 
00057   class DynamicSystem : public Element {
00058     public:
00062       DynamicSystem(const std::string &name);
00063 
00067       virtual ~DynamicSystem();
00068 
00069       /* INHERITED INTERFACE OF OBJECTINTERFACE */
00070       virtual void updateT(double t); 
00071       virtual void updateh(double t, int i=0); 
00072       virtual void updateh0Fromh1(double t); 
00073       virtual void updateW0FromW1(double t); 
00074       virtual void updateV0FromV1(double t); 
00075       virtual void updateStateDependentVariables(double t) = 0;
00076       virtual void updateStateDerivativeDependentVariables(double t);
00077       virtual void updateM(double t, int i=0);
00078       virtual void updateJacobians(double t, int j=0) = 0;
00079       virtual void updatedq(double t, double dt); 
00080       virtual void updateud(double t, int i=0) { throw MBSimError("ERROR in "+getName()+" (DynamicSystem::updateud): Not implemented!"); }
00081       virtual void updatezd(double t) = 0;
00082       virtual void updatedu(double t, double dt) = 0;
00083       virtual void updateqd(double t);
00084       virtual void sethSize(int hSize_, int i=0);
00085       virtual int gethSize(int i=0) const { return hSize[i]; }
00086       virtual int getqSize() const { return qSize; }
00087       virtual int getuSize(int i=0) const { return uSize[i]; }
00088       virtual void calcqSize();
00089       virtual void calcuSize(int j=0);
00090       //virtual int getqInd(DynamicSystem* sys);
00091       virtual int getuInd(int i=0) { return uInd[i]; }
00092       //virtual int getuInd(DynamicSystem* sys, int i=0);
00093       //virtual void setqInd(int qInd_) { qInd = qInd_; }
00094       //virtual void setuInd(int uInd_, int i=0) { uInd[i] = uInd_; }
00095       virtual void setqInd(int qInd_);
00096       virtual void setuInd(int uInd_, int i=0);
00097       virtual void sethInd(int hInd_, int i=0);
00098       virtual void setxInd(int xInd_);
00099       //virtual int gethInd(DynamicSystem* sys, int i=0); 
00100       virtual const fmatvec::Vec& getq() const { return q; };
00101       virtual fmatvec::Vec& getq() { return q; };
00102       virtual const fmatvec::Vec& getu() const { return u; };
00103       virtual fmatvec::Vec& getu() { return u; };
00104       virtual H5::Group *getPlotGroup() { return plotGroup; }
00105       virtual PlotFeatureStatus getPlotFeature(PlotFeature fp) { return Element::getPlotFeature(fp); };
00106       virtual PlotFeatureStatus getPlotFeatureForChildren(PlotFeature fp) { return Element::getPlotFeatureForChildren(fp); };
00107 #ifdef HAVE_OPENMBVCPPINTERFACE
00108       virtual OpenMBV::Group* getOpenMBVGrp();
00109 #endif
00110 
00114       std::string getPath(char pathDelim='.') { return parent?parent->getPath()+pathDelim+name:name; }
00115       /*****************************************************/
00116 
00117       /* INHERITED INTERFACE OF LINKINTERFACE */
00118       virtual void updatewb(double t, int j=0); 
00119       virtual void updateW(double t, int j=0); 
00120       virtual void updateV(double t, int j=0); 
00121       virtual void updateg(double t);
00122       virtual void updategd(double t);
00123       virtual void updateStopVector(double t); 
00124       virtual void updateLinkStatus(double t);
00125       virtual void updateLinkStatusReg(double t);
00126 
00127       virtual void updategInverseKinetics(double t); 
00128       virtual void updategdInverseKinetics(double t);
00129       virtual void updateWInverseKinetics(double t, int j=0); 
00130       virtual void updatehInverseKinetics(double t, int j=0); 
00131       virtual void updateJacobiansInverseKinetics(double t, int j=0); 
00132       virtual void updatebInverseKinetics(double t); 
00133       /*****************************************************/
00134 
00135       /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
00136       virtual void updatedx(double t, double dt); 
00137       virtual void updatexd(double t);
00138       virtual void calcxSize();
00139       const fmatvec::Vec& getx() const { return x; };
00140       fmatvec::Vec& getx() { return x; };
00141       int getxSize() const { return xSize; }
00142       void updatexRef(const fmatvec::Vec &ref);
00143       void updatexdRef(const fmatvec::Vec &ref);
00144       virtual void init(InitStage stage);
00145       virtual void initz();
00146       virtual void writez(const H5::CommonFG & group);
00147       virtual void readz0(const H5::CommonFG & parent);
00148       /*****************************************************/
00149 
00150       /* INHERITED INTERFACE OF ELEMENT */
00152       virtual std::string getType() const { return "DynamicSystem"; }
00153       virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00154       virtual void plot(double t, double dt);
00155       virtual void plotAtSpecialEvent(double t, double dt=1.);
00156       virtual void closePlot();
00157       /*****************************************************/
00158 
00159       /* INTERFACE FOR DERIVED CLASSES */
00163       virtual void facLLM(int i=0) = 0;
00164 
00168       virtual int solveConstraintsFixpointSingle();
00169 
00173       virtual int solveImpactsFixpointSingle(double dt);
00174 
00178       virtual int solveConstraintsGaussSeidel();
00179 
00183       virtual int solveImpactsGaussSeidel(double dt);
00184 
00188       virtual int solveConstraintsRootFinding();
00189 
00193       virtual int solveImpactsRootFinding(double dt);
00194 
00198       virtual int jacobianConstraints();
00199 
00203       virtual int jacobianImpacts();
00204 
00208       virtual void checkConstraintsForTermination();
00209 
00213       virtual void checkImpactsForTermination(double dt);
00214 
00218       virtual void updaterFactors();
00219 
00225       virtual Frame* getFrame(const std::string &name, bool check=true);
00226 
00232       virtual Contour* getContour(const std::string &name, bool check=true);
00233       /*****************************************************/
00234 
00235       /* GETTER / SETTER */
00236       void setPosition(const fmatvec::Vec3& PrPF_) { PrPF = PrPF_; }
00237       void setOrientation(const fmatvec::SqrMat3& APF_) { APF = APF_; }
00238       void setFrameOfReference(Frame *frame) { R = frame; };
00239       const fmatvec::Vec3& getPosition() const { return PrPF; }
00240       const fmatvec::SqrMat3& getOrientation() const { return APF; }
00241       const Frame* getFrameOfReference() const { return R; };
00242 
00243       const fmatvec::Vec& getxd() const { return xd; };
00244       fmatvec::Vec& getxd() { return xd; };
00245       const fmatvec::Vec& getx0() const { return x0; };
00246       fmatvec::Vec& getx0() { return x0; };
00247 
00248       const fmatvec::Mat& getT() const { return T; };
00249       const fmatvec::SymMat& getM(int i=0) const { return M[i]; };
00250       const fmatvec::SymMat& getLLM(int i=0) const { return LLM[i]; };
00251       const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00252       const fmatvec::Vec& getf() const { return f; };
00253       fmatvec::Vec& getf() { return f; };
00254 
00255       const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00256       fmatvec::Mat& getW(int i=0) { return W[i]; }
00257       const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00258       fmatvec::Mat& getV(int i=0) { return V[i]; }
00259 
00260       const fmatvec::Vec& getla() const { return la; }
00261       fmatvec::Vec& getla() { return la; }
00262       const fmatvec::Vec& getg() const { return g; }
00263       fmatvec::Vec& getg() { return g; }
00264       const fmatvec::Vec& getgd() const { return gd; }
00265       fmatvec::Vec& getgd() { return gd; }
00266       const fmatvec::Vec& getrFactor() const { return rFactor; }
00267       fmatvec::Vec& getrFactor() { return rFactor; }
00268       fmatvec::Vec& getsv() { return sv; }
00269       const fmatvec::Vec& getsv() const { return sv; }
00270       fmatvec::VecInt& getjsv() { return jsv; }
00271       const fmatvec::VecInt& getjsv() const { return jsv; }
00272       fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00273       fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00274       const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00275       const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00276       const fmatvec::Vec& getres() const { return res; }
00277       fmatvec::Vec& getres() { return res; }
00278       const fmatvec::Vec& getcorr() const { return corr; };
00279       fmatvec::Vec& getcorr() { return corr; };
00280 
00281       void setx(const fmatvec::Vec& x_) { x = x_; }
00282       void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00283       void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00284 
00285       int getxInd() { return xInd; }
00286       int getlaInd() const { return laInd; } 
00287 
00288       int gethInd(int i=0) { return hInd[i]; }
00289       void setlaInd(int ind) { laInd = ind; }
00290       void setgInd(int ind) { gInd = ind; }
00291       void setgdInd(int ind) { gdInd = ind; }
00292       void setrFactorInd(int ind) { rFactorInd = ind; }
00293       void setsvInd(int svInd_) { svInd = svInd_; };
00294       void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};      
00295       void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00296 
00297       int getzSize() const { return qSize + uSize[0] + xSize; }
00298 
00299       void setqSize(int qSize_) { qSize = qSize_; }
00300       void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00301       void setxSize(int xSize_) { xSize = xSize_; }
00302 
00303       int getlaSize() const { return laSize; } 
00304       int getgSize() const { return gSize; } 
00305       int getgdSize() const { return gdSize; } 
00306       int getrFactorSize() const { return rFactorSize; } 
00307       int getsvSize() const { return svSize; }
00308       int getLinkStatusSize() const { return LinkStatusSize; }
00309       int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00310       /*****************************************************/
00311 
00312       const std::vector<Object*>& getObjects() const { return object; }
00313       const std::vector<Link*>& getLinks() const { return link; }
00314       const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00315       const std::vector<Frame*>& getFrames() const { return frame; }
00316       const std::vector<Contour*>& getContours() const { return contour; }
00317 
00322       void updateqRef(const fmatvec::Vec &ref); 
00323 
00328       void updateqdRef(const fmatvec::Vec &ref);
00329 
00334       void updateuRef(const fmatvec::Vec &ref);
00335 
00340       void updateuallRef(const fmatvec::Vec &ref);
00341 
00346       void updateudRef(const fmatvec::Vec &ref, int i=0);
00347 
00352       void updateudallRef(const fmatvec::Vec &ref, int i=0);
00353 
00361       void updatehRef(const fmatvec::Vec &hRef, int i=0);
00362 
00367       void updatefRef(const fmatvec::Vec &ref);
00368 
00373       void updaterRef(const fmatvec::Vec &ref, int j=0);
00374 
00379       void updateTRef(const fmatvec::Mat &ref);
00380 
00386       void updateMRef(const fmatvec::SymMat &ref, int i=0);
00387 
00393       void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00394 
00399       void updategRef(const fmatvec::Vec &ref);
00400 
00405       void updategdRef(const fmatvec::Vec &ref);
00406 
00411       void updatelaRef(const fmatvec::Vec &ref);
00412 
00413       void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00414       void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00415 
00420       void updatewbRef(const fmatvec::Vec &ref);
00421 
00427       void updateWRef(const fmatvec::Mat &ref, int i=0);
00428 
00429       void updateWnVRefObjects();
00430 
00436       void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00437 
00443       void updateVRef(const fmatvec::Mat &ref, int i=0);
00444 
00449       void updatesvRef(const fmatvec::Vec& ref);
00450 
00455       void updatejsvRef(const fmatvec::VecInt &ref);
00456 
00461       void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00462 
00467       void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);      
00468             
00473       void updateresRef(const fmatvec::Vec &ref);
00474 
00479       void updaterFactorRef(const fmatvec::Vec &ref);
00480 
00486       virtual void buildListOfObjects(std::vector<Object*> &obj, bool recursive=false);
00487 
00493       virtual void buildListOfLinks(std::vector<Link*> &lnk, bool recursive=false);
00494 
00500       virtual void buildListOfSetValuedLinks(std::vector<Link*> &lnk, bool recursive=false);
00501 
00507       virtual void buildListOfFrames(std::vector<Frame*> &frm, bool recursive);
00508 
00514       virtual void buildListOfContours(std::vector<Contour*> &cnt, bool recursive);
00515 
00521       void buildListOfModels(std::vector<ModellingInterface*> &model, bool recursive = true);
00522 
00528       virtual void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk, bool recursive=false);
00529 
00535       virtual void buildListOfObservers(std::vector<Observer*> &obsrv, bool recursive=false);
00536 
00540       void setUpInverseKinetics();
00541 
00545       void setUpLinks();
00546 
00550       bool gActiveChanged();
00551       
00555       bool gActiveChangedReg();
00556       
00560       void calcsvSize();
00561 
00565       void calclaSize(int j);
00566 
00570       void calcLinkStatusSize();
00571 
00575       void calcLinkStatusRegSize();
00576 
00580       void calclaInverseKineticsSize();
00581 
00585       void calcbInverseKineticsSize();
00586 
00590       void calcgSize(int j);
00591 
00595       void calcgdSize(int j);
00596 
00600       void calcrFactorSize(int j);
00601 
00605       void setUpActiveLinks();
00606 
00610       void checkActive(int i);
00611 
00615       void checkActiveReg(int i);
00616 
00620       virtual void setgTol(double tol);
00621 
00625       virtual void setgdTol(double tol);
00626 
00630       virtual void setgddTol(double tol);
00631 
00635       virtual void setlaTol(double tol);
00636 
00640       virtual void setLaTol(double tol);
00641 
00645       void setrMax(double rMax);
00646 
00647       void addFrame(FixedRelativeFrame *frame);
00648 
00655       void addFrame(Frame *frame, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00656 
00663       void addFrame(const std::string &str, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00664 
00671       void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const fmatvec::SqrMat3 &ARC, const Frame* refFrame=0);
00672 
00678       void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const Frame* refFrame=0) { addContour(contour,RrRC,fmatvec::SqrMat3(fmatvec::EYE),refFrame); }
00679 
00684       int frameIndex(const Frame *frame_) const;
00685 
00689       void addGroup(DynamicSystem *dynamicsystem);
00690 
00696       DynamicSystem* getGroup(const std::string &name,bool check=true);
00697 
00701       void addObject(Object *object);
00702 
00708       Object* getObject(const std::string &name,bool check=true);
00709 
00713       void addLink(Link *link);
00714 
00718       void addInverseKineticsLink(Link *link);
00719 
00720       Observer* getObserver(const std::string &name,bool check=true);
00721       void addObserver(Observer *element);
00722 
00728       Link* getLink(const std::string &name,bool check=true);
00729 
00733       void addModel(ModellingInterface *modell);
00734 
00740       ModellingInterface* getModel(const std::string &name, bool check=true);
00741 
00743       FixedRelativeFrame *getFrameI() { return I; }
00744 
00745       virtual Element *getByPathSearch(std::string path);
00746 
00747       virtual void updatecorr(int j);
00748       void updatecorrRef(const fmatvec::Vec &ref);
00749       void calccorrSize(int j);
00750 
00751       void checkRoot();
00752 
00753     protected:
00757       Frame *R;
00758 
00762       fmatvec::Vec3 PrPF;
00763 
00767       fmatvec::SqrMat3 APF;
00768 
00772       std::vector<Object*> object;
00773       std::vector<Link*> link;
00774       std::vector<Link*> linkSingleValued;
00775       std::vector<Link*> linkSetValued;
00776       std::vector<Link*> linkSetValuedActive;
00777       std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00778       std::vector<ModellingInterface*> model;
00779       std::vector<DynamicSystem*> dynamicsystem;
00780       std::vector<Link*> inverseKineticsLink;
00781       std::vector<Observer*> observer;
00782 
00786       fmatvec::Mat T;
00787 
00791       fmatvec::SymMat M[2];
00792 
00796       fmatvec::SymMat LLM[2];
00797 
00801       fmatvec::Vec q, qd, q0;
00802 
00806       fmatvec::Vec u, ud[2], u0;
00807 
00811       fmatvec::Vec x, xd, x0;
00812 
00816       fmatvec::Vec h[2], r[2], f;
00817 
00821       fmatvec::Mat W[2], V[2];
00822 
00826       fmatvec::Vec la;
00827 
00831       fmatvec::Vec g, gd;
00832 
00836       fmatvec::Vec wb;
00837 
00841       fmatvec::Vec res;
00842 
00846       fmatvec::Vec rFactor;
00847 
00851       fmatvec::Vec sv;
00852 
00856       fmatvec::VecInt jsv;
00857 
00861       fmatvec::VecInt LinkStatus;
00862 
00866       fmatvec::VecInt LinkStatusReg;
00867 
00871       int qSize, qInd;
00872 
00876       int uSize[2], uInd[2];
00877 
00881       int xSize, xInd;
00882 
00886       int hSize[2], hInd[2];
00887 
00891       int gSize, gInd;
00892 
00896       int gdSize, gdInd;
00897 
00901       int laSize, laInd;
00902 
00906       int rFactorSize, rFactorInd;
00907 
00911       int svSize, svInd;
00912 
00916       int LinkStatusSize, LinkStatusInd;
00917       
00921       int LinkStatusRegSize, LinkStatusRegInd;
00922       
00926       std::vector<Frame*> frame;
00927       std::vector<Contour*> contour;
00928 
00929 #ifdef HAVE_OPENMBVCPPINTERFACE
00930       OpenMBV::Group* openMBVGrp;
00931 #endif
00932 
00936       void addContour(Contour* contour);
00937 
00939       FixedRelativeFrame *I;
00940 
00944       int laInverseKineticsSize, bInverseKineticsSize;
00945 
00946       fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00947       fmatvec::Vec laInverseKinetics;
00948 
00949       int corrSize, corrInd;
00950       fmatvec::Vec corr;
00951   };
00952 }
00953 
00954 #endif
00955