00001
00002
00003
00004 #include <cassert>
00005 #include "newPGstepStudy.h"
00006
00007 #define DELAY_1 0.005
00008 #define DELAY_2 0.2
00009
00010 using namespace std;
00011
00012 CnewPGstepStudy::CnewPGstepStudy()
00013 {
00014
00015 }
00016
00017 CnewPGstepStudy::~CnewPGstepStudy()
00018 {
00019
00020 }
00021
00022 void CnewPGstepStudy::drawSeqStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable, double coefFeet)
00023 {
00024
00025 double downBound, upBound, leftBound, rightBound;
00026
00027 StepFeatures stepF;
00028 produceSeqStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, t4, t5, vect_input, leftOrRightFootStable);
00029
00030 double centre_x;
00031 double centre_y;
00032 double abs_orientation;
00033
00034 for(int i=0; i < (int) (vect_input.size()-6)/4+2 ; i++)
00035 {
00036
00037 double abs_orientationRAD;
00038
00039 if(i==0) {
00040 centre_x = vect_input[0];
00041 centre_y = vect_input[1];
00042 abs_orientation = vect_input[2] * PI/180;
00043
00044 leftBound = -centre_y;
00045 rightBound = -centre_y;
00046 upBound = centre_x;
00047 downBound = centre_x;
00048 }
00049 else if(i==1) {
00050 centre_x = vect_input[3];
00051 centre_y = vect_input[4];
00052 abs_orientationRAD = vect_input[5] * PI/180;
00053 }
00054 else if(i==2) {
00055 centre_x = vect_input[0];
00056 centre_y = vect_input[1];
00057 abs_orientation = vect_input[2];
00058 }
00059
00060 if(i>=2){
00061
00062 abs_orientationRAD = abs_orientation * PI/180;
00063
00064 centre_x = centre_x + cos(abs_orientationRAD)*vect_input[4*i-1]-sin(abs_orientationRAD)*vect_input[4*i];
00065 centre_y = centre_y + sin(abs_orientationRAD)*vect_input[4*i-1]+cos(abs_orientationRAD)*vect_input[4*i];
00066 abs_orientation = abs_orientation + vect_input[4*i+1];
00067
00068 abs_orientationRAD = abs_orientation * PI/180;
00069
00070 }
00071
00072 leftBound = min(leftBound,-centre_y-0.24*coefFeet);
00073 rightBound = max(rightBound,-centre_y+0.24*coefFeet);
00074 downBound = min(downBound,centre_x-0.24*coefFeet);
00075 upBound = max(upBound,centre_x+0.24*coefFeet);
00076
00077 vector<double> cosinuss (4, 0);
00078 vector<double> sinuss (4, 0);
00079 vector<double> x (4, 0);
00080 vector<double> y (4, 0);
00081
00082 cosinuss[0] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*0.065;
00083 sinuss[0] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*0.115;
00084
00085 cosinuss[1] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*0.065;
00086 sinuss[1] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*(-0.115);
00087
00088 cosinuss[2] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*(-0.065);
00089 sinuss[2] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*(-0.115);
00090
00091 cosinuss[3] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*(-0.065);
00092 sinuss[3] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*0.115;
00093
00094 for(int j = 0; j<4; j++)
00095 {
00096 x[j] = centre_x + cosinuss[j]*coefFeet;
00097 y[j] = centre_y + sinuss[j]*coefFeet;
00098 }
00099
00100 for(int j = 0; j<4; j++)
00101 {
00102 fb << -y[j]
00103 << " " << x[j]
00104 << " " << -y[(j+1) % 4] + y[j]
00105 << " " << x[(j+1) % 4] - x[j]
00106 << endl;
00107 }
00108 fb << -y[0]
00109 << " " << +x[0]
00110 << " " << -y[0]
00111 << " " << +x[0]
00112 << endl << endl;
00113
00114 }
00115
00116 double squareCenter_h = (rightBound + leftBound)/2;
00117 double squareCenter_v = (upBound + downBound)/2;
00118 double halfSide = max((rightBound - leftBound)/2,(upBound - downBound)/2);
00119
00120 leftBound = squareCenter_h - halfSide;
00121 rightBound = squareCenter_h + halfSide;
00122 downBound = squareCenter_v - halfSide;
00123 upBound =squareCenter_v + halfSide;
00124
00125
00126 fb << leftBound
00127 << " " << downBound
00128 << " " << leftBound
00129 << " " << upBound
00130 << endl;
00131 fb << leftBound
00132 << " " << upBound
00133 << " " << rightBound
00134 << " " << upBound
00135 << endl;
00136 fb << rightBound
00137 << " " << upBound
00138 << " " << rightBound
00139 << " " << downBound
00140 << endl;
00141 fb << rightBound
00142 << " " << downBound
00143 << " " << leftBound
00144 << " " << downBound
00145 << endl;
00146 fb << leftBound
00147 << " " << downBound
00148 << " " << leftBound
00149 << " " << downBound
00150 << endl << endl;
00151
00152 fb << endl;
00153
00154
00155 for(int i=0; i < stepF.size-1 ; i++)
00156 {
00157 fb << -stepF.rightfootYtraj[i]
00158 << " " << stepF.rightfootXtraj[i]
00159 << " " << -stepF.rightfootYtraj[i+1]
00160 << " " << stepF.rightfootXtraj[i+1]
00161 << endl;
00162 fb << -stepF.rightfootYtraj[i+1]
00163 << " " << stepF.rightfootXtraj[i+1]
00164 << " " << -stepF.rightfootYtraj[i+1]
00165 << " " << stepF.rightfootXtraj[i+1]
00166 << endl << endl;
00167 }
00168 fb << endl;
00169
00170
00171 for(int i=0; i < stepF.size-1 ; i++)
00172 {
00173 fb << -stepF.leftfootYtraj[i]
00174 << " " << stepF.leftfootXtraj[i]
00175 << " " << -stepF.leftfootYtraj[i+1]
00176 << " " << stepF.leftfootXtraj[i+1]
00177 << endl;
00178 fb << -stepF.leftfootYtraj[i+1]
00179 << " " << stepF.leftfootXtraj[i+1]
00180 << " " << -stepF.leftfootYtraj[i+1]
00181 << " " << stepF.leftfootXtraj[i+1]
00182 << endl << endl;
00183 }
00184 fb << endl;
00185
00186
00187 for(int i=0; i < stepF.size-1 ; i++)
00188 {
00189 fb << -stepF.comTrajY[i]
00190 << " " << stepF.comTrajX[i]
00191 << " " << -stepF.comTrajY[i+1]
00192 << " " << stepF.comTrajX[i+1]
00193 << endl;
00194 fb << -stepF.comTrajY[i+1]
00195 << " " << stepF.comTrajX[i+1]
00196 << " " << -stepF.comTrajY[i+1]
00197 << " " << stepF.comTrajX[i+1]
00198 << endl << endl;
00199 }
00200 fb << endl;
00201
00202
00203 for(int i=0; i < stepF.size-1 ; i++)
00204 {
00205 fb << -stepF.zmpTrajY[i]
00206 << " " << stepF.zmpTrajX[i]
00207 << " " << -stepF.zmpTrajY[i+1]
00208 << " " << stepF.zmpTrajX[i+1]
00209 << endl;
00210 fb << -stepF.zmpTrajY[i+1]
00211 << " " << stepF.zmpTrajX[i+1]
00212 << " " << -stepF.zmpTrajY[i+1]
00213 << " " << stepF.zmpTrajX[i+1]
00214 << endl << endl;
00215 }
00216 fb << endl;
00217
00218
00219 ofstream fb_com("com.dat");
00220 ofstream fb_zmp("zmp.dat");
00221 ofstream fb_la("left-ankle.dat");
00222 ofstream fb_ra("right-ankle.dat");
00223
00224 for(int i=0; i < stepF.size-1 ; i++)
00225 {
00226 fb_com << stepF.comTrajX[i]
00227 << " " << stepF.comTrajY[i] << endl;
00228
00229 fb_zmp << stepF.zmpTrajX[i]
00230 << " " << stepF.zmpTrajY[i] << endl;
00231
00232 fb_la << stepF.leftfootXtraj[i]
00233 << " " << stepF.leftfootYtraj[i]
00234 << " " << stepF.leftfootHeight[i]
00235 << " " << stepF.leftfootOrient[i] << endl;
00236
00237 fb_ra << stepF.rightfootXtraj[i]
00238 << " " << stepF.rightfootYtraj[i]
00239 << " " << stepF.rightfootHeight[i]
00240 << " " << stepF.rightfootOrient[i] << endl;
00241 }
00242 }
00243
00244 void CnewPGstepStudy::drawSeqHalfStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable, double coefFeet)
00245 {
00246
00247 StepFeatures stepF;
00248 produceSeqHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00249
00250 double downBound, upBound, leftBound, rightBound;
00251
00252 double centre_x;
00253 double centre_y;
00254 double abs_orientation;
00255
00256 for(int i=0; i < (int) (vect_input.size()-6)/5+2 ; i++)
00257 {
00258
00259 double abs_orientationRAD;
00260
00261 if(i==0) {
00262 centre_x = vect_input[0];
00263 centre_y = vect_input[1];
00264 abs_orientation = vect_input[2] * PI/180;
00265
00266 leftBound = -centre_y;
00267 rightBound = -centre_y;
00268 upBound = centre_x;
00269 downBound = centre_x;
00270 }
00271 else if(i==1) {
00272 centre_x = vect_input[3];
00273 centre_y = vect_input[4];
00274 abs_orientationRAD = vect_input[5] * PI/180;
00275 }
00276 else if(i==2) {
00277 centre_x = vect_input[0];
00278 centre_y = vect_input[1];
00279 abs_orientation = vect_input[2];
00280 }
00281
00282 if(i>=2){
00283
00284 abs_orientationRAD = abs_orientation * PI/180;
00285
00286 centre_x = centre_x + cos(abs_orientationRAD)*vect_input[5*i-2]-sin(abs_orientationRAD)*vect_input[5*i-1];
00287 centre_y = centre_y + sin(abs_orientationRAD)*vect_input[5*i-2]+cos(abs_orientationRAD)*vect_input[5*i-1];
00288 abs_orientation = abs_orientation + vect_input[5*i];
00289
00290 abs_orientationRAD = abs_orientation * PI/180;
00291
00292 }
00293
00294 leftBound = min(leftBound,-centre_y-0.24*coefFeet);
00295 rightBound = max(rightBound,-centre_y+0.24*coefFeet);
00296 downBound = min(downBound,centre_x-0.24*coefFeet);
00297 upBound = max(upBound,centre_x+0.24*coefFeet);
00298
00299 vector<double> cosinuss (4, 0);
00300 vector<double> sinuss (4, 0);
00301 vector<double> x (4, 0);
00302 vector<double> y (4, 0);
00303
00304 cosinuss[0] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*0.065;
00305 sinuss[0] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*0.115;
00306
00307 cosinuss[1] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*0.065;
00308 sinuss[1] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*(-0.115);
00309
00310 cosinuss[2] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*(-0.065);
00311 sinuss[2] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*(-0.115);
00312
00313 cosinuss[3] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*(-0.065);
00314 sinuss[3] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*0.115;
00315
00316 for(int j = 0; j<4; j++)
00317 {
00318 x[j] = centre_x + cosinuss[j]*coefFeet;
00319 y[j] = centre_y + sinuss[j]*coefFeet;
00320 }
00321
00322 for(int j = 0; j<4; j++)
00323 {
00324 fb << -y[j]
00325 << " " << x[j]
00326 << " " << -y[(j+1) % 4] + y[j]
00327 << " " << x[(j+1) % 4] - x[j]
00328 << endl;
00329 }
00330 fb << -y[0]
00331 << " " << +x[0]
00332 << " " << -y[0]
00333 << " " << +x[0]
00334 << endl << endl;
00335 }
00336
00337 double squareCenter_h = (rightBound + leftBound)/2;
00338 double squareCenter_v = (upBound + downBound)/2;
00339 double halfSide = max((rightBound - leftBound)/2,(upBound - downBound)/2);
00340
00341 leftBound = squareCenter_h - halfSide;
00342 rightBound = squareCenter_h + halfSide;
00343 downBound = squareCenter_v - halfSide;
00344 upBound =squareCenter_v + halfSide;
00345
00346
00347 fb << leftBound
00348 << " " << downBound
00349 << " " << leftBound
00350 << " " << upBound
00351 << endl;
00352 fb << leftBound
00353 << " " << upBound
00354 << " " << rightBound
00355 << " " << upBound
00356 << endl;
00357 fb << rightBound
00358 << " " << upBound
00359 << " " << rightBound
00360 << " " << downBound
00361 << endl;
00362 fb << rightBound
00363 << " " << downBound
00364 << " " << leftBound
00365 << " " << downBound
00366 << endl;
00367 fb << leftBound
00368 << " " << downBound
00369 << " " << leftBound
00370 << " " << downBound
00371 << endl << endl;
00372
00373 fb << endl;
00374
00375
00376 for(int i=0; i < stepF.size-1 ; i++)
00377 {
00378 fb << -stepF.rightfootYtraj[i]
00379 << " " << stepF.rightfootXtraj[i]
00380 << " " << -stepF.rightfootYtraj[i+1]
00381 << " " << stepF.rightfootXtraj[i+1]
00382 << endl;
00383 fb << -stepF.rightfootYtraj[i+1]
00384 << " " << stepF.rightfootXtraj[i+1]
00385 << " " << -stepF.rightfootYtraj[i+1]
00386 << " " << stepF.rightfootXtraj[i+1]
00387 << endl << endl;
00388 }
00389 fb << endl;
00390
00391
00392 for(int i=0; i < stepF.size-1 ; i++)
00393 {
00394 fb << -stepF.leftfootYtraj[i]
00395 << " " << stepF.leftfootXtraj[i]
00396 << " " << -stepF.leftfootYtraj[i+1]
00397 << " " << stepF.leftfootXtraj[i+1]
00398 << endl;
00399 fb << -stepF.leftfootYtraj[i+1]
00400 << " " << stepF.leftfootXtraj[i+1]
00401 << " " << -stepF.leftfootYtraj[i+1]
00402 << " " << stepF.leftfootXtraj[i+1]
00403 << endl << endl;
00404 }
00405 fb << endl;
00406
00407
00408 for(int i=0; i < stepF.size-1 ; i++)
00409 {
00410 fb << -stepF.comTrajY[i]
00411 << " " << stepF.comTrajX[i]
00412 << " " << -stepF.comTrajY[i+1]
00413 << " " << stepF.comTrajX[i+1]
00414 << endl;
00415 fb << -stepF.comTrajY[i+1]
00416 << " " << stepF.comTrajX[i+1]
00417 << " " << -stepF.comTrajY[i+1]
00418 << " " << stepF.comTrajX[i+1]
00419 << endl << endl;
00420 }
00421 fb << endl;
00422
00423
00424 for(int i=0; i < stepF.size-1 ; i++)
00425 {
00426 fb << -stepF.zmpTrajY[i]
00427 << " " << stepF.zmpTrajX[i]
00428 << " " << -stepF.zmpTrajY[i+1]
00429 << " " << stepF.zmpTrajX[i+1]
00430 << endl;
00431 fb << -stepF.zmpTrajY[i+1]
00432 << " " << stepF.zmpTrajX[i+1]
00433 << " " << -stepF.zmpTrajY[i+1]
00434 << " " << stepF.zmpTrajX[i+1]
00435 << endl << endl;
00436 }
00437 fb << endl;
00438
00439 ofstream fb_com("com.dat");
00440 ofstream fb_zmp("zmp.dat");
00441 ofstream fb_la("left-ankle.dat");
00442 ofstream fb_ra("right-ankle.dat");
00443
00444 for(int i=0; i < stepF.size-1 ; i++)
00445 {
00446 fb_com << stepF.comTrajX[i]
00447 << " " << stepF.comTrajY[i] << endl;
00448
00449 fb_zmp << stepF.zmpTrajX[i]
00450 << " " << stepF.zmpTrajY[i] << endl;
00451
00452 fb_la << stepF.leftfootXtraj[i]
00453 << " " << stepF.leftfootYtraj[i]
00454 << " " << stepF.leftfootHeight[i]
00455 << " " << stepF.leftfootOrient[i] << endl;
00456
00457 fb_ra << stepF.rightfootXtraj[i]
00458 << " " << stepF.rightfootYtraj[i]
00459 << " " << stepF.rightfootHeight[i]
00460 << " " << stepF.rightfootOrient[i] << endl;
00461 }
00462 }
00463
00464 void CnewPGstepStudy::drawSeqSlidedHalfStepFeatures(ofstream & fb, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable, double coefFeet)
00465 {
00466
00467 StepFeatures stepF;
00468 produceSeqSlidedHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00469
00470 double downBound, upBound, leftBound, rightBound;
00471
00472 double centre_x;
00473 double centre_y;
00474 double abs_orientation;
00475
00476 for(int i=0; i < (int) (vect_input.size()-6)/7+2 ; i++)
00477 {
00478
00479 double abs_orientationRAD;
00480
00481 if(i==0) {
00482 centre_x = vect_input[0];
00483 centre_y = vect_input[1];
00484 abs_orientation = vect_input[2] * PI/180;
00485
00486 leftBound = -centre_y;
00487 rightBound = -centre_y;
00488 upBound = centre_x;
00489 downBound = centre_x;
00490 }
00491 else if(i==1) {
00492 centre_x = vect_input[3];
00493 centre_y = vect_input[4];
00494 abs_orientationRAD = vect_input[5] * PI/180;
00495 }
00496 else if(i==2) {
00497 centre_x = vect_input[0];
00498 centre_y = vect_input[1];
00499 abs_orientation = vect_input[2];
00500 }
00501
00502 if(i>=2){
00503
00504 abs_orientationRAD = abs_orientation * PI/180;
00505
00506 centre_x = centre_x + cos(abs_orientationRAD)*vect_input[7*i-4]-sin(abs_orientationRAD)*vect_input[7*i-3];
00507 centre_y = centre_y + sin(abs_orientationRAD)*vect_input[7*i-4]+cos(abs_orientationRAD)*vect_input[7*i-3];
00508 abs_orientation = abs_orientation + vect_input[7*i-2];
00509
00510 abs_orientationRAD = abs_orientation * PI/180;
00511
00512 }
00513
00514 leftBound = min(leftBound,-centre_y-0.24*coefFeet);
00515 rightBound = max(rightBound,-centre_y+0.24*coefFeet);
00516 downBound = min(downBound,centre_x-0.24*coefFeet);
00517 upBound = max(upBound,centre_x+0.24*coefFeet);
00518
00519 vector<double> cosinuss (4, 0);
00520 vector<double> sinuss (4, 0);
00521 vector<double> x (4, 0);
00522 vector<double> y (4, 0);
00523
00524 cosinuss[0] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*0.065;
00525 sinuss[0] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*0.115;
00526
00527 cosinuss[1] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*0.065;
00528 sinuss[1] = cos(abs_orientationRAD)*0.065 + sin(abs_orientationRAD)*(-0.115);
00529
00530 cosinuss[2] = cos(abs_orientationRAD)*(-0.115) - sin(abs_orientationRAD)*(-0.065);
00531 sinuss[2] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*(-0.115);
00532
00533 cosinuss[3] = cos(abs_orientationRAD)*0.115 - sin(abs_orientationRAD)*(-0.065);
00534 sinuss[3] = cos(abs_orientationRAD)*(-0.065) + sin(abs_orientationRAD)*0.115;
00535
00536 for(int j = 0; j<4; j++)
00537 {
00538 x[j] = centre_x + cosinuss[j]*coefFeet;
00539 y[j] = centre_y + sinuss[j]*coefFeet;
00540 }
00541
00542 for(int j = 0; j<4; j++)
00543 {
00544 fb << -y[j]
00545 << " " << x[j]
00546 << " " << -y[(j+1) % 4] + y[j]
00547 << " " << x[(j+1) % 4] - x[j]
00548 << endl;
00549 }
00550 fb << -y[0]
00551 << " " << +x[0]
00552 << " " << -y[0]
00553 << " " << +x[0]
00554 << endl << endl;
00555 }
00556
00557 double squareCenter_h = (rightBound + leftBound)/2;
00558 double squareCenter_v = (upBound + downBound)/2;
00559 double halfSide = max((rightBound - leftBound)/2,(upBound - downBound)/2);
00560
00561 leftBound = squareCenter_h - halfSide;
00562 rightBound = squareCenter_h + halfSide;
00563 downBound = squareCenter_v - halfSide;
00564 upBound =squareCenter_v + halfSide;
00565
00566
00567 fb << leftBound
00568 << " " << downBound
00569 << " " << leftBound
00570 << " " << upBound
00571 << endl;
00572 fb << leftBound
00573 << " " << upBound
00574 << " " << rightBound
00575 << " " << upBound
00576 << endl;
00577 fb << rightBound
00578 << " " << upBound
00579 << " " << rightBound
00580 << " " << downBound
00581 << endl;
00582 fb << rightBound
00583 << " " << downBound
00584 << " " << leftBound
00585 << " " << downBound
00586 << endl;
00587 fb << leftBound
00588 << " " << downBound
00589 << " " << leftBound
00590 << " " << downBound
00591 << endl << endl;
00592
00593 fb << endl;
00594
00595
00596 for(int i=0; i < stepF.size-1 ; i++)
00597 {
00598 fb << -stepF.rightfootYtraj[i]
00599 << " " << stepF.rightfootXtraj[i]
00600 << " " << -stepF.rightfootYtraj[i+1]
00601 << " " << stepF.rightfootXtraj[i+1]
00602 << endl;
00603 fb << -stepF.rightfootYtraj[i+1]
00604 << " " << stepF.rightfootXtraj[i+1]
00605 << " " << -stepF.rightfootYtraj[i+1]
00606 << " " << stepF.rightfootXtraj[i+1]
00607 << endl << endl;
00608 }
00609 fb << endl;
00610
00611
00612 for(int i=0; i < stepF.size-1 ; i++)
00613 {
00614 fb << -stepF.leftfootYtraj[i]
00615 << " " << stepF.leftfootXtraj[i]
00616 << " " << -stepF.leftfootYtraj[i+1]
00617 << " " << stepF.leftfootXtraj[i+1]
00618 << endl;
00619 fb << -stepF.leftfootYtraj[i+1]
00620 << " " << stepF.leftfootXtraj[i+1]
00621 << " " << -stepF.leftfootYtraj[i+1]
00622 << " " << stepF.leftfootXtraj[i+1]
00623 << endl << endl;
00624 }
00625 fb << endl;
00626
00627
00628 for(int i=0; i < stepF.size-1 ; i++)
00629 {
00630 fb << -stepF.comTrajY[i]
00631 << " " << stepF.comTrajX[i]
00632 << " " << -stepF.comTrajY[i+1]
00633 << " " << stepF.comTrajX[i+1]
00634 << endl;
00635 fb << -stepF.comTrajY[i+1]
00636 << " " << stepF.comTrajX[i+1]
00637 << " " << -stepF.comTrajY[i+1]
00638 << " " << stepF.comTrajX[i+1]
00639 << endl << endl;
00640 }
00641 fb << endl;
00642
00643
00644 for(int i=0; i < stepF.size-1 ; i++)
00645 {
00646 fb << -stepF.zmpTrajY[i]
00647 << " " << stepF.zmpTrajX[i]
00648 << " " << -stepF.zmpTrajY[i+1]
00649 << " " << stepF.zmpTrajX[i+1]
00650 << endl;
00651 fb << -stepF.zmpTrajY[i+1]
00652 << " " << stepF.zmpTrajX[i+1]
00653 << " " << -stepF.zmpTrajY[i+1]
00654 << " " << stepF.zmpTrajX[i+1]
00655 << endl << endl;
00656 }
00657 fb << endl;
00658
00659 ofstream fb_com("com.dat");
00660 ofstream fb_zmp("zmp.dat");
00661 ofstream fb_la("left-ankle.dat");
00662 ofstream fb_ra("right-ankle.dat");
00663
00664 for(int i=0; i < stepF.size-1 ; i++)
00665 {
00666 fb_com << stepF.comTrajX[i]
00667 << " " << stepF.comTrajY[i] << endl;
00668
00669 fb_zmp << stepF.zmpTrajX[i]
00670 << " " << stepF.zmpTrajY[i] << endl;
00671
00672 fb_la << stepF.leftfootXtraj[i]
00673 << " " << stepF.leftfootYtraj[i]
00674 << " " << stepF.leftfootHeight[i]
00675 << " " << stepF.leftfootOrient[i] << endl;
00676
00677 fb_ra << stepF.rightfootXtraj[i]
00678 << " " << stepF.rightfootYtraj[i]
00679 << " " << stepF.rightfootHeight[i]
00680 << " " << stepF.rightfootOrient[i] << endl;
00681 }
00682 }
00683
00684 void CnewPGstepStudy::plotOneDimensionCOMZMPSeqStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable) {
00685
00686 StepFeatures stepF;
00687 produceSeqStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, t4, t5, vect_input, leftOrRightFootStable);
00688
00689 if(whichDimension=='x') {
00690
00691 for(int i=0; i < stepF.size-1 ; i++)
00692 {
00693 fb << (double) i*incrTime << " " << stepF.comTrajX[i] << endl;
00694 }
00695 fb << endl << endl;
00696
00697
00698 for(int i=0; i < stepF.size-1 ; i++)
00699 {
00700 fb << (double) i*incrTime << " " << stepF.zmpTrajX[i] << endl;
00701 }
00702 fb << endl << endl;
00703
00704
00705 }
00706 else if(whichDimension=='y') {
00707
00708 for(int i=0; i < stepF.size-1 ; i++)
00709 {
00710 fb << (double) i*incrTime << " " << stepF.comTrajY[i] << endl;
00711 }
00712 fb << endl << endl;
00713
00714
00715 for(int i=0; i < stepF.size-1 ; i++)
00716 {
00717 fb << (double) i*incrTime << " " << stepF.zmpTrajY[i] << endl;
00718 }
00719 fb << endl << endl;
00720
00721
00722 }
00723 }
00724
00725 void CnewPGstepStudy::plotFootHeightSeqStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vect_input, char leftOrRightFootStable) {
00726
00727 StepFeatures stepF;
00728 produceSeqStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, t4, t5, vect_input, leftOrRightFootStable);
00729
00730 if(whichDimension=='L') {
00731
00732 for(int i=0; i < stepF.size-1 ; i++)
00733 {
00734 fb << (double) i*incrTime << " " << stepF.leftfootHeight[i] << endl;
00735 }
00736 fb << endl << endl;
00737
00738 }
00739 else if(whichDimension=='R') {
00740
00741 for(int i=0; i < stepF.size-1 ; i++)
00742 {
00743 fb << (double) i*incrTime << " " << stepF.rightfootHeight[i] << endl;
00744 }
00745 fb << endl << endl;
00746
00747 }
00748 }
00749
00750 void CnewPGstepStudy::plotOneDimensionCOMZMPSeqHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable){
00751
00752 StepFeatures stepF;
00753 produceSeqHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00754
00755 if(whichDimension=='x') {
00756
00757 for(int i=0; i < stepF.size-1 ; i++)
00758 {
00759 fb << (double) i*incrTime << " " << stepF.comTrajX[i] << endl;
00760 }
00761 fb << endl << endl;
00762
00763
00764 for(int i=0; i < stepF.size-1 ; i++)
00765 {
00766 fb << (double) i*incrTime << " " << stepF.zmpTrajX[i] << endl;
00767 }
00768 fb << endl << endl;
00769
00770
00771 }
00772 else if(whichDimension=='y') {
00773
00774 for(int i=0; i < stepF.size-1 ; i++)
00775 {
00776 fb << (double) i*incrTime << " " << stepF.comTrajY[i] << endl;
00777 }
00778 fb << endl << endl;
00779
00780
00781 for(int i=0; i < stepF.size-1 ; i++)
00782 {
00783 fb << (double) i*incrTime << " " << stepF.zmpTrajY[i] << endl;
00784 }
00785 fb << endl << endl;
00786
00787
00788 }
00789 }
00790
00791 void CnewPGstepStudy::plotFootHeightSeqHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable){
00792
00793 StepFeatures stepF;
00794 produceSeqHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00795
00796 if(whichDimension=='L') {
00797
00798 for(int i=0; i < stepF.size-1 ; i++)
00799 {
00800 fb << (double) i*incrTime << " " << stepF.leftfootHeight[i] << endl;
00801 }
00802 fb << endl << endl;
00803
00804 }
00805 else if(whichDimension=='R') {
00806
00807 for(int i=0; i < stepF.size-1 ; i++)
00808 {
00809 fb << (double) i*incrTime << " " << stepF.rightfootHeight[i] << endl;
00810 }
00811 fb << endl << endl;
00812
00813 }
00814 }
00815
00816 void CnewPGstepStudy::plotOneDimensionCOMZMPSeqSlidedHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable){
00817
00818 StepFeatures stepF;
00819 produceSeqSlidedHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00820
00821 if(whichDimension=='x') {
00822
00823 for(int i=0; i < stepF.size-1 ; i++)
00824 {
00825 fb << (double) i*incrTime << " " << stepF.comTrajX[i] << endl;
00826 }
00827 fb << endl << endl;
00828
00829
00830 for(int i=0; i < stepF.size-1 ; i++)
00831 {
00832 fb << (double) i*incrTime << " " << stepF.zmpTrajX[i] << endl;
00833 }
00834 fb << endl << endl;
00835
00836
00837 }
00838 else if(whichDimension=='y') {
00839
00840 for(int i=0; i < stepF.size-1 ; i++)
00841 {
00842 fb << (double) i*incrTime << " " << stepF.comTrajY[i] << endl;
00843 }
00844 fb << endl << endl;
00845
00846
00847 for(int i=0; i < stepF.size-1 ; i++)
00848 {
00849 fb << (double) i*incrTime << " " << stepF.zmpTrajY[i] << endl;
00850 }
00851 fb << endl << endl;
00852
00853
00854 }
00855 }
00856
00857 void CnewPGstepStudy::plotFootHeightSeqSlidedHalfStep(ofstream & fb, char whichDimension, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vect_input, char leftOrRightFootStable){
00858
00859 StepFeatures stepF;
00860 produceSeqSlidedHalfStepFeatures(stepF, incrTime, zc, g, t1, t2, t3, vect_input, leftOrRightFootStable);
00861
00862 if(whichDimension=='L') {
00863
00864 for(int i=0; i < stepF.size-1 ; i++)
00865 {
00866 fb << (double) i*incrTime << " " << stepF.leftfootHeight[i] << endl;
00867 }
00868 fb << endl << endl;
00869
00870 }
00871 else if(whichDimension=='R') {
00872
00873 for(int i=0; i < stepF.size-1 ; i++)
00874 {
00875 fb << (double) i*incrTime << " " << stepF.rightfootHeight[i] << endl;
00876 }
00877 fb << endl << endl;
00878
00879 }
00880 }
00881
00882
00883
00884
00885 double w (double t, double g, double zc, double delta0, double deltaX, double t1, double t2, double V, double W)
00886 {
00887
00888 return(delta0+(V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc/pow(t2-t1,2.0)/g)*cosh(sqrt(g/zc)*(t-t1))+(V*sinh(sqrt(g/zc)*t1)*sqrt(g/zc
00889 )+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*sinh(
00890 sqrt(g/zc)*(t-t1))/sqrt(g/zc)-2.0*deltaX*pow(t-t1,3.0)/pow(t2-t1,3.0)+3.0*
00891 deltaX*pow(t-t1,2.0)/pow(t2-t1,2.0)-12.0*deltaX*zc*(t-t1)/pow(t2-t1,3.0
00892 )/g+6.0*deltaX*zc/pow(t2-t1,2.0)/g);
00893
00894 };
00895
00896 double w2 (double t, double g, double zc, double deltaX2, double t2, double t3, double t4, double K2, double V2, double W2)
00897 {
00898
00899 return(K2+(V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*
00900 deltaX2*zc/pow(t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t-t3))+(V2*sinh(sqrt(g/zc)*(
00901 t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2*zc/
00902 pow(t4-t3,3.0)/g)*sinh(sqrt(g/zc)*(t-t3))/sqrt(g/zc)-2.0*deltaX2*pow(t-t3,
00903 3.0)/pow(t4-t3,3.0)+3.0*deltaX2*pow(t-t3,2.0)/pow(t4-t3,2.0)-12.0*deltaX2
00904 *zc*(t-t3)/pow(t4-t3,3.0)/g+6.0*deltaX2*zc/pow(t4-t3,2.0)/g);
00905
00906 };
00907
00908 double u (double t, double g, double zc, double t2, double K2, double V2, double W2)
00909 {
00910
00911 return(V2*cosh(sqrt(g/zc)*(t-t2))+W2*sinh(sqrt(g/zc)*(t-t2))+K2);
00912
00913 };
00914
00915 double u2 (double t, double g, double zc, double t4, double K3, double V3, double W3)
00916 {
00917
00918 return(V3*cosh(sqrt(g/zc)*(t-t4))+W3*sinh(sqrt(g/zc)*(t-t4))+K3);
00919
00920 };
00921
00922 double hZMP (double t, double g, double zc, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double V, double W, double K2, double V2, double W2, double K3, double V3, double W3)
00923 {
00924
00925 if(t <= t1)
00926 {
00927 return delta0;
00928 }
00929 else if(t <= t2)
00930 {
00931 return (delta0*t1*t1*t1-delta0*t2*t2*t2+2.0*deltaX*t*t*t+deltaX*t1*t1*t1-3.0*deltaX*t*t*t1-3.0*deltaX*t*t*t2+6.0*deltaX*t*t1*t2
00932 -3.0*delta0*t1*t1*t2+3.0*delta0*t1*t2*t2-3.0*deltaX*t1*t1*t2)/pow(t1-
00933 t2,3.0);
00934 }
00935 else if(t <= t3)
00936 {
00937 return K2;
00938 }
00939 else if(t <= t4)
00940 {
00941 return (-K2*t4*t4*t4+K2*t3*t3*t3+2.0*deltaX2*t*t*t+deltaX2*t3*t3*t3
00942 -3.0*deltaX2*t*t*t3-3.0*deltaX2*t*t*t4+6.0*deltaX2*t*t3*t4-3.0*
00943 deltaX2*t4*t3*t3+3.0*K2*t4*t4*t3-3.0*K2*t4*t3*t3)/pow(-t4+t3,3.0);
00944 }
00945 else
00946 {
00947 return K3;
00948 }
00949
00950 };
00951
00952 double h (double t, double g, double zc, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double V, double W, double K2, double V2, double W2, double K3, double V3, double W3)
00953 {
00954
00955 if(t <= t1)
00956 {
00957 return V*cosh(sqrt(g/zc)*t)+W*sinh(sqrt(g/zc)*t)+delta0;
00958 }
00959 else if(t <= t2)
00960 {
00961 return w(t, g, zc, delta0, deltaX, t1, t2, V, W);
00962 }
00963 else if(t <= t3)
00964 {
00965 return u(t, g, zc, t2, K2, V2, W2);
00966 }
00967 else if(t <= t4)
00968 {
00969 return w2(t, g, zc, deltaX2, t2, t3, t4, K2, V2, W2);
00970 }
00971 else
00972 {
00973 return u2(t, g, zc, t4, K3, V3, W3);
00974 }
00975
00976 };
00977
00978 vector<double> hVinit (double t, double g, double zc, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double t5, double pinit, double vinit)
00979 {
00980
00981 vector<double> PairToReturn;
00982 double V = pinit-delta0;
00983 double W = vinit/sqrt(g/zc);
00984 double K2 = delta0+(V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0
00985 *deltaX*zc/pow(t2-t1,2.0)/g)*cosh(sqrt(g/zc)*(t2-t1))+(V*sinh(sqrt(g/zc)*t1
00986 )*sqrt(g/zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)
00987 /g)*sinh(sqrt(g/zc)*(t2-t1))/sqrt(g/zc)+deltaX-6.0*deltaX*zc/pow(t2-t1,2.0)/g-zc/g*((V*
00988 cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc/pow(t2-t1,2.0)/g)*
00989 cosh(sqrt(g/zc)*(t2-t1))*g/zc+(V*sinh(sqrt(g/zc)*t1)*sqrt(g/zc)+W*cosh(sqrt(g/
00990 zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*sinh(sqrt(g/zc)*(t2-t1)
00991 )*sqrt(g/zc)-6.0*deltaX/pow(t2-t1,2.0));
00992 double V2 = zc/g*((V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc
00993 /pow(t2-t1,2.0)/g)*cosh(sqrt(g/zc)*(t2-t1))*g/zc+(V*sinh(sqrt(g/zc)*t1)*sqrt(g/
00994 zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*sinh(
00995 sqrt(g/zc)*(t2-t1))*sqrt(g/zc)-6.0*deltaX/pow(t2-t1,2.0));
00996 double W2 = ((V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc/pow(
00997 t2-t1,2.0)/g)*sinh(sqrt(g/zc)*(t2-t1))*sqrt(g/zc)+(V*sinh(sqrt(g/zc)*t1)*sqrt(g
00998 /zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*cosh
00999 (sqrt(g/zc)*(t2-t1))-12.0*deltaX*zc/pow(t2-t1,3.0)/g)/sqrt(g/zc);
01000 double K3 = K2+(V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2)
01001 )-6.0*deltaX2*zc/pow(t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))+(V2*sinh(sqrt(g
01002 /zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01003 *zc/pow(t4-t3,3.0)/g)*sinh(sqrt(g/zc)*(t4-t3))/sqrt(g/zc)+deltaX2-6.0*deltaX2*zc/pow(t4-t3,2.0)/g-zc/g*((
01004 V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*deltaX2*zc/pow(
01005 t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))*g/zc+(V2*sinh(sqrt(g/zc)*(t3-t2))*sqrt(g
01006 /zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2*zc/pow(t4-t3,3.0)/
01007 g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)-6.0*deltaX2/pow(t4-t3,2.0));
01008 double V3 = zc/g*((V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*
01009 deltaX2*zc/pow(t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))*g/zc+(V2*sinh(sqrt(g/
01010 zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01011 *zc/pow(t4-t3,3.0)/g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)-6.0*deltaX2/pow(
01012 t4-t3,2.0));
01013 double W3 = ((V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*deltaX2*
01014 zc/pow(t4-t3,2.0)/g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)+(V2*sinh(sqrt(g/
01015 zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01016 *zc/pow(t4-t3,3.0)/g)*cosh(sqrt(g/zc)*(t4-t3))-12.0*deltaX2*zc/pow(t4-t3,
01017 3.0)/g)/sqrt(g/zc);
01018 PairToReturn.push_back(h(t, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, V, W, K2, V2, W2, K3, V3, W3));
01019 PairToReturn.push_back(hZMP(t, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, V, W, K2, V2, W2, K3, V3, W3));
01020 return PairToReturn;
01021 };
01022
01023 double hVinitCOMonly (double t, double g, double zc, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double t5, double pinit, double vinit)
01024 {
01025
01026 double V = pinit-delta0;
01027 double W = vinit/sqrt(g/zc);
01028 double K2 = delta0+(V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0
01029 *deltaX*zc/pow(t2-t1,2.0)/g)*cosh(sqrt(g/zc)*(t2-t1))+(V*sinh(sqrt(g/zc)*t1
01030 )*sqrt(g/zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)
01031 /g)*sinh(sqrt(g/zc)*(t2-t1))/sqrt(g/zc)+deltaX-6.0*deltaX*zc/pow(t2-t1,2.0)/g-zc/g*((V*
01032 cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc/pow(t2-t1,2.0)/g)*
01033 cosh(sqrt(g/zc)*(t2-t1))*g/zc+(V*sinh(sqrt(g/zc)*t1)*sqrt(g/zc)+W*cosh(sqrt(g/
01034 zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*sinh(sqrt(g/zc)*(t2-t1)
01035 )*sqrt(g/zc)-6.0*deltaX/pow(t2-t1,2.0));
01036 double V2 = zc/g*((V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc
01037 /pow(t2-t1,2.0)/g)*cosh(sqrt(g/zc)*(t2-t1))*g/zc+(V*sinh(sqrt(g/zc)*t1)*sqrt(g/
01038 zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*sinh(
01039 sqrt(g/zc)*(t2-t1))*sqrt(g/zc)-6.0*deltaX/pow(t2-t1,2.0));
01040 double W2 = ((V*cosh(sqrt(g/zc)*t1)+W*sinh(sqrt(g/zc)*t1)-6.0*deltaX*zc/pow(
01041 t2-t1,2.0)/g)*sinh(sqrt(g/zc)*(t2-t1))*sqrt(g/zc)+(V*sinh(sqrt(g/zc)*t1)*sqrt(g
01042 /zc)+W*cosh(sqrt(g/zc)*t1)*sqrt(g/zc)+12.0*deltaX*zc/pow(t2-t1,3.0)/g)*cosh
01043 (sqrt(g/zc)*(t2-t1))-12.0*deltaX*zc/pow(t2-t1,3.0)/g)/sqrt(g/zc);
01044 double K3 = K2+(V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2)
01045 )-6.0*deltaX2*zc/pow(t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))+(V2*sinh(sqrt(g
01046 /zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01047 *zc/pow(t4-t3,3.0)/g)*sinh(sqrt(g/zc)*(t4-t3))/sqrt(g/zc)+deltaX2-6.0*deltaX2*zc/pow(t4-t3,2.0)/g-zc/g*((
01048 V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*deltaX2*zc/pow(
01049 t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))*g/zc+(V2*sinh(sqrt(g/zc)*(t3-t2))*sqrt(g
01050 /zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2*zc/pow(t4-t3,3.0)/
01051 g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)-6.0*deltaX2/pow(t4-t3,2.0));
01052 double V3 = zc/g*((V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*
01053 deltaX2*zc/pow(t4-t3,2.0)/g)*cosh(sqrt(g/zc)*(t4-t3))*g/zc+(V2*sinh(sqrt(g/
01054 zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01055 *zc/pow(t4-t3,3.0)/g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)-6.0*deltaX2/pow(
01056 t4-t3,2.0));
01057 double W3 = ((V2*cosh(sqrt(g/zc)*(t3-t2))+W2*sinh(sqrt(g/zc)*(t3-t2))-6.0*deltaX2*
01058 zc/pow(t4-t3,2.0)/g)*sinh(sqrt(g/zc)*(t4-t3))*sqrt(g/zc)+(V2*sinh(sqrt(g/
01059 zc)*(t3-t2))*sqrt(g/zc)+W2*cosh(sqrt(g/zc)*(t3-t2))*sqrt(g/zc)+12.0*deltaX2
01060 *zc/pow(t4-t3,3.0)/g)*cosh(sqrt(g/zc)*(t4-t3))-12.0*deltaX2*zc/pow(t4-t3,
01061 3.0)/g)/sqrt(g/zc);
01062 return h(t, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, V, W, K2, V2, W2, K3, V3, W3);
01063 };
01064
01065 double searchVinit (double g, double zc, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double t5, double pinit)
01066 {
01067
01068 double vinitBmin = -10.0;
01069
01070 double vinitBmax = 10.0;
01071
01072 if (
01073 hVinitCOMonly(t5, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, t5, pinit, vinitBmin) >= delta0 + deltaX + deltaX2
01074 ||
01075 hVinitCOMonly(t5, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, t5, pinit, vinitBmax) <= delta0 + deltaX + deltaX2
01076 ) return -999;
01077
01078 while (vinitBmax - vinitBmin > 0.00000001)
01079 {
01080
01081 if (hVinitCOMonly(t5, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, t5, pinit, (vinitBmax + vinitBmin) / 2) > delta0 + deltaX + deltaX2)
01082 {
01083 vinitBmax = (vinitBmax + vinitBmin) / 2;
01084 }
01085 else
01086 {
01087 vinitBmin = (vinitBmax + vinitBmin) / 2;
01088 }
01089
01090 }
01091
01092 return (vinitBmax + vinitBmin) / 2;
01093
01094 }
01095
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111 void CnewPGstepStudy::genCOMZMPtrajectory(vector<double> & outputCOM, vector<double> & outputZMP, double incrTime, double zc, double g, double delta0, double deltaX, double deltaX2, double t1, double t2, double t3, double t4, double t5)
01112 {
01113
01114 double sensitivityLimit = 0.00001;
01115
01116 outputCOM.clear();
01117 outputZMP.clear();
01118
01119 double vinit = searchVinit(g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, t5, 0);
01120
01121 if(abs(deltaX) < sensitivityLimit && abs(deltaX2) < sensitivityLimit) {
01122 for(double i = 0.0 ; i < t5 ; i += incrTime)
01123 {
01124 outputCOM.push_back(delta0);
01125 outputZMP.push_back(delta0);
01126 }
01127 }
01128 else {
01129
01130 int count = 0;
01131 int countSav = 0;
01132
01133 double valPrev = 99999999;
01134 double valTmp;
01135 for(double i = 0.0 ; i < t5 ; i += incrTime)
01136 {
01137
01138
01139 vector<double> ComZmp = hVinit(i, g, zc, delta0, deltaX, deltaX2, t1, t2, t3, t4, t5, 0, vinit);
01140
01141 outputCOM.push_back(ComZmp[0]);
01142 outputZMP.push_back(ComZmp[1]);
01143
01144 valTmp = abs(ComZmp[0] - delta0 - deltaX - deltaX2);
01145 if(valTmp < valPrev) {
01146 countSav = count;
01147 }
01148 valPrev = valTmp;
01149 count++;
01150 }
01151
01152
01153 if(countSav != outputCOM.size()-1) {
01154 for(unsigned int i = countSav ; i < outputCOM.size() ; i++)
01155 {
01156 outputCOM[i] = (outputCOM[countSav]*(outputCOM.size()-1-i) + (delta0 + deltaX + deltaX2)*(i-countSav))/(outputCOM.size()-1-countSav);
01157 }
01158 }
01159
01160
01161
01162 }
01163
01164 }
01165
01166
01167 void CnewPGstepStudy::genFOOTposition(vector<double> & outputX, vector<double> & outputY, double incrTime, double xinit, double yinit, double xend, double yend, double delay, double t1, double t2, double t3, double t4, double t5, char du)
01168 {
01169
01170 if(du == '2') {
01171
01172 outputX.clear();
01173 outputY.clear();
01174
01175 for(double i = 0.0 ; i < t5 ; i += incrTime)
01176 {
01177
01178 if(i < t2+delay)
01179 {
01180
01181 outputX.push_back(xinit);
01182 outputY.push_back(yinit);
01183
01184 }
01185 else if(i < t3-delay)
01186 {
01187
01188 outputX.push_back(xinit + (-2/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0)+3/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0))*(xend-xinit));
01189 outputY.push_back(yinit + (-2/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0)+3/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0))*(yend-yinit));
01190
01191 }
01192 else
01193 {
01194
01195 outputX.push_back(xend);
01196 outputY.push_back(yend);
01197
01198 }
01199
01200 }
01201
01202 }
01203
01204 if(du == 'd') {
01205
01206 outputX.clear();
01207 outputY.clear();
01208
01209 for(double i = 0.0 ; i < t5 ; i += incrTime)
01210 {
01211
01212 if(i < t2)
01213 {
01214
01215 outputX.push_back(xinit);
01216 outputY.push_back(yinit);
01217
01218 }
01219 else if(i < t3-delay)
01220 {
01221
01222 outputX.push_back(xinit + (-2/pow(t3-t2-delay,3.0)*pow(i-t2,3.0)+3/pow(t3-t2-delay,2.0)*pow(i-t2,2.0))*(xend-xinit));
01223 outputY.push_back(yinit + (-2/pow(t3-t2-delay,3.0)*pow(i-t2,3.0)+3/pow(t3-t2-delay,2.0)*pow(i-t2,2.0))*(yend-yinit));
01224
01225 }
01226 else
01227 {
01228
01229 outputX.push_back(xend);
01230 outputY.push_back(yend);
01231
01232 }
01233
01234 }
01235
01236 }
01237
01238 if(du == 'u') {
01239
01240 outputX.clear();
01241 outputY.clear();
01242
01243 for(double i = 0.0 ; i < t5 ; i += incrTime)
01244 {
01245
01246 if(i < t2+delay)
01247 {
01248
01249 outputX.push_back(xinit);
01250 outputY.push_back(yinit);
01251
01252 }
01253 else if(i < t3)
01254 {
01255
01256 outputX.push_back(xinit + (-2/pow(t3-t2-delay,3.0)*pow(i-t2-delay,3.0)+3/pow(t3-t2-delay,2.0)*pow(i-t2-delay,2.0))*(xend-xinit));
01257 outputY.push_back(yinit + (-2/pow(t3-t2-delay,3.0)*pow(i-t2-delay,3.0)+3/pow(t3-t2-delay,2.0)*pow(i-t2-delay,2.0))*(yend-yinit));
01258
01259 }
01260 else
01261 {
01262
01263 outputX.push_back(xend);
01264 outputY.push_back(yend);
01265
01266 }
01267
01268 }
01269
01270 }
01271
01272 }
01273
01274 void CnewPGstepStudy::genFOOTheight(vector<double> & output, double incrTime, double heightMax, double delay, double t1, double t2, double t3, double t4, double t5)
01275 {
01276
01277 output.clear();
01278
01279 for(double i = 0.0 ; i < t5 ; i += incrTime)
01280 {
01281
01282 if(i < t2+delay)
01283 {
01284
01285 output.push_back(0);
01286
01287 }
01288 else if(i < t3-delay)
01289 {
01290
01291 output.push_back( 16*heightMax/pow(t3-t2-2*delay,4.0)*pow(i-t2-delay,4.0) - 32*heightMax/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0) + 16*heightMax/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0));
01292
01293 }
01294 else
01295 {
01296
01297 output.push_back(0);
01298
01299 }
01300
01301 }
01302
01303 }
01304
01305 void CnewPGstepStudy::genFOOTdownUPheight(vector<double> & output, double incrTime, double heightMax, double delay, double t1, double t2, double t3)
01306 {
01307
01308 output.clear();
01309
01310 for(double i = 0.0 ; i < t3 ; i += incrTime)
01311 {
01312
01313 if(i < t2+delay)
01314 {
01315
01316 output.push_back(0);
01317
01318 }
01319 else if(i < t3-delay)
01320 {
01321
01322 output.push_back( -2*heightMax/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0) + 3*heightMax/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0));
01323
01324 }
01325 else
01326 {
01327
01328 output.push_back(heightMax);
01329
01330 }
01331
01332 }
01333
01334 }
01335
01336 void CnewPGstepStudy::genFOOTupDOWNheight(vector<double> & output, double incrTime, double heightMax, double delay, double t1, double t2, double t3)
01337 {
01338
01339 output.clear();
01340
01341 for(double i = 0.0 ; i < t3 ; i += incrTime)
01342 {
01343 if(i < delay) {
01344 output.push_back(heightMax);
01345 } else if(i < t1-delay)
01346 {
01347
01348 output.push_back( -2*heightMax/pow(delay-t1+delay,3.0)*pow(i-t1+delay,3.0) + 3*heightMax/pow(delay-t1+delay,2.0)*pow(i-t1+delay,2.0));
01349
01350 }
01351 else
01352 {
01353
01354 output.push_back(0);
01355
01356 }
01357
01358 }
01359
01360 }
01361
01362 void CnewPGstepStudy::genFOOTorientation(vector<double> & output, double incrTime, double initOrient, double endOrient, double delay, double t1, double t2, double t3, double t4, double t5, char du)
01363 {
01364
01365 if(du == '2') {
01366
01367 output.clear();
01368
01369 for(double i = 0.0 ; i < t5 ; i += incrTime)
01370 {
01371
01372 if(i < t2+delay)
01373 {
01374
01375 output.push_back(initOrient);
01376
01377 }
01378 else if(i < t3-delay)
01379 {
01380
01381 output.push_back( initOrient + (-2/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0) + 3/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0))*(endOrient - initOrient) );
01382
01383 }
01384 else
01385 {
01386
01387 output.push_back(endOrient);
01388
01389 }
01390
01391 }
01392
01393 }
01394
01395 if(du == 'd') {
01396
01397 output.clear();
01398
01399 for(double i = 0.0 ; i < t5 ; i += incrTime)
01400 {
01401
01402 if(i < t2)
01403 {
01404
01405 output.push_back(initOrient);
01406
01407 }
01408 else if(i < t3-delay)
01409 {
01410
01411 output.push_back( initOrient + (-2/pow(t3-t2-delay,3.0)*pow(i-t2,3.0) + 3/pow(t3-t2-delay,2.0)*pow(i-t2,2.0))*(endOrient - initOrient) );
01412
01413 }
01414 else
01415 {
01416
01417 output.push_back(endOrient);
01418
01419 }
01420
01421 }
01422
01423 }
01424
01425 if(du == 'u') {
01426
01427 output.clear();
01428
01429 for(double i = 0.0 ; i < t5 ; i += incrTime)
01430 {
01431
01432 if(i < t2+delay)
01433 {
01434
01435 output.push_back(initOrient);
01436
01437 }
01438 else if(i < t3)
01439 {
01440
01441 output.push_back( initOrient + (-2/pow(t3-t2-delay,3.0)*pow(i-t2-delay,3.0) + 3/pow(t3-t2-delay,2.0)*pow(i-t2-delay,2.0))*(endOrient - initOrient) );
01442
01443 }
01444 else
01445 {
01446
01447 output.push_back(endOrient);
01448
01449 }
01450
01451 }
01452
01453 }
01454
01455 }
01456
01457
01458 void CnewPGstepStudy::genWAISTorientation(vector<double> & output, double incrTime, double initOrient, double endOrient, double delay, double t1, double t2, double t3, double t4, double t5, char du)
01459 {
01460
01461 if(du == '2') {
01462
01463 output.clear();
01464
01465 for(double i = 0.0 ; i < t5 ; i += incrTime)
01466 {
01467
01468 if(i < t2+delay)
01469 {
01470
01471 output.push_back(initOrient);
01472
01473 }
01474 else if(i < t3-delay)
01475 {
01476
01477 output.push_back( initOrient + (-2/pow(t3-t2-2*delay,3.0)*pow(i-t2-delay,3.0) + 3/pow(t3-t2-2*delay,2.0)*pow(i-t2-delay,2.0))*(endOrient - initOrient) );
01478
01479 }
01480 else
01481 {
01482
01483 output.push_back(endOrient);
01484
01485 }
01486
01487 }
01488
01489 }
01490 if(du == 'd') {
01491
01492 output.clear();
01493
01494 for(double i = 0.0 ; i < t5 ; i += incrTime)
01495 {
01496
01497 if(i < t2)
01498 {
01499
01500 output.push_back(initOrient);
01501
01502 }
01503 else if(i < t3-delay)
01504 {
01505
01506 output.push_back( initOrient + (-2/pow(t3-t2-delay,3.0)*pow(i-t2,3.0) + 3/pow(t3-t2-delay,2.0)*pow(i-t2,2.0))*(endOrient - initOrient) );
01507
01508 }
01509 else
01510 {
01511
01512 output.push_back(endOrient);
01513
01514 }
01515
01516 }
01517
01518 }
01519 if(du == 'u') {
01520
01521 output.clear();
01522
01523 for(double i = 0.0 ; i < t5 ; i += incrTime)
01524 {
01525
01526 if(i < t2+delay)
01527 {
01528
01529 output.push_back(initOrient);
01530
01531 }
01532 else if(i < t3)
01533 {
01534
01535 output.push_back( initOrient + (-2/pow(t3-t2-delay,3.0)*pow(i-t2-delay,3.0) + 3/pow(t3-t2-delay,2.0)*pow(i-t2-delay,2.0))*(endOrient - initOrient) );
01536
01537 }
01538 else
01539 {
01540
01541 output.push_back(endOrient);
01542
01543 }
01544
01545 }
01546
01547 }
01548
01549
01550 }
01551
01552
01553 void CnewPGstepStudy::addStepFeaturesWithSlide(
01554 StepFeatures & stepF1,
01555 StepFeatures & stepF2,
01556 double negativeSlideTime
01557 )
01558 {
01559
01560
01561 double lastwaistX = stepF1.comTrajX[stepF1.size - 1];
01562 double lastwaistY = stepF1.comTrajY[stepF1.size - 1];
01563 double radlastwaistOrient = stepF1.waistOrient[stepF1.size - 1]*PI/180;
01564
01565 for(unsigned int count = 0 ; count < stepF2.size ; count++) {
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576
01577
01578
01579
01580
01581
01582
01583
01584 double newcomX = (stepF2.comTrajX[count])*cos(radlastwaistOrient)
01585 -(stepF2.comTrajY[count])*sin(radlastwaistOrient)+lastwaistX;
01586 double newcomY = (stepF2.comTrajX[count])*sin(radlastwaistOrient)
01587 +(stepF2.comTrajY[count])*cos(radlastwaistOrient)+lastwaistY;
01588 double newzmpX = (stepF2.zmpTrajX[count])*cos(radlastwaistOrient)
01589 -(stepF2.zmpTrajY[count])*sin(radlastwaistOrient)+lastwaistX;
01590 double newzmpY = (stepF2.zmpTrajX[count])*sin(radlastwaistOrient)
01591 +(stepF2.zmpTrajY[count])*cos(radlastwaistOrient)+lastwaistY;
01592 double newlfX = (stepF2.leftfootXtraj[count])*cos(radlastwaistOrient)
01593 -(stepF2.leftfootYtraj[count])*sin(radlastwaistOrient)+lastwaistX;
01594 double newlfY = (stepF2.leftfootXtraj[count])*sin(radlastwaistOrient)
01595 +(stepF2.leftfootYtraj[count])*cos(radlastwaistOrient)+lastwaistY;
01596 double newrfX = (stepF2.rightfootXtraj[count])*cos(radlastwaistOrient)
01597 -(stepF2.rightfootYtraj[count])*sin(radlastwaistOrient)+lastwaistX;
01598 double newrfY = (stepF2.rightfootXtraj[count])*sin(radlastwaistOrient)
01599 +(stepF2.rightfootYtraj[count])*cos(radlastwaistOrient)+lastwaistY;
01600
01601 stepF2.comTrajX[count] = newcomX;
01602 stepF2.zmpTrajX[count] = newzmpX;
01603
01604 stepF2.comTrajY[count] = newcomY;
01605 stepF2.zmpTrajY[count] = newzmpY;
01606
01607 stepF2.leftfootXtraj[count] = newlfX;
01608 stepF2.leftfootYtraj[count] = newlfY;
01609
01610 stepF2.leftfootOrient[count] += stepF1.waistOrient[stepF1.size - 1];
01611
01612 stepF2.rightfootXtraj[count] = newrfX;
01613 stepF2.rightfootYtraj[count] = newrfY;
01614
01615 stepF2.rightfootOrient[count] += stepF1.waistOrient[stepF1.size - 1];
01616
01617 stepF2.waistOrient[count] += stepF1.waistOrient[stepF1.size - 1];
01618
01619 }
01620
01621 int delayInt = (int) (abs(negativeSlideTime)/stepF2.incrTime);
01622
01623
01624 for(unsigned int count = 0 ; count < stepF2.size ; count++) {
01625
01626 if(count < delayInt) {
01627
01628 stepF1.comTrajX[stepF1.size - delayInt + count] =
01629 (stepF1.comTrajX[stepF1.size - delayInt + count] + stepF2.comTrajX[count])
01630 -stepF1.comTrajX[stepF1.size - 1];
01631 stepF1.zmpTrajX[stepF1.size - delayInt + count] =
01632 (stepF1.zmpTrajX[stepF1.size - delayInt + count] + stepF2.zmpTrajX[count])
01633 -stepF1.zmpTrajX[stepF1.size - 1];
01634
01635 stepF1.comTrajY[stepF1.size - delayInt + count] =
01636 ( stepF1.comTrajY[stepF1.size - delayInt + count] + stepF2.comTrajY[count])
01637 -stepF1.comTrajY[stepF1.size - 1];
01638 stepF1.zmpTrajY[stepF1.size - delayInt + count] =
01639 ( stepF1.zmpTrajY[stepF1.size - delayInt + count] + stepF2.zmpTrajY[count])
01640 -stepF1.zmpTrajY[stepF1.size - 1];
01641
01642 stepF1.leftfootXtraj[stepF1.size - delayInt + count] =
01643 ( stepF1.leftfootXtraj[stepF1.size - delayInt + count] + stepF2.leftfootXtraj[count])
01644 -stepF1.leftfootXtraj[stepF1.size - 1];
01645 stepF1.leftfootYtraj[stepF1.size - delayInt + count] =
01646 ( stepF1.leftfootYtraj[stepF1.size - delayInt + count] + stepF2.leftfootYtraj[count])
01647 -stepF1.leftfootYtraj[stepF1.size - 1];
01648
01649 stepF1.leftfootOrient[stepF1.size - delayInt + count] =
01650 ( stepF1.leftfootOrient[stepF1.size - delayInt + count] + stepF2.leftfootOrient[count])
01651 -stepF1.leftfootOrient[stepF1.size - 1];
01652 stepF1.leftfootHeight[stepF1.size - delayInt + count] =
01653 ( stepF1.leftfootHeight[stepF1.size - delayInt + count] + stepF2.leftfootHeight[count])
01654 -stepF1.leftfootHeight[stepF1.size - 1];
01655
01656 stepF1.rightfootXtraj[stepF1.size - delayInt + count] =
01657 ( stepF1.rightfootXtraj[stepF1.size - delayInt + count] + stepF2.rightfootXtraj[count])
01658 -stepF1.rightfootXtraj[stepF1.size - 1];
01659 stepF1.rightfootYtraj[stepF1.size - delayInt + count] =
01660 ( stepF1.rightfootYtraj[stepF1.size - delayInt + count] + stepF2.rightfootYtraj[count])
01661 -stepF1.rightfootYtraj[stepF1.size - 1];
01662
01663 stepF1.rightfootOrient[stepF1.size - delayInt + count] =
01664 ( stepF1.rightfootOrient[stepF1.size - delayInt + count] + stepF2.rightfootOrient[count])
01665 -stepF1.rightfootOrient[stepF1.size - 1];
01666 stepF1.rightfootHeight[stepF1.size - delayInt + count] =
01667 ( stepF1.rightfootHeight[stepF1.size - delayInt + count] + stepF2.rightfootHeight[count])
01668 -stepF1.rightfootHeight[stepF1.size - 1];
01669
01670 stepF1.waistOrient[stepF1.size - delayInt + count] =
01671 ( stepF1.waistOrient[stepF1.size - delayInt + count] + stepF2.waistOrient[count])
01672 -stepF1.waistOrient[stepF1.size - 1];
01673
01674 } else {
01675
01676 stepF1.comTrajX.push_back(stepF2.comTrajX[count]);
01677 stepF1.zmpTrajX.push_back(stepF2.zmpTrajX[count]);
01678
01679 stepF1.comTrajY.push_back(stepF2.comTrajY[count]);
01680 stepF1.zmpTrajY.push_back(stepF2.zmpTrajY[count]);
01681
01682 stepF1.leftfootXtraj.push_back(stepF2.leftfootXtraj[count]);
01683 stepF1.leftfootYtraj.push_back(stepF2.leftfootYtraj[count]);
01684
01685 stepF1.leftfootOrient.push_back(stepF2.leftfootOrient[count]);
01686 stepF1.leftfootHeight.push_back(stepF2.leftfootHeight[count]);
01687
01688 stepF1.rightfootXtraj.push_back(stepF2.rightfootXtraj[count]);
01689 stepF1.rightfootYtraj.push_back(stepF2.rightfootYtraj[count]);
01690
01691 stepF1.rightfootOrient.push_back(stepF2.rightfootOrient[count]);
01692 stepF1.rightfootHeight.push_back(stepF2.rightfootHeight[count]);
01693
01694 stepF1.waistOrient.push_back(stepF2.waistOrient[count]);
01695
01696 }
01697
01698 }
01699
01700 stepF1.size = stepF1.size + stepF2.size - delayInt;
01701
01702 }
01703
01704
01705 void CnewPGstepStudy::produceOneStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vectStep_input, char leftOrRightFootStable)
01706 {
01707
01708 double stepHeight = vectStep_input[6];
01709
01710 vector<double> comTrajX;
01711 vector<double> zmpTrajX;
01712 genCOMZMPtrajectory(comTrajX, zmpTrajX, incrTime, zc, g, 0, vectStep_input[0], vectStep_input[7]/2, t1, t2, t3, t4, t5);
01713
01714 vector<double> comTrajY;
01715 vector<double> zmpTrajY;
01716 genCOMZMPtrajectory(comTrajY, zmpTrajY, incrTime, zc, g, 0, vectStep_input[1], vectStep_input[8]/2, t1, t2, t3, t4, t5);
01717
01718 vector<double> footXtraj;
01719 vector<double> footYtraj;
01720 genFOOTposition(footXtraj, footYtraj, incrTime, vectStep_input[3], vectStep_input[4], vectStep_input[0]+vectStep_input[7], vectStep_input[1]+vectStep_input[8], DELAY_2, t1, t2, t3, t4, t5, '2');
01721
01722 vector<double> footHeight;
01723 genFOOTheight(footHeight, incrTime, stepHeight, DELAY_1, t1, t2, t3, t4, t5);
01724
01725 vector<double> footOrient;
01726 genFOOTorientation(footOrient, incrTime, vectStep_input[5], vectStep_input[9], DELAY_2, t1, t2, t3, t4, t5, '2');
01727
01728 vector<double> stablefootXtraj;
01729 vector<double> stablefootYtraj;
01730 vector<double> stablefootHeight;
01731 vector<double> stablefootOrient;
01732
01733 int count = -1;
01734
01735 for(double i = 0.0 ; i < t5 ; i += incrTime)
01736 {
01737 count++;
01738 stablefootXtraj.push_back(vectStep_input[0]);
01739 stablefootYtraj.push_back(vectStep_input[1]);
01740 stablefootHeight.push_back(0);
01741 stablefootOrient.push_back(0);
01742 }
01743
01744 vector<double> waistOrient;
01745 genWAISTorientation(waistOrient, incrTime, 0, vectStep_input[9], DELAY_1, t1, t2, t3, t4, t5, '2');
01746
01747
01748 stepF.comTrajX = comTrajX;
01749 stepF.zmpTrajX = zmpTrajX;
01750 stepF.comTrajY = comTrajY;
01751 stepF.zmpTrajY = zmpTrajY;
01752
01753 if(leftOrRightFootStable == 'L') {
01754 stepF.leftfootXtraj = stablefootXtraj;
01755 stepF.leftfootYtraj = stablefootYtraj;
01756 stepF.leftfootHeight = stablefootHeight;
01757 stepF.leftfootOrient = stablefootOrient;
01758 stepF.rightfootXtraj = footXtraj;
01759 stepF.rightfootYtraj = footYtraj;
01760 stepF.rightfootHeight = footHeight;
01761 stepF.rightfootOrient = footOrient;
01762 } else {
01763 stepF.leftfootXtraj = footXtraj;
01764 stepF.leftfootYtraj = footYtraj;
01765 stepF.leftfootHeight = footHeight;
01766 stepF.leftfootOrient = footOrient;
01767 stepF.rightfootXtraj = stablefootXtraj;
01768 stepF.rightfootYtraj = stablefootYtraj;
01769 stepF.rightfootHeight = stablefootHeight;
01770 stepF.rightfootOrient = stablefootOrient;
01771 }
01772
01773 stepF.waistOrient = waistOrient;
01774 stepF.incrTime = incrTime;
01775 stepF.zc = zc;
01776 stepF.size = waistOrient.size();
01777
01778 }
01779
01780
01781 void CnewPGstepStudy::produceOneUPHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectUPHalfStep_input, char leftOrRightFootStable)
01782 {
01783
01784 vector<double> comTrajX;
01785 vector<double> zmpTrajX;
01786 genCOMZMPtrajectory(comTrajX, zmpTrajX, incrTime, zc, g, 0, 0, vectUPHalfStep_input[0], t1/2, t1*3/4, t1, t2, t3);
01787
01788 vector<double> comTrajY;
01789 vector<double> zmpTrajY;
01790 genCOMZMPtrajectory(comTrajY, zmpTrajY, incrTime, zc, g, 0, 0, vectUPHalfStep_input[1], t1/2, t1*3/4, t1, t2, t3);
01791
01792 vector<double> footXtraj;
01793 vector<double> footYtraj;
01794 int leftRightCoef = 0;
01795 if(leftOrRightFootStable == 'L') leftRightCoef = -1; else leftRightCoef = 1;
01796 genFOOTposition(footXtraj, footYtraj, incrTime, vectUPHalfStep_input[3], vectUPHalfStep_input[4], vectUPHalfStep_input[0], vectUPHalfStep_input[1]+leftRightCoef*vectUPHalfStep_input[6], DELAY_2, t1, t2, t3, t3, t3, 'u');
01797
01798 vector<double> footHeight;
01799 genFOOTdownUPheight(footHeight, incrTime, vectUPHalfStep_input[7], DELAY_1, t1, t2, t3);
01800
01801 vector<double> footOrient;
01802 genFOOTorientation(footOrient, incrTime, vectUPHalfStep_input[5], 0, DELAY_2, t1, t2, t3, t3, t3, 'u');
01803
01804 vector<double> stablefootXtraj;
01805 vector<double> stablefootYtraj;
01806 vector<double> stablefootHeight;
01807 vector<double> stablefootOrient;
01808
01809 int count = -1;
01810
01811 for(double i = 0.0 ; i < t3 ; i += incrTime)
01812 {
01813 count++;
01814 stablefootXtraj.push_back(vectUPHalfStep_input[0]);
01815 stablefootYtraj.push_back(vectUPHalfStep_input[1]);
01816 stablefootHeight.push_back(0);
01817 stablefootOrient.push_back(0);
01818 }
01819
01820 vector<double> waistOrient;
01821 genWAISTorientation(waistOrient, incrTime, 0, 0, DELAY_1, t1, t2, t3, t3, t3, 'u');
01822
01823
01824 stepF.comTrajX = comTrajX;
01825 stepF.zmpTrajX = zmpTrajX;
01826 stepF.comTrajY = comTrajY;
01827 stepF.zmpTrajY = zmpTrajY;
01828
01829 if(leftOrRightFootStable == 'L') {
01830 stepF.leftfootXtraj = stablefootXtraj;
01831 stepF.leftfootYtraj = stablefootYtraj;
01832 stepF.leftfootHeight = stablefootHeight;
01833 stepF.leftfootOrient = stablefootOrient;
01834 stepF.rightfootXtraj = footXtraj;
01835 stepF.rightfootYtraj = footYtraj;
01836 stepF.rightfootHeight = footHeight;
01837 stepF.rightfootOrient = footOrient;
01838 } else {
01839 stepF.leftfootXtraj = footXtraj;
01840 stepF.leftfootYtraj = footYtraj;
01841 stepF.leftfootHeight = footHeight;
01842 stepF.leftfootOrient = footOrient;
01843 stepF.rightfootXtraj = stablefootXtraj;
01844 stepF.rightfootYtraj = stablefootYtraj;
01845 stepF.rightfootHeight = stablefootHeight;
01846 stepF.rightfootOrient = stablefootOrient;
01847 }
01848
01849 stepF.waistOrient = waistOrient;
01850 stepF.incrTime = incrTime;
01851 stepF.zc = zc;
01852 stepF.size = waistOrient.size();
01853
01854 }
01855
01856
01857 void CnewPGstepStudy::produceOneDOWNHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectDOWNHalfStep_input, char leftOrRightFootStable)
01858 {
01859
01860 vector<double> comTrajX;
01861 vector<double> zmpTrajX;
01862 genCOMZMPtrajectory(comTrajX, zmpTrajX, incrTime, zc, g, 0, 0, vectDOWNHalfStep_input[2]/2, t1/2, t1*3/4, t1, t2, t3);
01863
01864 vector<double> comTrajY;
01865 vector<double> zmpTrajY;
01866 genCOMZMPtrajectory(comTrajY, zmpTrajY, incrTime, zc, g, 0, 0, vectDOWNHalfStep_input[3]/2, t1/2, t1*3/4, t1, t2, t3);
01867
01868 vector<double> footXtraj;
01869 vector<double> footYtraj;
01870 int leftRightCoef = 0;
01871 if(leftOrRightFootStable == 'L') leftRightCoef = -1; else leftRightCoef = 1;
01872 genFOOTposition(footXtraj, footYtraj, incrTime, 0, leftRightCoef*vectDOWNHalfStep_input[0], vectDOWNHalfStep_input[2], vectDOWNHalfStep_input[3], DELAY_2, 0, 0, t1, t2, t3, 'd');
01873
01874 vector<double> footHeight;
01875 genFOOTupDOWNheight(footHeight, incrTime, vectDOWNHalfStep_input[1], DELAY_1, t1, t2, t3);
01876
01877 vector<double> footOrient;
01878 genFOOTorientation(footOrient, incrTime, 0, vectDOWNHalfStep_input[4], DELAY_2, 0, 0, t1, t2, t3, 'd');
01879
01880 vector<double> waistOrient;
01881 genWAISTorientation(waistOrient, incrTime, 0, vectDOWNHalfStep_input[4], DELAY_1, 0, 0, t1, t2, t3, 'd');
01882
01883 vector<double> stablefootXtraj;
01884 vector<double> stablefootYtraj;
01885 vector<double> stablefootHeight;
01886 vector<double> stablefootOrient;
01887
01888 for(double i = 0.0 ; i < t3 ; i += incrTime)
01889 {
01890 stablefootXtraj.push_back(0);
01891 stablefootYtraj.push_back(0);
01892 stablefootHeight.push_back(0);
01893 stablefootOrient.push_back(0);
01894 }
01895
01896 stepF.comTrajX = comTrajX;
01897 stepF.zmpTrajX = zmpTrajX;
01898 stepF.comTrajY = comTrajY;
01899 stepF.zmpTrajY = zmpTrajY;
01900
01901 if(leftOrRightFootStable == 'L') {
01902 stepF.leftfootXtraj = stablefootXtraj;
01903 stepF.leftfootYtraj = stablefootYtraj;
01904 stepF.leftfootHeight = stablefootHeight;
01905 stepF.leftfootOrient = stablefootOrient;
01906 stepF.rightfootXtraj = footXtraj;
01907 stepF.rightfootYtraj = footYtraj;
01908 stepF.rightfootHeight = footHeight;
01909 stepF.rightfootOrient = footOrient;
01910 } else {
01911 stepF.leftfootXtraj = footXtraj;
01912 stepF.leftfootYtraj = footYtraj;
01913 stepF.leftfootHeight = footHeight;
01914 stepF.leftfootOrient = footOrient;
01915 stepF.rightfootXtraj = stablefootXtraj;
01916 stepF.rightfootYtraj = stablefootYtraj;
01917 stepF.rightfootHeight = stablefootHeight;
01918 stepF.rightfootOrient = stablefootOrient;
01919 }
01920
01921 stepF.waistOrient = waistOrient;
01922 stepF.incrTime = incrTime;
01923 stepF.zc = zc;
01924 stepF.size = waistOrient.size();
01925
01926 }
01927
01928
01929 void CnewPGstepStudy::produceSeqStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, double t4, double t5, vector<double> vectSteps_input, char leftOrRightFootStable)
01930 {
01931
01932 char alternate = leftOrRightFootStable;
01933
01934 vector<StepFeatures> vectSFeat;
01935
01936 vector<double> tmpNine;
01937 tmpNine.resize(10);
01938
01939 tmpNine[7] = (vectSteps_input[0]*2)*cos(-vectSteps_input[5]*PI/180)-(vectSteps_input[1]*2)*sin(-vectSteps_input[5]*PI/180);
01940 tmpNine[8] = (vectSteps_input[0]*2)*sin(-vectSteps_input[5]*PI/180)+(vectSteps_input[1]*2)*cos(-vectSteps_input[5]*PI/180) ;
01941 tmpNine[9] = -vectSteps_input[5];
01942
01943 for(unsigned int i = 1; i <= (vectSteps_input.size()-6)/4; i++) {
01944
01945 tmpNine[0] = (tmpNine[7]/2)*cos(-tmpNine[9]*PI/180) - (tmpNine[8]/2)*sin(-tmpNine[9]*PI/180);
01946 tmpNine[1] = (tmpNine[7]/2)*sin(-tmpNine[9]*PI/180) + (tmpNine[8]/2)*cos(-tmpNine[9]*PI/180);
01947 tmpNine[2] = 0;
01948 tmpNine[3] = -tmpNine[0];
01949 tmpNine[4] = -tmpNine[1];
01950 tmpNine[5] = -tmpNine[9];
01951 tmpNine[6] = vectSteps_input[4*i+2];
01952 tmpNine[7] = vectSteps_input[4*i+3];
01953 tmpNine[8] = vectSteps_input[4*i+4];
01954 tmpNine[9] = vectSteps_input[4*i+5];
01955
01956 vector<vector<double> > tmp_fb;
01957 vector<vector<double> > tmp_fbZMP;
01958
01959 StepFeatures tmp_step;
01960
01961 produceOneStepFeatures(tmp_step, incrTime, zc, g, t1, t2, t3, t4, t5, tmpNine, alternate);
01962
01963 vectSFeat.push_back(tmp_step);
01964
01965 if(alternate == 'L') alternate = 'R'; else alternate = 'L';
01966
01967 }
01968
01969 for(unsigned int i = 1; i < vectSFeat.size(); i++) {
01970 addStepFeaturesWithSlide(vectSFeat[0],vectSFeat[i],0);
01971 }
01972
01973 stepF = vectSFeat[0];
01974
01975 }
01976
01977
01978 void CnewPGstepStudy::produceSeqHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectSteps_input, char leftOrRightFootStable)
01979 {
01980
01981 char alternate = leftOrRightFootStable;
01982
01983 vector<StepFeatures> vectSFeat;
01984
01985 vector<double> tmpEleven;
01986 tmpEleven.resize(11);
01987
01988 vector<double> tmpPart1;
01989 tmpPart1.resize(8);
01990
01991 vector<double> tmpPart2;
01992 tmpPart2.resize(5);
01993
01994 tmpEleven[8] = (vectSteps_input[0]*2)*cos(-vectSteps_input[5]*PI/180)-(vectSteps_input[1]*2)*sin(-vectSteps_input[5]*PI/180);
01995 tmpEleven[9] = (vectSteps_input[0]*2)*sin(-vectSteps_input[5]*PI/180)+(vectSteps_input[1]*2)*cos(-vectSteps_input[5]*PI/180) ;
01996 tmpEleven[10] = -vectSteps_input[5];
01997
01998 for(unsigned int i = 1; i <= (vectSteps_input.size()-6)/5; i++) {
01999
02000 tmpEleven[0] = (tmpEleven[8]/2)*cos(-tmpEleven[10]*PI/180) - (tmpEleven[9]/2)*sin(-tmpEleven[10]*PI/180);
02001 tmpEleven[1] = (tmpEleven[8]/2)*sin(-tmpEleven[10]*PI/180) + (tmpEleven[9]/2)*cos(-tmpEleven[10]*PI/180);
02002 tmpEleven[2] = 0;
02003 tmpEleven[3] = -tmpEleven[0];
02004 tmpEleven[4] = -tmpEleven[1];
02005 tmpEleven[5] = -tmpEleven[10];
02006
02007 tmpEleven[6] = vectSteps_input[5*i+1];
02008 tmpEleven[7] = vectSteps_input[5*i+2];
02009
02010 tmpEleven[8] = vectSteps_input[5*i+3];
02011 tmpEleven[9] = vectSteps_input[5*i+4];
02012 tmpEleven[10] = vectSteps_input[5*i+5];
02013
02014 StepFeatures tmp_hstepUp;
02015 StepFeatures tmp_hstepDown;
02016
02017 tmpPart1[0] = tmpEleven[0];
02018 tmpPart1[1] = tmpEleven[1];
02019 tmpPart1[2] = tmpEleven[2];
02020 tmpPart1[3] = tmpEleven[3];
02021 tmpPart1[4] = tmpEleven[4];
02022 tmpPart1[5] = tmpEleven[5];
02023 tmpPart1[6] = tmpEleven[6];
02024 tmpPart1[7] = tmpEleven[7];
02025
02026 tmpPart2[0] = tmpEleven[6];
02027 tmpPart2[1] = tmpEleven[7];
02028 tmpPart2[2] = tmpEleven[8];
02029 tmpPart2[3] = tmpEleven[9];
02030 tmpPart2[4] = tmpEleven[10];
02031
02032 produceOneUPHalfStepFeatures(tmp_hstepUp, incrTime, zc, g, t1, t2, t3, tmpPart1, alternate);
02033
02034 vectSFeat.push_back(tmp_hstepUp);
02035
02036 produceOneDOWNHalfStepFeatures(tmp_hstepDown, incrTime, zc, g, t1, t2, t3, tmpPart2, alternate);
02037
02038 vectSFeat.push_back(tmp_hstepDown);
02039
02040 if(alternate == 'L') alternate = 'R'; else alternate = 'L';
02041
02042 }
02043
02044 for(unsigned int i = 1; i < vectSFeat.size(); i++) {
02045 addStepFeaturesWithSlide(vectSFeat[0],vectSFeat[i],0);
02046 }
02047
02048 stepF = vectSFeat[0];
02049
02050 }
02051
02052
02053 void CnewPGstepStudy::produceSeqSlidedHalfStepFeatures(StepFeatures & stepF, double incrTime, double zc, double g, double t1, double t2, double t3, vector<double> vectSteps_input, char leftOrRightFootStable)
02054 {
02055
02056 char alternate = leftOrRightFootStable;
02057
02058 vector<StepFeatures> vectSFeat;
02059 vector<double> slideProfile;
02060
02061 vector<double> tmpEleven;
02062 tmpEleven.resize(11);
02063
02064 vector<double> tmpPart1;
02065 tmpPart1.resize(8);
02066
02067 vector<double> tmpPart2;
02068 tmpPart2.resize(5);
02069
02070 tmpEleven[8] = (vectSteps_input[0]*2)*cos(-vectSteps_input[5]*PI/180)-(vectSteps_input[1]*2)*sin(-vectSteps_input[5]*PI/180);
02071 tmpEleven[9] = (vectSteps_input[0]*2)*sin(-vectSteps_input[5]*PI/180)+(vectSteps_input[1]*2)*cos(-vectSteps_input[5]*PI/180) ;
02072 tmpEleven[10] = -vectSteps_input[5];
02073
02074 for(unsigned int i = 1; i <= (vectSteps_input.size()-6)/7; i++) {
02075
02076 tmpEleven[0] = (tmpEleven[8]/2)*cos(-tmpEleven[10]*PI/180) - (tmpEleven[9]/2)*sin(-tmpEleven[10]*PI/180);
02077 tmpEleven[1] = (tmpEleven[8]/2)*sin(-tmpEleven[10]*PI/180) + (tmpEleven[9]/2)*cos(-tmpEleven[10]*PI/180);
02078 tmpEleven[2] = 0;
02079 tmpEleven[3] = -tmpEleven[0];
02080 tmpEleven[4] = -tmpEleven[1];
02081 tmpEleven[5] = -tmpEleven[10];
02082
02083 slideProfile.push_back(vectSteps_input[7*i-1]);
02084 tmpEleven[6] = vectSteps_input[7*i];
02085 tmpEleven[7] = vectSteps_input[7*i+1];
02086
02087 slideProfile.push_back(vectSteps_input[7*i+2]);
02088 tmpEleven[8] = vectSteps_input[7*i+3];
02089 tmpEleven[9] = vectSteps_input[7*i+4];
02090 tmpEleven[10] = vectSteps_input[7*i+5];
02091
02092 StepFeatures tmp_hstepUp;
02093 StepFeatures tmp_hstepDown;
02094
02095 tmpPart1[0] = tmpEleven[0];
02096 tmpPart1[1] = tmpEleven[1];
02097 tmpPart1[2] = tmpEleven[2];
02098 tmpPart1[3] = tmpEleven[3];
02099 tmpPart1[4] = tmpEleven[4];
02100 tmpPart1[5] = tmpEleven[5];
02101 tmpPart1[6] = tmpEleven[6];
02102 tmpPart1[7] = tmpEleven[7];
02103
02104 tmpPart2[0] = tmpEleven[6];
02105 tmpPart2[1] = tmpEleven[7];
02106 tmpPart2[2] = tmpEleven[8];
02107 tmpPart2[3] = tmpEleven[9];
02108 tmpPart2[4] = tmpEleven[10];
02109
02110 produceOneUPHalfStepFeatures(tmp_hstepUp, incrTime, zc, g, t1, t2, t3, tmpPart1, alternate);
02111
02112 vectSFeat.push_back(tmp_hstepUp);
02113
02114 produceOneDOWNHalfStepFeatures(tmp_hstepDown, incrTime, zc, g, t1, t2, t3, tmpPart2, alternate);
02115
02116 vectSFeat.push_back(tmp_hstepDown);
02117
02118 if(alternate == 'L') alternate = 'R'; else alternate = 'L';
02119
02120 }
02121
02122 for(unsigned int i = 1; i < vectSFeat.size(); i++) {
02123 addStepFeaturesWithSlide(vectSFeat[0],vectSFeat[i],slideProfile[i]);
02124 }
02125
02126 assert(!vectSFeat.empty());
02127 stepF = vectSFeat[0];
02128
02129 }