mbsimcontrol  3.3.0
frame_sensors.h
00001 /* Copyright (C) 2004-2009 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: markus.ms.schneider@gmail.com
00018  */
00019 
00020 #ifndef _FRAME_SENSORS_H_
00021 #define _FRAME_SENSORS_H_
00022 
00023 #include "mbsimControl/sensor.h"
00024 
00025 namespace MBSim {
00026   class Frame;
00027 }
00028 
00029 namespace MBSimControl {
00030 
00035   class AbsolutCoordinateSensor : public Sensor {
00036     public:
00037       AbsolutCoordinateSensor(const std::string &name) : Sensor(name), frame(NULL), direction(), frameString("") {}
00038       std::string getType() const { return "AbsolutCoordinateSensor"; }
00039       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00040       void init(MBSim::InitStage stage);
00041       void setFrame(MBSim::Frame * frame_) {frame=frame_; }
00042       void setDirection(fmatvec::Mat direction_) {
00043         direction=direction_;
00044         for (int i=0; i<direction_.cols(); i++)
00045           direction.col(i)=direction.col(i)/nrm2(direction.col(i)); 
00046         assert(direction.rows()==3); 
00047       }
00048     protected:
00049       MBSim::Frame * frame;
00050       fmatvec::Mat direction;
00051       std::string frameString;
00052   };
00053 
00058   class AbsolutePositionSensor : public AbsolutCoordinateSensor {
00059     public:
00060       AbsolutePositionSensor(const std::string &name="") : AbsolutCoordinateSensor(name) {}
00061       std::string getType() const { return "AbsolutePositionSensor"; }
00062       fmatvec::Vec getSignal();
00063   };
00064 
00069   class AbsoluteVelocitySensor : public AbsolutCoordinateSensor {
00070     public:
00071       AbsoluteVelocitySensor(const std::string &name="") : AbsolutCoordinateSensor(name) {}
00072       std::string getType() const { return "AbsoluteVelocitySensor"; }
00073       fmatvec::Vec getSignal();
00074   };
00075 
00080   class AbsoluteAngularPositionSensor : public AbsolutCoordinateSensor {
00081     public:
00082       AbsoluteAngularPositionSensor(const std::string &name="") : AbsolutCoordinateSensor(name) {}
00083       std::string getType() const { return "AbsoluteAngularPositionSensor"; }
00084       fmatvec::Vec getSignal();
00085 
00086       void calcxSize() {xSize=direction.cols(); }
00087       void init(MBSim::InitStage stage) {
00088         if (stage==MBSim::resize) {
00089           AbsolutCoordinateSensor::init(stage);
00090           g.resize(direction.cols()); 
00091           gd.resize(direction.cols()); 
00092           x.resize(direction.cols());
00093         }
00094         else
00095           AbsolutCoordinateSensor::init(stage);
00096       }
00097       void updateg(double t) {g=x; }
00098       void updategd(double t);
00099       void updatexd(double t) {xd=gd; }
00100       void updatedx(double t, double dt) {xd=gd*dt; }
00101   };
00102 
00107   class AbsoluteAngularVelocitySensor : public AbsolutCoordinateSensor {
00108     public:
00109       AbsoluteAngularVelocitySensor(const std::string &name="") : AbsolutCoordinateSensor(name) {}
00110       std::string getType() const { return "AbsoluteAngularVelocitySensor"; }
00111       fmatvec::Vec getSignal();
00112   };
00113   
00114   
00119   class RelativeCoordinateSensor : public Sensor {
00120     public:
00121       RelativeCoordinateSensor(const std::string &name) : Sensor(name), refFrame(NULL), relFrame(NULL), direction(), refFrameString(""), relFrameString("") {}
00122       std::string getType() const { return "RelativeCoordinateSensor"; }
00123       void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00124       void init(MBSim::InitStage stage);
00125       void setReferenceFrame(MBSim::Frame * refFrame_) {refFrame=refFrame_; }
00126       void setRelativeFrame(MBSim::Frame * relFrame_) {relFrame=relFrame_; }
00127       void setDirection(fmatvec::Mat direction_) {
00128         direction=direction_;
00129         for (int i=0; i<direction_.cols(); i++)
00130           direction.col(i)=direction.col(i)/nrm2(direction.col(i)); 
00131         assert(direction.rows()==3); 
00132       }
00133     protected:
00134       MBSim::Frame * refFrame;
00135       MBSim::Frame * relFrame;
00136       fmatvec::Mat direction;
00137       std::string refFrameString, relFrameString;
00138   };
00139   
00144   class RelativePositionSensor : public RelativeCoordinateSensor {
00145     public:
00146       RelativePositionSensor(const std::string &name="") : RelativeCoordinateSensor(name) {}
00147       std::string getType() const { return "RelativePositionSensor"; }
00148       fmatvec::Vec getSignal();
00149   };
00150   
00155   class RelativeVelocitySensor : public RelativeCoordinateSensor {
00156     public:
00157       RelativeVelocitySensor(const std::string &name="") : RelativeCoordinateSensor(name) {}
00158       std::string getType() const { return "RelativeVelocitySensor"; }
00159       fmatvec::Vec getSignal();
00160   };
00161   
00166   class RelativeAngularPositionSensor : public RelativeCoordinateSensor {
00167     public:
00168       RelativeAngularPositionSensor(const std::string &name="") : RelativeCoordinateSensor(name) {}
00169       std::string getType() const { return "RelativeAngularPositionSensor"; }
00170       fmatvec::Vec getSignal();
00171       
00172       void calcxSize() {xSize=direction.cols(); }
00173       void init(MBSim::InitStage stage) {
00174         if (stage==MBSim::resize) {
00175           RelativeCoordinateSensor::init(stage);
00176           g.resize(direction.cols()); 
00177           gd.resize(direction.cols());
00178           x.resize(direction.cols()); 
00179         }
00180         else
00181           RelativeCoordinateSensor::init(stage);
00182       }
00183       void updateg(double t) {g=x; }
00184       void updategd(double t);
00185       void updatexd(double t) {xd=gd; }
00186       void updatedx(double t, double dt) {xd=gd*dt; }
00187   };
00188   
00193   class RelativeAngularVelocitySensor : public RelativeCoordinateSensor {
00194     public:
00195       RelativeAngularVelocitySensor(const std::string &name="") : RelativeCoordinateSensor(name) {}
00196       std::string getType() const { return "RelativeAngularVelocitySensor"; }
00197       fmatvec::Vec getSignal();
00198   };
00199 
00200 }
00201 
00202 #endif /* _FRAME_SENSORS_H_ */