MBSim

Project Page
dynamic_system_solver.h
00001 /* Copyright (C) 2004-2009 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 Frame;
00030   class Node;
00031   class Graph;
00032   class Integrator;
00033 
00051   class DynamicSystemSolver : public Group {
00052     public:
00053 
00057       enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
00058 
00062       enum Strategy { global, local };
00063 
00067       enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
00068 
00072       DynamicSystemSolver();
00073 
00078       DynamicSystemSolver(const std::string &projectName);
00079 
00083       virtual ~DynamicSystemSolver();
00084 
00123       void initialize();
00124 
00125       /*
00126        * If true (the default) then
00127        * the simulation output files (h5 files) are deleted/truncated/regenerated.
00128        * If false, then these files left are untouched from a previous run.
00129        * This is useful to regenerate the
00130        * e.g. OpenMBV XML files without doing a new integration.
00131        */
00132       void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00133 
00134       /* INHERITED INTERFACE OF GROUP */
00135       void init(InitStage stage);
00136       using Group::plot;
00137       /***************************************************/
00138 
00139       /* INHERITED INTERFACE OF DYNAMICSYSTEM */
00140       virtual int solveConstraintsFixpointSingle();
00141       virtual int solveImpactsFixpointSingle(double dt = 0);
00142       virtual int solveConstraintsGaussSeidel();
00143       virtual int solveImpactsGaussSeidel(double dt = 0);
00144       virtual int solveConstraintsRootFinding();
00145       virtual int solveImpactsRootFinding(double dt = 0);
00146       virtual void checkConstraintsForTermination();
00147       virtual void checkImpactsForTermination(double dt = 0);
00148       /***************************************************/
00149 
00150       /* INHERITED INTERFACE OF OBJECTINTERFACE */
00151       virtual void updateh(double t, int i=0);
00152       virtual void updateh0Fromh1(double t);
00153       virtual void updateW0FromW1(double t);
00154       virtual void updateV0FromV1(double t);
00155       virtual void updateM(double t, int i=0);
00156       virtual void updateStateDependentVariables(double t); // this function is called once every time step by every integrator
00157       /***************************************************/
00158 
00159       /* INHERITED INTERFACE OF LINKINTERFACE */
00160 
00171       virtual void updater(double t, int j=0);
00172       virtual void updatewb(double t, int j=0);
00173       virtual void updateW(double t, int j=0);
00174       virtual void updateV(double t, int j=0);
00175       /***************************************************/
00176 
00177       /* INHERITED INTERFACE OF ELEMENT */
00179       virtual std::string getType() const { return "DynamicSystemSolver"; }
00180       virtual void plot(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00181       virtual void plot2(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00182 
00183       virtual void closePlot();
00184       /***************************************************/
00185 
00186       /* INHERITED INTERFACE FOR DERIVED CLASS */
00191       virtual int solveConstraints();
00192 
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& getImpactSolver() { return impactSolver; }
00204       void setTermination(bool term_) { term = term_; }
00205       void setStrategy(Strategy strategy_) { strategy = strategy_; }
00206       void setMaxIter(int iter) { maxIter = iter; }
00207       void setHighIter(int iter) { highIter = iter; }
00208       void setNumJacProj(bool numJac_) { numJac = numJac_; }
00209       void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
00210       void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
00211       void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
00212 
00213       void setUseOldla(bool flag) { useOldla = flag; }
00214       void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
00215       void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
00216       void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
00217       void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
00218 
00219       const fmatvec::SparseMat& getGs() const { return Gs; }
00220       fmatvec::SparseMat& getGs() { return Gs; }
00221       const fmatvec::SqrMat& getG() const { return G; }
00222       fmatvec::SqrMat& getG() { return G; }
00223       const fmatvec::Vec& getb() const { return b; }
00224       fmatvec::Vec& getb() { return b; }
00225       const fmatvec::SqrMat& getJprox() const { return Jprox; }
00226       fmatvec::SqrMat& getJprox() { return Jprox; }
00227 
00228       const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
00229       const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
00230       const fmatvec::Vec& getlaParent() const { return laParent; }
00231       const fmatvec::Vec& getgdParent() const { return gdParent; }
00232       const fmatvec::Vec& getresParent() const { return resParent; }
00233       const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
00234 
00235       DynamicSystemSolver* getDynamicSystemSolver() { return this; }
00236       bool getIntegratorExitRequest() { return integratorExitRequest; }
00237       int getMaxIter()  {return maxIter;}
00238       /***************************************************/
00239 
00243       void computeInitialCondition();
00244 
00251       fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
00252 
00259       fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
00260 
00267       fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
00268 
00272       virtual void initz(fmatvec::Vec& z0);
00273 
00277       int (DynamicSystemSolver::*solveConstraints_)();
00278 
00283       int (DynamicSystemSolver::*solveImpacts_)(double dt);
00284 
00289       int solveConstraintsLinearEquations();
00290 
00295       int solveImpactsLinearEquations(double dt = 0);
00296 
00301       virtual void updateG(double t, int i=0);
00302 
00306       void decreaserFactors();
00307 
00315       void update(const fmatvec::Vec &z, double t, int options=0);
00316 
00323       virtual void shift(fmatvec::Vec& z, const fmatvec::VecInt& jsv, double t);
00324 
00331       virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
00332 
00338       virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
00339 
00346       virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
00347 
00352       void getLinkStatus(fmatvec::VecInt &LinkStatusExt, double t);
00353 
00359       void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt, double t);
00364       void projectGeneralizedPositions(double t, int mode);
00365 
00370       void projectGeneralizedVelocities(double t, int mode);
00371 
00376       void savela(double dt=1.0);
00377 
00382       void initla(double dt=1.0);
00383 
00387       double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
00388 
00393       double computePotentialEnergy();
00394 
00399       void addElement(Element *element_);
00400 
00406       Element* getElement(const std::string &name);
00407 
00411       std::string getSolverInfo();
00412 
00417       void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
00418 
00422       void dropContactMatrices();
00423 
00427       static void sigInterruptHandler(int);
00428 
00432       static void sigAbortHandler(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       fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
00506       fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
00507       fmatvec::Mat dhdx(double t);
00508       fmatvec::Vec dhdt(double t);
00509 
00510       void setRootID(int ID) {rootID = ID;}
00511       int getRootID() const {return rootID;}
00512     protected:
00516       fmatvec::SymMat MParent[2];
00517 
00521       fmatvec::SymMat LLMParent[2];
00522 
00526       fmatvec::Mat TParent;
00527 
00531       fmatvec::Mat WParent[2];
00532 
00536       fmatvec::Mat VParent[2];
00537 
00541       fmatvec::Vec wbParent;
00542 
00546       fmatvec::Vec laParent;
00547 
00551       fmatvec::Vec rFactorParent;
00552 
00556       fmatvec::Vec sParent;
00557 
00561       fmatvec::Vec resParent;
00562 
00566       fmatvec::Vec gParent;
00567 
00571       fmatvec::Vec gdParent;
00572 
00576       fmatvec::Vec zdParent;
00577 
00578       fmatvec::Vec udParent1;
00579 
00583       fmatvec::Vec hParent[2];
00584 
00588       fmatvec::Vec rParent[2];
00589 
00593       fmatvec::Vec fParent;
00594 
00598       fmatvec::Vec svParent;
00599 
00603       fmatvec::VecInt jsvParent;
00604 
00608       fmatvec::VecInt LinkStatusParent;
00609 
00614       fmatvec::VecInt LinkStatusRegParent;
00615 
00619       fmatvec::SparseMat Gs;
00620 
00624       fmatvec::SqrMat Jprox;
00625 
00629       fmatvec::SqrMat G;
00630 
00634       fmatvec::Vec b;
00635 
00639       bool term;
00640 
00644       int maxIter, highIter, maxDampingSteps;
00645 
00649       double lmParm;
00650 
00654       Solver contactSolver, impactSolver;
00655 
00659       Strategy strategy;
00660 
00664       LinAlg linAlg;
00665 
00669       bool stopIfNoConvergence;
00670 
00674       bool dropContactInfo;
00675 
00679       bool useOldla;
00680 
00684       bool numJac;
00685 
00689       fmatvec::VecInt decreaseLevels;
00690 
00694       fmatvec::VecInt checkTermLevels;
00695 
00699       bool checkGSize;
00700 
00704       int limitGSize;
00705 
00709       int warnLevel;
00710 
00714       bool peds;
00715 
00719       unsigned int driftCount;
00720 
00725       unsigned int flushEvery;
00726 
00731       unsigned int flushCount;
00732 
00736       double tolProj;
00737 
00742       void updatezdRef(const fmatvec::Vec &ext);
00743 
00748       void updaterFactors();
00749 
00754       void computeConstraintForces(double t);
00755 
00759       fmatvec::Vec laInverseKineticsParent;
00760       fmatvec::Mat bInverseKineticsParent;
00761 
00765       fmatvec::Mat WInverseKineticsParent[2];
00766 
00767       bool alwaysConsiderContact;
00768       bool inverseKinetics;
00769 
00770       fmatvec::Vec corrParent;
00771 
00772       int rootID;
00773 
00774       double gTol, gdTol, gddTol, laTol, LaTol;
00775 
00776     private:
00780       void constructor();
00781 
00785       bool integratorExitRequest;
00786 
00790       static bool exitRequest;
00791 
00795       bool READZ0;
00796 
00805       void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
00806 
00807       bool truncateSimulationFiles;
00808 
00809   };
00810 
00811 }
00812 
00813 #endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */
00814