Robotsystem from Robot Group Leipzig: invertmotorspace.cpp Source File

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

- File List
- File Members

# invertmotorspace.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  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: invertmotorspace.cpp,v $
    00023  *   Revision 1.24.6.3  2006/03/29 15:12:09  martius
    00024  *   damping for Model
    00025  *
    00026  *   Revision 1.24.6.2  2006/01/18 16:48:35  martius
    00027  *   stored and restore
    00028  *
    00029  *   Revision 1.24.6.1  2005/11/14 14:55:01  martius
    00030  *   *** empty log message ***
    00031  *
    00032  *   Revision 1.24  2005/10/27 15:46:38  martius
    00033  *   inspectable interface is expanded to structural information for network visualiser
    00034  *
    00035  *   Revision 1.23  2005/10/21 11:51:17  martius
    00036  *   id is now toId
    00037  *
    00038  *   Revision 1.22  2005/10/06 17:08:40  martius
    00039  *   switched to stl lists
    00040  *   Bias noise
    00041  *   correct error_factor for controller learning
    00042  *
    00043  *   Revision 1.21  2005/09/27 10:51:39  martius
    00044  *   scaled error_factor
    00045  *
    00046  *   Revision 1.20  2005/08/22 20:32:09  martius
    00047  *   C- initialisation variable
    00048  *
    00049  *   Revision 1.19  2005/08/03 20:31:14  martius
    00050  *   destructor added
    00051  *
    00052  *   Revision 1.18  2005/07/25 12:15:39  fhesse
    00053  *   now C's and R's are available as internal parameters
    00054  *
    00055  *   Revision 1.17  2005/07/21 11:33:01  fhesse
    00056  *   R as internal parameter instead of C
    00057  *
    00058  *   Revision 1.16  2005/07/21 09:41:02  martius
    00059  *   *** empty log message ***
    00060  *
    00061  *   Revision 1.15  2005/07/12 15:05:54  fhesse
    00062  *   model learning decoupled from conroller learning therefore factorA changed to epsA and eps changed to epsC;
    00063  *   squared and logarithmic error functions (in model and in controller learning) and their switches (params rootE and logaE) added
    00064  *
    00065  *   Revision 1.14  2005/07/07 10:25:34  martius
    00066  *   zeta update disabled
    00067  *
    00068  *   Revision 1.13  2005/07/05 14:47:39  martius
    00069  *   desens deleted.
    00070  *   zetaupdate is continuous
    00071  *
    00072  *   Revision 1.12  2005/07/04 15:22:12  martius
    00073  *   inserted desens (which is not really great!)
    00074  *   used x_smooth also for controller learning
    00075  *
    00076  *   Revision 1.11  2005/07/04 08:31:56  martius
    00077  *   delay is used now
    00078  *
    00079  *   Revision 1.10  2005/07/01 08:54:52  martius
    00080  *   use sensor-compliant (zeta-update) rule
    00081  *   fixed bug in bias update rule
    00082  *
    00083  *   Revision 1.9  2005/06/29 08:31:05  martius
    00084  *   Model BIAS
    00085  *
    00086  *   Revision 1.8  2005/06/23 09:47:52  martius
    00087  *   set more reasonable default values for eps and factor_a
    00088  *
    00089  *   Revision 1.7  2005/06/21 15:31:11  martius
    00090  *   getSensorNumber and getMotorMumber added controller interface
    00091  *
    00092  *   Revision 1.6  2005/06/20 09:04:16  martius
    00093  *   init function added to controller-interface
    00094  *
    00095  *   Revision 1.5  2005/06/16 14:23:42  martius
    00096  *   GPL and name creation from CVS added
    00097  *
    00098  ***************************************************************************/
    00099
    00100 #include "invertmotorspace.h"
    00101
    00102 InvertMotorSpace::InvertMotorSpace( int buffersize, double cInit /* = 0.1 */, bool someInternalParams /* = true*/)
    00103   : InvertMotorController(buffersize) {
    00104
    00105   BNoiseGen = 0;
    00106
    00107   this->cInit=cInit;
    00108   this->someInternalParams=someInternalParams;
    00109
    00110   // prepare name;
    00111   Configurable::insertCVSInfo(name, "$RCSfile: invertmotorspace.cpp,v $", "$Revision: 1.24.6.3 $");
    00112 };
    00113
    00114 InvertMotorSpace::~InvertMotorSpace() {
    00115   if(BNoiseGen) free(BNoiseGen);
    00116 }
    00117
    00118 //double somevalue(double){ return 0.3;}
    00119
    00120 void InvertMotorSpace::init(int sensornumber, int motornumber){
    00121
    00122   number_motors  = motornumber;
    00123   number_sensors = sensornumber;
    00124   A.set(number_sensors, number_motors);
    00125   C.set(number_motors,  number_sensors);
    00126   R.set(number_motors, number_motors);
    00127   H.set(number_motors,  1);
    00128   B.set(number_sensors, 1);
    00129
    00130   BNoiseGen = new WhiteUniformNoise();
    00131   BNoiseGen->init(number_sensors);
    00132
    00133   A.toId(); // set a to identity matrix;
    00134   // Matrix Init(number_sensors, number_motors);
    00135   // Init.map(somevalue); // set to constant 0.3;
    00136   A = A;//*0.5; // + Init; //
    00137
    00138   C.toId(); // set a to identity matrix;
    00139   C*= cInit;
    00140   x_buffer = new Matrix[buffersize];
    00141   y_buffer = new Matrix[buffersize];
    00142   for (unsigned int k = 0; k < buffersize; k++) {
    00143     x_buffer[k].set(number_sensors,1);
    00144     y_buffer[k].set(number_motors,1);
    00145   }
    00146
    00147   initialised = true;
    00148 }
    00149
    00150
    00151 /// performs one step (includes learning). Calulates motor commands from sensor inputs.
    00152 void InvertMotorSpace::step(const sensor* x_, int number_sensors,
    00153                             motor* y_, int number_motors){
    00154   fillBuffersAndControl(x_, number_sensors, y_, number_motors);
    00155   if(t>buffersize){
    00156     // get effective motor values which caused present sensor values
    00157     const Matrix& y_effective = y_buffer[(t-int(s4delay))%buffersize];
    00158     const Matrix& x = x_buffer[t%buffersize];
    00159
    00160     // learn controller with effective input/output
    00161     learnController(x, x_smooth, max(int(s4delay)-1,0));
    00162
    00163     // learn Model with actual sensors and with effective motors;
    00164     learnModel(x, y_effective);
    00165   }
    00166   // update step counter
    00167   t++;
    00168 };
    00169
    00170 /// performs one step without learning. Calulates motor commands from sensor inputs.
    00171 void InvertMotorSpace::stepNoLearning(const sensor* x, int number_sensors,
    00172                                             motor*  y, int number_motors ){
    00173   fillBuffersAndControl(x, number_sensors, y, number_motors);
    00174   // update step counter
    00175   t++;
    00176 };
    00177
    00178 void InvertMotorSpace::fillBuffersAndControl(const sensor* x_, int number_sensors,
    00179                                               motor* y_, int number_motors){
    00180   assert((unsigned)number_sensors == this->number_sensors
    00181          && (unsigned)number_motors == this->number_motors);
    00182
    00183   Matrix x(number_sensors,1,x_);
    00184
    00185   // put new input vector in ring buffer x_buffer
    00186   putInBuffer(x_buffer, x);
    00187
    00188   // averaging over the last s4avg values of x_buffer
    00189   x_smooth = calculateSmoothValues(x_buffer, t < s4avg ? 1 : int(s4avg));
    00190
    00191   // calculate controller values based on smoothed input values
    00192   Matrix y = calculateControllerValues(x_smooth);
    00193
    00194   // put new output vector in ring buffer y_buffer
    00195   putInBuffer(y_buffer, y);
    00196
    00197   // convert y to motor*
    00198   y.convertToBuffer(y_, number_motors);
    00199 }
    00200
    00201 /// learn values H,C
    00202 // @param delay 0 for no delay and n>0 for n timesteps delay in the time loop
    00203 void InvertMotorSpace::learnController(const Matrix& x, const Matrix& x_smooth, int delay){
    00204   const Matrix& y = y_buffer[t%buffersize];     // current motors
    00205   const Matrix& y_tm1 = y_buffer[(t-1-delay)%buffersize];     // motors, which correspond to x
    00206   //  const Matrix& x_tm1 = x_buffer[(t-1)%buffersize];
    00207   const Matrix xsi    = x -  (A * y_tm1 + B); // Modelling error in sensor-space
    00208   // eta = A^-1 xsi (first shift in motor-space at current time)
    00209   //  we use pseudoinverse U=A^T A -> eta = U^-1 A^T xsi
    00210   const Matrix eta = (A.multTM()^-1) * ( (A^T) * xsi );
    00211
    00212   // zeta = G' omega
    00213   // RR^T zeta = mu = G'^{-1} eta
    00214
    00215   const Matrix z = C * x_smooth + H;  // Todo: check whether x_smooth should be x
    00216   const Matrix g_prime_inv = z.map(g_s_inv);
    00217   R = C * A;
    00218   const Matrix mu = eta.multrowwise(g_prime_inv); // G'^{-1} eta
    00219   const Matrix zeta = (R.multMT()^-1) * mu; // (RR^T)^{-1} * mu
    00220
    00221   const Matrix v = (R^T) * zeta;
    00222   const Matrix omega = zeta.multrowwise(g_prime_inv);
    00223   // correction of negled terms (LL^T)^-1 from learning rule, which drive the system out of saturation.
    00224   // double f=1;
    00225   //   for(int i=0; i< g_prime_inv.getM(); i++){
    00226   //     f *= g_prime_inv.val(i,0);
    00227   //   }
    00228   //   const Matrix omega = eta * f;
    00229   const Matrix alpha = A * v;
    00230   const Matrix beta = omega.multrowwise(eta).multrowwise(y) * -2;
    00231
    00232   const Matrix zmodel = R * y + H;
    00233   Matrix C_update;
    00234   Matrix H_update;
    00235
    00236   // apply updates to H,C
    00237   if(zetaupdate==0){
    00238     // delta C = eps * (zeta * alpha^T + beta x^T)
    00239     C_update = (zeta * (alpha^T) + beta * (x_smooth^T)) * epsC;
    00240     H_update = (beta * epsC); // delta H = beta
    00241   }else{
    00242     // delta C = eps * (zeta * alpha^T + beta x^T + zeta x^T)
    00243     C_update = (zeta * (alpha^T) + (beta + zeta*zetaupdate) * (x_smooth^T)) * epsC;
    00244     H_update = (beta + zeta*zetaupdate) * epsC;     // delta H = beta + zeta
    00245   }
    00246
    00247   if (!logaE==0){   // using logarithmic error E=ln(v^T*v)
    00248     double temp= 1/(v.multTM().val(0,0)+0.0000001)*0.01;
    00249     C_update *= temp;
    00250     H_update *= temp;
    00251   }
    00252   if (!rootE==0){  // using root error E=(v^T*v)^(1/2)
    00253     double temp= 1/sqrt(v.multTM().val(0,0)+0.0000001)*0.1;
    00254     C_update *= temp;
    00255     H_update *= temp;
    00256   }
    00257
    00258   C += C_update.map(squash);
    00259   H += H_update.map(squash);
    00260 };
    00261
    00262 // normal delta rule
    00263 void InvertMotorSpace::learnModel(const Matrix& x, const Matrix& y){
    00264   Matrix xsi = x -  (A * y + B);
    00265
    00266   Matrix A_update;
    00267   Matrix B_update;
    00268   A_update=(( xsi*(y^T) ) * epsA );
    00269   Matrix B_noise  = noiseMatrix(B.getM(),B.getN(), *BNoiseGen, -noiseB, noiseB); // noise for bias
    00270   B_update=(  xsi * epsA + B_noise) * factorB;
    00271
    00272   if (!logaE==0){   // using logarithmic error E=ln(xsi^T*xsi)
    00273     Matrix xsi2 = (xsi^T)*xsi;
    00274     double temp= 1/(xsi2.val(0,0)+0.0000001);
    00275     A_update *= temp;
    00276     B_update *= temp;
    00277   }
    00278   if (!rootE==0){  // using root error E=(xsi^T*xsi)^(1/2)
    00279     const Matrix xsi2=(xsi^T)*xsi;
    00280     double temp= 1/sqrt(xsi2.val(0,0)+0.0000001);
    00281     A_update *= temp;
    00282     B_update *= temp;
    00283   }
    00284
    00285   A += A_update.map(squash);
    00286   B += B_update.map(squash);
    00287
    00288 };
    00289
    00290 /// calculate controller outputs
    00291 /// @param x_smooth smoothed sensors Matrix(number_channels,1)
    00292 Matrix InvertMotorSpace::calculateControllerValues(const Matrix& x_smooth){
    00293   return (C*x_smooth+H).map(g);
    00294 };
    00295
    00296 /// calculate delayed values
    00297 Matrix InvertMotorSpace::calculateDelayedValues(const Matrix* buffer,
    00298                                                        int number_steps_of_delay_){
    00299   // number_steps_of_delay must not be smaller than buffersize
    00300   assert ((unsigned)number_steps_of_delay_ < buffersize);
    00301   return buffer[(t - number_steps_of_delay_) % buffersize];
    00302 };
    00303
    00304 Matrix InvertMotorSpace::calculateSmoothValues(const Matrix* buffer,
    00305                                                int number_steps_for_averaging_){
    00306   // number_steps_for_averaging_ must not be larger than buffersize
    00307   assert ((int)number_steps_for_averaging_ <= buffersize);
    00308
    00309   Matrix result(buffer[t % buffersize]);
    00310   for (int k = 1; k < number_steps_for_averaging_; k++) {
    00311     result += buffer[(t - k) % buffersize];
    00312   }
    00313   result *= 1/((double) (number_steps_for_averaging_)); // scalar multiplication
    00314   return result;
    00315 };
    00316
    00317
    00318 /** stores the controller values to a given file. */
    00319 bool InvertMotorSpace::store(const char* filename){
    00320   // save matrix values
    00321   FILE* f;
    00322     if(!(f = fopen(filename, "w"))) return false;
    00323   storeMatrix(C,f);
    00324   storeMatrix(H,f);
    00325   storeMatrix(A,f);
    00326   storeMatrix(B,f);
    00327   fclose(f);
    00328   return true;
    00329 }
    00330
    00331 /** loads the controller values from a given file. */
    00332 bool InvertMotorSpace::restore(const char* filename){
    00333   // save matrix values
    00334   FILE* f;
    00335   if(!(f = fopen(filename, "r"))) return false;
    00336   restoreMatrix(C,f);
    00337   restoreMatrix(H,f);
    00338   restoreMatrix(A,f);
    00339   restoreMatrix(B,f);
    00340   fclose(f);
    00341   t=0; // set time to zero to ensure proper filling of buffers
    00342   return true;
    00343 }
    00344
    00345
    00346 list<Inspectable::iparamkey> InvertMotorSpace::getInternalParamNames() const{
    00347   list<iparamkey> keylist;
    00348   if(someInternalParams){
    00349     keylist+=store4x4AndDiagonalFieldNames(A,"A");
    00350     keylist+=store4x4AndDiagonalFieldNames(C,"C");
    00351     keylist+=store4x4AndDiagonalFieldNames(R,"R");
    00352   }else{
    00353     keylist+=storeMatrixFieldNames(A,"A");
    00354     keylist+=storeMatrixFieldNames(C,"C");
    00355     keylist+=storeMatrixFieldNames(R,"R");
    00356   }
    00357   keylist+=storeVectorFieldNames(H,"H");
    00358   keylist+=storeVectorFieldNames(B,"B");
    00359   return keylist;
    00360 }
    00361
    00362 list<Inspectable::iparamval> InvertMotorSpace::getInternalParams() const{
    00363   list<iparamval> l;
    00364   if(someInternalParams){
    00365     l+=store4x4AndDiagonal(A);
    00366     l+=store4x4AndDiagonal(C);
    00367     l+=store4x4AndDiagonal(R);
    00368   }else{
    00369     l+=A.convertToList();
    00370     l+=C.convertToList();
    00371     l+=R.convertToList();
    00372   }
    00373   l+=H.convertToList();
    00374   l+=B.convertToList();
    00375   return l;
    00376 }
    00377
    00378 list<Inspectable::ILayer> InvertMotorSpace::getStructuralLayers() const {
    00379   list<Inspectable::ILayer> l;
    00380   l+=ILayer("x","", number_sensors, 0, "Sensors");
    00381   l+=ILayer("y","H", number_motors, 1,"Motors");
    00382   l+=ILayer("xP","B", number_sensors, 2,"Prediction");
    00383   return l;
    00384 }
    00385
    00386 list<Inspectable::IConnection> InvertMotorSpace::getStructuralConnections() const {
    00387   list<Inspectable::IConnection> l;
    00388   l+=IConnection("C", "x", "y");
    00389   l+=IConnection("A", "y", "xP");
    00390   return l;
    00391 }
    00392