1 | /* |
---|
2 | * approx_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 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_2d.h" |
---|
17 | |
---|
18 | ApproxESC2D::ApproxESC2D(){ |
---|
19 | data_size_ = 0; |
---|
20 | k_grad_ = 0; |
---|
21 | init_vel_ = 0; |
---|
22 | sampling_ = 0; |
---|
23 | ptr_ = 0; |
---|
24 | initialized_ = false; |
---|
25 | } |
---|
26 | |
---|
27 | ApproxESC2D::ApproxESC2D(int data_size, double k_grad, double init_vel, int sampling){ |
---|
28 | init(data_size,k_grad,init_vel,sampling); |
---|
29 | } |
---|
30 | |
---|
31 | void ApproxESC2D::init(int data_size, double k_grad, double init_vel, int sampling){ |
---|
32 | data_size_ = data_size; |
---|
33 | k_grad_ = k_grad; |
---|
34 | init_vel_ = init_vel; |
---|
35 | sampling_ = sampling; |
---|
36 | sample_ = 0; |
---|
37 | ptr_ = 0; |
---|
38 | states_ = Eigen::MatrixXf::Zero(2,data_size_); |
---|
39 | obj_vals_ = Eigen::VectorXf::Zero(data_size_); |
---|
40 | state_curr_.resize(2); |
---|
41 | vel_ref_.resize(2); |
---|
42 | initialized_ = true; |
---|
43 | } |
---|
44 | |
---|
45 | std::vector<double> ApproxESC2D::step(std::vector<double> state, double obj_val){ |
---|
46 | if(initialized_){ |
---|
47 | if(sample_ % sampling_ == 0){ |
---|
48 | |
---|
49 | states_(0,ptr_) = state[0]; |
---|
50 | states_(1,ptr_) = state[1]; |
---|
51 | obj_vals_(ptr_) = obj_val; |
---|
52 | |
---|
53 | |
---|
54 | |
---|
55 | if (sample_<sampling_*data_size_ ){ |
---|
56 | vel_ref_(0) = init_vel_*2.0/3.0; |
---|
57 | vel_ref_(1) = init_vel_; |
---|
58 | }/* |
---|
59 | else if(grad_val[0]>2 || grad_val[0]<-2){ |
---|
60 | vel_ref_(0) = init_vel_; |
---|
61 | vel_ref_(1) = init_vel_; |
---|
62 | }*/ |
---|
63 | else{ |
---|
64 | Eigen::MatrixXf V(data_size_,4); |
---|
65 | V = Eigen::MatrixXf::Zero(data_size_, 4); |
---|
66 | |
---|
67 | for (int i = 0; i<data_size_; i++){ |
---|
68 | V(i,0) = states_(0,i)*states_(1,i); |
---|
69 | V(i,1) = states_(0,i); |
---|
70 | V(i,2) = states_(1,i); |
---|
71 | V(i,3) = 1; |
---|
72 | } |
---|
73 | |
---|
74 | Eigen::VectorXf coef = Eigen::VectorXf::Zero(data_size_); |
---|
75 | |
---|
76 | printf("V = \n"); |
---|
77 | for (int i = 0; i<data_size_; i++) |
---|
78 | printf(" %f %f %f %f %f \n",V(i,0),V(i,1),V(i,2),V(i,3),obj_vals_(i)); |
---|
79 | |
---|
80 | //coef = V.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(obj_vals_); |
---|
81 | coef = V.colPivHouseholderQr().solve(obj_vals_); |
---|
82 | state_curr_(0) = states_(0,ptr_); |
---|
83 | state_curr_(1) = states_(1,ptr_); |
---|
84 | Eigen::VectorXf grad_val = Eigen::VectorXf::Zero(2); |
---|
85 | |
---|
86 | grad_val(0) = coef(0)*state_curr_(1)+coef(1); |
---|
87 | grad_val(1) = coef(0)*state_curr_(0)+coef(2); |
---|
88 | |
---|
89 | printf("grad_val[0] = %f \n",grad_val(0)); |
---|
90 | printf("coef[0] = %f coef[1] = %f coef[2] = %f\n",coef(0),coef(1),coef(2)); |
---|
91 | printf("in \n"); |
---|
92 | vel_ref_ = -k_grad_*grad_val; |
---|
93 | } |
---|
94 | |
---|
95 | printf("\n"); |
---|
96 | |
---|
97 | sample_ = sample_+1; |
---|
98 | ptr_ = ptr_+1; |
---|
99 | if(ptr_>=data_size_) |
---|
100 | ptr_ = 0; |
---|
101 | |
---|
102 | |
---|
103 | std::vector<double> out; |
---|
104 | out.push_back(vel_ref_(0)); |
---|
105 | out.push_back(vel_ref_(1)); |
---|
106 | return out; |
---|
107 | } |
---|
108 | else{ |
---|
109 | sample_ = sample_+1; |
---|
110 | std::vector<double> out; |
---|
111 | out.push_back(vel_ref_(0)); |
---|
112 | out.push_back(vel_ref_(1)); |
---|
113 | return out; |
---|
114 | } |
---|
115 | } |
---|
116 | else{ |
---|
117 | fprintf(stderr,"The approximation based ESC (1D) is not initialized... It will not be executed. \n"); |
---|
118 | return std::vector<double>(); |
---|
119 | } |
---|
120 | } |
---|
121 | |
---|
122 | ESC::inputType ApproxESC2D::getInputType(){ |
---|
123 | return ESC::inputStateValue; |
---|
124 | } |
---|
125 | ESC::outputType ApproxESC2D::getOutputType(){ |
---|
126 | return ESC::outputVelocity; |
---|
127 | } |
---|
128 | |
---|
129 | std::vector<double> ApproxESC2D::monitor(){ |
---|
130 | return std::vector<double> (); |
---|
131 | } |
---|
132 | std::vector<std::string> ApproxESC2D::monitorNames(){ |
---|
133 | return std::vector<std::string>(); |
---|
134 | } |
---|
135 | |
---|
136 | void ApproxESC2D::reset(){ |
---|
137 | sample_ = 0; |
---|
138 | ptr_ = 0; |
---|
139 | states_ = Eigen::MatrixXf::Zero(2,data_size_); |
---|
140 | obj_vals_ = Eigen::VectorXf::Zero(data_size_); |
---|
141 | } |
---|