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 | } |
---|