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

# Modules/WalkingEngine/InvKinWalkingParameters.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file InvKinWalkingParameters.cpp
    00003 *
    00004 * Implementation of class InvKinWalkingParameters
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "InvKinWalkingParameters.h"
    00010 #include "Tools/RobotConfiguration.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012 #include "Tools/Actorics/Kinematics.h"
    00013
    00014 InvKinWalkingParameters::InvKinWalkingParameters(const GT2004Parameters& gt2004Param)
    00015 {
    00016   foreHeight =gt2004Param.foreHeight;
    00017   foreWidth  =gt2004Param.foreWidth;
    00018   foreCenterX=gt2004Param.foreCenterX;
    00019
    00020   hindHeight =gt2004Param.hindHeight;
    00021   hindWidth  =gt2004Param.hindWidth;
    00022   hindCenterX=gt2004Param.hindCenterX;
    00023
    00024   foreFootLift=gt2004Param.foreFootLift;
    00025   hindFootLift=gt2004Param.hindFootLift;
    00026
    00027   foreFootTilt=gt2004Param.foreFootTilt;
    00028   hindFootTilt=gt2004Param.hindFootTilt;
    00029
    00030   stepLen=gt2004Param.stepLen;
    00031
    00032   switch(gt2004Param.footMode)
    00033   {
    00034   case GT2004Parameters::rectangle:
    00035     footMode=rectangle;
    00036     break;
    00037   case GT2004Parameters::halfCircle:
    00038   default:
    00039     footMode=halfCircle;
    00040     break;
    00041   }
    00042
    00043   int i;
    00044   for (i=0;i<2;i++)
    00045   {
    00046     groundPhase[i]=gt2004Param.groundPhase[i];
    00047     liftPhase[i]=gt2004Param.liftPhase[i];         //temp for BB support
    00048     loweringPhase[i]=gt2004Param.loweringPhase[i]; //temp for BB support
    00049   }
    00050   for (i=0;i<4;i++)
    00051   {
    00052     legPhase[i]=gt2004Param.legPhase[i];
    00053   }
    00054
    00055   headTilt=gt2004Param.headTilt;
    00056   headPan=gt2004Param.headPan;
    00057   headRoll=gt2004Param.headRoll;
    00058   mouth=gt2004Param.mouth;
    00059
    00060   name[0]=0;
    00061
    00062   legSpeedFactorX=1;
    00063   legSpeedFactorY=1;
    00064   legSpeedFactorR=1;
    00065
    00066   maxStepSizeX=(gt2004Param.index==(int)GT2004ParametersSet::numberOfParameters)?gt2004Param.correctedMotion.translation.x:45;
    00067   maxStepSizeY=(gt2004Param.index==(int)GT2004ParametersSet::numberOfParameters)?gt2004Param.correctedMotion.translation.y:45;
    00068   maxSpeedXChange=100;
    00069   maxSpeedYChange=100;
    00070   maxRotationChange=1;
    00071   counterRotation=0;
    00072
    00073   for (int i2=0;i2<4;i2++)
    00074   {
    00075     for (int i3=0;i3<3;i3++)
    00076     {
    00077       freeFormQuadPos[0][i2][i3]=0;
    00078     }
    00079   }
    00080
    00081   leaveAnytime=true;
    00082   bodyShiftX=0;
    00083   bodyShiftY=0;
    00084   bodyShiftOffset=0;
    00085
    00086   neckHeight=120;
    00087   bodyTilt=0;
    00088   bodyTiltOffset=0;
    00089
    00090   fitness=0;
    00091 }
    00092
    00093 void InvKinWalkingParameters::init()
    00094 {
    00095   bodyTilt = asin((hindHeight - foreHeight)/getRobotConfiguration().getRobotDimensions().lengthBetweenLegs);
    00096
    00097   int i;
    00098   for(i=0;i<2;i++)
    00099   {
    00100     groundTime[i] = (int)(groundPhase[i] * stepLen);
    00101     liftTime[i] = (int)(liftPhase[i] * stepLen);
    00102     loweringTime[i] = (int)(loweringPhase[i] * stepLen);
    00103
    00104     airTime[i]=stepLen-groundTime[i]-liftTime[i]-loweringTime[i];
    00105     stepLift[i]=groundTime[i];
    00106     stepAir[i]=stepLift[i]+liftTime[i];
    00107     stepLower[i]=stepAir[i]+airTime[i];
    00108   }
    00109
    00110   for (i=0;i<4;i++) {
    00111     legPhaseIndex[i]=(int)(((double)stepLen)*legPhase[i]);
    00112   }
    00113
    00114   //search first step where all feet are on ground
    00115   firstStep=0;
    00116   while(firstStep<stepLen)
    00117   {
    00118     if (allFeetOnGround(firstStep)) break;
    00119     firstStep++;
    00120   }
    00121   if (firstStep==stepLen)
    00122   {
    00123     if (!leaveAnytime)
    00124     {
    00125       // this crashes, because InvKinWalkingParameters are initialized in places where OUTPUT does not work:
    00126       // OUTPUT(idText,text,"InvKinWalkingEngine:" << name << " : bad parameter set, never all feet on ground.");
    00127     }
    00128     firstStep = 0;
    00129   }
    00130
    00131   if (maxStepSizeX == 0) maxStepSizeX=0.01;
    00132   if (maxStepSizeY == 0) maxStepSizeY=0.01;
    00133   //maxStepSizeR is radius of biggest square fitting into ellipse from maxStepSizeX,maxStepSizeY
    00134   maxStepSizeR =
    00135     maxStepSizeX*maxStepSizeY/
    00136     sqrt(maxStepSizeX*maxStepSizeX + maxStepSizeY*maxStepSizeY);
    00137 }
    00138
    00139 void InvKinWalkingParameters::readValues()
    00140 {
    00141   if (strlen(name)==0)
    00142   {
    00143     correctionValues.forward = correctionValues.backward =
    00144       correctionValues.sideward = 1.0;
    00145     correctionValues.turning = 100.0;
    00146     correctionValues.rotationCenter = 0;
    00147     neckHeight = 0;
    00148   } else {
    00149     char section[100];
    00150     strcpy(section, "InvKinWalkingEngine:");
    00151     strcat(section, name);
    00152
    00153     InConfigFile odometryFile("Odometry.cfg",section);
    00154
    00155     if (!odometryFile.exists() || odometryFile.eof()) {
    00156       OUTPUT(idText,text,"InvKinWalkingEngine : Error, odometry not found for : " << name);
    00157       correctionValues.forward = correctionValues.backward =
    00158         correctionValues.sideward = 1.0;
    00159       correctionValues.turning = 100.0;
    00160       correctionValues.rotationCenter = 0;
    00161       neckHeight = 0;
    00162     } else {
    00163       double speed;
    00164       odometryFile >> speed;
    00165       correctionValues.forward = maxStepSizeX / speed;
    00166       odometryFile >> speed;
    00167       correctionValues.backward = maxStepSizeX / speed;
    00168       odometryFile >> speed;
    00169       correctionValues.sideward = maxStepSizeY / speed;
    00170       odometryFile >> speed;
    00171       correctionValues.turning = maxStepSizeR / speed;
    00172       odometryFile >> neckHeight;
    00173       odometryFile >> correctionValues.rotationCenter;
    00174     }
    00175   }
    00176 }
    00177
    00178 bool InvKinWalkingParameters::allFeetOnGround(int currentStep)
    00179 {
    00180   int leg;
    00181   for (leg=0;leg<4;leg++)
    00182     if((currentStep+legPhaseIndex[leg])%stepLen > stepLift[FORELEG(leg)?0:1])
    00183       break;
    00184   return (leg==4);
    00185 }
    00186
    00187 void InvKinWalkingParameters::copyValuesFrom(const InvKinWalkingParameters & paramsToCopy)
    00188 {
    00189   strcpy(this->name, paramsToCopy.name);
    00190   this->footMode = paramsToCopy.footMode;
    00191   this->foreHeight = paramsToCopy.foreHeight;
    00192   this->foreWidth = paramsToCopy.foreWidth;
    00193   this->foreCenterX = paramsToCopy.foreCenterX;
    00194   this->hindHeight = paramsToCopy.hindHeight;
    00195   this->hindWidth = paramsToCopy.hindWidth;
    00196   this->hindCenterX = paramsToCopy.hindCenterX;
    00197   this->foreFootLift = paramsToCopy.foreFootLift;
    00198   this->hindFootLift = paramsToCopy.hindFootLift;
    00199   this->foreFootTilt = paramsToCopy.foreFootTilt;
    00200   this->hindFootTilt = paramsToCopy.hindFootTilt;
    00201   this->legSpeedFactorX = paramsToCopy.legSpeedFactorX;
    00202   this->legSpeedFactorY = paramsToCopy.legSpeedFactorY;
    00203   this->legSpeedFactorR = paramsToCopy.legSpeedFactorR;
    00204   this->maxStepSizeX = paramsToCopy.maxStepSizeX;
    00205   this->maxStepSizeY = paramsToCopy.maxStepSizeY;
    00206   this->maxSpeedXChange = paramsToCopy.maxSpeedXChange;
    00207   this->maxSpeedYChange = paramsToCopy.maxSpeedYChange;
    00208   this->maxRotationChange = paramsToCopy.maxRotationChange;
    00209   this->counterRotation = paramsToCopy.counterRotation;
    00210   this->stepLen = paramsToCopy.stepLen;
    00211   this->groundPhase[0] = paramsToCopy.groundPhase[0];
    00212   this->liftPhase[0] = paramsToCopy.liftPhase[0];
    00213   this->loweringPhase[0] = paramsToCopy.loweringPhase[0];
    00214   this->groundPhase[1] = paramsToCopy.groundPhase[1];
    00215   this->liftPhase[1] = paramsToCopy.liftPhase[1];
    00216   this->loweringPhase[1] = paramsToCopy.loweringPhase[1];
    00217   this->legPhase[0] = paramsToCopy.legPhase[0];
    00218   this->legPhase[1] = paramsToCopy.legPhase[1];
    00219   this->legPhase[2] = paramsToCopy.legPhase[2];
    00220   this->legPhase[3] = paramsToCopy.legPhase[3];
    00221   this->bodyShiftX = paramsToCopy.bodyShiftX;
    00222   this->bodyShiftY = paramsToCopy.bodyShiftY;
    00223   this->bodyShiftOffset = paramsToCopy.bodyShiftOffset;
    00224 }
    00225
    00226 In& operator>>(In& stream,InvKinWalkingParameters& walkingParameters)
    00227 {
    00228   stream >> walkingParameters.name;
    00229   int m;
    00230   stream >> m;
    00231   walkingParameters.footMode = (InvKinWalkingParameters::FootMode)m;
    00232   stream >> walkingParameters.foreHeight;
    00233   stream >> walkingParameters.foreWidth;
    00234   stream >> walkingParameters.foreCenterX;
    00235   stream >> walkingParameters.hindHeight;
    00236   stream >> walkingParameters.hindWidth;
    00237   stream >> walkingParameters.hindCenterX;
    00238   stream >> walkingParameters.foreFootLift;
    00239   stream >> walkingParameters.hindFootLift;
    00240   stream >> walkingParameters.foreFootTilt;
    00241   stream >> walkingParameters.hindFootTilt;
    00242   stream >> walkingParameters.legSpeedFactorX;
    00243   stream >> walkingParameters.legSpeedFactorY;
    00244   stream >> walkingParameters.legSpeedFactorR;
    00245   stream >> walkingParameters.maxStepSizeX;
    00246   stream >> walkingParameters.maxStepSizeY;
    00247   stream >> walkingParameters.maxSpeedXChange;
    00248   stream >> walkingParameters.maxSpeedYChange;
    00249   stream >> walkingParameters.maxRotationChange;
    00250   stream >> walkingParameters.counterRotation;
    00251   stream >> walkingParameters.stepLen;
    00252   stream >> walkingParameters.groundPhase[0];
    00253   stream >> walkingParameters.liftPhase[0];
    00254   stream >> walkingParameters.loweringPhase[0];
    00255   stream >> walkingParameters.groundPhase[1];
    00256   stream >> walkingParameters.liftPhase[1];
    00257   stream >> walkingParameters.loweringPhase[1];
    00258   stream >> walkingParameters.legPhase[0];
    00259   stream >> walkingParameters.legPhase[1];
    00260   stream >> walkingParameters.legPhase[2];
    00261   stream >> walkingParameters.legPhase[3];
    00262   stream >> walkingParameters.bodyShiftX;
    00263   stream >> walkingParameters.bodyShiftY;
    00264   stream >> walkingParameters.bodyShiftOffset;
    00265
    00266   return stream;
    00267 }
    00268
    00269 Out& operator<<(Out& stream, const InvKinWalkingParameters& walkingParameters)
    00270 {
    00271   stream << walkingParameters.name;
    00272   stream << (int)walkingParameters.footMode;
    00273   stream << walkingParameters.foreHeight;
    00274   stream << walkingParameters.foreWidth;
    00275   stream << walkingParameters.foreCenterX;
    00276   stream << walkingParameters.hindHeight;
    00277   stream << walkingParameters.hindWidth;
    00278   stream << walkingParameters.hindCenterX;
    00279   stream << walkingParameters.foreFootLift;
    00280   stream << walkingParameters.hindFootLift;
    00281   stream << walkingParameters.foreFootTilt;
    00282   stream << walkingParameters.hindFootTilt;
    00283   stream << walkingParameters.legSpeedFactorX;
    00284   stream << walkingParameters.legSpeedFactorY;
    00285   stream << walkingParameters.legSpeedFactorR;
    00286   stream << walkingParameters.maxStepSizeX;
    00287   stream << walkingParameters.maxStepSizeY;
    00288   stream << walkingParameters.maxSpeedXChange;
    00289   stream << walkingParameters.maxSpeedYChange;
    00290   stream << walkingParameters.maxRotationChange;
    00291   stream << walkingParameters.counterRotation;
    00292   stream << walkingParameters.stepLen;
    00293   stream << walkingParameters.groundPhase[0];
    00294   stream << walkingParameters.liftPhase[0];
    00295   stream << walkingParameters.loweringPhase[0];
    00296   stream << walkingParameters.groundPhase[1];
    00297   stream << walkingParameters.liftPhase[1];
    00298   stream << walkingParameters.loweringPhase[1];
    00299   stream << walkingParameters.legPhase[0];
    00300   stream << walkingParameters.legPhase[1];
    00301   stream << walkingParameters.legPhase[2];
    00302   stream << walkingParameters.legPhase[3];
    00303   stream << walkingParameters.bodyShiftX;
    00304   stream << walkingParameters.bodyShiftY;
    00305   stream << walkingParameters.bodyShiftOffset;
    00306
    00307   return stream;
    00308 }