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: controller_misc.h,v $ 00023 * Revision 1.11 2010/01/26 09:49:37 martius 00024 * added power function for matrix map 00025 * 00026 * Revision 1.10 2009/08/05 22:48:27 martius 00027 * changed many function for mapP to a normal double parameter 00028 * 00029 * Revision 1.9 2008/05/30 11:58:27 martius 00030 * use cmath instead of math.h 00031 * 00032 * Revision 1.8 2008/05/07 16:45:52 martius 00033 * code cosmetics and documentation 00034 * 00035 * Revision 1.7 2008/04/30 14:54:48 guettler 00036 * support for new matrix indices integrated 00037 * 00038 * Revision 1.6 2008/04/29 15:31:44 guettler 00039 * modified toBinary map functions 00040 * 00041 * Revision 1.5 2008/04/29 10:29:39 guettler 00042 * added toBinaryWithProbability for mapP 00043 * 00044 * Revision 1.4 2008/04/28 15:32:46 guettler 00045 * doxygen corrected 00046 * 00047 * Revision 1.3 2008/04/25 10:31:18 guettler 00048 * added toBinaryWithThreshold function for map2 00049 * 00050 * Revision 1.2 2008/04/17 14:54:45 martius 00051 * randomGen added, which is a random generator with long period and an 00052 * internal state. Each Agent has an instance and passed it to the controller 00053 * and the wiring. This is good for 00054 * a) repeatability on agent basis, 00055 * b) parallel execution as done in ode_robots 00056 * 00057 * Revision 1.1 2008/04/16 12:44:16 martius 00058 * moved to utils 00059 * 00060 * Revision 1.24 2008/03/01 01:47:34 martius 00061 * plus_ and lowercutof 00062 * 00063 * Revision 1.23 2007/12/13 16:43:24 martius 00064 * noiseMatrix adapted 00065 * 00066 * Revision 1.22 2007/12/11 14:43:27 martius 00067 * clipping for matrix mapP 00068 * 00069 * Revision 1.21 2007/08/24 12:02:22 martius 00070 * getKSmallestElement 00071 * 00072 * Revision 1.20 2007/07/16 08:51:57 martius 00073 * argmax 00074 * 00075 * Revision 1.19 2007/06/21 16:27:43 martius 00076 * min and max for matrices 00077 * 00078 * Revision 1.18 2007/06/08 15:44:57 martius 00079 * constant function 00080 * 00081 * Revision 1.17 2007/05/22 16:01:45 martius 00082 * argmin 00083 * 00084 * Revision 1.16 2007/03/22 08:07:47 robot3 00085 * added two methods for generating names and a list from arrays for the interface 00086 * inspectable: getInternalParams() and getInternalParamNames() 00087 * 00088 * Revision 1.15 2007/03/22 08:05:03 robot3 00089 * this is an adapter class which implements all needed things. This class is 00090 * used for example by the DiscreteControllerAdapter. 00091 * 00092 * Revision 1.14 2007/02/20 15:41:06 martius 00093 * big model stuff, elman and co 00094 * 00095 * Revision 1.13 2007/02/02 11:16:18 martius 00096 * getKthLargestElementc 00097 * 00098 * Revision 1.12 2006/07/20 17:14:34 martius 00099 * removed std namespace from matrix.h 00100 * storable interface 00101 * abstract model and invertablemodel as superclasses for networks 00102 * 00103 * Revision 1.11 2006/07/14 12:23:57 martius 00104 * selforg becomes HEAD 00105 * 00106 * Revision 1.9.6.4 2006/07/10 11:59:23 martius 00107 * Matrixlib now in selforg 00108 * no namespace std in header files 00109 * 00110 * Revision 1.9.6.3 2006/01/17 16:58:39 martius 00111 * loading and storing 00112 * 00113 * Revision 1.9.6.2 2005/12/15 17:04:39 martius 00114 * min, max and so on are template functions now 00115 * 00116 * Revision 1.9.6.1 2005/11/15 12:30:27 martius 00117 * new selforg structure and OdeAgent, OdeRobot ... 00118 * 00119 * Revision 1.9 2005/11/10 09:13:33 martius 00120 * readded defines like min, max sqr,... 00121 * 00122 * Revision 1.8 2005/10/27 14:17:17 martius 00123 * some functions are now found in mathutils.h (dir: ../ode_robots/utils) 00124 * 00125 * Revision 1.7 2005/10/21 11:50:33 martius 00126 * adapt functions 00127 * random number function for usage with Matrix::map 00128 * 00129 * Revision 1.6 2005/10/06 17:06:57 martius 00130 * switched to stl lists 00131 * 00132 * Revision 1.5 2005/09/22 10:33:07 martius 00133 * max and min more save 00134 * 00135 * Revision 1.4 2005/08/29 09:05:31 fhesse 00136 * removed weird bug in get4x4AndDiagonalSize, probably caused by some compiler optimization 00137 * 00138 * Revision 1.3 2005/08/06 20:47:54 martius 00139 * Commented 00140 * 00141 * Revision 1.2 2005/07/29 14:26:56 martius 00142 * fixed bug in length calculation of 4x4 diagonal routines 00143 * 00144 * Revision 1.1 2005/07/28 11:14:52 martius 00145 * helper functions for controller 00146 * support for internal parameters more easy 00147 * 00148 * 00149 ***************************************************************************/ 00150 #ifndef __CONTOLLER_MISC_H 00151 #define __CONTOLLER_MISC_H 00152 00153 #include <selforg/matrix.h> 00154 #include <assert.h> 00155 #include <stdlib.h> 00156 #include <cmath> 00157 00158 #include "stl_adds.h" 00159 #include "noisegenerator.h" 00160 #include "inspectable.h" 00161 00162 00163 template<typename T> 00164 inline T sign(T v) { return v<(T)0?(T)-1:(T)1; } 00165 00166 template<typename T> 00167 inline T sqr(T v) { return v*v; } 00168 00169 template<typename T> 00170 inline T clip(T x, T lobound, T highbound) { 00171 return (x)<(lobound) ? (lobound) : ( (x) > (highbound) ? (highbound) : (x) ); 00172 } 00173 00174 00175 /// calculates 1/x 00176 double one_over(double x); 00177 00178 /// returns c (useful for Matrix::mapP to fill matrix with constant value) 00179 double constant(double c, double); 00180 00181 /// returns x the power c (as a double) 00182 double power(void* c, double x); 00183 00184 /// creates random number from -1 to 1 00185 double random_minusone_to_one(double); 00186 00187 /// creates random number from -1 to 1 00188 // using the RandGen* which is give as a void pointer (Matrix::mapP) 00189 double random_minusone_to_one(void *, double); 00190 00191 /// clipping function to the interval [-r, r] (for use with mapP) 00192 double clip(double r,double); 00193 00194 /// cutof function for mapP 00195 double lowercutof(void* theta, double); 00196 00197 /// returns -1 if probability is to low, otherwise 1 for mapP 00198 double toBinaryWithProbability(void* r,double x); 00199 00200 /// returns -1 if below threshold, otherwise 1 for map2 00201 double toBinaryWithThreshold(double x, double threshold); 00202 00203 /// plus function for mapP 00204 double plus_(double b, double a); 00205 00206 00207 /** stores at least left top 4x4 submatrix (row-wise) (if exists) and 00208 then the rest of the diagonal elements into a list 00209 @return list of values 00210 */ 00211 std::list<matrix::D> store4x4AndDiagonal(const matrix::Matrix& m); 00212 00213 /** stores at least left top 4x4 submatrix (row-wise) (if exists) and 00214 then the rest of the diagonal elements 00215 @param m matrix to be stored 00216 @param len Length of the provided buffer 00217 (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 00218 @return number of actually written elements 00219 */ 00220 matrix::I store4x4AndDiagonal(const matrix::Matrix& m, matrix::D* buffer, matrix::I len); 00221 00222 /** returns the number of elements stored by store4x4AndDiagonal 00223 (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 00224 */ 00225 matrix::I get4x4AndDiagonalSize(const matrix::Matrix& m); 00226 00227 /** writes the names of the fields stored by store4x4AndDiagonal into a list 00228 @param m matrix to be stored 00229 @param matrixName name of the matrix (prefix for all fields) 00230 @param keylist list for field names 00231 @param len Length of the provided buffer 00232 (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 00233 @return number of actually written elements 00234 */ 00235 std::list<Inspectable::iparamkey> store4x4AndDiagonalFieldNames(const matrix::Matrix& m, 00236 const std::string& matrixName); 00237 00238 /** stores the names of the fields stored by store4x4AndDiagonal 00239 @param m matrix to be stored 00240 @param matrixName name of the matrix (prefix for all fields) 00241 @param keylist list for field names 00242 @param len Length of the provided buffer 00243 (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 00244 @return number of actually written elements 00245 */ 00246 matrix::I store4x4AndDiagonalFieldNames(const matrix::Matrix& m, 00247 const std::string& matrixName, 00248 char** keylist, matrix::I len); 00249 00250 /** stores the names of the all matrix fieldnames produces by convertToBuffer into a list 00251 @return list of names 00252 */ 00253 std::list<Inspectable::iparamkey> storeMatrixFieldNames(const matrix::Matrix& m, const std::string& matrixName); 00254 00255 /** stores the names of the all vector (mx1 matrix) fieldnames produces by convertToBuffer into a list 00256 @return list of names 00257 */ 00258 std::list<Inspectable::iparamkey> storeVectorFieldNames(const matrix::Matrix& m, const std::string& vectorName); 00259 00260 /** stores the names of the all matrix fieldnames produces by convertToBuffer 00261 @param m matrix to be stored 00262 @param matrixName name of the matrix (prefix for all fields) 00263 @param keylist list for field names 00264 @param len Length of the provided buffer 00265 (should be getN()*getM() 00266 @return number of actually written elements 00267 */ 00268 matrix::I storeMatrixFieldNames(const matrix::Matrix& m, const char* matrixName, 00269 char** keylist, matrix::I len); 00270 00271 /** stores the names of the all vector (mx1 matrix) fieldnames produces by convertToBuffer 00272 @param m vector to be stored 00273 @param vectorName name of the vector (prefix for all fields) 00274 @param keylist list for field names 00275 @param len Length of the provided buffer (should be getM()) 00276 @return number of actually written elements 00277 */ 00278 matrix::I storeVectorFieldNames(const matrix::Matrix& m, const char* vectorName, 00279 char** keylist, matrix::I len); 00280 00281 /** stores the Matrix into the given file stream (binary) 00282 */ 00283 bool storeMatrix(const matrix::Matrix& m, FILE* f); 00284 00285 /** reads a Matrix from the given file stream (binary) 00286 */ 00287 bool restoreMatrix(matrix::Matrix& m, FILE* f); 00288 00289 /// returns a Matrix with values generated by the given noise generator 00290 matrix::Matrix noiseMatrix(matrix::I m, matrix::I n, NoiseGenerator& ng, 00291 double strength,double unused = 0); 00292 00293 /** calculates to linear matrix norm (sum of absolute values divided by number of values (m*n) ) 00294 */ 00295 double matrixNorm1(const matrix::Matrix& m); 00296 00297 /** calculates to square matrix norm (sum of square values divided by number of values (m*n) ) 00298 */ 00299 double matrixNorm2(const matrix::Matrix& m); 00300 00301 00302 /** returns the k. largest element of the matrix 00303 Attention: it will detroy the given matrix! (sorting) 00304 Assumption: k>0 and k<=matrixsize 00305 */ 00306 double getKthLargestElement(matrix::Matrix& matrix, matrix::I k); 00307 00308 /** returns the k. smallest element of the matrix 00309 Attention: it will detroy the given matrix! (sorting) 00310 Assumption: k>0 and k<=matrixsize 00311 */ 00312 double getKthSmallestElement(matrix::Matrix& matrix, matrix::I k); 00313 00314 /// considers the matrix as vector (mx1) and returns the index of the smallest element 00315 matrix::I argmin(const matrix::Matrix& v); 00316 00317 /// considers the matrix as vector (mx1) and returns the index of the largest element 00318 matrix::I argmax(const matrix::Matrix& v); 00319 00320 /// returns the smallest element 00321 double min(const matrix::Matrix& v); 00322 00323 /// minimum function for doubles without templates 00324 double min(double a, double b); 00325 00326 /// returns the largest element 00327 double max(const matrix::Matrix& v); 00328 00329 /// maximum function for doubles without templates 00330 double max(double a, double b); 00331 00332 00333 /// samples from the pdf (rowwise stored with sum = 1) 00334 matrix::I sample(const matrix::Matrix& pdf); 00335 00336 00337 /// parameter adaptation algorithm. 00338 // @param p current parameter value 00339 // @param actual actual value of some size controlled by p 00340 // @param nominal nominal value of some size controlled by p 00341 // @param up_rate adaptation rate for increasing p (<< 1) 00342 // @param down_rate adaptation rate for decreasing p (<< 1) 00343 // @return new value of p (there is no clipping done) 00344 double adapt(double p, double actual, double nominal, double up_rate, double down_rate); 00345 00346 /// like adapt but that the adaption is just done if the actual value is outside the given interval (_min, _max) 00347 double adaptMinMax(double p, double actual, double _min, double _max, double up_rate, double down_rate); 00348 00349 /** 00350 * Helper function for converting an array with double values to a list. 00351 * Is used for the method getInternalParams() interface inspectable. 00352 */ 00353 std::list<Inspectable::iparamval> convertArrayToList(double* array,int arraySize); 00354 /** 00355 * Helper function for getting the array names of an array 00356 * Is used for the method getInternalParamNames() interface inspectable. 00357 */ 00358 std::list<Inspectable::iparamkey> getArrayNames(int arraySize,const char* name); 00359 00360 #endif