Modules/WalkingEngine/GT2004ParameterSet.h

Go to the documentation of this file.

/**
* @file GT2004ParameterSet.h
* 
* Definition of class GT2004Parameters and GT2004ParametersSet
*
* @author Uwe Düffert
*/

#ifndef __GT2004Parameters_h_
#define __GT2004Parameters_h_

#include "InvKinWalkingParameters.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Evolution/Individual.h"
#include "Tools/Math/Pose2D.h"
#include <string.h>

class InvKinWalkingParameters;

/**
* @class GT2004Parameters
*
* Parameters for GT2004WalkingEngine
* 
* @author Uwe Düffert
*/
class GT2004Parameters: public Individual
{
public:
  int index;           ///< good parameters now, which index they have in a parametersSet
  Pose2D requestedMotion; ///< these parameters are optimized for this requestedMotion
  Pose2D correctedMotion; ///< these parameters use correctedMotion to get something like requestedMotion as result
  
  double foreHeight;   ///< fore walking height
  double foreWidth;    ///< fore walking width distance robot center to foot
  double foreCenterX;  ///< fore x-axis foot center position
  
  double hindHeight;   ///< hind walking height
  double hindWidth;    ///< hind walking width distance robot center to foot
  double hindCenterX;  ///< hind x-axis foot center position
  
  double foreFootLift; ///< height of lifted fore foots
  double hindFootLift; ///< height of lifted hind foots
  
  double foreFootTilt; ///< tangens of tilt angle for fore foot movement
  double hindFootTilt; ///< tangens of tilt angle for hind foot movement
  
  int stepLen;         ///< number of ticks for one complete step
  
  enum FootMode {
    rectangle = 0,
      halfCircle,
      circle,
      rounded,
      optimized,
      freeFormQuad,
      numOfFootModes
  };
  
  FootMode footMode; ///< how to move the feet
  double groundPhase[2];   ///<part of leg phase with foot on ground, front/hind legs
  double legPhase[4]; ///< relative leg phases, time indexes for lifting each leg
  
double liftPhase[2];     ///<part of leg phase lifting foot, front/hind legs
double loweringPhase[2]; ///<part of leg phase lowering foot, front/hind legs
double bodyShiftX;       ///<value for shifting body away from lifted legs in x direction (mm when leg fully lifted)
double bodyShiftY;       ///<value for shifting body away from lifted legs in y direction (mm when leg fully lifted)
double bodyShiftOffset;  ///<value by which the shift body motion is delayed (relative part of leg phase)

  
  //do we need double freeFormQuadPos[2][4][3];
  //do we need canterZ-front/hind or is changing groundphase enough?
  
  /**
  * joint angles for joints not used for walking
  * these must be jointDataInvalidValue to enable head motion
  * use these for special walks that use the head eg for grabbing the ball
  */
  long headTilt, headPan, headRoll, mouth;
  
