Modules/WalkingEngine/InvKinWalkingParameters.cpp

Go to the documentation of this file.

/**
* @file InvKinWalkingParameters.cpp
* 
* Implementation of class InvKinWalkingParameters
*
* @author Max Risler
*/

#include "InvKinWalkingParameters.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Actorics/Kinematics.h"

InvKinWalkingParameters::InvKinWalkingParameters(const GT2004Parameters& gt2004Param)
{
  foreHeight =gt2004Param.foreHeight;
  foreWidth  =gt2004Param.foreWidth;
  foreCenterX=gt2004Param.foreCenterX;
  
  hindHeight =gt2004Param.hindHeight;
  hindWidth  =gt2004Param.hindWidth;
  hindCenterX=gt2004Param.hindCenterX;
  
  foreFootLift=gt2004Param.foreFootLift;
  hindFootLift=gt2004Param.hindFootLift;
  
  foreFootTilt=gt2004Param.foreFootTilt;
  hindFootTilt=gt2004Param.hindFootTilt;
  
  stepLen=gt2004Param.stepLen;
  
  switch(gt2004Param.footMode)
  {
  case GT2004Parameters::rectangle:
    footMode=rectangle;
    break;
  case GT2004Parameters::halfCircle:
  default:
    footMode=halfCircle;
    break;
  }
  
  int i;
  for (i=0;i<2;i++)
  {
    groundPhase[i]=gt2004Param.groundPhase[i];
    liftPhase[i]=gt2004Param.liftPhase[i];         //temp for BB support
    loweringPhase[i]=gt2004Param.loweringPhase[i]; //temp for BB support
  }
  for (i=0;i<4;i++)
  {
    legPhase[i]=gt2004Param.legPhase[i];
  }
  
  headTilt=gt2004Param.headTilt;
  headPan=gt2004Param.headPan;
  headRoll=gt2004Param.headRoll;
  mouth=gt2004Param.mouth;
  
  name[0]=0;
  
  legSpeedFactorX=1;
  legSpeedFactorY=1;
  legSpeedFactorR=1;
  
  maxStepSizeX=(gt2004Param.index==(int)GT2004ParametersSet::numberOfParameters)?gt2004Param.correctedMotion.translation.x:45;
  maxStepSizeY=(gt2004Param.index==(int)GT2004ParametersSet::numberOfParameters)?gt2004Param.correctedMotion.translation.y:45;
  maxSpeedXChange=100;
  maxSpeedYChange=100;
  maxRotationChange=1;
  counterRotation=0;
  
  for (int i2=0;i2<4;i2++)
  {
    for (int i3=0;i3<3;i3++)
    {
      freeFormQuadPos[0][i2][i3]=0;
    }
  }

  leaveAnytime=true;
  bodyShiftX=0;
  bodyShiftY=0;
  bodyShiftOffset=0;

  neckHeight=120;
  bodyTilt=0;
  bodyTiltOffset=0;

  fitness=0;
}

void InvKinWalkingParameters::init()
{
  bodyTilt = asin((hindHeight - foreHeight)/getRobotConfiguration().getRobotDimensions().lengthBetweenLegs);
  
  int i;
  for(i=0;i<2;i++) 
  {
    groundTime[i] = (int)(groundPhase[i] * stepLen);
    liftTime[i] = (int)(liftPhase[i] * stepLen);
    loweringTime[i] = (int)(loweringPhase[i] * stepLen); 
  
    airTime[i]=stepLen-groundTime[i]-liftTime[i]-loweringTime[i];
    stepLift[i]=groundTime[i];      
    stepAir[i]=stepLift[i]+liftTime[i];        
    stepLower[i]=stepAir[i]+airTime[i];      
  }
  
  for (i=0;i<4;i++) {
    legPhaseIndex[i]=(int)(((double)stepLen)*legPhase[i]);
  }

  //search first step where all feet are on ground
  firstStep=0;
  while(firstStep<stepLen)
  {
    if (allFeetOnGround(firstStep)) break;
    firstStep++;
  }
  if (firstStep==stepLen)
  {
    if (!leaveAnytime)
    {
      // this crashes, because InvKinWalkingParameters are initialized in places where OUTPUT does not work:
      // OUTPUT(idText,text,"InvKinWalkingEngine:" << name << " : bad parameter set, never all feet on ground.");
    }
    firstStep = 0;
  }
  
  if (maxStepSizeX == 0) maxStepSizeX=0.01;
  if (maxStepSizeY == 0) maxStepSizeY=0.01;
  //maxStepSizeR is radius of biggest square fitting into ellipse from maxStepSizeX,maxStepSizeY
  maxStepSizeR = 
    maxStepSizeX*maxStepSizeY/
    sqrt(maxStepSizeX*maxStepSizeX + maxStepSizeY*maxStepSizeY);
}

