8 #ifndef __sot_talos_balance_ft_calibration_H__ 9 #define __sot_talos_balance_ft_calibration_H__ 16 # if defined (__sot_talos_balance_ft_calibration_H__) 17 # define SOTFTCALIBRATION_EXPORT __declspec(dllexport) 19 # define SOTFTCALIBRATION_EXPORT __declspec(dllimport) 22 # define SOTFTCALIBRATION_EXPORT 30 #include <pinocchio/fwd.hpp> 31 #include <dynamic-graph/signal-helper.h> 32 #include <dynamic-graph/real-time-logger.h> 33 #include <sot/core/matrix-geometry.hh> 34 #include <sot/core/robot-utils.hh> 36 #include "boost/assign.hpp" 43 namespace talos_balance {
50 :public::dynamicgraph::Entity
53 DYNAMIC_GRAPH_ENTITY_DECL();
59 void init(
const std::string & robotRef);
70 void setRightFootWeight(
const double &rightW);
71 void setLeftFootWeight(
const double &leftW);
74 void calibrateFeetSensor();
76 void displayRobotUtil();
79 virtual void display( std::ostream& os )
const;
86 int m_right_calibration_iter = -1;
87 int m_left_calibration_iter = -1;
104 #endif // #ifndef __sot_talos_balance_ft_calibration_H__ Vector6d m_right_FT_offset_calibration_sum
Offset or bias to be removed from Left FT sensor.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Vector6d m_left_foot_weight
Vector6d m_right_foot_weight
true if the entity has been successfully initialized
RobotUtilShrPtr m_robot_util
bool m_initSucceeded
Variable used durring average computation of the offset.
Vector6d m_left_FT_offset
Offset or bias to be removed from Right FT sensor.
Vector6d m_right_FT_offset
Number of iteration left for calibration (-1= not cailbrated, 0=caibration done)
Vector6d m_left_FT_offset_calibration_sum
Variable used durring average computation of the offset.
#define SOTFTCALIBRATION_EXPORT
Eigen::Matrix< double, 6, 1 > Vector6d