LowPassFiniteDifferences.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#include <mc_filter/LowPass.h>
 
namespace mc_filter
{
template<typename T>
struct LowPassFiniteDifferences : public LowPass<T>
{
  using LowPassT = LowPass<T>;
 
  LowPassFiniteDifferences(double dt, double period) : LowPassT(dt, period) { LowPassT::reset(T::Zero()); }
 
  void reset(const T & pos, const T & vel)
  {
    LowPassT::reset(vel);
    prevValue_ = pos;
  }
 
  void update(const T & newPos)
  {
    T discVel = (newPos - prevValue_) / LowPassT::dt();
    LowPassT::update(discVel);
    prevValue_ = newPos;
  }
 
  const T & prevValue() const { return prevValue_; }
 
protected:
  T prevValue_;
 
private:
  // Prevent calling the single-argument reset from parent's LowPass<T>
  using LowPass<T>::reset;
};
} // namespace mc_filter