source: trunk/extremum_seeking/esc_nn/src/nn_esc_2d.cpp

/*
 * nn_esc_2d.cpp
 *
 *  Created on: Jul 31, 2012
 *      Author: Berk Calli
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 * Class for two dimensional neural network extremum seeking control
 *
 * * References:
 * - M. Teixeira and S. Zak, “Analog neural nonderivative optimizers,” IEEE Transactions on Neural Networks, vol. 9, pp. 629–638, 1998.
 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
 */
#include "esc_nn/nn_esc_2d.h"

NNESC2D::NNESC2D(){
        M_ = 0;
        A_ = 0;
        ddelta1_ = 0;
        ddelta2_ = 0;
        ddelta3_ = 0;
        delta_ = 0;
        B_ = 0;
        w_switch_old_ = 0;
        a_switch1_old_ = 0;
        a_switch2_old_ = 0;
        a_switch3_old_ = 0;
        yr_ = 0;
        period_ = 0;
        min_peak_ = 0;
        vel_ref_.resize(2);
        vel_ref_old_.resize(2);
        vel_ref_[0] = 0;
        vel_ref_[1] = 0;
        w_switch_ = 0;
        min_peak_detect_init_ = false;
        initialized_ = false;
}

ESC::inputType NNESC2D::getInputType(){
        return inputValue;
}

ESC::outputType NNESC2D::getOutputType(){
        return outputVelocity;
}

std::vector<double> NNESC2D::monitor(){
        std::vector<double> monitor_vals;
        monitor_vals.push_back(yr_);
        monitor_vals.push_back(min_peak_);
        monitor_vals.push_back(w_switch_);
        monitor_vals.push_back(yr_+ddelta1_);
        monitor_vals.push_back(yr_+ddelta2_);
        monitor_vals.push_back(yr_+ddelta3_);
        monitor_vals.push_back(yr_+delta_);

        return monitor_vals;

}

std::vector<std::string> NNESC2D::monitorNames(){
        std::vector<std::string> monitor_names;
        monitor_names.push_back("driving input value");
        monitor_names.push_back("minimum peak detector output");
        monitor_names.push_back("w switch value");
        monitor_names.push_back("threshold value 1");
        monitor_names.push_back("threshold value 2");
        monitor_names.push_back("threshold value 3");
        monitor_names.push_back("threshold value 4");

        return monitor_names;
}

NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period, stopping_cycle_number, stoping_min_val);
}

void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period, int stopping_cycle_number, double stoping_min_val){
        A_ = A;
        M_ = M;
        B_ = B;
        ddelta1_ = ddelta1;
        ddelta2_ = ddelta2;
        ddelta3_ = ddelta3;
        delta_ = delta;
        period_ = period;
        reset();
        initialized_ = true;
        stopping_cycle_number_ = stopping_cycle_number;
        stoping_min_val_ = stoping_min_val;
}

std::vector<double> NNESC2D::step(double obj_val){
        if (!initialized_){
                fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
                return std::vector<double>();
        }

        if(!min_peak_detect_init_){
                min_peak_ = obj_val;
                yr_ = min_peak_;
                min_peak_detect_init_ = true;
                obj_val_cycle_init_ = obj_val;
                vel_ref_old_[0] = vel_ref_[0];
                vel_ref_old_[1] = vel_ref_[1];
        }

        double e = yr_ - obj_val;
        double s[3];
        s[0] = aSwitch1(e);
        s[1] = aSwitch2(e);
        s[2] = aSwitch3(e);

        vel_ref_[1] = s[0]+s[1];
        vel_ref_[0] = s[1]+s[2];

        min_peak_ = minPeakDetect(e);
        w_switch_ = wSwitch(e);
        yr_ = yr_ + (w_switch_+min_peak_)*period_;

        if((vel_ref_old_[0] != vel_ref_[0] || vel_ref_old_[1] != vel_ref_[1]) && vel_ref_[1] == A_ && vel_ref_[0] == 0){
                if(obj_val_cycle_init_ - obj_val < stoping_min_val_){
                        nn_cycle_count_++;
                }
                else
                        nn_cycle_count_ = 0;
                obj_val_cycle_init_ = obj_val;
        }
        //printf("[nn_esc_2d]: nn_cycle_count = %d \n",nn_cycle_count_);
        vel_ref_old_[0] = vel_ref_[0];
        vel_ref_old_[1] = vel_ref_[1];

        return vel_ref_;
}
double NNESC2D::wSwitch(double e){
        if(e>delta_){
                w_switch_old_ = 0;
                return 0;
        }
        else if(e<-delta_){
                w_switch_old_ = B_;
                return B_;
        }
        else
                return w_switch_old_;

}

double NNESC2D::minPeakDetect(double e){
        if(e<=0)
                return 0;
        else
                return -M_;
}

double NNESC2D::aSwitch1(double e){
        if( e < -ddelta1_ ){
                a_switch1_old_ = -A_;
                return -A_;
        }
        else if(e>ddelta1_){
                a_switch1_old_ = A_;
                return A_;
        }
        else
                return a_switch1_old_;
}

double NNESC2D::aSwitch2(double e){

        if( e < -ddelta2_ ){
                a_switch2_old_ = A_;
                return A_;

        }
        else if(e>ddelta2_){
                a_switch2_old_ = 0;
                return 0;
        }
        else
                return a_switch2_old_;
}

double NNESC2D::aSwitch3(double e){
        if( e < -ddelta3_ ){
                a_switch3_old_ = -2*A_;
                return -2*A_;
        }
        else if(e>ddelta3_){
                a_switch3_old_ = 0;
                return 0;
        }
        else
                return a_switch3_old_;
}

void NNESC2D::reset(){
        w_switch_old_ = 0;
        a_switch1_old_ = A_;
        a_switch2_old_ = 0;
        a_switch3_old_ = 0;
        vel_ref_.resize(2);
        vel_ref_[0] = 0;
        vel_ref_[1] = 0;
        vel_ref_old_.resize(2);
        vel_ref_old_[0] = 0;
        vel_ref_old_[1] = 0;
        yr_ = 0;
        min_peak_ = 0;
        w_switch_ = 0;
        nn_cycle_count_ = 0;
        min_peak_detect_init_ = false;
}

bool NNESC2D::isStoppingConditionsMet(){
        if(stopping_cycle_number_ <= 0)
                return false;
        else if(nn_cycle_count_ > stopping_cycle_number_){
                return true;
        }
        else
                return false;
}