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

Last change on this file was 18, checked in by wcaarls, 12 years ago

Updated extremum_seeking to revision 1177
File size: 5.0 KB
RevLine 
[5]1/*
2 * nn_esc_2d.cpp
3 *
4 *  Created on: Jul 31, 2012
5 *      Author: Berk Calli
6 *      Organization: Delft Biorobotics Lab., Delft University of Technology
7 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
8 *
9 * Class for two dimensional neural network extremum seeking control
10 *
11 * * References:
12 * - M. Teixeira and S. Zak, “Analog neural nonderivative optimizers,” IEEE Transactions on Neural Networks, vol. 9, pp. 629–638, 1998.
13 * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications", IROS 2012.
14 */
15#include "esc_nn/nn_esc_2d.h"
16
17NNESC2D::NNESC2D(){
18        M_ = 0;
19        A_ = 0;
20        ddelta1_ = 0;
21        ddelta2_ = 0;
[13]22        ddelta3_ = 0;
[5]23        delta_ = 0;
24        B_ = 0;
25        w_switch_old_ = 0;
26        a_switch1_old_ = 0;
27        a_switch2_old_ = 0;
28        a_switch3_old_ = 0;
29        yr_ = 0;
30        period_ = 0;
31        min_peak_ = 0;
32        vel_ref_.resize(2);
[18]33        vel_ref_old_.resize(2);
[5]34        vel_ref_[0] = 0;
35        vel_ref_[1] = 0;
36        w_switch_ = 0;
37        min_peak_detect_init_ = false;
38        initialized_ = false;
39}
40
41ESC::inputType NNESC2D::getInputType(){
42        return inputValue;
43}
44
45ESC::outputType NNESC2D::getOutputType(){
46        return outputVelocity;
47}
48
49std::vector<double> NNESC2D::monitor(){
50        std::vector<double> monitor_vals;
51        monitor_vals.push_back(yr_);
52        monitor_vals.push_back(min_peak_);
53        monitor_vals.push_back(w_switch_);
[13]54        monitor_vals.push_back(yr_+ddelta1_);
55        monitor_vals.push_back(yr_+ddelta2_);
56        monitor_vals.push_back(yr_+ddelta3_);
57        monitor_vals.push_back(yr_+delta_);
[5]58
59        return monitor_vals;
60
61}
62
63std::vector<std::string> NNESC2D::monitorNames(){
64        std::vector<std::string> monitor_names;
65        monitor_names.push_back("driving input value");
66        monitor_names.push_back("minimum peak detector output");
67        monitor_names.push_back("w switch value");
[13]68        monitor_names.push_back("threshold value 1");
69        monitor_names.push_back("threshold value 2");
70        monitor_names.push_back("threshold value 3");
71        monitor_names.push_back("threshold value 4");
[5]72
73        return monitor_names;
74}
75
[18]76NNESC2D::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){
77        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period, stopping_cycle_number, stoping_min_val);
[5]78}
79
[18]80void 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){
[5]81        A_ = A;
82        M_ = M;
83        B_ = B;
84        ddelta1_ = ddelta1;
85        ddelta2_ = ddelta2;
[13]86        ddelta3_ = ddelta3;
[5]87        delta_ = delta;
88        period_ = period;
[18]89        reset();
[5]90        initialized_ = true;
[18]91        stopping_cycle_number_ = stopping_cycle_number;
92        stoping_min_val_ = stoping_min_val;
[5]93}
94
95std::vector<double> NNESC2D::step(double obj_val){
96        if (!initialized_){
97                fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
98                return std::vector<double>();
99        }
100
101        if(!min_peak_detect_init_){
[13]102                min_peak_ = obj_val;
103                yr_ = min_peak_;
[5]104                min_peak_detect_init_ = true;
[18]105                obj_val_cycle_init_ = obj_val;
106                vel_ref_old_[0] = vel_ref_[0];
107                vel_ref_old_[1] = vel_ref_[1];
[5]108        }
109
110        double e = yr_ - obj_val;
[13]111        double s[3];
112        s[0] = aSwitch1(e);
113        s[1] = aSwitch2(e);
114        s[2] = aSwitch3(e);
115
116        vel_ref_[1] = s[0]+s[1];
117        vel_ref_[0] = s[1]+s[2];
118
119        min_peak_ = minPeakDetect(e);
120        w_switch_ = wSwitch(e);
[5]121        yr_ = yr_ + (w_switch_+min_peak_)*period_;
[13]122
[18]123        if((vel_ref_old_[0] != vel_ref_[0] || vel_ref_old_[1] != vel_ref_[1]) && vel_ref_[1] == A_ && vel_ref_[0] == 0){
124                if(obj_val_cycle_init_ - obj_val < stoping_min_val_){
125                        nn_cycle_count_++;
126                }
127                else
128                        nn_cycle_count_ = 0;
129                obj_val_cycle_init_ = obj_val;
130        }
131        //printf("[nn_esc_2d]: nn_cycle_count = %d \n",nn_cycle_count_);
132        vel_ref_old_[0] = vel_ref_[0];
133        vel_ref_old_[1] = vel_ref_[1];
134
[5]135        return vel_ref_;
136}
[13]137double NNESC2D::wSwitch(double e){
138        if(e>delta_){
[5]139                w_switch_old_ = 0;
140                return 0;
141        }
[13]142        else if(e<-delta_){
[5]143                w_switch_old_ = B_;
144                return B_;
145        }
146        else
147                return w_switch_old_;
148
149}
150
[13]151double NNESC2D::minPeakDetect(double e){
152        if(e<=0)
[5]153                return 0;
154        else
155                return -M_;
156}
157
158double NNESC2D::aSwitch1(double e){
159        if( e < -ddelta1_ ){
160                a_switch1_old_ = -A_;
161                return -A_;
162        }
163        else if(e>ddelta1_){
164                a_switch1_old_ = A_;
165                return A_;
166        }
167        else
168                return a_switch1_old_;
169}
170
171double NNESC2D::aSwitch2(double e){
[13]172
[5]173        if( e < -ddelta2_ ){
[13]174                a_switch2_old_ = A_;
175                return A_;
176
177        }
178        else if(e>ddelta2_){
[5]179                a_switch2_old_ = 0;
180                return 0;
181        }
182        else
183                return a_switch2_old_;
184}
185
186double NNESC2D::aSwitch3(double e){
187        if( e < -ddelta3_ ){
188                a_switch3_old_ = -2*A_;
189                return -2*A_;
190        }
191        else if(e>ddelta3_){
192                a_switch3_old_ = 0;
193                return 0;
194        }
195        else
196                return a_switch3_old_;
197}
[13]198
199void NNESC2D::reset(){
200        w_switch_old_ = 0;
201        a_switch1_old_ = A_;
202        a_switch2_old_ = 0;
203        a_switch3_old_ = 0;
204        vel_ref_.resize(2);
205        vel_ref_[0] = 0;
206        vel_ref_[1] = 0;
[18]207        vel_ref_old_.resize(2);
208        vel_ref_old_[0] = 0;
209        vel_ref_old_[1] = 0;
[13]210        yr_ = 0;
211        min_peak_ = 0;
212        w_switch_ = 0;
[18]213        nn_cycle_count_ = 0;
[13]214        min_peak_detect_init_ = false;
215}
[18]216
217bool NNESC2D::isStoppingConditionsMet(){
218        if(stopping_cycle_number_ <= 0)
219                return false;
220        else if(nn_cycle_count_ > stopping_cycle_number_){
221                return true;
222        }
223        else
224                return false;
225}
Note: See TracBrowser for help on using the repository browser.