MBSim

Project Page
kinematics_observer.h
00001 /* Copyright (C) 2004-2013 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@gmail.com
00018  */
00019 
00020 #ifndef _KINEMATICS_OBSERVER_H__
00021 #define _KINEMATICS_OBSERVER_H__
00022 #include "mbsim/observer.h"
00023 
00024 #ifdef HAVE_OPENMBVCPPINTERFACE
00025 #include <openmbvcppinterface/arrow.h>
00026 #endif
00027 
00028 namespace MBSim {
00029   class Frame;
00030 
00031   class KinematicsObserver : public Observer {
00032     protected:
00033       Frame* frame;
00034       std::string saved_frame;
00035 #ifdef HAVE_OPENMBVCPPINTERFACE
00036       OpenMBV::Group *openMBVPosGrp, *openMBVVelGrp, *openMBVAngVelGrp, *openMBVAccGrp, *openMBVAngAccGrp;
00037       OpenMBV::Arrow *openMBVPositionArrow, *openMBVVelocityArrow, *openMBVAngularVelocityArrow, *openMBVAccelerationArrow, *openMBVAngularAccelerationArrow;
00038 #endif
00039 
00040     public:
00041       KinematicsObserver(const std::string &name="");
00042       void setFrame(Frame *frame_) { frame = frame_; } 
00043       void init(InitStage stage);
00044       virtual void plot(double t, double dt);
00045       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00046 
00047 #ifdef HAVE_OPENMBVCPPINTERFACE
00048       void setOpenMBVPositionArrow(OpenMBV::Arrow *arrow) { openMBVPositionArrow = arrow; }
00049       void setOpenMBVVelocityArrow(OpenMBV::Arrow *arrow) { openMBVVelocityArrow = arrow; }
00050       void setOpenMBVAngularVelocityArrow(OpenMBV::Arrow *arrow) { openMBVAngularVelocityArrow = arrow; }
00051       void setOpenMBVAccelerationArrow(OpenMBV::Arrow *arrow) { openMBVAccelerationArrow = arrow; }
00052       void setOpenMBVAngularAccelerationArrow(OpenMBV::Arrow *arrow) { openMBVAngularAccelerationArrow = arrow; }
00053 
00054       virtual void enableOpenMBVPosition(double diameter=0.5, double headDiameter=1, double headLength=1, double color=0.5);
00055       virtual void enableOpenMBVVelocity(double scale=1, OpenMBV::Arrow::ReferencePoint refPoint=OpenMBV::Arrow::fromPoint, double diameter=0.5, double headDiameter=1, double headLength=1, double color=0.5);
00056       virtual void enableOpenMBVAngularVelocity(double scale=1, OpenMBV::Arrow::ReferencePoint refPoint=OpenMBV::Arrow::fromPoint, double diameter=0.5, double headDiameter=1, double headLength=1, double color=0.5);
00057       virtual void enableOpenMBVAcceleration(double scale=1, OpenMBV::Arrow::ReferencePoint refPoint=OpenMBV::Arrow::fromPoint, double diameter=0.5, double headDiameter=1, double headLength=1, double color=0.5);
00058       virtual void enableOpenMBVAngularAcceleration(double scale=1, OpenMBV::Arrow::ReferencePoint refPoint=OpenMBV::Arrow::fromPoint, double diameter=0.5, double headDiameter=1, double headLength=1, double color=0.5);
00059 #endif
00060   };
00061 
00062   class AbsoluteKinematicsObserver : public KinematicsObserver {
00063     public:
00064       AbsoluteKinematicsObserver(const std::string &name="") : KinematicsObserver(name) {}
00065   };
00066 
00067   class RelativeKinematicsObserver : public KinematicsObserver {
00068     private:
00069       Frame* refFrame;
00070       std::string saved_frameOfReference;
00071 #ifdef HAVE_OPENMBVCPPINTERFACE
00072       OpenMBV::Arrow *openMBVrTrans, *openMBVrRel;
00073       OpenMBV::Arrow *openMBVvTrans, *openMBVvRot, *openMBVvRel, *openMBVvF;
00074       OpenMBV::Arrow *openMBVaTrans, *openMBVaRot, *openMBVaZp, *openMBVaCor, *openMBVaRel, *openMBVaF;
00075       OpenMBV::Arrow *openMBVomTrans, *openMBVomRel;
00076       OpenMBV::Arrow *openMBVpsiTrans, *openMBVpsiRot, *openMBVpsiRel;
00077 #endif
00078 
00079     public:
00080       RelativeKinematicsObserver(const std::string &name="");
00081       void setFrames(Frame *frame0, Frame *frame1) { frame = frame0; refFrame = frame1; } 
00082       void setFrameOfReference(Frame *frame_) { refFrame = frame_; }
00083 
00084       void init(InitStage stage);
00085       virtual void plot(double t, double dt);
00086       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00087   };
00088 
00089 }  
00090 
00091 #endif
00092