sot-torque-control  1.5.3
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
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__
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_update_cycles_left
int m_update_cycles_left
sampling time in seconds
Definition: imu_offset_compensation.hh:76
dynamicgraph::sot::torque_control::Vector3
Eigen::Matrix< double, 3, 1 > Vector3
Definition: admittance-controller.cpp:46
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_initSucceeded
bool m_initSucceeded
Definition: imu_offset_compensation.hh:74
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_dt
float m_dt
true if the entity has been successfully initialized
Definition: imu_offset_compensation.hh:75
vector-conversions.hh
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_update_cycles
int m_update_cycles
number of update cycles left
Definition: imu_offset_compensation.hh:77
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_gyro_sum
Vector3 m_gyro_sum
accelerometer offset
Definition: imu_offset_compensation.hh:83
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_a_gyro_DC_blocker
double m_a_gyro_DC_blocker
total number of update cycles to perform
Definition: imu_offset_compensation.hh:78
dynamicgraph::sot::torque_control::ImuOffsetCompensation
Definition: imu_offset_compensation.hh:42
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_acc_offset
Vector3 m_acc_offset
gyrometer offset
Definition: imu_offset_compensation.hh:81
dynamicgraph::sot::torque_control::ImuOffsetCompensation::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
Definition: imu_offset_compensation.hh:69
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
SOTIMUOFFSETCOMPENSATION_EXPORT
#define SOTIMUOFFSETCOMPENSATION_EXPORT
Definition: imu_offset_compensation.hh:19
dynamicgraph::sot::torque_control::ImuOffsetCompensation::m_acc_sum
Vector3 m_acc_sum
tmp variable to store the sum of the gyro measurements during update phase
Definition: imu_offset_compensation.hh:84