All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
dynamic_system.h
1 /* Copyright (C) 2004-2014 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/mbsim_event.h"
24 
25 namespace H5 {
26  class Group;
27 }
28 
29 namespace MBSim {
30  class Frame;
31  class FixedRelativeFrame;
32  class Contour;
33  class RigidContour;
34  class Object;
35  class Link;
36  class Constraint;
37  class ModellingInterface;
38  class Contact;
39  class InverseKineticsJoint;
40  class Observer;
41 
59  class DynamicSystem : public Element {
60  public:
64  DynamicSystem(const std::string &name);
65 
69  virtual ~DynamicSystem();
70 
71  /* INTERFACE FOR DERIVED CLASSES */
72  virtual void updateT();
73  virtual void updateh(int i=0);
74  virtual void updateM();
75  virtual void updatedq();
76  virtual void updatezd() = 0;
77  virtual void updatedu() = 0;
78  virtual void sethSize(int hSize_, int i=0);
79  virtual int gethSize(int i=0) const { return hSize[i]; }
80  virtual int getqSize() const { return qSize; }
81  virtual int getuSize(int i=0) const { return uSize[i]; }
82  virtual void calcqSize();
83  virtual void calcuSize(int j=0);
84  //virtual int getqInd(DynamicSystem* sys);
85  virtual int getuInd(int i=0) { return uInd[i]; }
86  //virtual int getuInd(DynamicSystem* sys, int i=0);
87  //virtual void setqInd(int qInd_) { qInd = qInd_; }
88  //virtual void setuInd(int uInd_, int i=0) { uInd[i] = uInd_; }
89  virtual void setqInd(int qInd_);
90  virtual void setuInd(int uInd_, int i=0);
91  virtual void sethInd(int hInd_, int i=0);
92  virtual void setxInd(int xInd_);
93  //virtual int gethInd(DynamicSystem* sys, int i=0);
94  virtual const fmatvec::Vec& getq() const { return q; };
95  virtual fmatvec::Vec& getq() { return q; };
96  virtual const fmatvec::Vec& getu() const { return u; };
97  virtual fmatvec::Vec& getu() { return u; };
98  void setq(const fmatvec::Vec& q_){ q = q_;}
99  void setu(const fmatvec::Vec& u_){ u = u_;}
100  void setjsv(const fmatvec::VecInt& jsv_){ jsv = jsv_;}
101  virtual H5::GroupBase *getPlotGroup() { return plotGroup; }
102  virtual PlotFeatureStatus getPlotFeature(PlotFeature fp) { return Element::getPlotFeature(fp); };
103  virtual PlotFeatureStatus getPlotFeatureForChildren(PlotFeature fp) { return Element::getPlotFeatureForChildren(fp); };
104  virtual std::shared_ptr<OpenMBV::Group> getOpenMBVGrp();
105 
106  virtual void updatewb();
107  virtual void updateW(int j=0);
108  virtual void updateV(int j=0);
109  virtual void updateg();
110  virtual void updategd();
111  virtual void updateStopVector();
112  virtual void updateLinkStatus();
113  virtual void updateLinkStatusReg();
114 
115  virtual void updateWInverseKinetics();
116  virtual void updatebInverseKinetics();
117 
118  virtual void updatedx();
119  virtual void calcxSize();
120  const fmatvec::Vec& getx() const { return x; };
121  fmatvec::Vec& getx() { return x; };
122  int getxSize() const { return xSize; }
123  void updatexRef(const fmatvec::Vec &ref);
124  void updatexdRef(const fmatvec::Vec &ref);
125  void updatedxRef(const fmatvec::Vec &ref);
126  virtual void init(InitStage stage);
127  virtual void initz();
128  virtual void writez(H5::GroupBase *group);
129  virtual void readz0(H5::GroupBase *parent);
130  /*****************************************************/
131 
132  /* INHERITED INTERFACE OF ELEMENT */
134  virtual std::string getType() const { return "DynamicSystem"; }
135  virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
136  virtual void plot();
137  virtual void plotAtSpecialEvent();
138  virtual void closePlot();
139  /*****************************************************/
140 
141  /* INTERFACE FOR DERIVED CLASSES */
145  virtual void updateLLM() = 0;
146 
151  virtual int solveConstraintsFixpointSingle();
152 
158  virtual int solveImpactsFixpointSingle();
159 
164  virtual int solveConstraintsGaussSeidel();
165 
171  virtual int solveImpactsGaussSeidel();
172 
177  virtual int solveConstraintsRootFinding();
178 
184  virtual int solveImpactsRootFinding();
185 
189  virtual int jacobianConstraints();
190 
194  virtual int jacobianImpacts();
195 
199  virtual void checkConstraintsForTermination();
200 
204  virtual void checkImpactsForTermination();
205 
209  virtual void updaterFactors();
210 
216  virtual Frame* getFrame(const std::string &name, bool check=true) const;
217 
223  virtual Contour* getContour(const std::string &name, bool check=true) const;
224  /*****************************************************/
225 
226  /* GETTER / SETTER */
227  void setPosition(const fmatvec::Vec3& PrPF_) { PrPF = PrPF_; }
228  void setOrientation(const fmatvec::SqrMat3& APF_) { APF = APF_; }
229  void setFrameOfReference(Frame *frame) { R = frame; };
230  const fmatvec::Vec3& getPosition() const { return PrPF; }
231  const fmatvec::SqrMat3& getOrientation() const { return APF; }
232  const Frame* getFrameOfReference() const { return R; };
233 
234  const fmatvec::Vec& getx0() const { return x0; };
235  fmatvec::Vec& getx0() { return x0; };
236 
237  const fmatvec::Mat& getT(bool check=true) const;
238  const fmatvec::Vec& geth(int j=0, bool check=true) const;
239  const fmatvec::SymMat& getM(bool check=true) const;
240  const fmatvec::SymMat& getLLM(bool check=true) const;
241  const fmatvec::Vec& getdq(bool check=true) const;
242  const fmatvec::Vec& getdu(bool check=true) const;
243  const fmatvec::Vec& getdx(bool check=true) const;
244  const fmatvec::Mat& getW(int i=0, bool check=true) const;
245  const fmatvec::Mat& getV(int i=0, bool check=true) const;
246  const fmatvec::Vec& getla(bool check=true) const;
247  const fmatvec::Vec& getLa(bool check=true) const;
248  const fmatvec::Vec& getg(bool check=true) const;
249  const fmatvec::Vec& getgd(bool check=true) const;
250  fmatvec::Vec& getla(bool check=true);
251  fmatvec::Vec& getLa(bool check=true);
252  void setla(const fmatvec::Vec &la_) { la = la_; }
253  void setLa(const fmatvec::Vec &La_) { La = La_; }
254  fmatvec::VecInt& getjsv() { return jsv; }
255  const fmatvec::VecInt& getjsv() const { return jsv; }
256  fmatvec::Mat& getW(int i=0, bool check=true);
257  fmatvec::SymMat& getLLM(bool check=true);
258  fmatvec::Vec& getdq(bool check=true);
259  fmatvec::Vec& getdu(bool check=true);
260  fmatvec::Vec& getdx(bool check=true);
261 
262  fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
263  fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
264  const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
265  const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
266 
267  const fmatvec::Mat& evalT();
268  const fmatvec::Vec& evalh(int i=0);
269  const fmatvec::SymMat& evalM();
270  const fmatvec::SymMat& evalLLM();
271  const fmatvec::Mat& evalW(int i=0);
272  const fmatvec::Mat& evalV(int i=0);
273  const fmatvec::Vec& evalwb();
274  const fmatvec::Vec& evalr(int i=0);
275  const fmatvec::Vec& evalrdt();
276  const fmatvec::Vec& evalg();
277  const fmatvec::Vec& evalgd();
278  const fmatvec::Mat& evalWInverseKinetics();
279  const fmatvec::Mat& evalbInverseKinetics();
280 
281  void setx(const fmatvec::Vec& x_) { x = x_; }
282  void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
283  void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
284 
285  void setqd(const fmatvec::Vec& qd_) { qd = qd_; }
286  void setud(const fmatvec::Vec& ud_) { ud = ud_; }
287  void setxd(const fmatvec::Vec& xd_) { xd = xd_; }
288 
289  int getxInd() { return xInd; }
290  int getlaInd() const { return laInd; }
291 
292  int gethInd(int i=0) { return hInd[i]; }
293  void setlaInd(int ind) { laInd = ind; }
294  void setgInd(int ind) { gInd = ind; }
295  void setgdInd(int ind) { gdInd = ind; }
296  void setrFactorInd(int ind) { rFactorInd = ind; }
297  virtual void setsvInd(int svInd_);
298  void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
299  void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
300 
301  int getzSize() const { return qSize + uSize[0] + xSize; }
302 
303  void setqSize(int qSize_) { qSize = qSize_; }
304  void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
305  void setxSize(int xSize_) { xSize = xSize_; }
306 
307  int getlaSize() const { return laSize; }
308  int getgSize() const { return gSize; }
309  int getgdSize() const { return gdSize; }
310  int getrFactorSize() const { return rFactorSize; }
311  int getsvSize() const { return svSize; }
312  int getLinkStatusSize() const { return LinkStatusSize; }
313  int getLinkStatusRegSize() const { return LinkStatusRegSize; }
314  /*****************************************************/
315 
316  const std::vector<Object*>& getObjects() const { return object; }
317  const std::vector<Link*>& getLinks() const { return link; }
318  const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
319  const std::vector<Frame*>& getFrames() const { return frame; }
320  const std::vector<Contour*>& getContours() const { return contour; }
321  const std::vector<Link*>& getSetValuedLinks() const { return linkSetValued; }
322 
327  void updateqRef(const fmatvec::Vec &ref);
328 
333  void updateqdRef(const fmatvec::Vec &ref);
334 
335  void updatedqRef(const fmatvec::Vec &ref);
336 
341  void updateuRef(const fmatvec::Vec &ref);
342 
347  void updateuallRef(const fmatvec::Vec &ref);
348 
353  void updateudRef(const fmatvec::Vec &ref);
354 
355  void updateduRef(const fmatvec::Vec &ref);
356 
361  void updateudallRef(const fmatvec::Vec &ref);
362 
370  void updatehRef(const fmatvec::Vec &hRef, int i=0);
371 
376  void updaterRef(const fmatvec::Vec &ref, int j=0);
377 
382  void updaterdtRef(const fmatvec::Vec &ref);
383 
388  void updateTRef(const fmatvec::Mat &ref);
389 
395  void updateMRef(const fmatvec::SymMat &ref);
396 
402  void updateLLMRef(const fmatvec::SymMat &ref);
403 
408  virtual void updategRef(const fmatvec::Vec &ref);
409 
414  virtual void updategdRef(const fmatvec::Vec &ref);
415 
420  void updatelaRef(const fmatvec::Vec &ref);
421 
426  void updateLaRef(const fmatvec::Vec &ref);
427 
428  void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
429  void updatebInverseKineticsRef(const fmatvec::Mat &ref);
430 
435  virtual void updatewbRef(const fmatvec::Vec &ref);
436 
442  virtual void updateWRef(const fmatvec::Mat &ref, int i=0);
443 
449  void updateWInverseKineticsRef(const fmatvec::Mat &ref);
450 
456  virtual void updateVRef(const fmatvec::Mat &ref, int i=0);
457 
462  void updatesvRef(const fmatvec::Vec& ref);
463 
468  void updatejsvRef(const fmatvec::VecInt &ref);
469 
474  void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
475 
480  void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
481 
486  void updateresRef(const fmatvec::Vec &ref);
487 
492  void updaterFactorRef(const fmatvec::Vec &ref);
493 
494  void clearElementLists();
495 
500  void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
501 
506  void buildListOfObjects(std::vector<Object*> &obj);
507 
512  void buildListOfLinks(std::vector<Link*> &lnk);
513 
518  void buildListOfConstraints(std::vector<Constraint*> &crt);
519 
524  void buildListOfFrames(std::vector<Frame*> &frm);
525 
530  void buildListOfContours(std::vector<Contour*> &cnt);
531 
536  void buildListOfModels(std::vector<ModellingInterface*> &model);
537 
542  void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
543 
548  void buildListOfObservers(std::vector<Observer*> &obsrv);
549 
553  void setUpInverseKinetics();
554 
558  void setUpLinks();
559 
563  bool gActiveChanged();
564 
568  bool gActiveChangedReg();
569 
573  bool detectImpact();
574 
578  void calcsvSize();
579 
583  void calclaSize(int j);
584 
588  void calcLinkStatusSize();
589 
593  void calcLinkStatusRegSize();
594 
599 
604 
608  void calcgSize(int j);
609 
620  void calcgdSize(int j);
621 
625  void calcrFactorSize(int j);
626 
630  void setUpActiveLinks();
631 
646  void checkActive(int i);
647 
651  void checkActiveReg(int i);
652 
656  virtual void setgTol(double tol);
657 
661  virtual void setgdTol(double tol);
662 
666  virtual void setgddTol(double tol);
667 
671  virtual void setlaTol(double tol);
672 
676  virtual void setLaTol(double tol);
677 
681  void setrMax(double rMax);
682 
687  int frameIndex(const Frame *frame_) const;
688 
689  void addFrame(FixedRelativeFrame *frame);
690 
691  void addContour(RigidContour *contour);
692 
696  void addGroup(DynamicSystem *dynamicsystem);
697 
703  DynamicSystem* getGroup(const std::string &name,bool check=true) const;
704 
708  void addObject(Object *object);
709 
715  Object* getObject(const std::string &name,bool check=true) const;
716 
720  void addLink(Link *link);
721 
725  void addConstraint(Constraint *constraint);
726 
730  void addInverseKineticsLink(Link *link);
731 
732  Observer* getObserver(const std::string &name,bool check=true) const;
733  void addObserver(Observer *element);
734 
740  Link* getLink(const std::string &name,bool check=true) const;
741 
747  Constraint* getConstraint(const std::string &name,bool check=true) const;
748 
752  void addModel(ModellingInterface *modell);
753 
759  ModellingInterface* getModel(const std::string &name, bool check=true) const;
760 
762  Frame *getFrameI() { return I; }
763 
764  virtual Element *getChildByContainerAndName(const std::string &container, const std::string &name) const;
765 
766  virtual void updatecorr(int j);
767  void updatecorrRef(const fmatvec::Vec &ref);
768  void calccorrSize(int j);
769 
770  void checkRoot();
771 
772  void resetUpToDate();
773 
774  private:
775  friend class DynamicSystemSolver;
776  void addFrame(Frame *frame);
777 
778  void addContour(Contour *contour);
779  protected:
780 
785 
789  fmatvec::Vec3 PrPF;
790 
794  fmatvec::SqrMat3 APF;
795 
799  std::vector<Object*> object;
800  std::vector<Link*> link;
801  std::vector<Link*> linkSingleValued;
802  std::vector<Link*> linkSetValued;
803  std::vector<Link*> linkSetValuedActive;
804  std::vector<ModellingInterface*> model;
805  std::vector<DynamicSystem*> dynamicsystem;
806  std::vector<Link*> inverseKineticsLink;
807  std::vector<Observer*> observer;
808  std::vector<Link*> linkSmoothPart;
809  std::vector< std::vector<Element*> > elementOrdered;
810  std::vector< std::vector<Link*> > linkOrdered;
811  std::vector<Constraint*> constraint;
812 
817 
822 
827 
831  fmatvec::Vec q, qd, dq, q0;
832 
836  fmatvec::Vec u, ud, du, u0;
837 
841  fmatvec::Vec x, xd, dx, x0;
842 
846  fmatvec::Vec h[2], r[2], rdt;
847 
851  fmatvec::Mat W[2], V[2];
852 
857 
862 
867 
872 
877 
882 
886  fmatvec::VecInt jsv;
887 
891  fmatvec::VecInt LinkStatus;
892 
896  fmatvec::VecInt LinkStatusReg;
897 
901  int qSize, qInd;
902 
906  int uSize[2], uInd[2];
907 
911  int xSize, xInd;
912 
916  int hSize[2], hInd[2];
917 
921  int gSize, gInd;
922 
926  int gdSize, gdInd;
927 
931  int laSize, laInd;
932 
936  int rFactorSize, rFactorInd;
937 
941  int svSize, svInd;
942 
946  int LinkStatusSize, LinkStatusInd;
947 
951  int LinkStatusRegSize, LinkStatusRegInd;
952 
956  std::vector<Frame*> frame;
957  std::vector<Contour*> contour;
958 
959  std::shared_ptr<OpenMBV::Group> openMBVGrp;
960  std::shared_ptr<H5::File> hdf5File;
961 
964 
968  int laInverseKineticsSize, bInverseKineticsSize;
969 
970  fmatvec::Mat WInverseKinetics, bInverseKinetics;
971  fmatvec::Vec laInverseKinetics;
972 
973  int corrSize, corrInd;
974  fmatvec::Vec corr;
975 
976  std::string saved_frameOfReference;
977  };
978 }
979 
980 #endif
981 
int rFactorSize
size and local start index of rfactors relative to parent
Definition: dynamic_system.h:936
bool gActiveChangedReg()
Definition: dynamic_system.cc:1091
virtual void setlaTol(double tol)
Definition: dynamic_system.cc:1302
fmatvec::Vec3 PrPF
relative translation with respect to parent frame
Definition: dynamic_system.h:789
fmatvec::Vec q
positions, differentiated positions, initial positions
Definition: dynamic_system.h:831
virtual int jacobianConstraints()
compute JACOBIAN of contact equations
Definition: dynamic_system.cc:535
void buildListOfModels(std::vector< ModellingInterface * > &model)
build flat list of models
Definition: dynamic_system.cc:1014
int laInverseKineticsSize
size of contact force parameters of special links relative to parent
Definition: dynamic_system.h:968
Link * getLink(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:1414
int laSize
size and local start index of contact force parameters relative to parent
Definition: dynamic_system.h:931
Vector< Ref, double > Vec
virtual void init(InitStage stage)
plots time series header
Definition: dynamic_system.cc:388
solver interface for modelling and simulation of dynamic systems
Definition: dynamic_system_solver.h:48
void addModel(ModellingInterface *modell)
Definition: dynamic_system.cc:1442
void addGroup(DynamicSystem *dynamicsystem)
Definition: dynamic_system.cc:1465
virtual int solveImpactsRootFinding()
solve impact equations with Newton scheme on velocity level
Definition: dynamic_system.cc:527
Frame * getFrameI()
Definition: dynamic_system.h:762
void buildListOfObjects(std::vector< Object * > &obj)
build flat list of objects
Definition: dynamic_system.cc:969
fmatvec::SymMat LLM
Cholesky decomposition of mass matrix.
Definition: dynamic_system.h:826
void checkActiveReg(int i)
check if single-valued contacts are active
Definition: dynamic_system.cc:1276
void buildListOfContours(std::vector< Contour * > &cnt)
build flat list of contours
Definition: dynamic_system.cc:1005
virtual void plot()
plots time dependent data
Definition: dynamic_system.cc:330
virtual void updateWRef(const fmatvec::Mat &ref, int i=0)
references to contact force direction matrix of dynamic system parent
Definition: dynamic_system.cc:824
fmatvec::Mat T
linear relation matrix of position and velocity parameters
Definition: dynamic_system.h:816
int frameIndex(const Frame *frame_) const
Definition: dynamic_system.cc:1349
fmatvec::VecInt LinkStatus
status of set-valued links
Definition: dynamic_system.h:891
void setUpLinks()
distribute links to set- and single valued container
Definition: dynamic_system.cc:1052
fmatvec::Vec u
velocities, differentiated velocities, initial velocities
Definition: dynamic_system.h:836
virtual void updaterFactors()
update relaxation factors for contact equations
Definition: dynamic_system.cc:563
void updateqdRef(const fmatvec::Vec &ref)
references to differentiated positions of dynamic system parent
Definition: dynamic_system.cc:607
void updateLLMRef(const fmatvec::SymMat &ref)
references to Cholesky decomposition of dynamic system parent
Definition: dynamic_system.cc:769
int uSize[2]
size and local start index of velocities relative to parent
Definition: dynamic_system.h:906
fmatvec::Vec x
order one parameters, differentiated order one parameters, initial order one parameters ...
Definition: dynamic_system.h:841
virtual void checkImpactsForTermination()
validate force laws concerning given tolerances on velocity level
Definition: dynamic_system.cc:557
bool detectImpact()
Definition: dynamic_system.cc:1105
virtual void plotAtSpecialEvent()
plots time dependent data at special events
Definition: dynamic_system.cc:351
void updaterRef(const fmatvec::Vec &ref, int j=0)
references to nonsmooth right hand side of dynamic system parent
Definition: dynamic_system.cc:729
virtual int solveConstraintsRootFinding()
solve contact equations with Newton scheme
Definition: dynamic_system.cc:519
void updateresRef(const fmatvec::Vec &ref)
references to residuum of contact equations of dynamic system parent
Definition: dynamic_system.cc:872
int LinkStatusSize
size and local start index of set-valued link status vector relative to parent
Definition: dynamic_system.h:946
basic class for contour definition for rigid (which do not know about their shape) and flexible (they...
Definition: contour.h:40
virtual void closePlot()
closes plot file
Definition: dynamic_system.cc:372
virtual void setgTol(double tol)
Definition: dynamic_system.cc:1281
basic class of MBSim mainly for plotting
Definition: element.h:58
void updateqRef(const fmatvec::Vec &ref)
references to positions of dynamic system parent
Definition: dynamic_system.cc:597
int gSize
size and local start index of relative distances relative to parent
Definition: dynamic_system.h:921
virtual void setLaTol(double tol)
Definition: dynamic_system.cc:1309
void updatesvRef(const fmatvec::Vec &ref)
references to stopvector (rootfunction for event driven integrator) of dynamic system parent ...
Definition: dynamic_system.cc:852
virtual Contour * getContour(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:583
void buildListOfConstraints(std::vector< Constraint * > &crt)
build flat list of all constraints
Definition: dynamic_system.cc:987
void updateudRef(const fmatvec::Vec &ref)
references to differentiated velocities of dynamic system parent
Definition: dynamic_system.cc:645
int gdSize
size and local start index of relative velocities relative to parent
Definition: dynamic_system.h:926
fmatvec::Vec h[2]
smooth, smooth with respect to objects, smooth with respect to links and nonsmooth ...
Definition: dynamic_system.h:846
void calcrFactorSize(int j)
calculates size of relaxation factors for contact equations
Definition: dynamic_system.cc:1250
void calcbInverseKineticsSize()
calculates size of contact force parameters
Definition: dynamic_system.cc:1220
std::vector< Frame * > frame
vector of frames and contours
Definition: dynamic_system.h:956
void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent)
references to status vector of single valued links
Definition: dynamic_system.cc:896
fmatvec::Vec res
residuum of nonlinear contact equations for Newton scheme
Definition: dynamic_system.h:871
virtual void updategdRef(const fmatvec::Vec &ref)
references to relative velocities of dynamic system parent
Definition: dynamic_system.cc:786
fmatvec::SqrMat3 APF
relative rotation with respect to parent frame
Definition: dynamic_system.h:794
void updaterFactorRef(const fmatvec::Vec &ref)
references to relaxation factors for contact equations of dynamic system parent
Definition: dynamic_system.cc:879
void buildListOfFrames(std::vector< Frame * > &frm)
build flat list of frames
Definition: dynamic_system.cc:996
virtual void updateLLM()=0
compute Cholesky decomposition of mass matrix TODO necessary?
dynamic system as topmost hierarchical level
Definition: dynamic_system.h:59
void updateuallRef(const fmatvec::Vec &ref)
references to velocities of dynamic system parent
Definition: dynamic_system.cc:637
void buildListOfObservers(std::vector< Observer * > &obsrv)
build flat list of observers
Definition: dynamic_system.cc:1032
virtual std::string getType() const
Definition: dynamic_system.h:134
Frame * I
Definition: dynamic_system.h:963
Frame * R
parent frame
Definition: dynamic_system.h:784
void calclaInverseKineticsSize()
calculates size of contact force parameters
Definition: dynamic_system.cc:1210
void calcLinkStatusSize()
calculates size of set-valued link status vector
Definition: dynamic_system.cc:1157
virtual int solveImpactsGaussSeidel()
solve impact equations with Gauss-Seidel scheme on velocity level
Definition: dynamic_system.cc:511
Constraint * getConstraint(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:1428
virtual void updategRef(const fmatvec::Vec &ref)
references to relative distances of dynamic system parent
Definition: dynamic_system.cc:779
fmatvec::VecInt LinkStatusReg
status of single-valued links
Definition: dynamic_system.h:896
void checkActive(int i)
check if set-valued contacts are active and set corresponding attributes
Definition: dynamic_system.cc:1270
InitStage
The stages of the initialization.
Definition: element.h:97
virtual Element * getChildByContainerAndName(const std::string &container, const std::string &name) const
Get the Element named name in the container named container.
Definition: dynamic_system.cc:1483
int qSize
size and local start index of positions relative to parent
Definition: dynamic_system.h:901
virtual int solveConstraintsGaussSeidel()
solve contact equations with Gauss-Seidel scheme
Definition: dynamic_system.cc:503
DynamicSystem(const std::string &name)
constructor
Definition: dynamic_system.cc:45
void setUpActiveLinks()
rearrange vector of active setvalued links
Definition: dynamic_system.cc:1260
void updateudallRef(const fmatvec::Vec &ref)
references to velocities of dynamic system parent
Definition: dynamic_system.cc:665
PlotFeatureStatus getPlotFeature(PlotFeature pf)
Definition: element.h:214
virtual void setgdTol(double tol)
Definition: dynamic_system.cc:1288
void buildListOfLinks(std::vector< Link * > &lnk)
build flat list of links
Definition: dynamic_system.cc:978
void updateTRef(const fmatvec::Mat &ref)
references to linear transformation matrix between differentiated positions and velocities of dynamic...
Definition: dynamic_system.cc:749
Object * getObject(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:1371
virtual void updateVRef(const fmatvec::Mat &ref, int i=0)
references to condensed contact force direction matrix of dynamic system parent
Definition: dynamic_system.cc:845
void buildListOfInverseKineticsLinks(std::vector< Link * > &lnk)
build flat list of inverse kinetics links
Definition: dynamic_system.cc:1023
void setrMax(double rMax)
Definition: dynamic_system.cc:1316
std::string name
name of element
Definition: element.h:298
int LinkStatusRegSize
size and local start index of single-valued link status vector relative to parent ...
Definition: dynamic_system.h:951
DynamicSystem * getGroup(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:1357
void addLink(Link *link)
Definition: dynamic_system.cc:1385
void calcgdSize(int j)
calculates size of gap velocities
Definition: dynamic_system.cc:1240
ModellingInterface * getModel(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:1451
fmatvec::Vec wb
TODO.
Definition: dynamic_system.h:866
void updatejsvRef(const fmatvec::VecInt &ref)
references to boolean evaluation of stopvector concerning roots of dynamic system parent ...
Definition: dynamic_system.cc:862
virtual void checkConstraintsForTermination()
validate force laws concerning given tolerances
Definition: dynamic_system.cc:551
int svSize
size and local start index of stop vector relative to parent
Definition: dynamic_system.h:941
cartesian frame on bodies used for application of e.g. links and loads
Definition: frame.h:37
void updateWInverseKineticsRef(const fmatvec::Mat &ref)
references to contact force direction matrix of dynamic system parent
Definition: dynamic_system.cc:831
fmatvec::Vec rFactor
rfactors for relaxation nonlinear contact equations
Definition: dynamic_system.h:876
void calcgSize(int j)
calculates size of relative distances
Definition: dynamic_system.cc:1230
fmatvec::Vec sv
stop vector (root functions for event driven integration
Definition: dynamic_system.h:881
virtual int solveImpactsFixpointSingle()
solve impact equations with single step fixed point scheme on velocity level
Definition: dynamic_system.cc:495
fmatvec::Vec g
relative distances and velocities
Definition: dynamic_system.h:861
void calcLinkStatusRegSize()
calculates size of single-valued link status vector
Definition: dynamic_system.cc:1172
void updaterdtRef(const fmatvec::Vec &ref)
references to nonsmooth right hand side of dynamic system parent
Definition: dynamic_system.cc:739
bool gActiveChanged()
Definition: dynamic_system.cc:1077
void updatehRef(const fmatvec::Vec &hRef, int i=0)
references to smooth right hand side of dynamic system parent
Definition: dynamic_system.cc:712
void addInverseKineticsLink(Link *link)
Definition: dynamic_system.cc:1405
H5::GroupBase * plotGroup
associated plot group
Definition: element.h:332
void calcsvSize()
calculates size of stop vector
Definition: dynamic_system.cc:1187
virtual ~DynamicSystem()
destructor
Definition: dynamic_system.cc:61
void updateLaRef(const fmatvec::Vec &ref)
references to contact impulses of dynamic system parent
Definition: dynamic_system.cc:800
std::vector< Object * > object
container for possible ingredients
Definition: dynamic_system.h:799
fmatvec::Vec la
contact force parameters
Definition: dynamic_system.h:856
virtual void setDynamicSystemSolver(DynamicSystemSolver *sys)
sets the used dynamics system solver to the element
Definition: dynamic_system.cc:303
void updateuRef(const fmatvec::Vec &ref)
references to velocities of dynamic system parent
Definition: dynamic_system.cc:627
void setUpInverseKinetics()
analyse constraints of dynamic systems for usage in inverse kinetics
Definition: dynamic_system.cc:1041
int hSize[2]
size and local start index of order smooth right hand side relative to parent
Definition: dynamic_system.h:916
virtual Frame * getFrame(const std::string &name, bool check=true) const
Definition: dynamic_system.cc:569
int xSize
size and local start index of order one parameters relative to parent
Definition: dynamic_system.h:911
void calclaSize(int j)
calculates size of contact force parameters
Definition: dynamic_system.cc:1200
fmatvec::VecInt jsv
boolean evaluation of stop vector concerning roots
Definition: dynamic_system.h:886
PlotFeatureStatus
Plot feature status.
Definition: element.h:61
virtual int solveConstraintsFixpointSingle()
solve contact equations with single step fixed point scheme
Definition: dynamic_system.cc:487
virtual int jacobianImpacts()
compute JACOBIAN of contact equations on velocity level
Definition: dynamic_system.cc:543
fmatvec::SymMat M
mass matrix
Definition: dynamic_system.h:821
void updateMRef(const fmatvec::SymMat &ref)
references to mass matrix of dynamic system parent
Definition: dynamic_system.cc:759
void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent)
references to status vector of set valued links with piecewise link equations (which piece is valid) ...
Definition: dynamic_system.cc:886
void buildListOfDynamicSystems(std::vector< DynamicSystem * > &sys)
build flat list of dynamic systems
Definition: dynamic_system.cc:960
void addConstraint(Constraint *constraint)
Definition: dynamic_system.cc:1395
PlotFeature
Plot Features.
Definition: element.h:74
virtual void updatewbRef(const fmatvec::Vec &ref)
references to TODO of dynamic system parent
Definition: dynamic_system.cc:817
virtual void setgddTol(double tol)
Definition: dynamic_system.cc:1295
PlotFeatureStatus getPlotFeatureForChildren(PlotFeature pf)
Definition: element.h:219
void addObject(Object *object)
Definition: dynamic_system.cc:1474
void updatelaRef(const fmatvec::Vec &ref)
references to contact forces of dynamic system parent
Definition: dynamic_system.cc:793

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML