Loading...
Searching...
No Matches
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
38#include <sot/core/matrix-geometry.hh>
39
40/* STD */
41#include <string>
42
43namespace dynamicgraph {
44namespace sot {
45namespace dg = dynamicgraph;
46
47/* --------------------------------------------------------------------- */
48/* --- CLASS ----------------------------------------------------------- */
49/* --------------------------------------------------------------------- */
50
51class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
52 public:
53 static const std::string CLASS_NAME;
54 virtual const std::string& getClassName(void) const { return CLASS_NAME; }
55
56 public: /* --- CONSTRUCTION --- */
57 AngleEstimator(const std::string& name);
58 virtual ~AngleEstimator(void);
59
60 public: /* --- SIGNAL --- */
61 dg::SignalPtr<MatrixRotation, int>
62 sensorWorldRotationSIN; // estimate(worldRc)
63 dg::SignalPtr<MatrixHomogeneous, int>
65 dg::SignalPtr<MatrixHomogeneous, int>
66 contactWorldPositionSIN; // estimate(worldRf)
67 dg::SignalPtr<MatrixHomogeneous, int>
69 dg::SignalTimeDependent<dynamicgraph::Vector, int>
70 anglesSOUT; // [ flex1 flex2 yaw_drift ]
71 dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
72 dg::SignalTimeDependent<MatrixRotation, int>
73 driftSOUT; // Ryaw = worldRc est(wRc)^-1
74 dg::SignalTimeDependent<MatrixRotation, int>
76 dg::SignalTimeDependent<MatrixRotation, int>
77 waistWorldRotationSOUT; // worldRwaist
78 dg::SignalTimeDependent<MatrixHomogeneous, int>
79 waistWorldPositionSOUT; // worldMwaist
80 dg::SignalTimeDependent<dynamicgraph::Vector, int>
81 waistWorldPoseRPYSOUT; // worldMwaist
82
83 dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
84 dg::SignalPtr<dynamicgraph::Vector, int> qdotSIN;
85 dg::SignalTimeDependent<dynamicgraph::Vector, int> xff_dotSOUT;
86 dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
87
88 public: /* --- FUNCTIONS --- */
89 dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res,
90 const int& time);
91 MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res,
92 const int& time);
93 MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const int& time);
94 MatrixRotation& computeSensorWorldRotation(MatrixRotation& res,
95 const int& time);
96 MatrixRotation& computeWaistWorldRotation(MatrixRotation& res,
97 const int& time);
98 MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res,
99 const int& time);
100 dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res,
101 const int& time);
102 dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res,
103 const int& time);
104 dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res,
105 const int& time);
106
107 public: /* --- PARAMS --- */
108 void fromSensor(const bool& fs) { fromSensor_ = fs; }
109 bool fromSensor() const { return fromSensor_; }
110
111 private:
112 bool fromSensor_;
113};
114
115} /* namespace sot */
116} /* namespace dynamicgraph */
117
118#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
#define SOTANGLEESTIMATOR_EXPORT
Definition: angle-estimator.h:23
Definition: angle-estimator.h:51
bool fromSensor() const
Definition: angle-estimator.h:109
dynamicgraph::Vector & computeAngles(dynamicgraph::Vector &res, const int &time)
dg::SignalTimeDependent< MatrixRotation, int > flexibilitySOUT
Definition: angle-estimator.h:71
dg::SignalTimeDependent< dynamicgraph::Vector, int > anglesSOUT
Definition: angle-estimator.h:70
dg::SignalPtr< MatrixHomogeneous, int > sensorEmbeddedPositionSIN
Definition: angle-estimator.h:64
dg::SignalTimeDependent< MatrixRotation, int > sensorWorldRotationSOUT
Definition: angle-estimator.h:75
dg::SignalTimeDependent< dynamicgraph::Vector, int > qdotSOUT
Definition: angle-estimator.h:86
dg::SignalPtr< MatrixHomogeneous, int > contactEmbeddedPositionSIN
Definition: angle-estimator.h:68
MatrixRotation & computeSensorWorldRotation(MatrixRotation &res, const int &time)
MatrixRotation & computeWaistWorldRotation(MatrixRotation &res, const int &time)
static const std::string CLASS_NAME
Definition: angle-estimator.h:53
dg::SignalTimeDependent< MatrixRotation, int > waistWorldRotationSOUT
Definition: angle-estimator.h:77
dynamicgraph::Vector & compute_xff_dotSOUT(dynamicgraph::Vector &res, const int &time)
dg::SignalTimeDependent< dynamicgraph::Vector, int > xff_dotSOUT
Definition: angle-estimator.h:85
void fromSensor(const bool &fs)
Definition: angle-estimator.h:108
dg::SignalPtr< MatrixHomogeneous, int > contactWorldPositionSIN
Definition: angle-estimator.h:66
dg::SignalPtr< dynamicgraph::Matrix, int > jacobianSIN
Definition: angle-estimator.h:83
dg::SignalTimeDependent< dynamicgraph::Vector, int > waistWorldPoseRPYSOUT
Definition: angle-estimator.h:81
dg::SignalPtr< MatrixRotation, int > sensorWorldRotationSIN
Definition: angle-estimator.h:62
AngleEstimator(const std::string &name)
dg::SignalPtr< dynamicgraph::Vector, int > qdotSIN
Definition: angle-estimator.h:84
MatrixRotation & computeDriftFromAngles(MatrixRotation &res, const int &time)
virtual const std::string & getClassName(void) const
Definition: angle-estimator.h:54
dg::SignalTimeDependent< MatrixRotation, int > driftSOUT
Definition: angle-estimator.h:73
dynamicgraph::Vector & compute_qdotSOUT(dynamicgraph::Vector &res, const int &time)
dynamicgraph::Vector & computeWaistWorldPoseRPY(dynamicgraph::Vector &res, const int &time)
MatrixHomogeneous & computeWaistWorldPosition(MatrixHomogeneous &res, const int &time)
dg::SignalTimeDependent< MatrixHomogeneous, int > waistWorldPositionSOUT
Definition: angle-estimator.h:79
MatrixRotation & computeFlexibilityFromAngles(MatrixRotation &res, const int &time)
Definition: angle-estimator.h:43