MBSim

Project Page
frame.h
00001 /* Copyright (C) 2004-2014 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
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       /* INHERITED INTERFACE ELEMENT */
00060       std::string getType() const { return "Frame"; }
00061       virtual void plot(double t, double dt = 1); 
00062       virtual void closePlot(); 
00063       /***************************************************/
00064 
00065       /* INTERFACE FOR DERIVED CLASSES */
00066       virtual int gethSize(int i=0) const { return hSize[i]; }
00067       virtual int gethInd(int i=0) const { return hInd[i]; }
00068       //virtual ObjectInterface* getParent() { return parent; }
00069       //virtual void setParent(ObjectInterface* parent_) { parent = parent_; }
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       /* GETTER / SETTER */
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       // * \brief parent object for plot invocation
00130       // */
00131       //ObjectInterface* parent;
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 /* _FRAME_H_ */
00242