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

# Modules/WalkingEngine/GT2005WalkingEngine.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002 * @file GT2005WalkingEngine.cpp
    00003 * class GT2005WalkingEngine
    00004 * @author Denis Fisseler
    00005 ****************************************************************************/
    00006
    00007 #include "GT2005WalkingEngine.h"
    00008 #include "Tools/Player.h"
    00009 #include "Tools/RobotConfiguration.h"
    00010 #include "Tools/Actorics/RobotDimensions.h"
    00011 #include "InvKinWalkingParameterSets.h"
    00012 #include "Tools/Debugging/Debugging.h"
    00013 #include "GT2004ParameterSet.h"
    00014 #include "Tools/Actorics/RobotVertices.h"
    00015
    00016 const int GT2005WalkingEngine::Leg_FR = 0;
    00017 const int GT2005WalkingEngine::Leg_FL = 1;
    00018 const int GT2005WalkingEngine::Leg_HR = 2;
    00019 const int GT2005WalkingEngine::Leg_HL = 3;
    00020
    00021 GT2005WalkingEngine::GT2005WalkingEngine(const WalkingEngineInterfaces& interfaces):
    00022 WalkingEngine(interfaces), currentRequest(0,0,0), currentStepPercentage(0)
    00023 {
    00024   OUTPUT(idText, text, "");
    00025   OUTPUT(idText, text, "GT2005WalkingEngine: loading...");
    00026
    00027   loadingFailed = false;
    00028   activePoints = 4;
    00029
    00030   neckHeight = 180;
    00031   bodyTilt = 0;
    00032   currentStepPercentage = 0;
    00033   currentStep = 0;
    00034   currentParameters = paramSet.mergedParameters; // only needed for head/tail movement
    00035   standing = false;
    00036
    00037   debugging = false;
    00038   debugRequest = false;
    00039   debugLoop = true;
    00040
    00041   resampleRequest = false;
    00042   resampling = false;
    00043
    00044   polygonsRecieved = 0;
    00045
    00046   currentFIPSParameters = new GT2005Parameters();
    00047
    00048   if (currentFIPSParameters->loadingFailed)
    00049   {
    00050     loadingFailed = true;
    00051     OUTPUT(idText, text, "GT2005WalkingEngine: Loading of parameter set failed!");
    00052   }
    00053
    00054   // nur ne initialisierung, damit er sich beim ersten delete nicht aufhängt
    00055   currentPoly[Leg_FR] = new GT2005Polygon(1);
    00056   currentPoly[Leg_FL] = new GT2005Polygon(1);
    00057   currentPoly[Leg_HR] = new GT2005Polygon(1);
    00058   currentPoly[Leg_HL] = new GT2005Polygon(1);
    00059
    00060   gt2005DebugData.gt2005Status = 0;
    00061
    00062   readOdometryCalibrationData();
    00063
    00064   if (loadingFailed)
    00065   {
    00066     OUTPUT(idText, text, "GT2005WalkingEngine: Not successfuly loaded!");
    00067   }
    00068   else
    00069   {
    00070     OUTPUT(idText, text, "GT2005WalkingEngine: Successfuly loaded! Have a nice walk!");
    00071   }
    00072   OUTPUT(idText, text, "");
    00073 }
    00074
    00075 GT2005WalkingEngine::~GT2005WalkingEngine()
    00076 {
    00077   for (int i = 0; i < 4; i++)
    00078   {
    00079     delete currentPoly[i];
    00080   }
    00081   delete currentFIPSParameters;
    00082 }
    00083
    00084 void GT2005WalkingEngine::loadOdometryPlane(int planeNum, char* filename, int alternateDir)
    00085 {
    00086   int i, j;
    00087
    00088   char o_filename[30];
    00089   sprintf(o_filename, "mshwep/%02u/%s", alternateDir, filename);
    00090
    00091   // load special odometry if available
    00092   InBinaryFile ofile(o_filename);
    00093   if (ofile.exists() && (alternateDir > 0))
    00094   {
    00095     for(i = 0; i < 15; i++)
    00096     {
    00097       for(j = 0; j < 13; j++)
    00098       {
    00099         ofile >> odometryCalibrationData[i][j][planeNum].controlled >> odometryCalibrationData[i][j][planeNum].measured;
    00100         // reset for standing
    00101         if (odometryCalibrationData[i][j][planeNum].controlled == Pose2D(0.0, 0.0, 0.0))
    00102         {
    00103           odometryCalibrationData[i][j][planeNum].measured = Pose2D(0.0, 0.0, 0.0);
    00104         }
    00105       }
    00106     }
    00107     OUTPUT(idText, text, "GT2005WalkingEngine: Alternative odometry parameters loaded. (" << o_filename << ")");
    00108   }
    00109   else // load default odometry
    00110   {
    00111     char o_filename_default[30];
    00112     sprintf(o_filename_default, "mshwep/%s", filename);
    00113
    00114     InBinaryFile ofile_default(o_filename_default);
    00115     if (ofile_default.exists())
    00116     {
    00117       for(i = 0; i < 15; i++)
    00118       {
    00119         for(j = 0; j < 13; j++)
    00120         {
    00121           ofile_default >> odometryCalibrationData[i][j][planeNum].controlled >> odometryCalibrationData[i][j][planeNum].measured;
    00122           // reset for standing
    00123           if (odometryCalibrationData[i][j][planeNum].controlled == Pose2D(0.0, 0.0, 0.0))
    00124           {
    00125             odometryCalibrationData[i][j][planeNum].measured = Pose2D(0.0, 0.0, 0.0);
    00126           }
    00127         }
    00128       }
    00129     }
    00130     else
    00131     {
    00132       loadingFailed = false;
    00133       OUTPUT(idText, text, "GT2005WalkingEngine: Loading of odometry parameters failed! (" << o_filename_default << ")");
    00134     }
    00135   }
    00136 }
    00137
    00138 void GT2005WalkingEngine::loadOdometryRotation(int rotNum, char* filename, int alternateDir)
    00139 {
    00140   int i;
    00141
    00142   char o_filename[30];
    00143   sprintf(o_filename, "mshwep/%02u/%s", alternateDir, filename);
    00144
    00145   // load special odometry if available
    00146   InBinaryFile ofile(o_filename);
    00147   if (ofile.exists() && (alternateDir > 0))
    00148   {
    00149     for(i = 0; i < 21; i++)
    00150     {
    00151       ofile >> odometryCalibrationData2[rotNum][i].controlled >> odometryCalibrationData2[rotNum][i].measured;
    00152       if (odometryCalibrationData2[rotNum][i].controlled.rotation == 0.0)
    00153       {
    00154         odometryCalibrationData2[rotNum][i].controlled = odometryCalibrationData2[rotNum][i].measured = Pose2D(0.0, 0.0, 0.0);
    00155       }
    00156     }
    00157     OUTPUT(idText, text, "GT2005WalkingEngine: Alternative odometry parameters loaded. (" << o_filename << ")");
    00158   }
    00159   else // load default odometry
    00160   {
    00161     char o_filename_default[30];
    00162     sprintf(o_filename_default, "mshwep/%s", filename);
    00163
    00164     InBinaryFile ofile_default(o_filename_default);
    00165     if (ofile_default.exists())
    00166     {
    00167       for(i = 0; i < 21; i++)
    00168       {
    00169         ofile_default >> odometryCalibrationData2[rotNum][i].controlled >> odometryCalibrationData2[rotNum][i].measured;
    00170         if (odometryCalibrationData2[rotNum][i].controlled.rotation == 0.0)
    00171         {
    00172           odometryCalibrationData2[rotNum][i].controlled = odometryCalibrationData2[rotNum][i].measured = Pose2D(0.0, 0.0, 0.0);
    00173         }
    00174       }
    00175     }
    00176     else
    00177     {
    00178       loadingFailed = false;
    00179       OUTPUT(idText, text, "GT2005WalkingEngine: Loading of odometry parameters failed! (" << o_filename_default << ")");
    00180     }
    00181   }
    00182 }
    00183
    00184 void GT2005WalkingEngine::readOdometryCalibrationData()
    00185 {
    00186   // load odometry mac dependent
    00187   int d = 0;
    00188   unsigned char address[6];
    00189   SystemCall::getMacAddress(address);
    00190   char macAddress[13];
    00191   sprintf(macAddress, "%02X%02X%02X%02X%02X%02X", int(address[0]), int(address[1]), int(address[2]), int(address[3]), int(address[4]), int(address[5]));
    00192   InConfigFile macCFG("mshwep/mac.dat", macAddress);
    00193   if(macCFG.exists() && !macCFG.eof())
    00194   {
    00195     char ignore[50];
    00196     macCFG >> ignore >> d;
    00197   }
    00198   OUTPUT(idText, text, "GT2005WalkingEngine: Robot recognised: " << macAddress);
    00199
    00200   loadOdometryPlane(0, "od_200m.dat", d);
    00201   loadOdometryPlane(1, "od_100m.dat", d);
    00202   loadOdometryPlane(2, "od_50m.dat", d);
    00203   loadOdometryPlane(3, "od_0.dat", d);
    00204   loadOdometryPlane(4, "od_50.dat", d);
    00205   loadOdometryPlane(5, "od_100.dat", d);
    00206   loadOdometryPlane(6, "od_200.dat", d);
    00207   loadOdometryRotation(0, "od_r.dat", d);
    00208   loadOdometryRotation(1, "od_rg.dat", d);
    00209 }
    00210
    00211 Pose2D GT2005WalkingEngine::calculateOdometryCalibrationData(Pose2D request, WalkRequest::WalkType walkType)
    00212 {
    00213   Pose2D temp;
    00214   int i = 0;
    00215   int j = 0;
    00216   int k = 0;
    00217   bool oSet = false;
    00218
    00219   if ((fabs(request.rotation) > 3.4906) || ((request.translation.x == 0.0) && (request.translation.y == 0.0))) // old rotation calibration
    00220   {
    00221     // rotation stuff
    00222     if (walkType == WalkRequest::walkWithBall)
    00223     {
    00224       for (i = 1; i < 21; i++)
    00225       {
    00226         if (request.rotation <= odometryCalibrationData2[1][i].controlled.rotation)
    00227         {
    00228           double dr = odometryCalibrationData2[1][i].controlled.rotation - odometryCalibrationData2[1][i-1].controlled.rotation;
    00229           double pr = request.rotation - odometryCalibrationData2[1][i-1].controlled.rotation;
    00230
    00231           //temp.translation.x = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.translation.x + (pr / dr) * odometryCalibrationData2[1][i].measured.translation.x;
    00232           //temp.translation.y = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.translation.y + (pr / dr) * odometryCalibrationData2[1][i].measured.translation.y;
    00233           temp.rotation = (1 - (pr / dr)) * odometryCalibrationData2[1][i-1].measured.rotation + (pr / dr) * odometryCalibrationData2[1][i].measured.rotation;
    00234           oSet = true;
    00235           break;
    00236         }
    00237       }
    00238       if (oSet == false)
    00239       {
    00240         //temp.translation.x = odometryCalibrationData2[1][20].measured.translation.x;
    00241         //temp.translation.y = odometryCalibrationData2[1][20].measured.translation.y;
    00242         temp.rotation = odometryCalibrationData2[1][20].measured.rotation;
    00243       }
    00244       // blöde Berechnung, um ne reine Rotationsbewegung zu korrigieren
    00245       temp.rotation *= motionCycleTime;
    00246       temp.translation.y += sin(temp.rotation) * 80;
    00247       temp.rotation /= motionCycleTime;
    00248       temp.translation /= motionCycleTime;
    00249     }
    00250     else
    00251     {
    00252       for (i = 1; i < 21; i++)
    00253       {
    00254         if (request.rotation <= odometryCalibrationData2[0][i].controlled.rotation)
    00255         {
    00256           double dr = odometryCalibrationData2[0][i].controlled.rotation - odometryCalibrationData2[0][i-1].controlled.rotation;
    00257           double pr = request.rotation - odometryCalibrationData2[0][i-1].controlled.rotation;
    00258
    00259           //temp.translation.x = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.translation.x + (pr / dr) * odometryCalibrationData2[0][i].measured.translation.x;
    00260           //temp.translation.y = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.translation.y + (pr / dr) * odometryCalibrationData2[0][i].measured.translation.y;
    00261           temp.rotation = (1 - (pr / dr)) * odometryCalibrationData2[0][i-1].measured.rotation + (pr / dr) * odometryCalibrationData2[0][i].measured.rotation;
    00262           oSet = true;
    00263           break;
    00264         }
    00265       }
    00266       if (oSet == false)
    00267       {
    00268         //temp.translation.x = odometryCalibrationData2[0][20].measured.translation.x;
    00269         //temp.translation.y = odometryCalibrationData2[0][20].measured.translation.y;
    00270         temp.rotation = odometryCalibrationData2[0][20].measured.rotation;
    00271       }
    00272       // blöde Berechnung, um ne reine Rotationsbewegung zu korrigieren
    00273       temp.rotation *= motionCycleTime;
    00274       temp.translation.y += sin(temp.rotation) * 80;
    00275       temp.rotation /= motionCycleTime;
    00276       temp.translation /= motionCycleTime;
    00277     }
    00278     return temp;
    00279   }
    00280   else
    00281   {
    00282     oSet = false;
    00283     // translation stuff
    00284     for (i = 1; i < 15; i++)
    00285     {
    00286       if (request.translation.x <= odometryCalibrationData[i][0][0].controlled.translation.x)
    00287       {
    00288         double dx = odometryCalibrationData[i][0][0].controlled.translation.x - odometryCalibrationData[i-1][0][0].controlled.translation.x;
    00289         double px = request.translation.x - odometryCalibrationData[i-1][0][0].controlled.translation.x;
    00290         for (j = 1; j < 13; j++)
    00291         {
    00292           if (request.translation.y <= odometryCalibrationData[i][j][0].controlled.translation.y)
    00293           {
    00294             // lengths of current segments
    00295             double dy = odometryCalibrationData[i][j][0].controlled.translation.y - odometryCalibrationData[i][j-1][0].controlled.translation.y;
    00296             // positions in current segments
    00297             double py = request.translation.y - odometryCalibrationData[i][j-1][0].controlled.translation.y;
    00298
    00299             for (k = 1; k < 7; k++)
    00300             {
    00301               // lengths of current segments
    00302               double dr = odometryCalibrationData[i][j][k].controlled.rotation - odometryCalibrationData[i][j][k-1].controlled.rotation;
    00303               // positions in current segments
    00304               double pr = request.rotation - odometryCalibrationData[i][j][k-1].controlled.rotation;
    00305
    00306               if (request.rotation <= odometryCalibrationData[i][j][k].controlled.rotation)
    00307               {
    00308                 double wx = px / dx;
    00309                 double wy = py / dy;
    00310                 double wr = pr / dr;
    00311
    00312                 // linear interpolation
    00313                 temp.translation.x = (1 - wr) * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k-1].measured.translation.x + wx * odometryCalibrationData[i][j-1][k-1].measured.translation.x)
    00314                   + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k-1].measured.translation.x + wx * odometryCalibrationData[i][j][k-1].measured.translation.x))
    00315                   + wr * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k].measured.translation.x + wx * odometryCalibrationData[i][j-1][k].measured.translation.x)
    00316                   + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k].measured.translation.x + wx * odometryCalibrationData[i][j][k].measured.translation.x));
    00317
    00318                 temp.translation.y = (1 - wr) * ((1 - wx) * ((1 - wy) * odometryCalibrationData[i-1][j-1][k-1].measured.translation.y + wy * odometryCalibrationData[i-1][j][k-1].measured.translation.y)
    00319                   + wx * ((1 - wy) * odometryCalibrationData[i][j-1][k-1].measured.translation.y + wy * odometryCalibrationData[i][j][k-1].measured.translation.y))
    00320                   + wr * ((1 - wx) * ((1 - wy) * odometryCalibrationData[i-1][j-1][k].measured.translation.y + wy * odometryCalibrationData[i-1][j][k].measured.translation.y)
    00321                   + wx * ((1 - wy) * odometryCalibrationData[i][j-1][k].measured.translation.y + wy * odometryCalibrationData[i][j][k].measured.translation.y));
    00322
    00323                 temp.rotation = (1 - wr) * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k-1].measured.rotation + wx * odometryCalibrationData[i][j-1][k-1].measured.rotation)
    00324                   + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k-1].measured.rotation + wx * odometryCalibrationData[i][j][k-1].measured.rotation))
    00325                   + wr * ((1 - wy) * ((1 - wx) * odometryCalibrationData[i-1][j-1][k].measured.rotation + wx * odometryCalibrationData[i][j-1][k].measured.rotation)
    00326                   + wy * ((1 - wx) * odometryCalibrationData[i-1][j][k].measured.rotation + wx * odometryCalibrationData[i][j][k].measured.rotation));
    00327                 oSet = true;
    00328                 return temp;
    00329               }
    00330             }
    00331           }
    00332         }
    00333       }
    00334     }
    00335     if (oSet == false)
    00336     {
    00337       temp.translation.x = odometryCalibrationData[14][12][6].measured.translation.x;
    00338       temp.translation.y = odometryCalibrationData[14][12][6].measured.translation.y;
    00339       temp.rotation = odometryCalibrationData[14][12][6].measured.rotation;
    00340     }
    00341   }
    00342
    00343   return Pose2D(0.0, 0.0, 0.0);
    00344 }
    00345
    00346 bool GT2005WalkingEngine::executeParameterized(JointData& jointData, const WalkRequest& walkRequest, double positionInWalkingCycle)
    00347 {
    00348   // set PID values
    00349   pidData.setLegFJ1Values(38, 8, 1);
    00350   pidData.setLegHJ1Values(38, 8, 1);
    00351   pidData.setLegFJ2Values(30, 4, 1);
    00352   pidData.setLegHJ2Values(30, 4, 1);
    00353   pidData.setLegFJ3Values(40, 8, 1);
    00354   pidData.setLegHJ3Values(40, 8, 1);
    00355
    00356   // evolution message handling
    00357   switch (gt2005Parameters.bMessage)
    00358   {
    00359   case 0: // reset special mode for rotation
    00360     {
    00361       currentFIPSParameters->specialMode = false;
    00362       gt2005DebugData.gt2005Status = 0;
    00363       polygonsRecieved = 0;
    00364       break;
    00365     }
    00366   case 1: // integrate polygons send from evolution
    00367     {
    00368       if (polygonsRecieved == 0)
    00369       {
    00370         delete currentFIPSParameters->evoPoly[Leg_FR];
    00371         delete currentFIPSParameters->evoPoly[Leg_FL];
    00372         delete currentFIPSParameters->evoPoly[Leg_HR];
    00373         delete currentFIPSParameters->evoPoly[Leg_HL];
    00374
    00375         currentFIPSParameters->evoPoly[Leg_FR] = gt2005Parameters.walkingParameters.evoPoly[Leg_FR]->copy();
    00376         currentFIPSParameters->evoPoly[Leg_FL] = gt2005Parameters.walkingParameters.evoPoly[Leg_FL]->copy();
    00377         currentFIPSParameters->evoPoly[Leg_HR] = gt2005Parameters.walkingParameters.evoPoly[Leg_HR]->copy();
    00378         currentFIPSParameters->evoPoly[Leg_HL] = gt2005Parameters.walkingParameters.evoPoly[Leg_HL]->copy();
    00379         polygonsRecieved = 1;
    00380         gt2005DebugData.gt2005Status = 1; // signalize "polygons recieved" to behaviour
    00381         //OUTPUT(idText, text, "GT2005WalkingEngine: Polygons recieved.");
    00382       }
    00383
    00384       break;
    00385     }
    00386   case 2: // switch to special mode for rotation
    00387     {
    00388       currentFIPSParameters->specialMode = true;
    00389       break;
    00390     }
    00391   default:
    00392     {
    00393       gt2005DebugData.gt2005Status = 0;
    00394       break;
    00395     }
    00396   }
    00397
    00398   // actualize leg phases
    00399   footPhase[Leg_FR] = currentFIPSParameters->footPhase[Leg_FR];
    00400   footPhase[Leg_FL] = currentFIPSParameters->footPhase[Leg_FL];
    00401   footPhase[Leg_HR] = currentFIPSParameters->footPhase[Leg_HR];
    00402   footPhase[Leg_HL] = currentFIPSParameters->footPhase[Leg_HL];
    00403
    00404   // adjust controled speed to fit real speed
    00405   Pose2D request = walkRequest.walkParams;
    00406   request.translation.x /= 1.5; // 0..450 -> 0..300
    00407   request.translation.y /= 1.5; // 0..450 -> 0..300
    00408   request.rotation *= 1.5; // 0..200 -> 0..300
    00409   //currentRequest = request;
    00410   smoothMotionRequest(request, currentRequest);
    00411
    00412   // odometry correction
    00413   //currentRequest = MSH2005WalkCalibration::calibrateRequest(currentRequest);
    00414
    00415   // clip values
    00416   if (currentRequest.translation.x > 300.0) currentRequest.translation.x = 300.0;
    00417   else if (currentRequest.translation.x < -300.0) currentRequest.translation.x = -300.0;
    00418   if (currentRequest.translation.y > 300.0) currentRequest.translation.y = 300.0;
    00419   else if (currentRequest.translation.y < -300.0) currentRequest.translation.y = -300.0;
    00420   if (currentRequest.rotation > 5.236) currentRequest.rotation = 5.236;
    00421   else if (currentRequest.rotation < -5.236) currentRequest.rotation = -5.236;
    00422
    00423   standing = (currentRequest.translation.abs() < 10 && fabs(currentRequest.rotation) < 0.05);
    00424
    00425   // calculate current polygons and set current stepLength
    00426   delete currentPoly[Leg_FR]; delete currentPoly[Leg_FL]; delete currentPoly[Leg_HR]; delete currentPoly[Leg_HL];
    00427   currentPoly[Leg_FR] = currentFIPSParameters->calculateCurrentPoly(Leg_FR, currentRequest, walkRequest.walkType);
    00428   currentPoly[Leg_FL] = currentFIPSParameters->calculateCurrentPoly(Leg_FL, currentRequest, walkRequest.walkType);
    00429   currentPoly[Leg_HR] = currentFIPSParameters->calculateCurrentPoly(Leg_HR, currentRequest, walkRequest.walkType);
    00430   currentPoly[Leg_HL] = currentFIPSParameters->calculateCurrentPoly(Leg_HL, currentRequest, walkRequest.walkType);
    00431   currentFIPSParameters->stepLength = currentPoly[Leg_FR]->stepLength;
    00432
    00433   // calculate current position in walk cycle if walking
    00434   if (standing)
    00435   {
    00436     footPhase[Leg_FR] = footPhase[Leg_FL] = footPhase[Leg_HR] = footPhase[Leg_HL] = 0;
    00437
    00438     // end current walk cycle
    00439     currentStepPercentage = positionInWalkingCycle;
    00440     if (currentStepPercentage != 0.0)
    00441     {
    00442       currentStepPercentage += 1.0 / (double)currentFIPSParameters->stepLength;
    00443       currentStep++;
    00444       if (currentStepPercentage >= 1.0)
    00445       {
    00446         currentStepPercentage -= 1.0;
    00447         currentStep = 0;
    00448       }
    00449     }
    00450   }
    00451   else
    00452   {
    00453     currentStepPercentage = positionInWalkingCycle;
    00454     currentStepPercentage += 1.0 / (double)currentFIPSParameters->stepLength;
    00455     currentStep++;
    00456     if (currentStepPercentage >= 1.0)
    00457     {
    00458       currentStepPercentage -= 1.0;
    00459       currentStep = 0;
    00460
    00461       // switch debugging mode
    00462       if (debugRequest && (debugging == false))
    00463       {
    00464         debugging = true;
    00465         OUTPUT(idText, text, "GT2005WalkingEngine: Debugging started.");
    00466       }
    00467       else if ((debugRequest == false) && debugging)
    00468       {
    00469         debugging = false;
    00470         streamDebugData();
    00471         OUTPUT(idText, text, "GT2005WalkingEngine: Debugging stopped.");
    00472       }
    00473
    00474       if (resampleRequest && (resampling == false))
    00475       {
    00476         resampling = true;
    00477         OUTPUT(idText, text, "GT2005WalkingEngine: Resampling started.");
    00478       }
    00479       else if ((resampleRequest == false) && resampling)
    00480       {
    00481         resampling = false;
    00482         streamDebugData();
    00483         OUTPUT(idText, text, "GT2005WalkingEngine: Resampling stopped.");
    00484       }
    00485     }
    00486   }
    00487
    00488   // fake odometry ;)
    00489   odometry = calculateOdometryCalibrationData(currentRequest, walkRequest.walkType);
    00490   //OUTPUT(idText, text, "tr: " << odometry.translation.y << "  rot: " << odometry.rotation);
    00491   //odometry = request; // original request
    00492
    00493   //convert odometry from mm/s to mm/frame
    00494   odometry.translation *= motionCycleTime;
    00495   //convert odometry from rad/s to rad/frame
    00496   odometry.rotation *= motionCycleTime;
    00497   //correction for rotation center = foot middle instead of neck:
    00498   //odometry.translation.y += sin(odometry.rotation) * 75;
    00499   odometryData.conc(odometry);
    00500
    00501   // calculate body tilt (funzt nur wenn die footZeroPositions für links und rechts gleich sind)
    00502   bodyTilt = asin((fabs(currentFIPSParameters->footZeroPosition[Leg_HR].z) - fabs(currentFIPSParameters->footZeroPosition[Leg_FR].z)) / lengthBetweenLegs);
    00503
    00504   // calculate foot positions
    00505   calculateData(jointData);
    00506
    00507   // set motion info stuff
    00508   motionInfo.neckHeight = neckHeight;
    00509   motionInfo.motionIsStable = true;
    00510   motionInfo.bodyTilt = bodyTilt + getRobotConfiguration().getRobotCalibration().bodyTiltOffset;
    00511   motionInfo.executedMotionRequest.motionType = MotionRequest::walk;
    00512   motionInfo.executedMotionRequest.walkRequest.walkType = walkRequest.walkType;
    00513   motionInfo.executedMotionRequest.walkRequest.walkParams = currentRequest;
    00514   motionInfo.positionInWalkCycle = currentStepPercentage;
    00515
    00516   /*/ override head control mode
    00517   if (currentFIPSParameters->specialMode)
    00518   {
    00519   //pidData.setValues(JointData::neckTilt, 0x0A, 0x04, 0x02);
    00520   jointData.data[JointData::headTilt] = toMicroRad(fromDegrees(0.0));
    00521   jointData.data[JointData::headPan] = toMicroRad(fromDegrees(0.0));
    00522   jointData.data[JointData::neckTilt] = toMicroRad(fromDegrees(0.0));
    00523   jointData.data[JointData::mouth] = toMicroRad(fromDegrees(0.0));
    00524   }
    00525   else
    00526   {
    00527   //pidData.setValues(JointData::neckTilt, 0x00, 0x00, 0x00);
    00528   jointData.data[JointData::headTilt] = toMicroRad(fromDegrees(50.0));
    00529   jointData.data[JointData::headPan] = toMicroRad(fromDegrees(0.0));
    00530   jointData.data[JointData::neckTilt] = toMicroRad(fromDegrees(-60.0));
    00531   jointData.data[JointData::mouth] = toMicroRad(fromDegrees(-55.0));
    00532   }//*/
    00533
    00534   return false;
    00535 }
    00536
    00537 void GT2005WalkingEngine::calculateData(JointData &j)
    00538 {
    00539   double j1,j2,j3;
    00540   double shapeOffset[4];
    00541   double a[4];
    00542   double vLen[4];
    00543   double speedScaleFactor[4];
    00544
    00545   //head, mouth
    00546   j.data[JointData::neckTilt] = currentParameters->headTilt;
    00547   j.data[JointData::headPan] = currentParameters->headPan;
    00548   j.data[JointData::headTilt] = currentParameters->headRoll;
    00549   j.data[JointData::mouth] = currentParameters->mouth;
    00550
    00551   // ears, tail
    00552   j.data[JointData::earL] =j.data[JointData::earR] = j.data[JointData::tailTilt] = j.data[JointData::tailPan] = jointDataInvalidValue;
    00553
    00554   // get foot positions
    00555   Vector3<double> footPos[4];
    00556   footPos[Leg_FR] = currentFIPSParameters->footZeroPosition[Leg_FR];
    00557   footPos[Leg_FL] = currentFIPSParameters->footZeroPosition[Leg_FL];
    00558   footPos[Leg_HR] = currentFIPSParameters->footZeroPosition[Leg_HR];
    00559   footPos[Leg_HL] = currentFIPSParameters->footZeroPosition[Leg_HL];
    00560
    00561   if (standing == false) // if moving-
    00562   {
    00563     // calculate shapeOffsets
    00564     shapeOffset[Leg_FR] = currentStepPercentage + footPhase[Leg_FR];
    00565     shapeOffset[Leg_FL] = currentStepPercentage + footPhase[Leg_FL];
    00566     shapeOffset[Leg_HR] = currentStepPercentage + footPhase[Leg_HR];
    00567     shapeOffset[Leg_HL] = currentStepPercentage + footPhase[Leg_HL];
    00568     for (int i = 0; i < 4; i++)
    00569     {
    00570       if (shapeOffset[i] > 1)
    00571       {
    00572         shapeOffset[i] -= 1;
    00573       }
    00574     }
    00575
    00576     // define rotation vectors
    00577     double rotLen = currentPoly[Leg_FR]->polyLengthX;
    00578     Vector3<double> footRotation[4];
    00579     if (currentRequest.rotation < 0) // rotate counterclockwise
    00580     {
    00581       footRotation[Leg_FR].x = -rotLen; footRotation[Leg_FR].y = -rotLen; footRotation[Leg_FR].z = 0;
    00582       footRotation[Leg_FL].x = rotLen;  footRotation[Leg_FL].y = -rotLen; footRotation[Leg_FL].z = 0;
    00583       footRotation[Leg_HR].x = -rotLen; footRotation[Leg_HR].y = rotLen;  footRotation[Leg_HR].z = 0;
    00584       footRotation[Leg_HL].x = rotLen;  footRotation[Leg_HL].y = rotLen;  footRotation[Leg_HL].z = 0;
    00585     }
    00586     else // rotate clockwise
    00587     {
    00588       footRotation[Leg_FR].x = rotLen;  footRotation[Leg_FR].y = rotLen;  footRotation[Leg_FR].z = 0;
    00589       footRotation[Leg_FL].x = -rotLen; footRotation[Leg_FL].y = rotLen;  footRotation[Leg_FL].z = 0;
    00590       footRotation[Leg_HR].x = rotLen;  footRotation[Leg_HR].y = -rotLen; footRotation[Leg_HR].z = 0;
    00591       footRotation[Leg_HL].x = -rotLen; footRotation[Leg_HL].y = -rotLen; footRotation[Leg_HL].z = 0;
    00592     }
    00593
    00594     // define translation vector
    00595     Vector3<double> v;
    00596     v.x = currentRequest.translation.x;
    00597     v.y = currentRequest.translation.y;
    00598     v.z = 0;
    00599
    00600     // calculate robot rotation scale factor
    00601     double maxRot = 1.0; // maximum rotation (sorry geschätzt!!!)
    00602     double rsf = currentRequest.rotation / maxRot;
    00603     if (rsf < 0)
    00604     {
    00605       rsf *= -1;
    00606     }
    00607
    00608     // scale rotation vectors
    00609     footRotation[Leg_FR] *= rsf;
    00610     footRotation[Leg_FL] *= rsf;
    00611     footRotation[Leg_HR] *= rsf;
    00612     footRotation[Leg_HL] *= rsf;
    00613
    00614     // put translation vectors on rotation vectors
    00615     footRotation[Leg_FR] += v;
    00616     footRotation[Leg_FL] += v;
    00617     footRotation[Leg_HR] += v;
    00618     footRotation[Leg_HL] += v;
    00619
    00620     // calculate directions of the combined translation+rotation vectors
    00621     a[Leg_FR] = atan2(footRotation[Leg_FR].y, footRotation[Leg_FR].x);
    00622     a[Leg_FL] = atan2(footRotation[Leg_FL].y, footRotation[Leg_FL].x);
    00623     a[Leg_HR] = atan2(footRotation[Leg_HR].y, footRotation[Leg_HR].x);
    00624     a[Leg_HL] = atan2(footRotation[Leg_HL].y, footRotation[Leg_HL].x);
    00625
    00626     // calculeate length of the combined translation+rotation vectors
    00627     vLen[Leg_FR] = sqrt(footRotation[Leg_FR].x * footRotation[Leg_FR].x + footRotation[Leg_FR].y * footRotation[Leg_FR].y);
    00628     vLen[Leg_FL] = sqrt(footRotation[Leg_FL].x * footRotation[Leg_FL].x + footRotation[Leg_FL].y * footRotation[Leg_FL].y);
    00629     vLen[Leg_HR] = sqrt(footRotation[Leg_HR].x * footRotation[Leg_HR].x + footRotation[Leg_HR].y * footRotation[Leg_HR].y);
    00630     vLen[Leg_HL] = sqrt(footRotation[Leg_HL].x * footRotation[Leg_HL].x + footRotation[Leg_HL].y * footRotation[Leg_HL].y);
    00631
    00632     // calculate resulting speed scale factor
    00633     speedScaleFactor[0] = speedScaleFactor[1] = speedScaleFactor[2] = speedScaleFactor[3] = 1;
    00634     speedScaleFactor[Leg_FR] = vLen[Leg_FR] / (currentPoly[Leg_FR]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_FR]->groundTime));
    00635     speedScaleFactor[Leg_FL] = vLen[Leg_FL] / (currentPoly[Leg_FL]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_FL]->groundTime));
    00636     speedScaleFactor[Leg_HR] = vLen[Leg_HR] / (currentPoly[Leg_HR]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_HR]->groundTime));
    00637     speedScaleFactor[Leg_HL] = vLen[Leg_HL] / (currentPoly[Leg_HL]->polyLengthX / (currentFIPSParameters->stepLength * motionCycleTime * currentPoly[Leg_HL]->groundTime));
    00638
    00639     // calculate foot positions on polygon (use zero positions for standing)
    00640     footPos[Leg_FR] += currentPoly[Leg_FR]->scaleX(speedScaleFactor[Leg_FR]).rotateZC(a[Leg_FR]).getPositionOnShape(shapeOffset[Leg_FR]);
    00641     footPos[Leg_FL] += currentPoly[Leg_FL]->scaleX(speedScaleFactor[Leg_FL]).rotateZC(a[Leg_FL]).getPositionOnShape(shapeOffset[Leg_FL]);
    00642     footPos[Leg_HR] += currentPoly[Leg_HR]->scaleX(speedScaleFactor[Leg_HR]).rotateZC(a[Leg_HR]).getPositionOnShape(shapeOffset[Leg_HR]);
    00643     footPos[Leg_HL] += currentPoly[Leg_HL]->scaleX(speedScaleFactor[Leg_HL]).rotateZC(a[Leg_HL]).getPositionOnShape(shapeOffset[Leg_HL]);
    00644   }
    00645
    00646   // calculate leg angles from foot positions with inverse kinematics and set joint data
    00647   for (int leg = 0; leg < 4; leg++)
    00648   {
    00649     if (Kinematics::jointsFromLegPosition((Kinematics::LegIndex)leg, footPos[leg].x, footPos[leg].y, footPos[leg].z, j1, j2, j3, bodyTilt))
    00650     {
    00651       j.data[JointData::legFR1+leg*3] = toMicroRad(j1);
    00652       j.data[JointData::legFR2+leg*3] = toMicroRad(j2);
    00653       j.data[JointData::legFR3+leg*3] = toMicroRad(j3);
    00654     }
    00655     else
    00656     {
    00657       j.data[JointData::tailTilt] = -300000;
    00658     }
    00659   }
    00660
    00661   if (debugging)
    00662   {
    00663     readDebugData(speedScaleFactor, a, shapeOffset, j);
    00664   }
    00665   if (resampling)
    00666   {
    00667     readResampleData(speedScaleFactor, a, shapeOffset, j, footPos);
    00668   }
    00669 }
    00670
    00671 bool GT2005WalkingEngine::handleMessage(InMessage& message)
    00672 {
    00673   switch(message.getMessageID())
    00674   {
    00675   case idGT2005Request:
    00676     {
    00677       int actionID;
    00678       message.bin >> actionID;
    00679
    00680       switch(actionID)
    00681       {
    00682       case 0:
    00683         {
    00684           // request debug data
    00685           debugRequest = true;
    00686           break;
    00687         }
    00688       case 1:
    00689         {
    00690           // request resample data
    00691           resampleRequest = true;
    00692           break;
    00693         }
    00694       case 2: // load recieved 4 polygons into a specific polygon set in the walking engine
    00695         {
    00696           int polyID;
    00697           message.bin >> polyID;
    00698           loadPolys(polyID, message);
    00699           OUTPUT(idText, text, "Polygons loaded in polyID: " << polyID);
    00700           break;
    00701         }
    00702       case 3: // send 4 polygons from a specific polygon set in the walking engine
    00703         {
    00704           int polyID;
    00705           message.bin >> polyID;
    00706           sendPolys(polyID);
    00707           OUTPUT(idText, text, "Polygons send from polyID: " << polyID);
    00708           break;
    00709         }
    00710       }
    00711       OUTPUT(idText, text, "Request successful with actionID: " << actionID);
    00712       return true;
    00713     }
    00714   }
    00715   return false;
    00716 }
    00717
    00718 void GT2005WalkingEngine::loadPolys(int polyID, InMessage& message)
    00719 {
    00720   switch(polyID)
    00721   {
    00722   case 0: //evoPoly
    00723     {
    00724       message.bin >> *currentFIPSParameters->evoPoly[Leg_FR];
    00725       message.bin >> *currentFIPSParameters->evoPoly[Leg_FL];
    00726       message.bin >> *currentFIPSParameters->evoPoly[Leg_HR];
    00727       message.bin >> *currentFIPSParameters->evoPoly[Leg_HL];
    00728       break;
    00729     }
    00730   case 1: //omniPoly
    00731     {
    00732       message.bin >> *currentFIPSParameters->omniPoly[Leg_FR];
    00733       message.bin >> *currentFIPSParameters->omniPoly[Leg_FL];
    00734       message.bin >> *currentFIPSParameters->omniPoly[Leg_HR];
    00735       message.bin >> *currentFIPSParameters->omniPoly[Leg_HL];
    00736       break;
    00737     }
    00738   case 2: //rotPoly
    00739     {
    00740       message.bin >> *currentFIPSParameters->rotPoly[Leg_FR];
    00741       message.bin >> *currentFIPSParameters->rotPoly[Leg_FL];
    00742       message.bin >> *currentFIPSParameters->rotPoly[Leg_HR];
    00743       message.bin >> *currentFIPSParameters->rotPoly[Leg_HL];
    00744       break;
    00745     }
    00746   case 3: //sforwardPoly
    00747     {
    00748       message.bin >> *currentFIPSParameters->sforwardPoly[Leg_FR];
    00749       message.bin >> *currentFIPSParameters->sforwardPoly[Leg_FL];
    00750       message.bin >> *currentFIPSParameters->sforwardPoly[Leg_HR];
    00751       message.bin >> *currentFIPSParameters->sforwardPoly[Leg_HL];
    00752       break;
    00753     }
    00754   case 4: //forwardPoly
    00755     {
    00756       message.bin >> *currentFIPSParameters->forwardPoly[Leg_FR];
    00757       message.bin >> *currentFIPSParameters->forwardPoly[Leg_FL];
    00758       message.bin >> *currentFIPSParameters->forwardPoly[Leg_HR];
    00759       message.bin >> *currentFIPSParameters->forwardPoly[Leg_HL];
    00760       break;
    00761     }
    00762   case 5: //dforwardPoly
    00763     {
    00764       message.bin >> *currentFIPSParameters->dforwardPoly[Leg_FR];
    00765       message.bin >> *currentFIPSParameters->dforwardPoly[Leg_FL];
    00766       message.bin >> *currentFIPSParameters->dforwardPoly[Leg_HR];
    00767       message.bin >> *currentFIPSParameters->dforwardPoly[Leg_HL];
    00768       break;
    00769     }
    00770   case 6: //sbackwardPoly
    00771     {
    00772       message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_FR];
    00773       message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_FL];
    00774       message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_HR];
    00775       message.bin >> *currentFIPSParameters->sbackwardPoly[Leg_HL];
    00776       break;
    00777     }
    00778   case 7: //backwardPoly
    00779     {
    00780       message.bin >> *currentFIPSParameters->backwardPoly[Leg_FR];
    00781       message.bin >> *currentFIPSParameters->backwardPoly[Leg_FL];
    00782       message.bin >> *currentFIPSParameters->backwardPoly[Leg_HR];
    00783       message.bin >> *currentFIPSParameters->backwardPoly[Leg_HL];
    00784       break;
    00785     }
    00786   case 8: //dbackwardPoly
    00787     {
    00788       message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_FR];
    00789       message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_FL];
    00790       message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_HR];
    00791       message.bin >> *currentFIPSParameters->dbackwardPoly[Leg_HL];
    00792       break;
    00793     }
    00794   case 9: //sidewardsPoly
    00795     {
    00796       message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_FR];
    00797       message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_FL];
    00798       message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_HR];
    00799       message.bin >> *currentFIPSParameters->sidewardsPoly[Leg_HL];
    00800       break;
    00801     }
    00802   case 10: //boostPoly
    00803     {
    00804       message.bin >> *currentFIPSParameters->boostPoly[Leg_FR];
    00805       message.bin >> *currentFIPSParameters->boostPoly[Leg_FL];
    00806       message.bin >> *currentFIPSParameters->boostPoly[Leg_HR];
    00807       message.bin >> *currentFIPSParameters->boostPoly[Leg_HL];
    00808       break;
    00809     }
    00810   case 11: //grabPoly
    00811     {
    00812       message.bin >> *currentFIPSParameters->grabPoly[Leg_FR];
    00813       message.bin >> *currentFIPSParameters->grabPoly[Leg_FL];
    00814       message.bin >> *currentFIPSParameters->grabPoly[Leg_HR];
    00815       message.bin >> *currentFIPSParameters->grabPoly[Leg_HL];
    00816       break;
    00817     }
    00818   case 12: //rgrabPoly
    00819     {
    00820       message.bin >> *currentFIPSParameters->rgrabPoly[Leg_FR];
    00821       message.bin >> *currentFIPSParameters->rgrabPoly[Leg_FL];
    00822       message.bin >> *currentFIPSParameters->rgrabPoly[Leg_HR];
    00823       message.bin >> *currentFIPSParameters->rgrabPoly[Leg_HL];
    00824       break;
    00825     }
    00826   }
    00827 }
    00828
    00829 void GT2005WalkingEngine::sendPolys(int polyID)
    00830 {
    00831   // Get debug stream
    00832   Out& out = getDebugOut().bin;
    00833
    00834   int dataID = 1;
    00835   // stream data
    00836   out << dataID;
    00837
    00838   out << polyID;
    00839   switch(polyID)
    00840   {
    00841   case 0: //evoPoly
    00842     {
    00843       out << *currentFIPSParameters->evoPoly[Leg_FR];
    00844       out << *currentFIPSParameters->evoPoly[Leg_FL];
    00845       out << *currentFIPSParameters->evoPoly[Leg_HR];
    00846       out << *currentFIPSParameters->evoPoly[Leg_HL];
    00847       break;
    00848     }
    00849   case 1: //omniPoly
    00850     {
    00851       out << *currentFIPSParameters->omniPoly[Leg_FR];
    00852       out << *currentFIPSParameters->omniPoly[Leg_FL];
    00853       out << *currentFIPSParameters->omniPoly[Leg_HR];
    00854       out << *currentFIPSParameters->omniPoly[Leg_HL];
    00855       break;
    00856     }
    00857   case 2: //rotPoly
    00858     {
    00859       out << *currentFIPSParameters->rotPoly[Leg_FR];
    00860       out << *currentFIPSParameters->rotPoly[Leg_FL];
    00861       out << *currentFIPSParameters->rotPoly[Leg_HR];
    00862       out << *currentFIPSParameters->rotPoly[Leg_HL];
    00863       break;
    00864     }
    00865   case 3: //sforwardPoly
    00866     {
    00867       out << *currentFIPSParameters->sforwardPoly[Leg_FR];
    00868       out << *currentFIPSParameters->sforwardPoly[Leg_FL];
    00869       out << *currentFIPSParameters->sforwardPoly[Leg_HR];
    00870       out << *currentFIPSParameters->sforwardPoly[Leg_HL];
    00871       break;
    00872     }
    00873   case 4: //forwardPoly
    00874     {
    00875       out << *currentFIPSParameters->forwardPoly[Leg_FR];
    00876       out << *currentFIPSParameters->forwardPoly[Leg_FL];
    00877       out << *currentFIPSParameters->forwardPoly[Leg_HR];
    00878       out << *currentFIPSParameters->forwardPoly[Leg_HL];
    00879       break;
    00880     }
    00881   case 5: //forwardPoly
    00882     {
    00883       out << *currentFIPSParameters->dforwardPoly[Leg_FR];
    00884       out << *currentFIPSParameters->dforwardPoly[Leg_FL];
    00885       out << *currentFIPSParameters->dforwardPoly[Leg_HR];
    00886       out << *currentFIPSParameters->dforwardPoly[Leg_HL];
    00887       break;
    00888     }
    00889   case 6: //sbackwardPoly
    00890     {
    00891       out << *currentFIPSParameters->sbackwardPoly[Leg_FR];
    00892       out << *currentFIPSParameters->sbackwardPoly[Leg_FL];
    00893       out << *currentFIPSParameters->sbackwardPoly[Leg_HR];
    00894       out << *currentFIPSParameters->sbackwardPoly[Leg_HL];
    00895       break;
    00896     }
    00897   case 7: //backwardPoly
    00898     {
    00899       out << *currentFIPSParameters->backwardPoly[Leg_FR];
    00900       out << *currentFIPSParameters->backwardPoly[Leg_FL];
    00901       out << *currentFIPSParameters->backwardPoly[Leg_HR];
    00902       out << *currentFIPSParameters->backwardPoly[Leg_HL];
    00903       break;
    00904     }
    00905   case 8: //backwardPoly
    00906     {
    00907       out << *currentFIPSParameters->dbackwardPoly[Leg_FR];
    00908       out << *currentFIPSParameters->dbackwardPoly[Leg_FL];
    00909       out << *currentFIPSParameters->dbackwardPoly[Leg_HR];
    00910       out << *currentFIPSParameters->dbackwardPoly[Leg_HL];
    00911       break;
    00912     }
    00913   case 9: //sidewardsPoly
    00914     {
    00915       out << *currentFIPSParameters->sidewardsPoly[Leg_FR];
    00916       out << *currentFIPSParameters->sidewardsPoly[Leg_FL];
    00917       out << *currentFIPSParameters->sidewardsPoly[Leg_HR];
    00918       out << *currentFIPSParameters->sidewardsPoly[Leg_HL];
    00919       break;
    00920     }
    00921   case 10: //boostPoly
    00922     {
    00923       out << *currentFIPSParameters->boostPoly[Leg_FR];
    00924       out << *currentFIPSParameters->boostPoly[Leg_FL];
    00925       out << *currentFIPSParameters->boostPoly[Leg_HR];
    00926       out << *currentFIPSParameters->boostPoly[Leg_HL];
    00927       break;
    00928     }
    00929   case 11: //grabPoly
    00930     {
    00931       out << *currentFIPSParameters->grabPoly[Leg_FR];
    00932       out << *currentFIPSParameters->grabPoly[Leg_FL];
    00933       out << *currentFIPSParameters->grabPoly[Leg_HR];
    00934       out << *currentFIPSParameters->grabPoly[Leg_HL];
    00935       break;
    00936     }
    00937   case 12: //rgrabPoly
    00938     {
    00939       out << *currentFIPSParameters->rgrabPoly[Leg_FR];
    00940       out << *currentFIPSParameters->rgrabPoly[Leg_FL];
    00941       out << *currentFIPSParameters->rgrabPoly[Leg_HR];
    00942       out << *currentFIPSParameters->rgrabPoly[Leg_HL];
    00943       break;
    00944     }
    00945   }
    00946
    00947   // Finish message
    00948   getDebugOut().finishMessage(idGT2005DebugData);
    00949 }
    00950
    00951 void GT2005WalkingEngine::smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest)
    00952 {
    00953   //this is done per frame, so maximum speed change is 3.2mm/s/frame = 400mm/s^2
    00954   //and 0.032rad/s/frame = 4rad/s^2:
    00955   if ((request.translation.x - currentRequest.translation.x) > 25)
    00956   {
    00957     currentRequest.translation.x += 25;
    00958   }
    00959   else if ((request.translation.x - currentRequest.translation.x) < -25)
    00960   {
    00961     currentRequest.translation.x -= 25;
    00962   }
    00963   else
    00964   {
    00965     currentRequest.translation.x = request.translation.x;
    00966   }
    00967
    00968   if ((request.translation.y - currentRequest.translation.y) > 25)
    00969   {
    00970     currentRequest.translation.y += 25;
    00971   }
    00972   else if ((request.translation.y - currentRequest.translation.y) < -25)
    00973   {
    00974     currentRequest.translation.y -= 25;
    00975   }
    00976   else
    00977   {
    00978     currentRequest.translation.y = request.translation.y;
    00979   }
    00980
    00981   if ((request.rotation - currentRequest.rotation) > 0.4)
    00982   {
    00983     currentRequest.rotation += 0.4;
    00984   }
    00985   else if ((request.rotation - currentRequest.rotation) < -0.4)
    00986   {
    00987     currentRequest.rotation -= 0.4;
    00988   }
    00989   else
    00990   {
    00991     currentRequest.rotation = request.rotation;
    00992   }
    00993 }
    00994
    00995 void GT2005WalkingEngine::readDebugData(double c_scale[4], double c_angle[4], double c_offset[4], JointData &j)
    00996 {
    00997   double x, y, z;
    00998   Vector3<double> v[4];
    00999   Vector3<double> s[4];
    01000
    01001   Vector3<double> real[4]; // corrected real vectors from sensor data
    01002   Vector3<double> ideal[4]; // corrected ideal vectors from sensor data
    01003
    01004   // define some hardcoded shoulder positions
    01005   Vector3<double> shoulderPosition[4];
    01006   shoulderPosition[Leg_FR].x = 0; shoulderPosition[Leg_FR].y = -67.2; shoulderPosition[Leg_FR].z = -19.5;
    01007   shoulderPosition[Leg_FL].x = 0; shoulderPosition[Leg_FL].y = 67.2;  shoulderPosition[Leg_FL].z = -19.5;
    01008   shoulderPosition[Leg_HR].x = -130;  shoulderPosition[Leg_HR].y = -67.2; shoulderPosition[Leg_HR].z = -19.5;
    01009   shoulderPosition[Leg_HL].x = -130;  shoulderPosition[Leg_HL].y = 67.2;  shoulderPosition[Leg_HL].z = -19.5;
    01010
    01011   //sensorDataBuffer.lastFrame().frameNumber
    01012
    01013   // read forced angles
    01014   // calculate position in space from joint angles
    01015   Kinematics::legPositionFromJoints(Kinematics::fr,
    01016     fromMicroRad(j.data[JointData::legFR1]),
    01017     fromMicroRad(j.data[JointData::legFR2]),
    01018     fromMicroRad(j.data[JointData::legFR3]),
    01019     x, y, z);
    01020   s[Leg_FR].x = x; s[Leg_FR].y = y - 67.2; s[Leg_FR].z = z;
    01021   // set zero point to footZeroPosition
    01022   s[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
    01023   // undo rotate
    01024   ideal[Leg_FR].x = s[Leg_FR].x * cos(-c_angle[Leg_FR]) - s[Leg_FR].y * sin(-c_angle[Leg_FR]);
    01025   ideal[Leg_FR].y = s[Leg_FR].x * sin(-c_angle[Leg_FR]) + s[Leg_FR].y * cos(-c_angle[Leg_FR]);
    01026   ideal[Leg_FR].z = s[Leg_FR].z;
    01027   // undo x-scale
    01028   ideal[Leg_FR].x = ideal[Leg_FR].x / c_scale[Leg_FR];
    01029   // transform back to foot position
    01030   ideal[Leg_FR] += currentFIPSParameters->footZeroPosition[Leg_FR];
    01031   //ideal[Leg_FR] += shoulderPosition[Leg_FR];
    01032
    01033   Kinematics::legPositionFromJoints(Kinematics::fl,
    01034     fromMicroRad(j.data[JointData::legFL1]),
    01035     fromMicroRad(j.data[JointData::legFL2]),
    01036     fromMicroRad(j.data[JointData::legFL3]),
    01037     x, y, z);
    01038   s[Leg_FL].x = x; s[Leg_FL].y = y + 67.2; s[Leg_FL].z = z;
    01039   // set zero point to footZeroPosition
    01040   s[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
    01041   // undo rotate
    01042   ideal[Leg_FL].x = s[Leg_FL].x * cos(-c_angle[Leg_FL]) - s[Leg_FL].y * sin(-c_angle[Leg_FL]);
    01043   ideal[Leg_FL].y = s[Leg_FL].x * sin(-c_angle[Leg_FL]) + s[Leg_FL].y * cos(-c_angle[Leg_FL]);
    01044   ideal[Leg_FL].z = s[Leg_FL].z;
    01045   // undo x-scale
    01046   ideal[Leg_FL].x = ideal[Leg_FL].x / c_scale[Leg_FL];
    01047   // transform back to foot position
    01048   ideal[Leg_FL] += currentFIPSParameters->footZeroPosition[Leg_FL];
    01049   //ideal[Leg_FL] += shoulderPosition[Leg_FL];
    01050
    01051   Kinematics::legPositionFromJoints(Kinematics::hr,
    01052     fromMicroRad(j.data[JointData::legHR1]),
    01053     fromMicroRad(j.data[JointData::legHR2]),
    01054     fromMicroRad(j.data[JointData::legHR3]),
    01055     x, y, z);
    01056   s[Leg_HR].x = x; s[Leg_HR].y = y - 67.2; s[Leg_HR].z = z;
    01057   // set zero point to footZeroPosition
    01058   s[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
    01059   // undo rotate
    01060   ideal[Leg_HR].x = s[Leg_HR].x * cos(-c_angle[Leg_HR]) - s[Leg_HR].y * sin(-c_angle[Leg_HR]);
    01061   ideal[Leg_HR].y = s[Leg_HR].x * sin(-c_angle[Leg_HR]) + s[Leg_HR].y * cos(-c_angle[Leg_HR]);
    01062   ideal[Leg_HR].z = s[Leg_HR].z;
    01063   // undo x-scale
    01064   ideal[Leg_HR].x = ideal[Leg_HR].x / c_scale[Leg_HR];
    01065   // transform back to foot position
    01066   ideal[Leg_HR] += currentFIPSParameters->footZeroPosition[Leg_HR];
    01067   //ideal[Leg_HR] += shoulderPosition[Leg_HR];
    01068
    01069   Kinematics::legPositionFromJoints(Kinematics::hl,
    01070     fromMicroRad(j.data[JointData::legHL1]),
    01071     fromMicroRad(j.data[JointData::legHL2]),
    01072     fromMicroRad(j.data[JointData::legHL3]),
    01073     x, y, z);
    01074   s[Leg_HL].x = x; s[Leg_HL].y = y + 67.2; s[Leg_HL].z = z;
    01075   // set zero point to footZeroPosition
    01076   s[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
    01077   // undo rotate
    01078   ideal[Leg_HL].x = s[Leg_HL].x * cos(-c_angle[Leg_HL]) - s[Leg_HL].y * sin(-c_angle[Leg_HL]);
    01079   ideal[Leg_HL].y = s[Leg_HL].x * sin(-c_angle[Leg_HL]) + s[Leg_HL].y * cos(-c_angle[Leg_HL]);
    01080   ideal[Leg_HL].z = s[Leg_HL].z;
    01081   // undo x-scale
    01082   ideal[Leg_HL].x = ideal[Leg_HL].x / c_scale[Leg_HL];
    01083   // transform back to foot position
    01084   ideal[Leg_HL] += currentFIPSParameters->footZeroPosition[Leg_HL];
    01085   //ideal[Leg_HL] += shoulderPosition[Leg_HL];
    01086
    01087   // read real angles
    01088   int frameNumber = currentStep % 4;
    01089
    01090   if (frameNumber == 0)
    01091   {
    01092     debugDataBuffer[0][0] = sensorDataBuffer.frame[0].data[SensorData::legFR1];
    01093     debugDataBuffer[0][1] = sensorDataBuffer.frame[0].data[SensorData::legFR2];
    01094     debugDataBuffer[0][2] = sensorDataBuffer.frame[0].data[SensorData::legFR3];
    01095     debugDataBuffer[0][3] = sensorDataBuffer.frame[1].data[SensorData::legFR1];
    01096     debugDataBuffer[0][4] = sensorDataBuffer.frame[1].data[SensorData::legFR2];
    01097     debugDataBuffer[0][5] = sensorDataBuffer.frame[1].data[SensorData::legFR3];
    01098     debugDataBuffer[0][6] = sensorDataBuffer.frame[2].data[SensorData::legFR1];
    01099     debugDataBuffer[0][7] = sensorDataBuffer.frame[2].data[SensorData::legFR2];
    01100     debugDataBuffer[0][8] = sensorDataBuffer.frame[2].data[SensorData::legFR3];
    01101     debugDataBuffer[0][9] = sensorDataBuffer.frame[3].data[SensorData::legFR1];
    01102     debugDataBuffer[0][10] = sensorDataBuffer.frame[3].data[SensorData::legFR2];
    01103     debugDataBuffer[0][11] = sensorDataBuffer.frame[3].data[SensorData::legFR3];
    01104
    01105     debugDataBuffer[1][0] = sensorDataBuffer.frame[0].data[SensorData::legFL1];
    01106     debugDataBuffer[1][1] = sensorDataBuffer.frame[0].data[SensorData::legFL2];
    01107     debugDataBuffer[1][2] = sensorDataBuffer.frame[0].data[SensorData::legFL3];
    01108     debugDataBuffer[1][3] = sensorDataBuffer.frame[1].data[SensorData::legFL1];
    01109     debugDataBuffer[1][4] = sensorDataBuffer.frame[1].data[SensorData::legFL2];
    01110     debugDataBuffer[1][5] = sensorDataBuffer.frame[1].data[SensorData::legFL3];
    01111     debugDataBuffer[1][6] = sensorDataBuffer.frame[2].data[SensorData::legFL1];
    01112     debugDataBuffer[1][7] = sensorDataBuffer.frame[2].data[SensorData::legFL2];
    01113     debugDataBuffer[1][8] = sensorDataBuffer.frame[2].data[SensorData::legFL3];
    01114     debugDataBuffer[1][9] = sensorDataBuffer.frame[3].data[SensorData::legFL1];
    01115     debugDataBuffer[1][10] = sensorDataBuffer.frame[3].data[SensorData::legFL2];
    01116     debugDataBuffer[1][11] = sensorDataBuffer.frame[3].data[SensorData::legFL3];
    01117
    01118     debugDataBuffer[2][0] = sensorDataBuffer.frame[0].data[SensorData::legHR1];
    01119     debugDataBuffer[2][1] = sensorDataBuffer.frame[0].data[SensorData::legHR2];
    01120     debugDataBuffer[2][2] = sensorDataBuffer.frame[0].data[SensorData::legHR3];
    01121     debugDataBuffer[2][3] = sensorDataBuffer.frame[1].data[SensorData::legHR1];
    01122     debugDataBuffer[2][4] = sensorDataBuffer.frame[1].data[SensorData::legHR2];
    01123     debugDataBuffer[2][5] = sensorDataBuffer.frame[1].data[SensorData::legHR3];
    01124     debugDataBuffer[2][6] = sensorDataBuffer.frame[2].data[SensorData::legHR1];
    01125     debugDataBuffer[2][7] = sensorDataBuffer.frame[2].data[SensorData::legHR2];
    01126     debugDataBuffer[2][8] = sensorDataBuffer.frame[2].data[SensorData::legHR3];
    01127     debugDataBuffer[2][9] = sensorDataBuffer.frame[3].data[SensorData::legHR1];
    01128     debugDataBuffer[2][10] = sensorDataBuffer.frame[3].data[SensorData::legHR2];
    01129     debugDataBuffer[2][11] = sensorDataBuffer.frame[3].data[SensorData::legHR3];
    01130
    01131     debugDataBuffer[3][0] = sensorDataBuffer.frame[0].data[SensorData::legHL1];
    01132     debugDataBuffer[3][1] = sensorDataBuffer.frame[0].data[SensorData::legHL2];
    01133     debugDataBuffer[3][2] = sensorDataBuffer.frame[0].data[SensorData::legHL3];
    01134     debugDataBuffer[3][3] = sensorDataBuffer.frame[1].data[SensorData::legHL1];
    01135     debugDataBuffer[3][4] = sensorDataBuffer.frame[1].data[SensorData::legHL2];
    01136     debugDataBuffer[3][5] = sensorDataBuffer.frame[1].data[SensorData::legHL3];
    01137     debugDataBuffer[3][6] = sensorDataBuffer.frame[2].data[SensorData::legHL1];
    01138     debugDataBuffer[3][7] = sensorDataBuffer.frame[2].data[SensorData::legHL2];
    01139     debugDataBuffer[3][8] = sensorDataBuffer.frame[2].data[SensorData::legHL3];
    01140     debugDataBuffer[3][9] = sensorDataBuffer.frame[3].data[SensorData::legHL1];
    01141     debugDataBuffer[3][10] = sensorDataBuffer.frame[3].data[SensorData::legHL2];
    01142     debugDataBuffer[3][11] = sensorDataBuffer.frame[3].data[SensorData::legHL3];
    01143   }
    01144
    01145   // calculate position in space from joint angles
    01146   Kinematics::legPositionFromJoints(Kinematics::fr,
    01147     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 0]),
    01148     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 1]),
    01149     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 2]),
    01150     x, y, z);
    01151   v[Leg_FR].x = x; v[Leg_FR].y = y - 67.2; v[Leg_FR].z = z;
    01152   // set zero point to footZeroPosition
    01153   v[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
    01154   // undo rotate
    01155   real[Leg_FR].x = v[Leg_FR].x * cos(-c_angle[Leg_FR]) - v[Leg_FR].y * sin(-c_angle[Leg_FR]);
    01156   real[Leg_FR].y = v[Leg_FR].x * sin(-c_angle[Leg_FR]) + v[Leg_FR].y * cos(-c_angle[Leg_FR]);
    01157   real[Leg_FR].z = v[Leg_FR].z;
    01158   // undo x-scale
    01159   real[Leg_FR].x = real[Leg_FR].x / c_scale[Leg_FR];
    01160   // transform back to foot position
    01161   real[Leg_FR] += currentFIPSParameters->footZeroPosition[Leg_FR];
    01162   //real[Leg_FR] += shoulderPosition[Leg_FR];
    01163
    01164   Kinematics::legPositionFromJoints(Kinematics::fl,
    01165     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 0]),
    01166     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 1]),
    01167     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 2]),
    01168     x, y, z);
    01169   v[Leg_FL].x = x; v[Leg_FL].y = y + 67.2; v[Leg_FL].z = z;
    01170   // set zero point to footZeroPosition
    01171   v[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
    01172   // undo rotate
    01173   real[Leg_FL].x = v[Leg_FL].x * cos(-c_angle[Leg_FL]) - v[Leg_FL].y * sin(-c_angle[Leg_FL]);
    01174   real[Leg_FL].y = v[Leg_FL].x * sin(-c_angle[Leg_FL]) + v[Leg_FL].y * cos(-c_angle[Leg_FL]);
    01175   real[Leg_FL].z = v[Leg_FL].z;
    01176   // undo x-scale
    01177   real[Leg_FL].x = real[Leg_FL].x / c_scale[Leg_FL];
    01178   // transform back to foot position
    01179   real[Leg_FL] += currentFIPSParameters->footZeroPosition[Leg_FL];
    01180   //real[Leg_FL] += shoulderPosition[Leg_FL];
    01181
    01182   Kinematics::legPositionFromJoints(Kinematics::hr,
    01183     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 0]),
    01184     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 1]),
    01185     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 2]),
    01186     x, y, z);
    01187   v[Leg_HR].x = x; v[Leg_HR].y = y - 67.2; v[Leg_HR].z = z;
    01188   // set zero point to footZeroPosition
    01189   v[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
    01190   // undo rotate
    01191   real[Leg_HR].x = v[Leg_HR].x * cos(-c_angle[Leg_HR]) - v[Leg_HR].y * sin(-c_angle[Leg_HR]);
    01192   real[Leg_HR].y = v[Leg_HR].x * sin(-c_angle[Leg_HR]) + v[Leg_HR].y * cos(-c_angle[Leg_HR]);
    01193   real[Leg_HR].z = v[Leg_HR].z;
    01194   // undo x-scale
    01195   real[Leg_HR].x = real[Leg_HR].x / c_scale[Leg_HR];
    01196   // transform back to foot position
    01197   real[Leg_HR] += currentFIPSParameters->footZeroPosition[Leg_HR];
    01198   //real[Leg_HR] += shoulderPosition[Leg_HR];
    01199
    01200   Kinematics::legPositionFromJoints(Kinematics::hl,
    01201     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 0]),
    01202     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 1]),
    01203     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 2]),
    01204     x, y, z);
    01205   v[Leg_HL].x = x; v[Leg_HL].y = y + 67.2; v[Leg_HL].z = z;
    01206   // set zero point to footZeroPosition
    01207   v[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
    01208   // undo rotate
    01209   real[Leg_HL].x = v[Leg_HL].x * cos(-c_angle[Leg_HL]) - v[Leg_HL].y * sin(-c_angle[Leg_HL]);
    01210   real[Leg_HL].y = v[Leg_HL].x * sin(-c_angle[Leg_HL]) + v[Leg_HL].y * cos(-c_angle[Leg_HL]);
    01211   real[Leg_HL].z = v[Leg_HL].z;
    01212   // undo x-scale
    01213   real[Leg_HL].x = real[Leg_HL].x / c_scale[Leg_HL];
    01214   // transform back to foot position
    01215   real[Leg_HL] += currentFIPSParameters->footZeroPosition[Leg_HL];
    01216   //real[Leg_HL] += shoulderPosition[Leg_HL];
    01217
    01218   // write in array
    01219   debugDataReal[Leg_FR][currentStep] = real[Leg_FR];
    01220   debugDataReal[Leg_FL][currentStep] = real[Leg_FL];
    01221   debugDataReal[Leg_HR][currentStep] = real[Leg_HR];
    01222   debugDataReal[Leg_HL][currentStep] = real[Leg_HL];
    01223
    01224   debugDataIdeal[Leg_FR][currentStep] = ideal[Leg_FR];
    01225   debugDataIdeal[Leg_FL][currentStep] = ideal[Leg_FL];
    01226   debugDataIdeal[Leg_HR][currentStep] = ideal[Leg_HR];
    01227   debugDataIdeal[Leg_HL][currentStep] = ideal[Leg_HL];
    01228
    01229   debugRequest = false;
    01230 }
    01231
    01232 void GT2005WalkingEngine::readResampleData(double c_scale[4], double c_angle[4], double c_offset[4], JointData &j, Vector3<double> fp[4])
    01233 {
    01234   double x, y, z;
    01235   Vector3<double> v[4];
    01236   Vector3<double> s[4];
    01237
    01238   Vector3<double> real[4]; // corrected real vectors from sensor data
    01239   Vector3<double> ideal[4]; // corrected ideal vectors from sensor data
    01240
    01241   // define some hardcoded shoulder positions
    01242   Vector3<double> shoulderPosition[4];
    01243   shoulderPosition[Leg_FR].x = 0; shoulderPosition[Leg_FR].y = -67.2; shoulderPosition[Leg_FR].z = -19.5;
    01244   shoulderPosition[Leg_FL].x = 0; shoulderPosition[Leg_FL].y = 67.2;  shoulderPosition[Leg_FL].z = -19.5;
    01245   shoulderPosition[Leg_HR].x = -130;  shoulderPosition[Leg_HR].y = -67.2; shoulderPosition[Leg_HR].z = -19.5;
    01246   shoulderPosition[Leg_HL].x = -130;  shoulderPosition[Leg_HL].y = 67.2;  shoulderPosition[Leg_HL].z = -19.5;
    01247
    01248   // read forced angles
    01249   // calculate position in space from joint angles
    01250   Kinematics::legPositionFromJoints(Kinematics::fr,
    01251     fromMicroRad(j.data[JointData::legFR1]) - bodyTilt,
    01252     fromMicroRad(j.data[JointData::legFR2]),
    01253     fromMicroRad(j.data[JointData::legFR3]),
    01254     x, y, z);
    01255   s[Leg_FR].x = x; s[Leg_FR].y = y - 67.2; s[Leg_FR].z = z;
    01256   // set zero point to footZeroPosition
    01257   s[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
    01258   // undo rotate
    01259   ideal[Leg_FR].x = s[Leg_FR].x * cos(-c_angle[Leg_FR]) - s[Leg_FR].y * sin(-c_angle[Leg_FR]);
    01260   ideal[Leg_FR].y = s[Leg_FR].x * sin(-c_angle[Leg_FR]) + s[Leg_FR].y * cos(-c_angle[Leg_FR]);
    01261   ideal[Leg_FR].z = s[Leg_FR].z;
    01262   // undo x-scale
    01263   ideal[Leg_FR].x = ideal[Leg_FR].x / c_scale[Leg_FR];
    01264
    01265   Kinematics::legPositionFromJoints(Kinematics::fl,
    01266     fromMicroRad(j.data[JointData::legFL1]) - bodyTilt,
    01267     fromMicroRad(j.data[JointData::legFL2]),
    01268     fromMicroRad(j.data[JointData::legFL3]),
    01269     x, y, z);
    01270   s[Leg_FL].x = x; s[Leg_FL].y = y + 67.2; s[Leg_FL].z = z;
    01271   // set zero point to footZeroPosition
    01272   s[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
    01273   // undo rotate
    01274   ideal[Leg_FL].x = s[Leg_FL].x * cos(-c_angle[Leg_FL]) - s[Leg_FL].y * sin(-c_angle[Leg_FL]);
    01275   ideal[Leg_FL].y = s[Leg_FL].x * sin(-c_angle[Leg_FL]) + s[Leg_FL].y * cos(-c_angle[Leg_FL]);
    01276   ideal[Leg_FL].z = s[Leg_FL].z;
    01277   // undo x-scale
    01278   ideal[Leg_FL].x = ideal[Leg_FL].x / c_scale[Leg_FL];
    01279
    01280   Kinematics::legPositionFromJoints(Kinematics::hr,
    01281     fromMicroRad(j.data[JointData::legHR1]) + bodyTilt,
    01282     fromMicroRad(j.data[JointData::legHR2]),
    01283     fromMicroRad(j.data[JointData::legHR3]),
    01284     x, y, z);
    01285   s[Leg_HR].x = x; s[Leg_HR].y = y - 67.2; s[Leg_HR].z = z;
    01286   // set zero point to footZeroPosition
    01287   s[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
    01288   // undo rotate
    01289   ideal[Leg_HR].x = s[Leg_HR].x * cos(-c_angle[Leg_HR]) - s[Leg_HR].y * sin(-c_angle[Leg_HR]);
    01290   ideal[Leg_HR].y = s[Leg_HR].x * sin(-c_angle[Leg_HR]) + s[Leg_HR].y * cos(-c_angle[Leg_HR]);
    01291   ideal[Leg_HR].z = s[Leg_HR].z;
    01292   // undo x-scale
    01293   ideal[Leg_HR].x = ideal[Leg_HR].x / c_scale[Leg_HR];
    01294
    01295   Kinematics::legPositionFromJoints(Kinematics::hl,
    01296     fromMicroRad(j.data[JointData::legHL1]) + bodyTilt,
    01297     fromMicroRad(j.data[JointData::legHL2]),
    01298     fromMicroRad(j.data[JointData::legHL3]),
    01299     x, y, z);
    01300   s[Leg_HL].x = x; s[Leg_HL].y = y + 67.2; s[Leg_HL].z = z;
    01301   // set zero point to footZeroPosition
    01302   s[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
    01303   // undo rotate
    01304   ideal[Leg_HL].x = s[Leg_HL].x * cos(-c_angle[Leg_HL]) - s[Leg_HL].y * sin(-c_angle[Leg_HL]);
    01305   ideal[Leg_HL].y = s[Leg_HL].x * sin(-c_angle[Leg_HL]) + s[Leg_HL].y * cos(-c_angle[Leg_HL]);
    01306   ideal[Leg_HL].z = s[Leg_HL].z;
    01307   // undo x-scale
    01308   ideal[Leg_HL].x = ideal[Leg_HL].x / c_scale[Leg_HL];
    01309
    01310   // read real angles
    01311   int frameNumber = currentStep % 4;
    01312
    01313   if (frameNumber == 0)
    01314   {
    01315     debugDataBuffer[0][0] = sensorDataBuffer.frame[0].data[SensorData::legFR1];
    01316     debugDataBuffer[0][1] = sensorDataBuffer.frame[0].data[SensorData::legFR2];
    01317     debugDataBuffer[0][2] = sensorDataBuffer.frame[0].data[SensorData::legFR3];
    01318     debugDataBuffer[0][3] = sensorDataBuffer.frame[1].data[SensorData::legFR1];
    01319     debugDataBuffer[0][4] = sensorDataBuffer.frame[1].data[SensorData::legFR2];
    01320     debugDataBuffer[0][5] = sensorDataBuffer.frame[1].data[SensorData::legFR3];
    01321     debugDataBuffer[0][6] = sensorDataBuffer.frame[2].data[SensorData::legFR1];
    01322     debugDataBuffer[0][7] = sensorDataBuffer.frame[2].data[SensorData::legFR2];
    01323     debugDataBuffer[0][8] = sensorDataBuffer.frame[2].data[SensorData::legFR3];
    01324     debugDataBuffer[0][9] = sensorDataBuffer.frame[3].data[SensorData::legFR1];
    01325     debugDataBuffer[0][10] = sensorDataBuffer.frame[3].data[SensorData::legFR2];
    01326     debugDataBuffer[0][11] = sensorDataBuffer.frame[3].data[SensorData::legFR3];
    01327
    01328     debugDataBuffer[1][0] = sensorDataBuffer.frame[0].data[SensorData::legFL1];
    01329     debugDataBuffer[1][1] = sensorDataBuffer.frame[0].data[SensorData::legFL2];
    01330     debugDataBuffer[1][2] = sensorDataBuffer.frame[0].data[SensorData::legFL3];
    01331     debugDataBuffer[1][3] = sensorDataBuffer.frame[1].data[SensorData::legFL1];
    01332     debugDataBuffer[1][4] = sensorDataBuffer.frame[1].data[SensorData::legFL2];
    01333     debugDataBuffer[1][5] = sensorDataBuffer.frame[1].data[SensorData::legFL3];
    01334     debugDataBuffer[1][6] = sensorDataBuffer.frame[2].data[SensorData::legFL1];
    01335     debugDataBuffer[1][7] = sensorDataBuffer.frame[2].data[SensorData::legFL2];
    01336     debugDataBuffer[1][8] = sensorDataBuffer.frame[2].data[SensorData::legFL3];
    01337     debugDataBuffer[1][9] = sensorDataBuffer.frame[3].data[SensorData::legFL1];
    01338     debugDataBuffer[1][10] = sensorDataBuffer.frame[3].data[SensorData::legFL2];
    01339     debugDataBuffer[1][11] = sensorDataBuffer.frame[3].data[SensorData::legFL3];
    01340
    01341     debugDataBuffer[2][0] = sensorDataBuffer.frame[0].data[SensorData::legHR1];
    01342     debugDataBuffer[2][1] = sensorDataBuffer.frame[0].data[SensorData::legHR2];
    01343     debugDataBuffer[2][2] = sensorDataBuffer.frame[0].data[SensorData::legHR3];
    01344     debugDataBuffer[2][3] = sensorDataBuffer.frame[1].data[SensorData::legHR1];
    01345     debugDataBuffer[2][4] = sensorDataBuffer.frame[1].data[SensorData::legHR2];
    01346     debugDataBuffer[2][5] = sensorDataBuffer.frame[1].data[SensorData::legHR3];
    01347     debugDataBuffer[2][6] = sensorDataBuffer.frame[2].data[SensorData::legHR1];
    01348     debugDataBuffer[2][7] = sensorDataBuffer.frame[2].data[SensorData::legHR2];
    01349     debugDataBuffer[2][8] = sensorDataBuffer.frame[2].data[SensorData::legHR3];
    01350     debugDataBuffer[2][9] = sensorDataBuffer.frame[3].data[SensorData::legHR1];
    01351     debugDataBuffer[2][10] = sensorDataBuffer.frame[3].data[SensorData::legHR2];
    01352     debugDataBuffer[2][11] = sensorDataBuffer.frame[3].data[SensorData::legHR3];
    01353
    01354     debugDataBuffer[3][0] = sensorDataBuffer.frame[0].data[SensorData::legHL1];
    01355     debugDataBuffer[3][1] = sensorDataBuffer.frame[0].data[SensorData::legHL2];
    01356     debugDataBuffer[3][2] = sensorDataBuffer.frame[0].data[SensorData::legHL3];
    01357     debugDataBuffer[3][3] = sensorDataBuffer.frame[1].data[SensorData::legHL1];
    01358     debugDataBuffer[3][4] = sensorDataBuffer.frame[1].data[SensorData::legHL2];
    01359     debugDataBuffer[3][5] = sensorDataBuffer.frame[1].data[SensorData::legHL3];
    01360     debugDataBuffer[3][6] = sensorDataBuffer.frame[2].data[SensorData::legHL1];
    01361     debugDataBuffer[3][7] = sensorDataBuffer.frame[2].data[SensorData::legHL2];
    01362     debugDataBuffer[3][8] = sensorDataBuffer.frame[2].data[SensorData::legHL3];
    01363     debugDataBuffer[3][9] = sensorDataBuffer.frame[3].data[SensorData::legHL1];
    01364     debugDataBuffer[3][10] = sensorDataBuffer.frame[3].data[SensorData::legHL2];
    01365     debugDataBuffer[3][11] = sensorDataBuffer.frame[3].data[SensorData::legHL3];
    01366   }
    01367
    01368   // calculate position in space from joint angles
    01369   Kinematics::legPositionFromJoints(Kinematics::fr,
    01370     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 0]) - bodyTilt,
    01371     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 1]),
    01372     fromMicroRad(debugDataBuffer[0][3 * frameNumber + 2]),
    01373     x, y, z);
    01374   v[Leg_FR].x = x; v[Leg_FR].y = y - 67.2; v[Leg_FR].z = z;
    01375   // set zero point to footZeroPosition
    01376   v[Leg_FR] -= currentFIPSParameters->footZeroPosition[Leg_FR];
    01377   // undo rotate
    01378   real[Leg_FR].x = v[Leg_FR].x * cos(-c_angle[Leg_FR]) - v[Leg_FR].y * sin(-c_angle[Leg_FR]);
    01379   real[Leg_FR].y = v[Leg_FR].x * sin(-c_angle[Leg_FR]) + v[Leg_FR].y * cos(-c_angle[Leg_FR]);
    01380   real[Leg_FR].z = v[Leg_FR].z;
    01381   // undo x-scale
    01382   real[Leg_FR].x = real[Leg_FR].x / c_scale[Leg_FR];
    01383
    01384   Kinematics::legPositionFromJoints(Kinematics::fl,
    01385     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 0]) - bodyTilt,
    01386     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 1]),
    01387     fromMicroRad(debugDataBuffer[1][3 * frameNumber + 2]),
    01388     x, y, z);
    01389   v[Leg_FL].x = x; v[Leg_FL].y = y + 67.2; v[Leg_FL].z = z;
    01390   // set zero point to footZeroPosition
    01391   v[Leg_FL] -= currentFIPSParameters->footZeroPosition[Leg_FL];
    01392   // undo rotate
    01393   real[Leg_FL].x = v[Leg_FL].x * cos(-c_angle[Leg_FL]) - v[Leg_FL].y * sin(-c_angle[Leg_FL]);
    01394   real[Leg_FL].y = v[Leg_FL].x * sin(-c_angle[Leg_FL]) + v[Leg_FL].y * cos(-c_angle[Leg_FL]);
    01395   real[Leg_FL].z = v[Leg_FL].z;
    01396   // undo x-scale
    01397   real[Leg_FL].x = real[Leg_FL].x / c_scale[Leg_FL];
    01398
    01399   Kinematics::legPositionFromJoints(Kinematics::hr,
    01400     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 0]) + bodyTilt,
    01401     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 1]),
    01402     fromMicroRad(debugDataBuffer[2][3 * frameNumber + 2]),
    01403     x, y, z);
    01404   v[Leg_HR].x = x; v[Leg_HR].y = y - 67.2; v[Leg_HR].z = z;
    01405   // set zero point to footZeroPosition
    01406   v[Leg_HR] -= currentFIPSParameters->footZeroPosition[Leg_HR];
    01407   // undo rotate
    01408   real[Leg_HR].x = v[Leg_HR].x * cos(-c_angle[Leg_HR]) - v[Leg_HR].y * sin(-c_angle[Leg_HR]);
    01409   real[Leg_HR].y = v[Leg_HR].x * sin(-c_angle[Leg_HR]) + v[Leg_HR].y * cos(-c_angle[Leg_HR]);
    01410   real[Leg_HR].z = v[Leg_HR].z;
    01411   // undo x-scale
    01412   real[Leg_HR].x = real[Leg_HR].x / c_scale[Leg_HR];
    01413
    01414   Kinematics::legPositionFromJoints(Kinematics::hl,
    01415     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 0]) + bodyTilt,
    01416     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 1]),
    01417     fromMicroRad(debugDataBuffer[3][3 * frameNumber + 2]),
    01418     x, y, z);
    01419   v[Leg_HL].x = x; v[Leg_HL].y = y + 67.2; v[Leg_HL].z = z;
    01420   // set zero point to footZeroPosition
    01421   v[Leg_HL] -= currentFIPSParameters->footZeroPosition[Leg_HL];
    01422   // undo rotate
    01423   real[Leg_HL].x = v[Leg_HL].x * cos(-c_angle[Leg_HL]) - v[Leg_HL].y * sin(-c_angle[Leg_HL]);
    01424   real[Leg_HL].y = v[Leg_HL].x * sin(-c_angle[Leg_HL]) + v[Leg_HL].y * cos(-c_angle[Leg_HL]);
    01425   real[Leg_HL].z = v[Leg_HL].z;
    01426   // undo x-scale
    01427   real[Leg_HL].x = real[Leg_HL].x / c_scale[Leg_HL];
    01428
    01429   // write in array
    01430   debugDataReal[Leg_FR][currentStep] = real[Leg_FR];
    01431   debugDataReal[Leg_FL][currentStep] = real[Leg_FL];
    01432   debugDataReal[Leg_HR][currentStep] = real[Leg_HR];
    01433   debugDataReal[Leg_HL][currentStep] = real[Leg_HL];
    01434
    01435   debugDataIdeal[Leg_FR][currentStep] = ideal[Leg_FR];
    01436   debugDataIdeal[Leg_FL][currentStep] = ideal[Leg_FL];
    01437   debugDataIdeal[Leg_HR][currentStep] = ideal[Leg_HR];
    01438   debugDataIdeal[Leg_HL][currentStep] = ideal[Leg_HL];
    01439
    01440   resampleRequest = false;
    01441 }
    01442
    01443 void GT2005WalkingEngine::streamDebugData()
    01444 {
    01445   int i;
    01446
    01447   // diese prozedur muss ich noch mal für das neue debugformat überarbeieten
    01448   GT2005Polygon* idealPoly[4];
    01449   GT2005Polygon* realPoly[4];
    01450   for (i = 0; i < 4; i++)
    01451   {
    01452     idealPoly[i] = new GT2005Polygon(1, 0);
    01453     realPoly[i] = new GT2005Polygon(1, 0);
    01454   }
    01455
    01456   idealPoly[Leg_FR]->numPoints = currentFIPSParameters->stepLength;
    01457   idealPoly[Leg_FL]->numPoints = currentFIPSParameters->stepLength;
    01458   idealPoly[Leg_HR]->numPoints = currentFIPSParameters->stepLength;
    01459   idealPoly[Leg_HL]->numPoints = currentFIPSParameters->stepLength;
    01460   realPoly[Leg_FR]->numPoints = currentFIPSParameters->stepLength;
    01461   realPoly[Leg_FL]->numPoints = currentFIPSParameters->stepLength;
    01462   realPoly[Leg_HR]->numPoints = currentFIPSParameters->stepLength;
    01463   realPoly[Leg_HL]->numPoints = currentFIPSParameters->stepLength;
    01464
    01465   // Get debug stream
    01466   Out& out = getDebugOut().bin;
    01467
    01468   // prepare data
    01469   int dataID = 0;
    01470   // real data
    01471   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01472   {
    01473     idealPoly[Leg_FR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_FR][i]);
    01474   }
    01475   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01476   {
    01477     idealPoly[Leg_FL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_FL][i]);
    01478   }
    01479   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01480   {
    01481     idealPoly[Leg_HR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_HR][i]);
    01482   }
    01483   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01484   {
    01485     idealPoly[Leg_HL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataIdeal[Leg_HL][i]);
    01486   }
    01487   // ideal data
    01488   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01489   {
    01490     realPoly[Leg_FR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_FR][i]);
    01491   }
    01492   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01493   {
    01494     realPoly[Leg_FL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_FL][i]);
    01495   }
    01496   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01497   {
    01498     realPoly[Leg_HR]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_HR][i]);
    01499   }
    01500   for (i = 0; i < currentFIPSParameters->stepLength; i++)
    01501   {
    01502     realPoly[Leg_HL]->setSegment(i, 1/currentFIPSParameters->stepLength, debugDataReal[Leg_HL][i]);
    01503   }
    01504
    01505   // stream data
    01506   out << dataID;
    01507   out << *idealPoly[Leg_FR];
    01508   out << *idealPoly[Leg_FL];
    01509   out << *idealPoly[Leg_HR];
    01510   out << *idealPoly[Leg_HL];
    01511   out << *realPoly[Leg_FR];
    01512   out << *realPoly[Leg_FL];
    01513   out << *realPoly[Leg_HR];
    01514   out << *realPoly[Leg_HL];
    01515
    01516   // Finish message
    01517   getDebugOut().finishMessage(idGT2005DebugData);
    01518
    01519   for (i = 0; i < 4; i++)
    01520   {
    01521     delete idealPoly[i];
    01522     delete realPoly[i];
    01523   }
    01524 }