Modules/SensorBehaviorControl/BB2004Calibrator.cpp

Go to the documentation of this file.

/**
* @file BB2004Calibrator.cpp
*
* Implementation of class BB2004Calibrator.
*
* @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
*/

#include "BB2004Calibrator.h"
#include "Tools/Actorics/Kinematics.h"
#include "Representations/Perception/BodyPosture.h"

void CalibrationIndividual::init(const BBIndividual&)
{
//  ASSERT(sizeof(RobotCalibration) / sizeof(double) == numOfGenes);
  memcpy(&genes[0], (const void*) &getRobotConfiguration().getRobotCalibration(), sizeof(RobotCalibration));
  fitness = 1; 
  mutate();
}
  
void CalibrationIndividual::interpolate(const BBIndividual& other)
{
  const CalibrationIndividual& o = (const CalibrationIndividual&) other;
  for(int i = 0; i < numOfGenes; ++i)
  {
    double f = random(),
          f1 = f * fitness,
          f2 = (1 - f) * o.fitness;
    f = f1 / (f1 + f2);
    genes[i] = genes[i] * f + o.genes[i] * (1 - f);
  }
}

void CalibrationIndividual::mutate()
{
  for(int i = 0; i < numOfGenes; ++i)
    genes[i] += 0.1 * (-0.5 + random());
}

void CalibrationIndividual::extrapolate(const BBIndividual& other)
{
  const CalibrationIndividual& o = (const CalibrationIndividual&) other;
  for(int i = 0; i < numOfGenes; ++i)
  {
    double f = random(),
           f1 = f * fitness,
           f2 = (1 - f) * o.fitness;
    f = f1 / (f1 + f2) * 2;
    if(f < 1) // other dominates
      genes[i] = o.genes[i] + (o.genes[i] - genes[i]) * (1 - f);
    else // this dominates
      genes[i] += (genes[i] - o.genes[i]) * (f - 1);
  }
}

void CalibrationIndividual::select() const
{
  RobotCalibration& r = const_cast<RobotCalibration&>((const RobotCalibration&) genes[0]);
  /*r.bodyTiltOffset = 0.00454082;
  r.bodyRollOffset = 0.0243264;
  r.neckTiltOffset = 0.0188557;
  r.headTiltOffset = -0.0326878;*/
  r.tiltFactor = r.panFactor = r.tilt2Factor = 1;
  getRobotConfiguration().setRobotCalibration(r);
  //getRobotConfiguration().setRobotCalibration((const RobotCalibration&) genes[0]);
}

void CalibrationIndividual::dump() const
{
  for(int i = 0; i < numOfGenes; ++i)
    getDebugOut().text << " " << genes[i];
  getDebugOut().finishMessage(idText);
}

BB2004Calibrator::BB2004Calibrator(const SensorBehaviorControlInterfaces& interfaces)
: SensorBehaviorControl(interfaces),
  population(numOfIndividuals, CalibrationIndividual()),
  imageProcessor(ImageProcessorInterfaces(
    imageBuffer,
    cameraMatrix,
    const_cast<ColorTable&>(colorTable),
    robotPose,
    ballModel,
    playerPoseCollection,
    robotState,
    calibrationRequest,
    landmarksPercept,
    ballPercept,
    linesPercept,
    edgesPercept,
    playersPercept,
    obstaclesPercept,
    specialPercept)),
  evolutions(-1)
{
  for(int i = 0; i < numOfIndividuals; ++i)
    count[i] = 0;
}

