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 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
00127
00128
00129
00130
00131
00132 void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00133
00134
00135 void init(InitStage stage);
00136 using Group::plot;
00137
00138
00139
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
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);
00157
00158
00159
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
00179 virtual std::string getType() const { return "DynamicSystemSolver"; }
00180 virtual void plot(const fmatvec::Vec& z, double t, double dt=1);
00181 virtual void plot2(const fmatvec::Vec& z, double t, double dt=1);
00182
00183 virtual void closePlot();
00184
00185
00186
00191 virtual int solveConstraints();
00192
00196 virtual int solveImpacts(double dt = 0);
00197
00198
00199
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
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
00814