verify_limits.cc
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <float.h>
00003 #include <assert.h>
00004 #include <string.h>
00005 #include <math.h>
00006 
00007 #include "../src/FRICheck.hh"
00008 
00009 #define DEG *M_PI/180.0
00010 #define RAD /M_PI*180.0
00011 
00013 // helper function
00014 
00015 // for concatenating file names
00016 char* scat(const char* str1, const char* str2)
00017 {
00018   static char buffer[1024];
00019   buffer[0] = '\0';
00020 
00021   return strcat(strcat(buffer, str1), str2);
00022 }
00023 
00024 
00025 void extractLimits(FILE* f, double *angles, int len,
00026                    double* min6, double* min7, double* max6, double *max7)
00027 {
00028   char buffer[1024];
00029 
00030   // initialize limits
00031   for(int i=0; i < len; i++) {
00032     min6[i] =  FLT_MAX;  min7[i] =  FLT_MAX;
00033     max6[i] = -FLT_MAX;  max7[i] = -FLT_MAX;
00034   }
00035 
00036   // bin limits
00037   double bin_limits[len+1];
00038   double *bin_low=&(bin_limits[0]), *bin_high=&(bin_limits[1]);
00039 
00040   // set collection bins around desired angles for J6
00041   bin_limits[0] = -FLT_MAX;
00042   for(int i=0; i < len; i++)
00043     bin_limits[i+1] = (angles[i+1] + angles[i])/2.0;
00044   bin_limits[len] = FLT_MAX;
00045 
00046   int bin=0;
00047   float j6, j7;
00048   while(fgets(buffer, sizeof(buffer), f) != 0) {
00049     if(buffer[0] == '#')
00050       continue;
00051     if(sscanf(buffer, "%f %f", &j6, &j7) != 2)
00052       continue;
00053 
00054     // find correct bin
00055     while(j6 < bin_low[bin]  && bin > 0)
00056       bin--;
00057     while(j6 > bin_high[bin] && bin < len)
00058       bin++;
00059 
00060     // now we should have the correct bin ...
00061     assert(bin_low[bin] <= j6 && j6 <= bin_high[bin]);
00062 
00063     // record min and max
00064     if(j7 > max7[bin]) {
00065       max6[bin] = j6;
00066       max7[bin] = j7;
00067     }
00068     if(j7 < min7[bin]) {
00069       min6[bin] = j6;
00070       min7[bin] = j7;
00071     }
00072   }
00073 }
00074 
00075 void printRawLimits(FILE* f, int len, double* min6, double* min7, double* max6, double *max7)
00076 {
00077   for(int i=0; i < len; i++)
00078     fprintf(f, "%f %f\n", min6[i], min7[i]);
00079 
00080   for(int i=len-1; i >= 0; i--)
00081     fprintf(f, "%f %f\n", max6[i], max7[i]);
00082 
00083   fprintf(f, "%f %f\n", min6[0], min7[0]);
00084 }
00085 
00086 void printLimits(FILE* f, double* angles, double* min, double* max, int len)
00087 {
00088   for(int i=0; i < len; i++)
00089     fprintf(f, "%f %f\n", angles[i], min[i]);
00090 
00091   for(int i=len-1; i >= 0; i--)
00092     fprintf(f, "%f %f\n", angles[i], max[i]);
00093 
00094   fprintf(f, "%f %f\n", angles[0], min[0]);
00095 }
00096 
00097 double interpolate(double x1, double x2, double y1, double y2, double x)
00098 {
00099   double t = (x - x1)/(x2 - x1);
00100   return (1 - t)*y1 + t*y2;
00101 }
00102 
00103 void interpolateLimits(int len, double* min6, double* min7, double* max6, double *max7, double* angles, double *min, double *max)
00104 {
00105   // no interpolation possible at j6 limits
00106   min[0] = min7[0];
00107   max[0] = max7[0];
00108 
00109   min[len-1] = min7[len-1];
00110   max[len-1] = max7[len-1];
00111 
00112   // do interpolation with other points
00113   for(int i=1; i < len-1; i++) {
00114     if(min6[i] < angles[i])
00115       min[i] = interpolate(min6[i], min6[i+1], min7[i], min7[i+1], angles[i]);
00116     else
00117       min[i] = interpolate(min6[i-1], min6[i], min7[i-1], min7[i], angles[i]);
00118     
00119     if(max6[i] < angles[i])
00120       max[i] = interpolate(max6[i], max6[i+1], max7[i], max7[i+1], angles[i]);
00121     else
00122       max[i] = interpolate(max6[i-1], max6[i], max7[i-1], max7[i], angles[i]);
00123   }
00124 }
00125 
00126 #define MARGIN_IN  17 DEG
00127 #define MARGIN_OUT  3 DEG
00128 
00129 bool testLimits(int len, double *angles, double* min, double *max, double* mine, double* maxe)
00130 {
00131   bool result = true;
00132   for(int i=0; i < len; i++) {
00133     if(min[i] < mine[i] - MARGIN_OUT) {
00134       printf("lower limit @%f too low  (%f << %f) \n", angles[i] RAD, min[i] RAD, mine[i] RAD);
00135       result = false;
00136     }
00137     if(min[i] > mine[i] + MARGIN_IN) {
00138       printf("lower limit @%f too high (%f >> %f) \n", angles[i] RAD, min[i] RAD, mine[i] RAD);
00139       result = false;
00140     }
00141 
00142     if(max[i] > maxe[i] + MARGIN_OUT) {
00143       printf("upper limit @%f too high (%f >> %f) \n", angles[i] RAD, max[i] RAD, maxe[i] RAD);
00144       result = false;
00145     }
00146     if(max[i] < maxe[i] - MARGIN_IN) {
00147       printf("upper limit @%f too low (%f << %f) \n", angles[i] RAD, max[i] RAD, maxe[i] RAD);
00148       result = false;
00149     }
00150   }
00151   return result;
00152 }
00153 
00154 bool verifyHandAngles(int len, double* angles, double* min, double *max, const char* side)
00155 {
00156   FILE *f;
00157 
00158   // auto-extract limits
00159   f = fopen(scat("limits_recording_", side), "r");
00160   double min6[len], max6[len], min7[len], max7[len], mine[len], maxe[len];
00161   extractLimits(f, angles, len, min6, min7, max6, max7);
00162   fclose(f);
00163 
00164   interpolateLimits(len, min6, min7, max6, max7, angles, mine, maxe);
00165 
00166   bool result = testLimits(len, angles, min, max, mine, maxe);
00167 
00168   // print auto-extracted limits
00169   f = fopen(scat("limits_extracted_", side), "w");
00170   printRawLimits(f, len, min6, min7, max6, max7);
00171   fclose(f);
00172 
00173   // print interpolated limits
00174   f = fopen(scat("limits_interpolated_", side), "w");
00175   printLimits(f, angles, mine, maxe, len);
00176   fclose(f);
00177 
00178   // print current state
00179   f = fopen(scat("limit_", side), "w");
00180   printLimits(f, angles, min, max, len);
00181   fclose(f);
00182 
00183   return result;
00184 }
00185 
00186 int main()
00187 {
00188   bool r, result = true;
00189 
00190   printf("testing left hand angles ... ");
00191   r = verifyHandAngles(FRICheck::length_left,
00192                        FRICheck::j5_angles_left,
00193                        FRICheck::j6_min_left,
00194                        FRICheck::j6_max_left,
00195                        "left");
00196   printf("%s\n", (r) ? "PASSED" : "FAILED");
00197   result &= r;
00198 
00199   printf("testing right hand angles ... ");
00200   verifyHandAngles(FRICheck::length_right,
00201                    FRICheck::j5_angles_right,
00202                    FRICheck::j6_min_right,
00203                    FRICheck::j6_max_right,
00204                    "right");
00205   printf("%s\n", (r) ? "PASSED" : "FAILED");
00206   result &= r;
00207 
00208   return (result) ? 0 : -1;
00209 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Thu May 23 2013 05:28:18