test_checker.cc
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <math.h>
00003 #include <assert.h>
00004 
00005 #include <stdlib.h>
00006 
00007 #include "FRICheck_legacy.hh"
00008 
00009 #include "../src/FRICheck.hh"
00010 
00011 #define DEG *M_PI/180.0
00012 #define RAD /M_PI*180.0
00013 
00014 #define ITER 20000000
00015 #define MAX_STEP (0.5 DEG)
00016 #define EPS (0.001 DEG)
00017 
00018 const float margin = 0.5 DEG;
00019 const double lim_low_[7] = {-170 DEG, -120 DEG, -170 DEG, -120 DEG, -170 DEG, -120 DEG, -170 DEG};
00020 const double lim_high_[7] = { 170 DEG,  120 DEG,  170 DEG,  120 DEG,  170 DEG,  120 DEG,  170 DEG};
00021 
00022 
00023 class FRICheck_test
00024 {
00025 public:
00026 static bool hand_check_right();
00027 static bool hand_check_movePoint(FRICheck checker, int index, double j5, double j6, double vel_j5, double vel_j6, bool should_brake);
00028 static bool hand_check_random();
00029 };
00030 
00031 
00032 bool limit_crash_check()
00033 {
00034   bool test_passed = true;
00035 
00036   float pos_start[] = {100 DEG, 30 DEG, 70 DEG, 50 DEG, 20 DEG, 60 DEG, 70 DEG, 80 DEG};
00037   float rate = 0.001;
00038 
00039   FRICheck checker;
00040 
00041   // run against positive limit
00042   float command_speed = 1.0; // try to drive at 1 rad/s = 57 deg/s. way below limits
00043 
00044   float t=0.0;
00045 
00046   for(int joint=0; joint < 1; joint++)
00047   {
00048     float pos[7];
00049     for(int i=0; i < 7; i++)
00050       pos[i] = pos_start[i];
00051 
00052     // set the starting position and let it settle
00053     checker.setPos(pos);
00054     for(int i=0; i < 10; i++)
00055       checker.adjust(pos, rate, 1.0);
00056 
00057     float q_last = pos[joint];
00058     float q_last_last = pos[joint];
00059 
00060     // run against positive limits
00061     for(int i=0; i < 10000; i++)
00062     {
00063       t += rate;
00064 
00065       pos[joint] += command_speed * rate;
00066 
00067       checker.adjust(pos, rate, 1.0);
00068 
00069       // vel = (q - q_last)*rate;
00070       // vel_last = (q_last - q_last_last)*rate;
00071       //
00072       // acc = (vel - vel_last)*rate;
00073       //     = (q - 2*q_last + q_last_last)*rate*rate
00074 
00075       // units(acc): rad*(1/s)*(1/s) = rad*s^-2
00076 
00077       float vel = ((double)pos[joint] - (double) q_last) / ((double) rate);
00078       float acc = ((double)pos[joint] - 2.0*((double)q_last) + (double)q_last_last) / (((double)rate) * ((double)rate));
00079 
00080       printf("%f   %f %f %f\n", t, pos[joint], vel, acc);
00081 
00082       // TODO write assertions for vel and acc
00083 
00084       // remember last position values
00085       q_last_last = q_last;
00086       q_last = pos[joint];
00087     }
00088   }
00089 
00090 }
00091 
00092 bool explicit_check()
00093 {
00094   bool test_passed = true;
00095 
00096   float pos[] = {100 DEG, 30 DEG, 70 DEG, 50 DEG, 20 DEG, 60 DEG, 70 DEG, 80 DEG};
00097   float rate = 0.001;
00098 
00099   FRICheck checker_new;
00100 
00101   checker_new.setHandSide(FRICheck::RIGHT);
00102   checker_new.setPos(pos);
00103 
00104   for(int i=0; i < 10; i++) {
00105     checker_new.adjust(pos, rate, 0.9);
00106   }
00107 
00108   for(int iter=0; iter < ITER; iter++) {
00109     if((iter % (ITER / 40)) == 0) {
00110       printf(".");
00111       fflush(stdout);
00112     }
00113 
00114     //create brownian motion
00115     for(int i=0; i < 7; i++)
00116       pos[i] += (drand48()*2-1)*MAX_STEP;
00117   
00118     checker_new.adjust(pos, rate, 1.0);
00119 
00120     for(int i=0; i < 7; i++) {
00121       double lim_lo = lim_low_[i] + margin;
00122       double lim_hi = lim_high_[i] - margin;
00123 
00124       if(pos[i] > lim_hi) {
00125         printf("%d[%d]: angle over limit by %f (%f)\n", iter, i, (pos[i] - lim_hi) RAD, pos[i] RAD);
00126         test_passed = false;
00127       }
00128       if(pos[i] < lim_lo) {
00129         printf("%d[%d]: angle under limit by %f (%f)\n", iter, i, (pos[i] - lim_lo) RAD, pos[i] RAD);
00130         test_passed = false;
00131       }
00132     }
00133 
00134     //TODO: also check for vel and acc limits
00135 
00136   }
00137   printf("Test %s\n", (test_passed) ? "PASSED" : "FAILED");
00138 
00139   return test_passed;
00140 }
00141 
00142 bool FRICheck_test::hand_check_random()
00143 {
00144   bool test_passed = true;
00145 
00146   float pos[] = {100 DEG, 30 DEG, 70 DEG, 50 DEG, 20 DEG, 60 DEG, 70 DEG, 80 DEG};
00147   float rate = 0.001;
00148 
00149   FRICheck checker_new;
00150 
00151   checker_new.setHandSide(FRICheck::RIGHT);
00152   checker_new.setPos(pos);
00153 
00154   for(int i=0; i < 10; i++) {
00155     checker_new.adjust(pos, rate, 0.9);
00156   }
00157 
00158   double max_violation = 0.0;
00159 
00160   for(int iter=0; iter < ITER; iter++) {
00161     if((iter % (ITER / 40)) == 0) {
00162       printf(".");
00163       fflush(stdout);
00164     }
00165 
00166     //create brownian motion
00167     for(int i=0; i < 7; i++)
00168       pos[i] += (drand48()*2-1)*MAX_STEP;
00169 
00170     checker_new.adjust(pos, rate, 0.9);
00171 
00172     int index = checker_new.find_index(pos[5]);
00173     double lim_lo = checker_new.min_j6(pos[5], index);
00174     double lim_hi = checker_new.max_j6(pos[5], index);
00175 
00176     if(pos[6] > lim_hi) {
00177       printf("%d[%d]: angle over limit by %f (%f)\n", iter, 6, (pos[6] - lim_hi) RAD, pos[6] RAD);
00178       test_passed = false;
00179 
00180       max_violation = (fabs(pos[6] - lim_hi) > fabs(max_violation)) ? pos[6] - lim_hi : max_violation;
00181     }
00182     if(pos[6] < lim_lo) {
00183       printf("%d[%d]: angle under limit by %f (%f)\n", iter, 6, (pos[6] - lim_lo) RAD, pos[6] RAD);
00184       test_passed = false;
00185 
00186       max_violation = (fabs(pos[6] - lim_lo) > fabs(max_violation)) ? pos[6] - lim_lo : max_violation;
00187     }
00188   }
00189 
00190   printf("Test %s\n", (test_passed) ? "PASSED" : "FAILED");
00191 
00192   if(!test_passed)
00193     printf("max_violation: %f\n", max_violation RAD);
00194 
00195   return test_passed;
00196 
00197 }
00198 
00199 bool FRICheck_test::hand_check_movePoint(FRICheck checker, int index, double j5, double j6, double vel_j5, double vel_j6, bool should_brake)
00200 {
00201   float pos[]={0,0,0,0,0,j5, j6};
00202   float vel[]={0,0,0,0,0,vel_j5, vel_j6};
00203   float vel_old[]={0,0,0,0,0,0,0};
00204   float rate = 0.001;
00205 
00206   checker.hand_check(vel, vel_old, pos, rate);
00207   bool res;
00208   if(should_brake)
00209     res =  (vel[5] == 0 && vel[6] == 0);
00210   else
00211     res = !(vel[5] == 0 && vel[6] == 0);
00212   
00213   if(!res)
00214     printf("%c pos=(%5.3f, %5.3f): vel (%5.3f %5.3f) -> (%5.3f, %5.3f)\n", (res)?'*':'x', j5 RAD, j6 RAD, vel_j5, vel_j6, vel[5], vel[6]);
00215 
00216   return res;
00217 }
00218 
00219 bool FRICheck_test::hand_check_right()
00220 {
00221   bool test_passed = true;
00222 
00223   float pos[]={0,0,0,0,0,0,0};
00224   double* angles = FRICheck::j5_angles_right;
00225   double* min = FRICheck::j6_min_right;
00226   double* max = FRICheck::j6_max_right;
00227 
00228   double inc=0.1 DEG;
00229 
00230   FRICheck checker;
00231 
00232   checker.setHandSide(FRICheck::RIGHT);
00233   checker.setPos(pos);
00234 
00235   // rising slope, max
00236   for(int i=2; i < 10; i++) {
00237     // select a point outside limits
00238     double a5 = (angles[i] + angles[i+1]) / 2.0;
00239     double a6 = ((max[i] + max[i+1])/2.0 + 1 DEG);
00240 
00241     assert(i == checker.find_index(a5));
00242 
00243     // These movements should get stopped
00244     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, inc, true); // move up
00245     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, 0.0, true); // move left
00246     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, inc, true); // move up-left
00247 
00248     // These movements should NOT get stopped
00249     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, -inc, false); // move down
00250     test_passed &= hand_check_movePoint(checker, i,a5, a6,  inc,  0.0, false); // move right
00251     test_passed &= hand_check_movePoint(checker, i,a5, a6,  inc, -inc, false); // move down-right
00252   }
00253 
00254   // rising slope, min
00255   for(int i=14; i < 19; i++) {
00256     // select a point outside limits
00257     double a5 = (angles[i] + angles[i+1]) / 2.0;
00258     double a6 = ((min[i] + min[i+1])/2.0 - 1 DEG);
00259 
00260     assert(i == checker.find_index(a5));
00261 
00262     // These movements should get stopped
00263     test_passed &= hand_check_movePoint(checker, i,a5, a6, 0.0, -inc, true); // move down
00264     test_passed &= hand_check_movePoint(checker, i,a5, a6, inc,  0.0, true); // move right
00265     test_passed &= hand_check_movePoint(checker, i,a5, a6, inc, -inc, true); // move down-right
00266 
00267     // These movements should NOT get stopped
00268     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, inc, false); // move up
00269     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, 0.0, false); // move left
00270     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, inc, false); // move up-left
00271   }
00272 
00273   // falling slope, max
00274   for(int i=1; i < 1; i++) {
00275     // select a point outside limits
00276     double a5 = (angles[i] + angles[i+1]) / 2.0;
00277     double a6 = ((max[i] + max[i+1])/2.0 + 1 DEG);
00278 
00279     assert(i == checker.find_index(a5));
00280 
00281     // These movements should get stopped
00282     test_passed &= hand_check_movePoint(checker, i,a5, a6, 0.0, inc, true); // move up
00283     test_passed &= hand_check_movePoint(checker, i,a5, a6, inc, 0.0, true); // move right
00284     test_passed &= hand_check_movePoint(checker, i,a5, a6, inc, inc, true); // move up-right
00285 
00286     // These movements should NOT get stopped
00287     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc,  0.0, false); // move left
00288     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, -inc, false); // move down
00289     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, -inc, false); // move down-left
00290   }
00291 
00292   // falling slope, min
00293   for(int i=0; i < 2; i++) {
00294     // select a point outside limits
00295     double a5 = (angles[i] + angles[i+1]) / 2.0;
00296     double a6 = ((min[i] + min[i+1])/2.0 - 1 DEG);
00297 
00298     assert(i == checker.find_index(a5));
00299 
00300     // These movements should get stopped
00301     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, -inc, true); // move up
00302     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc,  0.0, true); // move left
00303     test_passed &= hand_check_movePoint(checker, i,a5, a6, -inc, -inc, true); // move up-left
00304 
00305     // These movements should NOT get stopped
00306     test_passed &= hand_check_movePoint(checker, i,a5, a6,  0.0, inc, false); // move down
00307     test_passed &= hand_check_movePoint(checker, i,a5, a6,  inc, 0.0, false); // move right
00308     test_passed &= hand_check_movePoint(checker, i,a5, a6,  inc, inc, false); // move down-right
00309   }
00310   return test_passed;
00311 }
00312 
00313 
00314 int main()
00315 {
00316   //limit_crash_check();
00317 
00318   bool t = FRICheck_test::hand_check_right();
00319   printf("hand check %s\n", (t) ? "PASSED" : "FAILED");
00320   explicit_check();
00321   //FRICheck_test::hand_check_random();
00322   return 0;
00323 }


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Mon Oct 6 2014 09:27:40