Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

UPennWalkMC.cc

Go to the documentation of this file.
00001 #include "UPennWalkMC.h"
00002 
00003 //This class is ported from University of Pennsylvania's 2004 Robosoccer entry, and falls under their license:
00004 /*=========================================================================
00005     This software is distributed under the GNU General Public License,
00006     version 2.  If you do not have a copy of this licence, visit
00007     www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
00008     Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
00009     in the hope that it will be useful, but WITHOUT ANY WARRANTY,
00010     including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00011   ========================================================================= */
00012 
00013 //better to put this here instead of the header
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; // in pixel units (.5*WIDTH/tan(.5*FIELD_VIEW_H))
00030 const unsigned int IMAGE_WIDTH = 208;
00031 const unsigned int IMAGE_HEIGHT = 160;
00032 
00033 const double BODY_TILT = -13*PI/180; // Negative is head closer to ground
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 // Should optimize by also storing LENGTH and ANGLE of each segment:
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;  // From (71.5, 28.3) @ 30 deg.
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; // From (76.5, 21.3) @ 30 deg.
00050 const double LEG_HIND_LOWER_Y = -9.0;
00051 //const double LEG_HIND_LOWER_Z = 80.0; // From (76.5, 21.3) @ 30 deg.
00052 //const double LEG_HIND_LOWER_Y = 0.0;
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 // Maximum parameters for walk:
00067 
00068 
00069 // Imitating Paul's walk:
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 //const double STANCE_FORE_Y0 = 50.;
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.; // 390 (mm/sec) /30 frames = 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 //const double WALK_FORE_YMAX = 85;
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 // Imitating 2004 US Open walk:
00103 const double STANCE_BODY_TILT = -15*PI/180;
00104 const double STANCE_SHOULDER_HEIGHT = 95.;
00105 
00106 const double STANCE_FORE_X0 = 5.;
00107 const double STANCE_FORE_Y0 = 70.;
00108 const double STANCE_HIND_X0 = 10.;
00109 const double STANCE_HIND_Y0 = -40.;
00110 
00111 const int WALK_QUARTER_PERIOD = 4;
00112 const double WALK_MAX_DISTANCE = 8.; // 390 (mm/sec) /30 frames = 13
00113 
00114 const double WALK_FORE_LIFT_INITIAL = 20;
00115 const double WALK_FORE_LIFT_FINAL = 30;
00116 const double WALK_HIND_LIFT_INITIAL = 30;
00117 const double WALK_HIND_LIFT_FINAL = 20;
00118 
00119 const double WALK_FORE_XMIN = -20;
00120 const double WALK_FORE_XMAX = 40;
00121 const double WALK_FORE_YMIN = 0;
00122 const double WALK_FORE_YMAX = 100;
00123 
00124 const double WALK_HIND_XMIN = -20;
00125 const double WALK_HIND_XMAX = 50;
00126 const double WALK_HIND_YMIN = -150;
00127 const double WALK_HIND_YMAX = 20;
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     // Default stance parameters:
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     // Default walk parameters:
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 // Calculate 12 leg joint angles from leg positions
00220 // Note positions are relative to stance parameters
00221 // so all zero inputs -> stance angles
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       // Left Fore: Reverse x
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       // Left Hind: Reverse x,y
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       // Right Fore:
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       // Right Hind: Reverse y
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);  // Positive
00301 
00302     double dLower = sqrt(dLowerY*dLowerY+dLowerZ*dLowerZ);
00303     double angleLower = tan(dLowerY/dLowerZ);  // Negative
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     // Correct for angle offsets of leg segments
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/*=0*/, double y/*=0*/, double z/*=0*/)
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     // Advance pointer to next leg angles:
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/*=0.0*/, double yWalk/*=0.0*/, double aWalk/*=0.0*/)
00388 {
00389   // Check to see if legs should stand rather than walk
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   //  double afactor = .7*BODY_LENGTH*aWalk; // 1/2*sqrt(2)
00401   double afactor = .85*BODY_LENGTH*aWalk; // 1/2*sqrt(2)
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   //  printf("WalkLegs: phase = %d, phase_direction = %d\n", walk_phase, walk_phase_direction);
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; // Stance position center
00419     double leg_offset_x = 0, leg_offset_y = 0; // Relative to center of body
00420     double leg_lift_initial = 0, leg_lift_final = 0; // Lift heights
00421     double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0; // Workspace limits relative to shoulder
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     // Make relative to stance center:
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     // leg is up if leg_sign == walk_phase_direction
00499     if (leg_sign == walk_phase_direction) {
00500       // Leg is up
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       //      printf("Leg %d destination: %g, %g\n", iLeg, destination_x, destination_y);
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       // Leg is down
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; // Travels opposite direction as dx
00535       current_y -= dy; // Travels opposite direction as dy
00536 
00537       a[0] = current_x;
00538       a[1] = current_y;
00539       a[2] = 0;
00540 
00541       // Check if leg is going outside workspace:
00542       // This check is after writing leg positions to give extra reach
00543       current_x = clip(current_x, xmin, xmax);
00544       current_y = clip(current_y, ymin, ymax);
00545 
00546       /*
00547       Old code to advance phase to speed up cycle
00548       if (walk_current_x[iLeg] < xmin) {
00549   walk_current_x[iLeg] = xmin;
00550   // Only switch direction after quarter_period has elapsed:
00551   if (walk_phase_direction*walk_phase > 0)
00552     switch_phase_direction = true;
00553       }
00554       else if (walk_current_x[iLeg] > xmax) {
00555   walk_current_x[iLeg] = xmax;
00556   if (walk_phase_direction*walk_phase > 0)
00557     switch_phase_direction = true;
00558       }
00559       if (walk_current_y[iLeg] < ymin) {
00560   walk_current_y[iLeg] = ymin;
00561   if (walk_phase_direction*walk_phase > 0)
00562     switch_phase_direction = true;
00563       }
00564       else if (walk_current_y[iLeg] > ymax) {
00565   walk_current_y[iLeg] = ymax;
00566   if (walk_phase_direction*walk_phase > 0)
00567     switch_phase_direction = true;
00568       }
00569       */
00570 
00571     }
00572 
00573     //    printf("iLeg = %d: %g %g %g\n", iLeg, a[0],a[1],a[2]);
00574 
00575     // Store current leg positions in coordinates relative to shoulder:
00576     walk_current_x[iLeg] = current_x+center_x;
00577     walk_current_y[iLeg] = current_y+center_y;
00578 
00579     // Shift pointer to next leg:
00580     a += 3;
00581   }
00582 
00583   /*
00584   if (switch_phase_direction)
00585   printf("WalkLegs: workspace switch: phase = %d, phase_direction = %d\n", walk_phase, walk_phase_direction);
00586   */
00587 
00588   // walk_phase goes oscillates between
00589   // -walk_quarter_period to +walk_quarter_period:
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     // Switch legs:
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   // Update odometry:
00606   // Commenting out to put odometry control in Perl:
00607   //  Self.world->AddMotionUpdate(xWalk, yWalk, aWalk);
00608   
00609 }
00610 
00611 
00612 
00613 
00614 /*! @file
00615  * @brief Defines UPennWalkMC, which uses the UPennalizers' 2004 RoboCup code to compute walking gaits
00616  * @author UPennalizers 2004 (Creator)
00617  * @author ejt (Ported)
00618  *
00619  * The UPennalizers code was released under the GPL:\n
00620  *  ------------------------------------------------------------------------- \n
00621  *    This software is distributed under the GNU General Public License,      \n
00622  *    version 2.  If you do not have a copy of this licence, visit            \n
00623  *    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,       \n
00624  *    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed       \n
00625  *    in the hope that it will be useful, but WITHOUT ANY WARRANTY,           \n
00626  *    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.          \n
00627  *  ------------------------------------------------------------------------- \n
00628  *
00629  * $Author: ejt $
00630  * $Name: tekkotsu-2_4_1 $
00631  * $Revision: 1.1 $
00632  * $State: Exp $
00633  * $Date: 2005/04/12 21:33:31 $
00634  */

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:49 2005 by Doxygen 1.4.4