1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
*/ |
4 |
|
|
|
5 |
|
|
#ifndef __sot_torque_control_imu_offset_compensation_H__ |
6 |
|
|
#define __sot_torque_control_imu_offset_compensation_H__ |
7 |
|
|
|
8 |
|
|
/* --------------------------------------------------------------------- */ |
9 |
|
|
/* --- API ------------------------------------------------------------- */ |
10 |
|
|
/* --------------------------------------------------------------------- */ |
11 |
|
|
|
12 |
|
|
#if defined(WIN32) |
13 |
|
|
#if defined(imu_offset_compensation_EXPORTS) |
14 |
|
|
#define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport) |
15 |
|
|
#else |
16 |
|
|
#define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport) |
17 |
|
|
#endif |
18 |
|
|
#else |
19 |
|
|
#define SOTIMUOFFSETCOMPENSATION_EXPORT |
20 |
|
|
#endif |
21 |
|
|
|
22 |
|
|
/* --------------------------------------------------------------------- */ |
23 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
24 |
|
|
/* --------------------------------------------------------------------- */ |
25 |
|
|
|
26 |
|
|
#include <pinocchio/fwd.hpp> |
27 |
|
|
|
28 |
|
|
// include pinocchio first |
29 |
|
|
|
30 |
|
|
#include <dynamic-graph/signal-helper.h> |
31 |
|
|
|
32 |
|
|
#include <boost/assign.hpp> |
33 |
|
|
#include <map> |
34 |
|
|
#include <sot/core/matrix-geometry.hh> |
35 |
|
|
#include <sot/core/robot-utils.hh> |
36 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
37 |
|
|
|
38 |
|
|
namespace dynamicgraph { |
39 |
|
|
namespace sot { |
40 |
|
|
namespace torque_control { |
41 |
|
|
|
42 |
|
|
/* --------------------------------------------------------------------- */ |
43 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
44 |
|
|
/* --------------------------------------------------------------------- */ |
45 |
|
|
|
46 |
|
|
class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation |
47 |
|
|
: public ::dynamicgraph::Entity { |
48 |
|
|
typedef ImuOffsetCompensation EntityClassName; |
49 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
50 |
|
|
typedef Eigen::Vector3d Vector3; |
51 |
|
|
|
52 |
|
|
public: |
53 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
54 |
|
|
|
55 |
|
|
/* --- CONSTRUCTOR ---- */ |
56 |
|
|
ImuOffsetCompensation(const std::string& name); |
57 |
|
|
|
58 |
|
|
/* --- COMMANDS --- */ |
59 |
|
|
void init(const double& dt); |
60 |
|
|
void update_offset(const double& duration); |
61 |
|
|
void setGyroDCBlockerParameter(const double& alpha); |
62 |
|
|
/* --- SIGNALS --- */ |
63 |
|
|
DECLARE_SIGNAL_IN(accelerometer_in, |
64 |
|
|
dynamicgraph::Vector); /// raw accelerometer data |
65 |
|
|
DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector); /// raw gyrometer data |
66 |
|
|
DECLARE_SIGNAL_OUT(accelerometer_out, |
67 |
|
|
dynamicgraph::Vector); /// compensated accelerometer data |
68 |
|
|
DECLARE_SIGNAL_OUT(gyrometer_out, |
69 |
|
|
dynamicgraph::Vector); /// compensated gyrometer data |
70 |
|
|
|
71 |
|
|
protected: |
72 |
|
|
/* --- ENTITY INHERITANCE --- */ |
73 |
|
|
virtual void display(std::ostream& os) const; |
74 |
|
|
|
75 |
|
|
/* --- METHODS --- */ |
76 |
|
|
void update_offset_impl(int iter); |
77 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
78 |
|
|
const char* file = "", int line = 0) { |
79 |
|
|
logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, |
80 |
|
|
file, line); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
protected: |
84 |
|
|
bool |
85 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
86 |
|
|
float m_dt; /// sampling time in seconds |
87 |
|
|
int m_update_cycles_left; /// number of update cycles left |
88 |
|
|
int m_update_cycles; /// total number of update cycles to perform |
89 |
|
|
double m_a_gyro_DC_blocker; /// filter parameter to remove DC from gyro |
90 |
|
|
/// online (should be close to <1.0 and equal |
91 |
|
|
/// to 1.0 for disabling) |
92 |
|
|
Vector3 m_gyro_offset; /// gyrometer offset |
93 |
|
|
Vector3 m_acc_offset; /// accelerometer offset |
94 |
|
|
|
95 |
|
|
Vector3 m_gyro_sum; /// tmp variable to store the sum of the gyro |
96 |
|
|
/// measurements during update phase |
97 |
|
|
Vector3 m_acc_sum; /// tmp variable to store the sum of the acc measurements |
98 |
|
|
/// during update phase |
99 |
|
|
|
100 |
|
|
}; // class ImuOffsetCompensation |
101 |
|
|
} // namespace torque_control |
102 |
|
|
} // namespace sot |
103 |
|
|
} // namespace dynamicgraph |
104 |
|
|
|
105 |
|
|
#endif // #ifndef __sot_torque_control_imu_offset_compensation_H__ |