[5] | 1 | /* |
---|
| 2 | * approx_esc_1d.cpp |
---|
| 3 | * |
---|
| 4 | * Created on: Jul 30, 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 approximation based extremum seeking control |
---|
| 10 | * |
---|
| 11 | * * References: |
---|
| 12 | * - C. Zhang and R. Ordonez, âRobust and adaptive design of numerical optimization-based extremum seeking control,â Automatica, vol. 45, pp. 634â646, 2009. |
---|
| 13 | * - B. Calli, W. Caarls, P. Jonker and M. Wisse, "Comparison of Extremum Seeking Control Algorithms for Robotic Applications," IROS 2012. |
---|
| 14 | */ |
---|
| 15 | |
---|
| 16 | #include "esc_approx/approx_esc_1d.h" |
---|
| 17 | |
---|
| 18 | ApproxESC1D::ApproxESC1D(){ |
---|
| 19 | data_size_ = 0; |
---|
| 20 | poly_degree_ = 0; |
---|
| 21 | k_grad_ = 0; |
---|
| 22 | init_vel_ = 0; |
---|
| 23 | sampling_ = 0; |
---|
| 24 | ptr_ = 0; |
---|
| 25 | initialized_ = false; |
---|
| 26 | } |
---|
| 27 | |
---|
| 28 | ApproxESC1D::ApproxESC1D(int data_size,int poly_degree, double k_grad, double init_vel, int sampling){ |
---|
| 29 | init(data_size,poly_degree,k_grad,init_vel,sampling); |
---|
| 30 | } |
---|
| 31 | |
---|
| 32 | void ApproxESC1D::init(int data_size,int poly_degree, double k_grad, double init_vel, int sampling){ |
---|
| 33 | data_size_ = data_size; |
---|
| 34 | poly_degree_ = poly_degree; |
---|
| 35 | k_grad_ = k_grad; |
---|
| 36 | init_vel_ = init_vel; |
---|
| 37 | sample_ = 0; |
---|
| 38 | sampling_ = sampling; |
---|
| 39 | ptr_ = 0; |
---|
| 40 | states_.resize(data_size); |
---|
| 41 | obj_vals_.resize(data_size); |
---|
| 42 | initialized_ = true; |
---|
| 43 | } |
---|
| 44 | |
---|
| 45 | std::vector<double> ApproxESC1D::step(std::vector<double> state, double obj_val){ |
---|
| 46 | if(initialized_){ |
---|
| 47 | if(sample_ % sampling_ == 0){ |
---|
| 48 | |
---|
| 49 | states_(ptr_) = state[0]; |
---|
| 50 | obj_vals_(ptr_) = obj_val; |
---|
| 51 | |
---|
| 52 | Eigen::MatrixXf V(data_size_,poly_degree_+1); |
---|
| 53 | V = Eigen::MatrixXf::Zero(data_size_, poly_degree_+1); |
---|
| 54 | |
---|
| 55 | for (int i = 0; i<data_size_; i++) |
---|
| 56 | for (int j = 0; j<poly_degree_+1; j++) |
---|
| 57 | V(i,j) = std::pow(states_(i),(poly_degree_-j)); |
---|
| 58 | |
---|
| 59 | Eigen::VectorXf coef(data_size_); |
---|
| 60 | |
---|
| 61 | coef = V.colPivHouseholderQr().solve(obj_vals_); |
---|
| 62 | state_curr_ = states_(ptr_); |
---|
| 63 | double grad_val = 0; |
---|
| 64 | Eigen::VectorXf vec(data_size_); |
---|
| 65 | |
---|
| 66 | for (int i = 0; i<poly_degree_; i++) |
---|
| 67 | grad_val = grad_val + coef(i)*(poly_degree_-i)*std::pow(state_curr_,(poly_degree_-i-1)); |
---|
| 68 | |
---|
| 69 | if (sample_<sampling_*data_size_+1){ |
---|
| 70 | vel_ref_ = init_vel_; |
---|
| 71 | } |
---|
| 72 | else{ |
---|
| 73 | vel_ref_ = -k_grad_*grad_val; |
---|
| 74 | if (vel_ref_!= vel_ref_) |
---|
| 75 | vel_ref_ = 0; |
---|
| 76 | } |
---|
| 77 | |
---|
| 78 | sample_ = sample_+1; |
---|
| 79 | ptr_ = ptr_+1; |
---|
| 80 | if(ptr_>=data_size_) |
---|
| 81 | ptr_ = 0; |
---|
| 82 | |
---|
| 83 | std::vector<double> out; |
---|
| 84 | out.push_back(vel_ref_); |
---|
| 85 | return out; |
---|
| 86 | } |
---|
| 87 | else{ |
---|
| 88 | sample_ = sample_+1; |
---|
| 89 | std::vector<double> out; |
---|
| 90 | out.push_back(vel_ref_); |
---|
| 91 | return out; |
---|
| 92 | } |
---|
| 93 | } |
---|
| 94 | else{ |
---|
| 95 | fprintf(stderr,"The approximation based ESC (1D) is not initialized... It will not be executed. \n"); |
---|
| 96 | return std::vector<double>(); |
---|
| 97 | } |
---|
| 98 | } |
---|
| 99 | |
---|
| 100 | ESC::inputType ApproxESC1D::getInputType(){ |
---|
| 101 | return ESC::inputStateValue; |
---|
| 102 | } |
---|
| 103 | ESC::outputType ApproxESC1D::getOutputType(){ |
---|
| 104 | return ESC::outputVelocity; |
---|
| 105 | } |
---|
| 106 | |
---|
| 107 | std::vector<double> ApproxESC1D::monitor(){ |
---|
| 108 | return std::vector<double> (); |
---|
| 109 | } |
---|
| 110 | std::vector<std::string> ApproxESC1D::monitorNames(){ |
---|
| 111 | return std::vector<std::string>(); |
---|
| 112 | } |
---|