Robot Simulator of the Robotics Group for Self-Organization of Control: controller_misc.h Source File

# controller_misc.h

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005-2011 LpzRobots development team                    *
    00003  *    Georg Martius  <georg dot martius at web dot de>                     *
    00004  *    Frank Guettler <guettler at informatik dot uni-leipzig dot de        *
    00005  *    Frank Hesse    <frank at nld dot ds dot mpg dot de>                  *
    00006  *    Ralf Der       <ralfder at mis dot mpg dot de>                       *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  ***************************************************************************/
    00024 #ifndef __CONTOLLER_MISC_H
    00025 #define __CONTOLLER_MISC_H
    00026
    00027 #include <selforg/matrix.h>
    00028 #include <assert.h>
    00029 #include <stdlib.h>
    00030 #include <cmath>
    00031
    00032 #include "stl_adds.h"
    00033 #include "noisegenerator.h"
    00034 #include "inspectable.h"
    00035
    00036
    00037 template<typename T>
    00038 inline T sign(T v) { return v<(T)0?(T)-1:(T)1; }
    00039
    00040 template<typename T>
    00041 inline T sqr(T v) { return v*v; }
    00042
    00043 template<typename T>
    00044 inline T clip(T x, T lobound, T highbound) {
    00045   return (x)<(lobound) ? (lobound) : ( (x) > (highbound) ? (highbound) : (x) );
    00046 }
    00047
    00048
    00049 /// calculates 1/x
    00050 double one_over(double x);
    00051
    00052 /// returns c (useful for Matrix::mapP to fill matrix with constant value)
    00053 double constant(double c, double);
    00054
    00055 /// returns x the power c (as a double)
    00056 double power(void* c, double x);
    00057
    00058 /// power 3 for matrix::map
    00059 double power3(double x);
    00060
    00061 /// creates random number from -1 to 1
    00062 double random_minusone_to_one(double);
    00063
    00064 /// creates random number from -1 to 1
    00065 //  using the RandGen* which is give as a void pointer (Matrix::mapP)
    00066 double random_minusone_to_one(void *, double);
    00067
    00068 /// clipping function to the interval [-r, r] (for use with mapP)
    00069 double clip(double r,double);
    00070
    00071 /// cutof function for mapP
    00072 double lowercutof(void* theta, double);
    00073
    00074 /// returns -1 if probability is to low, otherwise 1 for mapP
    00075 double toBinaryWithProbability(void* r,double x);
    00076
    00077 /// returns -1 if below threshold, otherwise 1 for map2
    00078 double toBinaryWithThreshold(double x, double threshold);
    00079
    00080 /// plus function for mapP
    00081 double plus_(double b, double a);
    00082
    00083
    00084 /** stores at least left top 4x4 submatrix (row-wise) (if exists) and
    00085    then the rest of the diagonal elements into a list
    00086    @return list of values
    00087 */
    00088 std::list<matrix::D> store4x4AndDiagonal(const matrix::Matrix& m);
    00089
    00090 /** stores at least left top 4x4 submatrix (row-wise) (if exists) and
    00091    then the rest of the diagonal elements
    00092   @param m matrix to be stored
    00093   @param len Length of the provided buffer
    00094   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00095   @return number of actually written elements
    00096 */
    00097 matrix::I store4x4AndDiagonal(const matrix::Matrix& m, matrix::D* buffer, matrix::I len);
    00098
    00099 /** returns the number of elements stored by store4x4AndDiagonal
    00100   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00101 */
    00102 matrix::I get4x4AndDiagonalSize(const matrix::Matrix& m);
    00103
    00104 /** writes the names of the fields stored by store4x4AndDiagonal into a list
    00105   @param m matrix to be stored
    00106   @param matrixName name of the matrix (prefix for all fields)
    00107   @return list of keys
    00108 */
    00109 std::list<Inspectable::iparamkey> store4x4AndDiagonalFieldNames(const matrix::Matrix& m,
    00110                                                                 const std::string& matrixName);
    00111
    00112 /** stores the names of the fields stored by store4x4AndDiagonal
    00113   @param m matrix to be stored
    00114   @param matrixName name of the matrix (prefix for all fields)
    00115   @param keylist list for field names
    00116   @param len Length of the provided buffer
    00117   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00118   @return number of actually written elements
    00119 */
    00120 matrix::I store4x4AndDiagonalFieldNames(const matrix::Matrix& m,
    00121                                         const std::string& matrixName,
    00122                                         char** keylist, matrix::I len);
    00123
    00124 /** stores the names of all matrix fieldnames
    00125   in the order produced by convertToBuffer (row-wise)
    00126   @return list of names
    00127 */
    00128 std::list<Inspectable::iparamkey> storeMatrixFieldNames(const matrix::Matrix& m, const std::string& matrixName);
    00129
    00130 /** stores the names of all vector (mx1 or 1xn matrix) fieldnames
    00131   in the order produced by convertToBuffer (row-wise)
    00132   @return list of names
    00133 */
    00134 std::list<Inspectable::iparamkey> storeVectorFieldNames(const matrix::Matrix& m, const std::string& vectorName);
    00135
    00136 /** stores the names of all matrix fieldnames
    00137    in the order produced by convertToBuffer (row-wise)
    00138   @param m matrix to be stored
    00139   @param matrixName name of the matrix (prefix for all fields)
    00140   @param keylist list for field names
    00141   @param len Length of the provided buffer
    00142   (should be getN()*getM()
    00143   @return number of actually written elements
    00144 */
    00145 matrix::I storeMatrixFieldNames(const matrix::Matrix& m, const char* matrixName,
    00146                         char** keylist, matrix::I len);
    00147
    00148 /** stores the names of all vector (mx1 or 1xn matrix) fieldnames
    00149   in the order produced by convertToBuffer (row-wise)
    00150   @param m vector to be stored
    00151   @param vectorName name of the vector (prefix for all fields)
    00152   @param keylist list for field names
    00153   @param len Length of the provided buffer (should be getM())
    00154   @return number of actually written elements
    00155 */
    00156 matrix::I storeVectorFieldNames(const matrix::Matrix& m, const char* vectorName,
    00157                                 char** keylist, matrix::I len);
    00158
    00159 /** stores the Matrix into the given file stream (binary)
    00160  */
    00161 bool storeMatrix(const matrix::Matrix& m, FILE* f);
    00162
    00163 /** reads a Matrix from the given file stream (binary)
    00164  */
    00165 bool restoreMatrix(matrix::Matrix& m, FILE* f);
    00166
    00167 /// returns a Matrix with values generated by the given noise generator
    00168 matrix::Matrix noiseMatrix(matrix::I m, matrix::I n, NoiseGenerator& ng,
    00169                            double strength,double unused = 0);
    00170
    00171 /** calculates to linear matrix norm (sum of absolute values divided by number of values (m*n) )
    00172 */
    00173 double matrixNorm1(const matrix::Matrix& m);
    00174
    00175 /** calculates to square matrix norm (sum of square values divided by number of values (m*n) )
    00176 */
    00177 double matrixNorm2(const matrix::Matrix& m);
    00178
    00179
    00180 /** returns the k. largest element of the matrix
    00181     Attention: it will detroy the given matrix! (sorting)
    00182     Assumption: k>0 and k<=matrixsize
    00183 */
    00184 double getKthLargestElement(matrix::Matrix& matrix, matrix::I k);
    00185
    00186 /** returns the k. smallest element of the matrix
    00187     Attention: it will detroy the given matrix! (sorting)
    00188     Assumption: k>0 and k<=matrixsize
    00189 */
    00190 double getKthSmallestElement(matrix::Matrix& matrix, matrix::I k);
    00191
    00192 /// considers the matrix as vector (mx1) and returns the index of the smallest element
    00193 matrix::I argmin(const matrix::Matrix& v);
    00194
    00195 /// considers the matrix as vector (mx1) and returns the index of the largest element
    00196 matrix::I argmax(const matrix::Matrix& v);
    00197
    00198 /// returns the smallest element
    00199 double min(const matrix::Matrix& v);
    00200
    00201 /// minimum function for doubles without templates
    00202 double min(double a, double b);
    00203
    00204 /// returns the largest element
    00205 double max(const matrix::Matrix& v);
    00206
    00207 /// maximum function for doubles without templates
    00208 double max(double a, double b);
    00209
    00210
    00211 /// samples from the pdf (rowwise stored with sum = 1)
    00212 matrix::I sample(const matrix::Matrix& pdf);
    00213
    00214
    00215 /// parameter adaptation algorithm.
    00216 //   @param p current parameter value
    00217 //   @param actual actual value of some size controlled by p
    00218 //   @param nominal nominal value of some size controlled by p
    00219 //   @param up_rate adaptation rate for increasing p (<< 1)
    00220 //   @param down_rate adaptation rate for decreasing p (<< 1)
    00221 //   @return new value of p (there is no clipping done)
    00222 double adapt(double p, double actual, double nominal, double up_rate, double down_rate);
    00223
    00224 /// like adapt but that the adaption is just done if the actual value is outside the given interval (_min, _max)
    00225 double adaptMinMax(double p, double actual, double _min, double _max, double up_rate, double down_rate);
    00226
    00227         /**
    00228          * Helper function for converting an array with double values to a list.
    00229          * Is used for the method getInternalParams() interface inspectable.
    00230          */
    00231 std::list<Inspectable::iparamval> convertArrayToList(double* array,int arraySize);
    00232         /**
    00233          * Helper function for getting the array names of an array
    00234          * Is used for the method getInternalParamNames() interface inspectable.
    00235          */
    00236 std::list<Inspectable::iparamkey> getArrayNames(int arraySize,const char* name);
    00237
    00238 #endif