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

# Tools/Evolution/Parcour.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Parcour.cpp
    00003 *
    00004 * Implementation of class Parcour.
    00005 *
    00006 * @author <a href=mailto:dueffert@informatik.hu-berlin.de>Uwe Düffert</a>
    00007 */
    00008
    00009 #include "Parcour.h"
    00010 #include "Platform/SystemCall.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012 #include "Tools/Streams/OutStreams.h"
    00013
    00014 void Parcour::start()
    00015 {
    00016   updateCount=0;
    00017   startTime=SystemCall::getCurrentSystemTime();
    00018   lastTime=startTime;
    00019   minX=-1000;
    00020   maxX=-1000;
    00021   yDiffIntegral=0;
    00022   arcDiffIntegral=0;
    00023   blindCount=0;
    00024   zAccelIntegral=0;
    00025 }
    00026
    00027 void Parcour::update(Pose2D robotPose, Vector3<double> accel, bool blind)
    00028 {
    00029   //2do: shouldnt this be done per frame instead of time?
    00030   unsigned long time = SystemCall::getCurrentSystemTime();
    00031   Pose2D pos=getTargetPose(time);
    00032   yDiffIntegral += fabs((pos.translation-robotPose.translation).y);
    00033   arcDiffIntegral += fabs(normalize(pos.rotation-robotPose.rotation));
    00034   blindCount += (int)(blind);
    00035   zAccelIntegral += fabs(accel.z);
    00036   lastRobotPose = robotPose;
    00037   lastTime = time;
    00038   updateCount++;
    00039   if (robotPose.translation.x<minX) { minX=robotPose.translation.x; }
    00040   else if ((robotPose.translation.x>maxX)&&(robotPose.translation.x!=-300)) { maxX=robotPose.translation.x; }
    00041 }
    00042
    00043 Pose2D Parcour::getMotionRequest()
    00044 {
    00045   //calculate where we should be in 850ms:
    00046   Pose2D target=getTargetPose(lastTime+850);
    00047   //90 ~ distance of real rotation center to neck joint in mm:
    00048   Pose2D targetCOG=target.minusDiff(Pose2D(0, 90,0));
    00049   Pose2D lastCOG=lastRobotPose.minusDiff(Pose2D(0, 90,0));
    00050   //only correct half of the y difference in 500ms:
    00051   targetCOG.translation.y += (lastCOG.translation.y-targetCOG.translation.y)/7;
    00052   //calculate the difference for our COG between now and in 850ms:
    00053   Pose2D diff=targetCOG-lastCOG;
    00054
    00055   //calculate speed per s (instead of per 850ms):
    00056   diff.translation /= 0.85;
    00057   diff.rotation /= 0.85;
    00058   return diff;
    00059 }
    00060
    00061 double Parcour::getUnifiedSpeed()
    00062 {
    00063   if (lastTime==startTime)
    00064   {
    00065     return 0;
    00066   }
    00067   else
    00068   {
    00069     double speed=1000*(maxX-minX)/(lastTime-startTime);
    00070     double yDiffAvg=yDiffIntegral/updateCount;
    00071     double arcDiffAvg=arcDiffIntegral/updateCount;
    00072     double zAccelAvg=zAccelIntegral/updateCount;
    00073     double blindTime=(double)blindCount/updateCount;
    00074     /*
    00075             ydiff  arcdiff   xAcc        yAcc       zAcc        blind
    00076       fotu  50-85  0.15-0.3  850K-1100K  650K-950K  900K-1300K  0.0-0.015
    00077       back  10-55  0.03-0.35 650K-900K   450K-800K  550K-1150K  0.05-0.15
    00078     */
    00079     double fitness=speed-yDiffAvg/6-arcDiffAvg/0.03-(zAccelAvg-500000)/100000-40*blindTime;
    00080     OUTPUT(idText,text,"speed:" << speed << ", yDiffAvg:" << yDiffAvg << ", arcDiffAvg:" << arcDiffAvg << ", zAccelAvg:" << zAccelAvg << ", blindTime:" << blindTime << " -> fitness:" << fitness);
    00081     OutTextRawFile fitLog("fitness.dat",true);
    00082     fitLog << "speed:" << speed << ", yDiffAvg:" << yDiffAvg << ", arcDiffAvg:" << arcDiffAvg << ", zAccelAvg:" << zAccelAvg << ", blindTime:" << blindTime << " -> fitness:" << fitness << "\n";
    00083     return fitness;
    00084     //    =speed-15-11-8-6 for a bad but still OK run
    00085     //    =speed-8-5-4-0   for a good forward turning run
    00086     //    =speed-2-1-1-2   for a good backward run
    00087   }
    00088 }
    00089
    00090
    00091 Pose2D ForwardTurningParcour::getTargetPose(unsigned long targetTime)
    00092 {
    00093   Pose2D target;
    00094   target.translation.x=lastRobotPose.translation.x+0.4*(targetTime-lastTime);
    00095   target.rotation=sin((target.translation.x+2600)/200/pi*4)*pi_4;
    00096   if (target.translation.x>-900)
    00097   {
    00098     target.rotation*=max(0.0,(target.translation.x+550.0)/(-450.0));
    00099   }
    00100   //90 ~ distance of real rotation center to neck joint in mm:
    00101   //45mm (instead of 0) makes curve amplitude a little bigger, but yDiff smaller
    00102   target.translation.y=45*sin(target.rotation);
    00103   return target;
    00104 }
    00105
    00106 Pose2D SimpleBackwardParcour::getTargetPose(unsigned long targetTime)
    00107 {
    00108   Pose2D target;
    00109   target.translation.x=lastRobotPose.translation.x-0.4*(targetTime-lastTime);
    00110   target.translation.y=0;
    00111   target.rotation=0;
    00112   return target;
    00113 }