void InvKinWalkingParameters::readValues()
{
  if (strlen(name)==0)
  {
    correctionValues.forward = correctionValues.backward = 
      correctionValues.sideward = 1.0;
    correctionValues.turning = 100.0;
    correctionValues.rotationCenter = 0;
    neckHeight = 0;
  } else {
    char section[100];
    strcpy(section, "InvKinWalkingEngine:");
    strcat(section, name);
  
    InConfigFile odometryFile("Odometry.cfg",section);
  
    if (!odometryFile.exists() || odometryFile.eof()) {
      OUTPUT(idText,text,"InvKinWalkingEngine : Error, odometry not found for : " << name);
      correctionValues.forward = correctionValues.backward = 
        correctionValues.sideward = 1.0;
      correctionValues.turning = 100.0;
      correctionValues.rotationCenter = 0;
      neckHeight = 0;
    } else {
      double speed;
      odometryFile >> speed;
      correctionValues.forward = maxStepSizeX / speed;
      odometryFile >> speed;
      correctionValues.backward = maxStepSizeX / speed;
      odometryFile >> speed;
      correctionValues.sideward = maxStepSizeY / speed;
      odometryFile >> speed;
      correctionValues.turning = maxStepSizeR / speed;
      odometryFile >> neckHeight;
      odometryFile >> correctionValues.rotationCenter;
    }
  }
}

bool InvKinWalkingParameters::allFeetOnGround(int currentStep)
{
  int leg;
  for (leg=0;leg<4;leg++)
    if((currentStep+legPhaseIndex[leg])%stepLen > stepLift[FORELEG(leg)?0:1])
      break;
  return (leg==4);
}

void InvKinWalkingParameters::copyValuesFrom(const InvKinWalkingParameters & paramsToCopy)
{
  strcpy(this->name, paramsToCopy.name);
  this->footMode = paramsToCopy.footMode;
  this->foreHeight = paramsToCopy.foreHeight;
  this->foreWidth = paramsToCopy.foreWidth; 
  this->foreCenterX = paramsToCopy.foreCenterX;
  this->hindHeight = paramsToCopy.hindHeight; 
  this->hindWidth = paramsToCopy.hindWidth;  
  this->hindCenterX = paramsToCopy.hindCenterX;
  this->foreFootLift = paramsToCopy.foreFootLift;
  this->hindFootLift = paramsToCopy.hindFootLift;
  this->foreFootTilt = paramsToCopy.foreFootTilt;
  this->hindFootTilt = paramsToCopy.hindFootTilt;
  this->legSpeedFactorX = paramsToCopy.legSpeedFactorX;
  this->legSpeedFactorY = paramsToCopy.legSpeedFactorY;
  this->legSpeedFactorR = paramsToCopy.legSpeedFactorR;
  this->maxStepSizeX = paramsToCopy.maxStepSizeX;
  this->maxStepSizeY = paramsToCopy.maxStepSizeY;
  this->maxSpeedXChange = paramsToCopy.maxSpeedXChange;
  this->maxSpeedYChange = paramsToCopy.maxSpeedYChange;
  this->maxRotationChange = paramsToCopy.maxRotationChange;
  this->counterRotation = paramsToCopy.counterRotation;
  this->stepLen = paramsToCopy.stepLen;
  this->groundPhase[0] = paramsToCopy.groundPhase[0];
  this->liftPhase[0] = paramsToCopy.liftPhase[0];
  this->loweringPhase[0] = paramsToCopy.loweringPhase[0];
  this->groundPhase[1] = paramsToCopy.groundPhase[1];
  this->liftPhase[1] = paramsToCopy.liftPhase[1];
  this->loweringPhase[1] = paramsToCopy.loweringPhase[1];
  this->legPhase[0] = paramsToCopy.legPhase[0];
  this->legPhase[1] = paramsToCopy.legPhase[1];
  this->legPhase[2] = paramsToCopy.legPhase[2];
  this->legPhase[3] = paramsToCopy.legPhase[3];
  this->bodyShiftX = paramsToCopy.bodyShiftX;
  this->bodyShiftY = paramsToCopy.bodyShiftY;
  this->bodyShiftOffset = paramsToCopy.bodyShiftOffset;
}

