sot-torque-control  1.5.2
imu_offset_compensation.hh
Go to the documentation of this file.
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 <map>
27 #include "boost/assign.hpp"
28 
29 #include <dynamic-graph/signal-helper.h>
30 #include <sot/core/matrix-geometry.hh>
31 #include <sot/core/robot-utils.hh>
33 
34 namespace dynamicgraph {
35 namespace sot {
36 namespace torque_control {
37 
38 /* --------------------------------------------------------------------- */
39 /* --- CLASS ----------------------------------------------------------- */
40 /* --------------------------------------------------------------------- */
41 
42 class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgraph::Entity {
44  DYNAMIC_GRAPH_ENTITY_DECL();
45  typedef Eigen::Vector3d Vector3;
46 
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  /* --- CONSTRUCTOR ---- */
51  ImuOffsetCompensation(const std::string& name);
52 
53  /* --- COMMANDS --- */
54  void init(const double& dt);
55  void update_offset(const double& duration);
56  void setGyroDCBlockerParameter(const double& alpha);
57  /* --- SIGNALS --- */
58  DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector);
59  DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector);
60  DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector);
61  DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector);
62 
63  protected:
64  /* --- ENTITY INHERITANCE --- */
65  virtual void display(std::ostream& os) const;
66 
67  /* --- METHODS --- */
68  void update_offset_impl(int iter);
69  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
70  logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
71  }
72 
73  protected:
75  float m_dt;
79  Vector3 m_gyro_offset;
81  Vector3 m_acc_offset;
82 
83  Vector3 m_gyro_sum;
84  Vector3 m_acc_sum;
85 
86 }; // class ImuOffsetCompensation
87 } // namespace torque_control
88 } // namespace sot
89 } // namespace dynamicgraph
90 
91 #endif // #ifndef __sot_torque_control_imu_offset_compensation_H__
double m_a_gyro_DC_blocker
total number of update cycles to perform
#define SOTIMUOFFSETCOMPENSATION_EXPORT
Eigen::Matrix< double, 3, 1 > Vector3
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
float m_dt
true if the entity has been successfully initialized
Vector3 m_acc_sum
tmp variable to store the sum of the gyro measurements during update phase
to read text file
Definition: treeview.dox:22