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("ERROR in "+getName()+" (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::Group *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
00114 std::string getPath(char pathDelim='.') { return parent?parent->getPath()+pathDelim+name:name; }
00115
00116
00117
00118 virtual void updatewb(double t, int j=0);
00119 virtual void updateW(double t, int j=0);
00120 virtual void updateV(double t, int j=0);
00121 virtual void updateg(double t);
00122 virtual void updategd(double t);
00123 virtual void updateStopVector(double t);
00124 virtual void updateLinkStatus(double t);
00125 virtual void updateLinkStatusReg(double t);
00126
00127 virtual void updategInverseKinetics(double t);
00128 virtual void updategdInverseKinetics(double t);
00129 virtual void updateWInverseKinetics(double t, int j=0);
00130 virtual void updatehInverseKinetics(double t, int j=0);
00131 virtual void updateJacobiansInverseKinetics(double t, int j=0);
00132 virtual void updatebInverseKinetics(double t);
00133
00134
00135
00136 virtual void updatedx(double t, double dt);
00137 virtual void updatexd(double t);
00138 virtual void calcxSize();
00139 const fmatvec::Vec& getx() const { return x; };
00140 fmatvec::Vec& getx() { return x; };
00141 int getxSize() const { return xSize; }
00142 void updatexRef(const fmatvec::Vec &ref);
00143 void updatexdRef(const fmatvec::Vec &ref);
00144 virtual void init(InitStage stage);
00145 virtual void initz();
00146 virtual void writez(const H5::CommonFG & group);
00147 virtual void readz0(const H5::CommonFG & parent);
00148
00149
00150
00152 virtual std::string getType() const { return "DynamicSystem"; }
00153 virtual void setDynamicSystemSolver(DynamicSystemSolver* sys);
00154 virtual void plot(double t, double dt);
00155 virtual void plotAtSpecialEvent(double t, double dt=1.);
00156 virtual void closePlot();
00157
00158
00159
00163 virtual void facLLM(int i=0) = 0;
00164
00168 virtual int solveConstraintsFixpointSingle();
00169
00173 virtual int solveImpactsFixpointSingle(double dt);
00174
00178 virtual int solveConstraintsGaussSeidel();
00179
00183 virtual int solveImpactsGaussSeidel(double dt);
00184
00188 virtual int solveConstraintsRootFinding();
00189
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);
00226
00232 virtual Contour* getContour(const std::string &name, bool check=true);
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 const fmatvec::Vec& geth(int j=0) const { return h[j]; };
00252 const fmatvec::Vec& getf() const { return f; };
00253 fmatvec::Vec& getf() { return f; };
00254
00255 const fmatvec::Mat& getW(int i=0) const { return W[i]; }
00256 fmatvec::Mat& getW(int i=0) { return W[i]; }
00257 const fmatvec::Mat& getV(int i=0) const { return V[i]; }
00258 fmatvec::Mat& getV(int i=0) { return V[i]; }
00259
00260 const fmatvec::Vec& getla() const { return la; }
00261 fmatvec::Vec& getla() { return la; }
00262 const fmatvec::Vec& getg() const { return g; }
00263 fmatvec::Vec& getg() { return g; }
00264 const fmatvec::Vec& getgd() const { return gd; }
00265 fmatvec::Vec& getgd() { return gd; }
00266 const fmatvec::Vec& getrFactor() const { return rFactor; }
00267 fmatvec::Vec& getrFactor() { return rFactor; }
00268 fmatvec::Vec& getsv() { return sv; }
00269 const fmatvec::Vec& getsv() const { return sv; }
00270 fmatvec::VecInt& getjsv() { return jsv; }
00271 const fmatvec::VecInt& getjsv() const { return jsv; }
00272 fmatvec::VecInt& getLinkStatus() { return LinkStatus; }
00273 fmatvec::VecInt& getLinkStatusReg() { return LinkStatusReg; }
00274 const fmatvec::VecInt& getLinkStatus() const { return LinkStatus; }
00275 const fmatvec::VecInt& getLinkStatusReg() const { return LinkStatusReg; }
00276 const fmatvec::Vec& getres() const { return res; }
00277 fmatvec::Vec& getres() { return res; }
00278 const fmatvec::Vec& getcorr() const { return corr; };
00279 fmatvec::Vec& getcorr() { return corr; };
00280
00281 void setx(const fmatvec::Vec& x_) { x = x_; }
00282 void setx0(const fmatvec::Vec &x0_) { x0 = x0_; }
00283 void setx0(double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
00284
00285 int getxInd() { return xInd; }
00286 int getlaInd() const { return laInd; }
00287
00288 int gethInd(int i=0) { return hInd[i]; }
00289 void setlaInd(int ind) { laInd = ind; }
00290 void setgInd(int ind) { gInd = ind; }
00291 void setgdInd(int ind) { gdInd = ind; }
00292 void setrFactorInd(int ind) { rFactorInd = ind; }
00293 void setsvInd(int svInd_) { svInd = svInd_; };
00294 void setLinkStatusInd(int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
00295 void setLinkStatusRegInd(int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
00296
00297 int getzSize() const { return qSize + uSize[0] + xSize; }
00298
00299 void setqSize(int qSize_) { qSize = qSize_; }
00300 void setuSize(int uSize_, int i=0) { uSize[i] = uSize_; }
00301 void setxSize(int xSize_) { xSize = xSize_; }
00302
00303 int getlaSize() const { return laSize; }
00304 int getgSize() const { return gSize; }
00305 int getgdSize() const { return gdSize; }
00306 int getrFactorSize() const { return rFactorSize; }
00307 int getsvSize() const { return svSize; }
00308 int getLinkStatusSize() const { return LinkStatusSize; }
00309 int getLinkStatusRegSize() const { return LinkStatusRegSize; }
00310
00311
00312 const std::vector<Object*>& getObjects() const { return object; }
00313 const std::vector<Link*>& getLinks() const { return link; }
00314 const std::vector<DynamicSystem*>& getDynamicSystems() const { return dynamicsystem; }
00315 const std::vector<Frame*>& getFrames() const { return frame; }
00316 const std::vector<Contour*>& getContours() const { return contour; }
00317
00322 void updateqRef(const fmatvec::Vec &ref);
00323
00328 void updateqdRef(const fmatvec::Vec &ref);
00329
00334 void updateuRef(const fmatvec::Vec &ref);
00335
00340 void updateuallRef(const fmatvec::Vec &ref);
00341
00346 void updateudRef(const fmatvec::Vec &ref, int i=0);
00347
00352 void updateudallRef(const fmatvec::Vec &ref, int i=0);
00353
00361 void updatehRef(const fmatvec::Vec &hRef, int i=0);
00362
00367 void updaterRef(const fmatvec::Vec &ref, int j=0);
00368
00373 void updateTRef(const fmatvec::Mat &ref);
00374
00380 void updateMRef(const fmatvec::SymMat &ref, int i=0);
00381
00387 void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00388
00393 void updategRef(const fmatvec::Vec &ref);
00394
00399 void updategdRef(const fmatvec::Vec &ref);
00400
00405 void updatelaRef(const fmatvec::Vec &ref);
00406
00407 void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00408 void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00409
00414 void updatewbRef(const fmatvec::Vec &ref);
00415
00421 void updateWRef(const fmatvec::Mat &ref, int i=0);
00422
00423 void updateWnVRefObjects();
00424
00430 void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00431
00437 void updateVRef(const fmatvec::Mat &ref, int i=0);
00438
00443 void updatesvRef(const fmatvec::Vec& ref);
00444
00449 void updatejsvRef(const fmatvec::VecInt &ref);
00450
00455 void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00456
00461 void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00462
00467 void updateresRef(const fmatvec::Vec &ref);
00468
00473 void updaterFactorRef(const fmatvec::Vec &ref);
00474
00475 void clearElementLists();
00476
00481 void buildListOfDynamicSystems(std::vector<DynamicSystem*> &sys);
00482
00487 void buildListOfObjects(std::vector<Object*> &obj);
00488
00493 void buildListOfLinks(std::vector<Link*> &lnk);
00494
00499 void buildListOfSetValuedLinks(std::vector<Link*> &lnk);
00500
00505 void buildListOfFrames(std::vector<Frame*> &frm);
00506
00511 void buildListOfContours(std::vector<Contour*> &cnt);
00512
00517 void buildListOfModels(std::vector<ModellingInterface*> &model);
00518
00523 void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk);
00524
00529 void buildListOfObservers(std::vector<Observer*> &obsrv);
00530
00534 void setUpInverseKinetics();
00535
00539 void setUpLinks();
00540
00544 bool gActiveChanged();
00545
00549 bool gActiveChangedReg();
00550
00554 void calcsvSize();
00555
00559 void calclaSize(int j);
00560
00564 void calcLinkStatusSize();
00565
00569 void calcLinkStatusRegSize();
00570
00574 void calclaInverseKineticsSize();
00575
00579 void calcbInverseKineticsSize();
00580
00584 void calcgSize(int j);
00585
00589 void calcgdSize(int j);
00590
00594 void calcrFactorSize(int j);
00595
00599 void setUpActiveLinks();
00600
00604 void checkActive(int i);
00605
00609 void checkActiveReg(int i);
00610
00614 virtual void setgTol(double tol);
00615
00619 virtual void setgdTol(double tol);
00620
00624 virtual void setgddTol(double tol);
00625
00629 virtual void setlaTol(double tol);
00630
00634 virtual void setLaTol(double tol);
00635
00639 void setrMax(double rMax);
00640
00641 void addFrame(FixedRelativeFrame *frame);
00642
00647 int frameIndex(const Frame *frame_) const;
00648
00652 void addGroup(DynamicSystem *dynamicsystem);
00653
00659 DynamicSystem* getGroup(const std::string &name,bool check=true);
00660
00664 void addObject(Object *object);
00665
00671 Object* getObject(const std::string &name,bool check=true);
00672
00676 void addLink(Link *link);
00677
00681 void addInverseKineticsLink(Link *link);
00682
00683 Observer* getObserver(const std::string &name,bool check=true);
00684 void addObserver(Observer *element);
00685
00691 Link* getLink(const std::string &name,bool check=true);
00692
00696 void addModel(ModellingInterface *modell);
00697
00703 ModellingInterface* getModel(const std::string &name, bool check=true);
00704
00706 FixedRelativeFrame *getFrameI() { return I; }
00707
00708 virtual Element *getByPathSearch(std::string path);
00709
00710 virtual void updatecorr(int j);
00711 void updatecorrRef(const fmatvec::Vec &ref);
00712 void calccorrSize(int j);
00713
00714 void checkRoot();
00715
00716 protected:
00720 Frame *R;
00721
00725 fmatvec::Vec3 PrPF;
00726
00730 fmatvec::SqrMat3 APF;
00731
00735 std::vector<Object*> object;
00736 std::vector<Link*> link;
00737 std::vector<Link*> linkSingleValued;
00738 std::vector<Link*> linkSetValued;
00739 std::vector<Link*> linkSetValuedActive;
00740 std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00741 std::vector<ModellingInterface*> model;
00742 std::vector<DynamicSystem*> dynamicsystem;
00743 std::vector<Link*> inverseKineticsLink;
00744 std::vector<Observer*> observer;
00745
00749 fmatvec::Mat T;
00750
00754 fmatvec::SymMat M[2];
00755
00759 fmatvec::SymMat LLM[2];
00760
00764 fmatvec::Vec q, qd, q0;
00765
00769 fmatvec::Vec u, ud[2], u0;
00770
00774 fmatvec::Vec x, xd, x0;
00775
00779 fmatvec::Vec h[2], r[2], f;
00780
00784 fmatvec::Mat W[2], V[2];
00785
00789 fmatvec::Vec la;
00790
00794 fmatvec::Vec g, gd;
00795
00799 fmatvec::Vec wb;
00800
00804 fmatvec::Vec res;
00805
00809 fmatvec::Vec rFactor;
00810
00814 fmatvec::Vec sv;
00815
00819 fmatvec::VecInt jsv;
00820
00824 fmatvec::VecInt LinkStatus;
00825
00829 fmatvec::VecInt LinkStatusReg;
00830
00834 int qSize, qInd;
00835
00839 int uSize[2], uInd[2];
00840
00844 int xSize, xInd;
00845
00849 int hSize[2], hInd[2];
00850
00854 int gSize, gInd;
00855
00859 int gdSize, gdInd;
00860
00864 int laSize, laInd;
00865
00869 int rFactorSize, rFactorInd;
00870
00874 int svSize, svInd;
00875
00879 int LinkStatusSize, LinkStatusInd;
00880
00884 int LinkStatusRegSize, LinkStatusRegInd;
00885
00889 std::vector<Frame*> frame;
00890 std::vector<Contour*> contour;
00891
00892 #ifdef HAVE_OPENMBVCPPINTERFACE
00893 OpenMBV::Group* openMBVGrp;
00894 #endif
00895
00899 void addContour(Contour* contour);
00900
00902 FixedRelativeFrame *I;
00903
00907 int laInverseKineticsSize, bInverseKineticsSize;
00908
00909 fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00910 fmatvec::Vec laInverseKinetics;
00911
00912 int corrSize, corrInd;
00913 fmatvec::Vec corr;
00914
00915 std::string saved_frameOfReference;
00916 };
00917 }
00918
00919 #endif
00920