void BB2004Calibrator::execute()
{
  int i;
  for(i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    buffer.add(sensorDataBuffer.frame[i]);
 
  for(i = 0; i < buffer.getNumberOfEntries(); ++i)
    if(buffer[i].frameNumber == imageBuffer.frameNumber)
      break;

  if(evolutions < 0)
  {
    if(--evolutions == 
#ifdef _WIN32
      -2
#else
      -300
#endif
      ) // wait 300 images and let the robot stand up
    {
      timeStamp = SystemCall::getCurrentSystemTime();
      evolutions = 0;
    }
  }
  else if(i < buffer.getNumberOfEntries() && image.frameNumber != imageBuffer.frameNumber)
    evolve(buffer[i]);

  imageBuffer = image;
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  motionRequest.walkRequest.walkParams = Pose2D();
  motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
  headControlMode.headControlMode = HeadControlMode::calibrate;
}

void BB2004Calibrator::evolve(const SensorData& sensorData)
{
  int i;
  for(i = 0; i < numOfIndividuals; ++i)
  {
    population[i].select();
    RobotVertices rob;
    Kinematics::calcNeckAndLegPositions(sensorData, rob);
    BodyPosture bodyPosture;
    bodyPosture.bodyTiltCalculatedFromLegSensors = rob.bodyTilt;
    bodyPosture.bodyRollCalculatedFromLegSensors = rob.bodyRoll;
    bodyPosture.neckHeightCalculatedFromLegSensors = rob.neckHeight;
    
    double tilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);
    double pan  = fromMicroRad(sensorData.data[SensorData::headPan]);
    double roll = fromMicroRad(sensorData.data[SensorData::headTilt]);
    Kinematics::calculateCameraMatrix(tilt, pan, roll, bodyPosture.bodyTiltCalculatedFromLegSensors, bodyPosture.neckHeightCalculatedFromLegSensors, cameraMatrix);
    imageProcessor.execute();
    if(linesPercept.numberOfPoints[LinesPercept::border])
    {
      for(int j = 0; j < linesPercept.numberOfPoints[LinesPercept::border]; ++j)
      {
        Vector2<double> v(linesPercept.points[LinesPercept::border][j].x, linesPercept.points[LinesPercept::border][j].y);
        Vector2<double> vMin = observationTable[LinesPercept::border].getClosestPoint(v);
        if(vMin.x != 1000000)
          fitness[i] += v - vMin; //fabs(zModel - zObs) + fabs(yModel - yObs);
        else
          fitness[i] += Vector2<double>(10000,10000);
        ++count[i];
      }
    }
  }

    //OUTPUT(idText, text, sensorData.frameNumber);
  if(SystemCall::getTimeSince(timeStamp) > waitPerGeneration)
  {
    for(i = 0; i < numOfIndividuals; ++i)
    {
      population[i].setFitness(exp(-0.1 * (count[i] ? fitness[i].abs() / count[i] : 1)));
      fitness[i] = Vector2<double>();
      count[i] = 0;
    }
    const CalibrationIndividual& best = (const CalibrationIndividual&) population.getFittest();
    ++evolutions;
    OUTPUT(idText, text, "Generation " << evolutions << ", fitness " << best.getFitness() << ", genes:");
    best.dump();

    // interchangingly interpolate and extrapolate
    if(evolutions & 1)
    {
      population.interpolate();
      getDebugOut().text << "Interpolating:";
    }
    else
    {
      population.extrapolate();
      getDebugOut().text << "Extrapolating:";
    }
    
    // print statistics
    Statistics& s = population.getStatistics();
    int num = s.analyze();
    for(i = 0; i < num; ++i)
      getDebugOut().text << " " << s[i];
    getDebugOut().finishMessage(idText);
    timeStamp = SystemCall::getCurrentSystemTime();
  }
}

bool BB2004Calibrator::handleMessage(InMessage& message)
{
  return false;
}

/*
* Change log :
* 
* $Log: BB2004Calibrator.cpp,v $
* Revision 1.7  2004/06/15 10:58:27  thomas
* added edge-specialist, edges-percept, debug-drawings etc. (not yet called from image-processor)
*
* Revision 1.6  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.5  2004/05/27 19:38:06  loetzsch
* compilation fixed
*
* Revision 1.4  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.3  2004/05/26 17:21:47  dueffert
* better data types used
*
* Revision 1.2  2004/05/22 22:52:02  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:20:51  cvsadm
* created new repository GT2004_WM
*
* Revision 1.8  2004/04/09 11:35:51  roefer
* Bremen Byters German Open check-in
*
* Revision 1.7  2004/03/20 09:55:25  roefer
* Preparation for improved odometry
*
* Revision 1.6  2004/03/13 17:20:42  roefer
* Different parameters for simulation and real robot
*
* Revision 1.5  2004/03/13 17:14:36  roefer
* Different parameters for simulation and real robot
*
* Revision 1.4  2004/03/11 00:06:18  roefer
* Parameter tuning
*
* Revision 1.3  2004/03/10 07:59:26  roefer
* New head control mode
*
* Revision 1.2  2004/03/08 02:11:49  roefer
* Interfaces should be const
*
* Revision 1.1  2004/03/04 23:00:54  roefer
* Added (so far empty) BB2004Calibrator
*
*/

---