Robotsystem from Robot Group Leipzig: invertnchannelcontroller.cpp Source File

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

- File List
- File Members

# invertnchannelcontroller.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   ANY COMMERCIAL USE FORBIDDEN!                                         *
    00008  *                                                                         *
    00009  *   $Log: invertnchannelcontroller.cpp,v $
    00010  *   Revision 1.1.2.2  2006/01/18 16:48:35  martius
    00011  *   stored and restore
    00012  *
    00013  *   Revision 1.1.2.1  2005/11/14 17:37:29  martius
    00014  *   moved to selforg
    00015  *
    00016  *   Revision 1.15  2005/10/27 15:46:38  martius
    00017  *   inspectable interface is expanded to structural information for network visualiser
    00018  *
    00019  *   Revision 1.14  2005/10/27 15:02:06  fhesse
    00020  *   commercial use added
    00021  *                                  *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024
    00025 #include "controller_misc.h"
    00026 #include "invertnchannelcontroller.h"
    00027
    00028 InvertNChannelController::InvertNChannelController(int _buffersize, bool _update_only_1/*=false*/){
    00029   t=0;
    00030   update_only_1 = _update_only_1;
    00031   buffersize    = _buffersize;
    00032
    00033   // prepare name;
    00034   Configurable::insertCVSInfo(name, "$RCSfile: invertnchannelcontroller.cpp,v $",
    00035                                     "$Revision: 1.1.2.2 $");
    00036 };
    00037
    00038 void InvertNChannelController::init(int sensornumber, int motornumber){
    00039   assert(sensornumber == motornumber);
    00040   number_channels=sensornumber;
    00041   A.set(number_channels, number_channels);
    00042   C.set(number_channels, number_channels);
    00043   h.set(number_channels, 1);
    00044   L.set(number_channels, number_channels);
    00045
    00046   A.toId(); // set a to identity matrix;
    00047   C.toId(); // set a to identity matrix;
    00048   //C*=0.1;
    00049   x_buffer = new Matrix[buffersize];
    00050   y_buffer = new Matrix[buffersize];
    00051   for (unsigned int k = 0; k < buffersize; k++) {
    00052     x_buffer[k].set(number_channels,1);
    00053     y_buffer[k].set(number_channels,1);
    00054   }
    00055 }
    00056
    00057 /// performs one step (includes learning). Calulates motor commands from sensor inputs.
    00058 void InvertNChannelController::step(const sensor* x_, int number_sensors,
    00059                                     motor* y_, int number_motors){
    00060   stepNoLearning(x_, number_sensors, y_, number_motors);
    00061   t--;
    00062
    00063   // calculate effective input/output, which is (actual-steps4delay) element of buffer
    00064   Matrix x_effective = calculateDelayedValues(x_buffer, int(s4delay));
    00065   Matrix y_effective = calculateDelayedValues(y_buffer, int(s4delay));
    00066
    00067   // learn controller with effective input/output
    00068   learn(x_effective, y_effective);
    00069   learnmodel(y_effective);
    00070
    00071   // update step counter
    00072   t++;
    00073 };
    00074
    00075
    00076 /// performs one step without learning. Calulates motor commands from sensor inputs.
    00077 void InvertNChannelController::stepNoLearning(const sensor* x_, int number_sensors,
    00078                                               motor* y_, int number_motors){
    00079   assert((unsigned)number_sensors <= number_channels
    00080          && (unsigned)number_motors <= number_channels);
    00081   Matrix x(number_channels,1,x_);
    00082
    00083   // put new input vector in ring buffer x_buffer
    00084   putInBuffer(x_buffer, x);
    00085
    00086   // averaging over the last s4avg values of x_buffer
    00087   Matrix x_smooth = calculateSmoothValues(x_buffer, int(s4avg));
    00088
    00089   // calculate controller values based on smoothed input values
    00090   Matrix y = calculateControllerValues(x_smooth);
    00091
    00092   // put new output vector in ring buffer y_buffer
    00093   putInBuffer(y_buffer, y);
    00094
    00095   // convert y to motor*
    00096   y.convertToBuffer(y_, number_motors);
    00097
    00098   // update step counter
    00099   t++;
    00100 };
    00101
    00102
    00103 // void iteration(double *colmn,double dommy[number_channels][number_channels],double *improvment){
    00104 //   double sum[number_channels]  ;
    00105 //   double norm=0.0 ;
    00106
    00107
    00108 //   for (int k= 0; k< number_channels; k++)
    00109 //     {
    00110 //       norm+=colmn[k]*colmn[k]  ;
    00111 //     }
    00112 //   norm=sqrt(norm)   ;
    00113 //   for(int t = 0; t <number_it ; t++)
    00114 //     {
    00115
    00116 //       //initialization
    00117 //       if(t==0)
    00118 //      {
    00119
    00120 //        for (int i = 0; i < number_channels; i++)
    00121 //          {
    00122 //            improvment[i]=0.0 ;
    00123
    00124 //          }
    00125 //         }
    00126
    00127
    00128 //       for (int k= 0; k< number_channels; k++)
    00129 //      {
    00130 //        sum[k]=colmn[k]/norm ;
    00131
    00132 //        for (int l= 0; l<number_channels; l++)
    00133 //          {
    00134 //            sum[k]-=dommy[k][l]*improvment[l]  ;
    00135 //          }
    00136 //      }
    00137
    00138
    00139 //       for (int j = 0; j< number_channels; j++){
    00140 //      for (int i = 0; i < number_channels; i++){
    00141 //        improvment[j]+=epsilon_it*dommy[i][j]*sum[i]       ;
    00142 //      }
    00143 //       }
    00144
    00145 //     }//endof-t-loop
    00146
    00147
    00148 //   for (int j = 0; j< number_channels; j++){
    00149 //     improvment[j]*=norm  ;
    00150 //   }
    00151 // };
    00152
    00153
    00154 double InvertNChannelController::calculateE(const Matrix& x_delay,
    00155                                             const Matrix& y_delay){
    00156   // Calculate z based on the delayed inputs since the present input x is
    00157   // produced by the outputs tau time steps before
    00158   // which on their hand are y = K(x_D)
    00159   // due to the delay in the feed back loop.
    00160   Matrix z = C * x_delay + h;
    00161
    00162   Matrix xsi = x_buffer[t%buffersize] - A * y_delay;
    00163   //Matrix xsi = x_buffer[t%buffersize] - A * z.map(g);
    00164
    00165   Matrix Cg = C.multrowwise(z.map(g_s)); // Cg_{ij} = g'_i * C_{ij}
    00166   L = A*Cg;                   // L_{ij}  = \sum_k A_{ik} g'_k c_{kj}
    00167
    00168   Matrix v = (L^-1)*xsi;
    00169
    00170   double E = ((v^T)*v).val(0, 0);
    00171   double Es = 0.0;
    00172   if(desens!=0){
    00173     Matrix diff_x = x_buffer[t%buffersize] - A*( (C*x_buffer[t%buffersize]+h).map(g) );
    00174     Es = ((diff_x^T)*diff_x).val(0, 0);
    00175   }
    00176   return (1-desens)*E + desens*Es;
    00177
    00178 //   iteration(xsi,A,eita_zero)  ;
    00179 //   for (int i = 0; i < number_channels; i++)
    00180 //     {
    00181 //       eita[i]=(1/(g_s(z[i])))*eita_zero[i];
    00182 //     }
    00183
    00184 //   iteration(eita,C,shift_value) ;
    00185 //   double E=0.0  ;
    00186 //   for (int i=0;i<number_channels;i++)
    00187 //     {
    00188 //       E+=shift_value[i]*shift_value[i];
    00189 //     }
    00190 //   for (int i = 0; i < number_channels; i++)
    00191 //     {
    00192 //       eita_sup[i]=eita[i];
    00193 //     }
    00194
    00195
    00196 //   // Berechnung des z mit aktuellem Sensorwert
    00197 //   for (int i = 0; i < number_channels; i++)
    00198 //     {
    00199 //       z[i] = h[i];
    00200 //       for (int j = 0; j < number_channels; j++)
    00201 //      {
    00202 //        z[i] += C[i][j] *x_buffer[(t+buffersize)%buffersize][j];
    00203 //      }
    00204 //       //y[i] = g(z[i]);
    00205 //     }
    00206
    00207 //   double E_s=0;
    00208 //   for (int i = 0; i < number_channels; i++)
    00209 //     {
    00210 //       for (int j = 0; j < number_channels; j++)
    00211 //      {
    00212 //        E_s += (A[i][j]*g(z[j]) - x_buffer[(t+buffersize)%buffersize][i]) * (A[i][j]*g(z[j]) - x_buffer[(t+buffersize)%buffersize][i]);
    00213 //      }
    00214 //     }
    00215
    00216
    00217 //   E=(1-m)*E+ m*E_s;
    00218
    00219 //   return E;
    00220 };
    00221
    00222
    00223 /// learn values h,C,A
    00224 void InvertNChannelController::learn(const Matrix& x_delay, const Matrix& y_delay){
    00225
    00226   Matrix C_update(number_channels,number_channels);
    00227   Matrix h_update(number_channels,1);
    00228
    00229   double E_0 = calculateE(x_delay,  y_delay);
    00230
    00231   // calculate updates for h,C,A
    00232   for (unsigned int i = 0; i < number_channels; i++)
    00233   {
    00234       h.val(i,0) += delta;
    00235       h_update.val(i,0) = -eps * (calculateE(x_delay, y_delay) - E_0) / delta;
    00236       //h_update[i] = -2*eps *eita[i]*eita[i]*g(y_delay[i]);
    00237       h.val(i,0) -= delta;
    00238  }
    00239
    00240   // only weights of one channel adapted in one time step
    00241   unsigned int start=0;
    00242   unsigned int end=number_channels;
    00243   if(update_only_1) {
    00244     start = t%number_channels;
    00245     end = (t%number_channels) + 1;
    00246   }
    00247   for (unsigned int i = start; i < end; i++){
    00248       for (unsigned int j = 0; j < number_channels; j++)
    00249         {
    00250           C.val(i,j) += delta;
    00251           C_update.val(i,j)  = - eps *  (calculateE(x_delay, y_delay) - E_0) / delta ;
    00252           C_update.val(i,j) -= damping_c*C.val(i,j) ;  // damping term
    00253           C.val(i,j) -= delta;
    00254           //A[i][j] += delta;
    00255           //A_update[i][j] = -eps * (calculateE(x_delay, y_delay,eita) - E_0) / delta;
    00256           //A[i][j] -= delta;
    00257         }
    00258     }
    00259   // apply updates to h,C
    00260   h += h_update.map(squash);
    00261   C += C_update.map(squash);
    00262 };
    00263
    00264 // normal delta rule
    00265 void InvertNChannelController::learnmodel(const Matrix& y_delay){
    00266   Matrix xsi = x_buffer[t%buffersize] -  A * y_delay;
    00267   A += (( xsi*(y_delay^T) ) * eps * factor_a).map(squash);
    00268 };
    00269
    00270 /// calculate delayed values
    00271 Matrix InvertNChannelController::calculateDelayedValues(const Matrix* buffer,
    00272                                                        unsigned int number_steps_of_delay_){
    00273   // number_steps_of_delay must not be smaller than buffersize
    00274   assert (number_steps_of_delay_ < buffersize);
    00275   return buffer[(t - number_steps_of_delay_ + buffersize) % buffersize];
    00276 };
    00277
    00278 Matrix InvertNChannelController::calculateSmoothValues(const Matrix* buffer,
    00279                                                        unsigned int number_steps_for_averaging_){
    00280   // number_steps_for_averaging_ must not be larger than buffersize
    00281   assert (number_steps_for_averaging_ <= buffersize);
    00282
    00283   Matrix result(number_channels,1); // initialised with 0
    00284   for (unsigned int k = 0; k < number_steps_for_averaging_; k++) {
    00285     result += buffer[(t - k + buffersize) % buffersize];
    00286   }
    00287   result *= 1/((double) (number_steps_for_averaging_)); // scalar multiplication
    00288   return result;
    00289 };
    00290
    00291
    00292 /// calculate controller outputs
    00293 /// @param x_smooth Matrix(number_channels,1)
    00294 Matrix InvertNChannelController::calculateControllerValues(const Matrix& x_smooth){
    00295   return (C*x_smooth+h).map(g);
    00296 };
    00297
    00298
    00299 // put new value in ring buffer
    00300 void InvertNChannelController::putInBuffer(Matrix* buffer, const Matrix& vec){
    00301   buffer[t%buffersize] = vec;
    00302 }
    00303
    00304 /** stores the controller values to a given file. */
    00305 bool InvertNChannelController::store(const char* filename){
    00306   // save matrix values
    00307   FILE* f;
    00308   if(!(f = fopen(filename, "w"))) return false;
    00309   storeMatrix(C,f);
    00310   storeMatrix(h,f);
    00311   storeMatrix(A,f);
    00312   fclose(f);
    00313   return true;
    00314 }
    00315
    00316 /** loads the controller values from a given file. */
    00317 bool InvertNChannelController::restore(const char* filename){
    00318   // save matrix values
    00319   FILE* f;
    00320   if(!(f = fopen(filename, "r"))) return false;
    00321   restoreMatrix(C,f);
    00322   restoreMatrix(h,f);
    00323   restoreMatrix(A,f);
    00324   fclose(f);
    00325   t=0; // set time to zero to ensure proper filling of buffers
    00326   return true;
    00327 }
    00328
    00329
    00330 list<Inspectable::iparamkey> InvertNChannelController::getInternalParamNames() const {
    00331   list<iparamkey> keylist;
    00332
    00333   keylist+=store4x4AndDiagonalFieldNames(A,"A");
    00334   keylist+=store4x4AndDiagonalFieldNames(C,"C");
    00335   keylist+=storeMatrixFieldNames(h,"h");
    00336   return keylist;
    00337 }
    00338
    00339 list<Inspectable::iparamval> InvertNChannelController::getInternalParams() const {
    00340   list<iparamval> l;
    00341   l+=store4x4AndDiagonal(A);
    00342   l+=store4x4AndDiagonal(C);
    00343   l+=h.convertToList();
    00344   return l;
    00345 }
    00346
    00347 list<Inspectable::ILayer> InvertNChannelController::getStructuralLayers() const {
    00348   list<Inspectable::ILayer> l;
    00349   l+=ILayer("x",  "",  number_channels, 0, "Sensors");
    00350   l+=ILayer("y",  "H", number_channels, 1, "Motors");
    00351   l+=ILayer("xP", "",  number_channels, 2, "Prediction");
    00352   return l;
    00353 }
    00354
    00355 list<Inspectable::IConnection> InvertNChannelController::getStructuralConnections() const {
    00356   list<Inspectable::IConnection> l;
    00357   l+=IConnection("C", "x", "y");
    00358   l+=IConnection("A", "y", "xP");
    00359   return l;
    00360 }
    00361
    00362
    00363
    00364
    00365
    00366
    00367