  /** Constructor */
  GT2004Parameters(int index=-1,
    Pose2D requestedMotion=Pose2D(0,0,0),
    double foreHeight = 0,
    double foreWidth = 0,
    double foreCenterX = 0,
    double hindHeight = 0,
    double hindWidth= 0,
    double hindCenterX = 0,
    double foreFootLift = 0,
    double hindFootLift = 0,
    double foreFootTilt = 0,
    double hindFootTilt = 0,
    int stepLen = 80,
    int footMode = (int)rectangle,
    double groundPhaseF = 0.5,
double liftPhaseF = 0, //temp for BB support
double loweringPhaseF = 0, //temp for BB support
    double groundPhaseH = 0.5,
double liftPhaseH = 0, //temp for BB support
double loweringPhaseH = 0, //temp for BB support
    double legPhase0 = 0,
    double legPhase1 = 0.5,
    double legPhase2 = 0.5,
    double legPhase3 = 0,
//double bodyShiftX = 0, //temp for BB support
//double bodyShiftY = 0, //temp for BB support
//double bodyShiftOffset = 0, //temp for BB support
    long headTilt = jointDataInvalidValue, 
    long headPan = jointDataInvalidValue, 
    long headRoll = jointDataInvalidValue, 
    long mouth = jointDataInvalidValue) :
  index(index),
    requestedMotion(requestedMotion),
    correctedMotion(requestedMotion),
    foreHeight(foreHeight),
    foreWidth(foreWidth),
    foreCenterX(foreCenterX),
    hindHeight(hindHeight),
    hindWidth(hindWidth),
    hindCenterX(hindCenterX),
    foreFootLift(foreFootLift),
    hindFootLift(hindFootLift),
    foreFootTilt(foreFootTilt),
    hindFootTilt(hindFootTilt),
    stepLen(stepLen),
//bodyShiftX(bodyShiftX), //temp for BB support
//bodyShiftY(bodyShiftY), //temp for BB support
//bodyShiftOffset(bodyShiftOffset), //temp for BB support
    footMode((FootMode)footMode), 
    headTilt(headTilt), 
    headPan(headPan), 
    headRoll(headRoll), 
    mouth(mouth)
  {
    legPhase[0]=legPhase0;
    legPhase[1]=legPhase1;
    legPhase[2]=legPhase2;
    legPhase[3]=legPhase3;
    groundPhase[0]=groundPhaseF;
liftPhase[0]=liftPhaseF; //temp for BB support
loweringPhase[0]=loweringPhaseF; //temp for BB support
    groundPhase[1]=groundPhaseH;
liftPhase[1]=liftPhaseH; //temp for BB support
loweringPhase[1]=loweringPhaseH; //temp for BB support
  }
  
  /** Constructor */
  GT2004Parameters(const InvKinWalkingParameters& invKinParam);
  
  /** report a measured (and may be averaged) speed of that walk parameters with this function.
  * It will modify correctedMotion accordingly if this seems to be usefull.
  * @param real measured motion speed in real life
  * @return true if correctedMotion was successfully changed, otherwise its unlikely that requestedMotion is reachable with this params - you might want to evolve params or reduce requestedMotion
  */
  bool reportRealMotion(const Pose2D& real);
  
  virtual void getDimension(int& dim1, int& dim2)
  {
    dim1=19;
    dim2=1;
  }
  
  virtual void getValue(int index1, int index2, double& min, double& max, double& value, ValueType& type)
  {
    switch(index1)
    {
      case 0: value=correctedMotion.translation.x; min=1; max=60; type=valueDouble; break;
      case 1: value=correctedMotion.translation.y; min=1; max=60; type=valueDouble; break;

      case 2: value=foreHeight; min=30; max=170; type=valueDouble; break;
      case 3: value=foreWidth; min=40; max=120; type=valueDouble; break;
      case 4: value=foreCenterX; min=-20; max=80; type=valueDouble; break;

      case 5: value=hindHeight; min=40; max=190; type=valueDouble; break;
      case 6: value=hindWidth; min=40; max=140; type=valueDouble; break;
      case 7: value=hindCenterX; min=-80; max=20; type=valueDouble; break;

      case 8: value=foreFootLift; min=0; max=40; type=valueDouble; break;
      case 9: value=hindFootLift; min=0; max=40; type=valueDouble; break;
      case 10: value=foreFootTilt; min=-0.44; max=0.44; type=valueDouble; break;
      case 11: value=hindFootTilt; min=-0.44; max=0.44; type=valueDouble; break;

      case 12: value=stepLen; min=20; max=120; type=valueInt; break;

      case 13: value=groundPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
      case 14: value=groundPhase[1]; min=0.01; max=0.99; type=valueDouble; break;

      case 15: value=legPhase[0]; min=0; max=0.99; type=valueDouble; break;
      case 16: value=legPhase[1]; min=0; max=0.99; type=valueDouble; break;
      case 17: value=legPhase[2]; min=0; max=0.99; type=valueDouble; break;
      case 18: value=legPhase[3]; min=0; max=0.99; type=valueDouble; break;

      //case 19: value=liftPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
      //case 20: value=loweringPhase[0]; min=0.01; max=0.99; type=valueDouble; break;
      //case 21: value=liftPhase[1]; min=0.01; max=0.99; type=valueDouble; break;
      //case 22: value=loweringPhase[1]; min=0.01; max=0.99; type=valueDouble; break;
    }
  }
 
