Robotsystem of the Robot Group Leipzig: invertmotorcontroller.h Source File

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

- Search for |

- File List
- File Members

# invertmotorcontroller.h

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  *   LICENSE:                                                              *
    00009  *   This work is licensed under the Creative Commons                      *
    00010  *   Attribution-NonCommercial-ShareAlike 2.5 License. To view a copy of   *
    00011  *   this license, visit http://creativecommons.org/licenses/by-nc-sa/2.5/ *
    00012  *   or send a letter to Creative Commons, 543 Howard Street, 5th Floor,   *
    00013  *   San Francisco, California, 94105, USA.                                *
    00014  *                                                                         *
    00015  *   This program is distributed in the hope that it will be useful,       *
    00016  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00017  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.                  *
    00018  *                                                                         *
    00019  *   $Log: invertmotorcontroller.h,v $
    00020  *   Revision 1.23  2006/08/02 09:30:16  martius
    00021  *   virtualized new calc functions
    00022  *   calcErrorFactor added
    00023  *
    00024  *   Revision 1.22  2006/07/20 17:14:34  martius
    00025  *   removed std namespace from matrix.h
    00026  *   storable interface
    00027  *   abstract model and invertablemodel as superclasses for networks
    00028  *
    00029  *   Revision 1.21  2006/07/18 15:17:16  martius
    00030  *   buffer operations like delayed values and smoothed values moved to invertmotorcontroller
    00031  *
    00032  *   Revision 1.20  2006/07/18 14:47:15  martius
    00033  *   new regularisation of g' and g''/g'
    00034  *   g''/g' uses the same clipped z which used for g'
    00035  *   sqashing function with variable squash size
    00036  *
    00037  *   Revision 1.19  2006/07/14 12:23:58  martius
    00038  *   selforg becomes HEAD
    00039  *
    00040  *   Revision 1.17.6.10  2006/07/10 13:05:16  martius
    00041  *   NON-COMMERICAL LICENSE added to controllers
    00042  *
    00043  *                                                                         *
    00044  ***************************************************************************/
    00045
    00046 #ifndef __INVERTMOTORCONTROLLER_H
    00047 #define __INVERTMOTORCONTROLLER_H
    00048
    00049 #include "abstractcontroller.h"
    00050 #include "controller_misc.h"
    00051 #include <stdlib.h>
    00052 #include <string.h>
    00053
    00054
    00055 /**
    00056  * Abstract class (interface) for robot controller that use direct matrix inversion and
    00057  * a time loop in motor domain
    00058  *
    00059  * Implements standart configureable interface for some useful parameters like  epsC, epsA, s4avg ...
    00060  */
    00061 class InvertMotorController : public AbstractController {
    00062 public:
    00063   InvertMotorController( unsigned short buffersize ,
    00064                          const std::string& name, const std::string& revision)
    00065     : AbstractController(name, revision){
    00066     this->buffersize = buffersize;
    00067     epsC=0.1;    // base learning rate
    00068     desens=0.0; // name form desensitization
    00069     s4delay=1;
    00070     s4avg=1;
    00071     steps=1;
    00072     epsA=0.1; // learning rate factor for model learning
    00073     dampA=0;  // no damping
    00074     factorB=0.2; // additional learning rate factor for model bias
    00075     zetaupdate=0; // don't use the zeta update line
    00076     squashSize=0.01; // maximum size of update components
    00077     logaE=0; // don't use logarithmic error ( ln(v^T*v) )
    00078     rootE=0; // don't use logarithmic error ( (v^T*v)^(1/2) )
    00079     relativeE=0; // don't use relative modelling error
    00080     adaptRate=0.005; // factor used for adapting the lerning rates.
    00081     nomUpdate=0.005; // size of nominal update in respect to matrix norm
    00082     noiseB = 0.001;   // noise that is added to bias update (before factorB is applied)
    00083     noiseY = 0.1;   // noise that is added to motor values
    00084     teacher = 5;      // factor for teaching signal
    00085     t=0;
    00086     initialised = false;
    00087   }
    00088
    00089   /***** CONFIGURABLE ******/
    00090
    00091   virtual paramval getParam(const paramkey& key) const{
    00092     if(key == "epsC") return epsC;
    00093     else if(key == "epsA") return epsA;
    00094     else if(key == "dampA") return dampA;
    00095     else if(key == "adaptrate") return adaptRate;
    00096     else if(key == "nomupdate") return nomUpdate;
    00097     else if(key == "desens") return desens;
    00098     else if(key == "s4delay") return s4delay;
    00099     else if(key == "s4avg") return s4avg;
    00100     else if(key == "steps") return steps;
    00101     else if(key == "zetaupdate") return zetaupdate;
    00102     else if(key == "squashsize") return squashSize;
    00103     else if(key == "logaE") return logaE;
    00104     else if(key == "rootE") return rootE;
    00105     else if(key == "relativeE") return relativeE;
    00106     else if(key == "factorB") return factorB;
    00107     else if(key == "noiseB") return noiseB;
    00108     else if(key == "noiseY") return noiseY;
    00109     else if(key == "teacher") return teacher;
    00110     else  return AbstractController::getParam(key);
    00111   }
    00112
    00113   virtual bool setParam(const paramkey& key, paramval val){
    00114     if(key == "epsC") epsC=val;
    00115     else if(key == "epsA") epsA=val;
    00116     else if(key == "dampA") dampA=val;
    00117     else if(key == "adaptrate") adaptRate=val;
    00118     else if(key == "nomupdate") nomUpdate=val;
    00119     else if(key == "desens") desens=val;
    00120     else if(key == "s4delay") s4delay=val;
    00121     else if(key == "s4avg") s4avg=val;
    00122     else if(key == "steps") steps=val;
    00123     else if(key == "zetaupdate") zetaupdate=val;
    00124     else if(key == "squashsize") squashSize=val;
    00125     else if(key == "logaE") logaE=(short)val;
    00126     else if(key == "rootE") rootE=(short)val;
    00127     else if(key == "relativeE") relativeE=(short)val;
    00128     else if(key == "factorB") factorB=val;
    00129     else if(key == "noiseB") noiseB=val;
    00130     else if(key == "noiseY") noiseY=val;
    00131     else if(key == "teacher") teacher=val;
    00132     else return AbstractController::setParam(key, val);
    00133     //    else fprintf(stderr, "parameter %s unknown\n", key);
    00134     return true;
    00135   }
    00136
    00137   virtual paramlist getParamList() const{
    00138     paramlist list;
    00139     list.push_back(std::pair<paramkey, paramval> ("epsA", epsA));
    00140     list.push_back(std::pair<paramkey, paramval> ("epsC", epsC));
    00141     list.push_back(std::pair<paramkey, paramval> ("dampA", dampA));
    00142     list.push_back(std::pair<paramkey, paramval> ("adaptrate", adaptRate));
    00143     list.push_back(std::pair<paramkey, paramval> ("nomupdate", nomUpdate));
    00144     list.push_back(std::pair<paramkey, paramval> ("desens", desens));
    00145     list.push_back(std::pair<paramkey, paramval> ("s4delay", s4delay ));
    00146     list.push_back(std::pair<paramkey, paramval> ("s4avg", s4avg));
    00147     list.push_back(std::pair<paramkey, paramval> ("steps", steps));
    00148     list.push_back(std::pair<paramkey, paramval> ("zetaupdate", zetaupdate));
    00149     list.push_back(std::pair<paramkey, paramval> ("squashsize", squashSize));
    00150     list.push_back(std::pair<paramkey, paramval> ("logaE", logaE));
    00151     list.push_back(std::pair<paramkey, paramval> ("rootE", rootE));
    00152     list.push_back(std::pair<paramkey, paramval> ("relativeE", relativeE));
    00153     list.push_back(std::pair<paramkey, paramval> ("factorB", factorB));
    00154     list.push_back(std::pair<paramkey, paramval> ("noiseB", noiseB));
    00155     list.push_back(std::pair<paramkey, paramval> ("noiseY", noiseY));
    00156     list.push_back(std::pair<paramkey, paramval> ("teacher", teacher));
    00157     return list;
    00158   }
    00159
    00160 protected:
    00161   paramval epsC; //< learning rate factor for controller learning
    00162   paramval desens;
    00163   paramval s4delay; //< number of timesteps of delay in the SML
    00164   paramval s4avg; //< number of timesteps used for smoothing the controller output values
    00165   paramval steps; //< number of timesteps is used for controller learning
    00166   paramval epsA; //< learning rate factor for model learning
    00167   paramval factorB; //< additional learning rate factor for model bias
    00168   paramval zetaupdate;
    00169   paramval dampA; //< damping term for model (0: no damping, 0.1 high damping)
    00170   short logaE;  //< logarithmic error is used for learning 1: controller 2: model 3: both
    00171   short rootE;  //< root error is used for learning 1: controller 2: model 3: both
    00172   short relativeE; //< if not 0: a relative error signal is used (xsi is normalised in respect to |y|)
    00173
    00174   paramval squashSize; //< size of the box, where the parameter updates are clipped to
    00175   paramval adaptRate; //< adaptation rate for learning rate adaptation
    00176   paramval nomUpdate; //< nominal update of controller in respect to matrix norm
    00177   paramval noiseB;    //< size of the additional noise for model bias B
    00178   paramval noiseY;    //< size of the additional noise for motor values
    00179   paramval teacher; //< factor for teaching signal
    00180
    00181   int t;
    00182   unsigned short buffersize;
    00183   std::string name;
    00184   bool initialised;
    00185
    00186 protected:
    00187   // put new value in ring buffer
    00188   void putInBuffer(matrix::Matrix* buffer, const matrix::Matrix& vec, int delay = 0){
    00189     buffer[(t-delay)%buffersize] = vec;
    00190   }
    00191
    00192   /// calculate delayed values
    00193   virtual matrix::Matrix calculateDelayedValues(const matrix::Matrix* buffer,
    00194                                                   int number_steps_of_delay_){
    00195     // number_steps_of_delay must not be smaller than buffersize
    00196     assert ((unsigned)number_steps_of_delay_ < buffersize);
    00197     return buffer[(t - number_steps_of_delay_) % buffersize];
    00198   };
    00199
    00200   /// calculate time-smoothed values
    00201   virtual matrix::Matrix calculateSmoothValues(const matrix::Matrix* buffer,
    00202                                                  int number_steps_for_averaging_){
    00203     // number_steps_for_averaging_ must not be larger than buffersize
    00204     assert ((int)number_steps_for_averaging_ <= buffersize);
    00205
    00206     matrix::Matrix result(buffer[t % buffersize]);
    00207     for (int k = 1; k < number_steps_for_averaging_; k++) {
    00208       result += buffer[(t - k) % buffersize];
    00209     }
    00210     result *= 1/((double) (number_steps_for_averaging_)); // scalar multiplication
    00211     return result;
    00212   };
    00213
    00214   /// calculates the error_factor for either logarithmic (E=ln(e^T*e)) or square (E=sqrt(e^t*e)) error
    00215   virtual double calcErrorFactor(const matrix::Matrix& e, bool loga, bool root) {
    00216     double error_factor = 1;
    00217     if (loga){   // using logarithmic error E=ln(v^T*v)
    00218       error_factor= 1/(e.multTM().val(0,0)+0.000001)*0.01; // factor 1/100 for normalising (empirically)
    00219     }
    00220     if (root){  // using root error E=(v^T*v)^(1/2)
    00221       error_factor= 1/sqrt(e.multTM().val(0,0)+0.000001)*0.1; // factor 1/10 for normalising (empirically)
    00222     }
    00223     return error_factor;
    00224   }
    00225
    00226
    00227   /// neuron transfer function
    00228   static double g(double z)
    00229   {
    00230     return tanh(z);
    00231   };
    00232
    00233   /// first dervative
    00234   static double g_s(double z)
    00235   {
    00236     double k=tanh(z);
    00237     return 1.025 - k*k;
    00238     // return 1/(1+z*z);    // softer
    00239     //return 1/(1+log(1+z*z)); // even softer
    00240   };
    00241
    00242   /// inverse of the first derivative
    00243   static double g_s_inv(double z)
    00244   {
    00245     double k=tanh(z);
    00246     return 1/(1.025 - k*k);
    00247     // return 1+z*z; // softer
    00248     //return 1+log(1+z*z); // even softer
    00249   };
    00250
    00251
    00252   /// an exact formula for inversion if neuron.
    00253   //  zeta = (1/(g'(z , xsi)) * xsi
    00254   static double g_s(double z, double xsi) {
    00255     double Z = clip(z, -3.0, 3.0) + clip(xsi, -1.0, 1.0);
    00256     double k=tanh(Z);  // approximation with Mittelwertsatz
    00257     return 1 - k*k;
    00258     /*   exact formula, which does not work for some reason: */
    00259     /*   double s = - fabs(xsi) * sign(z); */
    00260     /*     // for s some assumption have to be hold s must be in the inverval (- 2/(e^-2|z|+1), 2/(e^-2|z|+1)) without 0. */
    00261     /*     // in case of s=0 we return g' */
    00262     /*     if(fabs(s) < 0.0001) return g_s(z); // avoid singularities */
    00263     /*     else{ */
    00264     /*       double e_2z = exp(2*z); */
    00265     /*       double limit = 1.9/(exp(-2*fabs(z)) +1); */
    00266     /*       s = clip(s, -limit, limit); */
    00267     /*       return 2 * s * 1/(log( (2+(1/e_2z+1)*s) / (2 - (e_2z+1) * s) )); */
    00268     /*     } */
    00269   };
    00270
    00271   /// an exact formula for g''/g'
    00272   static double g_2s_div_s(double z, double xsi) {
    00273     // for consistency reasons we use the same clipped z as for g'.
    00274     double Z = clip(z, -3.0, 3.0) + clip(xsi, -1.0, 1.0);
    00275     // approximation with Mittelwertsatz (z is clipped)
    00276     return -2*g(Z);
    00277
    00278     // return -2*g(z);  // <- this is actually exact
    00279
    00280 /*   exact formula, which does not work for some reason: */
    00281 /*     double s = - fabs(xsi) * sign(z); */
    00282 /*     // for s some assumption have to be hold s must be in the inverval (- 2/(e^-2|z|+1), 2/(e^-2|z|+1)) without 0. */
    00283 /*     // in case of s=0 we return g''/g' = -2 g */
    00284 /*     if(fabs(s) < 0.0001) { */
    00285 /*       return -2*g(z); // avoid singularities */
    00286 /*     } else{ */
    00287 /*       double e_m2z = exp(-2*z); */
    00288 /*       double limit = 1.9/(exp(-2*fabs(z)) +1 ); */
    00289 /*       s = clip(s, -limit, limit); */
    00290 /*       return q_s(z,xsi) * ( ( sqr(1+e_m2z)*s + 2*(1 - sqr(e_m2z)) ) */
    00291 /*                          / ( sqr(1+e_m2z)*s*s + 2*(1-sqr(e_m2z))*s - 4*e_m2z ) ); */
    00292 /*     } */
    00293   };
    00294
    00295   /// squashing function (-0.1 to 0.1), to protect against to large weight updates
    00296   static double squash(double z)
    00297   {
    00298     return z < -0.1 ? -0.1 : ( z > 0.1 ? 0.1 : z );
    00299     //return 0.1 * tanh(10.0 * z);
    00300   };
    00301
    00302   /// squashing function with adjustable clipping size, to protect against too large weight updates
    00303   static double squash(void* d, double z) {
    00304     double size = *((double*)d);
    00305     return z < -size ? -size : ( z > size ? size : z );
    00306   };
    00307
    00308 };
    00309
    00310 #endif