sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
numerical-difference.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_NumericalDifference_H__
7 #define __sot_torque_control_NumericalDifference_H__
8 /* --------------------------------------------------------------------- */
9 /* --- API ------------------------------------------------------------- */
10 /* --------------------------------------------------------------------- */
11 
12 #if defined(WIN32)
13 #if defined(numerical_difference_EXPORTS)
14 #define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllexport)
15 #else
16 #define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllimport)
17 #endif
18 #else
19 #define SOTNUMERICALDIFFERENCE_EXPORT
20 #endif
21 
22 //#define VP_DEBUG 1 /// enable debug output
23 //#define VP_DEBUG_MODE 20
24 
25 /* --------------------------------------------------------------------- */
26 /* --- INCLUDE --------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 /* HELPER */
30 #include <boost/circular_buffer.hpp>
31 #include <dynamic-graph/signal-helper.h>
32 #include <sot/core/matrix-geometry.hh>
34 #include <sot/core/stop-watch.hh>
35 
36 /* Polynomial estimators */
39 
40 namespace dynamicgraph {
41 namespace sot {
42 namespace torque_control {
43 
47 class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph::Entity {
48  DYNAMIC_GRAPH_ENTITY_DECL();
49 
50  public: /* --- SIGNALS --- */
51  DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
52  DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
53  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
54  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
55 
62 
64  DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
65 
66  protected:
67  double m_dt;
68  double m_delay;
69  int x_size;
72  std::vector<double> m_ddx_filter_std;
73  std::vector<double> m_dx_filter_std;
74  std::vector<double> m_x_filter_std;
75  std::vector<double> m_x_std;
76 
79 
80  public:
81  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 
84  NumericalDifference(const std::string& name);
85 
95  void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder);
96 
97  protected:
98  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
99  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
100  }
101 
102  public: /* --- ENTITY INHERITANCE --- */
103  virtual void display(std::ostream& os) const;
104 
105 }; // class NumericalDifference
106 
107 } // namespace torque_control
108 } // namespace sot
109 } // namespace dynamicgraph
110 
111 #endif // #ifndef __sot_torque_control_NumericalDifference_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector)
This signal contains the estimated positions, velocities and accelerations.
DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector)
double m_delay
sampling timestep of the input signal
to read text file
Definition: treeview.dox:22
#define SOTNUMERICALDIFFERENCE_EXPORT