  virtual void setValue(int index1, int index2, double value)
  {
    switch(index1)
    {
      case 0: correctedMotion.translation.x=value; break;
      case 1: correctedMotion.translation.y=value; break;

      case 2: foreHeight=value; break;
      case 3: foreWidth=value; break;
      case 4: foreCenterX=value; break;

      case 5: hindHeight=value; break;
      case 6: hindWidth=value; break;
      case 7: hindCenterX=value; break;

      case 8: foreFootLift=value; break;
      case 9: hindFootLift=value; break;
      case 10: foreFootTilt=value; break;
      case 11: hindFootTilt=value; break;

      case 12: stepLen=(int)value; break;

      case 13: groundPhase[0]=value; break;
      case 14: groundPhase[1]=value; break;

      case 15: legPhase[0]=value; break;
      case 16: legPhase[1]=value; break;
      case 17: legPhase[2]=value; break;
      case 18: legPhase[3]=value; break;

      //case 19: liftPhase[0]=value; break;
      //case 20: loweringPhase[0]=value; break;
      //case 21: liftPhase[1]=value; break;
      //case 22: loweringPhase[1]=value; break;
    }
  }
   
#define interpolateInvalid(v1,v2,factor1) \
  (((v1==jointDataInvalidValue)||(v2==jointDataInvalidValue))? \
  ((factor1<0.5)?v2:v1):(factor1*v1+(1-factor1)*v2));
  
  inline void interpolate(GT2004Parameters& p1, GT2004Parameters& p2, double factor1)
  {
    index = -1; //these are merged parameters, they cant be found with index in a parametersSet
    requestedMotion.translation = p1.requestedMotion.translation*factor1+p2.requestedMotion.translation*(1-factor1);
    requestedMotion.rotation = factor1*p1.requestedMotion.rotation+(1-factor1)*p2.requestedMotion.rotation;
    correctedMotion.translation = p1.correctedMotion.translation*factor1+p2.correctedMotion.translation*(1-factor1);
    correctedMotion.rotation= factor1*p1.correctedMotion.rotation+(1-factor1)*p2.correctedMotion.rotation;
    
    foreHeight=factor1*p1.foreHeight+(1-factor1)*p2.foreHeight;
    foreWidth=factor1*p1.foreWidth+(1-factor1)*p2.foreWidth;
    foreCenterX=factor1*p1.foreCenterX+(1-factor1)*p2.foreCenterX;
    
    hindHeight=factor1*p1.hindHeight+(1-factor1)*p2.hindHeight;
    hindWidth=factor1*p1.hindWidth+(1-factor1)*p2.hindWidth;
    hindCenterX=factor1*p1.hindCenterX+(1-factor1)*p2.hindCenterX;
    
    foreFootLift=factor1*p1.foreFootLift+(1-factor1)*p2.foreFootLift;
    hindFootLift=factor1*p1.hindFootLift+(1-factor1)*p2.hindFootLift;
    foreFootTilt=factor1*p1.foreFootTilt+(1-factor1)*p2.foreFootTilt;
    hindFootTilt=factor1*p1.hindFootTilt+(1-factor1)*p2.hindFootTilt;
    
    stepLen=(int)(factor1*p1.stepLen+(1-factor1)*p2.stepLen);
    footMode=(factor1<0.5)?p2.footMode:p1.footMode;
    
    groundPhase[0]=factor1*p1.groundPhase[0]+(1-factor1)*p2.groundPhase[0];
liftPhase[0]=factor1*p1.liftPhase[0]+(1-factor1)*p2.liftPhase[0]; //temp for BB support
loweringPhase[0]=factor1*p1.loweringPhase[0]+(1-factor1)*p2.loweringPhase[0]; //temp for BB support
    groundPhase[1]=factor1*p1.groundPhase[1]+(1-factor1)*p2.groundPhase[1];
liftPhase[1]=factor1*p1.liftPhase[1]+(1-factor1)*p2.liftPhase[1]; //temp for BB support
loweringPhase[1]=factor1*p1.loweringPhase[1]+(1-factor1)*p2.loweringPhase[1]; //temp for BB support
    
    legPhase[0]=factor1*p1.legPhase[0]+(1-factor1)*p2.legPhase[0];
    legPhase[1]=factor1*p1.legPhase[1]+(1-factor1)*p2.legPhase[1];
    legPhase[2]=factor1*p1.legPhase[2]+(1-factor1)*p2.legPhase[2];
    legPhase[3]=factor1*p1.legPhase[3]+(1-factor1)*p2.legPhase[3];
    
//bodyShiftX=factor1*p1.bodyShiftX+(1-factor1)*p2.bodyShiftX; //temp for BB support
//bodyShiftY=factor1*p1.bodyShiftY+(1-factor1)*p2.bodyShiftY; //temp for BB support
//bodyShiftOffset=factor1*p1.bodyShiftOffset+(1-factor1)*p2.bodyShiftOffset; //temp for BB support
    
    headTilt=(long)interpolateInvalid(p1.headTilt,p2.headTilt,factor1);
    headPan=(long)interpolateInvalid(p1.headPan,p2.headPan,factor1);
    headRoll=(long)interpolateInvalid(p1.headRoll,p2.headRoll,factor1);
    mouth=(long)interpolateInvalid(p1.mouth,p2.mouth,factor1);
    
    //    neckHeight=factor1*p1.neckHeight+(1-factor1)*p2.neckHeight;
  }
};

