imu_offset_compensation.hh 3.56 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 */

#ifndef __sot_torque_control_imu_offset_compensation_H__
#define __sot_torque_control_imu_offset_compensation_H__

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Guilhem Saurel committed
12
13
14
#if defined(WIN32)
#if defined(imu_offset_compensation_EXPORTS)
#define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
15
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
16
17
18
19
#define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTIMUOFFSETCOMPENSATION_EXPORT
20
21
22
23
24
25
26
#endif

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <map>
27
28
29
30

#include <pinocchio/fwd.hpp>

#include <boost/assign.hpp>
31

32
33
34
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
35

36
37
#include <sot/torque_control/utils/vector-conversions.hh>

38
namespace dynamicgraph {
Guilhem Saurel's avatar
Guilhem Saurel committed
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
namespace sot {
namespace torque_control {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgraph::Entity {
  typedef ImuOffsetCompensation EntityClassName;
  DYNAMIC_GRAPH_ENTITY_DECL();
  typedef Eigen::Vector3d Vector3;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /* --- CONSTRUCTOR ---- */
  ImuOffsetCompensation(const std::string& name);

  /* --- COMMANDS --- */
  void init(const double& dt);
  void update_offset(const double& duration);
  void setGyroDCBlockerParameter(const double& alpha);
  /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector);    /// raw accelerometer data
  DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector);        /// raw gyrometer data
  DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector);  /// compensated accelerometer data
  DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector);      /// compensated gyrometer data

 protected:
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;

  /* --- METHODS --- */
  void update_offset_impl(int iter);
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Guilhem Saurel's avatar
Guilhem Saurel committed
74
    logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
Guilhem Saurel's avatar
Guilhem Saurel committed
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
  }

 protected:
  bool m_initSucceeded;        /// true if the entity has been successfully initialized
  float m_dt;                  /// sampling time in seconds
  int m_update_cycles_left;    /// number of update cycles left
  int m_update_cycles;         /// total number of update cycles to perform
  double m_a_gyro_DC_blocker;  /// filter parameter to remove DC from gyro online (should be close to <1.0 and equal
                               /// to 1.0 for disabling)
  Vector3 m_gyro_offset;       /// gyrometer offset
  Vector3 m_acc_offset;        /// accelerometer offset

  Vector3 m_gyro_sum;  /// tmp variable to store the sum of the gyro measurements during update phase
  Vector3 m_acc_sum;   /// tmp variable to store the sum of the acc measurements during update phase

};  // class ImuOffsetCompensation
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __sot_torque_control_imu_offset_compensation_H__