Changeset 13 for trunk


Ignore:
Timestamp:
08/29/12 16:52:52 (12 years ago)
Author:
wcaarls
Message:

Updated extremum_seeking to revision 1017

Location:
trunk/extremum_seeking
Files:
5 added
14 edited

Legend:

Unmodified
Added
Removed

  • trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_1d.h

    r5 r13  
    3737        std::vector<double> monitor();
    3838        std::vector<std::string> monitorNames();
     39        void reset();
    3940
    4041};

  • trunk/extremum_seeking/esc_approx/include/esc_approx/approx_esc_2d.h

    r5 r13  
    4040        std::vector<double> monitor();
    4141        std::vector<std::string> monitorNames();
     42        void reset();
    4243
    4344};

  • trunk/extremum_seeking/esc_approx/src/approx_esc_1d.cpp

    r5 r13  
    3535        k_grad_ = k_grad;
    3636        init_vel_ = init_vel;
    37         sample_ = 0;
    3837        sampling_ = sampling;
    39         ptr_ = 0;
    4038        states_.resize(data_size);
    4139        obj_vals_.resize(data_size);
     40        sample_ = 0;
     41        ptr_ = 0;
    4242        initialized_ = true;
    4343}
    … …  
    111111        return std::vector<std::string>();
    112112}
     113
     114void ApproxESC1D::reset(){
     115        sample_ = 0;
     116        ptr_ = 0;
     117}

  • trunk/extremum_seeking/esc_approx/src/approx_esc_2d.cpp

    r5 r13  
    3333        k_grad_ = k_grad;
    3434        init_vel_ = init_vel;
     35        sampling_ = sampling;
    3536        sample_ = 0;
    36         sampling_ = sampling;
    3737        ptr_ = 0;
    38         states_ = Eigen::MatrixXf::Zero(2,data_size);
     38        states_ = Eigen::MatrixXf::Zero(2,data_size_);
    3939        obj_vals_ = Eigen::VectorXf::Zero(data_size_);
    4040        state_curr_.resize(2);
    … …  
    133133        return std::vector<std::string>();
    134134}
     135
     136void ApproxESC2D::reset(){
     137        sample_ = 0;
     138        ptr_ = 0;
     139        states_ = Eigen::MatrixXf::Zero(2,data_size_);
     140        obj_vals_ = Eigen::VectorXf::Zero(data_size_);
     141}

  • trunk/extremum_seeking/esc_common/include/esc_common/esc.h

    r5 r13  
    1515#include <string>
    1616#define PI 3.141592654
     17
     18/// Superclass for extremum seeking control algorithms.
    1719class ESC
    1820{
    19 public:
    20 
    21         enum inputType { inputStateValue, inputValue };
    22         enum outputType { outputVelocity, outputPosition };
     21  public:
     22    /// Controller input type
     23    enum inputType
     24    {
     25      inputStateValue, ///< State-value input.
     26      inputValue       ///< Value input.
     27    };
     28       
     29    /// Controller output type.
     30    enum outputType
     31    {
     32      outputVelocity,  ///< Velocity reference output.
     33      outputPosition   ///< Position reference output.
     34    };
    2335
    2436public:
    25         virtual ~ESC() { }
     37    virtual ~ESC() { }
    2638
    27         virtual std::vector<std::string> monitorNames() { return std::vector<std::string>(); }
    28         virtual std::vector<double> monitor() { return std::vector<double>(); }
     39    /// Get internal monitor variable names.
     40    virtual std::vector<std::string> monitorNames() { return std::vector<std::string>(); }
     41       
     42    /// Get internal monitor variables.
     43    virtual std::vector<double> monitor() { return std::vector<double>(); }
    2944
    30         virtual inputType getInputType() = 0;
    31         virtual outputType getOutputType() = 0;
     45    /// Get controller input type.
     46    virtual inputType getInputType() = 0;
     47       
     48    /// Get controller output type.
     49    virtual outputType getOutputType() = 0;
    3250
    33         virtual std::vector<double> step(std::vector<double> state, double obj_val)
    34         {
    35                 return step(obj_val);
    36         }
     51    /// Control step function for value-input control algorithms.
     52    virtual std::vector<double> step(std::vector<double> state, double obj_val)
     53    {
     54      return step(obj_val);
     55    }
    3756
    38         virtual std::vector<double> step(double obj_val)
    39         {
    40                 return std::vector<double>();
    41         }
     57    /// Control step function for state-input control algorithms.
     58    virtual std::vector<double> step(double obj_val)
     59    {
     60      return std::vector<double>();
     61    }
     62
     63    /// Reset control algorithm to initial conditions.
     64    virtual void reset() = 0;
    4265};
    4366
    44 
    4567#endif /* ESC_H_ */

  • trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_1d.h

    r5 r13  
    4747        double minPeakDetect(double e_minus);
    4848        double aSwitch(double e);
     49        void reset();
    4950
    5051};

  • trunk/extremum_seeking/esc_nn/include/esc_nn/nn_esc_2d.h

    r5 r13  
    2525
    2626protected:
    27         double A_,M_,ddelta1_,ddelta2_,ddelta3_,A,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
     27        double A_,M_,ddelta1_,ddelta2_,ddelta3_,delta_,B_, mpd_init_, w_switch_old_, a_switch1_old_, a_switch2_old_, a_switch3_old_,yr_,period_;
    2828        double min_peak_,w_switch_;
    2929        std::vector<double> vel_ref_;
    … …  
    4242        std::vector<double> monitor();
    4343        std::vector<std::string> monitorNames();
     44        void reset();
    4445protected:
    45         double wSwitch(double e_minus);
    46         double minPeakDetect(double e_minus);
     46        double wSwitch(double e);
     47        double minPeakDetect(double e);
    4748        double aSwitch1(double e);
    4849        double aSwitch2(double e);

  • trunk/extremum_seeking/esc_nn/src/nn_esc_1d.cpp

    r5 r13  
    134134
    135135}
     136
     137void NNESC1D::reset(){
     138        w_switch_old_ = 0;
     139        a_switch_old_ = A;
     140        yr_ = 0;
     141        min_peak_ = 0;
     142        vel_ref_ = 0;
     143        w_switch_ = 0;
     144        min_peak_detect_init_ = false;
     145}

  • trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

    r5 r13  
    2020        ddelta1_ = 0;
    2121        ddelta2_ = 0;
    22         ddelta2_ = 0;
     22        ddelta3_ = 0;
    2323        delta_ = 0;
    2424        B_ = 0;
    … …  
    5151        monitor_vals.push_back(min_peak_);
    5252        monitor_vals.push_back(w_switch_);
     53        monitor_vals.push_back(yr_+ddelta1_);
     54        monitor_vals.push_back(yr_+ddelta2_);
     55        monitor_vals.push_back(yr_+ddelta3_);
     56        monitor_vals.push_back(yr_+delta_);
    5357
    5458        return monitor_vals;
    … …  
    6165        monitor_names.push_back("minimum peak detector output");
    6266        monitor_names.push_back("w switch value");
     67        monitor_names.push_back("threshold value 1");
     68        monitor_names.push_back("threshold value 2");
     69        monitor_names.push_back("threshold value 3");
     70        monitor_names.push_back("threshold value 4");
    6371
    6472        return monitor_names;
    … …  
    7583        ddelta1_ = ddelta1;
    7684        ddelta2_ = ddelta2;
    77         ddelta2_ = ddelta3;
     85        ddelta3_ = ddelta3;
    7886        delta_ = delta;
    7987        period_ = period;
    8088        w_switch_old_ = 0;
    8189        a_switch1_old_ = A_;
    82         a_switch2_old_ = A_;
     90        a_switch2_old_ = 0;
    8391        a_switch3_old_ = 0;
    8492        vel_ref_.resize(2);
    … …  
    99107
    100108        if(!min_peak_detect_init_){
    101                 yr_ = obj_val;
     109                min_peak_ = obj_val;
     110                yr_ = min_peak_;
    102111                min_peak_detect_init_ = true;
    103112        }
    104113
    105114        double e = yr_ - obj_val;
    106         vel_ref_[1] = aSwitch1(e)+aSwitch2(e);
    107         vel_ref_[0] = aSwitch2(e)+aSwitch3(e);
    108         min_peak_ = minPeakDetect(-e);
    109         w_switch_ = wSwitch(-e);
     115        double s[3];
     116        s[0] = aSwitch1(e);
     117        s[1] = aSwitch2(e);
     118        s[2] = aSwitch3(e);
     119
     120        vel_ref_[1] = s[0]+s[1];
     121        vel_ref_[0] = s[1]+s[2];
     122
     123        min_peak_ = minPeakDetect(e);
     124        w_switch_ = wSwitch(e);
    110125        yr_ = yr_ + (w_switch_+min_peak_)*period_;
     126
    111127        return vel_ref_;
    112128}
    113 double NNESC2D::wSwitch(double e_minus){
    114         if(e_minus<-delta_){
     129double NNESC2D::wSwitch(double e){
     130        if(e>delta_){
    115131                w_switch_old_ = 0;
    116132                return 0;
    117133        }
    118         else if(e_minus>delta_){
     134        else if(e<-delta_){
    119135                w_switch_old_ = B_;
    120136                return B_;
    … …  
    125141}
    126142
    127 double NNESC2D::minPeakDetect(double e_minus){
    128         if(e_minus>0)
     143double NNESC2D::minPeakDetect(double e){
     144        if(e<=0)
    129145                return 0;
    130146        else
    … …  
    146162
    147163double NNESC2D::aSwitch2(double e){
     164
    148165        if( e < -ddelta2_ ){
    149                 a_switch2_old_ = 0;
    150                 return 0;
    151         }
    152         else if(e>ddelta2_){
    153166                a_switch2_old_ = A_;
    154167                return A_;
     168
     169        }
     170        else if(e>ddelta2_){
     171                a_switch2_old_ = 0;
     172                return 0;
    155173        }
    156174        else
    … …  
    170188                return a_switch3_old_;
    171189}
     190
     191void NNESC2D::reset(){
     192        w_switch_old_ = 0;
     193        a_switch1_old_ = A_;
     194        a_switch2_old_ = 0;
     195        a_switch3_old_ = 0;
     196        vel_ref_.resize(2);
     197        vel_ref_[0] = 0;
     198        vel_ref_[1] = 0;
     199        yr_ = 0;
     200        min_peak_ = 0;
     201        w_switch_ = 0;
     202        min_peak_detect_init_ = false;
     203        initialized_ = true;
     204}

  • trunk/extremum_seeking/esc_sm/include/esc_sm/sm_esc_1d.h

    r5 r13  
    3636        std::vector<double> monitor();
    3737        std::vector<std::string> monitorNames();
     38        void reset();
    3839protected:
    3940        int sign(double value);

  • trunk/extremum_seeking/esc_sm/src/sm_esc_1d.cpp

    r5 r13  
    8787}
    8888
     89void SMESC1D::reset(){
     90        driving_input_ = 0;
     91        driving_input_init_ = false;
     92}

  • trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h

    r11 r13  
    1111   
    1212    ESCSystem *system_;
     13    std::vector<float> vel_, pos_;
    1314   
    1415  protected:

  • trunk/extremum_seeking/esc_test/include/esc_test/function.h

    r11 r13  
    99};
    1010
    11 class Gauss : public ESCFunction
     11class Gauss1D : public ESCFunction
    1212{
    1313  private:
    14     float a_, b_, c_;
     14    float a_, b_, c_, d_;
    1515
    1616  public:
    17     Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { }
     17    Gauss1D(float a=0, float b=1, float c=0, float d=1) : a_(a), b_(b), c_(c), d_(d) { }
    1818   
    1919    std::vector<float> init()
    … …  
    2929        throw std::runtime_error("invalid state size");
    3030       
    31       return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_));
     31      return a_ + b_*std::exp(-(state[0]-c_)*(state[0]-c_)/(2*d_*d_));
    3232    }
    3333};
     34
     35class Gauss2D : public ESCFunction
     36{
     37  private:
     38    float a_, b_;
     39    std::vector<float> c_, d_;
     40
     41  public:
     42    Gauss2D(float a, float b, std::vector<float> c, std::vector<float> d)
     43    {
     44      if (c.empty())
     45      {
     46        c_.push_back(0);
     47        c_.push_back(0);
     48      }
     49      else
     50        c_ = c;
     51       
     52      if (d.empty())
     53      {
     54        d_.push_back(1);
     55        d_.push_back(1);
     56      }
     57      else
     58        d_ = d;
     59    }
     60   
     61    std::vector<float> init()
     62    {
     63      std::vector<float> state;
     64      state.push_back(0);
     65      state.push_back(0);
     66      return state;
     67    }
     68   
     69    float value(const std::vector<float> &state) const
     70    {
     71      if (state.size() != 1)
     72        throw std::runtime_error("invalid state size");
     73     
     74      double exponent = (state[0]-c_[0])*(state[0]-c_[0])/(2*d_[0]*d_[0]) +
     75                        (state[1]-c_[1])*(state[1]-c_[1])/(2*d_[1]*d_[1]);
     76     
     77      return a_ + b_*std::exp(-exponent);
     78    }
     79};
     80
    3481
    3582class ESCSystem

  • trunk/extremum_seeking/esc_test/src/esc_test.cpp

    r11 r13  
    77void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
    88{
    9   system_->step(msg->data);
    10   publish();
     9  vel_ = msg->data;
     10  pos_.clear();
    1111}
    1212
    1313void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
    1414{
    15   system_->set(msg->data);
    16   publish();
     15  pos_ = msg->data;
     16  vel_.clear();
    1717}
    1818
    … …  
    3939  vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
    4040  pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
    41 
    42   publish(); 
    4341}
    4442
    … …  
    4745  ROS_INFO("Spinning");
    4846
    49   ros::spin();
     47  ros::Rate loop_rate(100);
     48  while (nh_.ok())
     49  {
     50    if (!vel_.empty())
     51      system_->step(vel_);
     52    else if (!pos_.empty())
     53      system_->set(pos_);
     54    publish();
     55
     56    loop_rate.sleep();
     57    ros::spinOnce();
     58  } 
    5059}
    5160
    … …  
    5463  ros::init(argc, argv, "esc_test");
    5564 
     65  ESCFunction *function;
     66 
     67  if (argc > 1 && strcmp(argv[1], "-2") == 0)
     68  {
     69    std::vector<float> mean, sigma;
     70    mean.push_back(0.5);
     71    mean.push_back(0.2);
     72    sigma.push_back(0.15);
     73    sigma.push_back(0.15);
     74    function = new Gauss2D(2, -1, mean, sigma);
     75  }
     76  else
     77    function = new Gauss1D(2.5, -1.5, 0.4, 0.27/sqrt(2.0));
     78 
    5679  ESCTest test;
    5780 
    58   test.init(new ESCSystem(new Gauss(-1, 2, 1)));
     81  test.init(new ESCSystem(function));
    5982  test.spin();
    6083 
Note: See TracChangeset for help on using the changeset viewer.