$search
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 }