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