GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/dynamic-pinocchio/angle-estimator.h Lines: 0 3 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 0 - %

Line Branch Exec Source
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
43
namespace dynamicgraph {
44
namespace sot {
45
namespace dg = dynamicgraph;
46
47
/* --------------------------------------------------------------------- */
48
/* --- CLASS ----------------------------------------------------------- */
49
/* --------------------------------------------------------------------- */
50
51
class 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>
64
      sensorEmbeddedPositionSIN;  // waistRchest
65
  dg::SignalPtr<MatrixHomogeneous, int>
66
      contactWorldPositionSIN;  // estimate(worldRf)
67
  dg::SignalPtr<MatrixHomogeneous, int>
68
      contactEmbeddedPositionSIN;  // waistRleg
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>
75
      sensorWorldRotationSOUT;  // worldRc
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__