source: trunk/extremum_seeking/esc_approx/src/approx_esc_2d.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: 3.5 KB
Line 
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
18ApproxESC2D::ApproxESC2D(){
19        data_size_ = 0;
20        k_grad_ = 0;
21        init_vel_ = 0;
22        sampling_ = 0;
23        ptr_ = 0;
24        initialized_ = false;
25}
26
27ApproxESC2D::ApproxESC2D(int data_size, double k_grad, double init_vel, int sampling){
28        init(data_size,k_grad,init_vel,sampling);
29}
30
31void 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        sample_ = 0;
36        sampling_ = sampling;
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
45std::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
122ESC::inputType ApproxESC2D::getInputType(){
123        return ESC::inputStateValue;
124}
125ESC::outputType ApproxESC2D::getOutputType(){
126        return ESC::outputVelocity;
127}
128
129std::vector<double> ApproxESC2D::monitor(){
130        return std::vector<double> ();
131}
132std::vector<std::string> ApproxESC2D::monitorNames(){
133        return std::vector<std::string>();
134}
Note: See TracBrowser for help on using the repository browser.