LeakyIntegrator.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/utils/clamp.h>
 
namespace mc_filter
{
template<typename VectorT>
struct LeakyIntegrator
{
  LeakyIntegrator() : integral_(VectorT::Zero()), rate_(0.1), saturation_(-1.) {}
 
  inline void add(const VectorT & value, double dt)
  {
    integral_ = (1. - rate_ * dt) * integral_ + dt * value;
    if(saturation_ > 0.) { utils::clampInPlace(integral_, -saturation_, saturation_); }
  }
 
  inline const VectorT & eval() const { return integral_; }
 
  inline double rate() const { return rate_; }
 
  inline void rate(double rate) { rate_ = rate; }
 
  inline void saturation(double s) { saturation_ = s; }
 
  inline void reset() { integral_.setZero(); }
 
private:
  VectorT integral_;
  double rate_;
  double saturation_;
};
} // namespace mc_filter