UPennWalkMC.ccGo to the documentation of this file.00001 #include "UPennWalkMC.h"
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 using namespace std;
00015
00016 #include <math.h>
00017
00018 #ifndef PI
00019 #define PI M_PI
00020 #endif
00021
00022 const size_t MAX_WIDTH = 208;
00023 const size_t MAX_HEIGHT = 160;
00024 const size_t LAYERM_WIDTH = 104;
00025 const size_t LAYERM_HEIGHT = 80;
00026
00027 const double FIELD_VIEW_H = 56.9*(PI/180);
00028 const double FIELD_VIEW_V = 45.2*(PI/180);
00029 const double FOCAL_LENGTH = 192.0;
00030 const unsigned int IMAGE_WIDTH = 208;
00031 const unsigned int IMAGE_HEIGHT = 160;
00032
00033 const double BODY_TILT = -13*PI/180;
00034
00035 const double BODY_WIDTH = 134.4;
00036 const double BODY_LENGTH = 130.0;
00037
00038 const size_t NUM_LEG = 4;
00039 const size_t NUM_LEG_JOINT = 3*NUM_LEG;
00040
00041
00042 const double LEG_FORE_UPPER_Z = 69.5;
00043 const double LEG_FORE_UPPER_Y = 9.0;
00044 const double LEG_FORE_LOWER_Z = 76.4;
00045 const double LEG_FORE_LOWER_Y = -9.0;
00046
00047 const double LEG_HIND_UPPER_Z = 69.5;
00048 const double LEG_HIND_UPPER_Y = 9.0;
00049 const double LEG_HIND_LOWER_Z = 78.9;
00050 const double LEG_HIND_LOWER_Y = -9.0;
00051
00052
00053
00054 const double NECK_TILT2_TO_CAMERA_Y = 81.0;
00055 const double NECK_TILT2_TO_CAMERA_Z = -14.6;
00056 const double NECK_TILT_TO_TILT2 = 80.0;
00057 const double SHOULDER_TO_NECK_TILT_Y = 2.5;
00058 const double SHOULDER_TO_NECK_TILT_Z = 19.5;
00059
00060 const double MIN_SHOULDER_HEIGHT = 50.0;
00061
00062 const double TURN_OFFSET = 75.0;
00063
00064
00065
00066
00067
00068
00069
00070
00071 const double STANCE_BODY_TILT = 0*PI/180;
00072 const double STANCE_SHOULDER_HEIGHT = 105.;
00073
00074 const double STANCE_FORE_X0 = 7.;
00075
00076 const double STANCE_FORE_Y0 = 60.;
00077 const double STANCE_HIND_X0 = 2.;
00078 const double STANCE_HIND_Y0 = -45.;
00079
00080 const int WALK_QUARTER_PERIOD = 3;
00081 const double WALK_MAX_DISTANCE = 13.;
00082
00083 const double WALK_FORE_LIFT_INITIAL = 25;
00084 const double WALK_FORE_LIFT_FINAL = 45;
00085
00086 const double WALK_HIND_LIFT_INITIAL = 25;
00087 const double WALK_HIND_LIFT_FINAL = 25;
00088
00089 const double WALK_FORE_XMIN = -10;
00090 const double WALK_FORE_XMAX = 30;
00091 const double WALK_FORE_YMIN = 25;
00092
00093 const double WALK_FORE_YMAX = 90;
00094
00095 const double WALK_HIND_XMIN = -15;
00096 const double WALK_HIND_XMAX = 35;
00097 const double WALK_HIND_YMIN = -70;
00098 const double WALK_HIND_YMAX = -15;
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131 enum LegIdentifier {
00132 LEG_LEFT_FORE = 0,
00133 LEG_LEFT_HIND = 1,
00134 LEG_RIGHT_FORE = 2,
00135 LEG_RIGHT_HIND = 3
00136 };
00137
00138 UPennWalkMC::UPennWalkMC()
00139 : MotionCommand(), xVel(0), yVel(0), aVel(0),
00140
00141 body_tilt(STANCE_BODY_TILT), shoulder_height(STANCE_SHOULDER_HEIGHT),
00142 fore_x0(STANCE_FORE_X0), fore_y0(STANCE_FORE_Y0),
00143 hind_x0(STANCE_HIND_X0), hind_y0(STANCE_HIND_Y0),
00144
00145 walk_phase(0),walk_phase_direction(1),
00146 walk_quarter_period(WALK_QUARTER_PERIOD), walk_max_distance(WALK_MAX_DISTANCE),
00147 walk_fore_lift_initial(WALK_FORE_LIFT_INITIAL), walk_fore_lift_final(WALK_FORE_LIFT_FINAL),
00148 walk_hind_lift_initial(WALK_HIND_LIFT_INITIAL), walk_hind_lift_final(WALK_HIND_LIFT_FINAL),
00149 walk_fore_xmin(WALK_FORE_XMIN), walk_fore_xmax(WALK_FORE_XMAX),
00150 walk_fore_ymin(WALK_FORE_YMIN), walk_fore_ymax(WALK_FORE_YMAX),
00151 walk_hind_xmin(WALK_HIND_XMIN), walk_hind_xmax(WALK_HIND_XMAX),
00152 walk_hind_ymin(WALK_HIND_YMIN), walk_hind_ymax(WALK_HIND_YMAX)
00153 {
00154 for (int i = 0; i < 4; i++) {
00155 walk_current_x[i] = 0.0;
00156 walk_current_y[i] = 0.0;
00157 }
00158 }
00159
00160 void
00161 UPennWalkMC::SetLegJoints(double * x) {
00162 motman->setOutput(this,LFrLegOffset+RotatorOffset,x[LEG_LEFT_FORE*JointsPerLeg+0]);
00163 motman->setOutput(this,LFrLegOffset+ElevatorOffset,x[LEG_LEFT_FORE*JointsPerLeg+1]);
00164 motman->setOutput(this,LFrLegOffset+KneeOffset,x[LEG_LEFT_FORE*JointsPerLeg+2]);
00165
00166 motman->setOutput(this,RFrLegOffset+RotatorOffset,x[LEG_RIGHT_FORE*JointsPerLeg+0]);
00167 motman->setOutput(this,RFrLegOffset+ElevatorOffset,x[LEG_RIGHT_FORE*JointsPerLeg+1]);
00168 motman->setOutput(this,RFrLegOffset+KneeOffset,x[LEG_RIGHT_FORE*JointsPerLeg+2]);
00169
00170 motman->setOutput(this,LBkLegOffset+RotatorOffset,x[LEG_LEFT_HIND*JointsPerLeg+0]);
00171 motman->setOutput(this,LBkLegOffset+ElevatorOffset,x[LEG_LEFT_HIND*JointsPerLeg+1]);
00172 motman->setOutput(this,LBkLegOffset+KneeOffset,x[LEG_LEFT_HIND*JointsPerLeg+2]);
00173
00174 motman->setOutput(this,RBkLegOffset+RotatorOffset,x[LEG_RIGHT_HIND*JointsPerLeg+0]);
00175 motman->setOutput(this,RBkLegOffset+ElevatorOffset,x[LEG_RIGHT_HIND*JointsPerLeg+1]);
00176 motman->setOutput(this,RBkLegOffset+KneeOffset,x[LEG_RIGHT_HIND*JointsPerLeg+2]);
00177 }
00178
00179 void
00180 UPennWalkMC::SetStanceParameters(double bodyTilt, double shoulderHeight,
00181 double foreX0, double foreY0,
00182 double hindX0, double hindY0) {
00183 body_tilt = bodyTilt;
00184 shoulder_height = shoulderHeight;
00185 fore_x0 = foreX0;
00186 fore_y0 = foreY0;
00187 hind_x0 = hindX0;
00188 hind_y0 = hindY0;
00189 }
00190
00191 void
00192 UPennWalkMC::SetWalkSpeeds(int quarterPeriod, double maxDistance,
00193 double foreLiftInitial, double foreLiftFinal,
00194 double hindLiftInitial, double hindLiftFinal) {
00195 walk_quarter_period = quarterPeriod;
00196 walk_max_distance = maxDistance;
00197 walk_fore_lift_initial = foreLiftInitial;
00198 walk_fore_lift_final = foreLiftFinal;
00199 walk_hind_lift_initial = hindLiftInitial;
00200 walk_hind_lift_final = hindLiftFinal;
00201 }
00202
00203 void
00204 UPennWalkMC::SetWalkWorkspace(double foreXMin, double foreXMax,
00205 double foreYMin, double foreYMax,
00206 double hindXMin, double hindXMax,
00207 double hindYMin, double hindYMax) {
00208 walk_fore_xmin = foreXMin;
00209 walk_fore_xmax = foreXMax;
00210 walk_fore_ymin = foreYMin;
00211 walk_fore_ymax = foreYMax;
00212
00213 walk_hind_xmin = hindXMin;
00214 walk_hind_xmax = hindXMax;
00215 walk_hind_ymin = hindYMin;
00216 walk_hind_ymax = hindYMax;
00217 }
00218
00219
00220
00221
00222 void
00223 UPennWalkMC::LegPositionsToAngles(double *a)
00224 {
00225 double cosTilt = cos(body_tilt);
00226 double sinTilt = sin(body_tilt);
00227
00228 double foreHeight = shoulder_height;
00229 double hindHeight = shoulder_height - BODY_LENGTH*sinTilt;
00230
00231 for (int iLeg = 0; iLeg < 4; iLeg++) {
00232 double posX=0, posY=0, posZ=0;
00233 double dUpperZ = 0.0, dUpperY = 0.0, dLowerZ = 0.0, dLowerY = 0.0;
00234
00235 switch (iLeg) {
00236 case LEG_LEFT_FORE:
00237
00238 a[0] -= fore_x0;
00239 a[1] += fore_y0;
00240 a[2] -= foreHeight;
00241
00242 posX = -(a[0]);
00243 posY = (cosTilt*a[1]+sinTilt*a[2]);
00244 posZ = (-sinTilt*a[1]+cosTilt*a[2]);
00245 dUpperZ = LEG_FORE_UPPER_Z;
00246 dUpperY = LEG_FORE_UPPER_Y;
00247 dLowerZ = LEG_FORE_LOWER_Z;
00248 dLowerY = LEG_FORE_LOWER_Y;
00249 break;
00250 case LEG_LEFT_HIND:
00251
00252 a[0] -= hind_x0;
00253 a[1] += hind_y0;
00254 a[2] -= hindHeight;
00255
00256 posX = -(a[0]);
00257 posY = -(cosTilt*a[1]+sinTilt*a[2]);
00258 posZ = (-sinTilt*a[1]+cosTilt*a[2]);
00259
00260 dUpperZ = LEG_HIND_UPPER_Z;
00261 dUpperY = LEG_HIND_UPPER_Y;
00262 dLowerZ = LEG_HIND_LOWER_Z;
00263 dLowerY = LEG_HIND_LOWER_Y;
00264 break;
00265 case LEG_RIGHT_FORE:
00266
00267 a[0] += fore_x0;
00268 a[1] += fore_y0;
00269 a[2] -= foreHeight;
00270
00271 posX = (a[0]);
00272 posY = (cosTilt*a[1]+sinTilt*a[2]);
00273 posZ = (-sinTilt*a[1]+cosTilt*a[2]);
00274
00275 dUpperZ = LEG_FORE_UPPER_Z;
00276 dUpperY = LEG_FORE_UPPER_Y;
00277 dLowerZ = LEG_FORE_LOWER_Z;
00278 dLowerY = LEG_FORE_LOWER_Y;
00279 break;
00280 case LEG_RIGHT_HIND:
00281
00282 a[0] += hind_x0;
00283 a[1] += hind_y0;
00284 a[2] -= hindHeight;
00285
00286 posX = (a[0]);
00287 posY = -(cosTilt*a[1]+sinTilt*a[2]);
00288 posZ = (-sinTilt*a[1]+cosTilt*a[2]);
00289
00290 dUpperZ = LEG_HIND_UPPER_Z;
00291 dUpperY = LEG_HIND_UPPER_Y;
00292 dLowerZ = LEG_HIND_LOWER_Z;
00293 dLowerY = LEG_HIND_LOWER_Y;
00294 break;
00295 default:
00296 cerr << "UPennWalkMC::LegPositionsToAngles(): Unknown leg.\n" << endl;
00297 }
00298
00299 double dUpper = sqrt(dUpperY*dUpperY+dUpperZ*dUpperZ);
00300 double angleUpper = tan(dUpperY/dUpperZ);
00301
00302 double dLower = sqrt(dLowerY*dLowerY+dLowerZ*dLowerZ);
00303 double angleLower = tan(dLowerY/dLowerZ);
00304
00305 double posSumSq = posX*posX+posY*posY+posZ*posZ;
00306
00307 double cosJ3 = .5*(posSumSq-dUpper*dUpper-dLower*dLower)/(dUpper*dLower);
00308 cosJ3 = clip(cosJ3, -1.0, 1.0);
00309
00310 a[2] = acos(cosJ3)+angleUpper-angleLower;
00311
00312 double aZ = -dUpperZ-dLower*cos(a[2]+angleLower);
00313 double aY = dUpperY+dLower*sin(a[2]+angleLower);
00314
00315 double sinJ2 = -posX/aZ;
00316 sinJ2 = clip(sinJ2, -1.0, 1.0);
00317 a[1] = asin(sinJ2);
00318
00319 double J1a = atan2(aZ*cos(a[1]), aY);
00320 double J1b = atan2(posZ, posY);
00321 double J1 = J1b-J1a;
00322 while (J1 > PI) J1 -= 2*PI;
00323 while (J1 < -PI) J1 += 2*PI;
00324 a[0] = J1;
00325
00326 a += 3;
00327 }
00328 }
00329
00330 void
00331 UPennWalkMC::StandLegs(double x, double y, double z)
00332 {
00333 static double leg_joints[NUM_LEG_JOINT];
00334 double *a = leg_joints;
00335
00336 for (int iLeg = 0; iLeg < 4; iLeg++) {
00337 double center_x = 0, center_y = 0;
00338 switch (iLeg) {
00339 case LEG_LEFT_FORE:
00340 center_x = -fore_x0;
00341 center_y = fore_y0;
00342 break;
00343 case LEG_LEFT_HIND:
00344 center_x = -hind_x0;
00345 center_y = hind_y0;
00346 break;
00347 case LEG_RIGHT_FORE:
00348 center_x = fore_x0;
00349 center_y = fore_y0;
00350 break;
00351 case LEG_RIGHT_HIND:
00352 center_x = hind_x0;
00353 center_y = hind_y0;
00354 break;
00355 default:
00356 cerr << "UPennWalkMC::StandLegs(): Unknown leg.\n" << endl;
00357 }
00358
00359 a[0] = -x;
00360 a[1] = -y;
00361 a[2] = -z;
00362
00363 walk_current_x[iLeg] = center_x - x;
00364 walk_current_y[iLeg] = center_y - y;
00365
00366
00367 a += 3;
00368 }
00369
00370 LegPositionsToAngles(leg_joints);
00371 SetLegJoints(leg_joints);
00372
00373 }
00374
00375 int
00376 UPennWalkMC::GetWalkPhase() {
00377 return walk_phase;
00378 }
00379
00380 void
00381 UPennWalkMC::SetWalkPhase(int phase) {
00382 walk_phase = phase;
00383 }
00384
00385
00386 void
00387 UPennWalkMC::WalkLegs(double xWalk, double yWalk, double aWalk)
00388 {
00389
00390 if ((walk_phase == 0) &&
00391 (xWalk == 0.0) && (yWalk == 0.0) && (aWalk == 0.0)) {
00392 StandLegs(0, 0, 0);
00393 return;
00394 }
00395
00396 static double leg_joints[NUM_LEG_JOINT];
00397 double *a = leg_joints;
00398 bool switch_phase_direction = false;
00399
00400
00401 double afactor = .85*BODY_LENGTH*aWalk;
00402 double rnorm = sqrt(afactor*afactor + xWalk*xWalk + yWalk*yWalk);
00403 if (rnorm > walk_max_distance) {
00404 double scale = walk_max_distance/rnorm;
00405 aWalk *= scale;
00406 xWalk *= scale;
00407 yWalk *= scale;
00408 }
00409
00410
00411 int phase_diff_from_switch = walk_quarter_period+walk_phase_direction*walk_phase;
00412 int phase_diff_to_switch = walk_quarter_period-walk_phase_direction*walk_phase;
00413
00414 double half_width = .5*BODY_WIDTH;
00415 double half_length = .5*BODY_LENGTH*cos(body_tilt);
00416
00417 for (int iLeg = 0; iLeg < 4; iLeg++) {
00418 double center_x = 0, center_y = 0;
00419 double leg_offset_x = 0, leg_offset_y = 0;
00420 double leg_lift_initial = 0, leg_lift_final = 0;
00421 double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
00422 int leg_sign = 1;
00423
00424 double current_x = walk_current_x[iLeg];
00425 double current_y = walk_current_y[iLeg];
00426
00427 switch (iLeg) {
00428 case LEG_LEFT_FORE:
00429 leg_sign = 1;
00430 center_x = -fore_x0;
00431 center_y = fore_y0;
00432
00433 leg_offset_x = -half_width+center_x;
00434 leg_offset_y = half_length+center_y;
00435
00436 leg_lift_initial = walk_fore_lift_initial;
00437 leg_lift_final = walk_fore_lift_final;
00438
00439 xmin = -walk_fore_xmax; xmax = -walk_fore_xmin;
00440 ymin = walk_fore_ymin; ymax = walk_fore_ymax;
00441 break;
00442 case LEG_LEFT_HIND:
00443 leg_sign = -1;
00444 center_x = -hind_x0;
00445 center_y = hind_y0;
00446
00447 leg_offset_x = -half_width+center_x;
00448 leg_offset_y = -half_length+center_y;
00449
00450 leg_lift_initial = walk_hind_lift_initial;
00451 leg_lift_final = walk_hind_lift_final;
00452
00453 xmin = -walk_hind_xmax; xmax = -walk_hind_xmin;
00454 ymin = walk_hind_ymin; ymax = walk_hind_ymax;
00455 break;
00456 case LEG_RIGHT_FORE:
00457 leg_sign = -1;
00458 center_x = fore_x0;
00459 center_y = fore_y0;
00460
00461 leg_offset_x = half_width+center_x;
00462 leg_offset_y = half_length+center_y;
00463
00464 leg_lift_initial = walk_fore_lift_initial;
00465 leg_lift_final = walk_fore_lift_final;
00466
00467 xmin = walk_fore_xmin; xmax = walk_fore_xmax;
00468 ymin = walk_fore_ymin; ymax = walk_fore_ymax;
00469 break;
00470 case LEG_RIGHT_HIND:
00471 leg_sign = 1;
00472 center_x = hind_x0;
00473 center_y = hind_y0;
00474
00475 leg_offset_x = half_width+center_x;
00476 leg_offset_y = -half_length+center_y;
00477
00478 leg_lift_initial = walk_hind_lift_initial;
00479 leg_lift_final = walk_hind_lift_final;
00480
00481 xmin = walk_hind_xmin; xmax = walk_hind_xmax;
00482 ymin = walk_hind_ymin; ymax = walk_hind_ymax;
00483 break;
00484 default:
00485 cerr << "UPennWalkMC::WalkLegs(): Unknown leg.\n" << endl;
00486 }
00487
00488
00489 current_x -= center_x;
00490 current_y -= center_y;
00491 xmin -= center_x;
00492 xmax -= center_x;
00493 ymin -= center_y;
00494 ymax -= center_y;
00495
00496 double dx, dy;
00497
00498
00499 if (leg_sign == walk_phase_direction) {
00500
00501 if (phase_diff_from_switch > 0) {
00502
00503 dx = xWalk + (cos(aWalk)-1)*leg_offset_x-sin(aWalk)*leg_offset_y;
00504 dy = yWalk + sin(aWalk)*leg_offset_x+(cos(aWalk)-1)*leg_offset_y;
00505
00506 double destination_x = walk_quarter_period*dx;
00507 double destination_y = walk_quarter_period*dy;
00508
00509
00510 dx = (destination_x-current_x)/(phase_diff_to_switch+1);
00511 dy = (destination_y-current_y)/(phase_diff_to_switch+1);
00512
00513 current_x += dx;
00514 current_y += dy;
00515 }
00516
00517 current_x = clip(current_x, xmin, xmax);
00518 current_y = clip(current_y, ymin, ymax);
00519
00520 a[0] = current_x;
00521 a[1] = current_y;
00522 a[2] = (phase_diff_from_switch*leg_lift_final+
00523 phase_diff_to_switch*leg_lift_initial)/(2*walk_quarter_period);
00524
00525 }
00526 else {
00527
00528
00529 leg_offset_x += current_x;
00530 leg_offset_y += current_y;
00531 dx = xWalk + (cos(aWalk)-1)*leg_offset_x-sin(aWalk)*leg_offset_y;
00532 dy = yWalk + sin(aWalk)*leg_offset_x+(cos(aWalk)-1)*leg_offset_y;
00533
00534 current_x -= dx;
00535 current_y -= dy;
00536
00537 a[0] = current_x;
00538 a[1] = current_y;
00539 a[2] = 0;
00540
00541
00542
00543 current_x = clip(current_x, xmin, xmax);
00544 current_y = clip(current_y, ymin, ymax);
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571 }
00572
00573
00574
00575
00576 walk_current_x[iLeg] = current_x+center_x;
00577 walk_current_y[iLeg] = current_y+center_y;
00578
00579
00580 a += 3;
00581 }
00582
00583
00584
00585
00586
00587
00588
00589
00590 walk_phase += walk_phase_direction;
00591 if ((walk_phase > walk_quarter_period) ||
00592 (walk_phase < -walk_quarter_period)) {
00593 switch_phase_direction = true;
00594 }
00595
00596 if (switch_phase_direction) {
00597
00598 walk_phase = walk_phase_direction*walk_quarter_period;
00599 walk_phase_direction = -walk_phase_direction;
00600 }
00601
00602 LegPositionsToAngles(leg_joints);
00603 SetLegJoints(leg_joints);
00604
00605
00606
00607
00608
00609 }
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
|