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 updaterRef(const fmatvec::Vec &ref, int j=0);
00368 
00373       void updateTRef(const fmatvec::Mat &ref);
00374 
00380       void updateMRef(const fmatvec::SymMat &ref, int i=0);
00381 
00387       void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00388 
00393       void updategRef(const fmatvec::Vec &ref);
00394 
00399       void updategdRef(const fmatvec::Vec &ref);
00400 
00405       void updatelaRef(const fmatvec::Vec &ref);
00406 
00407       void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00408       void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00409 
00414       void updatewbRef(const fmatvec::Vec &ref);
00415 
00421       void updateWRef(const fmatvec::Mat &ref, int i=0);
00422 
00423       void updateWnVRefObjects();
00424 
00430       void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00431 
00437       void updateVRef(const fmatvec::Mat &ref, int i=0);
00438 
00443       void updatesvRef(const fmatvec::Vec& ref);
00444 
00449       void updatejsvRef(const fmatvec::VecInt &ref);
00450 
00455       void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00456 
00461       void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);      
00462             
00467       void updateresRef(const fmatvec::Vec &ref);
00468 
00473       void updaterFactorRef(const fmatvec::Vec &ref);
00474 
00475       void clearElementLists();
00476 
00481       void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00482 
00487       void buildListOfObjects(std::vector<Object*> &obj);
00488 
00493       void buildListOfLinks(std::vector<Link*> &lnk);
00494 
00499       void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00500 
00505       void buildListOfFrames(std::vector<Frame*> &frm);
00506 
00511       void buildListOfContours(std::vector<Contour*> &cnt);
00512 
00517       void buildListOfModels(std::vector<ModellingInterface*> &model);
00518 
00523       void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00524 
00529       void buildListOfObservers(std::vector<Observer*> &obsrv);
00530 
00534       void setUpInverseKinetics();
00535 
00539       void setUpLinks();
00540 
00544       bool gActiveChanged();
00545       
00549       bool gActiveChangedReg();
00550       
00554       void calcsvSize();
00555 
00559       void calclaSize(int j);
00560 
00564       void calcLinkStatusSize();
00565 
00569       void calcLinkStatusRegSize();
00570 
00574       void calclaInverseKineticsSize();
00575 
00579       void calcbInverseKineticsSize();
00580 
00584       void calcgSize(int j);
00585 
00589       void calcgdSize(int j);
00590 
00594       void calcrFactorSize(int j);
00595 
00599       void setUpActiveLinks();
00600 
00604       void checkActive(int i);
00605 
00609       void checkActiveReg(int i);
00610 
00614       virtual void setgTol(double tol);
00615 
00619       virtual void setgdTol(double tol);
00620 
00624       virtual void setgddTol(double tol);
00625 
00629       virtual void setlaTol(double tol);
00630 
00634       virtual void setLaTol(double tol);
00635 
00639       void setrMax(double rMax);
00640 
00641       void addFrame(FixedRelativeFrame *frame);
00642 
00647       int frameIndex(const Frame *frame_) const;
00648 
00652       void addGroup(DynamicSystem *dynamicsystem);
00653 
00659       DynamicSystem* getGroup(const std::string &name,bool check=true);
00660 
00664       void addObject(Object *object);
00665 
00671       Object* getObject(const std::string &name,bool check=true);
00672 
00676       void addLink(Link *link);
00677 
00681       void addInverseKineticsLink(Link *link);
00682 
00683       Observer* getObserver(const std::string &name,bool check=true);
00684       void addObserver(Observer *element);
00685 
00691       Link* getLink(const std::string &name,bool check=true);
00692 
00696       void addModel(ModellingInterface *modell);
00697 
00703       ModellingInterface* getModel(const std::string &name, bool check=true);
00704 
00706       FixedRelativeFrame *getFrameI() { return I; }
00707 
00708       virtual Element *getByPathSearch(std::string path);
00709 
00710       virtual void updatecorr(int j);
00711       void updatecorrRef(const fmatvec::Vec &ref);
00712       void calccorrSize(int j);
00713 
00714       void checkRoot();
00715 
00716     protected:
00720       Frame *R;
00721 
00725       fmatvec::Vec3 PrPF;
00726 
00730       fmatvec::SqrMat3 APF;
00731 
00735       std::vector<Object*> object;
00736       std::vector<Link*> link;
00737       std::vector<Link*> linkSingleValued;
00738       std::vector<Link*> linkSetValued;
00739       std::vector<Link*> linkSetValuedActive;
00740       std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00741       std::vector<ModellingInterface*> model;
00742       std::vector<DynamicSystem*> dynamicsystem;
00743       std::vector<Link*> inverseKineticsLink;
00744       std::vector<Observer*> observer;
00745 
00749       fmatvec::Mat T;
00750 
00754       fmatvec::SymMat M[2];
00755 
00759       fmatvec::SymMat LLM[2];
00760 
00764       fmatvec::Vec q, qd, q0;
00765 
00769       fmatvec::Vec u, ud[2], u0;
00770 
00774       fmatvec::Vec x, xd, x0;
00775 
00779       fmatvec::Vec h[2], r[2], f;
00780 
00784       fmatvec::Mat W[2], V[2];
00785 
00789       fmatvec::Vec la;
00790 
00794       fmatvec::Vec g, gd;
00795 
00799       fmatvec::Vec wb;
00800 
00804       fmatvec::Vec res;
00805 
00809       fmatvec::Vec rFactor;
00810 
00814       fmatvec::Vec sv;
00815 
00819       fmatvec::VecInt jsv;
00820 
00824       fmatvec::VecInt LinkStatus;
00825 
00829       fmatvec::VecInt LinkStatusReg;
00830 
00834       int qSize, qInd;
00835 
00839       int uSize[2], uInd[2];
00840 
00844       int xSize, xInd;
00845 
00849       int hSize[2], hInd[2];
00850 
00854       int gSize, gInd;
00855 
00859       int gdSize, gdInd;
00860 
00864       int laSize, laInd;
00865 
00869       int rFactorSize, rFactorInd;
00870 
00874       int svSize, svInd;
00875 
00879       int LinkStatusSize, LinkStatusInd;
00880       
00884       int LinkStatusRegSize, LinkStatusRegInd;
00885       
00889       std::vector<Frame*> frame;
00890       std::vector<Contour*> contour;
00891 
00892 #ifdef HAVE_OPENMBVCPPINTERFACE
00893       OpenMBV::Group* openMBVGrp;
00894 #endif
00895 
00899       void addContour(Contour* contour);
00900 
00902       FixedRelativeFrame *I;
00903 
00907       int laInverseKineticsSize, bInverseKineticsSize;
00908 
00909       fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00910       fmatvec::Vec laInverseKinetics;
00911 
00912       int corrSize, corrInd;
00913       fmatvec::Vec corr;
00914 
00915       std::string saved_frameOfReference;
00916   };
00917 }
00918 
00919 #endif
00920