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 updateStateDependentVariables(double t) = 0;
00073 virtual void updateStateDerivativeDependentVariables(double t);
00074 virtual void updateM(double t, int i=0);
00075 virtual void updateJacobians(double t, int j=0) = 0;
00076 virtual void updatedq(double t, double dt);
00077 virtual void updateud(double t, int i=0) { THROW_MBSIMERROR("(DynamicSystem::updateud): Not implemented!"); }
00078 virtual void updatezd(double t) = 0;
00079 virtual void updatedu(double t, double dt) = 0;
00080 virtual void updateqd(double t);
00081 virtual void sethSize(int hSize_, int i=0);
00082 virtual int gethSize(int i=0) const { return hSize[i]; }
00083 virtual int getqSize() const { return qSize; }
00084 virtual int getuSize(int i=0) const { return uSize[i]; }
00085 virtual void calcqSize();
00086 virtual void calcuSize(int j=0);
00087
00088 virtual int getuInd(int i=0) { return uInd[i]; }
00089
00090
00091
00092 virtual void setqInd(int qInd_);
00093 virtual void setuInd(int uInd_, int i=0);
00094 virtual void sethInd(int hInd_, int i=0);
00095 virtual void setxInd(int xInd_);
00096
00097 virtual const fmatvec::Vec& getq() const { return q; };
00098 virtual fmatvec::Vec& getq() { return q; };
00099 virtual const fmatvec::Vec& getu() const { return u; };
00100 virtual fmatvec::Vec& getu() { return u; };
00101 virtual H5::GroupBase *getPlotGroup() { return plotGroup; }
00102 virtual PlotFeatureStatus getPlotFeature(PlotFeature fp) { return Element::getPlotFeature(fp); };
00103 virtual PlotFeatureStatus getPlotFeatureForChildren(PlotFeature fp) { return Element::getPlotFeatureForChildren(fp); };
00104 #ifdef HAVE_OPENMBVCPPINTERFACE
00105 virtual boost::shared_ptr<OpenMBV::Group> getOpenMBVGrp();
00106 #endif
00107
00108 virtual void updatewb(double t, int j=0);
00109 virtual void updateW(double t, int j=0);
00110 virtual void updateV(double t, int j=0);
00111 virtual void updateg(double t);
00112 virtual void updategd(double t);
00113 virtual void updateStopVector(double t);
00114 virtual void updateLinkStatus(double t);
00115 virtual void updateLinkStatusReg(double t);
00116
00117 virtual void updategInverseKinetics(double t);
00118 virtual void updategdInverseKinetics(double t);
00119 virtual void updateWInverseKinetics(double t, int j=0);
00120 virtual void updatehInverseKinetics(double t, int j=0);
00121 virtual void updateJacobiansInverseKinetics(double t, int j=0);
00122 virtual void updatebInverseKinetics(double t);
00123
00124 virtual void updatedx(double t, double dt);
00125 virtual void updatexd(double t);
00126 virtual void calcxSize();
00127 const fmatvec::Vec& getx() const { return x; };
00128 fmatvec::Vec& getx() { return x; };
00129 int getxSize() const { return xSize; }
00130 void updatexRef(const fmatvec::Vec &ref);
00131 void updatexdRef(const fmatvec::Vec &ref);
00132 virtual void init(InitStage stage);
00133 virtual void initz();
00134 virtual void writez(H5::GroupBase *group);
00135 virtual void readz0(H5::GroupBase *parent);
00136
00137
00138
00140 virtual std::string getType() const { return "DynamicSystem"; }
00141 virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00142 virtual void plot(double t, double dt);
00143 virtual void plotAtSpecialEvent(double t, double dt=1.);
00144 virtual void closePlot();
00145
00146
00147
00151 virtual void facLLM(int i=0) = 0;
00152
00157 virtual int solveConstraintsFixpointSingle();
00158
00164 virtual int solveImpactsFixpointSingle(double dt);
00165
00170 virtual int solveConstraintsGaussSeidel();
00171
00177 virtual int solveImpactsGaussSeidel(double dt);
00178
00183 virtual int solveConstraintsRootFinding();
00184
00190 virtual int solveImpactsRootFinding(double dt);
00191
00195 virtual int jacobianConstraints();
00196
00200 virtual int jacobianImpacts();
00201
00205 virtual void checkConstraintsForTermination();
00206
00210 virtual void checkImpactsForTermination(double dt);
00211
00215 virtual void updaterFactors();
00216
00222 virtual Frame* getFrame(const std::string &name, bool check=true) const;
00223
00229 virtual Contour* getContour(const std::string &name, bool check=true) const;
00230
00231
00232
00233 void setPosition(const fmatvec::Vec3& PrPF_) { PrPF = PrPF_; }
00234 void setOrientation(const fmatvec::SqrMat3& APF_) { APF = APF_; }
00235 void setFrameOfReference(Frame *frame) { R = frame; };
00236 const fmatvec::Vec3& getPosition() const { return PrPF; }
00237 const fmatvec::SqrMat3& getOrientation() const { return APF; }
00238 const Frame* getFrameOfReference() const { return R; };
00239
00240 const fmatvec::Vec& getxd() const { return xd; };
00241 fmatvec::Vec& getxd() { return xd; };
00242 const fmatvec::Vec& getx0() const { return x0; };
00243 fmatvec::Vec& getx0() { return x0; };
00244
00245 const fmatvec::Mat& getT() const { return T; };
00246 const fmatvec::SymMat& getM(int i=0) const { return M[i]; };
00247 const fmatvec::SymMat& getLLM(int i=0) const { return LLM[i]; };
00248 fmatvec::SymMat& getLLM(int i=0) { return LLM[i]; };
00249 const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00250 fmatvec::Vec& geth(int j=0) { return h[j]; };
00251 const fmatvec::Vec& getf() const { return f; };
00252 fmatvec::Vec& getf() { return f; };
00253
00254 const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00255 fmatvec::Mat& getW(int i=0) { return W[i]; }
00256 const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00257 fmatvec::Mat& getV(int i=0) { return V[i]; }
00258
00259 const fmatvec::Vec& getla() const { return la; }
00260 fmatvec::Vec& getla() { return la; }
00261 const fmatvec::Vec& getg() const { return g; }
00262 fmatvec::Vec& getg() { return g; }
00263 const fmatvec::Vec& getgd() const { return gd; }
00264 fmatvec::Vec& getgd() { return gd; }
00265 const fmatvec::Vec& getrFactor() const { return rFactor; }
00266 fmatvec::Vec& getrFactor() { return rFactor; }
00267 fmatvec::Vec& getsv() { return sv; }
00268 const fmatvec::Vec& getsv() const { return sv; }
00269 fmatvec::VecInt& getjsv() { return jsv; }
00270 const fmatvec::VecInt& getjsv() const { return jsv; }
00271 fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00272 fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00273 const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00274 const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00275 const fmatvec::Vec& getres() const { return res; }
00276 fmatvec::Vec& getres() { return res; }
00277 const fmatvec::Vec& getcorr() const { return corr; };
00278 fmatvec::Vec& getcorr() { return corr; };
00279
00280 void setx(const fmatvec::Vec& x_) { x = x_; }
00281 void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00282 void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00283
00284 int getxInd() { return xInd; }
00285 int getlaInd() const { return laInd; }
00286
00287 int gethInd(int i=0) { return hInd[i]; }
00288 void setlaInd(int ind) { laInd = ind; }
00289 void setgInd(int ind) { gInd = ind; }
00290 void setgdInd(int ind) { gdInd = ind; }
00291 void setrFactorInd(int ind) { rFactorInd = ind; }
00292 virtual void setsvInd(int svInd_);
00293 void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
00294 void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00295
00296 int getzSize() const { return qSize + uSize[0] + xSize; }
00297
00298 void setqSize(int qSize_) { qSize = qSize_; }
00299 void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00300 void setxSize(int xSize_) { xSize = xSize_; }
00301
00302 int getlaSize() const { return laSize; }
00303 int getgSize() const { return gSize; }
00304 int getgdSize() const { return gdSize; }
00305 int getrFactorSize() const { return rFactorSize; }
00306 int getsvSize() const { return svSize; }
00307 int getLinkStatusSize() const { return LinkStatusSize; }
00308 int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00309
00310
00311 const std::vector<Object*>& getObjects() const { return object; }
00312 const std::vector<Link*>& getLinks() const { return link; }
00313 const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00314 const std::vector<Frame*>& getFrames() const { return frame; }
00315 const std::vector<Contour*>& getContours() const { return contour; }
00316
00321 void updateqRef(const fmatvec::Vec &ref);
00322
00327 void updateqdRef(const fmatvec::Vec &ref);
00328
00333 void updateuRef(const fmatvec::Vec &ref);
00334
00339 void updateuallRef(const fmatvec::Vec &ref);
00340
00345 void updateudRef(const fmatvec::Vec &ref, int i=0);
00346
00351 void updateudallRef(const fmatvec::Vec &ref, int i=0);
00352
00360 void updatehRef(const fmatvec::Vec &hRef, int i=0);
00361
00366 void updaterRef(const fmatvec::Vec &ref, int j=0);
00367
00372 void updateTRef(const fmatvec::Mat &ref);
00373
00379 void updateMRef(const fmatvec::SymMat &ref, int i=0);
00380
00386 void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00387
00392 void updategRef(const fmatvec::Vec &ref);
00393
00398 void updategdRef(const fmatvec::Vec &ref);
00399
00404 void updatelaRef(const fmatvec::Vec &ref);
00405
00406 void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00407 void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00408
00413 void updatewbRef(const fmatvec::Vec &ref);
00414
00420 void updateWRef(const fmatvec::Mat &ref, int i=0);
00421
00427 void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00428
00434 void updateVRef(const fmatvec::Mat &ref, int i=0);
00435
00440 void updatesvRef(const fmatvec::Vec& ref);
00441
00446 void updatejsvRef(const fmatvec::VecInt &ref);
00447
00452 void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00453
00458 void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00459
00464 void updateresRef(const fmatvec::Vec &ref);
00465
00470 void updaterFactorRef(const fmatvec::Vec &ref);
00471
00472 void clearElementLists();
00473
00478 void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00479
00484 void buildListOfObjects(std::vector<Object*> &obj);
00485
00490 void buildListOfLinks(std::vector<Link*> &lnk);
00491
00496 void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00497
00502 void buildListOfFrames(std::vector<Frame*> &frm);
00503
00508 void buildListOfContours(std::vector<Contour*> &cnt);
00509
00514 void buildListOfModels(std::vector<ModellingInterface*> &model);
00515
00520 void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00521
00526 void buildListOfObservers(std::vector<Observer*> &obsrv);
00527
00531 void setUpInverseKinetics();
00532
00536 void setUpLinks();
00537
00541 bool gActiveChanged();
00542
00546 bool gActiveChangedReg();
00547
00551 bool detectImpact();
00552
00556 void calcsvSize();
00557
00561 void calclaSize(int j);
00562
00566 void calcLinkStatusSize();
00567
00571 void calcLinkStatusRegSize();
00572
00576 void calclaInverseKineticsSize();
00577
00581 void calcbInverseKineticsSize();
00582
00586 void calcgSize(int j);
00587
00598 void calcgdSize(int j);
00599
00603 void calcrFactorSize(int j);
00604
00608 void setUpActiveLinks();
00609
00624 void checkActive(int i);
00625
00629 void checkActiveReg(int i);
00630
00634 virtual void setgTol(double tol);
00635
00639 virtual void setgdTol(double tol);
00640
00644 virtual void setgddTol(double tol);
00645
00649 virtual void setlaTol(double tol);
00650
00654 virtual void setLaTol(double tol);
00655
00659 void setrMax(double rMax);
00660
00661 void addFrame(FixedRelativeFrame *frame);
00662
00667 int frameIndex(const Frame *frame_) const;
00668
00672 void addGroup(DynamicSystem *dynamicsystem);
00673
00679 DynamicSystem* getGroup(const std::string &name,bool check=true) const;
00680
00684 void addObject(Object *object);
00685
00691 Object* getObject(const std::string &name,bool check=true) const;
00692
00696 void addLink(Link *link);
00697
00701 void addInverseKineticsLink(Link *link);
00702
00703 Observer* getObserver(const std::string &name,bool check=true) const;
00704 void addObserver(Observer *element);
00705
00711 Link* getLink(const std::string &name,bool check=true) const;
00712
00716 void addModel(ModellingInterface *modell);
00717
00723 ModellingInterface* getModel(const std::string &name, bool check=true) const;
00724
00726 FixedRelativeFrame *getFrameI() { return I; }
00727
00728 virtual Element *getChildByContainerAndName(const std::string &container, const std::string &name) const;
00729
00730 virtual void updatecorr(int j);
00731 void updatecorrRef(const fmatvec::Vec &ref);
00732 void calccorrSize(int j);
00733
00734 void checkRoot();
00735
00736 protected:
00740 Frame *R;
00741
00745 fmatvec::Vec3 PrPF;
00746
00750 fmatvec::SqrMat3 APF;
00751
00755 std::vector<Object*> object;
00756 std::vector<Link*> link;
00757 std::vector<Link*> linkSingleValued;
00758 std::vector<Link*> linkSetValued;
00759 std::vector<Link*> linkSetValuedActive;
00760 std::vector<ModellingInterface*> model;
00761 std::vector<DynamicSystem*> dynamicsystem;
00762 std::vector<Link*> inverseKineticsLink;
00763 std::vector<Observer*> observer;
00764 std::vector< std::vector<Link*> > linkOrdered;
00765
00769 fmatvec::Mat T;
00770
00774 fmatvec::SymMat M[2];
00775
00779 fmatvec::SymMat LLM[2];
00780
00784 fmatvec::Vec q, qd, q0;
00785
00789 fmatvec::Vec u, ud[2], u0;
00790
00794 fmatvec::Vec x, xd, x0;
00795
00799 fmatvec::Vec h[2], r[2], f;
00800
00804 fmatvec::Mat W[2], V[2];
00805
00809 fmatvec::Vec la;
00810
00814 fmatvec::Vec g, gd;
00815
00819 fmatvec::Vec wb;
00820
00824 fmatvec::Vec res;
00825
00829 fmatvec::Vec rFactor;
00830
00834 fmatvec::Vec sv;
00835
00839 fmatvec::VecInt jsv;
00840
00844 fmatvec::VecInt LinkStatus;
00845
00849 fmatvec::VecInt LinkStatusReg;
00850
00854 int qSize, qInd;
00855
00859 int uSize[2], uInd[2];
00860
00864 int xSize, xInd;
00865
00869 int hSize[2], hInd[2];
00870
00874 int gSize, gInd;
00875
00879 int gdSize, gdInd;
00880
00884 int laSize, laInd;
00885
00889 int rFactorSize, rFactorInd;
00890
00894 int svSize, svInd;
00895
00899 int LinkStatusSize, LinkStatusInd;
00900
00904 int LinkStatusRegSize, LinkStatusRegInd;
00905
00909 std::vector<Frame*> frame;
00910 std::vector<Contour*> contour;
00911
00912 #ifdef HAVE_OPENMBVCPPINTERFACE
00913 boost::shared_ptr<OpenMBV::Group> openMBVGrp;
00914 #endif
00915 boost::shared_ptr<H5::File> hdf5File;
00916
00920 void addContour(Contour* contour);
00921
00923 FixedRelativeFrame *I;
00924
00928 int laInverseKineticsSize, bInverseKineticsSize;
00929
00930 fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00931 fmatvec::Vec laInverseKinetics;
00932
00933 int corrSize, corrInd;
00934 fmatvec::Vec corr;
00935
00936 std::string saved_frameOfReference;
00937 };
00938 }
00939
00940 #endif
00941