StationaryOffset.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_filter/ExponentialMovingAverage.h>
 
namespace mc_filter
{
template<typename VectorT>
struct StationaryOffset
{
  StationaryOffset(double dt, double timeConstant, const VectorT & initValue = VectorT::Zero())
  : average_(dt, timeConstant, initValue)
  {
    this->reset(initValue);
  }
 
  void update(const VectorT & value)
  {
    average_.append(value);
    filteredValue_ = value - average_.eval();
    if(saturation_ > 0.) { utils::clampInPlace(filteredValue_, -saturation_, saturation_); }
  }
 
  const VectorT & eval() const { return filteredValue_; }
 
  void reset(const VectorT & initValue)
  {
    average_.reset(initValue);
    filteredValue_ = initValue;
  }
 
  double timeConstant() const { return average_.timeConstant(); }
 
  void timeConstant(double T) { average_.timeConstant(T); }
 
  void saturation(double limit) { saturation_ = limit; }
 
private:
  VectorT filteredValue_ = VectorT::Zero();
  ExponentialMovingAverage<VectorT> average_;
  double saturation_ = -1;
};
} // namespace mc_filter