MBSim

Project Page
dynamic_system_solver.h
00001 /* Copyright (C) 2004-2014 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or
00004  * modify it under the terms of the GNU Lesser General Public
00005  * License as published by the Free Software Foundation; either
00006  * version 2.1 of the License, or (at your option) any later version.
00007  *
00008  * This library is distributed in the hope that it will be useful,
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011  * Lesser General Public License for more details.
00012  *
00013  * You should have received a copy of the GNU Lesser General Public
00014  * License along with this library; if not, write to the Free Software
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  *          rzander@users.berlios.de
00019  */
00020 
00021 #ifndef _DYNAMIC_SYSTEM_SOLVER_H_
00022 #define _DYNAMIC_SYSTEM_SOLVER_H_
00023 
00024 #include "mbsim/group.h"
00025 #include "fmatvec/sparse_matrix.h"
00026 
00027 namespace MBSim {
00028 
00029   class Graph;
00030 
00049   class DynamicSystemSolver : public Group {
00050     public:
00051 
00055       enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
00056 
00060       enum Strategy { global, local };
00061 
00065       enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
00066 
00071       DynamicSystemSolver(const std::string &name="");
00072 
00076       virtual ~DynamicSystemSolver();
00077 
00116       void initialize();
00117 
00118       /*
00119        * If true (the default) then
00120        * the simulation output files (h5 files) are deleted/truncated/regenerated.
00121        * If false, then these files left are untouched from a previous run.
00122        * This is useful to regenerate the
00123        * e.g. OpenMBV XML files without doing a new integration.
00124        */
00125       void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00126 
00127       /* INHERITED INTERFACE OF GROUP */
00128       void init(InitStage stage);
00129       using Group::plot;
00130       /***************************************************/
00131 
00132       /* INHERITED INTERFACE OF DYNAMICSYSTEM */
00133       virtual int solveConstraintsFixpointSingle();
00134       virtual int solveImpactsFixpointSingle(double dt = 0);
00135       virtual int solveConstraintsGaussSeidel();
00136       virtual int solveImpactsGaussSeidel(double dt = 0);
00137       virtual int solveConstraintsRootFinding();
00138       virtual int solveImpactsRootFinding(double dt = 0);
00139       virtual void checkConstraintsForTermination();
00140       virtual void checkImpactsForTermination(double dt = 0);
00141       /***************************************************/
00142 
00143       /* INHERITED INTERFACE OF OBJECTINTERFACE */
00144       virtual void updateh(double t, int i=0);
00145       virtual void updateM(double t, int i=0);
00146       virtual void updateStateDependentVariables(double t); // this function is called once every time step by every integrator
00147       /***************************************************/
00148 
00149       /* INHERITED INTERFACE OF LINKINTERFACE */
00150 
00161       virtual void updater(double t, int j=0);
00162       virtual void updatewb(double t, int j=0);
00163       virtual void updateW(double t, int j=0);
00164       virtual void updateV(double t, int j=0);
00165       /***************************************************/
00166 
00167       /* INHERITED INTERFACE OF ELEMENT */
00169       virtual std::string getType() const { return "DynamicSystemSolver"; }
00170       virtual void plot(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00171       virtual void plot2(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00172 
00173       virtual void closePlot();
00174       /***************************************************/
00175 
00176       /* INHERITED INTERFACE FOR DERIVED CLASS */
00181       virtual int solveConstraints();
00182 
00188       virtual int solveImpacts(double dt = 0);
00189       /***************************************************/
00190 
00191       /* GETTER / SETTER */
00192 
00193       void setConstraintSolver(Solver solver_) { contactSolver = solver_; }
00194       void setImpactSolver(Solver solver_) { impactSolver = solver_; }
00195       const Solver& getConstraintSolver() { return contactSolver; }
00196       const Solver& getImpactSolver() { return impactSolver; }
00197       void setTermination(bool term_) { term = term_; }
00198       void setStrategy(Strategy strategy_) { strategy = strategy_; }
00199       void setMaxIter(int iter) { maxIter = iter; }
00200       void setHighIter(int iter) { highIter = iter; }
00201       void setNumJacProj(bool numJac_) { numJac = numJac_; }
00202       void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
00203       void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
00204       void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
00205 
00206       void setUseOldla(bool flag) { useOldla = flag; }
00207       void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
00208       void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
00209       void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
00210       void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
00211 
00212       const fmatvec::SparseMat& getGs() const { return Gs; }
00213       fmatvec::SparseMat& getGs() { return Gs; }
00214       const fmatvec::SqrMat& getG() const { return G; }
00215       fmatvec::SqrMat& getG() { return G; }
00216       const fmatvec::Vec& getb() const { return b; }
00217       fmatvec::Vec& getb() { return b; }
00218       const fmatvec::SqrMat& getJprox() const { return Jprox; }
00219       fmatvec::SqrMat& getJprox() { return Jprox; }
00220 
00221       const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
00222       const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
00223       const fmatvec::Vec& getlaParent() const { return laParent; }
00224       const fmatvec::Vec& getgdParent() const { return gdParent; }
00225       const fmatvec::Vec& getresParent() const { return resParent; }
00226       const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
00227 
00228       DynamicSystemSolver* getDynamicSystemSolver() { return this; }
00229       bool getIntegratorExitRequest() { return integratorExitRequest; }
00230       int getMaxIter()  {return maxIter;}
00231       /***************************************************/
00232 
00236       void computeInitialCondition();
00237 
00244       fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
00245 
00252       fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
00253 
00260       fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
00261 
00265       virtual void initz(fmatvec::Vec& z0);
00266 
00271       int (DynamicSystemSolver::*solveConstraints_)();
00272 
00278       int (DynamicSystemSolver::*solveImpacts_)(double dt);
00279 
00285       int solveConstraintsLinearEquations();
00286 
00293       int solveImpactsLinearEquations(double dt = 0);
00294 
00299       virtual void updateG(double t, int i=0);
00300 
00304       void decreaserFactors();
00305 
00313       void update(const fmatvec::Vec &z, double t, int options=0);
00314 
00321       virtual void shift(fmatvec::Vec& z, const fmatvec::VecInt& jsv, double t);
00322 
00329       virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
00330 
00336       virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
00337 
00344       virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
00345 
00350       void getLinkStatus(fmatvec::VecInt &LinkStatusExt, double t);
00351 
00357       void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt, double t);
00362       void projectGeneralizedPositions(double t, int mode, bool fullUpdate=false);
00363 
00368       void projectGeneralizedVelocities(double t, int mode);
00369 
00374       void savela(double dt=1.0);
00375 
00380       void initla(double dt=1.0);
00381 
00385       double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
00386 
00391       double computePotentialEnergy();
00392 
00397       void addElement(Element *element_);
00398 
00404       Element* getElement(const std::string &name);
00405 
00409       std::string getSolverInfo();
00410 
00415       void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
00416 
00420       void dropContactMatrices();
00421 
00425       static void sigInterruptHandler(int);
00426 
00430       static void sigAbortHandler(int);
00431 
00432       static void sigSegfaultHandler(int);
00433 
00434       // TODO just for testing
00435       void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
00436 
00442       void writez(std::string fileName, bool formatH5=true);
00443 
00448       void readz0(std::string fileName);
00449 
00450       virtual void initializeUsingXML(xercesc::DOMElement *element);
00451       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00452 
00453       static DynamicSystemSolver* readXMLFile(const std::string &filename);
00454       void writeXMLFile(const std::string &name);
00455       void writeXMLFile() { writeXMLFile(getName()); }
00456 
00457 
00462       void setProjectionTolerance(double tol) { tolProj = tol; }
00463 
00467       void setgTol(double tol) {gTol = tol; Group::setgTol(tol);}
00468 
00472       void setgdTol(double tol) {gdTol = tol; Group::setgdTol(tol);}
00473 
00477       void setgddTol(double tol) {gddTol = tol; Group::setgddTol(tol);}
00478 
00482       void setlaTol(double tol) {laTol = tol; Group::setlaTol(tol);}
00483 
00487       void setLaTol(double tol) {LaTol = tol; Group::setLaTol(tol);}
00488 
00493       void updatezRef(const fmatvec::Vec &ext);
00494 
00499       void setFlushEvery(unsigned int every) {flushEvery = every;}
00500 
00501       void setAlwaysConsiderContact(bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
00502 
00503       void setInverseKinetics(bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
00504 
00505       void setInitialProjection(bool initialProjection_) {initialProjection = initialProjection_;}
00506 
00507       fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
00508       fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
00509       fmatvec::Mat dhdx(double t);
00510       fmatvec::Vec dhdt(double t);
00511 
00512       void setRootID(int ID) {rootID = ID;}
00513       int getRootID() const {return rootID;}
00514       void setq(const fmatvec::Vec& q_){ q = q_;}
00515       void setLa(const fmatvec::Vec& la_){ la = la_;}
00516 
00517     protected:
00521       fmatvec::SymMat MParent[2];
00522 
00526       fmatvec::SymMat LLMParent[2];
00527 
00531       fmatvec::Mat TParent;
00532 
00536       fmatvec::Mat WParent[2];
00537 
00541       fmatvec::Mat VParent[2];
00542 
00546       fmatvec::Vec wbParent;
00547 
00551       fmatvec::Vec laParent;
00552 
00556       fmatvec::Vec rFactorParent;
00557 
00561       fmatvec::Vec sParent;
00562 
00566       fmatvec::Vec resParent;
00567 
00571       fmatvec::Vec gParent;
00572 
00576       fmatvec::Vec gdParent;
00577 
00581       fmatvec::Vec zdParent;
00582 
00583       fmatvec::Vec udParent1;
00584 
00588       fmatvec::Vec hParent[2];
00589 
00593       fmatvec::Vec rParent[2];
00594 
00598       fmatvec::Vec fParent;
00599 
00603       fmatvec::Vec svParent;
00604 
00608       fmatvec::VecInt jsvParent;
00609 
00613       fmatvec::VecInt LinkStatusParent;
00614 
00619       fmatvec::VecInt LinkStatusRegParent;
00620 
00624       fmatvec::SparseMat Gs;
00625 
00629       fmatvec::SqrMat Jprox;
00630 
00634       fmatvec::SqrMat G;
00635 
00639       fmatvec::Vec b;
00640 
00644       bool term;
00645 
00649       int maxIter, highIter, maxDampingSteps;
00650 
00654       double lmParm;
00655 
00659       Solver contactSolver, impactSolver;
00660 
00664       Strategy strategy;
00665 
00669       LinAlg linAlg;
00670 
00674       bool stopIfNoConvergence;
00675 
00679       bool dropContactInfo;
00680 
00684       bool useOldla;
00685 
00689       bool numJac;
00690 
00694       fmatvec::VecInt decreaseLevels;
00695 
00699       fmatvec::VecInt checkTermLevels;
00700 
00704       bool checkGSize;
00705 
00709       int limitGSize;
00710 
00714       int warnLevel;
00715 
00719       bool peds;
00720 
00724       unsigned int driftCount;
00725 
00730       unsigned int flushEvery;
00731 
00736       unsigned int flushCount;
00737 
00741       double tolProj;
00742 
00747       void updatezdRef(const fmatvec::Vec &ext);
00748 
00753       virtual void updaterFactors();
00754 
00759       void computeConstraintForces(double t);
00760 
00764       fmatvec::Vec laInverseKineticsParent;
00765       fmatvec::Mat bInverseKineticsParent;
00766 
00770       fmatvec::Mat WInverseKineticsParent[2];
00771 
00772       bool alwaysConsiderContact;
00773       bool inverseKinetics;
00774       bool initialProjection;
00775 
00776       fmatvec::Vec corrParent;
00777 
00778       int rootID;
00779 
00780       double gTol, gdTol, gddTol, laTol, LaTol;
00781 
00782     private:
00786       void constructor();
00787 
00791       bool integratorExitRequest;
00792 
00796       static bool exitRequest;
00797 
00801       bool READZ0;
00802 
00811       void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
00812 
00813       bool truncateSimulationFiles;
00814 
00815       // Holds the dynamic systems before the "reorganize hierarchie" takes place.
00816       // This is required since all elements of all other containers from DynamicSystem are readded to DynamicSystemSolver,
00817       // except this container (which is a "pure" container = no calculation is done in DynamicSystem)
00818       // However, we must hold this container until the dtor of DynamicSystemSolver is called to avoid the deallocation of other
00819       // elements hold by DynamicSystem elements (especially (currently only) hdf5File)
00820       std::vector<DynamicSystem*> dynamicsystemPreReorganize;
00821   };
00822 
00823 }
00824 
00825 #endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */
00826