source: trunk/extremum_seeking/esc_test/include/esc_test/function.h @ 11

#include <limits>
#include <exception>

class ESCFunction
{
  public:
    virtual std::vector<float> init() = 0;
    virtual float value(const std::vector<float> &state) const = 0;
};

class Gauss : public ESCFunction
{
  private:
    float a_, b_, c_;

  public:
    Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { }
    
    std::vector<float> init()
    {
      std::vector<float> state;
      state.push_back(0);
      return state;
    }
    
    float value(const std::vector<float> &state) const
    {
      if (state.size() != 1)
        throw std::runtime_error("invalid state size");
        
      return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_));
    }
};

class ESCSystem
{
  protected:
    ESCFunction *function_;
    std::vector<float> state_;
        
  public:
    ESCSystem(ESCFunction *function) : function_(function)
    {
      if (!function)
        throw std::runtime_error("no function specified");
        
      reset();
    }
    
    void reset()
    {
      state_ = function_->init();
    }
  
    float step(const std::vector<float> &vel)
    {
      if (state_.size() != vel.size())
        throw std::runtime_error("invalid state size");
    
      for (size_t ii=0; ii < state_.size() && ii < vel.size(); ++ii)
        state_[ii] += vel[ii];
        
      return function_->value(state_);
    }
    
    float set(const std::vector<float> &pos)
    {
      if (state_.size() != pos.size())
        throw std::runtime_error("invalid state size");
    
      state_ = pos;
      return function_->value(state_);
    }
    
    float value() const
    {
      return function_->value(state_);
    }
    
    const std::vector<float> &state()
    {
      return state_;
    }
};