GT2004: Modules/GetupEngine/GT2004GetupEngine.cpp Source File

00001 /**
00002 * @file GT2004GetupEngine.cpp
00003 * 
00004 * This file contains the gt2004 getup engine.
00005 * 
00006 * @author Max Risler
00007 * @author Andreas Heinze
00008 */
00009 
00010 #include "GT2004GetupEngine.h"
00011 #include "Platform/SystemCall.h"
00012 #include "Tools/Debugging/Debugging.h"
00013 #include "Tools/RobotConfiguration.h"
00014 #include "Representations/Motion/MotionInfo.h"
00015 
00016 
00017 /* GT2001 Mof sequence
00018 * ~ ~ ~   ~ ~ ~   2000 0 2500      2000 0 2500      2000 0 2500      2000 0 2500       0 80
00019 * ~ ~ ~   ~ ~ ~   1500 1500 2500   1500 1500 2500   1500 1500 2500   1500 1500 2500    0 30
00020 * ~ ~ ~   ~ ~ ~   -1600 1500 2500  -1600 1500 2500  -1400 1600 2500  -1400 1600 2500   0 50
00021 * ~ ~ ~   ~ ~ ~   -1300 800 2100   -1300 800 2100   -1200 1000 2300  -1200 1000 2300   0 10
00022 * ~ ~ ~   ~ ~ ~   -426 356 1626   -426 356 1626   -662 456 1784   -662 456 1784   1 25
00023 *
00024 * current sequence 
00025 * ~ ~ ~   ~ ~ ~   2000 0 2500      2000 0 2500      2000 0 2500      2000 0 2500       0 80
00026 * ~ ~ ~   ~ ~ ~   1500 1500 2500   1500 1500 2500   1500 1500 2500   1500 1500 2500    0 30
00027 * ~ ~ ~   ~ ~ ~   -1600 1500 2500  -1600 1500 2500  -1400 1600 2500  -1400 1600 2500   0 50
00028 * ~ ~ ~   ~ ~ ~   0 800 2100   0 800 2100   -1200 1000 2300  -1200 1000 2300   0 10
00029 */
00030 
00031 
00032 /* GT2004 ERS7 Mof sequence
00033 
00034   * -------------for lying on one side------------------------
00035   *
00036   * right legs transformation:
00037   * -159  -230   540   ~ ~ ~    2112  -119  1683      -5   199  1916    1985   -91  2053    -488    39  1373   1 30
00038   * -159  -230   540   ~ ~ ~    2112  -119  1683      -5   199  1916    1985   -91  2053    -488    39  1373   1 50
00039   *  -82     7   507   ~ ~ ~    1498  -142  2120     -28   233  1911    1222  -125  2113    -464     5  1411   0 25
00040   * -142   -89   468   ~ ~ ~    1487  -142  2142     -28  1571  1905    1406  -125  2118      78  1423  2102   0 25
00041   *
00042   *
00043   * or
00044   *
00045   *
00046   * left legs transformation:
00047   * -159   230  -540   ~ ~ ~      -5   199  1916    2112  -119  1683    -488    39  1373    1985   -91  2053   1 30
00048   * -159   230  -540   ~ ~ ~      -5   199  1916    2112  -119  1683    -488    39  1373    1985   -91  2053   1 50
00049   * -82    -7  -507   ~ ~ ~     -28   233  1911    1498  -142  2120    -464     5  1411    1222  -125  2113   0 25
00050   * -142    89  -468   ~ ~ ~     -28  1571  1905    1487  -142  2142      78  1423  2102    1406  -125  2118   0 25
00051   *
00052   *
00053   *
00054   *
00055   * -----------for sitting upright-----------------------------
00056   * sit down:
00057   * -5 151 638  ~ ~ ~  -129 267 183  37 56 -79  -811 216 258 -699 11 -17  1 50
00058   *
00059   * legs to sides:
00060   * -82 125 553  ~ ~ ~  -73 239 120  93 68 -34  -823 1321 258 -724 1418 -62  1 25
00061   * -65 114 553  ~ ~ ~  -90 301 109  81 5 -17  816 1344 227 799 1383 -79  1 25
00062   *
00063   *
00064   *----------------------make a roll-----------------------
00065   *
00066   * to back:
00067   * -1346 1564 371  ~ ~ ~  -112 1623 114  -5 1623 219  -54 1520 221 0 1457 362  0 50
00068   *
00069   * legs up:
00070   * -1000 438 364  ~ ~ ~  0 -182 91  -84 -182 184  -73 -250 184 24 -250 338  0 60
00071   *
00072   
00073     
00074       *---------------------headstand to stand--------------------
00075       * move legs:
00076       * 16 -70 872  ~ ~ ~  -73 244 2125  -164 324 2044  -97 273 184 72 153 239  1 50
00077       
00078         * head press:
00079         * -1346 -3 586  ~ ~ ~  -73 273 2040  -90 353 2077  -73 267 184 109 148 307  1 70
00080         *
00081         *
00082         *-------------------------unknown--------------------------
00083         * stand:
00084         * -38 18 781 ~ ~ ~  -62 273 1902  -62 273 1902  -589 216 1385 -581 171 1378  0 60
00085         * -38 18 781 ~ ~ ~  -62 273 1902  -62 273 1902  -589 216 1385 -581 171 1378  0 60
00086         *
00087         * sit down:
00088         * -49 -14 573  ~ ~ ~  -140 261 160  24 -79 -153  -683 432 -45 -693 222 -51  0 70
00089         *
00090         * lay down
00091         * -141 98 646  ~ ~ ~  978 45 977  997 5 917  -1499 290 460 -1396 318 218  1 50
00092         *
00093         * move up:
00094         * -175 99 579  ~ ~ ~  1232 45 2125  1143 85 2066  -2038 261 559 -1872 313 104  0 50
00095         * -175 106 586  ~ ~ ~  894 28 2125  816 74 2038  -2033 381 1531 -1921 307 1552  0 50
00096         * -175 95 592  ~ ~ ~  905 56 2142  829 91 2077  -1036 102 2167 -1149 -11 2167  0 50
00097         *
00098         * stand:
00099         * -38 18 781 ~ ~ ~  -62 273 1902  -62 273 1902  -589 216 1385 -581 171 1378  0 60
00100         * -38 18 781 ~ ~ ~  -62 273 1902  -62 273 1902  -589 216 1385 -581 171 1378  0 60
00101         *
00102         *
00103         * --------new roll type--------------------
00104         * move right legs:
00105         * -1173 -23 -220  ~ ~ ~  1416 -68 2006  6 -239 1355  970 170 2047 -546 -159 1449  0 100
00106         * -1129 -116 -122  ~ ~ ~  2048 -102 2108  -22 -136 -28  1601 -79 460 96 -256 184  0 25
00107         *
00108         * move left legs
00109         * -1173    23   220   ~ ~ ~       6  -239  1355    1416   -68  2006    -546  -159  1449     970   170  2047   0 100
00110         * -1129   116   122   ~ ~ ~     -22  -136   -28    2048  -102  2108      96  -256   184    1601   -79   460   0 50
00111         
00112 */
00113 
00114 GT2004GetupEngine::GT2004GetupEngine(const GetupEngineInterfaces& interfaces) 
00115 : GetupEngine(interfaces),
00116 stateStartTime(SystemCall::getCurrentSystemTime()),
00117 lastGetupTime(0),
00118 state(standing),
00119 lastState(standing)
00120 {
00121   
00122 #define I jointDataInvalidValue
00123   
00124   
00125   sequence[unknown].numOfEntries = 3;
00126   sequence[unknown].loop = false;
00127   sequence[unknown].entry[0] = JointData(0,-1500000,0,I,I,I, 2000000,0,2500000, 2000000,0,2500000, 2000000,0,2500000, 2000000,0,2500000);
00128   sequence[unknown].interpolate[0] = false; 
00129   sequence[unknown].repeats[0] = 80;
00130   sequence[unknown].entry[1] = JointData(0,-1500000,0,I,I,I, 1500000,1500000,2500000, 1500000,1500000,2500000, 1500000,1500000,2500000, 1500000,1500000,2500000);
00131   sequence[unknown].interpolate[1] = false; 
00132   sequence[unknown].repeats[1] = 30;
00133   sequence[unknown].entry[2] = JointData(0,-1500000,0,I,I,I, -1600000,1500000,2500000, -1600000,1500000,2500000, -1400000,1600000,2500000, -1400000,1600000,2500000);
00134   sequence[unknown].interpolate[2] = false; 
00135   sequence[unknown].repeats[2] = 100;
00136   
00137   sequence[leftSideDown].numOfEntries = 2;
00138   sequence[leftSideDown].loop = false;
00139   sequence[leftSideDown].entry[0] = JointData(0,-1500000,0,I,I,I, 0,0,0, 2000000,0,0, 0,0,0, 2000000,0,0);
00140   sequence[leftSideDown].interpolate[0] = false; 
00141   sequence[leftSideDown].repeats[0] = 120;
00142   sequence[leftSideDown].entry[1] = JointData(0,-1500000,0,I,I,I, -1600000,1500000,2500000, -1600000,1500000,2500000, -1400000,1600000,2500000, -1400000,1600000,2500000);
00143   sequence[leftSideDown].interpolate[1] = false; 
00144   sequence[leftSideDown].repeats[1] = 100;
00145   
00146   sequence[rightSideDown].numOfEntries = 2;
00147   sequence[rightSideDown].loop = false;
00148   sequence[rightSideDown].entry[0] = JointData(0,1500000,0,I,I,I, 2000000,0,0, 0,0,0, 2000000,0,0, 0,0,0);
00149   sequence[rightSideDown].interpolate[0] = false;
00150   sequence[rightSideDown].repeats[0] = 120;
00151   sequence[rightSideDown].entry[1] = JointData(0,1500000,0,I,I,I, -1600000,1500000,2500000, -1600000,1500000,2500000, -1400000,1600000,2500000, -1400000,1600000,2500000);
00152   sequence[rightSideDown].interpolate[1] = false;
00153   sequence[rightSideDown].repeats[1] = 100;
00154   
00155   sequence[getupFromUpright].numOfEntries = 1;
00156   sequence[getupFromUpright].loop = false;
00157   sequence[getupFromUpright].entry[0] = JointData(I,I,I,I,I,I, 0,800000,2100000, 0,800000,2100000, -1200000,1000000,2300000, -1200000,1000000,2300000);
00158   sequence[getupFromUpright].interpolate[0] = false; 
00159   sequence[getupFromUpright].repeats[0] = 30;
00160   
00161   sequence[rollRight].numOfEntries = 10;
00162   sequence[rollRight].loop = false;
00163   sequence[rollRight].entry[0] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00164   sequence[rollRight].interpolate[0]=true;
00165   sequence[rollRight].repeats[0]=30;
00166   sequence[rollRight].entry[1] = JointData(-1500000, 0, 0, 0, I, I, 300000, -200000, 1300000, -200000, -200000, 300000, 0, -200000, 1700000, -400000, -200000, 800000);
00167   sequence[rollRight].interpolate[1]=true;
00168   sequence[rollRight].repeats[1]=40;
00169   sequence[rollRight].entry[2] = JointData(-1500000, 0, 0, 0, I, I, 300000, -200000, 1300000, -200000, -200000, 300000, 0, -200000, 1700000, -400000, -200000, 800000);
00170   sequence[rollRight].interpolate[2]=false;
00171   sequence[rollRight].repeats[2]=40;
00172   sequence[rollRight].entry[3] = JointData(-1500000, 0, 0, 0, I, I, 2000000, 400000, 1300000, 2000000, 400000, 1300000, 2000000, 400000, 1300000, 2000000, 400000, 1300000);
00173   sequence[rollRight].interpolate[3]=true;
00174   sequence[rollRight].repeats[3]=20;
00175   sequence[rollRight].entry[4] = JointData(0, 1500000, 0, I, I, I, 2000000, 300000, 0, -100000, 0, 0, 2000000, 300000, 0, -100000, 0, 0);
00176   sequence[rollRight].interpolate[4]=true;
00177   sequence[rollRight].repeats[4]=40;
00178   sequence[rollRight].entry[5] = JointData(-1500000, 0, 0, 0, I, I, 0, -200000, 0, 100000, 0, 0, 0, -200000, 0, 100000, 0, 0);
00179   sequence[rollRight].interpolate[5]=true;
00180   sequence[rollRight].repeats[5]=40;
00181   sequence[rollRight].entry[6] = JointData(-1500000, 0, 0, 0, I, I, 0, -200000, 0, 2000000, 0, 0, 0, -200000, 0, 2000000, 0, 0);
00182   sequence[rollRight].interpolate[6]=true;
00183   sequence[rollRight].repeats[6]=40;
00184   sequence[rollRight].entry[7] = JointData(-1500000, 0, 0, 0, I, I, 0, -200000, 0, 2000000, 400000, 0, 0, -200000, 0, 2000000, 400000, 0);
00185   sequence[rollRight].interpolate[7]=true;
00186   sequence[rollRight].repeats[7]=20;
00187   sequence[rollRight].entry[8] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00188   sequence[rollRight].interpolate[8]=true;
00189   sequence[rollRight].repeats[8]=30;
00190   sequence[rollRight].entry[9] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00191   sequence[rollRight].interpolate[9]=false;
00192   sequence[rollRight].repeats[9]=60;
00193   
00194   sequence[rollLeft].numOfEntries = 10;
00195   sequence[rollLeft].loop = false;
00196   sequence[rollLeft].entry[0] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00197   sequence[rollLeft].interpolate[0]=true;
00198   sequence[rollLeft].repeats[0]=30;
00199   sequence[rollLeft].entry[1] = JointData(-1500000, 0, 0, 0, I, I, -200000, -200000, 300000, 300000, -200000, 1300000, -400000, -200000, 800000, 0, -200000, 1700000);
00200   sequence[rollLeft].interpolate[1]=true;
00201   sequence[rollLeft].repeats[1]=40;
00202   sequence[rollLeft].entry[2] = JointData(-1500000, 0, 0, 0, I, I, -200000, -200000, 300000, 300000, -200000, 1300000, -400000, -200000, 800000, 0, -200000, 1700000);
00203   sequence[rollLeft].interpolate[2]=false;
00204   sequence[rollLeft].repeats[2]=40;
00205   sequence[rollLeft].entry[3] = JointData(-1500000, 0, 0, 0, I, I, 2000000, 400000, 1300000, 2000000, 400000, 1300000, 2000000, 400000, 1300000, 2000000, 400000, 1300000);
00206   sequence[rollLeft].interpolate[3]=true;
00207   sequence[rollLeft].repeats[3]=20;
00208   sequence[rollLeft].entry[4] = JointData(0, -1500000, 0, I, I, I, -100000, 0, 0, 2000000, 300000, 0, -100000, 0, 0, 2000000, 300000, 0);
00209   sequence[rollLeft].interpolate[4]=true;
00210   sequence[rollLeft].repeats[4]=40;
00211   sequence[rollLeft].entry[5] = JointData(-1500000, 0, 0, 0, I, I, 100000, 0, 0, 0, -200000, 0, 100000, 0, 0, 0, -200000, 0);
00212   sequence[rollLeft].interpolate[5]=true;
00213   sequence[rollLeft].repeats[5]=40;
00214   sequence[rollLeft].entry[6] = JointData(-1500000, 0, 0, 0, I, I, 2000000, 0, 0, 0, -200000, 0, 2000000, 0, 0, 0, -200000, 0);
00215   sequence[rollLeft].interpolate[6]=true;
00216   sequence[rollLeft].repeats[6]=40;
00217   sequence[rollLeft].entry[7] = JointData(-1500000, 0, 0, 0, I, I, 2000000, 400000, 0, 0, -200000, 0, 2000000, 400000, 0, 0, -200000, 0);
00218   sequence[rollLeft].interpolate[7]=true;
00219   sequence[rollLeft].repeats[7]=20;
00220   sequence[rollLeft].entry[8] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00221   sequence[rollLeft].interpolate[8]=true;
00222   sequence[rollLeft].repeats[8]=30;
00223   sequence[rollLeft].entry[9] = JointData(-1500000, 0, 0, 0, I, I, 0, 300000, 2000000, 0, 300000, 2000000, -600000, 200000, 1400000, -600000, 200000, 1400000);
00224   sequence[rollLeft].interpolate[9]=false;
00225   sequence[rollLeft].repeats[9]=60;
00226   
00227   
00228   
00229   
00230   /*---------------------movement for ERS7---------------------------------------------------------------*/
00231   
00232   //just a stand
00233   sequenceERS7[unknown].numOfEntries = 9;
00234   sequenceERS7[unknown].loop = false;
00235   
00236   sequenceERS7[unknown].entry[0] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00237   sequenceERS7[unknown].interpolate[0] = false; 
00238   sequenceERS7[unknown].repeats[0] = 60;
00239   
00240   sequenceERS7[unknown].entry[1] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00241   sequenceERS7[unknown].interpolate[1] = false; 
00242   sequenceERS7[unknown].repeats[1] = 60;
00243   
00244   //sit down
00245   sequenceERS7[unknown].entry[2] = JointData(-38000,18000,781000,I,I,I, 24000,-79000,-153000, 24000,-79000,-153000, -693000,222000,-51000, -693000,222000,-51000);
00246   sequenceERS7[unknown].interpolate[2] = false; 
00247   sequenceERS7[unknown].repeats[2] = 70;
00248   
00249   //lay down
00250   sequenceERS7[unknown].entry[3] = JointData(-38000,18000,781000,I,I,I, 997000,5000,917000, 997000,5000,917000, -1396000,318000,218000, -1396000,318000,218000);
00251   sequenceERS7[unknown].interpolate[3] = true; 
00252   sequenceERS7[unknown].repeats[3] = 50;
00253   
00254   //move up
00255   sequenceERS7[unknown].entry[4] = JointData(-38000,18000,781000,I,I,I, 1143000,85000,2066000, 1143000,85000,2066000, -1872000,313000,-104000, -1872000,313000,-104000);
00256   sequenceERS7[unknown].interpolate[4] = false;
00257   sequenceERS7[unknown].repeats[4] = 50;
00258   
00259   sequenceERS7[unknown].entry[5] = JointData(-38000,18000,781000,I,I,I, 816000,74000,2038000, 816000,74000,2038000, -1921000,307000,1552000, -1921000,307000,1552000);
00260   sequenceERS7[unknown].interpolate[5] = false; 
00261   sequenceERS7[unknown].repeats[5] = 50;
00262   
00263   sequenceERS7[unknown].entry[6] = JointData(-38000,18000,781000,I,I,I, 829000,91000,2077000, 829000,91000,2077000, -1149000,-11000,2167000, -1149000,-11000,2167000);
00264   sequenceERS7[unknown].interpolate[6] = false; 
00265   sequenceERS7[unknown].repeats[6] = 50;
00266   
00267   
00268   //back to stand
00269   sequenceERS7[unknown].entry[7] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00270   sequenceERS7[unknown].interpolate[7] = false; 
00271   sequenceERS7[unknown].repeats[7] = 60;
00272   
00273   sequenceERS7[unknown].entry[8] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00274   sequenceERS7[unknown].interpolate[8] = false; 
00275   sequenceERS7[unknown].repeats[8] = 60;
00276   
00277   
00278   /* standing */
00279   sequenceERS7[standing].numOfEntries = 2;
00280   sequenceERS7[standing].loop = false;
00281   
00282   sequenceERS7[standing].entry[0] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00283   sequenceERS7[standing].interpolate[0] = false; 
00284   sequenceERS7[standing].repeats[0] = 60;
00285   
00286   sequenceERS7[standing].entry[1] = JointData(-38000,18000,781000,I,I,I, -62000,273000,1902000, -62000,273000,1902000, -589000,216000,1385000, -589000,216000,1385000);
00287   sequenceERS7[standing].interpolate[1] = false; 
00288   sequenceERS7[standing].repeats[1] = 60;
00289   
00290   
00291   
00292   /* lying on one side */
00293   //left legs transformation
00294   sequenceERS7[leftSideDown].numOfEntries = 4;
00295   sequenceERS7[leftSideDown].loop = false;
00296   
00297   sequenceERS7[leftSideDown].entry[0] = JointData(-159000,-230000,540000,I,I,I, -5000,199000,1916000,2112000,-119000,1683000, -488000,39000,1373000, 1985000,-91000,2053000);
00298   sequenceERS7[leftSideDown].interpolate[0] = true; 
00299   sequenceERS7[leftSideDown].repeats[0] = 30;
00300   
00301   
00302   sequenceERS7[leftSideDown].entry[1] = JointData(-159000,-230000,540000,I,I,I, -5000,199000,1916000,2112000,-119000,1683000, -488000,39000,1373000, 1985000,-91000,2053000);
00303   sequenceERS7[leftSideDown].interpolate[1] = true; 
00304   sequenceERS7[leftSideDown].repeats[1] = 50;
00305   
00306   sequenceERS7[leftSideDown].entry[2] = JointData(-82000,7000,507000,I,I,I,  -28000,233000,1911000, 1498000,-142000,2120000, -464000,5000,1411000, 1222000,-125000,2113000);
00307   sequenceERS7[leftSideDown].interpolate[2] = false;
00308   sequenceERS7[leftSideDown].repeats[2] = 25;
00309   
00310   
00311   sequenceERS7[leftSideDown].entry[3] = JointData(-142000,-89000,468000,I,I,I, -28000,1571000,1905000, -1487000,-142000,2142000, 78000,1423000,2102000, -1406000,-125000,2118000);
00312   sequenceERS7[leftSideDown].interpolate[3] = false;
00313   sequenceERS7[leftSideDown].repeats[3] = 25;
00314   
00315   
00316   
00317   //right legs transformation
00318   sequenceERS7[rightSideDown].numOfEntries = 4;
00319   sequenceERS7[rightSideDown].loop = false;
00320   
00321   sequenceERS7[rightSideDown].entry[0] = JointData(-159000,-230000,540000,I,I,I, 2112000,-119000,1683000, -5000,199000,1916000, 1985000,-91000,2053000, -488000,39000,1373000);
00322   sequenceERS7[rightSideDown].interpolate[0] = true; 
00323   sequenceERS7[rightSideDown].repeats[0] = 30;
00324   
00325   
00326   sequenceERS7[rightSideDown].entry[1] = JointData(-159000,-230000,540000,I,I,I, 2112000,-119000,1683000, -5000,199000,1916000, 1985000,-91000,2053000, -488000,39000,1373000);
00327   sequenceERS7[rightSideDown].interpolate[1] = true; 
00328   sequenceERS7[rightSideDown].repeats[1] = 50;
00329   
00330   sequenceERS7[rightSideDown].entry[2] = JointData(-82000,7000,507000,I,I,I, 1498000,-142000,2120000, -28000,233000,1911000, 1222000,-125000,2113000, -464000,5000,1411000);
00331   sequenceERS7[rightSideDown].interpolate[2] = false;
00332   sequenceERS7[rightSideDown].repeats[2] = 25;
00333   
00334   
00335   sequenceERS7[rightSideDown].entry[3] = JointData(-142000,-89000,468000,I,I,I, -1487000,-142000,2142000, -28000,1571000,1905000, -1406000,-125000,2118000, 78000,1423000,2102000);
00336   sequenceERS7[rightSideDown].interpolate[3] = false;
00337   sequenceERS7[rightSideDown].repeats[3] = 25;
00338   
00339   
00340   /*  // make a roll
00341   
00342     //to back
00343     sequenceERS7[rollLeft].numOfEntries = 2;
00344     sequenceERS7[rollLeft].loop = false;
00345     
00346       sequenceERS7[rollLeft].entry[0] = JointData(-1346000,1564000,371000,0,I,I, -112000,1623000,114000, -5000,1623000,-219000, -54000,1520000,221000, 0,1457000,362000);
00347       sequenceERS7[rollLeft].interpolate[0] = false; 
00348       sequenceERS7[rollLeft].repeats[0] = 50;
00349       
00350         //legs up
00351         sequenceERS7[rollLeft].entry[1] = JointData(-1000000,438000,364000,0,I,I, 0,-182000,91000, -84000,-182000,184000, -73000,250000,184000, 24000,-250000,338000);
00352         sequenceERS7[rollLeft].interpolate[1] = false; 
00353         sequenceERS7[rollLeft].repeats[1] = 60;
00354         
00355           
00356             //to back
00357             sequenceERS7[rollRight].numOfEntries = 2;
00358             sequenceERS7[rollRight].loop = false;
00359             
00360               sequenceERS7[rollRight].entry[0] = JointData(-1346000,1564000,371000,0,I,I, -112000,1623000,114000, -5000,1623000,-219000, -54000,1520000,221000, 0,1457000,362000);
00361               sequenceERS7[rollRight].interpolate[0] = false; 
00362               sequenceERS7[rollRight].repeats[0] = 50;
00363               
00364                 //legs up
00365                 sequenceERS7[rollRight].entry[1] = JointData(-1000000,438000,364000,0,I,I, 0,-182000,91000, -84000,-182000,184000, -73000,250000,184000, 24000,-250000,338000);
00366                 sequenceERS7[rollRight].interpolate[1] = false; 
00367                 sequenceERS7[rollRight].repeats[1] = 60;
00368   */
00369   
00370   // make a roll
00371   
00372   //for rollLeft
00373   //to back
00374   /*  sequenceERS7[rollLeft].numOfEntries = 2;
00375   sequenceERS7[rollLeft].loop = false;
00376   
00377     sequenceERS7[rollLeft].entry[0] = JointData(-1173000,23000,220000,0,I,I, 6000,-239000, 1355000,1416000,-68000, 2006000,-546000, -159000, 1449000,970000, 170000,2047000);
00378     sequenceERS7[rollLeft].interpolate[0] = false; 
00379     sequenceERS7[rollLeft].repeats[0] = 100;
00380     
00381       //legs up
00382       sequenceERS7[rollLeft].entry[1] = JointData(-1129000,116000,122000,0,I,I, -22000,-136000, -28000,2048000,-102000, 2108000,960000,-256000,184000, 1601000,-79000,460000);
00383       sequenceERS7[rollLeft].interpolate[1] = false; 
00384       sequenceERS7[rollLeft].repeats[1] = 50;
00385       
00386         
00387           //for rollRight
00388           //to back
00389           sequenceERS7[rollRight].numOfEntries = 2;
00390           sequenceERS7[rollRight].loop = false;
00391           
00392             sequenceERS7[rollRight].entry[0] = JointData(-1173000,-23000,-220000,0,I,I, 1416000,-68000, 2006000, 6000,-239000, 1355000,970000, 170000,2047000,-546000, -159000, 1449000);
00393             sequenceERS7[rollRight].interpolate[0] = false; 
00394             sequenceERS7[rollRight].repeats[0] = 100;
00395             
00396               //legs up
00397               sequenceERS7[rollRight].entry[1] = JointData(-1000000,438000,364000,0,I,I, 2048000,-102000, 2108000,-22000,-136000, -28000, 1601000,-79000,460000,960000,-256000,184000);
00398               sequenceERS7[rollRight].interpolate[1] = false; 
00399               sequenceERS7[rollRight].repeats[1] = 50;
00400               
00401                 
00402   */  
00403   
00404   
00405   /*
00406   //headstand to stand 
00407   
00408     //move legs
00409     sequenceERS7[makeRoll7].numOfEntries = 2;
00410     sequenceERS7[makeRoll7].loop = false;
00411     
00412       sequenceERS7[makeRoll7].entry[0] = JointData(16000,-70000,872000,I,I,I, -73000,244000,2125000, -164000,324000,2044000, -97000,273000,184000, 72000,153000,239000);
00413       sequenceERS7[makeRoll7].interpolate[0] = true; 
00414       sequenceERS7[makeRoll7].repeats[0] = 50;
00415       
00416         //head press
00417         sequenceERS7[makeRoll7].entry[1] = JointData(-1000000,438000,364000,I,I,I, 0,-182000,91000, -84000,-182000,184000, -73000,250000,184000, 24000,-250000,338000);
00418         sequenceERS7[makeRoll7].interpolate[1] = true; 
00419         sequenceERS7[makeRoll7].repeats[1] = 70;
00420         
00421   */
00422 #undef I
00423   
00424   countTry=0;
00425 }
00426 
00427 bool GT2004GetupEngine::executeParameterized(JointData& jointData)
00428 {
00429   const RobotConfiguration& r = getRobotConfiguration();
00430   
00431   // do nothing if no valid acceleration values are set
00432   if (
00433     sensorDataBuffer.numOfFrames == 0 ||
00434     (sensorDataBuffer.lastFrame().data[SensorData::accelerationX]==0 &&
00435     sensorDataBuffer.lastFrame().data[SensorData::accelerationY]==0 && 
00436     sensorDataBuffer.lastFrame().data[SensorData::accelerationZ]==0))
00437     return true;
00438   
00439   if (r.getRobotDesign()==RobotDesign::ERS210)
00440   {
00441     
00442     motionInfo.neckHeight = 160;
00443     motionInfo.motionIsStable= (state==standing);
00444     motionInfo.executedMotionRequest.motionType = MotionRequest::getup; 
00445     
00446     
00447     if (lastMotionType == MotionRequest::getup && sequencer.isRunning())
00448     {
00449       sequencer.nextSequenceData(jointData);
00450       lastGetupTime=SystemCall::getCurrentSystemTime();
00451       return true;
00452     }
00453     
00454     if (bodyPosture.bodyRollCalculatedFromAccelerationSensors <= -gt2004GetupEngineFalldownRollAngle)
00455     {
00456       // left side down
00457       state = leftSideDown;
00458       
00459     }
00460     else if (bodyPosture.bodyRollCalculatedFromAccelerationSensors >= gt2004GetupEngineFalldownRollAngle)
00461     {
00462       // right side down
00463       state = rightSideDown;
00464       
00465     }
00466     else if (bodyPosture.bodyTiltCalculatedFromAccelerationSensors > gt2004GetupEngineFalldownTiltAngle || bodyPosture.bodyTiltCalculatedFromAccelerationSensors < -gt2004GetupEngineFalldownTiltAngle)
00467     { 
00468       // rolled by x axis
00469       state = unknown;
00470     }
00471     else if (bodyPosture.bodyRollCalculatedFromAccelerationSensors >= gt2004GetupEngineRollOnWallAngle)
00472       
00473     {
00474       
00475       state=rollRight;
00476       
00477     }
00478     
00479     else if (bodyPosture.bodyRollCalculatedFromAccelerationSensors <= -gt2004GetupEngineRollOnWallAngle)
00480     {
00481       
00482       state=rollLeft;
00483       
00484     }
00485     else if (state == getupFromUpright || state == standing)
00486     {
00487       state = standing;
00488       return false;
00489     }
00490     else
00491     {
00492       state = getupFromUpright;
00493     }
00494     
00495     if (state!=lastState)
00496     {
00497       stateStartTime=SystemCall::getCurrentSystemTime();
00498     }
00499     
00500     if ((SystemCall::getTimeSince(lastGetupTime)<200)&&
00501       (SystemCall::getTimeSince(stateStartTime)>3000))
00502     {
00503       if (state==rightSideDown)
00504       {
00505         
00506         state=rollRight;
00507         
00508       }
00509       else if (state==leftSideDown)
00510       {
00511         
00512         state=rollLeft;
00513         
00514       }
00515     }
00516     
00517   }
00518   else
00519   {
00520     
00521     //-------------------begin for ERS7---------------------
00522     
00523     
00524     motionInfo.neckHeight = 160;
00525     motionInfo.motionIsStable = (state==standing);
00526     motionInfo.executedMotionRequest.motionType = MotionRequest::getup; 
00527     
00528     
00529     if (lastMotionType == MotionRequest::getup && sequencer.isRunning())
00530     {
00531       sequencer.nextSequenceData(jointData);
00532       lastGetupTime=SystemCall::getCurrentSystemTime();
00533       return true;
00534     }
00535     
00536     
00537     if ((bodyPosture.bodyRollCalculatedFromAccelerationSensors >= gt2004GetupEngineRollOnWallAngle7)&&
00538       (countTry>2)&&
00539       (state == rightSideDown))
00540     {
00541       //  OUTPUT(idText,text,"ERS7: rollRight (Angle)");
00542       state=leftSideDown;
00543       countTry=0;
00544       
00545     }
00546     else if ((bodyPosture.bodyRollCalculatedFromAccelerationSensors <= -gt2004GetupEngineRollOnWallAngle7)&&
00547       (countTry>2)&&
00548       (state == leftSideDown))
00549     {
00550       //  OUTPUT(idText,text,"ERS7: rollLeft (Angle)");
00551       state = rightSideDown;
00552       countTry = 0;
00553     }
00554         else if ((bodyPosture.bodyRollCalculatedFromAccelerationSensors <= -gt2004GetupEngineFalldownRollAngle7)&&
00555           (countTry < 3))
00556         {
00557           // left side down
00558           //  OUTPUT(idText,text,"ERS7: leftSideDown");
00559           state = leftSideDown;
00560           countTry++;
00561         }
00562         else if (bodyPosture.bodyRollCalculatedFromAccelerationSensors >= gt2004GetupEngineFalldownRollAngle7)
00563         {
00564           // right side down
00565           //  OUTPUT(idText,text,"ERS7: rightSideDown");
00566           state = rightSideDown;
00567           countTry++;
00568         }
00569         else if ((bodyPosture.bodyRollCalculatedFromAccelerationSensors > gt2004GetupEngineFalldownTiltAngle7 || bodyPosture.bodyTiltCalculatedFromAccelerationSensors < gt2004GetupEngineFalldownTiltAngleBack7)&&
00570           (countTry<3))
00571         { 
00572           // rolled by x axis
00573           //  OUTPUT(idText,text,"ERS7: unknown");
00574           state = unknown;
00575           countTry++;
00576         }
00577         else if (state == unknown || state == standing || state == leftSideDown || state == rightSideDown)
00578         {
00579           state = standing;
00580           countTry = 0;
00581           return false;
00582         }
00583         
00584         /*  else if (state == getupFromUpright || state == standing)
00585         {
00586         state = standing;
00587         return false;
00588         }
00589         
00590           if (state!=lastState)
00591           {
00592           stateStartTime=SystemCall::getCurrentSystemTime();
00593           }
00594           
00595             
00596               if ((SystemCall::getTimeSince(lastGetupTime)<1000)&&
00597               (SystemCall::getTimeSince(lastGetupTime)>600)&&
00598               (SystemCall::getTimeSince(stateStartTime)>4000))
00599               {
00600               if (state==rightSideDown)
00601               {
00602               OUTPUT(idText,text,"ERS7: rollRight (GetUpTime)");
00603               // state=rollRight;
00604               //  state = leftSideDown;
00605               //  state = leftSideDown;
00606               
00607                 }
00608                 else if (state==leftSideDown)
00609                 {
00610                 OUTPUT(idText,text,"ERS7: rollLeft (GetUpTime)");
00611                 //  state=rollLeft;
00612                 //  state = rightSideDown;
00613                 //  state = rightSideDown;
00614                 
00615                   }
00616                   }
00617                   
00618                     
00619         */    
00620         //-------------------end for ERS7-----------------------
00621   }
00622   
00623   
00624   
00625   
00626   
00627   
00628   
00629   
00630   
00631   
00632   lastGetupTime=SystemCall::getCurrentSystemTime();
00633   lastState=state;
00634   
00635   if (r.getRobotDesign()==RobotDesign::ERS210)
00636   {
00637     //  OUTPUT(idText,text,"ERS210");
00638     sequencer = sequence[state];
00639     sequencer.start();
00640     sequencer.nextSequenceData(jointData);
00641     
00642     
00643   }
00644   else
00645   {
00646     //  OUTPUT(idText,text,"ERS7");
00647     sequencer = sequenceERS7[state];
00648     sequencer.start();
00649     sequencer.nextSequenceData(jointData);
00650   }
00651   return true;
00652 }
00653 
00654 
00655 
00656 /*
00657 * Change log :
00658 * 
00659 * $Log: GT2004GetupEngine.cpp,v $
00660 * Revision 1.1  2004/07/10 00:18:29  spranger
00661 * renamed (readded) for coderelease
00662 *
00663 * Revision 1.3  2004/05/27 09:13:04  loetzsch
00664 * removed executedMotionRequest from Interfaces
00665 *
00666 * Revision 1.2  2004/05/26 16:10:24  dueffert
00667 * better data types used
00668 *
00669 * Revision 1.1.1.1  2004/05/22 17:19:10  cvsadm
00670 * created new repository GT2004_WM
00671 *
00672 * Revision 1.18  2004/04/09 11:35:51  roefer
00673 * Bremen Byters German Open check-in
00674 *
00675 * Revision 1.17  2004/04/08 15:33:05  wachter
00676 * GT04 checkin of Microsoft-Hellounds
00677 *
00678 * Revision 1.17  2004/03/29 13:18:49  pg_joha
00679 * renamend new BycycleKick and added a mirrored variant of it
00680 *
00681 * Revision 1.16  2004/03/19 11:25:30  juengel
00682 * Bug fixed.
00683 *
00684 * Revision 1.15  2004/03/17 10:06:51  dueffert
00685 * shut mouth while rolling
00686 *
00687 * Revision 1.14  2004/03/08 01:38:51  roefer
00688 * Interfaces should be const
00689 *
00690 * Revision 1.13  2004/03/03 18:35:15  roefer
00691 * Acceleration sensors magically work again
00692 *
00693 * Revision 1.12  2004/02/29 18:32:07  heinze
00694 * changed some comments
00695 *
00696 * Revision 1.11  2004/02/29 14:57:37  roefer
00697 * Removed static variable, initialized state, added support for robot with broken acceleration sensors
00698 *
00699 * Revision 1.10  2004/02/27 20:32:07  brueckne
00700 * comment by heinze: getup-engine works for all common cases, specials aren't included yet
00701 *
00702 * Revision 1.9  2004/02/27 19:19:29  heinze
00703 * took out absurd behavior
00704 *
00705 * Revision 1.8  2004/02/27 18:01:10  heinze
00706 * still improving the engine for ERS7
00707 *
00708 * Revision 1.7  2004/02/27 17:32:27  heinze
00709 * improved getup-behavior for ERS7
00710 *
00711 * Revision 1.5  2004/02/26 11:47:27  heinze
00712 * bug removed
00713 *
00714 * Revision 1.4  2004/02/26 10:56:35  heinze
00715 * worked at the getUp-Engine for ERS7
00716 *
00717 * Revision 1.3  2004/02/26 09:44:23  heinze
00718 * took out some OUTPUTS
00719 *
00720 * Revision 1.2  2004/02/25 16:03:50  heinze
00721 * added/began getup-motion for ERS7
00722 *
00723 * Revision 1.1  2003/10/06 14:10:14  cvsadm
00724 * Created GT2004 (M.J.)
00725 *
00726 * Revision 1.2  2003/07/06 09:31:02  risler
00727 * wait state added at end of roll, preventing double roll
00728 *
00729 * Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
00730 * created new repository for the competitions in Padova from the 
00731 * tamara CVS (Tuesday 2:00 pm)
00732 *
00733 * removed unused solutions
00734 *
00735 * Revision 1.9  2003/06/20 15:32:32  dueffert
00736 * getting from down from wall by rolling added
00737 *
00738 * Revision 1.8  2003/06/19 15:53:40  dueffert
00739 * rolling if stuck added, beautified
00740 *
00741 * Revision 1.7  2003/06/18 18:12:11  dueffert
00742 * getup stuck recognition added
00743 *
00744 * Revision 1.6  2003/05/02 18:26:18  risler
00745 * SensorDataBuffer added
00746 * replaced SensorData with SensorDataBuffer
00747 * full SensorData resolution now accessible
00748 *
00749 * Revision 1.5  2003/04/06 19:41:01  dueffert
00750 * a little slower but better getup
00751 *
00752 * Revision 1.4  2003/03/06 12:05:43  dueffert
00753 * execute with parameters renamed to avoid inheritance warnings
00754 *
00755 * Revision 1.3  2002/09/17 23:55:20  loetzsch
00756 * - unraveled several datatypes
00757 * - changed the WATCH macro
00758 * - completed the process restructuring
00759 *
00760 * Revision 1.2  2002/09/11 00:06:58  loetzsch
00761 * continued change of module/solution mechanisms
00762 *
00763 * Revision 1.1  2002/09/10 15:36:14  cvsadm
00764 * Created new project GT2003 (M.L.)
00765 * - Cleaned up the /Src/DataTypes directory
00766 * - Removed challenge related source code
00767 * - Removed processing of incoming audio data
00768 * - Renamed AcousticMessage to SoundRequest
00769 *
00770 * Revision 1.1.1.1  2002/05/10 12:40:14  cvsadm
00771 * Moved GT2002 Project from ute to tamara.
00772 *
00773 * Revision 1.14  2002/05/06 16:03:28  risler
00774 * corrected bodyRoll angle
00775 *
00776 * Revision 1.13  2002/05/05 15:14:23  risler
00777 * changed stand implementation
00778 *
00779 * Revision 1.12  2002/05/04 18:23:43  risler
00780 * added calculation of executedMotionRequest
00781 *
00782 * Revision 1.11  2002/04/23 10:38:31  risler
00783 * renamed headOdometry to headState
00784 *
00785 * Revision 1.10  2002/04/17 17:04:41  risler
00786 * Darmstadt GO
00787 *
00788 * Revision 1.9  2002/04/08 17:49:24  risler
00789 * removed body tilt from odometry
00790 *
00791 * Revision 1.8  2002/04/02 14:01:39  dueffert
00792 * minor odometry enhancements
00793 *
00794 * Revision 1.7  2002/04/02 13:10:19  dueffert
00795 * big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
00796 *
00797 * Revision 1.6  2002/03/27 15:54:28  risler
00798 * added neckHeight and stableMotion in motion modules
00799 *
00800 * Revision 1.5  2002/02/26 20:55:58  risler
00801 * getup on startup after first sensordata received
00802 *
00803 * Revision 1.4  2002/02/26 20:54:20  risler
00804 * getup on startup after first sensordata received
00805 *
00806 * Revision 1.3  2002/02/23 17:07:22  risler
00807 * code cleaned up
00808 *
00809 * Revision 1.2  2002/02/23 16:33:07  risler
00810 * finished GetupEngine
00811 *
00812 * Revision 1.1  2002/02/20 16:40:50  risler
00813 * added GetupEngine
00814 *
00815 *
00816 */