MBSim

Project Page
dynamic_system.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  * This library is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU Lesser General Public
4  * License as published by the Free Software Foundation; either
5  * version 2.1 of the License, or (at your option) any later version.
6  *
7  * This library is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
10  * Lesser General Public License for more details.
11  *
12  * You should have received a copy of the GNU Lesser General Public
13  * License along with this library; if not, write to the Free Software
14  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
15  *
16  * Contact: martin.o.foerg@googlemail.com
17  */
18 
19 #ifndef _DYNAMIC_SYSTEM_H_
20 #define _DYNAMIC_SYSTEM_H_
21 
22 #include "mbsim/element.h"
23 //#include "mbsim/link_interface.h"
24 //#include "mbsim/object_interface.h"
25 //#include "mbsim/extradynamic_interface.h"
26 #include "mbsim/mbsim_event.h"
27 
28 namespace H5 {
29  class Group;
30 }
31 
32 namespace MBSim {
33  class Frame;
34  class Contour;
35  class ExtraDynamic;
36  class Object;
37  class Link;
38  class ModellingInterface;
39  class Contact;
40  class InverseKineticsJoint;
41 
58  //class DynamicSystem : public Element, public ObjectInterface, public LinkInterface, public ExtraDynamicInterface {
59  class DynamicSystem : public Element {
60  public:
64  DynamicSystem(const std::string &name);
65 
69  virtual ~DynamicSystem();
70 
71  /* INHERITED INTERFACE OF OBJECTINTERFACE */
72  virtual void updateT(double t);
73  virtual void updateh(double t, int i=0);
74  virtual void updateh0Fromh1(double t);
75  virtual void updateW0FromW1(double t);
76  virtual void updateV0FromV1(double t);
77  virtual void updateStateDependentVariables(double t) = 0;
78  virtual void updateStateDerivativeDependentVariables(double t);
79  virtual void updateM(double t, int i=0);
80  virtual void updateJacobians(double t, int j=0) = 0;
81  virtual void updatedq(double t, double dt);
82  virtual void updateud(double t, int i=0) { throw MBSimError("ERROR in "+getName()+" (DynamicSystem::updateud): Not implemented!"); }
83  virtual void updatezd(double t) = 0;
84  virtual void updatedu(double t, double dt) = 0;
85  virtual void updateqd(double t);
86  virtual void sethSize(int hSize_, int i=0);
87  virtual int gethSize(int i=0) const { return hSize[i]; }
88  virtual int getqSize() const { return qSize; }
89  virtual int getuSize(int i=0) const { return uSize[i]; }
90  virtual void calcqSize();
91  virtual void calcuSize(int j=0);
92  //virtual int getqInd(DynamicSystem* sys);
93  virtual int getuInd(int i=0) { return uInd[i]; }
94  //virtual int getuInd(DynamicSystem* sys, int i=0);
95  //virtual void setqInd(int qInd_) { qInd = qInd_; }
96  //virtual void setuInd(int uInd_, int i=0) { uInd[i] = uInd_; }
97  virtual void setqInd(int qInd_);
98  virtual void setuInd(int uInd_, int i=0);
99  virtual void sethInd(int hInd_, int i=0);
100  virtual void setxInd(int xInd_);
101  //virtual int gethInd(DynamicSystem* sys, int i=0);
102  virtual const fmatvec::Vec& getq() const { return q; };
103  virtual fmatvec::Vec& getq() { return q; };
104  virtual const fmatvec::Vec& getu() const { return u; };
105  virtual fmatvec::Vec& getu() { return u; };
106  virtual H5::Group *getPlotGroup() { return plotGroup; }
109 #ifdef HAVE_OPENMBVCPPINTERFACE
110  virtual OpenMBV::Group* getOpenMBVGrp();
111 #endif
112 
116  std::string getPath(char pathDelim='.') { return parent?parent->getPath()+pathDelim+name:name; }
117  /*****************************************************/
118 
119  /* INHERITED INTERFACE OF LINKINTERFACE */
120  virtual void updatewb(double t, int j=0);
121  virtual void updateW(double t, int j=0);
122  virtual void updateV(double t, int j=0);
123  virtual void updateg(double t);
124  virtual void updategd(double t);
125  virtual void updateStopVector(double t);
126  virtual void updateLinkStatus(double t);
127  virtual void updateLinkStatusReg(double t);
128 
129  virtual void updategInverseKinetics(double t);
130  virtual void updategdInverseKinetics(double t);
131  virtual void updateWInverseKinetics(double t, int j=0);
132  virtual void updatehInverseKinetics(double t, int j=0);
133  virtual void updateJacobiansInverseKinetics(double t, int j=0);
134  virtual void updatebInverseKinetics(double t);
135  /*****************************************************/
136 
137  /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
138  virtual void updatedx(double t, double dt);
139  virtual void updatexd(double t);
140  virtual void calcxSize();
141  const fmatvec::Vec& getx() const { return x; };
142  fmatvec::Vec& getx() { return x; };
143  int getxSize() const { return xSize; }
144  void updatexRef(const fmatvec::Vec &ref);
145  void updatexdRef(const fmatvec::Vec &ref);
146  virtual void init(InitStage stage);
147  virtual void initz();
148  /*****************************************************/
149 
150  /* INHERITED INTERFACE OF ELEMENT */
152  virtual std::string getType() const { return "DynamicSystem"; }
153  virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
154  virtual void plot(double t, double dt);
155  virtual void plotAtSpecialEvent(double t, double dt=1.);
156  virtual void closePlot();
157  /*****************************************************/
158 
159  /* INTERFACE FOR DERIVED CLASSES */
163  virtual void facLLM(int i=0) = 0;
164 
168  virtual int solveConstraintsFixpointSingle();
169 
173  virtual int solveImpactsFixpointSingle(double dt);
174 
178  virtual int solveConstraintsGaussSeidel();
179 
183  virtual int solveImpactsGaussSeidel(double dt);
184 
188  virtual int solveConstraintsRootFinding();
189 
193  virtual int solveImpactsRootFinding(double dt);
194 
198  virtual int jacobianConstraints();
199 
203  virtual int jacobianImpacts();
204 
208  virtual void checkConstraintsForTermination();
209 
213  virtual void checkImpactsForTermination(double dt);
214 
218  virtual void updaterFactors();
219 
225  virtual Frame* getFrame(const std::string &name, bool check=true);
226 
232  virtual Contour* getContour(const std::string &name, bool check=true);
233  /*****************************************************/
234 
235  /* GETTER / SETTER */
236  void setPosition(const fmatvec::Vec& PrPF_) { PrPF = PrPF_; }
237  void setOrientation(const fmatvec::SqrMat& APF_) { APF = APF_; }
238  void setFrameOfReference(Frame *frame) { frameParent = frame; };
239 
240  const fmatvec::Vec& getxd() const { return xd; };
241  fmatvec::Vec& getxd() { return xd; };
242  const fmatvec::Vec& getx0() const { return x0; };
243  fmatvec::Vec& getx0() { return x0; };
244 
245  const fmatvec::Mat& getT() const { return T; };
246  const fmatvec::SymMat& getM(int i=0) const { return M[i]; };
247  const fmatvec::SymMat& getLLM(int i=0) const { return LLM[i]; };
248  const fmatvec::Vec& geth(int j=0) const { return h[j]; };
249  const fmatvec::Vec& getf() const { return f; };
250  fmatvec::Vec& getf() { return f; };
251 
252  const fmatvec::Mat& getW(int i=0) const { return W[i]; }
253  fmatvec::Mat& getW(int i=0) { return W[i]; }
254  const fmatvec::Mat& getV(int i=0) const { return V[i]; }
255  fmatvec::Mat& getV(int i=0) { return V[i]; }
256 
257  const fmatvec::Vec& getla() const { return la; }
258  fmatvec::Vec& getla() { return la; }
259  const fmatvec::Vec& getg() const { return g; }
260  fmatvec::Vec& getg() { return g; }
261  const fmatvec::Vec& getgd() const { return gd; }
262  fmatvec::Vec& getgd() { return gd; }
263  const fmatvec::Vec& getrFactor() const { return rFactor; }
264  fmatvec::Vec& getrFactor() { return rFactor; }
265  fmatvec::Vec& getsv() { return sv; }
266  const fmatvec::Vec& getsv() const { return sv; }
267  fmatvec::Vector<int>& getjsv() { return jsv; }
268  const fmatvec::Vector<int>& getjsv() const { return jsv; }
269  fmatvec::Vector<int>& getLinkStatus() { return LinkStatus; }
270  fmatvec::Vector<int>& getLinkStatusReg() { return LinkStatusReg; }
271  const fmatvec::Vector<int>& getLinkStatus() const { return LinkStatus; }
272  const fmatvec::Vector<int>& getLinkStatusReg() const { return LinkStatusReg; }
273  const fmatvec::Vec& getres() const { return res; }
274  fmatvec::Vec& getres() { return res; }
275 
276  void setx(const fmatvec::Vec& x_) { x = x_; }
277  void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
278  void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
279 
280  int getxInd() { return xInd; }
281  int getlaInd() const { return laInd; }
282 
283  int gethInd(int i=0) { return hInd[i]; }
284  void setlaInd(int ind) { laInd = ind; }
285  void setgInd(int ind) { gInd = ind; }
286  void setgdInd(int ind) { gdInd = ind; }
287  void setrFactorInd(int ind) { rFactorInd = ind; }
288  void setsvInd(int svInd_) { svInd = svInd_; };
289  void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
290  void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
291 
292  int getzSize() const { return qSize + uSize[0] + xSize; }
293 
294  void setqSize(int qSize_) { qSize = qSize_; }
295  void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
296  void setxSize(int xSize_) { xSize = xSize_; }
297 
298  int getlaSize() const { return laSize; }
299  int getgSize() const { return gSize; }
300  int getgdSize() const { return gdSize; }
301  int getrFactorSize() const { return rFactorSize; }
302  int getsvSize() const { return svSize; }
303  int getLinkStatusSize() const { return LinkStatusSize; }
304  int getLinkStatusRegSize() const { return LinkStatusRegSize; }
305  /*****************************************************/
306 
311  void updateqRef(const fmatvec::Vec &ref);
312 
317  void updateqdRef(const fmatvec::Vec &ref);
318 
323  void updateuRef(const fmatvec::Vec &ref);
324 
329  void updateuallRef(const fmatvec::Vec &ref);
330 
335  void updateudRef(const fmatvec::Vec &ref, int i=0);
336 
341  void updateudallRef(const fmatvec::Vec &ref, int i=0);
342 
350  void updatehRef(const fmatvec::Vec &hRef, int i=0);
351 
356  void updatefRef(const fmatvec::Vec &ref);
357 
362  void updaterRef(const fmatvec::Vec &ref, int j=0);
363 
368  void updateTRef(const fmatvec::Mat &ref);
369 
375  void updateMRef(const fmatvec::SymMat &ref, int i=0);
376 
382  void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
383 
388  void updategRef(const fmatvec::Vec &ref);
389 
394  void updategdRef(const fmatvec::Vec &ref);
395 
400  void updatelaRef(const fmatvec::Vec &ref);
401 
402  void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
403  void updatebInverseKineticsRef(const fmatvec::Mat &ref);
404 
409  void updatewbRef(const fmatvec::Vec &ref);
410 
416  void updateWRef(const fmatvec::Mat &ref, int i=0);
417 
418  void updateWnVRefObjects();
419 
425  void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
426 
432  void updateVRef(const fmatvec::Mat &ref, int i=0);
433 
438  void updatesvRef(const fmatvec::Vec& ref);
439 
444  void updatejsvRef(const fmatvec::Vector<int> &ref);
445 
450  void updateLinkStatusRef(const fmatvec::Vector<int> &LinkStatusParent);
451 
456  void updateLinkStatusRegRef(const fmatvec::Vector<int> &LinkStatusRegParent);
457 
462  void updateresRef(const fmatvec::Vec &ref);
463 
468  void updaterFactorRef(const fmatvec::Vec &ref);
469 
475  virtual void buildListOfObjects(std::vector<Object*> &obj, bool recursive=false);
476 
482  virtual void buildListOfLinks(std::vector<Link*> &lnk, bool recursive=false);
483 
489  virtual void buildListOfSetValuedLinks(std::vector<Link*> &lnk, bool recursive=false);
490 
496  virtual void buildListOfFrames(std::vector<Frame*> &frm, bool recursive);
497 
503  virtual void buildListOfContours(std::vector<Contour*> &cnt, bool recursive);
504 
510  virtual void buildListOfExtraDynamic(std::vector<ExtraDynamic*> &ed, bool recursive=false);
511 
517  void buildListOfModels(std::vector<ModellingInterface*> &model, bool recursive = true);
518 
522  void setUpInverseKinetics();
523 
527  void setUpLinks();
528 
532  bool gActiveChanged();
533 
537  bool gActiveChangedReg();
538 
542  void calcsvSize();
543 
547  void calclaSize(int j);
548 
552  void calcLinkStatusSize();
553 
557  void calcLinkStatusRegSize();
558 
563 
568 
572  void calcgSize(int j);
573 
577  void calcgdSize(int j);
578 
582  void calcrFactorSize(int j);
583 
587  void setUpActiveLinks();
588 
592  void checkActive(int i);
593 
597  void checkActiveReg(int i);
598 
602  void setgTol(double tol);
603 
607  void setgdTol(double tol);
608 
612  void setgddTol(double tol);
613 
617  void setlaTol(double tol);
618 
622  void setLaTol(double tol);
623 
627  void setrMax(double rMax);
628 
635  void addFrame(Frame *frame_, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const std::string& refFrameName);
636 
643  void addFrame(Frame *frame_, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const Frame* refFrame=0);
644 
651  void addFrame(const std::string &str, const fmatvec::Vec &RrRF, const fmatvec::SqrMat &ARF, const Frame* refFrame=0);
652 
659  void addContour(Contour* contour, const fmatvec::Vec &RrRC, const fmatvec::SqrMat &ARC, const std::string& refFrameName);
660 
667  void addContour(Contour* contour, const fmatvec::Vec &RrRC, const fmatvec::SqrMat &ARC, const Frame* refFrame=0);
668 
674  void addContour(Contour* contour, const fmatvec::Vec &RrRC, const Frame* refFrame=0) { addContour(contour,RrRC,fmatvec::SqrMat(3,fmatvec::EYE),refFrame); }
675 
680  int frameIndex(const Frame *frame_) const;
681 
685  void addGroup(DynamicSystem *dynamicsystem);
686 
692  DynamicSystem* getGroup(const std::string &name,bool check=true);
693 
697  void addObject(Object *object);
698 
704  Object* getObject(const std::string &name,bool check=true);
705 
709  void addLink(Link *link);
710 
714  void addInverseKineticsLink(Link *link);
715 
716 #ifdef HAVE_OPENMBVCPPINTERFACE
717  Element* getOpenMBVElement(const std::string &name,bool check=true);
718  void addPlotElement(Element *element);
719 #endif
720 
726  Link* getLink(const std::string &name,bool check=true);
727 
731  void addExtraDynamic(ExtraDynamic *ed_);
732 
738  ExtraDynamic* getExtraDynamic(const std::string &name,bool check=true);
739 
743  void addModel(ModellingInterface *modell);
744 
750  ModellingInterface* getModel(const std::string &name, bool check=true);
751 
753  Frame *getFrameI() { return I; }
754 
755  virtual Element *getByPathSearch(std::string path);
756 
757  virtual void updatecorr(int j);
758  void updatecorrRef(const fmatvec::Vec &ref);
759  void calccorrSize(int j);
760 
761  void checkRoot();
762 
763  protected:
768 
772  fmatvec::Vec PrPF;
773 
777  fmatvec::SqrMat APF;
778 
782  std::vector<Object*> object;
783  std::vector<Link*> link;
784  std::vector<Link*> linkSingleValued;
785  std::vector<Link*> linkSetValued;
786  std::vector<Link*> linkSetValuedActive;
787  std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
788  std::vector<ExtraDynamic*> extraDynamic;
789  std::vector<ModellingInterface*> model;
790  std::vector<DynamicSystem*> dynamicsystem;
791  std::vector<Link*> inverseKineticsLink;
792 #ifdef HAVE_OPENMBVCPPINTERFACE
793  std::vector<Element*> plotElement;
794 #endif
795 
799  fmatvec::Mat T;
800 
804  fmatvec::SymMat M[2];
805 
809  fmatvec::SymMat LLM[2];
810 
814  fmatvec::Vec q, qd, q0;
815 
819  fmatvec::Vec u, ud[2], u0;
820 
824  fmatvec::Vec x, xd, x0;
825 
829  fmatvec::Vec h[2], r[2], f;
830 
834  fmatvec::Mat W[2], V[2];
835 
839  fmatvec::Vec la;
840 
844  fmatvec::Vec g, gd;
845 
849  fmatvec::Vec wb;
850 
854  fmatvec::Vec res;
855 
859  fmatvec::Vec rFactor;
860 
864  fmatvec::Vec sv;
865 
869  fmatvec::Vector<int> jsv;
870 
874  fmatvec::Vector<int> LinkStatus;
875 
879  fmatvec::Vector<int> LinkStatusReg;
880 
884  int qSize, qInd;
885 
889  int uSize[2], uInd[2];
890 
894  int xSize, xInd;
895 
899  int hSize[2], hInd[2];
900 
904  int gSize, gInd;
905 
909  int gdSize, gdInd;
910 
914  int laSize, laInd;
915 
919  int rFactorSize, rFactorInd;
920 
924  int svSize, svInd;
925 
929  int LinkStatusSize, LinkStatusInd;
930 
934  int LinkStatusRegSize, LinkStatusRegInd;
935 
939  std::vector<fmatvec::Vec> IrOF, IrOC;
940 
944  std::vector<fmatvec::SqrMat> AIF, AIC;
945 
949  std::vector<Frame*> frame;
950  std::vector<Contour*> contour;
951 
952 #ifdef HAVE_OPENMBVCPPINTERFACE
953  OpenMBV::Group* openMBVGrp;
954 #endif
955 
959  void addFrame(Frame *frame_);
960 
964  void addContour(Contour* contour);
965 
968 
972  int laInverseKineticsSize, bInverseKineticsSize;
973 
974  fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
975  fmatvec::Vec laInverseKinetics;
976 
977  int corrSize, corrInd;
978  fmatvec::Vec corr;
979 
980  private:
981  std::vector<std::string> saved_refFrameF, saved_refFrameC;
982  std::vector<fmatvec::Vec> saved_RrRF, saved_RrRC;
983  std::vector<fmatvec::SqrMat> saved_ARF, saved_ARC;
984  };
985 }
986 
987 #endif
988