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