20 #ifndef _FRAME_SENSORS_H_
21 #define _FRAME_SENSORS_H_
23 #include "mbsimControl/sensor.h"
29 namespace MBSimControl {
38 std::string getType()
const {
return "AbsolutCoordinateSensor"; }
39 void initializeUsingXML(TiXmlElement *element);
40 void init(MBSim::InitStage stage);
41 void setFrame(MBSim::Frame * frame_) {frame=frame_; }
42 void setDirection(fmatvec::Mat direction_) {
44 for (
int i=0; i<direction_.cols(); i++)
45 direction.col(i)=direction.col(i)/nrm2(direction.col(i));
46 assert(direction.rows()==3);
50 fmatvec::Mat direction;
51 std::string frameString;
61 std::string getType()
const {
return "AbsolutePositionSensor"; }
62 fmatvec::Vec getSignal();
72 std::string getType()
const {
return "AbsoluteVelocitySensor"; }
73 fmatvec::Vec getSignal();
83 std::string getType()
const {
return "AbsoluteAngularPositionSensor"; }
84 fmatvec::Vec getSignal();
86 void calcxSize() {xSize=direction.cols(); }
87 void init(MBSim::InitStage stage) {
88 if (stage==MBSim::resize) {
89 AbsolutCoordinateSensor::init(stage);
90 g.resize(direction.cols());
91 gd.resize(direction.cols());
92 x.resize(direction.cols());
95 AbsolutCoordinateSensor::init(stage);
97 void updateg(
double t) {g=x; }
98 void updategd(
double t);
99 void updatexd(
double t) {xd=gd; }
100 void updatedx(
double t,
double dt) {xd=gd*dt; }
110 std::string getType()
const {
return "AbsoluteAngularVelocitySensor"; }
111 fmatvec::Vec getSignal();
121 RelativeCoordinateSensor(
const std::string &name) :
Sensor(name), refFrame(NULL), relFrame(NULL), direction(0, fmatvec::INIT), refFrameString(
""), relFrameString(
"") {}
122 std::string getType()
const {
return "RelativeCoordinateSensor"; }
123 void initializeUsingXML(TiXmlElement *element);
124 void init(MBSim::InitStage stage);
125 void setReferenceFrame(MBSim::Frame * refFrame_) {refFrame=refFrame_; }
126 void setRelativeFrame(MBSim::Frame * relFrame_) {relFrame=relFrame_; }
127 void setDirection(fmatvec::Mat direction_) {
128 direction=direction_;
129 for (
int i=0; i<direction_.cols(); i++)
130 direction.col(i)=direction.col(i)/nrm2(direction.col(i));
131 assert(direction.rows()==3);
134 MBSim::Frame * refFrame;
135 MBSim::Frame * relFrame;
136 fmatvec::Mat direction;
137 std::string refFrameString, relFrameString;
147 std::string getType()
const {
return "RelativePositionSensor"; }
148 fmatvec::Vec getSignal();
158 std::string getType()
const {
return "RelativeVelocitySensor"; }
159 fmatvec::Vec getSignal();
169 std::string getType()
const {
return "RelativeAngularPositionSensor"; }
170 fmatvec::Vec getSignal();
172 void calcxSize() {xSize=direction.cols(); }
173 void init(MBSim::InitStage stage) {
174 if (stage==MBSim::resize) {
175 RelativeCoordinateSensor::init(stage);
176 g.resize(direction.cols());
177 gd.resize(direction.cols());
178 x.resize(direction.cols());
181 RelativeCoordinateSensor::init(stage);
183 void updateg(
double t) {g=x; }
184 void updategd(
double t);
185 void updatexd(
double t) {xd=gd; }
186 void updatedx(
double t,
double dt) {xd=gd*dt; }
196 std::string getType()
const {
return "RelativeAngularVelocitySensor"; }
197 fmatvec::Vec getSignal();