5 #ifndef __sot_torque_control_imu_offset_compensation_H__
6 #define __sot_torque_control_imu_offset_compensation_H__
13 #if defined(imu_offset_compensation_EXPORTS)
14 #define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
16 #define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
19 #define SOTIMUOFFSETCOMPENSATION_EXPORT
28 #include <pinocchio/fwd.hpp>
30 #include <boost/assign.hpp>
32 #include <dynamic-graph/signal-helper.h>
33 #include <sot/core/matrix-geometry.hh>
34 #include <sot/core/robot-utils.hh>
48 DYNAMIC_GRAPH_ENTITY_DECL();
49 typedef Eigen::Vector3d
Vector3;
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 void init(
const double& dt);
59 void update_offset(
const double& duration);
60 void setGyroDCBlockerParameter(
const double& alpha);
62 DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector);
63 DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector);
64 DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector);
65 DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector);
69 virtual void display(std::ostream& os)
const;
72 void update_offset_impl(
int iter);
73 void sendMsg(
const std::string& msg, MsgType t = MSG_TYPE_INFO,
const char* file =
"",
int line = 0) {
74 logger_.stream(t) << (
"[ImuOffsetCompensation-" + name +
"] " + msg, t, file, line);
83 Vector3 m_gyro_offset;
95 #endif // #ifndef __sot_torque_control_imu_offset_compensation_H__