/**
* Streaming operator that reads GT2004Parameters from a stream.
* @param stream The stream from which is read.
* @param walkingParameters The GT2004Parameters object.
* @return The stream.
*/ 
In& operator>>(In& stream,GT2004Parameters& walkingParameters);

/**
* Streaming operator that writes GT2004Parameters to a stream.
* @param stream The stream to write on.
* @param walkingParameters The GT2004Parameters object.
* @return The stream.
*/ 
Out& operator<<(Out& stream, const GT2004Parameters& walkingParameters);

/**
* @class GT2004ParametersSet
*
* ParametersSet for GT2004WalkingEngine
* 
* @author Uwe Düffert
*/
class GT2004ParametersSet
{
public:
/**
* enum describing indexes of a set of parameters
  */
  enum IndexName {
    much_rturn_fast, much_rturn_med, much_rturn_slow, stand, much_lturn_slow, much_lturn_med, much_lturn_fast,
    
    med_rturn_min180_slow, med_rturn_min135_slow, med_rturn_min90_slow, med_rturn_min45_slow,
    med_rturn_0_slow, med_rturn_45_slow, med_rturn_90_slow, med_rturn_135_slow,
    few_rturn_min180_slow, few_rturn_min135_slow, few_rturn_min90_slow, few_rturn_min45_slow,
    few_rturn_0_slow, few_rturn_45_slow, few_rturn_90_slow, few_rturn_135_slow,
    no_turn_min180_slow, no_turn_min135_slow, no_turn_min90_slow, no_turn_min45_slow,
    no_turn_0_slow, no_turn_45_slow, no_turn_90_slow, no_turn_135_slow,
    few_lturn_min180_slow, few_lturn_min135_slow, few_lturn_min90_slow, few_lturn_min45_slow,
    few_lturn_0_slow, few_lturn_45_slow, few_lturn_90_slow, few_lturn_135_slow,
    med_lturn_min180_slow, med_lturn_min135_slow, med_lturn_min90_slow, med_lturn_min45_slow,
    med_lturn_0_slow, med_lturn_45_slow, med_lturn_90_slow, med_lturn_135_slow,
    