In& operator>>(In& stream,InvKinWalkingParameters& walkingParameters)
{
  stream >> walkingParameters.name;
  int m;
  stream >> m;
  walkingParameters.footMode = (InvKinWalkingParameters::FootMode)m;
  stream >> walkingParameters.foreHeight;
  stream >> walkingParameters.foreWidth; 
  stream >> walkingParameters.foreCenterX;
  stream >> walkingParameters.hindHeight; 
  stream >> walkingParameters.hindWidth;  
  stream >> walkingParameters.hindCenterX;
  stream >> walkingParameters.foreFootLift;
  stream >> walkingParameters.hindFootLift;
  stream >> walkingParameters.foreFootTilt;
  stream >> walkingParameters.hindFootTilt;
  stream >> walkingParameters.legSpeedFactorX;
  stream >> walkingParameters.legSpeedFactorY;
  stream >> walkingParameters.legSpeedFactorR;
  stream >> walkingParameters.maxStepSizeX;
  stream >> walkingParameters.maxStepSizeY;
  stream >> walkingParameters.maxSpeedXChange;
  stream >> walkingParameters.maxSpeedYChange;
  stream >> walkingParameters.maxRotationChange;
  stream >> walkingParameters.counterRotation;
  stream >> walkingParameters.stepLen;
  stream >> walkingParameters.groundPhase[0];
  stream >> walkingParameters.liftPhase[0];
  stream >> walkingParameters.loweringPhase[0];
  stream >> walkingParameters.groundPhase[1];
  stream >> walkingParameters.liftPhase[1];
  stream >> walkingParameters.loweringPhase[1];
  stream >> walkingParameters.legPhase[0];
  stream >> walkingParameters.legPhase[1];
  stream >> walkingParameters.legPhase[2];
  stream >> walkingParameters.legPhase[3];
  stream >> walkingParameters.bodyShiftX;
  stream >> walkingParameters.bodyShiftY;
  stream >> walkingParameters.bodyShiftOffset;

  return stream;
}
 
Out& operator<<(Out& stream, const InvKinWalkingParameters& walkingParameters)
{
  stream << walkingParameters.name;
  stream << (int)walkingParameters.footMode;
  stream << walkingParameters.foreHeight;
  stream << walkingParameters.foreWidth; 
  stream << walkingParameters.foreCenterX;
  stream << walkingParameters.hindHeight; 
  stream << walkingParameters.hindWidth;  
  stream << walkingParameters.hindCenterX;
  stream << walkingParameters.foreFootLift;
  stream << walkingParameters.hindFootLift;
  stream << walkingParameters.foreFootTilt;
  stream << walkingParameters.hindFootTilt;
  stream << walkingParameters.legSpeedFactorX;
  stream << walkingParameters.legSpeedFactorY;
  stream << walkingParameters.legSpeedFactorR;
  stream << walkingParameters.maxStepSizeX;
  stream << walkingParameters.maxStepSizeY;
  stream << walkingParameters.maxSpeedXChange;
  stream << walkingParameters.maxSpeedYChange;
  stream << walkingParameters.maxRotationChange;
  stream << walkingParameters.counterRotation;
  stream << walkingParameters.stepLen;
  stream << walkingParameters.groundPhase[0];
  stream << walkingParameters.liftPhase[0];
  stream << walkingParameters.loweringPhase[0];
  stream << walkingParameters.groundPhase[1];
  stream << walkingParameters.liftPhase[1];
  stream << walkingParameters.loweringPhase[1];
  stream << walkingParameters.legPhase[0];
  stream << walkingParameters.legPhase[1];
  stream << walkingParameters.legPhase[2];
  stream << walkingParameters.legPhase[3];
  stream << walkingParameters.bodyShiftX;
  stream << walkingParameters.bodyShiftY;
  stream << walkingParameters.bodyShiftOffset;

  return stream;
}

/*
* Change log :
* 
* $Log: InvKinWalkingParameters.cpp,v $
* Revision 1.3  2004/07/10 00:13:54  spranger
* renaming for coderelease and preparations for gt2005
*
* Revision 1.2  2004/07/07 15:10:57  dueffert
* copy'n'paste invkin added for ud
*
* Revision 1.1.1.1  2004/05/22 17:22:31  cvsadm
* created new repository GT2004_WM
*
* Revision 1.4  2004/03/27 10:31:58  dueffert
* separation of engine and parameters completed
*
* Revision 1.3  2004/03/26 10:57:04  thomas
* added name of parameters to streaming operator
* call readValues after setting new parameters
*
* Revision 1.2  2004/03/09 22:32:49  cesarz
* added function for assigning certain parameters
*
* Revision 1.1  2004/02/16 17:52:48  dueffert
* InvKin engine and parameters separated
*
*
*/

---