Modules/WalkingEngine/BB2004InvKinWalkingEngine.cpp

Go to the documentation of this file.

/**
* @file BB2004InvKinWalkingEngine.cpp
* 
* This file contains an approach to evolutionary algorithms based on 
* a parameter set for the InvKinWalkingEngine.
*
* @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
*/

#include "BB2004InvKinWalkingEngine.h"
#include "Platform/GTAssert.h"
#include "Tools/Location.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"

static const double MAX_SPEED = 350,
                    MAX_ROT_SPEED = 2.24;

double BBInvKinIndividual::geneScale[numOfGenes] =
{
  200, 200, 200, 
  200, 200, 200,
  50, 50, 
  1, 1,
  1, 1, 1,
  50, 50,
  100,
  1, 1, 1, 
  1, 1, 1,
  0.5, 0.5,
  10, 10, 10
};

void BBInvKinIndividual::init(const BBIndividual& initial)
{
  ASSERT(sizeof(geneScale) / sizeof(geneScale[0]) == numOfGenes);
  *this = (const BBInvKinIndividual&) initial;
  fitness = 1; 
  mutate();
}
  
void BBInvKinIndividual::interpolate(const BBIndividual& other)
{
  const BBInvKinIndividual& o = (const BBInvKinIndividual&) 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 BBInvKinIndividual::mutate()
{
  for(int i = 0; i < numOfGenes; ++i)
    genes[i] += geneScale[i] * (-0.5 + random()) * 0.1;
}

void BBInvKinIndividual::extrapolate(const BBIndividual& other)
{
  const BBInvKinIndividual& o = (const BBInvKinIndividual&) 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 BBInvKinIndividual::setWalkCharacteristics(double speed, double smoothness)
{
  this->speed = speed;
  this->smoothness = smoothness;
  fitness = exp((speed + smoothness) / 50);
}

BBInvKinIndividual::BBInvKinIndividual(const InvKinWalkingParameters& parameters)
{
  double* g = &genes[0];
  *g++ = parameters.foreHeight;
  *g++ = parameters.foreWidth;
  *g++ = parameters.foreCenterX;
  
  *g++ = parameters.hindHeight;
  *g++ = parameters.hindWidth;
  *g++ = parameters.hindCenterX;
  
  *g++ = parameters.foreFootLift;
  *g++ = parameters.hindFootLift;
  
  *g++ = parameters.foreFootTilt;
  *g++ = parameters.hindFootTilt;
  
  *g++ = parameters.legSpeedFactorX;
  *g++ = parameters.legSpeedFactorY;
  *g++ = parameters.legSpeedFactorR;
  
  *g++ = parameters.maxStepSizeX;
  *g++ = parameters.maxStepSizeY;
  
  *g++ = parameters.stepLen;
  
  int i;
  for(i = 0; i < 2; ++i)
  {
    *g++ = parameters.groundPhase[i];
    *g++ = parameters.liftPhase[i];
    *g++ = parameters.loweringPhase[i];
  }

  *g++ = parameters.legPhase[0];
  *g++ = parameters.legPhase[3] - parameters.legPhase[0];
  
  *g++ = parameters.bodyShiftX;
  *g++ = parameters.bodyShiftY;
  *g++ = parameters.bodyShiftOffset;
  ASSERT(g - &genes[0] == numOfGenes);
}

void BBInvKinIndividual::getParameters(InvKinWalkingParameters& parameters) const
{
  parameters.footMode = InvKinWalkingParameters::halfCircle;
  const double* g = &genes[0];
  parameters.foreHeight = *g++;
  parameters.foreWidth = *g++;
  parameters.foreCenterX = *g++;
  
  parameters.hindHeight = *g++;
  parameters.hindWidth = *g++;
  parameters.hindCenterX = *g++;
  
  parameters.foreFootLift = *g++;
  parameters.hindFootLift = *g++;
  
  parameters.foreFootTilt = *g++;
  parameters.hindFootTilt = *g++;
  
  parameters.legSpeedFactorX = *g++;
  parameters.legSpeedFactorY = *g++;
  parameters.legSpeedFactorR = *g++;
  
  parameters.maxStepSizeX = *g++;
  parameters.maxStepSizeY = *g++;
  
  ASSERT(g == &genes[15]);
  parameters.stepLen = int(*g++);
  
  parameters.counterRotation = 0;
  
  int i;
  for(i = 0; i < 2; ++i)
  {
    parameters.groundPhase[i] = *g++;
    parameters.liftPhase[i] = *g++;
    parameters.loweringPhase[i] = *g++;
  }

  parameters.legPhase[0] = 0; // fmod(100 + g[0], 1.0);
  parameters.legPhase[3] = fmod(100 + g[1], 1.0);
  parameters.legPhase[1] = 0.5; //fmod(100.5 + g[0], 1.0);
  parameters.legPhase[2] = fmod(100.5 + g[1], 1.0);
  g += 2;
  
  parameters.bodyShiftX = *g++;
  parameters.bodyShiftY = *g++;
  parameters.bodyShiftOffset = *g++;
  ASSERT(g - &genes[0] == numOfGenes);
}

void BBInvKinIndividual::dump() const
{
  ERS7EvolveWalkingParameters temp;
  getParameters(temp);
  char buf[1000];
  OutTextMemory stream(buf);
  stream << temp;
  buf[stream.getLength()] = 0;
  getDebugOut().text << buf;
  getDebugOut().finishMessage(idText);
}

BB2004InvKinWalkingEngine::BB2004InvKinWalkingEngine(InvKinWalkingEngine* pEngine)
: WalkingEngine(*pEngine),
  pEngine(pEngine),
  currentParameters(0),
  currentIndividual(0),
  currentFrame(0),
  evolutions(0),
  bestFitness(0),
  odometryScaleX(1),
  odometryScaleY(1),
  odometryScaleRotation(1),
  odometryCounterRotation(0)
{
  loadParameters(forward, "forward");  
  loadParameters(fastForward, "fastForward");
  loadParameters(backward, "backward");
  loadParameters(stand, "stand");
  parameters[0] = parameters[1] = bestParameters = forward;
  population = new BBPopulation<BBInvKinIndividual>(numOfIndividuals, BBInvKinIndividual(bestParameters));
  InConfigFile stream("OdoScale.cfg", getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210 ? "ERS210" : "ERS7");
  if(stream.exists())
    stream >> odometryScaleX >> odometryScaleY >> odometryScaleRotation >> odometryCounterRotation;
  buffer.init();
}

void BB2004InvKinWalkingEngine::setParameters(const BBIndividual& p)
{
  currentParameters = 1 - currentParameters;
  ((const BBInvKinIndividual&) p).getParameters(parameters[currentParameters]);
  InvKinWalkingParameters& wp = parameters[currentParameters];
  wp.init();
  wp.leaveAnytime = true;
  wp.correctionValues.forward = wp.maxStepSizeX / MAX_SPEED;
  wp.correctionValues.backward = wp.maxStepSizeX / MAX_SPEED;
  wp.correctionValues.sideward = wp.maxStepSizeY / MAX_SPEED;
  wp.correctionValues.turning = wp.maxStepSizeR / MAX_ROT_SPEED;
  pEngine->setParameters(&parameters[currentParameters],64);
}

void BB2004InvKinWalkingEngine::loadParameters(InvKinWalkingParameters& parameters, const char* name)
{
  char section[100];
  strcpy(section, getRobotConfiguration().getRobotDesign() == RobotDesign::ERS210 ? "ERS210_" : "ERS7_");
  strcat(section, name);
  InConfigFile stream("Walking.cfg", section);
  if(stream.exists())
  {
    stream >> parameters;
    parameters.init();
    double speed;
    stream >> speed;
    parameters.correctionValues.forward = parameters.maxStepSizeX / speed;
    stream >> speed;
    parameters.correctionValues.backward = parameters.maxStepSizeX / speed;
    stream >> speed;
    parameters.correctionValues.sideward = parameters.maxStepSizeY / speed;
    stream >> speed;
    parameters.correctionValues.turning = parameters.maxStepSizeR / speed;
    stream >> parameters.correctionValues.rotationCenter;
    parameters.leaveAnytime = true;
  }
}

bool BB2004InvKinWalkingEngine::executeParameterized(JointData& jointData,
                                                     const WalkRequest& walkRequest,
                                                     double positionInWalkingCycle)
{
  if(!getDebugKeyTable().isActive(DebugKeyTable::learnWalking))
    if(currentIndividual || evolutions)
      pEngine->setParameters(&bestParameters, 64);
    else if(walkRequest.walkParams.translation.x < 0)
      pEngine->setParameters(&backward, 64);
    else if(walkRequest.walkParams.translation.x == 0 &&
            walkRequest.walkParams.translation.y == 0 &&
            walkRequest.walkParams.rotation == 0)
      pEngine->setParameters(&stand, 64);
    else
      pEngine->setParameters(&fastForward, 64);
  else 
    learn(walkRequest);

  Pose2D odometry = odometryData;
  bool result = pEngine->executeParameterized(jointData, walkRequest, positionInWalkingCycle);
#ifndef _WIN32
  (Pose2D&) odometryData = odometry;
  updateOdometry();
#endif
  return result;
}

void BB2004InvKinWalkingEngine::learn(const WalkRequest& walkRequest)
{
  if(/*walkRequest.walkParams.translation.x > 0 &&*/
     (walkRequest.walkParams.translation.abs() > 50 ||
     fabs(walkRequest.walkParams.rotation) > 0.3) &&
     !sensorDataBuffer.lastFrame().data[SensorData::back])
  {
    // set walking parameters on first call
    if(!evolutions && !currentIndividual && !currentFrame)
      setParameters((*population)[currentIndividual]);

    // update sensor data history
    for(int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
      if(sensorDataBuffer.frame[i].frameNumber > buffer[0].frameNumber)
        buffer.add(sensorDataBuffer.frame[i]);


    if(++currentFrame == ignoreFrames)
    {
      //reset
      measured = Pose2D();
      target = Pose2D();
    }

    // test of current individual finished?
    if(currentFrame == numOfFramesPerIndividual)
    {
      // calc fitness of individual
      BBInvKinIndividual& p = (BBInvKinIndividual&) (*population)[currentIndividual];
      double smoothness = (9e6 - stdV(SensorData::accelerationX) -
                                 stdV(SensorData::accelerationY) -
                                 stdV(SensorData::accelerationZ)) / 5e4,
             speed = 2100 - (fabs(measured.translation.x - target.translation.x) + 
                             fabs(measured.translation.y - target.translation.y) + 
                             fabs(measured.rotation - target.rotation) * 100);

      OUTPUT(idText, text, speed << " " << smoothness);
      p.setWalkCharacteristics(speed, smoothness);

      // test of population finished?
      if(++currentIndividual == numOfIndividuals)
      {
        const BBInvKinIndividual& best = (const BBInvKinIndividual&) population->getFittest();
        if(best.getFitness() > bestFitness)
        {
          best.getParameters(bestParameters);
          bestFitness = best.getFitness();
        }
        ++evolutions;
        OUTPUT(idText, text, "Generation " << evolutions << ", speed " << best.getSpeed() 
               << ", smoothness " << best.getSmoothness() << ", 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(int i = 0; i < num; ++i)
          getDebugOut().text << " " << s[i];
        getDebugOut().finishMessage(idText);

        // restart with first individual
        currentIndividual = 0;
      }
      setParameters((*population)[currentIndividual]);
      currentFrame = 0;
    }
  }
  else
    currentFrame = 0;

  // update target motion
  target.translation.x += walkRequest.walkParams.translation.x * motionCycleTime;
  target.translation.y += walkRequest.walkParams.translation.y * motionCycleTime;
  target.rotation += walkRequest.walkParams.rotation * motionCycleTime;
}

double BB2004InvKinWalkingEngine::stdV(SensorData::sensors s) const
{
  int i;
  double sum = 0;
  for(i = 0; i < useFrames; ++i)
    sum += buffer[i].data[s];
  double average = sum / numOfFramesPerIndividual;
  sum = 0;
  for(i = 0; i < numOfFramesPerIndividual; ++i)
    sum += pow(buffer[i].data[s] - average, 2);
  return sqrt(sum / numOfFramesPerIndividual);
}

void BB2004InvKinWalkingEngine::updateOdometry()
{
  if(receivedNewSensorData)
  {
    int i,
        leg;
    for(i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    {
      const SensorData& s = sensorDataBuffer.frame[i];
      RobotVertices robotVertices;
      Kinematics::calcNeckAndLegPositions(s, robotVertices);

      FootPositions currentPositions;
      for(leg = 0; leg < 2; ++leg)
      {
        currentPositions.positions[leg] = robotVertices.footPosition[2 + leg];
        currentPositions.onGround[leg] = s.data[leg ? SensorData::pawHL : SensorData::pawHR] != 0;
      }
      for(leg = 0; leg < 2; ++leg)
      {
        Vector2<double> sum;
        Vector3<double> diff3d = currentPositions.positions[leg] - prevPositions.positions[leg]; // legs move in opposite direction than the robot
        Vector2<double> diff2d(diff3d.x, diff3d.y);
        if(prevPositions.onGround[leg] && currentPositions.onGround[leg])
          sum += diff2d;
        if(prevPositions.onGround[1 - leg] && currentPositions.onGround[1 - leg])
          sum -= diff2d;
        odometrySums[leg].add(sum);
      }
      prevPositions = currentPositions;
    }
    Vector2<double> sums[2];
    const InvKinWalkingParameters& p = pEngine->getParameters();
    for(leg = 0; leg < 2; ++leg)
      for(i = 0; i < p.stepLen; ++i)
        sums[leg] += odometrySums[leg][i];
    sums[0] /= 2;
    sums[1] /= 2;
    odometrySpeed.translation.x = -(sums[0].x + sums[1].x) * odometryScaleX;
    odometrySpeed.translation.y = -(sums[0].y + sums[1].y - sums[1].x + sums[0].x);
    odometrySpeed.rotation = (sums[1].x - sums[0].x) * odometryScaleRotation / p.hindWidth +
                             odometryCounterRotation * odometrySpeed.translation.y;
    odometrySpeed.translation.y *= odometryScaleY;
    odometrySpeed.translation /= p.stepLen;
    odometrySpeed.rotation /= p.stepLen;
  }
  odometryData += odometrySpeed;

  if(getDebugKeyTable().isActive(DebugKeyTable::learnWalking))
  {
    // update measured motion
    measured.translation.x += odometrySpeed.translation.x;
    measured.translation.y += odometrySpeed.translation.y;
    measured.rotation += odometrySpeed.rotation;
  }
}

bool BB2004InvKinWalkingEngine::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
    case idOdometryScale:
    {
      GenericDebugData d;
      message.bin >> d;
      odometryScaleX = d.data[0];
      odometryScaleY = d.data[1];
      odometryScaleRotation = d.data[2];
      odometryCounterRotation = d.data[3];
      if(!odometryScaleX)
        odometryScaleX = 1;
      if(!odometryScaleY)
        odometryScaleY = 1;
      if(!odometryScaleRotation)
        odometryScaleRotation = 1;
      return true;
    }
  }
  return pEngine->handleMessage(message);
}

/*
 * Change log :
 * 
 * $Log: BB2004InvKinWalkingEngine.cpp,v $
 * Revision 1.8  2004/06/18 00:35:08  risler
 * only fastForward parameters
 *
 * Revision 1.7  2004/06/14 16:55:19  juengel
 * Removed some WalkingEngineParameterSets.
 *
 * Revision 1.6  2004/06/07 18:47:50  spranger
 * extended interface of executeParametrized by positionInWalkCycle
 *
 * Revision 1.5  2004/06/02 17:18:22  spranger
 * MotionRequest cleanup
 *
 * Revision 1.4  2004/05/27 12:51:14  roefer
 * Minor error fixed
 *
 * Revision 1.3  2004/05/26 20:18:44  juengel
 * local calculation of robotVertices
 *
 * Revision 1.2  2004/05/26 09:29:30  roefer
 * Remove redundant code
 *
 * Revision 1.5  2004/04/09 11:35:53  roefer
 * Bremen Byters German Open check-in
 *
 * Revision 1.4  2004/03/27 09:32:42  tim
 * changed parameter set selection limits
 *
 * Revision 1.3  2004/03/22 21:58:12  roefer
 * True odometry
 *
 * Revision 1.2  2004/03/11 00:05:38  roefer
 * OUTPUT removed
 *
 * Revision 1.1  2004/03/03 21:17:08  roefer
 * For the sake of consistency, renamed BB* to BB2004*
 *
 * Revision 1.9  2004/02/13 13:45:09  roefer
 * Mistakes corrected
 *
 * Revision 1.8  2004/02/06 20:23:00  roefer
 * Further improvements
 *
 * Revision 1.7  2004/02/04 07:57:20  roefer
 * Further improvements
 *
 * Revision 1.6  2004/01/28 21:55:49  roefer
 * RobotDimensions revised
 *
 * Revision 1.5  2004/01/05 22:59:53  roefer
 * Different parameters for ERS-210 and ERS-7
 *
 * Revision 1.4  2004/01/02 13:37:26  roefer
 * Handle if a single individual was drawn too often
 *
 * Revision 1.3  2004/01/01 10:26:08  roefer
 * Missing initialization added
 *
 * Revision 1.2  2003/12/29 21:46:49  roefer
 * Allow leaving the walking engine
 *
 * Revision 1.1  2003/12/29 15:48:54  roefer
 * Bremen Byters evo walking added
 *
 */

---