MBSim

Project Page
dynamic_system_solver.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  * rzander@users.berlios.de
19  */
20 
21 #ifndef _DYNAMIC_SYSTEM_SOLVER_H_
22 #define _DYNAMIC_SYSTEM_SOLVER_H_
23 
24 #include "mbsim/group.h"
25 #include "sparse_matrix.h"
26 
27 namespace MBSim {
28 
29  class Frame;
30  class Node;
31  class Graph;
32  class ExtraDynamicInterface;
33  class Integrator;
34 
38  enum Solver { FixedPointTotal, FixedPointSingle, GaussSeidel, LinearEquations, RootFinding };
39 
43  enum Strategy { global, local };
44 
48  enum LinAlg { LUDecomposition, LevenbergMarquardt, PseudoInverse };
49 
67  class DynamicSystemSolver : public Group {
68  public:
73 
78  DynamicSystemSolver(const std::string &projectName);
79 
83  virtual ~DynamicSystemSolver();
84 
123  void initialize();
124 
125  /*
126  * If true (the default) then
127  * the simulation output files (h5 files) are deleted/truncated/regenerated.
128  * If false, then these files left are untouched from a previous run.
129  * This is useful to regenerate the
130  * e.g. OpenMBV XML files without doing a new integration.
131  */
132  void setTruncateSimulationFiles(bool trunc) { truncateSimulationFiles=trunc; }
133 
134  /* INHERITED INTERFACE OF GROUP */
135  void init(InitStage stage);
136  using Group::plot;
137  /***************************************************/
138 
139  /* INHERITED INTERFACE OF DYNAMICSYSTEM */
140  virtual int solveConstraintsFixpointSingle();
141  virtual int solveImpactsFixpointSingle(double dt = 0);
142  virtual int solveConstraintsGaussSeidel();
143  virtual int solveImpactsGaussSeidel(double dt = 0);
144  virtual int solveConstraintsRootFinding();
145  virtual int solveImpactsRootFinding(double dt = 0);
146  virtual void checkConstraintsForTermination();
147  virtual void checkImpactsForTermination(double dt = 0);
148  /***************************************************/
149 
150  /* INHERITED INTERFACE OF OBJECTINTERFACE */
151  virtual void updateh(double t, int i=0);
152  virtual void updateh0Fromh1(double t);
153  virtual void updateW0FromW1(double t);
154  virtual void updateV0FromV1(double t);
155  virtual void updateM(double t, int i=0);
156  virtual void updateStateDependentVariables(double t); // this function is called once every time step by every integrator
157  /***************************************************/
158 
159  /* INHERITED INTERFACE OF LINKINTERFACE */
160 
171  virtual void updater(double t, int j=0);
172  virtual void updatewb(double t, int j=0);
173  virtual void updateW(double t, int j=0);
174  virtual void updateV(double t, int j=0);
175  /***************************************************/
176 
177  /* INHERITED INTERFACE OF ELEMENT */
179  virtual std::string getType() const { return "DynamicSystemSolver"; }
180  virtual void plot(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
181  virtual void plot2(const fmatvec::Vec& z, double t, double dt=1); // TODO completely rearrange
182 
183  virtual void closePlot();
184  /***************************************************/
185 
186  /* INHERITED INTERFACE FOR DERIVED CLASS */
191  virtual int solveConstraints();
192 
196  virtual int solveImpacts(double dt = 0);
197  /***************************************************/
198 
199  /* GETTER / SETTER */
200 
201  void setConstraintSolver(Solver solver_) { contactSolver = solver_; }
202  void setImpactSolver(Solver solver_) { impactSolver = solver_; }
203  const Solver& getImpactSolver() { return impactSolver; }
204  void setTermination(bool term_) { term = term_; }
205  void setStrategy(Strategy strategy_) { strategy = strategy_; }
206  void setMaxIter(int iter) { maxIter = iter; }
207  void setHighIter(int iter) { highIter = iter; }
208  void setNumJacProj(bool numJac_) { numJac = numJac_; }
209  void setMaxDampingSteps(int maxDSteps) { maxDampingSteps = maxDSteps; }
210  void setLevenbergMarquardtParam(double lmParm_) { lmParm = lmParm_; }
211  void setLinAlg(LinAlg linAlg_) { linAlg = linAlg_; }
212 
213  void setUseOldla(bool flag) { useOldla = flag; }
214  void setDecreaseLevels(const fmatvec::Vector<int> &decreaseLevels_) { decreaseLevels = decreaseLevels_; }
215  void setCheckTermLevels(const fmatvec::Vector<int> &checkTermLevels_) { checkTermLevels = checkTermLevels_; }
216  void setCheckGSize(bool checkGSize_) { checkGSize = checkGSize_; }
217  void setLimitGSize(int limitGSize_) { limitGSize = limitGSize_; checkGSize = false; }
218 
219  const fmatvec::Matrix<fmatvec::Sparse, double>& getGs() const { return Gs; }
220  fmatvec::Matrix<fmatvec::Sparse, double>& getGs() { return Gs; }
221  const fmatvec::SqrMat& getG() const { return G; }
222  fmatvec::SqrMat& getG() { return G; }
223  const fmatvec::Vec& getb() const { return b; }
224  fmatvec::Vec& getb() { return b; }
225  const fmatvec::SqrMat& getJprox() const { return Jprox; }
226  fmatvec::SqrMat& getJprox() { return Jprox; }
227 
228  const fmatvec::Mat& getWParent(int i=0) const { return WParent[i]; }
229  const fmatvec::Mat& getVParent(int i=0) const { return VParent[i]; }
230  const fmatvec::Vec& getlaParent() const { return laParent; }
231  const fmatvec::Vec& getgdParent() const { return gdParent; }
232  const fmatvec::Vec& getresParent() const { return resParent; }
233  const fmatvec::Vec& getrFactorParent() const { return rFactorParent; }
234 
236  bool getIntegratorExitRequest() { return integratorExitRequest; }
237  int getMaxIter() {return maxIter;}
238  /***************************************************/
239 
244 
251  fmatvec::Vec deltau(const fmatvec::Vec &zParent, double t, double dt);
252 
259  fmatvec::Vec deltaq(const fmatvec::Vec &zParent, double t, double dt);
260 
267  fmatvec::Vec deltax(const fmatvec::Vec &zParent, double t, double dt);
268 
272  void initz(fmatvec::Vec& z0);
273 
278 
283  int (DynamicSystemSolver::*solveImpacts_)(double dt);
284 
290 
295  int solveImpactsLinearEquations(double dt = 0);
296 
301  void updateG(double t, int i=0);
302 
306  void decreaserFactors();
307 
315  void update(const fmatvec::Vec &z, double t, int options=0);
316 
323  virtual void shift(fmatvec::Vec& z, const fmatvec::Vector<int>& jsv, double t);
324 
331  virtual void zdot(const fmatvec::Vec& z, fmatvec::Vec& zd, double t);
332 
338  virtual fmatvec::Vec zdot(const fmatvec::Vec &zParent, double t);
339 
346  virtual void getsv(const fmatvec::Vec& z, fmatvec::Vec& svExt, double t);
347 
352  void getLinkStatus(fmatvec::Vector<int> &LinkStatusExt, double t);
353 
359  void getLinkStatusReg(fmatvec::Vector<int> &LinkStatusRegExt, double t);
364  void projectGeneralizedPositions(double t, int mode);
365 
370  void projectGeneralizedVelocities(double t, int mode);
371 
376  void savela(double dt=1.0);
377 
382  void initla(double dt=1.0);
383 
387  double computeKineticEnergy() { return 0.5*u.T()*M[0]*u; }
388 
393  double computePotentialEnergy();
394 
399  void addElement(Element *element_);
400 
406  Element* getElement(const std::string &name);
407 
411  std::string getSolverInfo();
412 
417  void setStopIfNoConvergence(bool flag, bool dropInfo = false) { stopIfNoConvergence = flag; dropContactInfo=dropInfo; }
418 
422  void dropContactMatrices();
423 
427  static void sigInterruptHandler(int);
428 
432  static void sigAbortHandler(int);
433 
434  // TODO just for testing
435  void setPartialEventDrivenSolver(bool peds_) { peds = peds_; }
436 
442  void writez(std::string fileName, bool formatH5=true);
443 
448  void readz0(std::string fileName);
449 
450  virtual void initializeUsingXML(TiXmlElement *element);
451 
456  void setReorganizeHierarchy(bool flag) { reorganizeHierarchy = flag; }
457 
462  void setProjectionTolerance(double tol) { tolProj = tol; }
463 
467  void setInformationOutput(bool INFO_) { INFO = INFO_; }
468 
473  void updatezRef(const fmatvec::Vec &ext);
474 
479  void setFlushEvery(unsigned int every) {flushEvery = every;}
480 
481  void setAlwaysConsiderContact(bool alwaysConsiderContact_) {alwaysConsiderContact = alwaysConsiderContact_;}
482 
483  void setInverseKinetics(bool inverseKinetics_) {inverseKinetics = inverseKinetics_;}
484 
485  fmatvec::Mat dhdq(double t, int lb=0, int ub=0);
486  fmatvec::Mat dhdu(double t, int lb=0, int ub=0);
487  fmatvec::Mat dhdx(double t);
488  fmatvec::Vec dhdt(double t);
489 
490  void setRootID(int ID) {rootID = ID;}
491  int getRootID() const {return rootID;}
492  protected:
496  fmatvec::SymMat MParent[2];
497 
501  fmatvec::SymMat LLMParent[2];
502 
506  fmatvec::Mat TParent;
507 
511  fmatvec::Mat WParent[2];
512 
516  fmatvec::Mat VParent[2];
517 
521  fmatvec::Vec wbParent;
522 
526  fmatvec::Vec laParent;
527 
531  fmatvec::Vec rFactorParent;
532 
536  fmatvec::Vec sParent;
537 
541  fmatvec::Vec resParent;
542 
546  fmatvec::Vec gParent;
547 
551  fmatvec::Vec gdParent;
552 
556  fmatvec::Vec zdParent;
557 
558  fmatvec::Vec udParent1;
559 
563  fmatvec::Vec hParent[2];
564 
568  fmatvec::Vec rParent[2];
569 
573  fmatvec::Vec fParent;
574 
578  fmatvec::Vec svParent;
579 
583  fmatvec::Vector<int> jsvParent;
584 
588  fmatvec::Vector<int> LinkStatusParent;
589 
594  fmatvec::Vector<int> LinkStatusRegParent;
598  fmatvec::Matrix<fmatvec::Sparse, double> Gs;
599 
603  fmatvec::SqrMat Jprox;
604 
608  fmatvec::SqrMat G;
609 
613  fmatvec::Vec b;
614 
618  bool term;
619 
623  int maxIter, highIter, maxDampingSteps;
624 
628  double lmParm;
629 
633  Solver contactSolver, impactSolver;
634 
639 
644 
649 
654 
658  bool useOldla;
659 
663  bool numJac;
664 
668  fmatvec::Vector<int> decreaseLevels;
669 
673  fmatvec::Vector<int> checkTermLevels;
674 
679 
684 
689 
693  bool peds;
694 
698  unsigned int driftCount;
699 
704  unsigned int flushEvery;
705 
710  unsigned int flushCount;
711 
717 
721  double tolProj;
722 
727  void updatezdRef(const fmatvec::Vec &ext);
728 
733  void updaterFactors();
734 
739  void computeConstraintForces(double t);
740 
744  fmatvec::Vec laInverseKineticsParent;
745  fmatvec::Mat bInverseKineticsParent;
746 
750  fmatvec::Mat WInverseKineticsParent[2];
751 
752  bool alwaysConsiderContact;
753  bool inverseKinetics;
754 
755  fmatvec::Vec corrParent;
756 
757  int rootID;
758 
759  private:
763  void constructor();
764 
769 
773  static bool exitRequest;
774 
778  bool INFO;
779 
783  bool READZ0;
784 
793  void addToGraph(Graph* graph, fmatvec::SqrMat &A, int i, std::vector<Object*> &objList);
794 
795  bool truncateSimulationFiles;
796 
797  };
798 
799 }
800 
801 #endif /* _DYNAMIC_SYSTEM_SOLVER_H_ */
802