MBSim

Project Page
dynamic_system.h
00001 /* Copyright (C) 2004-2014 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       /* INTERFACE FOR DERIVED CLASSES */
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("(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::GroupBase *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 
00111       virtual void updatewb(double t, int j=0); 
00112       virtual void updateW(double t, int j=0); 
00113       virtual void updateV(double t, int j=0); 
00114       virtual void updateg(double t);
00115       virtual void updategd(double t);
00116       virtual void updateStopVector(double t); 
00117       virtual void updateLinkStatus(double t);
00118       virtual void updateLinkStatusReg(double t);
00119 
00120       virtual void updategInverseKinetics(double t); 
00121       virtual void updategdInverseKinetics(double t);
00122       virtual void updateWInverseKinetics(double t, int j=0); 
00123       virtual void updatehInverseKinetics(double t, int j=0); 
00124       virtual void updateJacobiansInverseKinetics(double t, int j=0); 
00125       virtual void updatebInverseKinetics(double t); 
00126 
00127       virtual void updatedx(double t, double dt); 
00128       virtual void updatexd(double t);
00129       virtual void calcxSize();
00130       const fmatvec::Vec& getx() const { return x; };
00131       fmatvec::Vec& getx() { return x; };
00132       int getxSize() const { return xSize; }
00133       void updatexRef(const fmatvec::Vec &ref);
00134       void updatexdRef(const fmatvec::Vec &ref);
00135       virtual void init(InitStage stage);
00136       virtual void initz();
00137       virtual void writez(H5::GroupBase *group);
00138       virtual void readz0(H5::GroupBase *parent);
00139       /*****************************************************/
00140 
00141       /* INHERITED INTERFACE OF ELEMENT */
00143       virtual std::string getType() const { return "DynamicSystem"; }
00144       virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00145       virtual void plot(double t, double dt);
00146       virtual void plotAtSpecialEvent(double t, double dt=1.);
00147       virtual void closePlot();
00148       /*****************************************************/
00149 
00150       /* INTERFACE FOR DERIVED CLASSES */
00154       virtual void facLLM(int i=0) = 0;
00155 
00160       virtual int solveConstraintsFixpointSingle();
00161 
00167       virtual int solveImpactsFixpointSingle(double dt);
00168 
00173       virtual int solveConstraintsGaussSeidel();
00174 
00180       virtual int solveImpactsGaussSeidel(double dt);
00181 
00186       virtual int solveConstraintsRootFinding();
00187 
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) const;
00226 
00232       virtual Contour* getContour(const std::string &name, bool check=true) const;
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       fmatvec::SymMat& getLLM(int i=0) { return LLM[i]; };
00252       const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00253       fmatvec::Vec& geth(int j=0) { return h[j]; };
00254       const fmatvec::Vec& getf() const { return f; };
00255       fmatvec::Vec& getf() { return f; };
00256 
00257       const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00258       fmatvec::Mat& getW(int i=0) { return W[i]; }
00259       const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00260       fmatvec::Mat& getV(int i=0) { return V[i]; }
00261 
00262       const fmatvec::Vec& getla() const { return la; }
00263       fmatvec::Vec& getla() { return la; }
00264       const fmatvec::Vec& getg() const { return g; }
00265       fmatvec::Vec& getg() { return g; }
00266       const fmatvec::Vec& getgd() const { return gd; }
00267       fmatvec::Vec& getgd() { return gd; }
00268       const fmatvec::Vec& getrFactor() const { return rFactor; }
00269       fmatvec::Vec& getrFactor() { return rFactor; }
00270       fmatvec::Vec& getsv() { return sv; }
00271       const fmatvec::Vec& getsv() const { return sv; }
00272       fmatvec::VecInt& getjsv() { return jsv; }
00273       const fmatvec::VecInt& getjsv() const { return jsv; }
00274       fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00275       fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00276       const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00277       const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00278       const fmatvec::Vec& getres() const { return res; }
00279       fmatvec::Vec& getres() { return res; }
00280       const fmatvec::Vec& getcorr() const { return corr; };
00281       fmatvec::Vec& getcorr() { return corr; };
00282 
00283       void setx(const fmatvec::Vec& x_) { x = x_; }
00284       void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00285       void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00286 
00287       int getxInd() { return xInd; }
00288       int getlaInd() const { return laInd; } 
00289 
00290       int gethInd(int i=0) { return hInd[i]; }
00291       void setlaInd(int ind) { laInd = ind; }
00292       void setgInd(int ind) { gInd = ind; }
00293       void setgdInd(int ind) { gdInd = ind; }
00294       void setrFactorInd(int ind) { rFactorInd = ind; }
00295       void setsvInd(int svInd_) { svInd = svInd_; };
00296       void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};      
00297       void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00298 
00299       int getzSize() const { return qSize + uSize[0] + xSize; }
00300 
00301       void setqSize(int qSize_) { qSize = qSize_; }
00302       void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00303       void setxSize(int xSize_) { xSize = xSize_; }
00304 
00305       int getlaSize() const { return laSize; } 
00306       int getgSize() const { return gSize; } 
00307       int getgdSize() const { return gdSize; } 
00308       int getrFactorSize() const { return rFactorSize; } 
00309       int getsvSize() const { return svSize; }
00310       int getLinkStatusSize() const { return LinkStatusSize; }
00311       int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00312       /*****************************************************/
00313 
00314       const std::vector<Object*>& getObjects() const { return object; }
00315       const std::vector<Link*>& getLinks() const { return link; }
00316       const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00317       const std::vector<Frame*>& getFrames() const { return frame; }
00318       const std::vector<Contour*>& getContours() const { return contour; }
00319 
00324       void updateqRef(const fmatvec::Vec &ref); 
00325 
00330       void updateqdRef(const fmatvec::Vec &ref);
00331 
00336       void updateuRef(const fmatvec::Vec &ref);
00337 
00342       void updateuallRef(const fmatvec::Vec &ref);
00343 
00348       void updateudRef(const fmatvec::Vec &ref, int i=0);
00349 
00354       void updateudallRef(const fmatvec::Vec &ref, int i=0);
00355 
00363       void updatehRef(const fmatvec::Vec &hRef, int i=0);
00364 
00369       void updaterRef(const fmatvec::Vec &ref, int j=0);
00370 
00375       void updateTRef(const fmatvec::Mat &ref);
00376 
00382       void updateMRef(const fmatvec::SymMat &ref, int i=0);
00383 
00389       void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00390 
00395       void updategRef(const fmatvec::Vec &ref);
00396 
00401       void updategdRef(const fmatvec::Vec &ref);
00402 
00407       void updatelaRef(const fmatvec::Vec &ref);
00408 
00409       void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00410       void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00411 
00416       void updatewbRef(const fmatvec::Vec &ref);
00417 
00423       void updateWRef(const fmatvec::Mat &ref, int i=0);
00424 
00425       void updateWnVRefObjects();
00426 
00432       void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00433 
00439       void updateVRef(const fmatvec::Mat &ref, int i=0);
00440 
00445       void updatesvRef(const fmatvec::Vec& ref);
00446 
00451       void updatejsvRef(const fmatvec::VecInt &ref);
00452 
00457       void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00458 
00463       void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);      
00464             
00469       void updateresRef(const fmatvec::Vec &ref);
00470 
00475       void updaterFactorRef(const fmatvec::Vec &ref);
00476 
00477       void clearElementLists();
00478 
00483       void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00484 
00489       void buildListOfObjects(std::vector<Object*> &obj);
00490 
00495       void buildListOfLinks(std::vector<Link*> &lnk);
00496 
00501       void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00502 
00507       void buildListOfFrames(std::vector<Frame*> &frm);
00508 
00513       void buildListOfContours(std::vector<Contour*> &cnt);
00514 
00519       void buildListOfModels(std::vector<ModellingInterface*> &model);
00520 
00525       void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00526 
00531       void buildListOfObservers(std::vector<Observer*> &obsrv);
00532 
00536       void setUpInverseKinetics();
00537 
00541       void setUpLinks();
00542 
00546       bool gActiveChanged();
00547       
00551       bool gActiveChangedReg();
00552       
00556       bool detectImpact();
00557 
00561       void calcsvSize();
00562 
00566       void calclaSize(int j);
00567 
00571       void calcLinkStatusSize();
00572 
00576       void calcLinkStatusRegSize();
00577 
00581       void calclaInverseKineticsSize();
00582 
00586       void calcbInverseKineticsSize();
00587 
00591       void calcgSize(int j);
00592 
00603       void calcgdSize(int j);
00604 
00608       void calcrFactorSize(int j);
00609 
00613       void setUpActiveLinks();
00614 
00629       void checkActive(int i);
00630 
00634       void checkActiveReg(int i);
00635 
00639       virtual void setgTol(double tol);
00640 
00644       virtual void setgdTol(double tol);
00645 
00649       virtual void setgddTol(double tol);
00650 
00654       virtual void setlaTol(double tol);
00655 
00659       virtual void setLaTol(double tol);
00660 
00664       void setrMax(double rMax);
00665 
00666       void addFrame(FixedRelativeFrame *frame);
00667 
00672       int frameIndex(const Frame *frame_) const;
00673 
00677       void addGroup(DynamicSystem *dynamicsystem);
00678 
00684       DynamicSystem* getGroup(const std::string &name,bool check=true) const;
00685 
00689       void addObject(Object *object);
00690 
00696       Object* getObject(const std::string &name,bool check=true) const;
00697 
00701       void addLink(Link *link);
00702 
00706       void addInverseKineticsLink(Link *link);
00707 
00708       Observer* getObserver(const std::string &name,bool check=true) const;
00709       void addObserver(Observer *element);
00710 
00716       Link* getLink(const std::string &name,bool check=true) const;
00717 
00721       void addModel(ModellingInterface *modell);
00722 
00728       ModellingInterface* getModel(const std::string &name, bool check=true) const;
00729 
00731       FixedRelativeFrame *getFrameI() { return I; }
00732 
00733       virtual Element *getChildByContainerAndName(const std::string &container, const std::string &name) const;
00734 
00735       virtual void updatecorr(int j);
00736       void updatecorrRef(const fmatvec::Vec &ref);
00737       void calccorrSize(int j);
00738 
00739       void checkRoot();
00740 
00741     protected:
00745       Frame *R;
00746 
00750       fmatvec::Vec3 PrPF;
00751 
00755       fmatvec::SqrMat3 APF;
00756 
00760       std::vector<Object*> object;
00761       std::vector<Link*> link;
00762       std::vector<Link*> linkSingleValued;
00763       std::vector<Link*> linkSetValued;
00764       std::vector<Link*> linkSetValuedActive;
00765       std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00766       std::vector<ModellingInterface*> model;
00767       std::vector<DynamicSystem*> dynamicsystem;
00768       std::vector<Link*> inverseKineticsLink;
00769       std::vector<Observer*> observer;
00770 
00774       fmatvec::Mat T;
00775 
00779       fmatvec::SymMat M[2];
00780 
00784       fmatvec::SymMat LLM[2];
00785 
00789       fmatvec::Vec q, qd, q0;
00790 
00794       fmatvec::Vec u, ud[2], u0;
00795 
00799       fmatvec::Vec x, xd, x0;
00800 
00804       fmatvec::Vec h[2], r[2], f;
00805 
00809       fmatvec::Mat W[2], V[2];
00810 
00814       fmatvec::Vec la;
00815 
00819       fmatvec::Vec g, gd;
00820 
00824       fmatvec::Vec wb;
00825 
00829       fmatvec::Vec res;
00830 
00834       fmatvec::Vec rFactor;
00835 
00839       fmatvec::Vec sv;
00840 
00844       fmatvec::VecInt jsv;
00845 
00849       fmatvec::VecInt LinkStatus;
00850 
00854       fmatvec::VecInt LinkStatusReg;
00855 
00859       int qSize, qInd;
00860 
00864       int uSize[2], uInd[2];
00865 
00869       int xSize, xInd;
00870 
00874       int hSize[2], hInd[2];
00875 
00879       int gSize, gInd;
00880 
00884       int gdSize, gdInd;
00885 
00889       int laSize, laInd;
00890 
00894       int rFactorSize, rFactorInd;
00895 
00899       int svSize, svInd;
00900 
00904       int LinkStatusSize, LinkStatusInd;
00905       
00909       int LinkStatusRegSize, LinkStatusRegInd;
00910       
00914       std::vector<Frame*> frame;
00915       std::vector<Contour*> contour;
00916 
00917 #ifdef HAVE_OPENMBVCPPINTERFACE
00918       OpenMBV::Group* openMBVGrp;
00919 #endif
00920       boost::shared_ptr<H5::File> hdf5File;
00921 
00925       void addContour(Contour* contour);
00926 
00928       FixedRelativeFrame *I;
00929 
00933       int laInverseKineticsSize, bInverseKineticsSize;
00934 
00935       fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00936       fmatvec::Vec laInverseKinetics;
00937 
00938       int corrSize, corrInd;
00939       fmatvec::Vec corr;
00940 
00941       std::string saved_frameOfReference;
00942   };
00943 }
00944 
00945 #endif
00946