[18] | 1 | /* |
---|
| 2 | * nn_esc_1d.cpp |
---|
| 3 | * |
---|
| 4 | * Created on: Jul 26, 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 one 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_1d.h" |
---|
| 16 | |
---|
| 17 | NNESC1D::NNESC1D(){ |
---|
| 18 | M_ = 0; |
---|
| 19 | A_ = 0; |
---|
| 20 | ddelta_ = 0; |
---|
| 21 | delta_ = 0; |
---|
| 22 | B_ = 0; |
---|
| 23 | w_switch_old_ = 0; |
---|
| 24 | a_switch_old_ = 0; |
---|
| 25 | yr_ = 0; |
---|
| 26 | period_ = 0; |
---|
| 27 | min_peak_ = 0; |
---|
| 28 | vel_ref_ = 0; |
---|
| 29 | w_switch_ = 0; |
---|
| 30 | min_peak_detect_init_ = false; |
---|
| 31 | initialized_ = false; |
---|
| 32 | |
---|
| 33 | } |
---|
| 34 | |
---|
| 35 | ESC::inputType NNESC1D::getInputType(){ |
---|
| 36 | return inputValue; |
---|
| 37 | } |
---|
| 38 | |
---|
| 39 | ESC::outputType NNESC1D::getOutputType(){ |
---|
| 40 | return outputVelocity; |
---|
| 41 | } |
---|
| 42 | |
---|
| 43 | std::vector<double> NNESC1D::monitor(){ |
---|
| 44 | std::vector<double> monitor_vals; |
---|
| 45 | monitor_vals.push_back(yr_); |
---|
| 46 | monitor_vals.push_back(min_peak_); |
---|
| 47 | monitor_vals.push_back(w_switch_); |
---|
| 48 | |
---|
| 49 | return monitor_vals; |
---|
| 50 | |
---|
| 51 | } |
---|
| 52 | |
---|
| 53 | std::vector<std::string> NNESC1D::monitorNames(){ |
---|
| 54 | std::vector<std::string> monitor_names; |
---|
| 55 | monitor_names.push_back("driving input value"); |
---|
| 56 | monitor_names.push_back("minimum peak detector output"); |
---|
| 57 | monitor_names.push_back("w switch value"); |
---|
| 58 | |
---|
| 59 | return monitor_names; |
---|
| 60 | } |
---|
| 61 | |
---|
| 62 | NNESC1D::NNESC1D(double A,double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val){ |
---|
| 63 | init(A, M, B, ddelta, delta, period,stopping_cycle_number,stoping_min_val); |
---|
| 64 | } |
---|
| 65 | |
---|
| 66 | void NNESC1D::init(double A, double M, double B, double ddelta, double delta, double period, int stopping_cycle_number, double stoping_min_val){ |
---|
| 67 | A_ = A; |
---|
| 68 | M_ = M; |
---|
| 69 | B_ = B; |
---|
| 70 | ddelta_ = ddelta; |
---|
| 71 | delta_ = delta; |
---|
| 72 | period_ = period; |
---|
| 73 | reset(); |
---|
| 74 | initialized_ = true; |
---|
| 75 | stopping_cycle_number_ = stopping_cycle_number; |
---|
| 76 | stoping_min_val_ = stoping_min_val; |
---|
| 77 | } |
---|
| 78 | |
---|
| 79 | std::vector<double> NNESC1D::step(double obj_val){ |
---|
| 80 | if (!initialized_){ |
---|
| 81 | fprintf(stderr,"The neural network ESC (1D) is not initialized... It will not be executed. \n"); |
---|
| 82 | return std::vector<double>(); |
---|
| 83 | } |
---|
| 84 | |
---|
| 85 | if(!min_peak_detect_init_){ |
---|
| 86 | yr_ = obj_val; |
---|
| 87 | min_peak_detect_init_ = true; |
---|
| 88 | obj_val_cycle_init_ = obj_val; |
---|
| 89 | } |
---|
| 90 | |
---|
| 91 | double e = yr_ - obj_val; |
---|
| 92 | vel_ref_ = aSwitch(e); |
---|
| 93 | min_peak_ = minPeakDetect(-e); |
---|
| 94 | w_switch_ = wSwitch(-e); |
---|
| 95 | yr_ = yr_ + (w_switch_+min_peak_)*period_; |
---|
| 96 | |
---|
| 97 | if(vel_ref_old_ != vel_ref_ && vel_ref_ == -A_){ |
---|
| 98 | if(obj_val_cycle_init_ - obj_val < stoping_min_val_) |
---|
| 99 | nn_cycle_count_++; |
---|
| 100 | else |
---|
| 101 | nn_cycle_count_ = 0; |
---|
| 102 | obj_val_cycle_init_ = obj_val; |
---|
| 103 | } |
---|
| 104 | |
---|
| 105 | vel_ref_old_ = vel_ref_; |
---|
| 106 | |
---|
| 107 | std::vector<double> output; |
---|
| 108 | output.push_back(vel_ref_); |
---|
| 109 | return output; |
---|
| 110 | } |
---|
| 111 | double NNESC1D::wSwitch(double e_minus){ |
---|
| 112 | if(e_minus<-delta_){ |
---|
| 113 | w_switch_old_ = 0; |
---|
| 114 | return 0; |
---|
| 115 | } |
---|
| 116 | else if(e_minus>delta_){ |
---|
| 117 | w_switch_old_ = B_; |
---|
| 118 | return B_; |
---|
| 119 | } |
---|
| 120 | else |
---|
| 121 | return w_switch_old_; |
---|
| 122 | |
---|
| 123 | } |
---|
| 124 | |
---|
| 125 | double NNESC1D::minPeakDetect(double e_minus){ |
---|
| 126 | if(e_minus>0) |
---|
| 127 | return 0; |
---|
| 128 | else |
---|
| 129 | return -M_; |
---|
| 130 | } |
---|
| 131 | |
---|
| 132 | double NNESC1D::aSwitch(double e){ |
---|
| 133 | if( e < -ddelta_ ){ |
---|
| 134 | a_switch_old_ = -A_; |
---|
| 135 | return -A_; |
---|
| 136 | } |
---|
| 137 | else if(e>=ddelta_){ |
---|
| 138 | a_switch_old_ = A_; |
---|
| 139 | return A_; |
---|
| 140 | } |
---|
| 141 | else |
---|
| 142 | return a_switch_old_; |
---|
| 143 | |
---|
| 144 | } |
---|
| 145 | |
---|
| 146 | void NNESC1D::reset(){ |
---|
| 147 | w_switch_old_ = 0; |
---|
| 148 | a_switch_old_ = A_; |
---|
| 149 | yr_ = 0; |
---|
| 150 | min_peak_ = 0; |
---|
| 151 | vel_ref_ = 0; |
---|
| 152 | w_switch_ = 0; |
---|
| 153 | nn_cycle_count_ = 0; |
---|
| 154 | vel_ref_old_ = 0; |
---|
| 155 | min_peak_detect_init_ = false; |
---|
| 156 | |
---|
| 157 | } |
---|
| 158 | |
---|
| 159 | |
---|
| 160 | bool NNESC1D::isStoppingConditionsMet(){ |
---|
| 161 | if(stopping_cycle_number_ <= 0) |
---|
| 162 | return false; |
---|
| 163 | else if(nn_cycle_count_ >= stopping_cycle_number_){ |
---|
| 164 | return true; |
---|
| 165 | } |
---|
| 166 | else |
---|
| 167 | return false; |
---|
| 168 | } |
---|