Robotsystem from Robot Group Leipzig: invert3channelcontroller.h Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# invert3channelcontroller.h

Go to the documentation of this file.

    00001 #ifndef __INVERT3CHANNELCONTROLLER_H
    00002 #define __INVERT3CHANNELCONTROLLER_H
    00003
    00004 #include "invertcontroller.h"
    00005
    00006 #include <cassert>
    00007 #include <math.h>
    00008
    00009
    00010 /**
    00011  * class for robot controller that use naglaa's direct matrix inversion for n channels
    00012  * (simple one layer networks)
    00013  *
    00014  * Implements standart parameters: eps, rho, mu, stepnumber4avg, stepnumber4delay
    00015  */
    00016 template <int NUMBER_CHANNELS, int BUFFER_SIZE=2> class Invert3ChannelController : public InvertController {
    00017
    00018 public:
    00019
    00020   /*
    00021     Invert3ChannelController();
    00022
    00023     //virtual ~Invert3ChannelController(){}
    00024
    00025
    00026     /// performs one step (includes learning). Calulates motor commands from sensor inputs.
    00027     virtual void step(const sensor*, motor*);
    00028
    00029     /// performs one step without learning. Calulates motor commands from sensor inputs.
    00030     virtual void stepNoLearning(const sensor*, motor*);
    00031   */
    00032
    00033
    00034   //protected:
    00035 public:
    00036   double A[NUMBER_CHANNELS][NUMBER_CHANNELS];    ///< model matrix
    00037   double C[NUMBER_CHANNELS][NUMBER_CHANNELS];    ///< controller matrix
    00038   double h[NUMBER_CHANNELS];       ///< bias vector
    00039
    00040   double x_buffer[BUFFER_SIZE][NUMBER_CHANNELS]; ///< buffer for input values, x[t%buffersize]=actual value, x[(t-1+buffersize)%buffersize]=x(t-1)
    00041   double y_buffer[BUFFER_SIZE][NUMBER_CHANNELS]; ///< buffer for output values, y[t%buffersize]=actual value(if already calculated!), y[(t-1+buffersize)%buffersize]=y(t-1)
    00042
    00043   double xsi4E[NUMBER_CHANNELS];
    00044   double xsi4Model[NUMBER_CHANNELS];
    00045
    00046   int    t;       ///< number of steps, needed for ringbuffer x_buffer
    00047
    00048
    00049
    00050
    00051
    00052   /*
    00053     virtual void inverseMatrix(double Q[NUMBER_CHANNELS][NUMBER_CHANNELS], double Q_1[NUMBER_CHANNELS][NUMBER_CHANNELS]);
    00054
    00055     virtual double calculateE(double *x_delay, double *y_delay);
    00056
    00057     /// learn values h,C   //,A
    00058     virtual void learn(double *x_delay, double *y_delay);
    00059
    00060     /// learn model parameter (matrix A) by gradient descent
    00061     virtual void learnModel(double *x_actual, double *y_effective);
    00062
    00063
    00064     /// calculate delayed values
    00065     virtual void calculateDelayedValues(double source[BUFFER_SIZE][NUMBER_CHANNELS], int number_steps_of_delay_, double *target);
    00066
    00067     virtual void calculateSmoothValues(double source[BUFFER_SIZE][NUMBER_CHANNELS], int number_steps_for_averaging_, double *target);
    00068
    00069     /// calculate controller ouptus
    00070     virtual void calculateControllerValues(double *x_smooth, double *y);
    00071
    00072     // put new value in ring buffer
    00073     virtual void putInBuffer(double buffer[BUFFER_SIZE][NUMBER_CHANNELS], double *values);
    00074
    00075   */
    00076   /// neuron transfer function
    00077   virtual double g(double z)
    00078   {
    00079     return tanh(z);
    00080   };
    00081
    00082
    00083
    00084   ///
    00085   virtual double g_s(double z)
    00086   {
    00087     return 1.0 - tanh(z) * tanh(z);
    00088   };
    00089
    00090
    00091
    00092   /// squashing function, to protect against to large weight updates
    00093   virtual double squash(double z)
    00094   {
    00095     return 0.1 * tanh(10.0 * z);
    00096   };
    00097
    00098
    00099   //template <int NUMBER_CHANNELS, int BUFFER_SIZES>
    00100   //    Invert3ChannelController<int NUMBER_CHANNELS, int BUFFER_SIZES>::
    00101   Invert3ChannelController(){
    00102
    00103     t=0;
    00104
    00105     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00106       {
    00107         h[i] = 0.0;
    00108         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00109           {
    00110             if (i == j)
    00111               {
    00112                 A[i][j] = 1.0;
    00113                 C[i][j] = 0.1;
    00114               }
    00115             else
    00116               {
    00117                 A[i][j] = 0.0;
    00118                 C[i][j] = 0.0;
    00119               }
    00120           }
    00121       }
    00122     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00123       {
    00124         for (int k = 0; k < BUFFER_SIZE; k++)
    00125           {
    00126             x_buffer[k][i] = 0;
    00127             y_buffer[k][i] = 0;
    00128           }
    00129       }
    00130
    00131     /*    // print initial values
    00132           std::cout<<"Constructor of RobotLearnControl:"<<std::endl;
    00133           std::cout<<"init: epsilon="<<eps<<std::endl;
    00134           std::cout<<"init: rho="<<rho<<std::endl;
    00135           std::cout<<"init: stepnumber4delay="<<stepnumber4delay<<std::endl;
    00136           std::cout<<"init: stepnumber4avg="<<stepnumber4avg<<std::endl;
    00137           std::cout<<"init: delta="<<delta<<std::endl;
    00138           std::cout<<"init: m (for calculation of E)="<<m<<std::endl;
    00139     */
    00140   };
    00141
    00142   /// performs one step (includes learning). Calulates motor commands from sensor inputs.
    00143   virtual void step(const sensor* x_, int sensornumber,
    00144                     motor* y_, int motornumber) {
    00145     double x_smooth[NUMBER_CHANNELS];
    00146     double x_effective[NUMBER_CHANNELS];
    00147     double y_effective[NUMBER_CHANNELS];
    00148     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00149       {
    00150         x_smooth[i] = 0.0;
    00151         x_effective[i] = 0.0;
    00152         y_effective[i] = 0.0;
    00153       }
    00154
    00155     // put new input value in ring buffer x_buffer
    00156     putInBuffer(x_buffer, x_);
    00157
    00158     // averaging over the last stepnumber4avg values of x_buffer
    00159     calculateSmoothValues(x_buffer, stepnumber4avg, x_smooth);
    00160
    00161     // calculate controller values based on smoothed input values
    00162     calculateControllerValues(x_smooth, y_);
    00163
    00164     // put new output value in ring buffer y_buffer
    00165     putInBuffer(y_buffer, y_);
    00166
    00167     // calculate effective input/output, which is (actual-stepnumber4delay) element of buffer
    00168     calculateDelayedValues(x_buffer, stepnumber4delay, x_effective);
    00169     calculateDelayedValues(y_buffer, stepnumber4delay, y_effective);
    00170
    00171     // learn controller with effective input/output
    00172     learn(x_, x_effective, y_effective);
    00173
    00174     // learn model with actual input and effective output (that produced the actual input)
    00175     learnModel(x_, y_effective);
    00176
    00177     // update step counter
    00178     t++;
    00179   };
    00180
    00181
    00182   /// performs one step without learning. Calulates motor commands from sensor inputs.
    00183   virtual void stepNoLearning(const sensor* x_, int sensornumber,
    00184                               motor* y_, int motornumber){
    00185     double x_smooth[NUMBER_CHANNELS];
    00186     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00187       {
    00188         x_smooth[i] = 0.0;
    00189       }
    00190
    00191
    00192     // put new input value in ring buffer x_buffer
    00193     putInBuffer(x_buffer, x_);
    00194
    00195     // averaging over the last stepnumber4avg values of x_buffer
    00196     calculateSmoothValues(x_buffer, stepnumber4avg, x_smooth);
    00197
    00198     // calculate controller values based on smoothed input values
    00199     calculateControllerValues(x_smooth, y_);
    00200
    00201     // put new output value in ring buffer y_buffer
    00202     putInBuffer(y_buffer, y_);
    00203
    00204     // update step counter
    00205     t++;
    00206   };
    00207
    00208
    00209 protected:
    00210   virtual void inverseMatrix(double Q[NUMBER_CHANNELS][NUMBER_CHANNELS], double Q_1[NUMBER_CHANNELS][NUMBER_CHANNELS]){
    00211     // Berechne Inverse von Q
    00212
    00213     // only if NUMBER_CHANNELS<4
    00214     assert(NUMBER_CHANNELS<4);
    00215     if (NUMBER_CHANNELS==2){
    00216
    00217       double det = Q[0][0] * Q[1][1] - Q[0][1] * Q[1][0];
    00218       Q_1[0][0] = Q[1][1] / det;
    00219       Q_1[1][1] = Q[0][0] / det;
    00220       Q_1[0][1] = -Q[0][1] / det;
    00221       Q_1[1][0] = -Q[1][0] / det;
    00222
    00223     }
    00224
    00225
    00226     if (NUMBER_CHANNELS==3){
    00227
    00228       double  Q_adjoint[NUMBER_CHANNELS][NUMBER_CHANNELS]  ;
    00229       double  detQ=0  ;
    00230
    00231       //calculate the inverse of Q
    00232       Q_adjoint[0][0]=Q[1][1]*Q[2][2]-Q[1][2]*Q[2][1] ;
    00233       Q_adjoint[0][1]=(Q[1][2]*Q[2][0]-Q[1][0]*Q[2][2]) ;
    00234       Q_adjoint[0][2]=Q[1][0]*Q[2][1]-Q[1][1]*Q[2][0] ;
    00235       Q_adjoint[1][0]=(Q[2][1]*Q[0][2]-Q[0][1]*Q[2][2]) ;
    00236       Q_adjoint[1][1]=Q[0][0]*Q[2][2]-Q[0][2]*Q[2][0] ;
    00237       Q_adjoint[1][2]=(Q[0][1]*Q[2][0]-Q[0][0]*Q[2][1]) ;
    00238       Q_adjoint[2][0]=Q[0][1]*Q[1][2]-Q[1][1]*Q[0][2] ;
    00239       Q_adjoint[2][1]=(Q[1][0]*Q[0][2]-Q[0][0]*Q[1][2]) ;
    00240       Q_adjoint[2][2]=Q[0][0]*Q[1][1]-Q[0][1]*Q[1][0] ;
    00241       detQ=Q[0][0]*Q_adjoint[0][0]+Q[0][1]*Q_adjoint[0][1]+Q[0][2]*Q_adjoint[0][2] ;
    00242       for(int i=0; i<NUMBER_CHANNELS; i++){
    00243         for(int j=0; j<NUMBER_CHANNELS; j++) {
    00244           Q_1[i][j]=(Q_adjoint[j][i])/detQ  ;
    00245         }
    00246       }
    00247     }
    00248   };
    00249
    00250
    00251   virtual double calculateE(const double *x_, const double *x_delay, const  double *y_delay) {
    00252     double L[NUMBER_CHANNELS][NUMBER_CHANNELS];
    00253     double Q[NUMBER_CHANNELS][NUMBER_CHANNELS];
    00254     double Q_1[NUMBER_CHANNELS][NUMBER_CHANNELS];
    00255     double z[NUMBER_CHANNELS];
    00256
    00257
    00258     // Calculate z based on the delayed inputs since the present input x is
    00259     // produced by the outputs tau time steps before
    00260     // which on their hand are y = K(x_D)
    00261     // due to the delay in the feed back loop.
    00262
    00263     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00264       {
    00265         z[i] = h[i];
    00266         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00267           {
    00268             z[i] += C[i][j] * x_delay[j];
    00269           }
    00270       }
    00271
    00272     // Berechne Matrix L
    00273     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00274       {
    00275         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00276           {
    00277             L[i][j] = 0.0;
    00278             for (int k = 0; k < NUMBER_CHANNELS; k++)
    00279               {
    00280                 L[i][j] += A[i][k] * g_s(z[k]) * C[k][j];
    00281               }
    00282           }
    00283       }
    00284
    00285     // Berechne Q=LL^T
    00286     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00287       {
    00288         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00289           {
    00290             Q[i][j] = 0.0;
    00291             for (int k = 0; k < NUMBER_CHANNELS; k++)
    00292               {
    00293                 Q[i][j] += L[i][k] * L[j][k];
    00294               }
    00295             if (i == j)
    00296               Q[i][j] += rho / NUMBER_CHANNELS; // Regularisation
    00297           }
    00298       }
    00299
    00300     // Berechne Inverse von Q
    00301     inverseMatrix(Q, Q_1);
    00302
    00303     // Berechne xsi
    00304     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00305       {
    00306         xsi4E[i] = x_[i];
    00307         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00308           {
    00309             xsi4E[i] -= A[i][j] * y_delay[j];  // using old y value -> no influence of changes (adding delta) in controller
    00310             // very very strange!!!!
    00311             //xsi4E[i] -= A[i][j] * g(z[j]);     // using recalculating y -> influence of changes (adding delta) in controller
    00312           }
    00313       }
    00314     double E = 0;
    00315     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00316       {
    00317         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00318           {
    00319             E += xsi4E[i] * Q_1[i][j] * xsi4E[j];
    00320           }
    00321       }
    00322
    00323     double E_s=0;
    00324     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00325       {
    00326         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00327           {
    00328             E_s += (A[i][j]*g(z[j]) - x_[i]) * (A[i][j]*g(z[j]) - x_[i]);
    00329           }
    00330       }
    00331
    00332     E=(1-m)*E+ m*E_s;
    00333     return (E);
    00334
    00335   };
    00336
    00337
    00338   virtual void learn(const double *x_, const double *x_delay, const double *y_delay)
    00339   {
    00340     //    double A_update[NUMBER_CHANNELS][NUMBER_CHANNELS];
    00341     double C_update[NUMBER_CHANNELS][NUMBER_CHANNELS];
    00342     double h_update[NUMBER_CHANNELS];
    00343
    00344     double E_0 = calculateE(x_,x_delay, y_delay);
    00345
    00346     // calculate updates for h,C,A
    00347     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00348       {
    00349         h[i] += delta;
    00350         h_update[i] = -eps * (calculateE(x_,x_delay, y_delay) - E_0) / delta;
    00351         h[i] -= delta;
    00352         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00353           {
    00354             C[i][j] += delta;
    00355             C_update[i][j] = -eps * (calculateE(x_,x_delay, y_delay) - E_0) / delta;
    00356             C[i][j] -= delta;
    00357             //A[i][j] += delta;
    00358             //A_update[i][j] = -eps * a_factor * (calculateE(x_delay, y_delay) - E_0) / delta;
    00359             //A[i][j] -= delta;
    00360           }
    00361       }
    00362
    00363     // apply updates to h,C,A
    00364     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00365       {
    00366         h[i] += squash(h_update[i]);
    00367         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00368           {
    00369             C[i][j] += squash(C_update[i][j]);
    00370             //A[i][j] += squash(A_update[i][j]);
    00371           }
    00372       }
    00373   };
    00374
    00375
    00376   virtual void learnModel(const double *x_actual, double *y_effective){
    00377     /*  double z[N_output];
    00378         for(int i=0; i<N_output; i++){
    00379         z[i]=h[i];
    00380         for(int j=0; j<N_input; j++) {
    00381         z[i]+=C[i][j]*x_D[j];
    00382         }
    00383         }
    00384     */
    00385     // Berechne xsi
    00386     for(int i=0; i<NUMBER_CHANNELS; i++){
    00387       xsi4Model[i]=x_actual[i];
    00388       for(int j=0; j<NUMBER_CHANNELS; j++){
    00389         xsi4Model[i]-= A[i][j]*y_effective[j];
    00390       }
    00391     }
    00392
    00393     for(int i=0; i<NUMBER_CHANNELS; i++){
    00394       for (int j=0; j<NUMBER_CHANNELS; j++){
    00395         A[i][j]+=squash( (factor_a*eps*0.2) *xsi4Model[i] * y_effective[j]) ;
    00396       }
    00397     }
    00398   };
    00399
    00400
    00401
    00402
    00403
    00404   /// calculate delayed values
    00405   virtual void calculateDelayedValues(double source[BUFFER_SIZE][NUMBER_CHANNELS], paramval number_steps_of_delay_, double *target)
    00406   {
    00407     // number_steps_of_delay_ must not be larger than BUFFER_SIZE
    00408     assert ((int)number_steps_of_delay_ < BUFFER_SIZE);
    00409
    00410     // get delayed value from ring buffer
    00411
    00412     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00413       {
    00414         target[i] = source[(t - (int)number_steps_of_delay_ + BUFFER_SIZE) % BUFFER_SIZE][i];
    00415       }
    00416   };
    00417
    00418   virtual void calculateSmoothValues(double source[BUFFER_SIZE][NUMBER_CHANNELS], paramval number_steps_for_averaging_, double *target)
    00419   {
    00420     // number_steps_for_averaging_ must not be larger than BUFFER_SIZE
    00421     assert ((int)number_steps_for_averaging_ <= BUFFER_SIZE);
    00422
    00423     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00424       {
    00425         target[i] = 0.0;
    00426         for (int k = 0; k < (int)number_steps_for_averaging_; k++)
    00427           {
    00428             target[i] += source[(t - k + BUFFER_SIZE) % BUFFER_SIZE][i]/ (double) (number_steps_for_averaging_);
    00429           }
    00430       }
    00431   };
    00432
    00433
    00434
    00435   /// calculate controller ouptus
    00436   virtual void calculateControllerValues(double *x_smooth, double *y)
    00437   {
    00438     double z[NUMBER_CHANNELS];
    00439
    00440     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00441       {
    00442         z[i] = h[i];
    00443         for (int j = 0; j < NUMBER_CHANNELS; j++)
    00444           {
    00445             z[i] += C[i][j] * x_smooth[j];
    00446           }
    00447         y[i] = g(z[i]);
    00448       }
    00449   };
    00450
    00451
    00452   // put new value in ring buffer
    00453   virtual void putInBuffer(double buffer[BUFFER_SIZE][NUMBER_CHANNELS], const double *values){
    00454     for (int i = 0; i < NUMBER_CHANNELS; i++)
    00455       {
    00456         buffer[(t+BUFFER_SIZE)% BUFFER_SIZE][i] = values[i];
    00457       }
    00458   };
    00459
    00460
    00461
    00462   virtual int getInternalParamNames(paramkey*& keylist){
    00463     keylist=(paramkey*)malloc(sizeof(paramkey)*10);
    00464     keylist[0]="C00";
    00465     keylist[1]="C01";
    00466     keylist[2]="C10";
    00467     keylist[3]="C11";
    00468     keylist[4]="H0";
    00469     keylist[5]="H1";
    00470     keylist[6]="A00";
    00471     keylist[7]="A01";
    00472     keylist[8]="A10";
    00473     keylist[9]="A11";
    00474     return 10;
    00475   }
    00476
    00477   virtual int getInternalParams(paramval* vallist, int length) {
    00478     if(length < 10) return 0;
    00479     vallist[0]=C[0][0];
    00480     vallist[1]=C[0][1];
    00481     vallist[2]=C[1][0];
    00482     vallist[3]=C[1][1];
    00483     vallist[4]=h[0];
    00484     vallist[5]=h[1];
    00485     vallist[6]=A[0][0];
    00486     vallist[7]=A[0][1];
    00487     vallist[8]=A[1][0];
    00488     vallist[9]=A[1][1];
    00489     return 10;
    00490   }
    00491
    00492
    00493 };
    00494
    00495 AbstractController* getController(int sensornumber, int motornumber,
    00496                                   int param1/*=0*/, double param2/*=0*/)
    00497 {
    00498   if(param1 < 2) param1 = 2;
    00499   return new Invert3ChannelController<2, 4>;
    00500 }
    00501
    00502
    00503
    00504 #endif