    med_rturn_min180_med, med_rturn_min135_med, med_rturn_min90_med, med_rturn_min45_med,
    med_rturn_0_med, med_rturn_45_med, med_rturn_90_med, med_rturn_135_med,
    few_rturn_min180_med, few_rturn_min135_med, few_rturn_min90_med, few_rturn_min45_med,
    few_rturn_0_med, few_rturn_45_med, few_rturn_90_med, few_rturn_135_med,
    no_turn_min180_med, no_turn_min135_med, no_turn_min90_med, no_turn_min45_med,
    no_turn_0_med, no_turn_45_med, no_turn_90_med, no_turn_135_med,
    few_lturn_min180_med, few_lturn_min135_med, few_lturn_min90_med, few_lturn_min45_med,
    few_lturn_0_med, few_lturn_45_med, few_lturn_90_med, few_lturn_135_med,
    med_lturn_min180_med, med_lturn_min135_med, med_lturn_min90_med, med_lturn_min45_med,
    med_lturn_0_med, med_lturn_45_med, med_lturn_90_med, med_lturn_135_med,
    
    med_rturn_min180_fast, med_rturn_min135_fast, med_rturn_min90_fast, med_rturn_min45_fast,
    med_rturn_0_fast, med_rturn_45_fast, med_rturn_90_fast, med_rturn_135_fast,
    few_rturn_min180_fast, few_rturn_min135_fast, few_rturn_min90_fast, few_rturn_min45_fast,
    few_rturn_0_fast, few_rturn_45_fast, few_rturn_90_fast, few_rturn_135_fast,
    no_turn_min180_fast, no_turn_min135_fast, no_turn_min90_fast, no_turn_min45_fast,
    no_turn_0_fast, no_turn_45_fast, no_turn_90_fast, no_turn_135_fast,
    few_lturn_min180_fast, few_lturn_min135_fast, few_lturn_min90_fast, few_lturn_min45_fast,
    few_lturn_0_fast, few_lturn_45_fast, few_lturn_90_fast, few_lturn_135_fast,
    med_lturn_min180_fast, med_lturn_min135_fast, med_lturn_min90_fast, med_lturn_min45_fast,
    med_lturn_0_fast, med_lturn_45_fast, med_lturn_90_fast, med_lturn_135_fast,
    
    numberOfParameters
  };
  
  //rotationWalkRatio determines, in which cone we are
  //double rotationWalkRatio=atan2(currentRequest.rotation/2.7,currentRequest.translation.abs()/300)/pi;
  //-> -1=turn right only, 0=walk only [8], 1=turn left only [1]
  //double movementStrength=sqrt((currentRequest.rotation/2.7)*(currentRequest.rotation/2.7)+((currentRequest.translation.x*currentRequest.translation.x+currentRequest.translation.y*currentRequest.translation.y)/(300*300)));
  GT2004Parameters rotationOnly[7]; //[-max, -med, -min, 0, min, med, max]
  GT2004Parameters withWalk[3][5][8];
  //3 speeds [min, med, max] *                  //speedIndex
  //5 rotWalkRatios [-0.3, -0.1, 0, 0.1, 0.3] * //ratioIndex
  //8 directions [-pi, -3/4pi, -pi/2, ...]      //directionIndex
  
  /** buffers for merging up to 8 parameter sets around the requested one */
  GT2004Parameters llBuf,luBuf,ulBuf,uuBuf,lBuf,uBuf,mBuf;
  
  /** pointers to merged parameter sets to minimize merging afford */
  GT2004Parameters *lowRatioLowSpeed, *lowRatioUpSpeed, *upRatioLowSpeed, *upRatioUpSpeed;
  GT2004Parameters *lowSpeed, *upSpeed, *mergedParameters;
  
  /** default constructor */
  GT2004ParametersSet();
  
  /** calculate merged parameters set according to motion request
  * @param currentRequest the request the merged parameters set shall be optimized for
  */
  void calculateMergedParameterSet(Pose2D& currentRequest);
  
  /** return one of the GT2004Parameters in the tables according to index */
  GT2004Parameters* getParameters(int index);

  /** returns true if the given index is for a parametersset designed for max speed */
  bool isMaxSpeedIndex(int index);
  
  /** returns the name string of a certain index
  * @param index the index of the parameters we would like to get the name of
  * @return the name of the index as char*
  */
  static char* getIndexString(int index);

