angle-estimator.h
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_ANGLE_ESTIMATOR_H__
11 #define __SOT_ANGLE_ESTIMATOR_H__
12 /* --------------------------------------------------------------------- */
13 /* --- API ------------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 #if defined(WIN32)
17 #if defined(angle_estimator_EXPORTS)
18 #define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
19 #else
20 #define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
21 #endif
22 #else
23 #define SOTANGLEESTIMATOR_EXPORT
24 #endif
25 
26 /* --------------------------------------------------------------------- */
27 /* --- INCLUDE --------------------------------------------------------- */
28 /* --------------------------------------------------------------------- */
29 
30 /* Matrix */
31 #include <dynamic-graph/linear-algebra.h>
32 
33 /* SOT */
34 #include <dynamic-graph/entity.h>
35 #include <dynamic-graph/signal-ptr.h>
36 #include <dynamic-graph/signal-time-dependent.h>
37 #include <sot/core/matrix-geometry.hh>
38 
39 /* STD */
40 #include <string>
41 
42 namespace dynamicgraph {
43 namespace sot {
44 namespace dg = dynamicgraph;
45 
46 /* --------------------------------------------------------------------- */
47 /* --- CLASS ----------------------------------------------------------- */
48 /* --------------------------------------------------------------------- */
49 
50 class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
51  public:
52  static const std::string CLASS_NAME;
53  virtual const std::string& getClassName(void) const { return CLASS_NAME; }
54 
55  public: /* --- CONSTRUCTION --- */
56  AngleEstimator(const std::string& name);
57  virtual ~AngleEstimator(void);
58 
59  public: /* --- SIGNAL --- */
60  dg::SignalPtr<MatrixRotation, int> sensorWorldRotationSIN; // estimate(worldRc)
61  dg::SignalPtr<MatrixHomogeneous, int> sensorEmbeddedPositionSIN; // waistRchest
62  dg::SignalPtr<MatrixHomogeneous, int> contactWorldPositionSIN; // estimate(worldRf)
63  dg::SignalPtr<MatrixHomogeneous, int> contactEmbeddedPositionSIN; // waistRleg
64  dg::SignalTimeDependent<dynamicgraph::Vector, int> anglesSOUT; // [ flex1 flex2 yaw_drift ]
65  dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
66  dg::SignalTimeDependent<MatrixRotation, int> driftSOUT; // Ryaw = worldRc est(wRc)^-1
67  dg::SignalTimeDependent<MatrixRotation, int> sensorWorldRotationSOUT; // worldRc
68  dg::SignalTimeDependent<MatrixRotation, int> waistWorldRotationSOUT; // worldRwaist
69  dg::SignalTimeDependent<MatrixHomogeneous, int> waistWorldPositionSOUT; // worldMwaist
70  dg::SignalTimeDependent<dynamicgraph::Vector, int> waistWorldPoseRPYSOUT; // worldMwaist
71 
72  dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
73  dg::SignalPtr<dynamicgraph::Vector, int> qdotSIN;
74  dg::SignalTimeDependent<dynamicgraph::Vector, int> xff_dotSOUT;
75  dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
76 
77  public: /* --- FUNCTIONS --- */
78  dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res, const int& time);
79  MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res, const int& time);
80  MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const int& time);
81  MatrixRotation& computeSensorWorldRotation(MatrixRotation& res, const int& time);
82  MatrixRotation& computeWaistWorldRotation(MatrixRotation& res, const int& time);
83  MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res, const int& time);
84  dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res, const int& time);
85  dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res, const int& time);
86  dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res, const int& time);
87 
88  public: /* --- PARAMS --- */
89  void fromSensor(const bool& fs) { fromSensor_ = fs; }
90  bool fromSensor() const { return fromSensor_; }
91 
92  private:
93  bool fromSensor_;
94 };
95 
96 } /* namespace sot */
97 } /* namespace dynamicgraph */
98 
99 #endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
dg::SignalTimeDependent< MatrixRotation, int > flexibilitySOUT
Definition: angle-estimator.h:65
bool fromSensor() const
Definition: angle-estimator.h:90
dg::SignalTimeDependent< dynamicgraph::Vector, int > qdotSOUT
Definition: angle-estimator.h:75
dg::SignalPtr< MatrixRotation, int > sensorWorldRotationSIN
Definition: angle-estimator.h:60
dg::SignalTimeDependent< dynamicgraph::Vector, int > anglesSOUT
Definition: angle-estimator.h:64
dg::SignalTimeDependent< MatrixHomogeneous, int > waistWorldPositionSOUT
Definition: angle-estimator.h:69
virtual const std::string & getClassName(void) const
Definition: angle-estimator.h:53
dg::SignalPtr< dynamicgraph::Vector, int > qdotSIN
Definition: angle-estimator.h:73
dg::SignalTimeDependent< MatrixRotation, int > driftSOUT
Definition: angle-estimator.h:66
static const std::string CLASS_NAME
Definition: angle-estimator.h:52
dg::SignalTimeDependent< MatrixRotation, int > sensorWorldRotationSOUT
Definition: angle-estimator.h:67
dg::SignalPtr< MatrixHomogeneous, int > contactWorldPositionSIN
Definition: angle-estimator.h:62
#define SOTANGLEESTIMATOR_EXPORT
Definition: angle-estimator.h:23
dg::SignalTimeDependent< dynamicgraph::Vector, int > waistWorldPoseRPYSOUT
Definition: angle-estimator.h:70
dg::SignalTimeDependent< MatrixRotation, int > waistWorldRotationSOUT
Definition: angle-estimator.h:68
dg::SignalPtr< MatrixHomogeneous, int > contactEmbeddedPositionSIN
Definition: angle-estimator.h:63
dg::SignalPtr< dynamicgraph::Matrix, int > jacobianSIN
Definition: angle-estimator.h:72
dg::SignalTimeDependent< dynamicgraph::Vector, int > xff_dotSOUT
Definition: angle-estimator.h:74
void fromSensor(const bool &fs)
Definition: angle-estimator.h:89
dg::SignalPtr< MatrixHomogeneous, int > sensorEmbeddedPositionSIN
Definition: angle-estimator.h:61
Definition: angle-estimator.h:42
Definition: angle-estimator.h:50