GT2005: Modules/CollisionDetector/GT2005CollisionDetector.cpp Source File

00001 /** 
00002 * @file GT2005CollisionDetector.cpp
00003 * 
00004 * This file contains a class for Sensor Data Processing.
00005 */
00006 //2005/06/28 for GT2005-WalkingEngine
00007 #include "GT2005CollisionDetector.h"
00008 #include "Tools/Debugging/Debugging.h"
00009 #include "Tools/Debugging/DebugDataTable.h"
00010 
00011 
00012     double maxfl1 = 0;
00013     double maxfl2 = 0;
00014     double maxfr1 = 0;
00015     double maxfr2 = 0;
00016     double maxhl1 = 0;
00017     double maxhl2 = 0;
00018     double maxhr1 = 0;
00019     double maxhr2 = 0;
00020  
00021 
00022 
00023 GT2005CollisionDetector::GT2005CollisionDetector
00024 (const CollisionDetectorInterfaces& interfaces)
00025 : CollisionDetector(interfaces)
00026 {
00027   infin = 1000000000;
00028   index = 0;
00029   min =  5;   // minimum delay value
00030   max = 12;   // maximum delay value
00031   oldVal = 0;
00032   newVal = 0;
00033 
00034   frontMin =  0;//20
00035   frontMax = infin;  //400 or 180
00036   backMin =  -0;//-20
00037   backMax = -infin;
00038   sideMin =   0;  //20
00039   sideMax =  infin; //400 or left 120
00040   rotMin = 0.00;
00041   rotMax = infin; //2.5
00042     
00043   
00044   // Behaviour motion change recognition  
00045   currentMotionX = 0;
00046   currentMotionY = 0;
00047   currentRotation = 0;
00048   lastMotionX = 0;
00049   lastMotionY = 0;
00050   lastRotation = 0;
00051   motionChangeThresholdX = 50;
00052   motionChangeThresholdY = 50;
00053   motionChangeThresholdR = 0.2;
00054   calmDown = 32; 
00055   lastMotionChange = calmDown + 1;
00056   lastTooBigMotionChange = 0;
00057   motionChangeThresholdFactor = 1;
00058   xRatio = 0;
00059   yRatio = 0;
00060   rRatio = 0;
00061   q = 12;
00062 
00063 
00064 
00065   // Robot sepecific parameters for direction weights
00066   scaleThresholdFore = scaleThresholdBack = scaleThresholdSide = 1;
00067   scaleThresholdRot = 2;
00068 
00069   walkingMode = 0;
00070   
00071   //Debugging variables  
00072  //* 
00073   //*/
00074   // reset the Ringbuffer and the FloatingAvg
00075   // i = number of joints, l = quadruple element, d = delay index
00076   
00077   for (int i = 0; i < SensorData::numOfSensor_ERS210; i++)
00078   { 
00079     for (int l = 0; l < SensorDataBuffer::maxNumOfFrames; l++)
00080     {
00081       for (int d = 0; d < 16; d++)
00082       {
00083         floatingAvg[i][l][d] = 0;
00084       }
00085       minAvg[i][l] = 0;
00086     }
00087     
00088     
00089     // i = number of joints, j = Ringbuffer element, k = sensor or actor data
00090     // t = lower or upper threshold bound
00091     
00092     for (int j = 0; j < 128; j++)
00093     {
00094       for (int k = 0; k < 2; k++) 
00095       {
00096         ringBuffer[i][j][k] = 0;
00097       }
00098     }
00099     for (int t = 0; t < 3; t++)
00100     {
00101       threshold[i][t] = 0;
00102     }
00103   }
00104   
00105   // unused angle values
00106   threshold[SensorData::legFL3][2] = infin;
00107   threshold[SensorData::legFR3][2] = infin;
00108   threshold[SensorData::legHL3][2] = infin;
00109   threshold[SensorData::legHR3][2] = infin;
00110   
00111 }
00112 
00113 
00114 void GT2005CollisionDetector::calculateThresholds() 
00115 {
00116   //Head thresholds are indifferent to motions and directions
00117   walkingMode = 0;
00118   
00119   threshold[SensorData::legFL1][2] = 1;
00120   threshold[SensorData::legHL1][2] = 1;
00121   threshold[SensorData::legFL2][2] = 1;
00122   threshold[SensorData::legHL2][2] = 1;
00123   
00124   //Threshold calculation for straight forward walking only
00125   if  ((currentMotionX > frontMin) && (currentMotionX <= frontMax))
00126   {
00127     walkingMode = 1; //foreward
00128    
00129     if (currentMotionX <= 5)
00130     {
00131       threshold[SensorData::legFL1][1] = 2500;  threshold[SensorData::legFL2][1] = 1050;  
00132       threshold[SensorData::legHL1][1] = 15000;  threshold[SensorData::legHL2][1] = 3000;   
00133     }
00134     else if  (currentMotionX <= 50)
00135     {
00136       threshold[SensorData::legFL1][1] =  2500;  threshold[SensorData::legFL2][1] = 1050;  
00137       threshold[SensorData::legHL1][1] = 15000;  threshold[SensorData::legHL2][1] = 3000;   
00138     }
00139     else if (currentMotionX <= 100)
00140     {
00141       threshold[SensorData::legFL1][1] =  1000;  threshold[SensorData::legFL2][1] =  2500;
00142       threshold[SensorData::legHL1][1] = 10000;  threshold[SensorData::legHL2][1] = 12000;
00143     }
00144     else if (currentMotionX <= 150)
00145     {
00146       threshold[SensorData::legFL1][1] = 6000;  threshold[SensorData::legFL2][1] = 10000;
00147       threshold[SensorData::legHL1][1] = 16000;  threshold[SensorData::legHL2][1] =  2200;
00148     }
00149     else if (currentMotionX <= 200)
00150     {
00151       threshold[SensorData::legFL1][1] = 10000;  threshold[SensorData::legFL2][1] = 10000;
00152       threshold[SensorData::legHL1][1] = 16000;  threshold[SensorData::legHL2][1] = 3200; 
00153     }
00154     else if (currentMotionX <= 250)
00155     {
00156       threshold[SensorData::legFL1][1] = 16000;  threshold[SensorData::legFL2][1] = 10000;
00157       threshold[SensorData::legHL1][1] = 16000;  threshold[SensorData::legHL2][1] = 3400; 
00158     }
00159     else
00160     {
00161       threshold[SensorData::legFL1][1] = 60000; threshold[SensorData::legFL2][1] = 3000000;  
00162       threshold[SensorData::legHL1][1] = 30000; threshold[SensorData::legHL2][1] = 60000; 
00163     }
00164 
00165   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdFore);
00166   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdFore);
00167   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdFore);
00168   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdFore);
00169  }
00170   
00171   
00172   //Threshold calculation for straight backward walking only
00173   else if  ((currentMotionX < backMin) && (currentMotionX >= backMax))
00174   {
00175     walkingMode = 2;
00176     
00177     if  (currentMotionX >= -5)
00178     {
00179       threshold[SensorData::legFL1][1] = 1300; threshold[SensorData::legFL2][1] = 1200;
00180       threshold[SensorData::legHL1][1] = 1600; threshold[SensorData::legHL2][1] =  600;
00181     }
00182     else if (currentMotionX >= -50)
00183     {
00184       threshold[SensorData::legFL1][1] = 3000; threshold[SensorData::legFL2][1] = 6000; 
00185       threshold[SensorData::legHL1][1] = 6000; threshold[SensorData::legHL2][1] = 3000; 
00186     }
00187     else if (currentMotionX >= -100)
00188     {
00189       threshold[SensorData::legFL1][1] = 5000; threshold[SensorData::legFL2][1] = 5000; 
00190       threshold[SensorData::legHL1][1] = 15000; threshold[SensorData::legHL2][1] = 6000; 
00191     }
00192     else if (currentMotionX >= -150)
00193     {
00194       threshold[SensorData::legFL1][1] =  10000; threshold[SensorData::legFL2][1] = 4500;
00195       threshold[SensorData::legHL1][1] =  15000; threshold[SensorData::legHL2][1] = 5000;
00196     }
00197     else if (currentMotionX >= -200)
00198     {
00199       threshold[SensorData::legFL1][1] =  11000; threshold[SensorData::legFL2][1] = 7000;
00200       threshold[SensorData::legHL1][1] =  65000; threshold[SensorData::legHL2][1] = 3000;
00201     }
00202     else if (currentMotionX >= -250)
00203     {
00204       threshold[SensorData::legFL1][1] =  15000; threshold[SensorData::legFL2][1] = 8000; 
00205       threshold[SensorData::legHL1][1] =  75000; threshold[SensorData::legHL2][1] = 4000; 
00206     }
00207     else
00208     {
00209       threshold[SensorData::legFL1][1] =  9000; threshold[SensorData::legHL1][1] = 1500;
00210       threshold[SensorData::legFL2][1] = 80000; threshold[SensorData::legHL2][1] = 2300;
00211     }
00212   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdBack);
00213   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdBack);
00214   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdBack);
00215   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdBack);
00216   }
00217  
00218   //Threshold calculation for left and right walking 
00219   
00220   if (
00221     (fabs(currentMotionY) > sideMin) &&
00222     (fabs(currentMotionY) <= sideMax)
00223     )
00224   {
00225     walkingMode = walkingMode * 10 + 3;
00226     
00227     if (fabs(currentMotionY) <= 5)
00228     {
00229       threshold[SensorData::legFL1][1] =  3000; threshold[SensorData::legFL2][1] =  70000; 
00230       threshold[SensorData::legHL1][1] = 16000; threshold[SensorData::legHL2][1] =  12000;
00231     }
00232     else if 
00233       (fabs(currentMotionY) <= 50)
00234     {
00235       threshold[SensorData::legFL1][1] =  3000; threshold[SensorData::legFL2][1] =  70000;
00236       threshold[SensorData::legHL1][1] = 17000; threshold[SensorData::legHL2][1] =  12000; 
00237     }
00238     else if 
00239       (fabs(currentMotionY) <= 100)
00240     {
00241       threshold[SensorData::legFL1][1] =  6000; threshold[SensorData::legFL2][1]  = 20000;
00242       threshold[SensorData::legHL1][1] = 25000; threshold[SensorData::legHL2][1] = 30000;
00243     }
00244     else if 
00245       (fabs(currentMotionY) <= 150)
00246     {
00247       threshold[SensorData::legFL1][1] =  10000; threshold[SensorData::legFL2][1] = 30000;
00248       threshold[SensorData::legHL1][1] =  50000; threshold[SensorData::legHL2][1] = 20000; 
00249     }
00250     else if 
00251       (fabs(currentMotionY) <= 200)
00252     {
00253       threshold[SensorData::legFL1][1] =  7000; threshold[SensorData::legFL2][1] =  30000;
00254       threshold[SensorData::legHL1][1] = 30000; threshold[SensorData::legHL2][1] = 30000; 
00255     }
00256     else 
00257     {
00258       threshold[SensorData::legFL1][1] = 34000; threshold[SensorData::legFL2][1] = 30000;
00259       threshold[SensorData::legHL1][1] = 40000; threshold[SensorData::legHL2][1] = 40000;
00260     }
00261   } //sideThresholds ended
00262 
00263   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdSide);
00264   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdSide);
00265   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdSide);
00266   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdSide);
00267   
00268   //Threshold calculation for rotation no matter if there is any other direction
00269   // rotation only
00270     if ((fabs(currentRotation) > rotMin) && (fabs(currentRotation) <= rotMax)) 
00271     {
00272       walkingMode = walkingMode * 10 + 5;     
00273       
00274       if (fabs(currentRotation) <= 0.05)
00275       {
00276         threshold[SensorData::legFL1][1] = 1700; threshold[SensorData::legFL2][1] =  2600; 
00277         threshold[SensorData::legHL1][1] = 5000; threshold[SensorData::legHL2][1] =  4600; 
00278       }
00279       else if (fabs(currentRotation) <= 0.5)
00280       {
00281         threshold[SensorData::legFL1][1] = 1800; threshold[SensorData::legFL2][1] =  2200; 
00282         threshold[SensorData::legHL1][1] = 3200; threshold[SensorData::legHL2][1] =  6000; 
00283       }
00284       else if (fabs(currentRotation) <= 1.0)
00285       {
00286         threshold[SensorData::legFL1][1] =  5000; threshold[SensorData::legFL2][1] = 4500;
00287         threshold[SensorData::legHL1][1] =  10200; threshold[SensorData::legHL2][1] = 6000; 
00288       }
00289       else if (fabs(currentRotation) <= 1.5)
00290       {
00291         threshold[SensorData::legFL1][1] =  3000; threshold[SensorData::legFL2][1] = 6000; 
00292         threshold[SensorData::legHL1][1] =  10000; threshold[SensorData::legHL2][1] = 6000; 
00293       }
00294       else if (fabs(currentRotation) <= 2.0)
00295       {
00296         threshold[SensorData::legFL1][1] =  2000; threshold[SensorData::legFL2][1] = 6000; 
00297         threshold[SensorData::legHL1][1] =  8000; threshold[SensorData::legHL2][1] = 6000; 
00298       }
00299       else if (fabs(currentRotation) <= 2.5)
00300       {
00301         threshold[SensorData::legFL1][1] =  6000; threshold[SensorData::legFL2][1] = 60000; 
00302         threshold[SensorData::legHL1][1] =  8000; threshold[SensorData::legHL2][1] = 12500; 
00303       }
00304       else if (fabs(currentRotation) <= 3.0)
00305       {
00306         threshold[SensorData::legFL1][1] =  8000; threshold[SensorData::legFL2][1] = 100000; 
00307         threshold[SensorData::legHL1][1] =  8000; threshold[SensorData::legHL2][1] = 12500; 
00308       }
00309       else //max
00310       {
00311         threshold[SensorData::legFL1][1] =  26000; threshold[SensorData::legFL2][1] = 100000;
00312         threshold[SensorData::legHL1][1] =  17000; threshold[SensorData::legHL2][1] = 30000;
00313       }
00314 
00315   threshold[SensorData::legFL1][2] += int(threshold[SensorData::legFL1][1] * scaleThresholdRot);
00316   threshold[SensorData::legFL2][2] += int(threshold[SensorData::legFL2][1] * scaleThresholdRot);
00317   threshold[SensorData::legHL1][2] += int(threshold[SensorData::legHL1][1] * scaleThresholdRot);
00318   threshold[SensorData::legHL2][2] += int(threshold[SensorData::legHL2][1] * scaleThresholdRot);
00319   }
00320   
00321 ///To Test 
00322 if (
00323     (
00324      (fabs(currentRotation) >= 0.75) && 
00325       (
00326        (fabs(currentMotionX) >= 100) ||
00327        (fabs(currentMotionY) >= 100 )
00328       )
00329      ) 
00330      ||
00331      (
00332       (fabs(currentMotionX) >= 100) &&
00333       (fabs(currentMotionY) >= 100)
00334      )
00335     )
00336   {
00337    threshold[SensorData::legFL1][2] *= 2;
00338    threshold[SensorData::legFL2][2] *= 2;
00339    threshold[SensorData::legHL1][2] *= 2;
00340    threshold[SensorData::legHL2][2] *= 2;
00341   }
00342    
00343       
00344   
00345   //default
00346   if (walkingMode == 0)
00347   {
00348     threshold[SensorData::legFL1][2] = infin;
00349     threshold[SensorData::legFL2][2] = infin;
00350     threshold[SensorData::legHL1][2] = infin;
00351     threshold[SensorData::legHL2][2] = infin;
00352   }
00353   /*
00354   threshold[SensorData::legFL1][2] =  1000;
00355   threshold[SensorData::legFL2][2] =  1000;
00356   threshold[SensorData::legHL1][2] =  1000;
00357   threshold[SensorData::legHL2][2] =  1000;
00358   */
00359   
00360   
00361   /* Head values */
00362   
00363   threshold[SensorData::neckTilt][2] = 100000;
00364   threshold[SensorData::headPan][2]  = 30000;
00365   threshold[SensorData::headTilt][2] = 20000;
00366 
00367   MODIFY("threshold[SensorData::neckTilt][2]",threshold[SensorData::neckTilt][2]);
00368   MODIFY("threshold[SensorData::headPan][2]",threshold[SensorData::headPan][2]);
00369   MODIFY("threshold[SensorData::headTilt][2]",threshold[SensorData::headTilt][2]);
00370 
00371 
00372   //DEBUG START Half the thresholds
00373   threshold[SensorData::legFL1][2] = (int)(threshold[SensorData::legFL1][2] * 1);
00374   threshold[SensorData::legFL2][2] = (int)(threshold[SensorData::legFL2][2] * 1);
00375   threshold[SensorData::legHL1][2] = (int)(threshold[SensorData::legHL1][2] * 1);
00376   threshold[SensorData::legHL2][2] = (int)(threshold[SensorData::legHL2][2] * 1);
00377   //DEBUG END
00378 
00379  
00380   /* Symmetric exchange */
00381   
00382   threshold[SensorData::legFR1][2] = threshold[SensorData::legFL1][2];
00383   threshold[SensorData::legFR2][2] = threshold[SensorData::legFL2][2];
00384   threshold[SensorData::legHR1][2] = threshold[SensorData::legHL1][2];
00385   threshold[SensorData::legHR2][2] = threshold[SensorData::legHL2][2];
00386   
00387 }
00388 
00389 
00390 
00391 void GT2005CollisionDetector::execute()
00392 { 
00393   // to test JHA 
00394 
00395 #ifdef NDEBUG
00396 #else
00397   bool enabled(false);
00398 DEBUG_RESPONSE("CollisionDetector:override", enabled = true;); 
00399 DEBUG_RESPONSE("CollisionDetector:show head collision thresholds", );
00400 DEBUG_RESPONSE("CollisionDetector:show leg collision thresholds", );
00401   if (enabled)
00402   {
00403   double value = 1001;
00404   collisionPercept.state[JointData::neckTilt]  = 0;
00405   collisionPercept.state[JointData::headPan]  = 0;
00406   collisionPercept.state[JointData::headTilt]  = 0;
00407   collisionPercept.state[JointData::legFL1]  = 0;
00408   collisionPercept.state[JointData::legFR1]  = 0;
00409   collisionPercept.state[JointData::legHL1]  = 0;
00410   collisionPercept.state[JointData::legHR1]  = 0;
00411   collisionPercept.state[JointData::legFL2]  = 0;
00412   collisionPercept.state[JointData::legFR2]  = 0;
00413   collisionPercept.state[JointData::legHL2]  = 0;
00414   collisionPercept.state[JointData::legHR2]  = 0;
00415   collisionPercept.state[JointData::legFL3]  = 0;
00416   collisionPercept.state[JointData::legHL3]  = 0;
00417   collisionPercept.state[JointData::legFR3]  = 0;
00418   collisionPercept.state[JointData::legHR3]  = 0;
00419 
00420   DEBUG_RESPONSE( "CollisionDetector:neckTilt:on",collisionPercept.state[JointData::neckTilt]  = value; );
00421   DEBUG_RESPONSE( "CollisionDetector:headPan:on",collisionPercept.state[JointData::headPan]  = value; );
00422   DEBUG_RESPONSE( "CollisionDetector:headTilt:on",collisionPercept.state[JointData::headTilt]  = value; );
00423   DEBUG_RESPONSE( "CollisionDetector:legFL1:on",collisionPercept.state[JointData::legFL1]  = value; );
00424   DEBUG_RESPONSE( "CollisionDetector:legFR1:on",collisionPercept.state[JointData::legFR1]  = value; );
00425   DEBUG_RESPONSE( "CollisionDetector:legHL1:on",collisionPercept.state[JointData::legHL1]  = value; );
00426   DEBUG_RESPONSE( "CollisionDetector:legHR1:on",collisionPercept.state[JointData::legHR1]  = value; );
00427   DEBUG_RESPONSE( "CollisionDetector:legFL2:on",collisionPercept.state[JointData::legFL2]  = value; );
00428   DEBUG_RESPONSE( "CollisionDetector:legFR2:on",collisionPercept.state[JointData::legFR2]  = value; );
00429   DEBUG_RESPONSE( "CollisionDetector:legHL2:on",collisionPercept.state[JointData::legHL2]  = value; );
00430   DEBUG_RESPONSE( "CollisionDetector:legHR2:on",collisionPercept.state[JointData::legHR2]  = value; );
00431   DEBUG_RESPONSE( "CollisionDetector:legFL3:on",collisionPercept.state[JointData::legFL3]  = value; );
00432   DEBUG_RESPONSE( "CollisionDetector:legHL3:on",collisionPercept.state[JointData::legHL3]  = value; );
00433   DEBUG_RESPONSE( "CollisionDetector:legFR3:on",collisionPercept.state[JointData::legFR3]  = value; );
00434   DEBUG_RESPONSE( "CollisionDetector:legHR3:on",collisionPercept.state[JointData::legHR3]  = value; );
00435   return;
00436   }
00437 #endif
00438   // to test;
00439 
00440   collisionPercept.setFrameNumber(sensorDataBuffer.frame[0].frameNumber);
00441   currentMotionX = motionInfo.executedMotionRequest.walkRequest.walkParams.translation.x; // get the motion values
00442   currentMotionY = motionInfo.executedMotionRequest.walkRequest.walkParams.translation.y;
00443   currentRotation = motionInfo.executedMotionRequest.walkRequest.walkParams.rotation;
00444   walkingMode = 0;                       // reset walking Mode
00445   
00446   collisionPercept.state[JointData::legFL3] = 0; // 3rd angles are not calculated
00447   collisionPercept.state[JointData::legHL3] = 0;
00448   collisionPercept.state[JointData::legFR3] = 0;
00449   collisionPercept.state[JointData::legHR3] = 0;
00450   
00451   
00452   for (int i = 0; i < SensorData::numOfSensor_ERS210; i++) 
00453   { // for all sensors    
00454     
00455     if ((i == SensorData::neckTilt) || (i == SensorData::headPan) ||
00456       (i == SensorData::headTilt) || 
00457       (i == SensorData::legFL1) || (i == SensorData::legFL2) ||
00458       (i == SensorData::legHL1) || (i == SensorData::legHL2) ||
00459       (i == SensorData::legFR1) || (i == SensorData::legFR2) ||
00460       (i == SensorData::legHR1) || (i == SensorData::legHR2)) //consider just 2*4 leg angles and 3 head angles
00461     {
00462       for (int quarter = 0; quarter < sensorDataBuffer.numOfFrames; quarter++) // for all SensorDataElements values in the buffer
00463       { 
00464         int current = index + quarter; // current is the actual frame
00465         
00466         min = 5;  //minimum delay value
00467         max = 12; //maximum delay value 
00468         minAvg[i][current%SensorDataBuffer::maxNumOfFrames] = min;
00469         
00470         
00471         for (int d = min; d <= max; d++) // for all possible delays
00472         {
00473           
00474           oldVal = (int)(((ringBuffer[i][(current + 128 - q)%128][0] - 
00475             ringBuffer[i][(current + 128 - q - d)%128][1])*
00476             (ringBuffer[i][(current + 128 - q)%128][0] - 
00477             ringBuffer[i][(current + 128 - q - d)%128][1]))/12); // the old value off the Queue
00478           
00479           
00480           
00481           ringBuffer[i][current%128][0] = (int)(sensorDataBuffer.frame[quarter].data[i]/32);
00482           ringBuffer[i][current%128][1] = (int)(sensorDataBuffer.frame[quarter].refValue[i]/32);
00483           
00484           if (current < 16) 
00485           {
00486             ringBuffer[i][current%128][0] = 0;
00487           }
00488           
00489           //unbedingt nach der Ringbufferuebergabe !!!
00490           
00491           newVal = (int)(((ringBuffer[i][current%128][0] - 
00492             ringBuffer[i][(current + 128 - d)%128][1])*
00493             (ringBuffer[i][current%128][0] - 
00494             ringBuffer[i][(current + 128 - d)%128][1]))/12); // the new value for the Queue
00495           
00496           
00497           floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][d] =  newVal - oldVal + 
00498             floatingAvg[i][(current+SensorDataBuffer::maxNumOfFrames-1)%SensorDataBuffer::maxNumOfFrames][d];  
00499           
00500           
00501           if (floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][d] < floatingAvg[i][current%SensorDataBuffer::maxNumOfFrames][minAvg[i][current%SensorDataBuffer::maxNumOfFrames]])
00502           {
00503             minAvg[i][current%SensorDataBuffer::maxNumOfFrames] = d;
00504           }
00505         } // all delays
00506       } // all SensorDataBufferElements
00507     } // just head and Legs
00508   } //all angles
00509   
00510   
00511   xRatio = fabs(currentMotionX-lastMotionX);
00512   yRatio = fabs(currentMotionY-lastMotionY);
00513   rRatio = fabs(currentRotation-lastRotation);
00514   
00515   collisionPercept.reset();
00516   
00517   
00518   if  // calculates the behaviour threshold
00519     (
00520     (motionChangeThresholdX < xRatio) ||
00521     (motionChangeThresholdY < yRatio) ||
00522     (motionChangeThresholdR < rRatio)
00523     )  
00524   {
00525     lastMotionChange = 0;
00526   }
00527   
00528   
00529   if (lastMotionChange <= calmDown) //calculates the behaviour cleaned thresholds
00530   {
00531     //DEBUGPAPER 2005-02-10 excluded this part
00532     motionChangeThresholdFactor = 1000000;
00533    
00534     
00535     if (lastMotionChange >= calmDown)
00536     {
00537       motionChangeThresholdFactor = 1;
00538     }
00539 
00540   }
00541   
00542   if  ((motionInfo.executedMotionRequest.motionType==MotionRequest::walk) &&
00543     (motionInfo.executedMotionRequest.walkRequest.walkType==WalkRequest::normal))
00544      // no calming down anymore
00545   {
00546     calculateThresholds(); // calculates the threshold values considering speed and direction
00547     
00548     collisionPercept.state[JointData::neckTilt]  = floatingAvg[SensorData::neckTilt][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2];  //neck tilt
00549     collisionPercept.state[JointData::headPan]   = floatingAvg[SensorData::headPan][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2];  //Head pan
00550     collisionPercept.state[JointData::headTilt]  = floatingAvg[SensorData::headTilt][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2];  //Head tilt
00551     
00552     collisionPercept.state[JointData::legFL1] = floatingAvg[SensorData::legFL1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor; //FrontLeft1
00553     collisionPercept.state[JointData::legFR1] = floatingAvg[SensorData::legFR1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor; //FrontRight1
00554     collisionPercept.state[JointData::legHL1] = floatingAvg[SensorData::legHL1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor; //HindLeft1
00555     collisionPercept.state[JointData::legHR1] = floatingAvg[SensorData::legHR1][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor; //HindRight1
00556     
00557     collisionPercept.state[JointData::legFL2] = floatingAvg[SensorData::legFL2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor; //FrontLeft2
00558     collisionPercept.state[JointData::legFR2] = floatingAvg[SensorData::legFR2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor; //FrontRight2
00559     collisionPercept.state[JointData::legHL2] = floatingAvg[SensorData::legHL2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor; //HindLeft2
00560     collisionPercept.state[JointData::legHR2] = floatingAvg[SensorData::legHR2][index%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][index%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor; //HindRight2
00561     
00562     
00563     // look for bigger values of the SensorDataBuffer
00564     for (int current = index + 1; current < (index + sensorDataBuffer.numOfFrames); current++)
00565     {
00566       if (collisionPercept.state[JointData::neckTilt] < floatingAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2])  
00567       {collisionPercept.state[JointData::neckTilt] =  floatingAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::neckTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::neckTilt][2];} //Neck tilt
00568       
00569       if (collisionPercept.state[JointData::headPan]  < floatingAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2])
00570       {collisionPercept.state[JointData::headPan] =  floatingAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headPan][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headPan][2];}  //Head pan
00571       
00572       if (collisionPercept.state[JointData::headTilt] < floatingAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2])
00573       {collisionPercept.state[JointData::headTilt] =  floatingAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::headTilt][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::headTilt][2];}//Head tilt
00574       
00575       
00576       
00577       if (collisionPercept.state[JointData::legFL1] < floatingAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor)
00578       {collisionPercept.state[JointData::legFL1] = floatingAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL1][2] / motionChangeThresholdFactor;}//FrontLeft1
00579       
00580       if (collisionPercept.state[JointData::legFR1] < floatingAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor) 
00581       {collisionPercept.state[JointData::legFR1] = floatingAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR1][2] / motionChangeThresholdFactor;}//FrontRight1
00582       
00583       if (collisionPercept.state[JointData::legHL1] < floatingAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor) 
00584       {collisionPercept.state[JointData::legHL1] = floatingAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL1][2] / motionChangeThresholdFactor;}//HindLeft1
00585       
00586       if (collisionPercept.state[JointData::legHR1] < floatingAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor) 
00587       {collisionPercept.state[JointData::legHR1] = floatingAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR1][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR1][2] / motionChangeThresholdFactor;}//HindRight1
00588       
00589       
00590       
00591       if (collisionPercept.state[JointData::legFL2] < floatingAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor)  
00592       {collisionPercept.state[JointData::legFL2] =  floatingAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFL2][2] / motionChangeThresholdFactor;}//FrontLeft2
00593       
00594       if (collisionPercept.state[JointData::legFR2] < floatingAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor) 
00595       {collisionPercept.state[JointData::legFR2] =  floatingAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legFR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legFR2][2] / motionChangeThresholdFactor;}//FrontRight2
00596       
00597       if (collisionPercept.state[JointData::legHL2] < floatingAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor) 
00598       {collisionPercept.state[JointData::legHL2] =  floatingAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHL2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHL2][2] / motionChangeThresholdFactor;}//HindLeft2
00599       
00600       if (collisionPercept.state[JointData::legHR2] < floatingAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor) 
00601       {collisionPercept.state[JointData::legHR2] =  floatingAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames][minAvg[SensorData::legHR2][current%SensorDataBuffer::maxNumOfFrames]] / threshold[SensorData::legHR2][2] / motionChangeThresholdFactor;}//HindRight2
00602     }
00603 
00604 
00605       /* DEBUG START*/
00606 
00607       //if (lastMotionChange < calmDown) 
00608    /*   {
00609       OUTPUT(idText, text, motionChangeThresholdFactor << " \r\n " <<
00610         "   FL1: " << collisionPercept.state[JointData::legFL1] << 
00611         " FL2: " << collisionPercept.state[JointData::legFL2] << " \r\n " <<
00612         "   HL1: " << collisionPercept.state[JointData::legHL1] << 
00613         " HL2: " << collisionPercept.state[JointData::legHL2] << " \r\n " <<
00614         "   FR1: " << collisionPercept.state[JointData::legFR1] << 
00615         " FR2: " << collisionPercept.state[JointData::legFR2] << " \r\n " <<
00616         "   HR1: " << collisionPercept.state[JointData::legHR1] << 
00617         " HR2: " << collisionPercept.state[JointData::legHR2] << " \r\n " <<  
00618         "  index:  " << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x<< 
00619         " Y:" << motionInfo.executedMotionRequest.walkParams.translation.y <<
00620         " R: " << motionInfo.executedMotionRequest.walkParams.rotation << "   \r\n \r\n ");
00621       }  
00622      */  
00623     
00624       /* RELEASE DEBUG
00625       if (collisionPercept.state[JointData::legFL1] > 700) {OUTPUT(idText, text, "FL1  " << collisionPercept.state[JointData::legFL1] << "  index:  " 
00626         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00627         << motionInfo.executedMotionRequest.walkParams.rotation);}
00628       if (collisionPercept.state[JointData::legFL2] > 700) {OUTPUT(idText, text, "FL2  " << collisionPercept.state[JointData::legFL2] << "  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::legHL1] > 700) {OUTPUT(idText, text, "HL1  " << collisionPercept.state[JointData::legHL1] << "  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::legHL2] > 700) {OUTPUT(idText, text, "HL2  " << collisionPercept.state[JointData::legHL2] << "  index:  " 
00635         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00636         << motionInfo.executedMotionRequest.walkParams.rotation);}
00637       
00638       if (collisionPercept.state[JointData::legFR1] > 700) {OUTPUT(idText, text, "FR1  " << collisionPercept.state[JointData::legFR1] << "  index:  " 
00639         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00640         << motionInfo.executedMotionRequest.walkParams.rotation);}
00641       if (collisionPercept.state[JointData::legFR2] > 700) {OUTPUT(idText, text, "FR2  " << collisionPercept.state[JointData::legFR2] << "  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::legHR1] > 700) {OUTPUT(idText, text, "HR1  " << collisionPercept.state[JointData::legHR1] << "  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::legHR2] > 700) {OUTPUT(idText, text, "HR2  " << collisionPercept.state[JointData::legHR2] << "  index:  " 
00648         << index << " X:" << motionInfo.executedMotionRequest.walkParams.translation.x << " Y: " << motionInfo.executedMotionRequest.walkParams.translation.y << " R: " 
00649         << motionInfo.executedMotionRequest.walkParams.rotation );}
00650       */
00651       
00652       
00653       
00654       //DEBUG BEGIN
00655       
00656         DEBUG_RESPONSE("CollisionDetector:show head collision thresholds", 
00657           OUTPUT(idText, text, "Head Tilt " << collisionPercept.state[JointData::headTilt] <<
00658                   "Head Pan " << collisionPercept.state[JointData::headPan] <<
00659                   "Neck Tilt " << collisionPercept.state[JointData::neckTilt]);
00660           );
00661 
00662 
00663       if (!(index%400))
00664       {     
00665         
00666         if (
00667           (currentMotionX) ||
00668           (currentMotionY) ||
00669           (currentRotation)
00670           )
00671           
00672         {
00673           DEBUG_RESPONSE("CollisionDetector:show leg collision thresholds", 
00674           OUTPUT(idText, text, "FL1 " << maxfl1 << "\r\n" <<  
00675             "FR1 " << maxfr1 << "\r\n" <<  
00676             "FL2 " << maxfl2 << "\r\n" <<  
00677             "FR2 " << maxfr2 << "\r\n" <<  
00678             "HL1 " << maxhl1 << "\r\n" <<  
00679             "HR1 " << maxhr1 << "\r\n" <<  
00680             "HL2 " << maxhl2 << "\r\n" <<  
00681             "HR2 " << maxhr2 << "\r\n" <<  
00682             "***********************" << "\r\n" << 
00683             index << " Mode: " << walkingMode << "\r\n" << 
00684             " X-sp " << currentMotionX << " X-Ratio " << xRatio << "\r\n" << 
00685             " Y-sp " << currentMotionY << " Y-Ratio " << yRatio << "\r\n" << 
00686             " R-sp " << currentRotation/3.1415*180 <<" r-Ratio " << rRatio << "\r\n" <<
00687             "*++++++++++++++*");   
00688           );
00689          
00690         } 
00691         
00692         maxfl1 = 0;
00693         maxfl2 = 0;
00694         maxfr1 = 0;
00695         maxfr2 = 0;
00696         maxhl1 = 0;
00697         maxhl2 = 0;
00698         maxhr1 = 0;
00699         maxhr2 = 0;
00700         
00701       }
00702       
00703       if ((walkingMode))
00704       {
00705         
00706         if (maxfl1 < collisionPercept.state[JointData::legFL1])
00707         {maxfl1 = collisionPercept.state[JointData::legFL1];}
00708         if (maxfl2 < collisionPercept.state[JointData::legFL2])
00709         {maxfl2 = collisionPercept.state[JointData::legFL2];}
00710         if (maxfr1 < collisionPercept.state[JointData::legFR1])
00711         {maxfr1 = collisionPercept.state[JointData::legFR1];}
00712         if (maxfr2 < collisionPercept.state[JointData::legFR2])
00713         {maxfr2 = collisionPercept.state[JointData::legFR2];}
00714         if (maxhl1 < collisionPercept.state[JointData::legHL1])
00715         {maxhl1 = collisionPercept.state[JointData::legHL1];}
00716         if (maxhl2 < collisionPercept.state[JointData::legHL2])
00717         {maxhl2 = collisionPercept.state[JointData::legHL2];}
00718         if (maxhr1 < collisionPercept.state[JointData::legHR1])
00719         {maxhr1 = collisionPercept.state[JointData::legHR1];}
00720         if (maxhr2 < collisionPercept.state[JointData::legHR2])
00721         {maxhr2 = collisionPercept.state[JointData::legHR2];}
00722         
00723       }//*/
00724        // DEBUG END
00725       
00726       
00727       
00728       }
00729       index += sensorDataBuffer.numOfFrames; // increase index value
00730       lastMotionChange += sensorDataBuffer.numOfFrames; // increase value since Motion Change
00731       lastMotionX = currentMotionX;          // remember last Motion X, Y and Rotation
00732       lastMotionY = currentMotionY;
00733       lastRotation= currentRotation;
00734 }