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 updatefRef(const fmatvec::Vec &ref);
00368
00373 void updaterRef(const fmatvec::Vec &ref, int j=0);
00374
00379 void updateTRef(const fmatvec::Mat &ref);
00380
00386 void updateMRef(const fmatvec::SymMat &ref, int i=0);
00387
00393 void updateLLMRef(const fmatvec::SymMat &ref, int i=0);
00394
00399 void updategRef(const fmatvec::Vec &ref);
00400
00405 void updategdRef(const fmatvec::Vec &ref);
00406
00411 void updatelaRef(const fmatvec::Vec &ref);
00412
00413 void updatelaInverseKineticsRef(const fmatvec::Vec &ref);
00414 void updatebInverseKineticsRef(const fmatvec::Mat &ref);
00415
00420 void updatewbRef(const fmatvec::Vec &ref);
00421
00427 void updateWRef(const fmatvec::Mat &ref, int i=0);
00428
00429 void updateWnVRefObjects();
00430
00436 void updateWInverseKineticsRef(const fmatvec::Mat &ref, int i=0);
00437
00443 void updateVRef(const fmatvec::Mat &ref, int i=0);
00444
00449 void updatesvRef(const fmatvec::Vec& ref);
00450
00455 void updatejsvRef(const fmatvec::VecInt &ref);
00456
00461 void updateLinkStatusRef(const fmatvec::VecInt &LinkStatusParent);
00462
00467 void updateLinkStatusRegRef(const fmatvec::VecInt &LinkStatusRegParent);
00468
00473 void updateresRef(const fmatvec::Vec &ref);
00474
00479 void updaterFactorRef(const fmatvec::Vec &ref);
00480
00486 virtual void buildListOfObjects(std::vector<Object*> &obj, bool recursive=false);
00487
00493 virtual void buildListOfLinks(std::vector<Link*> &lnk, bool recursive=false);
00494
00500 virtual void buildListOfSetValuedLinks(std::vector<Link*> &lnk, bool recursive=false);
00501
00507 virtual void buildListOfFrames(std::vector<Frame*> &frm, bool recursive);
00508
00514 virtual void buildListOfContours(std::vector<Contour*> &cnt, bool recursive);
00515
00521 void buildListOfModels(std::vector<ModellingInterface*> &model, bool recursive = true);
00522
00528 virtual void buildListOfInverseKineticsLinks(std::vector<Link*> &lnk, bool recursive=false);
00529
00535 virtual void buildListOfObservers(std::vector<Observer*> &obsrv, bool recursive=false);
00536
00540 void setUpInverseKinetics();
00541
00545 void setUpLinks();
00546
00550 bool gActiveChanged();
00551
00555 bool gActiveChangedReg();
00556
00560 void calcsvSize();
00561
00565 void calclaSize(int j);
00566
00570 void calcLinkStatusSize();
00571
00575 void calcLinkStatusRegSize();
00576
00580 void calclaInverseKineticsSize();
00581
00585 void calcbInverseKineticsSize();
00586
00590 void calcgSize(int j);
00591
00595 void calcgdSize(int j);
00596
00600 void calcrFactorSize(int j);
00601
00605 void setUpActiveLinks();
00606
00610 void checkActive(int i);
00611
00615 void checkActiveReg(int i);
00616
00620 virtual void setgTol(double tol);
00621
00625 virtual void setgdTol(double tol);
00626
00630 virtual void setgddTol(double tol);
00631
00635 virtual void setlaTol(double tol);
00636
00640 virtual void setLaTol(double tol);
00641
00645 void setrMax(double rMax);
00646
00647 void addFrame(FixedRelativeFrame *frame);
00648
00655 void addFrame(Frame *frame, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00656
00663 void addFrame(const std::string &str, const fmatvec::Vec3 &RrRF, const fmatvec::SqrMat3 &ARF, const Frame* refFrame=0);
00664
00671 void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const fmatvec::SqrMat3 &ARC, const Frame* refFrame=0);
00672
00678 void addContour(Contour* contour, const fmatvec::Vec3 &RrRC, const Frame* refFrame=0) { addContour(contour,RrRC,fmatvec::SqrMat3(fmatvec::EYE),refFrame); }
00679
00684 int frameIndex(const Frame *frame_) const;
00685
00689 void addGroup(DynamicSystem *dynamicsystem);
00690
00696 DynamicSystem* getGroup(const std::string &name,bool check=true);
00697
00701 void addObject(Object *object);
00702
00708 Object* getObject(const std::string &name,bool check=true);
00709
00713 void addLink(Link *link);
00714
00718 void addInverseKineticsLink(Link *link);
00719
00720 Observer* getObserver(const std::string &name,bool check=true);
00721 void addObserver(Observer *element);
00722
00728 Link* getLink(const std::string &name,bool check=true);
00729
00733 void addModel(ModellingInterface *modell);
00734
00740 ModellingInterface* getModel(const std::string &name, bool check=true);
00741
00743 FixedRelativeFrame *getFrameI() { return I; }
00744
00745 virtual Element *getByPathSearch(std::string path);
00746
00747 virtual void updatecorr(int j);
00748 void updatecorrRef(const fmatvec::Vec &ref);
00749 void calccorrSize(int j);
00750
00751 void checkRoot();
00752
00753 protected:
00757 Frame *R;
00758
00762 fmatvec::Vec3 PrPF;
00763
00767 fmatvec::SqrMat3 APF;
00768
00772 std::vector<Object*> object;
00773 std::vector<Link*> link;
00774 std::vector<Link*> linkSingleValued;
00775 std::vector<Link*> linkSetValued;
00776 std::vector<Link*> linkSetValuedActive;
00777 std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
00778 std::vector<ModellingInterface*> model;
00779 std::vector<DynamicSystem*> dynamicsystem;
00780 std::vector<Link*> inverseKineticsLink;
00781 std::vector<Observer*> observer;
00782
00786 fmatvec::Mat T;
00787
00791 fmatvec::SymMat M[2];
00792
00796 fmatvec::SymMat LLM[2];
00797
00801 fmatvec::Vec q, qd, q0;
00802
00806 fmatvec::Vec u, ud[2], u0;
00807
00811 fmatvec::Vec x, xd, x0;
00812
00816 fmatvec::Vec h[2], r[2], f;
00817
00821 fmatvec::Mat W[2], V[2];
00822
00826 fmatvec::Vec la;
00827
00831 fmatvec::Vec g, gd;
00832
00836 fmatvec::Vec wb;
00837
00841 fmatvec::Vec res;
00842
00846 fmatvec::Vec rFactor;
00847
00851 fmatvec::Vec sv;
00852
00856 fmatvec::VecInt jsv;
00857
00861 fmatvec::VecInt LinkStatus;
00862
00866 fmatvec::VecInt LinkStatusReg;
00867
00871 int qSize, qInd;
00872
00876 int uSize[2], uInd[2];
00877
00881 int xSize, xInd;
00882
00886 int hSize[2], hInd[2];
00887
00891 int gSize, gInd;
00892
00896 int gdSize, gdInd;
00897
00901 int laSize, laInd;
00902
00906 int rFactorSize, rFactorInd;
00907
00911 int svSize, svInd;
00912
00916 int LinkStatusSize, LinkStatusInd;
00917
00921 int LinkStatusRegSize, LinkStatusRegInd;
00922
00926 std::vector<Frame*> frame;
00927 std::vector<Contour*> contour;
00928
00929 #ifdef HAVE_OPENMBVCPPINTERFACE
00930 OpenMBV::Group* openMBVGrp;
00931 #endif
00932
00936 void addContour(Contour* contour);
00937
00939 FixedRelativeFrame *I;
00940
00944 int laInverseKineticsSize, bInverseKineticsSize;
00945
00946 fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
00947 fmatvec::Vec laInverseKinetics;
00948
00949 int corrSize, corrInd;
00950 fmatvec::Vec corr;
00951 };
00952 }
00953
00954 #endif
00955