source: trunk/extremum_seeking/esc_approx/src/approx_esc_1d.cpp @ 5

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

Imported extremum_seeking at revision 987

File size: 2.9 KB
Line 
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
18ApproxESC1D::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
28ApproxESC1D::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
32void 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
45std::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
100ESC::inputType ApproxESC1D::getInputType(){
101        return ESC::inputStateValue;
102}
103ESC::outputType ApproxESC1D::getOutputType(){
104        return ESC::outputVelocity;
105}
106
107std::vector<double> ApproxESC1D::monitor(){
108        return std::vector<double> ();
109}
110std::vector<std::string> ApproxESC1D::monitorNames(){
111        return std::vector<std::string>();
112}
Note: See TracBrowser for help on using the repository browser.