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 
00037   enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
00038 
00042   enum Strategy { global, local };
00043 
00047   enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
00048 
00066   class DynamicSystemSolver : public Group {
00067     public:
00071       DynamicSystemSolver();
00072 
00077       DynamicSystemSolver(const std::string &projectName);
00078 
00082       virtual ~DynamicSystemSolver();
00083 
00122       void initialize();
00123 
00124       /*
00125        * If true (the default) then
00126        * the simulation output files (h5 files) are deleted/truncated/regenerated.
00127        * If false, then these files left are untouched from a previous run.
00128        * This is useful to regenerate the
00129        * e.g. OpenMBV XML files without doing a new integration.
00130        */
00131       void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00132 
00133       /* INHERITED INTERFACE OF GROUP */
00134       void init(InitStage stage);
00135       using Group::plot;
00136       /***************************************************/
00137 
00138       /* INHERITED INTERFACE OF DYNAMICSYSTEM */
00139       virtual int solveConstraintsFixpointSingle();
00140       virtual int solveImpactsFixpointSingle(double dt = 0);
00141       virtual int solveConstraintsGaussSeidel();
00142       virtual int solveImpactsGaussSeidel(double dt = 0);
00143       virtual int solveConstraintsRootFinding();
00144       virtual int solveImpactsRootFinding(double dt = 0);
00145       virtual void checkConstraintsForTermination();
00146       virtual void checkImpactsForTermination(double dt = 0);
00147       /***************************************************/
00148 
00149       /* INHERITED INTERFACE OF OBJECTINTERFACE */
00150       virtual void updateh(double t, int i=0);
00151       virtual void updateh0Fromh1(double t);
00152       virtual void updateW0FromW1(double t);
00153       virtual void updateV0FromV1(double t);
00154       virtual void updateM(double t, int i=0);
00155       virtual void updateStateDependentVariables(double t); // this function is called once every time step by every integrator
00156       /***************************************************/
00157 
00158       /* INHERITED INTERFACE OF LINKINTERFACE */
00159 
00170       virtual void updater(double t, int j=0);
00171       virtual void updatewb(double t, int j=0);
00172       virtual void updateW(double t, int j=0);
00173       virtual void updateV(double t, int j=0);
00174       /***************************************************/
00175 
00176       /* INHERITED INTERFACE OF ELEMENT */
00178       virtual std::string getType() const { return "DynamicSystemSolver"; }
00179       virtual void plot(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00180       virtual void plot2(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
00181 
00182       virtual void closePlot();
00183       /***************************************************/
00184 
00185       /* INHERITED INTERFACE FOR DERIVED CLASS */
00190       virtual int solveConstraints();
00191 
00195       virtual int solveImpacts(double dt = 0);
00196       /***************************************************/
00197 
00198       /* GETTER / SETTER */
00199 
00200       void setConstraintSolver(Solver solver_) { contactSolver = solver_; }
00201       void setImpactSolver(Solver solver_) { impactSolver = solver_; }
00202       const Solver& getImpactSolver() { return impactSolver; }
00203       void setTermination(bool term_) { term = term_; }
00204       void setStrategy(Strategy strategy_) { strategy = strategy_; }
00205       void setMaxIter(int iter) { maxIter = iter; }
00206       void setHighIter(int iter) { highIter = iter; }
00207       void setNumJacProj(bool numJac_) { numJac = numJac_; }
00208       void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
00209       void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
00210       void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
00211 
00212       void setUseOldla(bool flag) { useOldla = flag; }
00213       void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
00214       void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
00215       void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
00216       void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
00217 
00218       const fmatvec::SparseMat& getGs() const { return Gs; }
00219       fmatvec::SparseMat& getGs() { return Gs; }
00220       const fmatvec::SqrMat& getG() const { return G; }
00221       fmatvec::SqrMat& getG() { return G; }
00222       const fmatvec::Vec& getb() const { return b; }
00223       fmatvec::Vec& getb() { return b; }
00224       const fmatvec::SqrMat& getJprox() const { return Jprox; }
00225       fmatvec::SqrMat& getJprox() { return Jprox; }
00226 
00227       const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
00228       const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
00229       const fmatvec::Vec& getlaParent() const { return laParent; }
00230       const fmatvec::Vec& getgdParent() const { return gdParent; }
00231       const fmatvec::Vec& getresParent() const { return resParent; }
00232       const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
00233 
00234       DynamicSystemSolver* getDynamicSystemSolver() { return this; }
00235       bool getIntegratorExitRequest() { return integratorExitRequest; }
00236       int getMaxIter()  {return maxIter;}
00237       /***************************************************/
00238 
00242       void computeInitialCondition();
00243 
00250       fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
00251 
00258       fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
00259 
00266       fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
00267 
00271       virtual void initz(fmatvec::Vec& z0);
00272 
00276       int (DynamicSystemSolver::*solveConstraints_)();
00277 
00282       int (DynamicSystemSolver::*solveImpacts_)(double dt);
00283 
00288       int solveConstraintsLinearEquations();
00289 
00294       int solveImpactsLinearEquations(double dt = 0);
00295 
00300       void updateG(double t, int i=0);
00301 
00305       void decreaserFactors();
00306 
00314       void update(const fmatvec::Vec &z, double t, int options=0);
00315 
00322       virtual void shift(fmatvec::Vec& z, const fmatvec::VecInt& jsv, double t);
00323 
00330       virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
00331 
00337       virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
00338 
00345       virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
00346 
00351       void getLinkStatus(fmatvec::VecInt &LinkStatusExt, double t);
00352 
00358       void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt, double t);
00363       void projectGeneralizedPositions(double t, int mode);
00364 
00369       void projectGeneralizedVelocities(double t, int mode);
00370 
00375       void savela(double dt=1.0);
00376 
00381       void initla(double dt=1.0);
00382 
00386       double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
00387 
00392       double computePotentialEnergy();
00393 
00398       void addElement(Element *element_);
00399 
00405       Element* getElement(const std::string &name);
00406 
00410       std::string getSolverInfo();
00411 
00416       void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
00417 
00421       void dropContactMatrices();
00422 
00426       static void sigInterruptHandler(int);
00427 
00431       static void sigAbortHandler(int);
00432 
00433       // TODO just for testing
00434       void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
00435 
00441       void writez(std::string fileName, bool formatH5=true);
00442 
00447       void readz0(std::string fileName);
00448 
00449       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00450       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00451 
00452       static DynamicSystemSolver* readXMLFile(const std::string &filename);
00453       void writeXMLFile(const std::string &name);
00454       void writeXMLFile() { writeXMLFile(getName()); }
00455 
00456 
00461       void setReorganizeHierarchy(bool flag) { reorganizeHierarchy = flag; }
00462 
00467       void setProjectionTolerance(double tol) { tolProj = tol; }
00468 
00472       void setgTol(double tol) {gTol = tol; Group::setgTol(tol);}
00473 
00477       void setgdTol(double tol) {gdTol = tol; Group::setgdTol(tol);}
00478 
00482       void setgddTol(double tol) {gddTol = tol; Group::setgddTol(tol);}
00483 
00487       void setlaTol(double tol) {laTol = tol; Group::setlaTol(tol);}
00488 
00492       void setLaTol(double tol) {LaTol = tol; Group::setLaTol(tol);}
00493 
00494 
00498       void setInformationOutput(bool INFO_) { INFO = INFO_; }
00499 
00504       void updatezRef(const fmatvec::Vec &ext);
00505 
00510       void setFlushEvery(unsigned int every) {flushEvery = every;}
00511 
00512       void setAlwaysConsiderContact(bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
00513 
00514       void setInverseKinetics(bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
00515 
00516       fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
00517       fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
00518       fmatvec::Mat dhdx(double t);
00519       fmatvec::Vec dhdt(double t);
00520 
00521       void setRootID(int ID) {rootID = ID;}
00522       int getRootID() const {return rootID;}
00523     protected:
00527       fmatvec::SymMat MParent[2];
00528 
00532       fmatvec::SymMat LLMParent[2];
00533 
00537       fmatvec::Mat TParent;
00538 
00542       fmatvec::Mat WParent[2];
00543 
00547       fmatvec::Mat VParent[2];
00548 
00552       fmatvec::Vec wbParent;
00553 
00557       fmatvec::Vec laParent;
00558 
00562       fmatvec::Vec rFactorParent;
00563 
00567       fmatvec::Vec sParent;
00568 
00572       fmatvec::Vec resParent;
00573 
00577       fmatvec::Vec gParent;
00578 
00582       fmatvec::Vec gdParent;
00583 
00587       fmatvec::Vec zdParent;
00588 
00589       fmatvec::Vec udParent1;
00590 
00594       fmatvec::Vec hParent[2];
00595 
00599       fmatvec::Vec rParent[2];
00600 
00604       fmatvec::Vec fParent;
00605 
00609       fmatvec::Vec svParent;
00610 
00614       fmatvec::VecInt jsvParent;
00615 
00619       fmatvec::VecInt LinkStatusParent;
00620 
00625       fmatvec::VecInt LinkStatusRegParent;
00626 
00630       fmatvec::SparseMat Gs;
00631 
00635       fmatvec::SqrMat Jprox;
00636 
00640       fmatvec::SqrMat G;
00641 
00645       fmatvec::Vec b;
00646 
00650       bool term;
00651 
00655       int maxIter, highIter, maxDampingSteps;
00656 
00660       double lmParm;
00661 
00665       Solver contactSolver, impactSolver;
00666 
00670       Strategy strategy;
00671 
00675       LinAlg linAlg;
00676 
00680       bool stopIfNoConvergence;
00681 
00685       bool dropContactInfo;
00686 
00690       bool useOldla;
00691 
00695       bool numJac;
00696 
00700       fmatvec::VecInt decreaseLevels;
00701 
00705       fmatvec::VecInt checkTermLevels;
00706 
00710       bool checkGSize;
00711 
00715       int limitGSize;
00716 
00720       int warnLevel;
00721 
00725       bool peds;
00726 
00730       unsigned int driftCount;
00731 
00736       unsigned int flushEvery;
00737 
00742       unsigned int flushCount;
00743 
00748       bool reorganizeHierarchy;
00749 
00753       double tolProj;
00754 
00759       void updatezdRef(const fmatvec::Vec &ext);
00760 
00765       void updaterFactors();
00766 
00771       void computeConstraintForces(double t);
00772 
00776       fmatvec::Vec laInverseKineticsParent;
00777       fmatvec::Mat bInverseKineticsParent;
00778 
00782       fmatvec::Mat WInverseKineticsParent[2];
00783 
00784       bool alwaysConsiderContact;
00785       bool inverseKinetics;
00786 
00787       fmatvec::Vec corrParent;
00788 
00789       int rootID;
00790 
00791       double gTol, gdTol, gddTol, laTol, LaTol;
00792 
00793     private:
00797       void constructor();
00798 
00802       bool integratorExitRequest;
00803 
00807       static bool exitRequest;
00808 
00812       bool INFO;
00813 
00817       bool READZ0;
00818 
00827       void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
00828 
00829       bool truncateSimulationFiles;
00830 
00831   };
00832 
00833 }
00834 
00835 #endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */
00836