20 #ifndef _RIGID_BODY_H_
21 #define _RIGID_BODY_H_
23 #include "mbsim/body.h"
25 #include "mbsim/kinematics.h"
26 #include "mbsim/utils/function.h"
28 #ifdef HAVE_OPENMBVCPPINTERFACE
68 constraint = constraint_;
71 virtual void updateT(
double t) {
if(
fT) TRel = (*fT)(qRel,t); }
72 virtual void updateh(
double t,
int j=0);
73 virtual void updateh0Fromh1(
double t);
74 virtual void updateW0FromW1(
double t);
75 virtual void updateV0FromV1(
double t);
76 virtual void updatehInverseKinetics(
double t,
int j=0);
77 virtual void updateStateDerivativeDependentVariables(
double t);
78 virtual void updateM(
double t,
int i=0) { (this->*
updateM_)(t,i); }
79 virtual void updateStateDependentVariables(
double t) {
83 virtual void updateJacobians(
double t,
int j=0) { (this->*updateJacobians_[j])(t); }
84 void updateJacobians0(
double t) {
88 void updateJacobians1(
double t) {
89 updateJacobiansForRemainingFramesAndContours1(t);
91 virtual void calcqSize();
92 virtual void calcuSize(
int j=0);
95 virtual void updateqRef(
const fmatvec::Vec& ref);
96 virtual void updateuRef(
const fmatvec::Vec& ref);
97 virtual void updateTRef(
const fmatvec::Mat &ref);
105 virtual std::string
getType()
const {
return "RigidBody"; }
106 virtual void plot(
double t,
double dt=1);
128 virtual void updateJacobiansForRemainingFramesAndContours1(
double t);
151 void setJacobianOfTranslation(
Jacobian* fPJT_) {
fPJT = fPJT_; }
152 void setJacobianOfRotation(Jacobian* fPJR_) {
fPJR = fPJR_; }
153 void setDerivativeOfJacobianOfTranslation(Function3<fmatvec::Mat, fmatvec::Vec, fmatvec::Vec, double>* fPdJT_) {
fPdJT = fPdJT_;}
154 void setDerivativeOfJacobianOfRotation(Function3<fmatvec::Mat, fmatvec::Vec, fmatvec::Vec, double>* fPdJR_) {
fPdJR = fPdJR_;}
167 void setMass(
double m_) {
m = m_; }
168 double getMass()
const {
return m; }
171 void isFrameOfBodyForRotation(
bool cb_) {
cb = cb_; }
172 std::vector<fmatvec::SqrMat> getContainerForFrameOrientations()
const {
return ASF; }
173 std::vector<fmatvec::Vec> getContainerForFramePositions()
const {
return SrSF; }
174 std::vector<fmatvec::SqrMat> getContainerForContourOrientations()
const {
return ASC; }
175 std::vector<fmatvec::Vec> getContainerForContourPositions()
const {
return SrSC; }
190 const fmatvec::SymMat& getInertiaTensor()
const {
return SThetaS;}
191 fmatvec::SymMat& getInertiaTensor() {
return SThetaS;}
199 void addFrame(Frame *frame_,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const std::string& refFrameName);
207 void addFrame(Frame *frame_,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const Frame* refFrame=0);
215 void addFrame(
const std::string &str,
const fmatvec::Vec &RrRF,
const fmatvec::SqrMat &ARF,
const Frame* refFrame=0);
223 void addContour(Contour* contour,
const fmatvec::Vec &RrRC,
const fmatvec::SqrMat &ARC,
const std::string& refFrameName);
231 void addContour(Contour* contour,
const fmatvec::Vec &RrRC,
const fmatvec::SqrMat &ARC,
const Frame* refFrame=0);
241 #ifdef HAVE_OPENMBVCPPINTERFACE
242 void setOpenMBVRigidBody(OpenMBV::RigidBody* body);
246 virtual void initializeUsingXML(TiXmlElement *element);
247 virtual void updatePositionAndOrientationOfFrame(
double t,
unsigned int i);
248 virtual void updateAccelerations(
double t,
unsigned int i);
249 virtual void updateRelativeJacobians(
double t,
unsigned int i);
250 virtual void updateRelativeJacobians(
double t,
unsigned int i, fmatvec::Mat &WJTrel, fmatvec::Mat &WJRrel);
251 const fmatvec::Mat& getWJTrel()
const {
return WJTrel;}
252 const fmatvec::Mat& getWJRrel()
const {
return WJRrel;}
253 fmatvec::Mat& getWJTrel() {
return WJTrel;}
254 fmatvec::Mat& getWJRrel() {
return WJRrel;}
255 fmatvec::Mat& getJRel(
int i=0) {
return JRel[i];}
256 fmatvec::Vec& getjRel() {
return jRel;}
257 fmatvec::Vec& getqRel() {
return qRel;}
258 fmatvec::Vec& getuRel() {
return uRel;}
261 fmatvec::Mat& getPJT(
int i=0) {
return PJT[i];}
262 fmatvec::Mat& getPJR(
int i=0) {
return PJR[i];}
293 fmatvec::Mat
PJT[2], PJR[2], PdJT, PdJR;
298 fmatvec::Vec
PjT, PjR, PdjT, PdjR;
323 std::vector<fmatvec::SqrMat>
ASF;
328 std::vector<fmatvec::Vec>
SrSF, WrSF;
334 std::vector<fmatvec::SqrMat>
ASC;
339 std::vector<fmatvec::Vec>
SrSC, WrSC;
426 void (
RigidBody::*updateJacobians_[2])(
double t);
433 fmatvec::Vec qRel, uRel;
434 fmatvec::Mat JRel[2];
437 fmatvec::Mat WJTrel,WJRrel;
438 fmatvec::Vec WjTrel,WjRrel;
449 std::vector<std::string> saved_refFrameF, saved_refFrameC;
450 std::vector<fmatvec::Vec> saved_RrRF, saved_RrRC;
451 std::vector<fmatvec::SqrMat> saved_ARF, saved_ARC;
452 #ifdef HAVE_OPENMBVCPPINTERFACE
456 Frame * openMBVFrame;