19 #ifndef _DYNAMIC_SYSTEM_H_
20 #define _DYNAMIC_SYSTEM_H_
22 #include "mbsim/element.h"
26 #include "mbsim/mbsim_event.h"
38 class ModellingInterface;
40 class InverseKineticsJoint;
72 virtual void updateT(
double t);
73 virtual void updateh(
double t,
int i=0);
74 virtual void updateh0Fromh1(
double t);
75 virtual void updateW0FromW1(
double t);
76 virtual void updateV0FromV1(
double t);
77 virtual void updateStateDependentVariables(
double t) = 0;
78 virtual void updateStateDerivativeDependentVariables(
double t);
79 virtual void updateM(
double t,
int i=0);
80 virtual void updateJacobians(
double t,
int j=0) = 0;
81 virtual void updatedq(
double t,
double dt);
82 virtual void updateud(
double t,
int i=0) {
throw MBSimError(
"ERROR in "+
getName()+
" (DynamicSystem::updateud): Not implemented!"); }
83 virtual void updatezd(
double t) = 0;
84 virtual void updatedu(
double t,
double dt) = 0;
85 virtual void updateqd(
double t);
86 virtual void sethSize(
int hSize_,
int i=0);
87 virtual int gethSize(
int i=0)
const {
return hSize[i]; }
88 virtual int getqSize()
const {
return qSize; }
89 virtual int getuSize(
int i=0)
const {
return uSize[i]; }
90 virtual void calcqSize();
91 virtual void calcuSize(
int j=0);
93 virtual int getuInd(
int i=0) {
return uInd[i]; }
97 virtual void setqInd(
int qInd_);
98 virtual void setuInd(
int uInd_,
int i=0);
99 virtual void sethInd(
int hInd_,
int i=0);
100 virtual void setxInd(
int xInd_);
102 virtual const fmatvec::Vec& getq()
const {
return q; };
103 virtual fmatvec::Vec& getq() {
return q; };
104 virtual const fmatvec::Vec& getu()
const {
return u; };
105 virtual fmatvec::Vec& getu() {
return u; };
109 #ifdef HAVE_OPENMBVCPPINTERFACE
110 virtual OpenMBV::Group* getOpenMBVGrp();
120 virtual void updatewb(
double t,
int j=0);
121 virtual void updateW(
double t,
int j=0);
122 virtual void updateV(
double t,
int j=0);
123 virtual void updateg(
double t);
124 virtual void updategd(
double t);
125 virtual void updateStopVector(
double t);
126 virtual void updateLinkStatus(
double t);
127 virtual void updateLinkStatusReg(
double t);
129 virtual void updategInverseKinetics(
double t);
130 virtual void updategdInverseKinetics(
double t);
131 virtual void updateWInverseKinetics(
double t,
int j=0);
132 virtual void updatehInverseKinetics(
double t,
int j=0);
133 virtual void updateJacobiansInverseKinetics(
double t,
int j=0);
134 virtual void updatebInverseKinetics(
double t);
138 virtual void updatedx(
double t,
double dt);
139 virtual void updatexd(
double t);
140 virtual void calcxSize();
141 const fmatvec::Vec& getx()
const {
return x; };
142 fmatvec::Vec& getx() {
return x; };
143 int getxSize()
const {
return xSize; }
144 void updatexRef(
const fmatvec::Vec &ref);
145 void updatexdRef(
const fmatvec::Vec &ref);
147 virtual void initz();
152 virtual std::string
getType()
const {
return "DynamicSystem"; }
154 virtual void plot(
double t,
double dt);
163 virtual void facLLM(
int i=0) = 0;
225 virtual Frame*
getFrame(
const std::string &name,
bool check=
true);
236 void setPosition(
const fmatvec::Vec& PrPF_) {
PrPF = PrPF_; }
237 void setOrientation(
const fmatvec::SqrMat& APF_) {
APF = APF_; }
240 const fmatvec::Vec& getxd()
const {
return xd; };
241 fmatvec::Vec& getxd() {
return xd; };
242 const fmatvec::Vec& getx0()
const {
return x0; };
243 fmatvec::Vec& getx0() {
return x0; };
245 const fmatvec::Mat& getT()
const {
return T; };
246 const fmatvec::SymMat& getM(
int i=0)
const {
return M[i]; };
247 const fmatvec::SymMat& getLLM(
int i=0)
const {
return LLM[i]; };
248 const fmatvec::Vec& geth(
int j=0)
const {
return h[j]; };
249 const fmatvec::Vec& getf()
const {
return f; };
250 fmatvec::Vec& getf() {
return f; };
252 const fmatvec::Mat& getW(
int i=0)
const {
return W[i]; }
253 fmatvec::Mat& getW(
int i=0) {
return W[i]; }
254 const fmatvec::Mat& getV(
int i=0)
const {
return V[i]; }
255 fmatvec::Mat& getV(
int i=0) {
return V[i]; }
257 const fmatvec::Vec& getla()
const {
return la; }
258 fmatvec::Vec& getla() {
return la; }
259 const fmatvec::Vec& getg()
const {
return g; }
260 fmatvec::Vec& getg() {
return g; }
261 const fmatvec::Vec& getgd()
const {
return gd; }
262 fmatvec::Vec& getgd() {
return gd; }
263 const fmatvec::Vec& getrFactor()
const {
return rFactor; }
264 fmatvec::Vec& getrFactor() {
return rFactor; }
265 fmatvec::Vec& getsv() {
return sv; }
266 const fmatvec::Vec& getsv()
const {
return sv; }
267 fmatvec::Vector<int>& getjsv() {
return jsv; }
268 const fmatvec::Vector<int>& getjsv()
const {
return jsv; }
269 fmatvec::Vector<int>& getLinkStatus() {
return LinkStatus; }
270 fmatvec::Vector<int>& getLinkStatusReg() {
return LinkStatusReg; }
271 const fmatvec::Vector<int>& getLinkStatus()
const {
return LinkStatus; }
272 const fmatvec::Vector<int>& getLinkStatusReg()
const {
return LinkStatusReg; }
273 const fmatvec::Vec& getres()
const {
return res; }
274 fmatvec::Vec& getres() {
return res; }
276 void setx(
const fmatvec::Vec& x_) {
x = x_; }
277 void setx0(
const fmatvec::Vec &x0_) { x0 = x0_; }
278 void setx0(
double x0_) { x0 = fmatvec::Vec(1,fmatvec::INIT,x0_); }
280 int getxInd() {
return xInd; }
281 int getlaInd()
const {
return laInd; }
283 int gethInd(
int i=0) {
return hInd[i]; }
284 void setlaInd(
int ind) { laInd = ind; }
285 void setgInd(
int ind) { gInd = ind; }
286 void setgdInd(
int ind) { gdInd = ind; }
287 void setrFactorInd(
int ind) { rFactorInd = ind; }
288 void setsvInd(
int svInd_) { svInd = svInd_; };
289 void setLinkStatusInd(
int LinkStatusInd_) {LinkStatusInd = LinkStatusInd_;};
290 void setLinkStatusRegInd(
int LinkStatusRegInd_) {LinkStatusRegInd = LinkStatusRegInd_;};
294 void setqSize(
int qSize_) {
qSize = qSize_; }
295 void setuSize(
int uSize_,
int i=0) {
uSize[i] = uSize_; }
296 void setxSize(
int xSize_) {
xSize = xSize_; }
298 int getlaSize()
const {
return laSize; }
299 int getgSize()
const {
return gSize; }
300 int getgdSize()
const {
return gdSize; }
302 int getsvSize()
const {
return svSize; }
335 void updateudRef(
const fmatvec::Vec &ref,
int i=0);
350 void updatehRef(
const fmatvec::Vec &hRef,
int i=0);
362 void updaterRef(
const fmatvec::Vec &ref,
int j=0);
375 void updateMRef(
const fmatvec::SymMat &ref,
int i=0);
402 void updatelaInverseKineticsRef(
const fmatvec::Vec &ref);
403 void updatebInverseKineticsRef(
const fmatvec::Mat &ref);
416 void updateWRef(
const fmatvec::Mat &ref,
int i=0);
418 void updateWnVRefObjects();
432 void updateVRef(
const fmatvec::Mat &ref,
int i=0);
482 virtual void buildListOfLinks(std::vector<Link*> &lnk,
bool recursive=
false);
517 void buildListOfModels(std::vector<ModellingInterface*> &model,
bool recursive =
true);
635 void addFrame(Frame *frame_,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const std::string& refFrameName);
643 void addFrame(Frame *frame_,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const Frame* refFrame=0);
651 void addFrame(
const std::string &str,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const Frame* refFrame=0);
659 void addContour(Contour* contour,
const fmatvec::Vec &RrRC,
const fmatvec::SqrMat &ARC,
const std::string& refFrameName);
667 void addContour(Contour* contour,
const fmatvec::Vec &RrRC,
const fmatvec::SqrMat &ARC,
const Frame* refFrame=0);
716 #ifdef HAVE_OPENMBVCPPINTERFACE
717 Element* getOpenMBVElement(
const std::string &name,
bool check=
true);
718 void addPlotElement(
Element *element);
726 Link*
getLink(
const std::string &name,
bool check=
true);
757 virtual void updatecorr(
int j);
758 void updatecorrRef(
const fmatvec::Vec &ref);
759 void calccorrSize(
int j);
783 std::vector<Link*> link;
784 std::vector<Link*> linkSingleValued;
785 std::vector<Link*> linkSetValued;
786 std::vector<Link*> linkSetValuedActive;
787 std::vector<Link*> linkSetValuedNotActiveWithSmoothPart;
788 std::vector<ExtraDynamic*> extraDynamic;
789 std::vector<ModellingInterface*> model;
790 std::vector<DynamicSystem*> dynamicsystem;
791 std::vector<Link*> inverseKineticsLink;
792 #ifdef HAVE_OPENMBVCPPINTERFACE
793 std::vector<Element*> plotElement;
804 fmatvec::SymMat
M[2];
814 fmatvec::Vec
q, qd, q0;
819 fmatvec::Vec
u, ud[2], u0;
824 fmatvec::Vec
x, xd, x0;
829 fmatvec::Vec
h[2], r[2], f;
834 fmatvec::Mat W[2], V[2];
939 std::vector<fmatvec::Vec>
IrOF, IrOC;
944 std::vector<fmatvec::SqrMat>
AIF, AIC;
950 std::vector<Contour*> contour;
952 #ifdef HAVE_OPENMBVCPPINTERFACE
953 OpenMBV::Group* openMBVGrp;
974 fmatvec::Mat WInverseKinetics[2], bInverseKinetics;
975 fmatvec::Vec laInverseKinetics;
977 int corrSize, corrInd;
981 std::vector<std::string> saved_refFrameF, saved_refFrameC;
982 std::vector<fmatvec::Vec> saved_RrRF, saved_RrRC;
983 std::vector<fmatvec::SqrMat> saved_ARF, saved_ARC;