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 updateStateDependentVariables(double t) = 0;
00073       virtual void updateStateDerivativeDependentVariables(double t);
00074       virtual void updateM(double t, int i=0);
00075       virtual void updateJacobians(double t, int j=0) = 0;
00076       virtual void updatedq(double t, double dt); 
00077       virtual void updateud(double t, int i=0) { THROW_MBSIMERROR("(DynamicSystem::updateud): Not implemented!"); }
00078       virtual void updatezd(double t) = 0;
00079       virtual void updatedu(double t, double dt) = 0;
00080       virtual void updateqd(double t);
00081       virtual void sethSize(int hSize_, int i=0);
00082       virtual int gethSize(int i=0) const { return hSize[i]; }
00083       virtual int getqSize() const { return qSize; }
00084       virtual int getuSize(int i=0) const { return uSize[i]; }
00085       virtual void calcqSize();
00086       virtual void calcuSize(int j=0);
00087       //virtual int getqInd(DynamicSystem* sys);
00088       virtual int getuInd(int i=0) { return uInd[i]; }
00089       //virtual int getuInd(DynamicSystem* sys, int i=0);
00090       //virtual void setqInd(int qInd_) { qInd = qInd_; }
00091       //virtual void setuInd(int uInd_, int i=0) { uInd[i] = uInd_; }
00092       virtual void setqInd(int qInd_);
00093       virtual void setuInd(int uInd_, int i=0);
00094       virtual void sethInd(int hInd_, int i=0);
00095       virtual void setxInd(int xInd_);
00096       //virtual int gethInd(DynamicSystem* sys, int i=0); 
00097       virtual const fmatvec::Vec& getq() const { return q; };
00098       virtual fmatvec::Vec& getq() { return q; };
00099       virtual const fmatvec::Vec& getu() const { return u; };
00100       virtual fmatvec::Vec& getu() { return u; };
00101       virtual H5::GroupBase *getPlotGroup() { return plotGroup; }
00102       virtual PlotFeatureStatus getPlotFeature(PlotFeature fp) { return Element::getPlotFeature(fp); };
00103       virtual PlotFeatureStatus getPlotFeatureForChildren(PlotFeature fp) { return Element::getPlotFeatureForChildren(fp); };
00104 #ifdef HAVE_OPENMBVCPPINTERFACE
00105       virtual boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp();
00106 #endif
00107 
00108       virtual void updatewb(double t, int j=0); 
00109       virtual void updateW(double t, int j=0); 
00110       virtual void updateV(double t, int j=0); 
00111       virtual void updateg(double t);
00112       virtual void updategd(double t);
00113       virtual void updateStopVector(double t); 
00114       virtual void updateLinkStatus(double t);
00115       virtual void updateLinkStatusReg(double t);
00116 
00117       virtual void updategInverseKinetics(double t); 
00118       virtual void updategdInverseKinetics(double t);
00119       virtual void updateWInverseKinetics(double t, int j=0); 
00120       virtual void updatehInverseKinetics(double t, int j=0); 
00121       virtual void updateJacobiansInverseKinetics(double t, int j=0); 
00122       virtual void updatebInverseKinetics(double t); 
00123 
00124       virtual void updatedx(double t, double dt); 
00125       virtual void updatexd(double t);
00126       virtual void calcxSize();
00127       const fmatvec::Vec& getx() const { return x; };
00128       fmatvec::Vec& getx() { return x; };
00129       int getxSize() const { return xSize; }
00130       void updatexRef(const fmatvec::Vec &ref);
00131       void updatexdRef(const fmatvec::Vec &ref);
00132       virtual void init(InitStage stage);
00133       virtual void initz();
00134       virtual void writez(H5::GroupBase *group);
00135       virtual void readz0(H5::GroupBase *parent);
00136       /*****************************************************/
00137 
00138       /* INHERITED INTERFACE OF ELEMENT */
00140       virtual std::string getType() const { return "DynamicSystem"; }
00141       virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00142       virtual void plot(double t, double dt);
00143       virtual void plotAtSpecialEvent(double t, double dt=1.);
00144       virtual void closePlot();
00145       /*****************************************************/
00146 
00147       /* INTERFACE FOR DERIVED CLASSES */
00151       virtual void facLLM(int i=0) = 0;
00152 
00157       virtual int solveConstraintsFixpointSingle();
00158 
00164       virtual int solveImpactsFixpointSingle(double dt);
00165 
00170       virtual int solveConstraintsGaussSeidel();
00171 
00177       virtual int solveImpactsGaussSeidel(double dt);
00178 
00183       virtual int solveConstraintsRootFinding();
00184 
00190       virtual int solveImpactsRootFinding(double dt);
00191 
00195       virtual int jacobianConstraints();
00196 
00200       virtual int jacobianImpacts();
00201 
00205       virtual void checkConstraintsForTermination();
00206 
00210       virtual void checkImpactsForTermination(double dt);
00211 
00215       virtual void updaterFactors();
00216 
00222       virtual Frame* getFrame(const std::string &name, bool check=true) const;
00223 
00229       virtual Contour* getContour(const std::string &name, bool check=true) const;
00230       /*****************************************************/
00231 
00232       /* GETTER / SETTER */
00233       void setPosition(const fmatvec::Vec3& PrPF_) { PrPF = PrPF_; }
00234       void setOrientation(const fmatvec::SqrMat3& APF_) { APF = APF_; }
00235       void setFrameOfReference(Frame *frame) { R = frame; };
00236       const fmatvec::Vec3& getPosition() const { return PrPF; }
00237       const fmatvec::SqrMat3& getOrientation() const { return APF; }
00238       const Frame* getFrameOfReference() const { return R; };
00239 
00240       const fmatvec::Vec& getxd() const { return xd; };
00241       fmatvec::Vec& getxd() { return xd; };
00242       const fmatvec::Vec& getx0() const { return x0; };
00243       fmatvec::Vec& getx0() { return x0; };
00244 
00245       const fmatvec::Mat& getT() const { return T; };
00246       const fmatvec::SymMat& getM(int i=0) const { return M[i]; };
00247       const fmatvec::SymMat& getLLM(int i=0) const { return LLM[i]; };
00248       fmatvec::SymMat& getLLM(int i=0) { return LLM[i]; };
00249       const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00250       fmatvec::Vec& geth(int j=0) { return h[j]; };
00251       const fmatvec::Vec& getf() const { return f; };
00252       fmatvec::Vec& getf() { return f; };
00253 
00254       const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00255       fmatvec::Mat& getW(int i=0) { return W[i]; }
00256       const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00257       fmatvec::Mat& getV(int i=0) { return V[i]; }
00258 
00259       const fmatvec::Vec& getla() const { return la; }
00260       fmatvec::Vec& getla() { return la; }
00261       const fmatvec::Vec& getg() const { return g; }
00262       fmatvec::Vec& getg() { return g; }
00263       const fmatvec::Vec& getgd() const { return gd; }
00264       fmatvec::Vec& getgd() { return gd; }
00265       const fmatvec::Vec& getrFactor() const { return rFactor; }
00266       fmatvec::Vec& getrFactor() { return rFactor; }
00267       fmatvec::Vec& getsv() { return sv; }
00268       const fmatvec::Vec& getsv() const { return sv; }
00269       fmatvec::VecInt& getjsv() { return jsv; }
00270       const fmatvec::VecInt& getjsv() const { return jsv; }
00271       fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00272       fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00273       const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00274       const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00275       const fmatvec::Vec& getres() const { return res; }
00276       fmatvec::Vec& getres() { return res; }
00277       const fmatvec::Vec& getcorr() const { return corr; };
00278       fmatvec::Vec& getcorr() { return corr; };
00279 
00280       void setx(const fmatvec::Vec& x_) { x = x_; }
00281       void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00282       void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00283 
00284       int getxInd() { return xInd; }
00285       int getlaInd() const { return laInd; } 
00286 
00287       int gethInd(int i=0) { return hInd[i]; }
00288       void setlaInd(int ind) { laInd = ind; }
00289       void setgInd(int ind) { gInd = ind; }
00290       void setgdInd(int ind) { gdInd = ind; }
00291       void setrFactorInd(int ind) { rFactorInd = ind; }
00292       virtual void setsvInd(int svInd_);
00293       void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};      
00294       void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00295 
00296       int getzSize() const { return qSize + uSize[0] + xSize; }
00297 
00298       void setqSize(int qSize_) { qSize = qSize_; }
00299       void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00300       void setxSize(int xSize_) { xSize = xSize_; }
00301 
00302       int getlaSize() const { return laSize; } 
00303       int getgSize() const { return gSize; } 
00304       int getgdSize() const { return gdSize; } 
00305       int getrFactorSize() const { return rFactorSize; } 
00306       int getsvSize() const { return svSize; }
00307       int getLinkStatusSize() const { return LinkStatusSize; }
00308       int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00309       /*****************************************************/
00310 
00311       const std::vector<Object*>& getObjects() const { return object; }
00312       const std::vector<Link*>& getLinks() const { return link; }
00313       const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00314       const std::vector<Frame*>& getFrames() const { return frame; }
00315       const std::vector<Contour*>& getContours() const { return contour; }
00316 
00321       void updateqRef(const fmatvec::Vec &ref); 
00322 
00327       void updateqdRef(const fmatvec::Vec &ref);
00328 
00333       void updateuRef(const fmatvec::Vec &ref);
00334 
00339       void updateuallRef(const fmatvec::Vec &ref);
00340 
00345       void updateudRef(const fmatvec::Vec &ref, int i=0);
00346 
00351       void updateudallRef(const fmatvec::Vec &ref, int i=0);
00352 
00360       void updatehRef(const fmatvec::Vec &hRef, int i=0);
00361 
00366       void updaterRef(const fmatvec::Vec &ref, int j=0);
00367 
00372       void updateTRef(const fmatvec::Mat &ref);
00373 
00379       void updateMRef(const fmatvec::SymMat &ref, int i=0);
00380 
00386       void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00387 
00392       void updategRef(const fmatvec::Vec &ref);
00393 
00398       void updategdRef(const fmatvec::Vec &ref);
00399 
00404       void updatelaRef(const fmatvec::Vec &ref);
00405 
00406       void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00407       void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00408 
00413       void updatewbRef(const fmatvec::Vec &ref);
00414 
00420       void updateWRef(const fmatvec::Mat &ref, int i=0);
00421 
00427       void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00428 
00434       void updateVRef(const fmatvec::Mat &ref, int i=0);
00435 
00440       void updatesvRef(const fmatvec::Vec& ref);
00441 
00446       void updatejsvRef(const fmatvec::VecInt &ref);
00447 
00452       void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00453 
00458       void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);      
00459             
00464       void updateresRef(const fmatvec::Vec &ref);
00465 
00470       void updaterFactorRef(const fmatvec::Vec &ref);
00471 
00472       void clearElementLists();
00473 
00478       void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00479 
00484       void buildListOfObjects(std::vector<Object*> &obj);
00485 
00490       void buildListOfLinks(std::vector<Link*> &lnk);
00491 
00496       void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00497 
00502       void buildListOfFrames(std::vector<Frame*> &frm);
00503 
00508       void buildListOfContours(std::vector<Contour*> &cnt);
00509 
00514       void buildListOfModels(std::vector<ModellingInterface*> &model);
00515 
00520       void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00521 
00526       void buildListOfObservers(std::vector<Observer*> &obsrv);
00527 
00531       void setUpInverseKinetics();
00532 
00536       void setUpLinks();
00537 
00541       bool gActiveChanged();
00542       
00546       bool gActiveChangedReg();
00547       
00551       bool detectImpact();
00552 
00556       void calcsvSize();
00557 
00561       void calclaSize(int j);
00562 
00566       void calcLinkStatusSize();
00567 
00571       void calcLinkStatusRegSize();
00572 
00576       void calclaInverseKineticsSize();
00577 
00581       void calcbInverseKineticsSize();
00582 
00586       void calcgSize(int j);
00587 
00598       void calcgdSize(int j);
00599 
00603       void calcrFactorSize(int j);
00604 
00608       void setUpActiveLinks();
00609 
00624       void checkActive(int i);
00625 
00629       void checkActiveReg(int i);
00630 
00634       virtual void setgTol(double tol);
00635 
00639       virtual void setgdTol(double tol);
00640 
00644       virtual void setgddTol(double tol);
00645 
00649       virtual void setlaTol(double tol);
00650 
00654       virtual void setLaTol(double tol);
00655 
00659       void setrMax(double rMax);
00660 
00661       void addFrame(FixedRelativeFrame *frame);
00662 
00667       int frameIndex(const Frame *frame_) const;
00668 
00672       void addGroup(DynamicSystem *dynamicsystem);
00673 
00679       DynamicSystem* getGroup(const std::string &name,bool check=true) const;
00680 
00684       void addObject(Object *object);
00685 
00691       Object* getObject(const std::string &name,bool check=true) const;
00692 
00696       void addLink(Link *link);
00697 
00701       void addInverseKineticsLink(Link *link);
00702 
00703       Observer* getObserver(const std::string &name,bool check=true) const;
00704       void addObserver(Observer *element);
00705 
00711       Link* getLink(const std::string &name,bool check=true) const;
00712 
00716       void addModel(ModellingInterface *modell);
00717 
00723       ModellingInterface* getModel(const std::string &name, bool check=true) const;
00724 
00726       FixedRelativeFrame *getFrameI() { return I; }
00727 
00728       virtual Element *getChildByContainerAndName(const std::string &container, const std::string &name) const;
00729 
00730       virtual void updatecorr(int j);
00731       void updatecorrRef(const fmatvec::Vec &ref);
00732       void calccorrSize(int j);
00733 
00734       void checkRoot();
00735 
00736     protected:
00740       Frame *R;
00741 
00745       fmatvec::Vec3 PrPF;
00746 
00750       fmatvec::SqrMat3 APF;
00751 
00755       std::vector<Object*> object;
00756       std::vector<Link*> link;
00757       std::vector<Link*> linkSingleValued;
00758       std::vector<Link*> linkSetValued;
00759       std::vector<Link*> linkSetValuedActive;
00760       std::vector<ModellingInterface*> model;
00761       std::vector<DynamicSystem*> dynamicsystem;
00762       std::vector<Link*> inverseKineticsLink;
00763       std::vector<Observer*> observer;
00764       std::vector< std::vector<Link*> > linkOrdered;
00765 
00769       fmatvec::Mat T;
00770 
00774       fmatvec::SymMat M[2];
00775 
00779       fmatvec::SymMat LLM[2];
00780 
00784       fmatvec::Vec q, qd, q0;
00785 
00789       fmatvec::Vec u, ud[2], u0;
00790 
00794       fmatvec::Vec x, xd, x0;
00795 
00799       fmatvec::Vec h[2], r[2], f;
00800 
00804       fmatvec::Mat W[2], V[2];
00805 
00809       fmatvec::Vec la;
00810 
00814       fmatvec::Vec g, gd;
00815 
00819       fmatvec::Vec wb;
00820 
00824       fmatvec::Vec res;
00825 
00829       fmatvec::Vec rFactor;
00830 
00834       fmatvec::Vec sv;
00835 
00839       fmatvec::VecInt jsv;
00840 
00844       fmatvec::VecInt LinkStatus;
00845 
00849       fmatvec::VecInt LinkStatusReg;
00850 
00854       int qSize, qInd;
00855 
00859       int uSize[2], uInd[2];
00860 
00864       int xSize, xInd;
00865 
00869       int hSize[2], hInd[2];
00870 
00874       int gSize, gInd;
00875 
00879       int gdSize, gdInd;
00880 
00884       int laSize, laInd;
00885 
00889       int rFactorSize, rFactorInd;
00890 
00894       int svSize, svInd;
00895 
00899       int LinkStatusSize, LinkStatusInd;
00900       
00904       int LinkStatusRegSize, LinkStatusRegInd;
00905       
00909       std::vector<Frame*> frame;
00910       std::vector<Contour*> contour;
00911 
00912 #ifdef HAVE_OPENMBVCPPINTERFACE
00913       boost::shared_ptr<OpenMBV::Group> openMBVGrp;
00914 #endif
00915       boost::shared_ptr<H5::File> hdf5File;
00916 
00920       void addContour(Contour* contour);
00921 
00923       FixedRelativeFrame *I;
00924 
00928       int laInverseKineticsSize, bInverseKineticsSize;
00929 
00930       fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00931       fmatvec::Vec laInverseKinetics;
00932 
00933       int corrSize, corrInd;
00934       fmatvec::Vec corr;
00935 
00936       std::string saved_frameOfReference;
00937   };
00938 }
00939 
00940 #endif
00941