GT2004: Modules/CollisionDetector/GT2004CollisionDetector.cpp Source File

00001 /** 
00002 * @file GT2004CollisionDetector.cpp
00003 * 
00004 * This file contains a class for Sensor Data Processing.
00005 */
00006 //2004/06/03 for UD-WalkingEngine inv kin 
00007 #include "GT2004CollisionDetector.h"
00008 #include "Tools/Debugging/Debugging.h"
00009 
00010     double maxfl1 = 0;
00011     double maxfl2 = 0;
00012     double maxfr1 = 0;
00013     double maxfr2 = 0;
00014     double maxhl1 = 0;
00015     double maxhl2 = 0;
00016     double maxhr1 = 0;
00017     double maxhr2 = 0;
00018  
00019 
00020 
00021 GT2004CollisionDetector::GT2004CollisionDetector
00022 (const CollisionDetectorInterfaces& interfaces)
00023 : CollisionDetector(interfaces)
00024 {
00025   infin = 1000000000;
00026   index = 0;
00027   min =  5;   // minimum delay value
00028   max = 12;   // maximum delay value
00029   oldVal = 0;
00030   newVal = 0;
00031 
00032   frontMin =  0;//20
00033   frontMax = infin;  //400 or 180
00034   backMin =  -0;//-20
00035   backMax = -infin;
00036   sideMin =   0;  //20
00037   sideMax =  infin; //400 or left 120
00038   rotMin = 0.00;
00039   rotMax = infin; //2.5
00040     
00041   
00042   // Behaviour motion change recognition  
00043   currentMotionX = 0;
00044   currentMotionY = 0;
00045   currentRotation = 0;
00046   lastMotionX = 0;
00047   lastMotionY = 0;
00048   lastRotation = 0;
00049   motionChangeThresholdX = 50;
00050   motionChangeThresholdY = 50;
00051   motionChangeThresholdR = 0.2;
00052   calmDown = 32; 
00053   lastMotionChange = calmDown + 1;
00054   lastTooBigMotionChange = 0;
00055   motionChangeThresholdFactor = 1;
00056   xRatio = 0;
00057   yRatio = 0;
00058   rRatio = 0;
00059   q = 12;
00060 
00061 
00062 
00063   // Robot sepecific parameters for direction weights
00064   scaleThresholdFore = scaleThresholdBack = scaleThresholdSide = 1;
00065   scaleThresholdRot = 2;
00066 
00067   walkingMode = 0;
00068   
00069   //Debugging variables  
00070  //* 
00071   //*/
00072   // reset the Ringbuffer and the FloatingAvg
00073   // i = number of joints, l = quadruple element, d = delay index
00074   
00075   for (int i = 0; i < SensorData::numOfSensor_ERS210; i++)
00076   { 
00077     for (int l = 0; l < SensorDataBuffer::maxNumOfFrames; l++)
00078     {
00079       for (int d = 0; d < 16; d++)
00080       {
00081         floatingAvg[i][l][d] = 0;
00082       }
00083       minAvg[i][l] = 0;
00084     }
00085     
00086     
00087     // i = number of joints, j = Ringbuffer element, k = sensor or actor data
00088     // t = lower or upper threshold bound
00089     
00090     for (int j = 0; j < 128; j++)
00091     {
00092       for (int k = 0; k < 2; k++) 
00093       {
00094         ringBuffer[i][j][k] = 0;
00095       }
00096     }
00097     for (int t = 0; t < 3; t++)
00098     {
00099       threshold[i][t] = 0;
00100     }
00101   }
00102   
00103   // unused angle values
00104   threshold[SensorData::legFL3][2] = infin;
00105   threshold[SensorData::legFR3][2] = infin;
00106   threshold[SensorData::legHL3][2] = infin;
00107   threshold[SensorData::legHR3][2] = infin;
00108   
00109 }
00110 
00111 
00112 void GT2004CollisionDetector::calculateThresholds() 
00113 {
00114   //Head thresholds are indifferent to motions and directions
00115   walkingMode = 0;
00116   
00117   threshold[SensorData::legFL1][2] = 1;
00118   threshold[SensorData::legHL1][2] = 1;
00119   threshold[SensorData::legFL2][2] = 1;
00120   threshold[SensorData::legHL2][2] = 1;
00121   
00122   //Threshold calculation for straight forward walking only
00123   if  ((currentMotionX > frontMin) && (currentMotionX <= frontMax))
00124   {
00125     walkingMode = 1; //foreward
00126    
00127     if (currentMotionX <= 5)
00128     {
00129       threshold[SensorData::legFL1][1] = 1200;  threshold[SensorData::legFL2][1] = 1000;  
00130       threshold[SensorData::legHL1][1] = 3300;  threshold[SensorData::legHL2][1] = 2500;   
00131     }
00132     else if  (currentMotionX <= 40)
00133     {
00134       threshold[SensorData::legFL1][1] = 1500;  threshold[SensorData::legFL2][1] = 3600;  
00135       threshold[SensorData::legHL1][1] = 3500;  threshold[SensorData::legHL2][1] = 3300;   
00136     }
00137     else if (currentMotionX <= 80)
00138     {
00139       threshold[SensorData::legFL1][1] = 2100;  threshold[SensorData::legFL2][1] = 1800;
00140       threshold[SensorData::legHL1][1] = 3500;  threshold[SensorData::legHL2][1] = 1800;
00141     }
00142     else if (currentMotionX <= 120)
00143     {
00144       threshold[SensorData::legFL1][1] = 2300;  threshold[SensorData::legFL2][1] = 2700;
00145       threshold[SensorData::legHL1][1] = 3300;  threshold[SensorData::legHL2][1] = 3300;
00146     }
00147     else if (currentMotionX <= 160)
00148     {
00149       threshold[SensorData::legFL1][1] = 2500;  threshold[SensorData::legFL2][1] = 2700;
00150       threshold[SensorData::legHL1][1] = 3300;  threshold[SensorData::legHL2][1] = 3300; 
00151     }
00152     else if (currentMotionX <= 200)
00153     {
00154       threshold[SensorData::legFL1][1] = 4500;  threshold[SensorData::legFL2][1] = 2000;
00155       threshold[SensorData::legHL1][1] = 3300;  threshold[SensorData::legHL2][1] = 2000; 
00156     }
00157     else if (currentMotionX <= 240)
00158     {
00159       threshold[SensorData::legFL1][1] = 4500;  threshold[SensorData::legFL2][1] = 2000;
00160       threshold[SensorData::legHL1][1] = 4500;  threshold[SensorData::legHL2][1] = 2000; 
00161     }
00162     else if (currentMotionX <= 280)
00163     {
00164       threshold[SensorData::legFL1][1] = 9000; threshold[SensorData::legFL2][1] = 2000;  
00165       threshold[SensorData::legHL1][1] = 8000; threshold[SensorData::legHL2][1] = 2000;  
00166     }
00167     else if (currentMotionX <= 320)
00168     {
00169       threshold[SensorData::legFL1][1] =11000; threshold[SensorData::legFL2][1] = 2000;
00170       threshold[SensorData::legHL1][1] = 6600; threshold[SensorData::legHL2][1] = 2000; 
00171     }
00172     else if (currentMotionX <= 360)
00173     {
00174       threshold[SensorData::legFL1][1] =11000; threshold[SensorData::legFL2][1] = 2000;
00175       threshold[SensorData::legHL1][1] =10000; threshold[SensorData::legHL2][1] = 2000; 
00176     }
00177     else     {
00178       threshold[SensorData::legFL1][1] =11000; threshold[SensorData::legFL2][1] = 3000;  
00179       threshold[SensorData::legHL1][1] =12000; threshold[SensorData::legHL2][1] = 3300; 
00180     }
00181   
00182   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdFore);
00183   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdFore);
00184   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdFore);
00185   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdFore);
00186  }
00187   
00188   
00189   //Threshold calculation for straight backward walking only
00190   else if  ((currentMotionX < backMin) && (currentMotionX >= backMax))
00191   {
00192     walkingMode = 2;
00193     
00194     if  (currentMotionX >= -5)
00195     {
00196       threshold[SensorData::legFL1][1] = 1200; threshold[SensorData::legFL2][1] = 2500;
00197       threshold[SensorData::legHL1][1] = 3600; threshold[SensorData::legHL2][1] = 2100;
00198     }
00199     else if (currentMotionX >= -40)
00200     {
00201       threshold[SensorData::legFL1][1] = 1500; threshold[SensorData::legFL2][1] = 1500; 
00202       threshold[SensorData::legHL1][1] = 4200; threshold[SensorData::legHL2][1] = 2700; 
00203     }
00204     else if (currentMotionX >= -80)
00205     {
00206       threshold[SensorData::legFL1][1] = 1300; threshold[SensorData::legFL2][1] = 1000; 
00207       threshold[SensorData::legHL1][1] = 7000; threshold[SensorData::legHL2][1] = 1500; 
00208     }
00209     else if (currentMotionX >= -120)
00210     {
00211       threshold[SensorData::legFL1][1] = 1200; threshold[SensorData::legFL2][1] = 1200;
00212       threshold[SensorData::legHL1][1] = 8000; threshold[SensorData::legHL2][1] = 1200;
00213     }
00214     else if (currentMotionX >= -160)
00215     {
00216       threshold[SensorData::legFL1][1] = 1200; threshold[SensorData::legFL2][1] = 2000;
00217       threshold[SensorData::legHL1][1] = 6000; threshold[SensorData::legHL2][1] = 1500;
00218     }
00219     else if (currentMotionX >= -200)
00220     {
00221       threshold[SensorData::legFL1][1] = 2100; threshold[SensorData::legFL2][1] = 1300; 
00222       threshold[SensorData::legHL1][1] = 9000; threshold[SensorData::legHL2][1] = 2200; 
00223     }
00224     else if (currentMotionX >= -240)
00225     {
00226       threshold[SensorData::legFL1][1] = 5000; threshold[SensorData::legFL2][1] = 1300;
00227       threshold[SensorData::legHL1][1] = 8000; threshold[SensorData::legHL2][1] = 2000;
00228     }
00229     else
00230     {
00231       threshold[SensorData::legFL1][1] = 8000; threshold[SensorData::legHL1][1] = 2500;
00232       threshold[SensorData::legFL2][1] =12000; threshold[SensorData::legHL2][1] = 3000;
00233     }
00234   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdBack);
00235   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdBack);
00236   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdBack);
00237   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdBack);
00238   }
00239  
00240   //Threshold calculation for left and right walking 
00241   
00242   if (
00243     (fabs(currentMotionY) > sideMin) &&
00244     (fabs(currentMotionY) <= sideMax)
00245     )
00246   {
00247     walkingMode = walkingMode * 10 + 3;
00248     
00249     if (fabs(currentMotionY) <= 5)
00250     {
00251       threshold[SensorData::legFL1][1] = 1500; threshold[SensorData::legFL2][1] = 12000; 
00252       threshold[SensorData::legHL1][1] =13000; threshold[SensorData::legHL2][1] = 4500;
00253     }
00254     else if 
00255       (fabs(currentMotionY) <= 40)
00256     {
00257       threshold[SensorData::legFL1][1] = 1500; threshold[SensorData::legFL2][1] = 14000;
00258       threshold[SensorData::legHL1][1] = 7500; threshold[SensorData::legHL2][1] = 4000; 
00259     }
00260     else if 
00261       (fabs(currentMotionY) <= 80)
00262     {
00263       threshold[SensorData::legFL1][1] = 2000; threshold[SensorData::legFL2][1] = 35000;
00264       threshold[SensorData::legHL1][1] = 8000; threshold[SensorData::legHL2][1] = 4000;
00265     }
00266     else if 
00267       (fabs(currentMotionY) <= 120)
00268     {
00269       threshold[SensorData::legFL1][1] = 2000; threshold[SensorData::legFL2][1] = 160000;
00270       threshold[SensorData::legHL1][1] = 7000; threshold[SensorData::legHL2][1] = 5000; 
00271     }
00272     else if 
00273       (fabs(currentMotionY) <= 160)
00274     {
00275       threshold[SensorData::legFL1][1] = 2000; threshold[SensorData::legFL2][1] = 350000;
00276       threshold[SensorData::legHL1][1] = 6000; threshold[SensorData::legHL2][1] = 15000; 
00277     }
00278     else if 
00279       (fabs(currentMotionY) <= 200)
00280     {
00281       threshold[SensorData::legFL1][1] = 1800; threshold[SensorData::legFL2][1] = 900000;
00282       threshold[SensorData::legHL1][1] = 6500; threshold[SensorData::legHL2][1] = 30000; 
00283     }
00284     else if 
00285       (fabs(currentMotionY) <= 240)
00286     {
00287       threshold[SensorData::legFL1][1] = 1500; threshold[SensorData::legFL2][1] = 1050000; 
00288       threshold[SensorData::legHL1][1] = 6000; threshold[SensorData::legHL2][1] = 35000; 
00289     }
00290     else 
00291     {
00292       threshold[SensorData::legFL1][1] = 3000; threshold[SensorData::legFL2][1] =1200000;
00293       threshold[SensorData::legHL1][1] =20000; threshold[SensorData::legHL2][1] = 40000;
00294     }
00295   } //sideThresholds ended
00296 
00297   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdSide);
00298   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdSide);
00299   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdSide);
00300   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdSide);
00301   
00302   //Threshold calculation for rotation no matter if there is any other direction
00303   // rotation only
00304     if ((fabs(currentRotation) > rotMin) && (fabs(currentRotation) <= rotMax)) 
00305     {
00306       walkingMode = walkingMode * 10 + 5;     
00307       
00308       if (fabs(currentRotation) <= 0.05)
00309       {
00310         threshold[SensorData::legFL1][1] =  600; threshold[SensorData::legFL2][1] =  1200; 
00311         threshold[SensorData::legHL1][1] = 1800; threshold[SensorData::legHL2][1] =  2000; 
00312       }
00313       else if (fabs(currentRotation) <= 0.5)
00314       {
00315         threshold[SensorData::legFL1][1] =  600; threshold[SensorData::legFL2][1] =  1200; 
00316         threshold[SensorData::legHL1][1] = 1800; threshold[SensorData::legHL2][1] =  2000; 
00317       }
00318       else if (fabs(currentRotation) <= 1.0)
00319       {
00320         threshold[SensorData::legFL1][1] =  1200; threshold[SensorData::legFL2][1] = 1500;
00321         threshold[SensorData::legHL1][1] =  4500; threshold[SensorData::legHL2][1] = 2000; 
00322       }
00323       else if (fabs(currentRotation) <= 1.5)
00324       {
00325         threshold[SensorData::legFL1][1] =  1500; threshold[SensorData::legFL2][1] = 3000; 
00326         threshold[SensorData::legHL1][1] =  3000; threshold[SensorData::legHL2][1] = 2700; 
00327       }
00328       else if (fabs(currentRotation) <= 2.0)
00329       {
00330         threshold[SensorData::legFL1][1] =  2400; threshold[SensorData::legFL2][1] = 6000; 
00331         threshold[SensorData::legHL1][1] =  3300; threshold[SensorData::legHL2][1] = 3600; 
00332       }
00333       else if (fabs(currentRotation) <= 2.5)
00334       {
00335         threshold[SensorData::legFL1][1] =  3600; threshold[SensorData::legFL2][1] =20000; 
00336         threshold[SensorData::legHL1][1] =  4500; threshold[SensorData::legHL2][1] = 4500; 
00337       }
00338       else if (fabs(currentRotation) <= 3.0)
00339       {
00340         threshold[SensorData::legFL1][1] =  4500; threshold[SensorData::legFL2][1] =60000; 
00341         threshold[SensorData::legHL1][1] =  6000; threshold[SensorData::legHL2][1] = 5000; 
00342       }
00343       else if (fabs(currentRotation) <= 3.5)
00344       {
00345         threshold[SensorData::legFL1][1] =  4500; threshold[SensorData::legFL2][1] =60000; 
00346         threshold[SensorData::legHL1][1] =  7500; threshold[SensorData::legHL2][1] = 6000; 
00347       }
00348       else if (fabs(currentRotation) <= 4.0)
00349       {
00350         threshold[SensorData::legFL1][1] =  5500; threshold[SensorData::legFL2][1] =70000; 
00351         threshold[SensorData::legHL1][1] =  9000; threshold[SensorData::legHL2][1] = 7000; 
00352       }
00353       
00354       else //max
00355       {
00356         threshold[SensorData::legFL1][1] =  6000; threshold[SensorData::legFL2][1] =75000;
00357         threshold[SensorData::legHL1][1] = 10000; threshold[SensorData::legHL2][1] = 7500;
00358       }
00359 
00360   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdRot);
00361   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdRot);
00362   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdRot);
00363   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdRot);
00364   }
00365  
00366   
00367 ///To Test 
00368 if (
00369     (
00370      (fabs(currentRotation) >= 0.75) && 
00371       (
00372        (fabs(currentMotionX) >= 100) ||
00373        (fabs(currentMotionY) >= 100 )
00374       )
00375      ) 
00376      ||
00377      (
00378       (fabs(currentMotionX) >= 100) &&
00379       (fabs(currentMotionY) >= 100)
00380      )
00381     )
00382   {
00383    threshold[SensorData::legFL1][2] *= 2;
00384    threshold[SensorData::legFL2][2] *= 2;
00385    threshold[SensorData::legHL1][2] *= 2;
00386    threshold[SensorData::legHL2][2] *= 2;
00387   }
00388    
00389       
00390   
00391   //default
00392   if (walkingMode == 0)
00393   {
00394     threshold[SensorData::legFL1][2] = infin;
00395     threshold[SensorData::legFL2][2] = infin;
00396     threshold[SensorData::legHL1][2] = infin;
00397     threshold[SensorData::legHL2][2] = infin;
00398   }
00399   /*
00400   threshold[SensorData::legFL1][2] =  1000;
00401   threshold[SensorData::legFL2][2] =  1000;
00402   threshold[SensorData::legHL1][2] =  1000;
00403   threshold[SensorData::legHL2][2] =  1000;
00404   */
00405   
00406   
00407   /* Head values */
00408   
00409   threshold[SensorData::neckTilt][2] = 20000;
00410   threshold[SensorData::headPan][2]  = 300000;
00411   threshold[SensorData::headTilt][2] = 20000;
00412 
00413  
00414   /* Symmetric exchange */
00415   
00416   threshold[SensorData::legFR1][2] = threshold[SensorData::legFL1][2];
00417   threshold[SensorData::legFR2][2] = threshold[SensorData::legFL2][2];
00418   threshold[SensorData::legHR1][2] = threshold[SensorData::legHL1][2];
00419   threshold[SensorData::legHR2][2] = threshold[SensorData::legHL2][2];
00420   
00421 }
00422 
00423 
00424 
00425 void GT2004CollisionDetector::execute()
00426 {     
00427   collisionPercept.setFrameNumber(sensorDataBuffer.frame[0].frameNumber);
00428   currentMotionX = motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x; // get the motion values
00429   currentMotionY = motionInfo.executedMotionRequest.walkRequest.walkParams.translation.y;
00430   currentRotation = motionInfo.executedMotionRequest.walkRequest.walkParams.rotation;
00431   walkingMode = 0;                       // reset walking Mode
00432   
00433   collisionPercept.state[JointData::legFL3] = 0; // 3rd angles are not calculated
00434   collisionPercept.state[JointData::legHL3] = 0;
00435   collisionPercept.state[JointData::legFR3] = 0;
00436   collisionPercept.state[JointData::legHR3] = 0;
00437   
00438   
00439   for (int i = 0; i < SensorData::numOfSensor_ERS210; i++) 
00440   { // for all sensors    
00441     
00442     if ((i == SensorData::neckTilt) || (i == SensorData::headPan) ||
00443       (i == SensorData::headTilt) || 
00444       (i == SensorData::legFL1) || (i == SensorData::legFL2) ||
00445       (i == SensorData::legHL1) || (i == SensorData::legHL2) ||
00446       (i == SensorData::legFR1) || (i == SensorData::legFR2) ||
00447       (i == SensorData::legHR1) || (i == SensorData::legHR2)) //consider just 2*4 leg angles and 3 head angles
00448     {
00449       for (int quarter = 0; quarter < sensorDataBuffer.numOfFrames; quarter++) // for all SensorDataElements values in the buffer
00450       { 
00451         int current = index + quarter; // current is the actual frame
00452         
00453         min = 5;  //minimum delay value
00454         max = 12; //maximum delay value 
00455         minAvg[i][current%SensorDataBuffer::maxNumOfFrames] = min;
00456         
00457         
00458         for (int d = min; d <= max; d++) // for all possible delays
00459         {
00460           
00461           oldVal = (int)(((ringBuffer[i][(current + 128 - q)%128][0] - 
00462             ringBuffer[i][(current + 128 - q - d)%128][1])*
00463             (ringBuffer[i][(current + 128 - q)%128][0] - 
00464             ringBuffer[i][(current + 128 - q - d)%128][1]))/12); // the old value off the Queue
00465           
00466           
00467           
00468           ringBuffer[i][current%128][0] = (int)(sensorDataBuffer.frame[quarter].data[i]/32);
00469           ringBuffer[i][current%128][1] = (int)(sensorDataBuffer.frame[quarter].refValue[i]/32);
00470           
00471           if (current < 16) 
00472           {
00473             ringBuffer[i][current%128][0] = 0;
00474           }
00475           
00476           //unbedingt nach der Ringbufferuebergabe !!!
00477           
00478           newVal = (int)(((ringBuffer[i][current%128][0] - 
00479             ringBuffer[i][(current + 128 - d)%128][1])*
00480             (ringBuffer[i][current%128][0] - 
00481             ringBuffer[i][(current + 128 - d)%128][1]))/12); // the new value for the Queue
00482           
00483           
00484           floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][d] =  newVal - oldVal + 
00485             floatingAvg[i][(current+SensorDataBuffer::maxNumOfFrames-1)%SensorDataBuffer::maxNumOfFrames][d];  
00486           
00487           
00488           if (floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][d] < floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][minAvg[i][current%SensorDataBuffer::maxNumOfFrames]])
00489           {
00490             minAvg[i][current%SensorDataBuffer::maxNumOfFrames] = d;
00491           }
00492         } // all delays
00493       } // all SensorDataBufferElements
00494     } // just head and Legs
00495   } //all angles
00496   
00497   
00498   xRatio = fabs(currentMotionX-lastMotionX);
00499   yRatio = fabs(currentMotionY-lastMotionY);
00500   rRatio = fabs(currentRotation-lastRotation);
00501   
00502   collisionPercept.reset();
00503   
00504   
00505   if  // calculates the behaviour threshold
00506     (
00507     (motionChangeThresholdX < xRatio) ||
00508     (motionChangeThresholdY < yRatio) ||
00509     (motionChangeThresholdR < rRatio)
00510     )  
00511   {
00512     lastMotionChange = 0;
00513     
00514     /* 09112003
00515     if (rRatio * 50 / motionChangeThresholdR > lastTooBigMotionChange)
00516     {lastTooBigMotionChange = rRatio * 50 / motionChangeThresholdR;}
00517     
00518     if (yRatio * 50 / motionChangeThresholdY > lastTooBigMotionChange)
00519     {lastTooBigMotionChange = yRatio * 50 / motionChangeThresholdY;}
00520     
00521     if (xRatio * 50 / motionChangeThresholdX> lastTooBigMotionChange)
00522     { lastTooBigMotionChange = xRatio * 50 / motionChangeThresholdX;}
00523     
00524     if (lastTooBigMotionChange > 100) { lastTooBigMotionChange = 100; } // lastTooBigMotionChange lies between 50 and 100    
00525     */
00526 
00527   }
00528   
00529   
00530   if (lastMotionChange <= calmDown) //calculates the behaviour cleaned thresholds
00531   {
00532     /* 09112003
00533     motionChangeThresholdFactor = lastTooBigMotionChange/100 * 200; // between 99 and 1
00534     motionChangeThresholdFactor = (motionChangeThresholdFactor - 1)*
00535       (1 - lastMotionChange/calmDown) + 1;
00536     */
00537 
00538     motionChangeThresholdFactor = 1000000;
00539     if (lastMotionChange == calmDown)
00540     {
00541       motionChangeThresholdFactor = 1;
00542     }
00543 
00544   }
00545   
00546   if  ((motionInfo.executedMotionRequest.motionType==MotionRequest::walk) &&
00547     (motionInfo.executedMotionRequest.walkRequest.walkType==WalkRequest::normal))
00548      // no calming down anymore
00549   {
00550     calculateThresholds(); // calculates the threshold values considering speed and direction
00551     
00552     collisionPercept.state[JointData::neckTilt]  = floatingAvg[SensorData::neckTilt][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2];  //neck tilt
00553     collisionPercept.state[JointData::headPan]   = floatingAvg[SensorData::headPan][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2];  //Head pan
00554     collisionPercept.state[JointData::headTilt]  = floatingAvg[SensorData::headTilt][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2];  //Head tilt
00555     
00556     collisionPercept.state[JointData::legFL1] = floatingAvg[SensorData::legFL1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor; //FrontLeft1
00557     collisionPercept.state[JointData::legFR1] = floatingAvg[SensorData::legFR1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor; //FrontRight1
00558     collisionPercept.state[JointData::legHL1] = floatingAvg[SensorData::legHL1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor; //HindLeft1
00559     collisionPercept.state[JointData::legHR1] = floatingAvg[SensorData::legHR1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor; //HindRight1
00560     
00561     collisionPercept.state[JointData::legFL2] = floatingAvg[SensorData::legFL2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor; //FrontLeft2
00562     collisionPercept.state[JointData::legFR2] = floatingAvg[SensorData::legFR2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor; //FrontRight2
00563     collisionPercept.state[JointData::legHL2] = floatingAvg[SensorData::legHL2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor; //HindLeft2
00564     collisionPercept.state[JointData::legHR2] = floatingAvg[SensorData::legHR2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor; //HindRight2
00565     
00566     
00567     // look for bigger values of the SensorDataBuffer
00568     for (int current = index + 1; current < (index + sensorDataBuffer.numOfFrames); current++)
00569     {
00570       if (collisionPercept.state[JointData::neckTilt] < floatingAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2])  
00571       {collisionPercept.state[JointData::neckTilt] =  floatingAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2];} //Head tilt
00572       
00573       if (collisionPercept.state[JointData::headPan]  < floatingAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2])
00574       {collisionPercept.state[JointData::headPan] =  floatingAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2];}  //Head pan
00575       
00576       if (collisionPercept.state[JointData::headTilt] < floatingAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2])
00577       {collisionPercept.state[JointData::headTilt] =  floatingAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2];}//Head roll
00578       
00579       
00580       
00581       if (collisionPercept.state[JointData::legFL1] < floatingAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor)
00582       {collisionPercept.state[JointData::legFL1] = floatingAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor;}//FrontLeft1
00583       
00584       if (collisionPercept.state[JointData::legFR1] < floatingAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor) 
00585       {collisionPercept.state[JointData::legFR1] = floatingAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor;}//FrontRight1
00586       
00587       if (collisionPercept.state[JointData::legHL1] < floatingAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor) 
00588       {collisionPercept.state[JointData::legHL1] = floatingAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor;}//HindLeft1
00589       
00590       if (collisionPercept.state[JointData::legHR1] < floatingAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor) 
00591       {collisionPercept.state[JointData::legHR1] = floatingAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor;}//HindRight1
00592       
00593       
00594       
00595       if (collisionPercept.state[JointData::legFL2] < floatingAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor)  
00596       {collisionPercept.state[JointData::legFL2] =  floatingAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor;}//FrontLeft2
00597       
00598       if (collisionPercept.state[JointData::legFR2] < floatingAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor) 
00599       {collisionPercept.state[JointData::legFR2] =  floatingAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor;}//FrontRight2
00600       
00601       if (collisionPercept.state[JointData::legHL2] < floatingAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor) 
00602       {collisionPercept.state[JointData::legHL2] =  floatingAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor;}//HindLeft2
00603       
00604       if (collisionPercept.state[JointData::legHR2] < floatingAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor) 
00605       {collisionPercept.state[JointData::legHR2] =  floatingAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor;}//HindRight2
00606     }
00607 
00608       /* DEBUG START*/
00609 
00610       //if (lastMotionChange < calmDown) 
00611    /*   {
00612       OUTPUT(idText, text, motionChangeThresholdFactor << " \r\n " <<
00613         "   FL1: " << collisionPercept.state[JointData::legFL1] << 
00614         " FL2: " << collisionPercept.state[JointData::legFL2] << " \r\n " <<
00615         "   HL1: " << collisionPercept.state[JointData::legHL1] << 
00616         " HL2: " << collisionPercept.state[JointData::legHL2] << " \r\n " <<
00617         "   FR1: " << collisionPercept.state[JointData::legFR1] << 
00618         " FR2: " << collisionPercept.state[JointData::legFR2] << " \r\n " <<
00619         "   HR1: " << collisionPercept.state[JointData::legHR1] << 
00620         " HR2: " << collisionPercept.state[JointData::legHR2] << " \r\n " <<  
00621         "  index:  " << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x<< 
00622         " Y:" << motionInfo.executedMotionRequest.walkParams.translation.y <<
00623         " R: " << motionInfo.executedMotionRequest.walkParams.rotation << "   \r\n \r\n ");
00624       }  
00625      */  
00626     
00627       /* RELEASE DEBUG
00628       if (collisionPercept.state[JointData::legFL1] > 700) {OUTPUT(idText, text, "FL1  " << collisionPercept.state[JointData::legFL1] << "  index:  " 
00629         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00630         << motionInfo.executedMotionRequest.walkParams.rotation);}
00631       if (collisionPercept.state[JointData::legFL2] > 700) {OUTPUT(idText, text, "FL2  " << collisionPercept.state[JointData::legFL2] << "  index:  " 
00632         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00633         << motionInfo.executedMotionRequest.walkParams.rotation);}
00634       if (collisionPercept.state[JointData::legHL1] > 700) {OUTPUT(idText, text, "HL1  " << collisionPercept.state[JointData::legHL1] << "  index:  " 
00635         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00636         << motionInfo.executedMotionRequest.walkParams.rotation);}
00637       if (collisionPercept.state[JointData::legHL2] > 700) {OUTPUT(idText, text, "HL2  " << collisionPercept.state[JointData::legHL2] << "  index:  " 
00638         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00639         << motionInfo.executedMotionRequest.walkParams.rotation);}
00640       
00641       if (collisionPercept.state[JointData::legFR1] > 700) {OUTPUT(idText, text, "FR1  " << collisionPercept.state[JointData::legFR1] << "  index:  " 
00642         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00643         << motionInfo.executedMotionRequest.walkParams.rotation);}
00644       if (collisionPercept.state[JointData::legFR2] > 700) {OUTPUT(idText, text, "FR2  " << collisionPercept.state[JointData::legFR2] << "  index:  " 
00645         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00646         << motionInfo.executedMotionRequest.walkParams.rotation);}
00647       if (collisionPercept.state[JointData::legHR1] > 700) {OUTPUT(idText, text, "HR1  " << collisionPercept.state[JointData::legHR1] << "  index:  " 
00648         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00649         << motionInfo.executedMotionRequest.walkParams.rotation );}
00650       if (collisionPercept.state[JointData::legHR2] > 700) {OUTPUT(idText, text, "HR2  " << collisionPercept.state[JointData::legHR2] << "  index:  " 
00651         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00652         << motionInfo.executedMotionRequest.walkParams.rotation );}
00653       */
00654       
00655       
00656       
00657       //DEBUG BEGIN
00658       /*
00659       if (!(index%400))
00660       {     
00661         
00662         if (
00663           (currentMotionX) ||
00664           (currentMotionY) ||
00665           (currentRotation)
00666           )
00667           
00668         {
00669           OUTPUT(idText, text, "FL1 " << maxfl1 << "\r\n" <<  
00670             "FR1 " << maxfr1 << "\r\n" <<  
00671             "FL2 " << maxfl2 << "\r\n" <<  
00672             "FR2 " << maxfr2 << "\r\n" <<  
00673             "HL1 " << maxhl1 << "\r\n" <<  
00674             "HR1 " << maxhr1 << "\r\n" <<  
00675             "HL2 " << maxhl2 << "\r\n" <<  
00676             "HR2 " << maxhr2 << "\r\n" <<  
00677             "***********************" << "\r\n" << 
00678             index << " Mode: " << walkingMode << "\r\n" << 
00679             " X-sp " << currentMotionX << " X-Ratio " << xRatio << "\r\n" << 
00680             " Y-sp " << currentMotionY << " Y-Ratio " << yRatio << "\r\n" << 
00681             " R-sp " << currentRotation/3.1415*180 <<" r-Ratio " << rRatio << "\r\n" <<
00682             "*++++++++++++++*");       
00683         } 
00684         
00685         maxfl1 = 0;
00686         maxfl2 = 0;
00687         maxfr1 = 0;
00688         maxfr2 = 0;
00689         maxhl1 = 0;
00690         maxhl2 = 0;
00691         maxhr1 = 0;
00692         maxhr2 = 0;
00693         
00694       }
00695       
00696       if ((walkingMode))
00697       {
00698         
00699         if (maxfl1 < collisionPercept.state[JointData::legFL1])
00700         {maxfl1 = collisionPercept.state[JointData::legFL1];}
00701         if (maxfl2 < collisionPercept.state[JointData::legFL2])
00702         {maxfl2 = collisionPercept.state[JointData::legFL2];}
00703         if (maxfr1 < collisionPercept.state[JointData::legFR1])
00704         {maxfr1 = collisionPercept.state[JointData::legFR1];}
00705         if (maxfr2 < collisionPercept.state[JointData::legFR2])
00706         {maxfr2 = collisionPercept.state[JointData::legFR2];}
00707         if (maxhl1 < collisionPercept.state[JointData::legHL1])
00708         {maxhl1 = collisionPercept.state[JointData::legHL1];}
00709         if (maxhl2 < collisionPercept.state[JointData::legHL2])
00710         {maxhl2 = collisionPercept.state[JointData::legHL2];}
00711         if (maxhr1 < collisionPercept.state[JointData::legHR1])
00712         {maxhr1 = collisionPercept.state[JointData::legHR1];}
00713         if (maxhr2 < collisionPercept.state[JointData::legHR2])
00714         {maxhr2 = collisionPercept.state[JointData::legHR2];}
00715         
00716       }*/
00717        // DEBUG END
00718       
00719       
00720       
00721       }
00722       index += sensorDataBuffer.numOfFrames; // increase index value
00723       lastMotionChange += sensorDataBuffer.numOfFrames; // increase value since Motion Change
00724       lastMotionX = currentMotionX;          // remember last Motion X, Y and Rotation
00725       lastMotionY = currentMotionY;
00726       lastRotation= currentRotation;
00727 }
00728 
00729 
00730 /*
00731 * Change log :
00732 * 
00733 * $Log: GT2004CollisionDetector.cpp,v $
00734 * Revision 1.1  2004/07/10 00:18:28  spranger
00735 * renamed (readded) for coderelease
00736 *
00737 * Revision 1.6  2004/06/24 14:05:22  goehring
00738 * Collisions allowed during walkRequest::normal only
00739 *
00740 * Revision 1.5  2004/06/03 15:27:58  goehring
00741 * Parameters for UD2004
00742 *
00743 * Revision 1.4  2004/06/02 17:18:23  spranger
00744 * MotionRequest cleanup
00745 *
00746 * Revision 1.3  2004/05/27 17:13:37  jhoffman
00747 * - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
00748 * - clipping included for setJoints
00749 * - removed some microrad/rad-bugs
00750 * - bodyPosture constructor and "=" operator fixed
00751 *
00752 * Revision 1.2  2004/05/27 09:09:19  loetzsch
00753 * clean up in the interfaces
00754 *
00755 * Revision 1.1.1.1  2004/05/22 17:19:07  cvsadm
00756 * created new repository GT2004_WM
00757 *
00758 * Revision 1.7  2004/04/19 10:31:38  goehring
00759 * no message
00760 *
00761 * Revision 1.6  2004/04/14 16:04:43  goehring
00762 * Thresholds modified to ERS7
00763 *
00764 * Revision 1.5  2004/03/08 01:38:49  roefer
00765 * Interfaces should be const
00766 *
00767 * Revision 1.4  2003/12/31 20:16:13  roefer
00768 * SensorData for ERS-7
00769 *
00770 * Revision 1.3  2003/11/14 19:02:26  goehring
00771 * frameNumber added
00772 *
00773 * Revision 1.2  2003/10/16 13:00:03  goehring
00774 * RotationThreshold doubled, TooBigMotionChangeThreshold changed from continous to discrete
00775 *
00776 * Revision 1.1  2003/10/06 14:10:14  cvsadm
00777 * Created GT2004 (M.J.)
00778 *
00779 * Revision 1.9  2003/09/26 21:23:20  loetzsch
00780 * renamed class JointState to CollisionPercept
00781 *
00782 * Revision 1.8  2003/07/15 12:49:14  goehring
00783 * Speed optimized
00784 *
00785 * Revision 1.7  2003/07/08 00:42:25  goehring
00786 * Review, Debug
00787 *
00788 * Revision 1.6  2003/07/06 19:43:52  goehring
00789 * First Release Version
00790 *
00791 * Revision 1.5  2003/07/06 07:26:31  goehring
00792 * CollisionModell changed to equidistant threshold intervalls
00793 *
00794 * Revision 1.4  2003/07/05 23:47:45  goehring
00795 * Debug
00796 *
00797 * Revision 1.3  2003/07/05 23:23:51  goehring
00798 * Debugging and Improvement, Adaptation to GT2003 Walking Engine
00799 *
00800 * Revision 1.2  2003/07/04 09:24:09  goehring
00801 * Improved the collision threshold modell
00802 *
00803 * Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
00804 * created new repository for the competitions in Padova from the 
00805 * tamara CVS (Tuesday 2:00 pm)
00806 *
00807 * removed unused solutions
00808 *
00809 * Revision 1.17  2003/06/21 16:46:48  goehring
00810 * Review
00811 *
00812 * Revision 1.16  2003/06/20 20:20:43  goehring
00813 * ExecutedMotionReques instead of MotionRequest implemented
00814 *
00815 * Revision 1.15  2003/06/20 16:57:45  goehring
00816 * CollisionDetectorValues improved
00817 *
00818 * Revision 1.14  2003/06/20 10:29:27  goehring
00819 * MotionCombination thresholds implemented, Review
00820 *
00821 * Revision 1.13  2003/06/18 18:12:07  goehring
00822 * Thresholds improved
00823 *
00824 * Revision 1.12  2003/06/13 12:39:51  goehring
00825 * Collision algorithm improved
00826 *
00827 * Revision 1.11  2003/06/10 15:05:32  goehring
00828 * Review
00829 *
00830 * Revision 1.10  2003/06/05 23:01:22  goehring
00831 * Several bugfixes  and value smoothing implemented
00832 *
00833 * Revision 1.9  2003/06/04 18:24:15  goehring
00834 * Debugging of the Interface for the HistoryViewer
00835 *
00836 * Revision 1.8  2003/06/04 14:44:14  goehring
00837 * Debug
00838 *
00839 * Revision 1.7  2003/06/04 12:22:27  goehring
00840 * Debug
00841 *
00842 * Revision 1.6  2003/06/03 09:27:54  goehring
00843 * HistoryValues implemented
00844 *
00845 * Revision 1.5  2003/06/02 09:40:13  goehring
00846 * Review
00847 *
00848 * Revision 1.4  2003/05/23 13:08:26  goehring
00849 * Number of Sensorvalues changed from 4 to numOfValues (BETA)
00850 *
00851 * Revision 1.3  2003/05/23 09:36:51  goehring
00852 * MotionStabilizer gets all 4 Sensor Values
00853 *
00854 * Revision 1.2  2003/05/16 08:09:34  goehring
00855 * Testroutines implemented
00856 *
00857 * Revision 1.1  2003/04/25 19:48:08  goehring
00858 * Added new module CollisionDetector
00859 *
00860 */