00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _FRAME_H__
00021 #define _FRAME_H__
00022
00023 #include "mbsim/element.h"
00024
00025 #ifdef HAVE_OPENMBVCPPINTERFACE
00026 #include "mbsim/utils/boost_parameters.h"
00027 #include "mbsim/utils/openmbv_utils.h"
00028 #endif
00029
00030 namespace MBSim {
00031
00039 class Frame : public Element {
00040 public:
00044 enum Feature {
00045 position, localPosition, firstTangent, normal, secondTangent, cosy, position_cosy, velocity, angularVelocity, velocity_cosy, velocities, velocities_cosy, angle, dotAngle, all
00046 };
00047
00052 Frame(const std::string &name = "dummy");
00053
00057 virtual ~Frame() {}
00058
00059
00060 std::string getType() const { return "Frame"; }
00061 virtual void plot(double t, double dt = 1);
00062 virtual void closePlot();
00063
00064
00065
00066 virtual int gethSize(int i=0) const { return hSize[i]; }
00067 virtual int gethInd(int i=0) const { return hInd[i]; }
00068
00069
00070 virtual const fmatvec::Vec3& getPosition() const { return WrOP; }
00071 virtual const fmatvec::Vec3& getLocalPosition() const { return LrOP; }
00072 virtual const fmatvec::SqrMat3& getOrientation() const { return AWP; }
00073 virtual fmatvec::Vec3& getPosition() { return WrOP; }
00074 virtual fmatvec::SqrMat3& getOrientation() { return AWP; }
00075 virtual void setPosition(const fmatvec::Vec3 &v) { WrOP = v; }
00076 virtual void setLocalPosition(const fmatvec::Vec3 &v) { LrOP = v; }
00077 virtual void setOrientation(const fmatvec::SqrMat3 &AWP_) { AWP = AWP_; }
00078 virtual const fmatvec::Vec3& getVelocity() const { return WvP; }
00079 virtual const fmatvec::Vec3& getAngularVelocity() const { return WomegaP; }
00080 virtual const fmatvec::Mat3xV& getJacobianOfTranslation(int j=0) const { return WJP[j]; }
00081 virtual const fmatvec::Mat3xV& getJacobianOfRotation(int j=0) const { return WJR[j]; }
00082 virtual const fmatvec::Vec3& getGyroscopicAccelerationOfTranslation(int j=0) const { return WjP[j]; }
00083 virtual const fmatvec::Vec3& getGyroscopicAccelerationOfRotation(int j=0) const { return WjR[j]; }
00084 virtual const fmatvec::Vec3& getAcceleration() const { return WaP; }
00085 virtual const fmatvec::Vec3& getAngularAcceleration() const { return WpsiP; }
00086 virtual void init(InitStage stage);
00087 #ifdef HAVE_OPENMBVCPPINTERFACE
00088 BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBV, tag, (optional (size,(double),1)(offset,(double),1)(transparency,(double),0))) {
00089 OpenMBVFrame ombv(size,offset,"[-1;1;1]",transparency);
00090 openMBVFrame=ombv.createOpenMBV();
00091 }
00092 void setOpenMBVFrame(const boost::shared_ptr<OpenMBV::Frame> &frame) { openMBVFrame = frame; }
00093 boost::shared_ptr<OpenMBV::Frame> &getOpenMBVFrame() {return openMBVFrame; }
00094 #endif
00095
00096
00097
00098 void sethSize(int size, int i=0) { hSize[i] = size; }
00099 void sethInd(int ind, int i=0) { hInd[i] = ind; }
00100
00101 fmatvec::Vec3& getVelocity() { return WvP; }
00102 fmatvec::Vec3& getAngularVelocity() { return WomegaP; }
00103 void setVelocity(const fmatvec::Vec3 &v) { WvP = v; }
00104 void setAngularVelocity(const fmatvec::Vec3 &omega) { WomegaP = omega; }
00105
00106 void setJacobianOfTranslation(const fmatvec::Mat3xV &WJP_, int j=0) { WJP[j]=WJP_; }
00107 void setGyroscopicAccelerationOfTranslation(const fmatvec::Vec3 &WjP_, int j=0) { WjP[j]=WjP_; }
00108 void setJacobianOfRotation(const fmatvec::Mat3xV &WJR_, int j=0) { WJR[j]=WJR_; }
00109 void setGyroscopicAccelerationOfRotation(const fmatvec::Vec3 &WjR_, int j=0) { WjR[j]=WjR_; }
00110 fmatvec::Mat3xV& getJacobianOfTranslation(int j=0) { return WJP[j]; }
00111 fmatvec::Mat3xV& getJacobianOfRotation(int j=0) { return WJR[j]; }
00112 fmatvec::Vec3& getGyroscopicAccelerationOfTranslation(int j=0) { return WjP[j]; }
00113 fmatvec::Vec3& getGyroscopicAccelerationOfRotation(int j=0) { return WjR[j]; }
00114 fmatvec::Vec3& getAcceleration() { return WaP; }
00115 fmatvec::Vec3& getAngularAcceleration() { return WpsiP; }
00116 void setAcceleration(const fmatvec::Vec3 &a) { WaP = a; }
00117 void setAngularAcceleration(const fmatvec::Vec3 &psi) { WpsiP = psi; }
00118 void setAnglesOfOrientation(const fmatvec::Vec3 &angles_) { angles = angles_; }
00119 const fmatvec::Vec3& getAnglesOfOrientation() const { return angles; }
00120 void setDotAnglesOfOrientation(const fmatvec::Vec3 &dotAngles_ ) { dotAngles = dotAngles_; }
00121 const fmatvec::Vec3& getDotAnglesOfOrientation() const { return dotAngles; }
00122
00123 virtual void initializeUsingXML(xercesc::DOMElement *element);
00124 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00125
00126
00127 protected:
00129
00130
00131
00132
00136 int hSize[2], hInd[2];
00137
00141 fmatvec::Vec3 WrOP;
00142
00146 fmatvec::Vec3 LrOP;
00147
00151 fmatvec::SqrMat3 AWP;
00152
00156 fmatvec::Vec3 WvP, WomegaP;
00157
00161 fmatvec::Mat3xV WJP[2], WJR[2];
00162
00166 fmatvec::Vec3 WjP[2], WjR[2];
00167
00171 fmatvec::Vec3 WaP, WpsiP;
00172
00176 fmatvec::Vec3 angles, dotAngles;
00177
00178 #ifdef HAVE_OPENMBVCPPINTERFACE
00179 boost::shared_ptr<OpenMBV::Frame> openMBVFrame;
00180 #endif
00181 };
00182
00187 class FixedRelativeFrame : public Frame {
00188
00189 public:
00190 FixedRelativeFrame(const std::string &name = "dummy", const fmatvec::Vec3 &r=fmatvec::Vec3(), const fmatvec::SqrMat3 &A=fmatvec::SqrMat3(fmatvec::EYE), const Frame *refFrame=0) : Frame(name), R(refFrame), RrRP(r), ARP(A) {}
00191
00192 std::string getType() const { return "FixedRelativeFrame"; }
00193
00194 virtual void init(InitStage stage);
00195
00196 void setRelativePosition(const fmatvec::Vec3 &r) { RrRP = r; }
00197 void setRelativeOrientation(const fmatvec::SqrMat3 &A) { ARP = A; }
00198 void setFrameOfReference(const Frame *frame) { R = frame; }
00199 void setFrameOfReference(const std::string &frame) { saved_frameOfReference = frame; }
00200
00201 const fmatvec::Vec3& getRelativePosition() const { return RrRP; }
00202 const fmatvec::SqrMat3& getRelativeOrientation() const { return ARP; }
00203 const Frame* getFrameOfReference() const { return R; }
00204 const fmatvec::Vec3& getWrRP() const { return WrRP; }
00205
00206 void updateRelativePosition() { WrRP = R->getOrientation()*RrRP; }
00207 void updatePosition() { updateRelativePosition(); setPosition(R->getPosition() + WrRP); }
00208 void updateOrientation() { setOrientation(R->getOrientation()*ARP); }
00209 void updateVelocity() { setVelocity(R->getVelocity() + crossProduct(R->getAngularVelocity(), WrRP)); }
00210 void updateAngularVelocity() { setAngularVelocity(R->getAngularVelocity()); }
00211 void updateStateDependentVariables() {
00212 updatePosition();
00213 updateOrientation();
00214 updateVelocity();
00215 updateAngularVelocity();
00216 }
00217 void updateJacobians(int j=0) {
00218 fmatvec::SqrMat3 tWrRP = tilde(WrRP);
00219 setJacobianOfTranslation(R->getJacobianOfTranslation(j) - tWrRP*R->getJacobianOfRotation(j),j);
00220 setJacobianOfRotation(R->getJacobianOfRotation(j),j);
00221 setGyroscopicAccelerationOfTranslation(R->getGyroscopicAccelerationOfTranslation(j) - tWrRP*R->getGyroscopicAccelerationOfRotation(j) + crossProduct(R->getAngularVelocity(),crossProduct(R->getAngularVelocity(),WrRP)),j);
00222 setGyroscopicAccelerationOfRotation(R->getGyroscopicAccelerationOfRotation(j),j);
00223 }
00224 void updateStateDerivativeDependentVariables(const fmatvec::Vec &ud) {
00225 setAcceleration(getJacobianOfTranslation()*ud + getGyroscopicAccelerationOfTranslation());
00226 setAngularAcceleration(getJacobianOfRotation()*ud + getGyroscopicAccelerationOfRotation());
00227 }
00228
00229 virtual void initializeUsingXML(xercesc::DOMElement *element);
00230 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00231
00232 protected:
00233 const Frame *R;
00234 fmatvec::Vec3 RrRP, WrRP;
00235 fmatvec::SqrMat3 ARP;
00236 std::string saved_frameOfReference;
00237 };
00238
00239 }
00240
00241 #endif
00242