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 
00070       DynamicSystemSolver();
00071 
00076       DynamicSystemSolver(const std::string &projectName);
00077 
00081       virtual ~DynamicSystemSolver();
00082 
00121       void initialize();
00122 
00123       /*
00124        * If true (the default) then
00125        * the simulation output files (h5 files) are deleted/truncated/regenerated.
00126        * If false, then these files left are untouched from a previous run.
00127        * This is useful to regenerate the
00128        * e.g. OpenMBV XML files without doing a new integration.
00129        */
00130       void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00131 
00132       /* INHERITED INTERFACE OF GROUP */
00133       void init(InitStage stage);
00134       using Group::plot;
00135       /***************************************************/
00136 
00137       /* INHERITED INTERFACE OF DYNAMICSYSTEM */
00138       virtual int solveConstraintsFixpointSingle();
00139       virtual int solveImpactsFixpointSingle(double dt = 0);
00140       virtual int solveConstraintsGaussSeidel();
00141       virtual int solveImpactsGaussSeidel(double dt = 0);
00142       virtual int solveConstraintsRootFinding();
00143       virtual int solveImpactsRootFinding(double dt = 0);
00144       virtual void checkConstraintsForTermination();
00145       virtual void checkImpactsForTermination(double dt = 0);
00146       /***************************************************/
00147 
00148       /* INHERITED INTERFACE OF OBJECTINTERFACE */
00149       virtual void updateh(double t, int i=0);
00150       virtual void updateh0Fromh1(double t);
00151       virtual void updateW0FromW1(double t);
00152       virtual void updateV0FromV1(double t);
00153       virtual void updateM(double t, int i=0);
00154       virtual void updateStateDependentVariables(double t); // this function is called once every time step by every integrator
00155       /***************************************************/
00156 
00157       /* INHERITED INTERFACE OF LINKINTERFACE */
00158 
00169       virtual void updater(double t, int j=0);
00170       virtual void updatewb(double t, int j=0);
00171       virtual void updateW(double t, int j=0);
00172       virtual void updateV(double t, int j=0);
00173       /***************************************************/
00174 
00175       /* INHERITED INTERFACE OF ELEMENT */
00177       virtual std::string getType() const { return "DynamicSystemSolver"; }
00178       virtual void plot(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00179       virtual void plot2(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00180 
00181       virtual void closePlot();
00182       /***************************************************/
00183 
00184       /* INHERITED INTERFACE FOR DERIVED CLASS */
00189       virtual int solveConstraints();
00190 
00196       virtual int solveImpacts(double dt = 0);
00197       /***************************************************/
00198 
00199       /* GETTER / SETTER */
00200 
00201       void setConstraintSolver(Solver solver_) { contactSolver = solver_; }
00202       void setImpactSolver(Solver solver_) { impactSolver = solver_; }
00203       const Solver& getConstraintSolver() { return contactSolver; }
00204       const Solver& getImpactSolver() { return impactSolver; }
00205       void setTermination(bool term_) { term = term_; }
00206       void setStrategy(Strategy strategy_) { strategy = strategy_; }
00207       void setMaxIter(int iter) { maxIter = iter; }
00208       void setHighIter(int iter) { highIter = iter; }
00209       void setNumJacProj(bool numJac_) { numJac = numJac_; }
00210       void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
00211       void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
00212       void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
00213 
00214       void setUseOldla(bool flag) { useOldla = flag; }
00215       void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
00216       void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
00217       void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
00218       void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
00219 
00220       const fmatvec::SparseMat& getGs() const { return Gs; }
00221       fmatvec::SparseMat& getGs() { return Gs; }
00222       const fmatvec::SqrMat& getG() const { return G; }
00223       fmatvec::SqrMat& getG() { return G; }
00224       const fmatvec::Vec& getb() const { return b; }
00225       fmatvec::Vec& getb() { return b; }
00226       const fmatvec::SqrMat& getJprox() const { return Jprox; }
00227       fmatvec::SqrMat& getJprox() { return Jprox; }
00228 
00229       const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
00230       const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
00231       const fmatvec::Vec& getlaParent() const { return laParent; }
00232       const fmatvec::Vec& getgdParent() const { return gdParent; }
00233       const fmatvec::Vec& getresParent() const { return resParent; }
00234       const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
00235 
00236       DynamicSystemSolver* getDynamicSystemSolver() { return this; }
00237       bool getIntegratorExitRequest() { return integratorExitRequest; }
00238       int getMaxIter()  {return maxIter;}
00239       /***************************************************/
00240 
00244       void computeInitialCondition();
00245 
00252       fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
00253 
00260       fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
00261 
00268       fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
00269 
00273       virtual void initz(fmatvec::Vec& z0);
00274 
00279       int (DynamicSystemSolver::*solveConstraints_)();
00280 
00286       int (DynamicSystemSolver::*solveImpacts_)(double dt);
00287 
00293       int solveConstraintsLinearEquations();
00294 
00301       int solveImpactsLinearEquations(double dt = 0);
00302 
00307       virtual void updateG(double t, int i=0);
00308 
00312       void decreaserFactors();
00313 
00321       void update(const fmatvec::Vec &z, double t, int options=0);
00322 
00329       virtual void shift(fmatvec::Vec& z, const fmatvec::VecInt& jsv, double t);
00330 
00337       virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
00338 
00344       virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
00345 
00352       virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
00353 
00358       void getLinkStatus(fmatvec::VecInt &LinkStatusExt, double t);
00359 
00365       void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt, double t);
00370       void projectGeneralizedPositions(double t, int mode);
00371 
00376       void projectGeneralizedVelocities(double t, int mode);
00377 
00382       void savela(double dt=1.0);
00383 
00388       void initla(double dt=1.0);
00389 
00393       double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
00394 
00399       double computePotentialEnergy();
00400 
00405       void addElement(Element *element_);
00406 
00412       Element* getElement(const std::string &name);
00413 
00417       std::string getSolverInfo();
00418 
00423       void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
00424 
00428       void dropContactMatrices();
00429 
00433       static void sigInterruptHandler(int);
00434 
00438       static void sigAbortHandler(int);
00439 
00440       static void sigSegfaultHandler(int);
00441 
00442       // TODO just for testing
00443       void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
00444 
00450       void writez(std::string fileName, bool formatH5=true);
00451 
00456       void readz0(std::string fileName);
00457 
00458       virtual void initializeUsingXML(xercesc::DOMElement *element);
00459       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00460 
00461       static DynamicSystemSolver* readXMLFile(const std::string &filename);
00462       void writeXMLFile(const std::string &name);
00463       void writeXMLFile() { writeXMLFile(getName()); }
00464 
00465 
00470       void setProjectionTolerance(double tol) { tolProj = tol; }
00471 
00475       void setgTol(double tol) {gTol = tol; Group::setgTol(tol);}
00476 
00480       void setgdTol(double tol) {gdTol = tol; Group::setgdTol(tol);}
00481 
00485       void setgddTol(double tol) {gddTol = tol; Group::setgddTol(tol);}
00486 
00490       void setlaTol(double tol) {laTol = tol; Group::setlaTol(tol);}
00491 
00495       void setLaTol(double tol) {LaTol = tol; Group::setLaTol(tol);}
00496 
00501       void updatezRef(const fmatvec::Vec &ext);
00502 
00507       void setFlushEvery(unsigned int every) {flushEvery = every;}
00508 
00509       void setAlwaysConsiderContact(bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
00510 
00511       void setInverseKinetics(bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
00512 
00513       fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
00514       fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
00515       fmatvec::Mat dhdx(double t);
00516       fmatvec::Vec dhdt(double t);
00517 
00518       void setRootID(int ID) {rootID = ID;}
00519       int getRootID() const {return rootID;}
00520     protected:
00524       fmatvec::SymMat MParent[2];
00525 
00529       fmatvec::SymMat LLMParent[2];
00530 
00534       fmatvec::Mat TParent;
00535 
00539       fmatvec::Mat WParent[2];
00540 
00544       fmatvec::Mat VParent[2];
00545 
00549       fmatvec::Vec wbParent;
00550 
00554       fmatvec::Vec laParent;
00555 
00559       fmatvec::Vec rFactorParent;
00560 
00564       fmatvec::Vec sParent;
00565 
00569       fmatvec::Vec resParent;
00570 
00574       fmatvec::Vec gParent;
00575 
00579       fmatvec::Vec gdParent;
00580 
00584       fmatvec::Vec zdParent;
00585 
00586       fmatvec::Vec udParent1;
00587 
00591       fmatvec::Vec hParent[2];
00592 
00596       fmatvec::Vec rParent[2];
00597 
00601       fmatvec::Vec fParent;
00602 
00606       fmatvec::Vec svParent;
00607 
00611       fmatvec::VecInt jsvParent;
00612 
00616       fmatvec::VecInt LinkStatusParent;
00617 
00622       fmatvec::VecInt LinkStatusRegParent;
00623 
00627       fmatvec::SparseMat Gs;
00628 
00632       fmatvec::SqrMat Jprox;
00633 
00637       fmatvec::SqrMat G;
00638 
00642       fmatvec::Vec b;
00643 
00647       bool term;
00648 
00652       int maxIter, highIter, maxDampingSteps;
00653 
00657       double lmParm;
00658 
00662       Solver contactSolver, impactSolver;
00663 
00667       Strategy strategy;
00668 
00672       LinAlg linAlg;
00673 
00677       bool stopIfNoConvergence;
00678 
00682       bool dropContactInfo;
00683 
00687       bool useOldla;
00688 
00692       bool numJac;
00693 
00697       fmatvec::VecInt decreaseLevels;
00698 
00702       fmatvec::VecInt checkTermLevels;
00703 
00707       bool checkGSize;
00708 
00712       int limitGSize;
00713 
00717       int warnLevel;
00718 
00722       bool peds;
00723 
00727       unsigned int driftCount;
00728 
00733       unsigned int flushEvery;
00734 
00739       unsigned int flushCount;
00740 
00744       double tolProj;
00745 
00750       void updatezdRef(const fmatvec::Vec &ext);
00751 
00756       virtual void updaterFactors();
00757 
00762       void computeConstraintForces(double t);
00763 
00767       fmatvec::Vec laInverseKineticsParent;
00768       fmatvec::Mat bInverseKineticsParent;
00769 
00773       fmatvec::Mat WInverseKineticsParent[2];
00774 
00775       bool alwaysConsiderContact;
00776       bool inverseKinetics;
00777 
00778       fmatvec::Vec corrParent;
00779 
00780       int rootID;
00781 
00782       double gTol, gdTol, gddTol, laTol, LaTol;
00783 
00784     private:
00788       void constructor();
00789 
00793       bool integratorExitRequest;
00794 
00798       static bool exitRequest;
00799 
00803       bool READZ0;
00804 
00813       void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
00814 
00815       bool truncateSimulationFiles;
00816 
00817       // Holds the dynamic systems before the "reorganize hierarchie" takes place.
00818       // This is required since all elements of all other containers from DynamicSystem are readded to DynamicSystemSolver,
00819       // except this container (which is a "pure" container = no calculation is done in DynamicSystem)
00820       // However, we must hold this container until the dtor of DynamicSystemSolver is called to avoid the deallocation of other
00821       // elements hold by DynamicSystem elements (especially (currently only) hdf5File)
00822       std::vector<DynamicSystem*> dynamicsystemPreReorganize;
00823   };
00824 
00825 }
00826 
00827 #endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */
00828