Robotsystem from Robot Group Leipzig: controller_misc.cpp Source File

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

- File List
- File Members

# controller_misc.cpp

Go to the documentation of this file.

    00001 #include "controller_misc.h"
    00002
    00003 /// calculates 1/x
    00004 double one_over(double x){
    00005   return 1/x;
    00006 }
    00007
    00008 /// creates random number from -1 to 1
    00009 double random_minusone_to_one(double){
    00010   return (((double)rand() / RAND_MAX) - 0.5) * 2.0;
    00011 }
    00012
    00013 /* stores at least left top 4x4 submatrix (row-wise) (if exists) and
    00014    then the rest of the diagonal elements into a list
    00015    @return list of values
    00016 */
    00017 list<D> store4x4AndDiagonal(const Matrix& m){
    00018   list<D> l;
    00019   unsigned short smalldimM = min(m.getM(), (unsigned short)4);
    00020   unsigned short smalldimN = min(m.getN(), (unsigned short)4);
    00021   unsigned short smallerdim = min(m.getM(), m.getN());
    00022   // 4x4
    00023   for(unsigned short i=0; i < smalldimM; i++){
    00024     for(unsigned short j=0; j < smalldimN; j++){
    00025       l.push_back(m.val(i,j));
    00026     }
    00027   }
    00028   // diagonal below 4x4
    00029   for(unsigned short i=4; i < smallerdim; i++){
    00030     l.push_back(m.val(i,i));
    00031   }
    00032   return l;
    00033 }
    00034
    00035 /* stores at least left top 4x4 submatrix (row-wise) (if exists) and
    00036    then the rest of the diagonal elements
    00037   @param len Length of the provided buffer
    00038   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00039   @return number of actually written elements
    00040 */
    00041 unsigned int store4x4AndDiagonal(const Matrix& m, D* buffer, unsigned int len){
    00042   unsigned short smalldimM = min(m.getM(), (unsigned short)4);
    00043   unsigned short smalldimN = min(m.getN(), (unsigned short)4);
    00044   unsigned short smallerdim = min(m.getM(), m.getN());
    00045   unsigned int written=0;
    00046   assert(len >= unsigned(smalldimM * smalldimN + max(0, signed(smallerdim) - 4)));
    00047   // 4x4
    00048   for(unsigned short i=0; i < smalldimM; i++){
    00049     for(unsigned short j=0; j < smalldimN; j++){
    00050       buffer[written]=m.val(i,j);
    00051       written++;
    00052     }
    00053   }
    00054   // diagonal below 4x4
    00055   for(unsigned short i=4; i < smallerdim; i++){
    00056     buffer[written]=m.val(i,i);
    00057     written++;
    00058   }
    00059   return written;
    00060 }
    00061
    00062
    00063 /* returns the number of elements stored by store4x4AndDiagonal
    00064   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00065 */
    00066 unsigned int get4x4AndDiagonalSize(const Matrix& m){
    00067   unsigned short smalldimM = min(m.getM(), (unsigned short)4);
    00068   unsigned short smalldimN = min(m.getN(), (unsigned short)4);
    00069   unsigned short smallerdim = min(m.getM(), m.getN());
    00070   unsigned short sm = unsigned(max(0, signed(smallerdim) - 4));
    00071   return smalldimM * smalldimN + sm;
    00072 }
    00073
    00074
    00075 /* writes the names of the fields stored by store4x4AndDiagonal into a list
    00076   @param matrixName name of the matrix (prefix for all fields)
    00077   @param keylist list for field names
    00078   @param len Length of the provided buffer
    00079   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00080   @return number of actually written elements
    00081 */
    00082 list<Inspectable::iparamkey> store4x4AndDiagonalFieldNames(const Matrix& m, const char* matrixName){
    00083   list<Inspectable::iparamkey> l;
    00084   char buffer[32];
    00085   unsigned short smalldimM = min(m.getM(), (unsigned short)4);
    00086   unsigned short smalldimN = min(m.getN(), (unsigned short)4);
    00087   unsigned short smallerdim = min(m.getM(), m.getN());
    00088   assert(matrixName);
    00089   // 4x4
    00090   for(unsigned short i=0; i < smalldimM; i++){
    00091     for(unsigned short j=0; j < smalldimN; j++){
    00092       sprintf(buffer,"%s[%d,%d]",matrixName,i,j);
    00093       l.push_back(string(buffer));
    00094     }
    00095   }
    00096   // diagonal below 4x4
    00097   for(unsigned short i=4; i < smallerdim; i++){
    00098     sprintf(buffer,"%s[%d,%d]",matrixName,i,i);
    00099     l.push_back(string(buffer));
    00100   }
    00101   return l;
    00102 }
    00103
    00104 /* stores the names of the fields stored by store4x4AndDiagonal
    00105   @param matrixName name of the matrix (prefix for all fields)
    00106   @param keylist list for field names
    00107   @param len Length of the provided buffer
    00108   (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4)))
    00109   @return number of actually written elements
    00110 */
    00111 unsigned int store4x4AndDiagonalFieldNames(const Matrix& m, const char* matrixName,
    00112                                            char** keylist, unsigned int len){
    00113   unsigned short smalldimM = min(m.getM(), (unsigned short)4);
    00114   unsigned short smalldimN = min(m.getN(), (unsigned short)4);
    00115   unsigned short smallerdim = min(m.getM(), m.getN());
    00116   unsigned int written=0;
    00117   assert(len >= get4x4AndDiagonalSize(m));
    00118   assert(matrixName);
    00119   unsigned char keyLen = strlen(matrixName)+10;
    00120   // 4x4
    00121   for(unsigned short i=0; i < smalldimM; i++){
    00122     for(unsigned short j=0; j < smalldimN; j++){
    00123       keylist[written] = (char*) malloc(keyLen);
    00124       sprintf(keylist[written],"%s[%d,%d]",matrixName,i,j);
    00125       written++;
    00126     }
    00127   }
    00128   // diagonal below 4x4
    00129   for(unsigned short i=4; i < smallerdim; i++){
    00130     keylist[written] = (char*) malloc(keyLen);
    00131     sprintf(keylist[written],"%s[%d,%d]",matrixName,i,i);
    00132     written++;
    00133   }
    00134   return written;
    00135 }
    00136
    00137 /* stores the names of the all matrix fieldnames produces by convertToBuffer into a list
    00138   @return list of names
    00139 */
    00140 list<Inspectable::iparamkey> storeMatrixFieldNames(const Matrix& m, const char* matrixName){
    00141   list<Inspectable::iparamkey> l;
    00142   char buffer[32];
    00143   unsigned int dimM = m.getM();
    00144   unsigned int dimN = m.getN();
    00145   assert(matrixName);
    00146   for(unsigned short i=0; i < dimM; i++){
    00147     for(unsigned short j=0; j < dimN; j++){
    00148       sprintf(buffer,"%s[%d,%d]",matrixName,i,j);
    00149       l.push_back(string(buffer));
    00150     }
    00151   }
    00152   return l;
    00153 }
    00154
    00155 /* stores the names of the all vector (mx1 matrix) fieldnames  produces by convertToBuffer into a list
    00156   @return list of names
    00157 */
    00158 list<Inspectable::iparamkey> storeVectorFieldNames(const Matrix& m, const char* vectorName){
    00159   list<Inspectable::iparamkey> l;
    00160   char buffer[32];
    00161   unsigned int dimM = m.getM();
    00162   assert(vectorName);
    00163   assert(m.getN()==1);
    00164   for(unsigned short i=0; i < dimM; i++){
    00165       sprintf(buffer,"%s[%d]",vectorName,i);
    00166       l.push_back(string(buffer));
    00167   }
    00168   return l;
    00169 }
    00170
    00171 /* stores the names of the all matrix fieldnames produces by convertToBuffer
    00172   @param matrixName name of the matrix (prefix for all fields)
    00173   @param keylist list for field names
    00174   @param len Length of the provided buffer
    00175   (should be getN()*getM()
    00176   @return number of actually written elements
    00177 */
    00178 unsigned int storeMatrixFieldNames(const Matrix& m, const char* matrixName,
    00179                                    char** keylist, unsigned int len){
    00180   unsigned int dimM = m.getM();
    00181   unsigned int dimN = m.getN();
    00182   unsigned int written=0;
    00183   assert(matrixName);
    00184   assert(len >= dimM*dimN);
    00185   unsigned char keyLen = strlen(matrixName)+10;
    00186   for(unsigned short i=0; i < dimM; i++){
    00187     for(unsigned short j=0; j < dimN; j++){
    00188       keylist[written] = (char*) malloc(keyLen);
    00189       sprintf(keylist[written],"%s[%d,%d]",matrixName,i,j);
    00190       written++;
    00191     }
    00192   }
    00193   return written;
    00194 }
    00195
    00196 /* stores the names of the all vector (mx1 matrix) fieldnames produces by convertToBuffer
    00197   @param vectorName name of the vector (prefix for all fields)
    00198   @param keylist list for field names
    00199   @param len Length of the provided buffer (should be getM())
    00200   @return number of actually written elements
    00201 */
    00202 unsigned int storeVectorFieldNames(const Matrix& m, const char* vectorName,
    00203                                    char** keylist, unsigned int len){
    00204   unsigned int dimM = m.getM();
    00205   unsigned int written=0;
    00206   assert(vectorName);
    00207   assert(m.getN()==1);
    00208   assert(len >= dimM);
    00209   unsigned char keyLen = strlen(vectorName)+5;
    00210   for(unsigned short i=0; i < dimM; i++){
    00211       keylist[written] = (char*) malloc(keyLen);
    00212       sprintf(keylist[written],"%s[%d]",vectorName,i);
    00213       written++;
    00214   }
    00215   return written;
    00216 }
    00217
    00218
    00219 /** stores the Matrix into the given file stream (binary)
    00220  */
    00221 bool storeMatrix(const Matrix& m, FILE* f){
    00222   int dim[2] = { m.getM(), m.getN() };
    00223   unsigned int len = dim[0] * dim[1];
    00224   D* buffer = (D*)malloc(len * sizeof(D));
    00225   m.convertToBuffer(buffer, len);
    00226   bool rval=false;
    00227   if(fwrite(dim, sizeof(int), 2, f) == 2)
    00228     if(fwrite(buffer, sizeof(D), len, f) == len)
    00229       rval=true;
    00230   free(buffer);
    00231   return rval;
    00232 }
    00233
    00234 /** reads a Matrix from the given file stream (binary)
    00235  */
    00236 bool restoreMatrix(Matrix& m, FILE* f){
    00237   int dim[2];
    00238   bool rval = false;
    00239   if(fread(dim, sizeof(int), 2, f) == 2){
    00240     if(dim[0] == m.getM() && dim[1] == m.getN()) {
    00241       unsigned int len = dim[0] * dim[1];
    00242       D* buffer = (D*)malloc(len * sizeof(D));
    00243       if(fread(buffer, sizeof(D), len, f) == len) {
    00244         m.set(buffer);
    00245         rval = true;
    00246       }else fprintf(stderr, "cannot read matrix");
    00247       free(buffer);
    00248     }else{
    00249       fprintf(stderr, "restoreMatrix: dimension is wrong: expected (%i,%i), got (%i,%i)",
    00250               m.getM(), m.getN(), dim[0], dim[1]);
    00251     }
    00252   }
    00253   return rval;
    00254 }
    00255
    00256 Matrix noiseMatrix(unsigned int m, unsigned int n, NoiseGenerator& ng, double p1, double p2){
    00257   int len = m*n;
    00258   D* noise = (D*) malloc(len*sizeof(D));
    00259   memset(noise, D_Zero, sizeof(D)*len);
    00260   ng.add(noise, p1, p2);
    00261   Matrix result(m, n, noise);
    00262   free(noise);
    00263   return result;
    00264 }
    00265
    00266 /// parameter adaptation algorithm.
    00267 //   @param p current parameter value
    00268 //   @param actual actual value of some size controlled by p
    00269 //   @param nominal nominal value of some size controlled by p
    00270 //   @param up_rate adaptation rate for increasing p (<< 1)
    00271 //   @param down_rate adaptation rate for decreasing p (<< 1)
    00272 //   @return new value of p (there is no clipping done)
    00273 double adapt(double p, double actual, double nominal, double up_rate, double down_rate){
    00274   double result;
    00275   // use a different rate for decreasing and increasing
    00276   result = p * ((actual - nominal) > 0 ? (1.0 - down_rate) : (1.0 + up_rate));
    00277   return result;
    00278 }
    00279
    00280 /// like adapt but that the adaption is just done if the actual value is outside the given interval (_min, _max)
    00281 double adaptMinMax(double p, double actual, double _min, double _max, double up_rate, double down_rate){
    00282   double result=p;
    00283   if(actual < _min){
    00284     //    printf("Under: %g, eps: %g\n", updateSize, eps);
    00285     result = p * (1+up_rate);
    00286   } else if(actual > _max){
    00287     //    printf("Over : %g, eps: %g\n", updateSize, eps);
    00288     result = p * (1-down_rate);
    00289   }
    00290   return result;
    00291 }