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

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

Updated extremum_seeking to revision 1017

File size: 4.2 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);
33        vel_ref_[0] = 0;
34        vel_ref_[1] = 0;
35        w_switch_ = 0;
36        min_peak_detect_init_ = false;
37        initialized_ = false;
38}
39
40ESC::inputType NNESC2D::getInputType(){
41        return inputValue;
42}
43
44ESC::outputType NNESC2D::getOutputType(){
45        return outputVelocity;
46}
47
48std::vector<double> NNESC2D::monitor(){
49        std::vector<double> monitor_vals;
50        monitor_vals.push_back(yr_);
51        monitor_vals.push_back(min_peak_);
52        monitor_vals.push_back(w_switch_);
[13]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_);
[5]57
58        return monitor_vals;
59
60}
61
62std::vector<std::string> NNESC2D::monitorNames(){
63        std::vector<std::string> monitor_names;
64        monitor_names.push_back("driving input value");
65        monitor_names.push_back("minimum peak detector output");
66        monitor_names.push_back("w switch value");
[13]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");
[5]71
72        return monitor_names;
73}
74
75NNESC2D::NNESC2D(double A,double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
76        init(A, M, B, ddelta1, ddelta2, ddelta3, delta, period);
77}
78
79void NNESC2D::init(double A, double M, double B, double ddelta1, double ddelta2, double ddelta3, double delta, double period){
80        A_ = A;
81        M_ = M;
82        B_ = B;
83        ddelta1_ = ddelta1;
84        ddelta2_ = ddelta2;
[13]85        ddelta3_ = ddelta3;
[5]86        delta_ = delta;
87        period_ = period;
88        w_switch_old_ = 0;
89        a_switch1_old_ = A_;
[13]90        a_switch2_old_ = 0;
[5]91        a_switch3_old_ = 0;
92        vel_ref_.resize(2);
93        vel_ref_[0] = 0;
94        vel_ref_[1] = 0;
95        yr_ = 0;
96        min_peak_ = 0;
97        w_switch_ = 0;
98        min_peak_detect_init_ = false;
99        initialized_ = true;
100}
101
102std::vector<double> NNESC2D::step(double obj_val){
103        if (!initialized_){
104                fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n");
105                return std::vector<double>();
106        }
107
108        if(!min_peak_detect_init_){
[13]109                min_peak_ = obj_val;
110                yr_ = min_peak_;
[5]111                min_peak_detect_init_ = true;
112        }
113
114        double e = yr_ - obj_val;
[13]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);
[5]125        yr_ = yr_ + (w_switch_+min_peak_)*period_;
[13]126
[5]127        return vel_ref_;
128}
[13]129double NNESC2D::wSwitch(double e){
130        if(e>delta_){
[5]131                w_switch_old_ = 0;
132                return 0;
133        }
[13]134        else if(e<-delta_){
[5]135                w_switch_old_ = B_;
136                return B_;
137        }
138        else
139                return w_switch_old_;
140
141}
142
[13]143double NNESC2D::minPeakDetect(double e){
144        if(e<=0)
[5]145                return 0;
146        else
147                return -M_;
148}
149
150double NNESC2D::aSwitch1(double e){
151        if( e < -ddelta1_ ){
152                a_switch1_old_ = -A_;
153                return -A_;
154        }
155        else if(e>ddelta1_){
156                a_switch1_old_ = A_;
157                return A_;
158        }
159        else
160                return a_switch1_old_;
161}
162
163double NNESC2D::aSwitch2(double e){
[13]164
[5]165        if( e < -ddelta2_ ){
[13]166                a_switch2_old_ = A_;
167                return A_;
168
169        }
170        else if(e>ddelta2_){
[5]171                a_switch2_old_ = 0;
172                return 0;
173        }
174        else
175                return a_switch2_old_;
176}
177
178double NNESC2D::aSwitch3(double e){
179        if( e < -ddelta3_ ){
180                a_switch3_old_ = -2*A_;
181                return -2*A_;
182        }
183        else if(e>ddelta3_){
184                a_switch3_old_ = 0;
185                return 0;
186        }
187        else
188                return a_switch3_old_;
189}
[13]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}
Note: See TracBrowser for help on using the repository browser.