dg_tools
calibrator.hpp
1 
9 #ifndef __SOT_Calibrator_HH__
10 #define __SOT_Calibrator_HH__
11 
12 /* --------------------------------------------------------------------- */
13 /* --- INCLUDE --------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 /* Matrix */
17 #include <dynamic-graph/linear-algebra.h>
18 namespace dg = dynamicgraph;
19 
20 /* SOT */
21 #include <dynamic-graph/signal-time-dependent.h>
22 #include <dynamic-graph/signal-ptr.h>
23 #include <dynamic-graph/entity.h>
24 
25 
26 /* --------------------------------------------------------------------- */
27 /* --- API ------------------------------------------------------------- */
28 /* --------------------------------------------------------------------- */
29 
30 #if defined (WIN32)
31 # if defined (calibrator_EXPORTS)
32 # define Calibrator_EXPORT __declspec(dllexport)
33 # else
34 # define Calibrator_EXPORT __declspec(dllimport)
35 # endif
36 #else
37 # define Calibrator_EXPORT
38 #endif
39 
40 namespace dynamicgraph {
41  namespace sot {
42 
43  /* --------------------------------------------------------------------- */
44  /* --- CLASS ----------------------------------------------------------- */
45  /* --------------------------------------------------------------------- */
46 
47  class Calibrator_EXPORT Calibrator
48  : public Entity
49  {
50 
51  public: /* --- CONSTRUCTOR ---- */
52 
53  Calibrator( const std::string & name );
54 
55  public: /* --- CONSTANTS --- */
56 
57  /* Default values. */
58  static const double TIME_STEP_DEFAULT; // = 0.001
59 
60  public: /* --- ENTITY INHERITANCE --- */
61  static const std::string CLASS_NAME;
62  virtual void display( std::ostream& os ) const;
63  virtual const std::string& getClassName( void ) const
64  {return CLASS_NAME;}
65 
66 
67  protected:
68 
69  double TimeStep;
70  double _dimension;
71 
72  public: /* --- SIGNALS --- */
73  SignalPtr<dg::Vector, int> positionSIN;
74  SignalPtr<dg::Vector, int> velocitySIN;
75  SignalPtr<dg::Vector, int> calibration_torqueSIN;
76  // this should be the offset that brings your legs to 0 configuration
77  SignalPtr<dg::Vector, int> hardstop2zeroSIN;
78 
79 
80  // OUTPUT SIGNALS
81  SignalTimeDependent<dg::Vector, int> positionSOUT;
82  // Only used during calibration part
83  SignalTimeDependent<dg::Vector, int> controlSOUT;
84  SignalTimeDependent<int, int> calibrated_flagSOUT;
85 
86  protected:
87 
88  // signal refresher can be used to trigger SOUTs that do not depend on
89  // any SINs.
90  SignalTimeDependent<int, int> internal_signal_refresher_;
91 
92  double& setsize(int dimension);
93  int threshold_time; // in timesteps
94  double threshold_velocity;
95  int t_start;
96  int init_flag;
97  int num_joints;
98  dg::Vector& calibrate( dg::Vector& tau, int t );
99  dg::Vector& compute_position( dg::Vector& pos, int t);
100  int& is_calibrated( int& calibrated_flag, int t);
101  int calibrated_flag_;
102  dg::Vector calibrated;
103  dg::Vector des_vel;
104  dg::Vector error;
105  dg::Vector start2hardstop; // this is recorded during calibration
106 
107  };
108 
109 
110 
111 } // namespace sot
112 } // namespace dynamicgraph
113 
114 
115 
116 #endif // #ifndef __SOT_Calibrator_HH__
dynamicgraph::sot::Calibrator
Definition: calibrator.hpp:47
dynamicgraph
Definition: ComImpedanceController.hpp:31