Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/CollisionDetector/GT2005CollisionDetector.cpp

Go to the documentation of this 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 }