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
00070 DynamicSystemSolver();
00071
00076 DynamicSystemSolver(const std::string &projectName);
00077
00081 virtual ~DynamicSystemSolver();
00082
00121 void initialize();
00122
00123
00124
00125
00126
00127
00128
00129
00130 void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
00131
00132
00133 void init(InitStage stage);
00134 using Group::plot;
00135
00136
00137
00138 virtual int solveConstraintsFixpointSingle();
00139 virtual int solveImpactsFixpointSingle(double dt = 0);
00140 virtual int solveConstraintsGaussSeidel();
00141 virtual int solveImpactsGaussSeidel(double dt = 0);
00142 virtual int solveConstraintsRootFinding();
00143 virtual int solveImpactsRootFinding(double dt = 0);
00144 virtual void checkConstraintsForTermination();
00145 virtual void checkImpactsForTermination(double dt = 0);
00146
00147
00148
00149 virtual void updateh(double t, int i=0);
00150 virtual void updateh0Fromh1(double t);
00151 virtual void updateW0FromW1(double t);
00152 virtual void updateV0FromV1(double t);
00153 virtual void updateM(double t, int i=0);
00154 virtual void updateStateDependentVariables(double t);
00155
00156
00157
00158
00169 virtual void updater(double t, int j=0);
00170 virtual void updatewb(double t, int j=0);
00171 virtual void updateW(double t, int j=0);
00172 virtual void updateV(double t, int j=0);
00173
00174
00175
00177 virtual std::string getType() const { return "DynamicSystemSolver"; }
00178 virtual void plot(const fmatvec::Vec& z, double t, double dt=1);
00179 virtual void plot2(const fmatvec::Vec& z, double t, double dt=1);
00180
00181 virtual void closePlot();
00182
00183
00184
00189 virtual int solveConstraints();
00190
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& getConstraintSolver() { return contactSolver; }
00204 const Solver& getImpactSolver() { return impactSolver; }
00205 void setTermination(bool term_) { term = term_; }
00206 void setStrategy(Strategy strategy_) { strategy = strategy_; }
00207 void setMaxIter(int iter) { maxIter = iter; }
00208 void setHighIter(int iter) { highIter = iter; }
00209 void setNumJacProj(bool numJac_) { numJac = numJac_; }
00210 void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
00211 void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
00212 void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
00213
00214 void setUseOldla(bool flag) { useOldla = flag; }
00215 void setDecreaseLevels(const fmatvec::VecInt &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
00216 void setCheckTermLevels(const fmatvec::VecInt &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
00217 void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
00218 void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
00219
00220 const fmatvec::SparseMat& getGs() const { return Gs; }
00221 fmatvec::SparseMat& getGs() { return Gs; }
00222 const fmatvec::SqrMat& getG() const { return G; }
00223 fmatvec::SqrMat& getG() { return G; }
00224 const fmatvec::Vec& getb() const { return b; }
00225 fmatvec::Vec& getb() { return b; }
00226 const fmatvec::SqrMat& getJprox() const { return Jprox; }
00227 fmatvec::SqrMat& getJprox() { return Jprox; }
00228
00229 const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
00230 const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
00231 const fmatvec::Vec& getlaParent() const { return laParent; }
00232 const fmatvec::Vec& getgdParent() const { return gdParent; }
00233 const fmatvec::Vec& getresParent() const { return resParent; }
00234 const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
00235
00236 DynamicSystemSolver* getDynamicSystemSolver() { return this; }
00237 bool getIntegratorExitRequest() { return integratorExitRequest; }
00238 int getMaxIter() {return maxIter;}
00239
00240
00244 void computeInitialCondition();
00245
00252 fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
00253
00260 fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
00261
00268 fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
00269
00273 virtual void initz(fmatvec::Vec& z0);
00274
00279 int (DynamicSystemSolver::*solveConstraints_)();
00280
00286 int (DynamicSystemSolver::*solveImpacts_)(double dt);
00287
00293 int solveConstraintsLinearEquations();
00294
00301 int solveImpactsLinearEquations(double dt = 0);
00302
00307 virtual void updateG(double t, int i=0);
00308
00312 void decreaserFactors();
00313
00321 void update(const fmatvec::Vec &z, double t, int options=0);
00322
00329 virtual void shift(fmatvec::Vec& z, const fmatvec::VecInt& jsv, double t);
00330
00337 virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
00338
00344 virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
00345
00352 virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
00353
00358 void getLinkStatus(fmatvec::VecInt &LinkStatusExt, double t);
00359
00365 void getLinkStatusReg(fmatvec::VecInt &LinkStatusRegExt, double t);
00370 void projectGeneralizedPositions(double t, int mode);
00371
00376 void projectGeneralizedVelocities(double t, int mode);
00377
00382 void savela(double dt=1.0);
00383
00388 void initla(double dt=1.0);
00389
00393 double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
00394
00399 double computePotentialEnergy();
00400
00405 void addElement(Element *element_);
00406
00412 Element* getElement(const std::string &name);
00413
00417 std::string getSolverInfo();
00418
00423 void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
00424
00428 void dropContactMatrices();
00429
00433 static void sigInterruptHandler(int);
00434
00438 static void sigAbortHandler(int);
00439
00440 static void sigSegfaultHandler(int);
00441
00442
00443 void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
00444
00450 void writez(std::string fileName, bool formatH5=true);
00451
00456 void readz0(std::string fileName);
00457
00458 virtual void initializeUsingXML(xercesc::DOMElement *element);
00459 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00460
00461 static DynamicSystemSolver* readXMLFile(const std::string &filename);
00462 void writeXMLFile(const std::string &name);
00463 void writeXMLFile() { writeXMLFile(getName()); }
00464
00465
00470 void setProjectionTolerance(double tol) { tolProj = tol; }
00471
00475 void setgTol(double tol) {gTol = tol; Group::setgTol(tol);}
00476
00480 void setgdTol(double tol) {gdTol = tol; Group::setgdTol(tol);}
00481
00485 void setgddTol(double tol) {gddTol = tol; Group::setgddTol(tol);}
00486
00490 void setlaTol(double tol) {laTol = tol; Group::setlaTol(tol);}
00491
00495 void setLaTol(double tol) {LaTol = tol; Group::setLaTol(tol);}
00496
00501 void updatezRef(const fmatvec::Vec &ext);
00502
00507 void setFlushEvery(unsigned int every) {flushEvery = every;}
00508
00509 void setAlwaysConsiderContact(bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
00510
00511 void setInverseKinetics(bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
00512
00513 fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
00514 fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
00515 fmatvec::Mat dhdx(double t);
00516 fmatvec::Vec dhdt(double t);
00517
00518 void setRootID(int ID) {rootID = ID;}
00519 int getRootID() const {return rootID;}
00520 protected:
00524 fmatvec::SymMat MParent[2];
00525
00529 fmatvec::SymMat LLMParent[2];
00530
00534 fmatvec::Mat TParent;
00535
00539 fmatvec::Mat WParent[2];
00540
00544 fmatvec::Mat VParent[2];
00545
00549 fmatvec::Vec wbParent;
00550
00554 fmatvec::Vec laParent;
00555
00559 fmatvec::Vec rFactorParent;
00560
00564 fmatvec::Vec sParent;
00565
00569 fmatvec::Vec resParent;
00570
00574 fmatvec::Vec gParent;
00575
00579 fmatvec::Vec gdParent;
00580
00584 fmatvec::Vec zdParent;
00585
00586 fmatvec::Vec udParent1;
00587
00591 fmatvec::Vec hParent[2];
00592
00596 fmatvec::Vec rParent[2];
00597
00601 fmatvec::Vec fParent;
00602
00606 fmatvec::Vec svParent;
00607
00611 fmatvec::VecInt jsvParent;
00612
00616 fmatvec::VecInt LinkStatusParent;
00617
00622 fmatvec::VecInt LinkStatusRegParent;
00623
00627 fmatvec::SparseMat Gs;
00628
00632 fmatvec::SqrMat Jprox;
00633
00637 fmatvec::SqrMat G;
00638
00642 fmatvec::Vec b;
00643
00647 bool term;
00648
00652 int maxIter, highIter, maxDampingSteps;
00653
00657 double lmParm;
00658
00662 Solver contactSolver, impactSolver;
00663
00667 Strategy strategy;
00668
00672 LinAlg linAlg;
00673
00677 bool stopIfNoConvergence;
00678
00682 bool dropContactInfo;
00683
00687 bool useOldla;
00688
00692 bool numJac;
00693
00697 fmatvec::VecInt decreaseLevels;
00698
00702 fmatvec::VecInt checkTermLevels;
00703
00707 bool checkGSize;
00708
00712 int limitGSize;
00713
00717 int warnLevel;
00718
00722 bool peds;
00723
00727 unsigned int driftCount;
00728
00733 unsigned int flushEvery;
00734
00739 unsigned int flushCount;
00740
00744 double tolProj;
00745
00750 void updatezdRef(const fmatvec::Vec &ext);
00751
00756 virtual void updaterFactors();
00757
00762 void computeConstraintForces(double t);
00763
00767 fmatvec::Vec laInverseKineticsParent;
00768 fmatvec::Mat bInverseKineticsParent;
00769
00773 fmatvec::Mat WInverseKineticsParent[2];
00774
00775 bool alwaysConsiderContact;
00776 bool inverseKinetics;
00777
00778 fmatvec::Vec corrParent;
00779
00780 int rootID;
00781
00782 double gTol, gdTol, gddTol, laTol, LaTol;
00783
00784 private:
00788 void constructor();
00789
00793 bool integratorExitRequest;
00794
00798 static bool exitRequest;
00799
00803 bool READZ0;
00804
00813 void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
00814
00815 bool truncateSimulationFiles;
00816
00817
00818
00819
00820
00821
00822 std::vector<DynamicSystem*> dynamicsystemPreReorganize;
00823 };
00824
00825 }
00826
00827 #endif
00828