numerical-difference.hh

Go to the documentation of this file.

/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 */
 
#ifndef __sot_torque_control_NumericalDifference_H__
#define __sot_torque_control_NumericalDifference_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(numerical_difference_EXPORTS)
#define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllexport)
#else
#define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllimport)
#endif
#else
#define SOTNUMERICALDIFFERENCE_EXPORT
#endif
 
// #define VP_DEBUG 1        /// enable debug output
// #define VP_DEBUG_MODE 20
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* HELPER */
#include <dynamic-graph/signal-helper.h>
 
#include <boost/circular_buffer.hpp>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/stop-watch.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
 
/* Polynomial estimators */
#include <sot/torque_control/utils/lin-estimator.hh>
#include <sot/torque_control/utils/quad-estimator.hh>
 
namespace dynamicgraph {
namespace sot {
namespace torque_control {
 
class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference
    : public ::dynamicgraph::Entity {
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 public: /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
 
 
  DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
 
 protected:
  double m_dt;     
  double m_delay;  
  int x_size;
  std::vector<double> m_ddx_filter_std;  
  std::vector<double> m_dx_filter_std;   
  std::vector<double> m_x_filter_std;    
  std::vector<double> m_x_std;           
 
  PolyEstimator* m_filter;
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  NumericalDifference(const std::string& name);
 
  void init(const double& timestep, const int& sigSize, const double& delay,
            const int& polyOrder);
 
 protected:
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
               const char* = "", int = 0) {
    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
  }
 
 public: /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;
 
};  // class NumericalDifference
 
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __sot_torque_control_NumericalDifference_H__