sot-talos-balance  1.5.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 <dynamic-graph/signal-helper.h>
31 #include <dynamic-graph/real-time-logger.h>
32 #include <sot/core/matrix-geometry.hh>
33 #include <sot/core/robot-utils.hh>
34 #include <map>
35 #include "boost/assign.hpp"
36 
37 
39 
40 namespace dynamicgraph {
41  namespace sot {
42  namespace talos_balance {
43 
44  /* --------------------------------------------------------------------- */
45  /* --- CLASS ----------------------------------------------------------- */
46  /* --------------------------------------------------------------------- */
47 
49  :public::dynamicgraph::Entity
50  {
51  //typedef FtCalibration EntityClassName;
52  DYNAMIC_GRAPH_ENTITY_DECL();
53 
54  public:
55  /* --- CONSTRUCTOR ---- */
56  FtCalibration( const std::string & name);
58  void init(const std::string & robotRef);
59 
60  /* --- SIGNALS --- */
61  DECLARE_SIGNAL_IN(right_foot_force_in, dynamicgraph::Vector);
62  DECLARE_SIGNAL_IN(left_foot_force_in, dynamicgraph::Vector);
63  DECLARE_SIGNAL_OUT(right_foot_force_out, dynamicgraph::Vector);
64  DECLARE_SIGNAL_OUT(left_foot_force_out, dynamicgraph::Vector);
65 
66  /* --- COMMANDS --- */
67 
69  void setRightFootWeight(const double &rightW);
70  void setLeftFootWeight(const double &leftW);
71 
73  void calibrateFeetSensor();
74 
75  void displayRobotUtil();
76 
77  /* --- ENTITY INHERITANCE --- */
78  virtual void display( std::ostream& os ) const;
79 
80  /* --- TYPEDEFS ---- */
81  typedef Eigen::Matrix<double, 6, 1> Vector6d;
82 
83  protected:
84  RobotUtilShrPtr m_robot_util;
85  int m_right_calibration_iter = -1;
86  int m_left_calibration_iter = -1;
87  Vector6d m_right_FT_offset;
88  Vector6d m_left_FT_offset;
92  Vector6d m_right_foot_weight; // weight of the right feet underneath the ft sensor
93  Vector6d m_left_foot_weight; // weight of the left feet underneath the ft sensor
94 
95  }; // class FtCalibration
96 
97  } // namespace talos_balance
98  } // namespace sot
99 } // namespace dynamicgraph
100 
101 
102 
103 #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