  /** return the index of the left right mirrored parameters set
  * @param index the index of the parameters set you want to get the mirrored version of
  * @return the index of the left right mirrored parameters set or -1 if that doesnt exist
  */
  static int getIndexOfMirror(int index);
  
  /** changes the mirrored parameters set to match the given one except for sign
  * @param index the index of the source parameters set, the mirrored one shall look like that
  */
  void mirrorThis(int index);

  /** changes all right turn parameters to match the according left turn parameters
  */
  void mirrorLeftTurnToRightTurn();

  /** some functions to access and modify polar parts of a (motionRequest) Pose2D */
  static double getSpeed(const Pose2D& request);
  static double getRatio(const Pose2D& request);
  static double getDirection(const Pose2D& request);
  static void setSpeed(Pose2D& request, double speed);
  static void setRatio(Pose2D& request, double ratio);
  static void setDirection(Pose2D& request, double direct);
  
  /** tries to load the parameters set from a file 
  * @return true, if reading was successful, false if set is unchanged
  */
  bool load(char* filename=0);
  
  /** saves the parameters set to a file */
  void save(char* filename=0);
};

/**
* Streaming operator that reads GT2004ParametersSets from a stream.
* @param stream The stream from which is read.
* @param walkingParametersSet The GT2004ParametersSet object.
* @return The stream.
*/ 
In& operator>>(In& stream,GT2004ParametersSet& walkingParametersSet);

/**
* Streaming operator that writes GT2004ParametersSets to a stream.
* @param stream The stream to write on.
* @param walkingParametersSet The GT2004ParametersSet object.
* @return The stream.
*/ 
Out& operator<<(Out& stream, const GT2004ParametersSet& walkingParametersSet);

#endif// __GT2004Parameters_h_

/*
* Change log :
* 
* $Log: GT2004ParameterSet.h,v $
* Revision 1.1  2004/07/10 00:18:30  spranger
* renamed (readded) for coderelease
*
* Revision 1.4  2004/07/07 15:10:57  dueffert
* copy'n'paste invkin added for ud
*
* Revision 1.3  2004/06/20 18:15:36  dueffert
* evolution converted from InvKin to UD
*
* Revision 1.2  2004/05/24 19:32:36  dueffert
* update to current version
*
* Revision 1.17  2004/05/24 13:03:22  dueffert
* complete lturn->rturn mirroring
*
* Revision 1.16  2004/05/20 17:20:13  dueffert
* request change methods moved to cpp
*
* Revision 1.15  2004/05/12 14:21:03  dueffert
* support for speed/ratio/direction improved
*
* Revision 1.14  2004/05/03 18:52:39  dueffert
* comments corrected; max speed / rest distinction added
*
* Revision 1.13  2004/04/21 05:56:14  dueffert
* (temporary?) using BB for noturn_fastforward
*
* Revision 1.12  2004/03/17 11:56:41  dueffert
* initialization and naming corrected
*
* Revision 1.11  2004/03/15 12:35:13  dueffert
* enum element renamed
*
* Revision 1.10  2004/03/03 08:31:29  dueffert
* load and save have design specific mode now
*
* Revision 1.9  2004/03/01 14:55:08  dueffert
* reportRealMotion improved; now I use result of omni optimization as initialization
*
* Revision 1.8  2004/02/29 17:28:22  dueffert
* UDParameters have char* names now
*
* Revision 1.7  2004/02/29 13:38:47  dueffert
* symmetries in UDParametersSet handled
*
* Revision 1.6  2004/02/23 16:44:32  dueffert
* index names, load and save added
*
* Revision 1.5  2004/02/18 14:50:38  dueffert
* UDParameters improved
*
* Revision 1.4  2004/02/16 17:56:32  dueffert
* InvKin engine and parameters separated
*
* Revision 1.3  2003/12/11 22:52:47  loetzsch
* fixed doxygen bugs
*
* Revision 1.2  2003/12/09 14:18:24  dueffert
* numerous improvements
*
* Revision 1.1  2003/12/02 18:07:14  dueffert
* first working not yet calibrated version of UDWalkingEngine added
*
*
*/

---