00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef _DYNAMIC_SYSTEM_H_
00020 #define _DYNAMIC_SYSTEM_H_
00021
00022 #include "mbsim/element.h"
00023 #include "mbsim/mbsim_event.h"
00024
00025 namespace H5 {
00026 class Group;
00027 }
00028
00029 namespace MBSim {
00030 class Frame;
00031 class FixedRelativeFrame;
00032 class Contour;
00033 class Object;
00034 class Link;
00035 class ModellingInterface;
00036 class Contact;
00037 class InverseKineticsJoint;
00038 class Observer;
00039
00057 class DynamicSystem : public Element {
00058 public:
00062 DynamicSystem(const std::string &name);
00063
00067 virtual ~DynamicSystem();
00068
00069
00070 virtual void updateT(double t);
00071 virtual void updateh(double t, int i=0);
00072 virtual void updateh0Fromh1(double t);
00073 virtual void updateW0FromW1(double t);
00074 virtual void updateV0FromV1(double t);
00075 virtual void updateStateDependentVariables(double t) = 0;
00076 virtual void updateStateDerivativeDependentVariables(double t);
00077 virtual void updateM(double t, int i=0);
00078 virtual void updateJacobians(double t, int j=0) = 0;
00079 virtual void updatedq(double t, double dt);
00080 virtual void updateud(double t, int i=0) { THROW_MBSIMERROR("(DynamicSystem::updateud): Not implemented!"); }
00081 virtual void updatezd(double t) = 0;
00082 virtual void updatedu(double t, double dt) = 0;
00083 virtual void updateqd(double t);
00084 virtual void sethSize(int hSize_, int i=0);
00085 virtual int gethSize(int i=0) const { return hSize[i]; }
00086 virtual int getqSize() const { return qSize; }
00087 virtual int getuSize(int i=0) const { return uSize[i]; }
00088 virtual void calcqSize();
00089 virtual void calcuSize(int j=0);
00090
00091 virtual int getuInd(int i=0) { return uInd[i]; }
00092
00093
00094
00095 virtual void setqInd(int qInd_);
00096 virtual void setuInd(int uInd_, int i=0);
00097 virtual void sethInd(int hInd_, int i=0);
00098 virtual void setxInd(int xInd_);
00099
00100 virtual const fmatvec::Vec& getq() const { return q; };
00101 virtual fmatvec::Vec& getq() { return q; };
00102 virtual const fmatvec::Vec& getu() const { return u; };
00103 virtual fmatvec::Vec& getu() { return u; };
00104 virtual H5::GroupBase *getPlotGroup() { return plotGroup; }
00105 virtual PlotFeatureStatus getPlotFeature(PlotFeature fp) { return Element::getPlotFeature(fp); };
00106 virtual PlotFeatureStatus getPlotFeatureForChildren(PlotFeature fp) { return Element::getPlotFeatureForChildren(fp); };
00107 #ifdef HAVE_OPENMBVCPPINTERFACE
00108 virtual OpenMBV::Group* getOpenMBVGrp();
00109 #endif
00110
00111 virtual void updatewb(double t, int j=0);
00112 virtual void updateW(double t, int j=0);
00113 virtual void updateV(double t, int j=0);
00114 virtual void updateg(double t);
00115 virtual void updategd(double t);
00116 virtual void updateStopVector(double t);
00117 virtual void updateLinkStatus(double t);
00118 virtual void updateLinkStatusReg(double t);
00119
00120 virtual void updategInverseKinetics(double t);
00121 virtual void updategdInverseKinetics(double t);
00122 virtual void updateWInverseKinetics(double t, int j=0);
00123 virtual void updatehInverseKinetics(double t, int j=0);
00124 virtual void updateJacobiansInverseKinetics(double t, int j=0);
00125 virtual void updatebInverseKinetics(double t);
00126
00127 virtual void updatedx(double t, double dt);
00128 virtual void updatexd(double t);
00129 virtual void calcxSize();
00130 const fmatvec::Vec& getx() const { return x; };
00131 fmatvec::Vec& getx() { return x; };
00132 int getxSize() const { return xSize; }
00133 void updatexRef(const fmatvec::Vec &ref);
00134 void updatexdRef(const fmatvec::Vec &ref);
00135 virtual void init(InitStage stage);
00136 virtual void initz();
00137 virtual void writez(H5::GroupBase *group);
00138 virtual void readz0(H5::GroupBase *parent);
00139
00140
00141
00143 virtual std::string getType() const { return "DynamicSystem"; }
00144 virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00145 virtual void plot(double t, double dt);
00146 virtual void plotAtSpecialEvent(double t, double dt=1.);
00147 virtual void closePlot();
00148
00149
00150
00154 virtual void facLLM(int i=0) = 0;
00155
00160 virtual int solveConstraintsFixpointSingle();
00161
00167 virtual int solveImpactsFixpointSingle(double dt);
00168
00173 virtual int solveConstraintsGaussSeidel();
00174
00180 virtual int solveImpactsGaussSeidel(double dt);
00181
00186 virtual int solveConstraintsRootFinding();
00187
00193 virtual int solveImpactsRootFinding(double dt);
00194
00198 virtual int jacobianConstraints();
00199
00203 virtual int jacobianImpacts();
00204
00208 virtual void checkConstraintsForTermination();
00209
00213 virtual void checkImpactsForTermination(double dt);
00214
00218 virtual void updaterFactors();
00219
00225 virtual Frame* getFrame(const std::string &name, bool check=true) const;
00226
00232 virtual Contour* getContour(const std::string &name, bool check=true) const;
00233
00234
00235
00236 void setPosition(const fmatvec::Vec3& PrPF_) { PrPF = PrPF_; }
00237 void setOrientation(const fmatvec::SqrMat3& APF_) { APF = APF_; }
00238 void setFrameOfReference(Frame *frame) { R = frame; };
00239 const fmatvec::Vec3& getPosition() const { return PrPF; }
00240 const fmatvec::SqrMat3& getOrientation() const { return APF; }
00241 const Frame* getFrameOfReference() const { return R; };
00242
00243 const fmatvec::Vec& getxd() const { return xd; };
00244 fmatvec::Vec& getxd() { return xd; };
00245 const fmatvec::Vec& getx0() const { return x0; };
00246 fmatvec::Vec& getx0() { return x0; };
00247
00248 const fmatvec::Mat& getT() const { return T; };
00249 const fmatvec::SymMat& getM(int i=0) const { return M[i]; };
00250 const fmatvec::SymMat& getLLM(int i=0) const { return LLM[i]; };
00251 fmatvec::SymMat& getLLM(int i=0) { return LLM[i]; };
00252 const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00253 fmatvec::Vec& geth(int j=0) { return h[j]; };
00254 const fmatvec::Vec& getf() const { return f; };
00255 fmatvec::Vec& getf() { return f; };
00256
00257 const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00258 fmatvec::Mat& getW(int i=0) { return W[i]; }
00259 const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00260 fmatvec::Mat& getV(int i=0) { return V[i]; }
00261
00262 const fmatvec::Vec& getla() const { return la; }
00263 fmatvec::Vec& getla() { return la; }
00264 const fmatvec::Vec& getg() const { return g; }
00265 fmatvec::Vec& getg() { return g; }
00266 const fmatvec::Vec& getgd() const { return gd; }
00267 fmatvec::Vec& getgd() { return gd; }
00268 const fmatvec::Vec& getrFactor() const { return rFactor; }
00269 fmatvec::Vec& getrFactor() { return rFactor; }
00270 fmatvec::Vec& getsv() { return sv; }
00271 const fmatvec::Vec& getsv() const { return sv; }
00272 fmatvec::VecInt& getjsv() { return jsv; }
00273 const fmatvec::VecInt& getjsv() const { return jsv; }
00274 fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00275 fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00276 const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00277 const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00278 const fmatvec::Vec& getres() const { return res; }
00279 fmatvec::Vec& getres() { return res; }
00280 const fmatvec::Vec& getcorr() const { return corr; };
00281 fmatvec::Vec& getcorr() { return corr; };
00282
00283 void setx(const fmatvec::Vec& x_) { x = x_; }
00284 void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00285 void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00286
00287 int getxInd() { return xInd; }
00288 int getlaInd() const { return laInd; }
00289
00290 int gethInd(int i=0) { return hInd[i]; }
00291 void setlaInd(int ind) { laInd = ind; }
00292 void setgInd(int ind) { gInd = ind; }
00293 void setgdInd(int ind) { gdInd = ind; }
00294 void setrFactorInd(int ind) { rFactorInd = ind; }
00295 void setsvInd(int svInd_) { svInd = svInd_; };
00296 void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
00297 void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00298
00299 int getzSize() const { return qSize + uSize[0] + xSize; }
00300
00301 void setqSize(int qSize_) { qSize = qSize_; }
00302 void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00303 void setxSize(int xSize_) { xSize = xSize_; }
00304
00305 int getlaSize() const { return laSize; }
00306 int getgSize() const { return gSize; }
00307 int getgdSize() const { return gdSize; }
00308 int getrFactorSize() const { return rFactorSize; }
00309 int getsvSize() const { return svSize; }
00310 int getLinkStatusSize() const { return LinkStatusSize; }
00311 int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00312
00313
00314 const std::vector<Object*>& getObjects() const { return object; }
00315 const std::vector<Link*>& getLinks() const { return link; }
00316 const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00317 const std::vector<Frame*>& getFrames() const { return frame; }
00318 const std::vector<Contour*>& getContours() const { return contour; }
00319
00324 void updateqRef(const fmatvec::Vec &ref);
00325
00330 void updateqdRef(const fmatvec::Vec &ref);
00331
00336 void updateuRef(const fmatvec::Vec &ref);
00337
00342 void updateuallRef(const fmatvec::Vec &ref);
00343
00348 void updateudRef(const fmatvec::Vec &ref, int i=0);
00349
00354 void updateudallRef(const fmatvec::Vec &ref, int i=0);
00355
00363 void updatehRef(const fmatvec::Vec &hRef, int i=0);
00364
00369 void updaterRef(const fmatvec::Vec &ref, int j=0);
00370
00375 void updateTRef(const fmatvec::Mat &ref);
00376
00382 void updateMRef(const fmatvec::SymMat &ref, int i=0);
00383
00389 void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00390
00395 void updategRef(const fmatvec::Vec &ref);
00396
00401 void updategdRef(const fmatvec::Vec &ref);
00402
00407 void updatelaRef(const fmatvec::Vec &ref);
00408
00409 void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00410 void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00411
00416 void updatewbRef(const fmatvec::Vec &ref);
00417
00423 void updateWRef(const fmatvec::Mat &ref, int i=0);
00424
00425 void updateWnVRefObjects();
00426
00432 void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00433
00439 void updateVRef(const fmatvec::Mat &ref, int i=0);
00440
00445 void updatesvRef(const fmatvec::Vec& ref);
00446
00451 void updatejsvRef(const fmatvec::VecInt &ref);
00452
00457 void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00458
00463 void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00464
00469 void updateresRef(const fmatvec::Vec &ref);
00470
00475 void updaterFactorRef(const fmatvec::Vec &ref);
00476
00477 void clearElementLists();
00478
00483 void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00484
00489 void buildListOfObjects(std::vector<Object*> &obj);
00490
00495 void buildListOfLinks(std::vector<Link*> &lnk);
00496
00501 void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00502
00507 void buildListOfFrames(std::vector<Frame*> &frm);
00508
00513 void buildListOfContours(std::vector<Contour*> &cnt);
00514
00519 void buildListOfModels(std::vector<ModellingInterface*> &model);
00520
00525 void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00526
00531 void buildListOfObservers(std::vector<Observer*> &obsrv);
00532
00536 void setUpInverseKinetics();
00537
00541 void setUpLinks();
00542
00546 bool gActiveChanged();
00547
00551 bool gActiveChangedReg();
00552
00556 bool detectImpact();
00557
00561 void calcsvSize();
00562
00566 void calclaSize(int j);
00567
00571 void calcLinkStatusSize();
00572
00576 void calcLinkStatusRegSize();
00577
00581 void calclaInverseKineticsSize();
00582
00586 void calcbInverseKineticsSize();
00587
00591 void calcgSize(int j);
00592
00603 void calcgdSize(int j);
00604
00608 void calcrFactorSize(int j);
00609
00613 void setUpActiveLinks();
00614
00629 void checkActive(int i);
00630
00634 void checkActiveReg(int i);
00635
00639 virtual void setgTol(double tol);
00640
00644 virtual void setgdTol(double tol);
00645
00649 virtual void setgddTol(double tol);
00650
00654 virtual void setlaTol(double tol);
00655
00659 virtual void setLaTol(double tol);
00660
00664 void setrMax(double rMax);
00665
00666 void addFrame(FixedRelativeFrame *frame);
00667
00672 int frameIndex(const Frame *frame_) const;
00673
00677 void addGroup(DynamicSystem *dynamicsystem);
00678
00684 DynamicSystem* getGroup(const std::string &name,bool check=true) const;
00685
00689 void addObject(Object *object);
00690
00696 Object* getObject(const std::string &name,bool check=true) const;
00697
00701 void addLink(Link *link);
00702
00706 void addInverseKineticsLink(Link *link);
00707
00708 Observer* getObserver(const std::string &name,bool check=true) const;
00709 void addObserver(Observer *element);
00710
00716 Link* getLink(const std::string &name,bool check=true) const;
00717
00721 void addModel(ModellingInterface *modell);
00722
00728 ModellingInterface* getModel(const std::string &name, bool check=true) const;
00729
00731 FixedRelativeFrame *getFrameI() { return I; }
00732
00733 virtual Element *getChildByContainerAndName(const std::string &container, const std::string &name) const;
00734
00735 virtual void updatecorr(int j);
00736 void updatecorrRef(const fmatvec::Vec &ref);
00737 void calccorrSize(int j);
00738
00739 void checkRoot();
00740
00741 protected:
00745 Frame *R;
00746
00750 fmatvec::Vec3 PrPF;
00751
00755 fmatvec::SqrMat3 APF;
00756
00760 std::vector<Object*> object;
00761 std::vector<Link*> link;
00762 std::vector<Link*> linkSingleValued;
00763 std::vector<Link*> linkSetValued;
00764 std::vector<Link*> linkSetValuedActive;
00765 std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00766 std::vector<ModellingInterface*> model;
00767 std::vector<DynamicSystem*> dynamicsystem;
00768 std::vector<Link*> inverseKineticsLink;
00769 std::vector<Observer*> observer;
00770
00774 fmatvec::Mat T;
00775
00779 fmatvec::SymMat M[2];
00780
00784 fmatvec::SymMat LLM[2];
00785
00789 fmatvec::Vec q, qd, q0;
00790
00794 fmatvec::Vec u, ud[2], u0;
00795
00799 fmatvec::Vec x, xd, x0;
00800
00804 fmatvec::Vec h[2], r[2], f;
00805
00809 fmatvec::Mat W[2], V[2];
00810
00814 fmatvec::Vec la;
00815
00819 fmatvec::Vec g, gd;
00820
00824 fmatvec::Vec wb;
00825
00829 fmatvec::Vec res;
00830
00834 fmatvec::Vec rFactor;
00835
00839 fmatvec::Vec sv;
00840
00844 fmatvec::VecInt jsv;
00845
00849 fmatvec::VecInt LinkStatus;
00850
00854 fmatvec::VecInt LinkStatusReg;
00855
00859 int qSize, qInd;
00860
00864 int uSize[2], uInd[2];
00865
00869 int xSize, xInd;
00870
00874 int hSize[2], hInd[2];
00875
00879 int gSize, gInd;
00880
00884 int gdSize, gdInd;
00885
00889 int laSize, laInd;
00890
00894 int rFactorSize, rFactorInd;
00895
00899 int svSize, svInd;
00900
00904 int LinkStatusSize, LinkStatusInd;
00905
00909 int LinkStatusRegSize, LinkStatusRegInd;
00910
00914 std::vector<Frame*> frame;
00915 std::vector<Contour*> contour;
00916
00917 #ifdef HAVE_OPENMBVCPPINTERFACE
00918 OpenMBV::Group* openMBVGrp;
00919 #endif
00920 boost::shared_ptr<H5::File> hdf5File;
00921
00925 void addContour(Contour* contour);
00926
00928 FixedRelativeFrame *I;
00929
00933 int laInverseKineticsSize, bInverseKineticsSize;
00934
00935 fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00936 fmatvec::Vec laInverseKinetics;
00937
00938 int corrSize, corrInd;
00939 fmatvec::Vec corr;
00940
00941 std::string saved_frameOfReference;
00942 };
00943 }
00944
00945 #endif
00946