sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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 <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>
37
38namespace dynamicgraph {
39namespace sot {
40namespace torque_control {
41
42/* --------------------------------------------------------------------- */
43/* --- CLASS ----------------------------------------------------------- */
44/* --------------------------------------------------------------------- */
45
47 : public ::dynamicgraph::Entity {
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);
65 DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector);
66 DECLARE_SIGNAL_OUT(accelerometer_out,
67 dynamicgraph::Vector);
68 DECLARE_SIGNAL_OUT(gyrometer_out,
69 dynamicgraph::Vector);
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
86 float m_dt;
92 Vector3 m_gyro_offset;
93 Vector3 m_acc_offset;
94
95 Vector3 m_gyro_sum;
97 Vector3 m_acc_sum;
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__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector)
raw gyrometer data
float m_dt
true if the entity has been successfully initialized
double m_a_gyro_DC_blocker
total number of update cycles to perform
DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector)
compensated accelerometer data
DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector)
raw accelerometer data
DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector)
#define SOTIMUOFFSETCOMPENSATION_EXPORT
to read text file
Definition treeview.dox:22