00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00120
00121
00122
00123
00124
00125 void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00126
00127
00128 void init(InitStage stage);
00129 using Group::plot;
00130
00131
00132
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
00144 virtual void updateh(double t, int i=0);
00145 virtual void updateM(double t, int i=0);
00146 virtual void updateStateDependentVariables(double t);
00147
00148
00149
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
00169 virtual std::string getType() const { return "DynamicSystemSolver"; }
00170 virtual void plot(const fmatvec::Vec& z, double t, double dt=1);
00171 virtual void plot2(const fmatvec::Vec& z, double t, double dt=1);
00172
00173 virtual void closePlot();
00174
00175
00176
00181 virtual int solveConstraints();
00182
00188 virtual int solveImpacts(double dt = 0);
00189
00190
00191
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
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
00816
00817
00818
00819
00820 std::vector<DynamicSystem*> dynamicsystemPreReorganize;
00821 };
00822
00823 }
00824
00825 #endif
00826