sot-talos-balance  1.7.0
ft-calibration.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * LAAS-CNRS
4  * F. Bailly
5  * T. Flayols
6  */
7 
8 #ifndef __sot_talos_balance_ft_calibration_H__
9 #define __sot_talos_balance_ft_calibration_H__
10 
11 /* --------------------------------------------------------------------- */
12 /* --- API ------------------------------------------------------------- */
13 /* --------------------------------------------------------------------- */
14 
15 #if defined (WIN32)
16 # if defined (__sot_talos_balance_ft_calibration_H__)
17 # define SOTFTCALIBRATION_EXPORT __declspec(dllexport)
18 # else
19 # define SOTFTCALIBRATION_EXPORT __declspec(dllimport)
20 # endif
21 #else
22 # define SOTFTCALIBRATION_EXPORT
23 #endif
24 
25 
26 /* --------------------------------------------------------------------- */
27 /* --- INCLUDE --------------------------------------------------------- */
28 /* --------------------------------------------------------------------- */
29 
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>
35 #include <map>
36 #include "boost/assign.hpp"
37 
38 
40 
41 namespace dynamicgraph {
42  namespace sot {
43  namespace talos_balance {
44 
45  /* --------------------------------------------------------------------- */
46  /* --- CLASS ----------------------------------------------------------- */
47  /* --------------------------------------------------------------------- */
48 
50  :public::dynamicgraph::Entity
51  {
52  //typedef FtCalibration EntityClassName;
53  DYNAMIC_GRAPH_ENTITY_DECL();
54 
55  public:
56  /* --- CONSTRUCTOR ---- */
57  FtCalibration( const std::string & name);
59  void init(const std::string & robotRef);
60 
61  /* --- SIGNALS --- */
62  DECLARE_SIGNAL_IN(right_foot_force_in, dynamicgraph::Vector);
63  DECLARE_SIGNAL_IN(left_foot_force_in, dynamicgraph::Vector);
64  DECLARE_SIGNAL_OUT(right_foot_force_out, dynamicgraph::Vector);
65  DECLARE_SIGNAL_OUT(left_foot_force_out, dynamicgraph::Vector);
66 
67  /* --- COMMANDS --- */
68 
70  void setRightFootWeight(const double &rightW);
71  void setLeftFootWeight(const double &leftW);
72 
74  void calibrateFeetSensor();
75 
76  void displayRobotUtil();
77 
78  /* --- ENTITY INHERITANCE --- */
79  virtual void display( std::ostream& os ) const;
80 
81  /* --- TYPEDEFS ---- */
82  typedef Eigen::Matrix<double, 6, 1> Vector6d;
83 
84  protected:
85  RobotUtilShrPtr m_robot_util;
86  int m_right_calibration_iter = -1;
87  int m_left_calibration_iter = -1;
88  Vector6d m_right_FT_offset;
89  Vector6d m_left_FT_offset;
93  Vector6d m_right_foot_weight; // weight of the right feet underneath the ft sensor
94  Vector6d m_left_foot_weight; // weight of the left feet underneath the ft sensor
95 
96  }; // class FtCalibration
97 
98  } // namespace talos_balance
99  } // namespace sot
100 } // namespace dynamicgraph
101 
102 
103 
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
Definition: math/fwd.hh:40
Vector6d m_right_foot_weight
true if the entity has been successfully initialized
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