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 26 #include <pinocchio/fwd.hpp> 30 #include <dynamic-graph/signal-helper.h> 32 #include <boost/assign.hpp> 34 #include <sot/core/matrix-geometry.hh> 35 #include <sot/core/robot-utils.hh> 47 :
public ::dynamicgraph::Entity {
49 DYNAMIC_GRAPH_ENTITY_DECL();
50 typedef Eigen::Vector3d
Vector3;
53 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59 void init(
const double& dt);
60 void update_offset(
const double& duration);
61 void setGyroDCBlockerParameter(
const double& alpha);
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);
73 virtual void display(std::ostream& os)
const;
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,
90 Vector3 m_gyro_offset;
105 #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
Vector3 m_gyro_sum
accelerometer offset
Eigen::Matrix< double, 3, 1 > Vector3
int m_update_cycles
number of update cycles left
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *file="", int line=0)
AdmittanceController EntityClassName
float m_dt
true if the entity has been successfully initialized
Vector3 m_acc_offset
gyrometer offset
int m_update_cycles_left
sampling time in seconds