Modules/WalkingEngine/BB2004InvKinWalkingEngine.h

Go to the documentation of this file.

/**
* @file BB2004InvKinWalkingEngine.h
* 
* 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>
*/

#ifndef __BB2004InvKinWalkingEngine_h_
#define __BB2004InvKinWalkingEngine_h_

#include "InvKinWalkingEngine.h"
#include "Tools/Evolution/BB2004Evo.h"
#include "Tools/RingBuffer.h"

/**
* @class BBInvKinIndividual
*
* The class represents an individual for a set of walking parameters.
*/
class BBInvKinIndividual : public BBIndividual
{
private:
  enum 
  {
    numOfGenes = 27 /**< The number of genes of the individual. */
  };
  static double geneScale[numOfGenes]; /**< An array that estimates the range of value of a certain gene. */
  double genes[numOfGenes]; /**< The genes. */
  double fitness, /**< The fitness of this individual. */
         speed, /**< The speed of the walk using this individual. */
         smoothness; /**< The smooothness of the walk using this individual. */

public:
  /**
  * Constructor.
  */
  BBInvKinIndividual() {fitness = 1;}

  /**
  * Constructor.
  * @param parameters The parameters that copied into the genes.
  */
  BBInvKinIndividual(const InvKinWalkingParameters& parameters);

  /**
  * The function initializes the current individual with another one.
  * @param initial The other individual the current one is initialized with.
  */
  virtual void init(const BBIndividual& initial);
  
  /**
  * The function interpolates between the current individual and another one.
  * The result is stored in the current individual.
  * @param other The other individual the current one is interpolated with.
  */
  virtual void interpolate(const BBIndividual& other);

  /**
  * The function extrapolates between the current individual and another one.
  * @param other The other individual the current one is extrapolated with.
  */
  virtual void extrapolate(const BBIndividual& other);

  /**
  * The function mutates the current individual.
  */
  virtual void mutate();

  /**
  * The function returns the fitness of the individual.
  * @return The fitness as a number >= 0. A higher result means a higher fitness.
  */
  virtual double getFitness() const {return fitness;}

  /**
  * The function returns the speed of the walk of the individual.
  * @return The speed.
  */
  double getSpeed() const {return speed;}

  /**
  * The function returns the smoothness of the walk of the individual.
  * @return The smoothness.
  */
  double getSmoothness() const {return smoothness;}

  /**
  * The function sets the speed and the smoothness of the walk using the individual.
  * @param speed The speed.
  * @param smoothness The smoothness.
  */
  void setWalkCharacteristics(double speed, double smoothness);

  /**
  * The function copies the genes into walking parameters.
  * @param parameters The parameters the genes are copied to.
  */
  void getParameters(InvKinWalkingParameters& parameters) const;

  /**
  * The function prints the genes.
  */
  void dump() const;
};

/**
* @class BB2004InvKinWalkingEngine
*
* This class implements a walking engine that optimizes its parameters
* using evolutionary algorithms.
*/

class BB2004InvKinWalkingEngine : public WalkingEngine
{
private:
  /**
  * @class FootPositions
  * The class represents a pair of positions of the back feet.
  */
  class FootPositions
  {
  public:
    Vector3<double> positions[2]; /**< The 3-D positions of the back feet. */
    bool onGround[2]; /**< Were the feet on the ground? */

    /**
    * Constructor.
    * The feet have no contact to the ground.
    */
    FootPositions() {onGround[0] = onGround[1] = false;}
  };

  InvKinWalkingEngine* pEngine; /**< The address of the actual walking engine. */
  ERS7EvolveWalkingParameters parameters[2], /**< Two parameter sets that are interchangingly used. */
                              bestParameters, /**< The best parameters found so far. */
                              forward, /**< Parameters for forward walking. */
                              fastForward, /**< Parameters for fast forward walking. */
                              backward, /**< Parameters for backward walking. */
                              stand; /**< Parameters for standing. */
  int currentParameters; /**< The index that defines which of the two "parameters" are currently used. */
  int currentIndividual; /**< The index of the current individual in the population. */
  int currentFrame; /**< The index of the current frame that measures the performance of the current individual. */
  int evolutions; /**< The number of evolutions done so far. */
  double bestFitness; /**< The best overall fitness found so far. */
  Pose2D odometrySpeed; /**< The current speed of the robot. */
  double odometryScaleX, /**< The scale factor for odometry in x direction. */
         odometryScaleY, /**< The scale factor for odometry in y direction. */
         odometryScaleRotation, /**< The scale factor for odometry rotation. */
         odometryCounterRotation; /**< A factor that compensates rotation resulting from sideward motion. */
  RingBuffer<Vector2<double>, 100> odometrySums[2]; /**< The current state of the odometers. */
  FootPositions prevPositions; /**< The previous positions of the feet. */
  enum 
  {
    numOfIndividuals = 15, /**< The number of individuals in the population. */
    numOfFramesPerIndividual = 625, /**< The number of frames tested per individual. */
    ignoreFrames = 250, /**< The number of frames to ignore at the beginning of an individual. */
    useFrames = numOfFramesPerIndividual - ignoreFrames /**< The number of frames that are assessed per individual. */
  };
  RingBuffer<SensorData, numOfFramesPerIndividual> buffer; /**< A buffer for previous sensor readings. */
  Pose2D measured, /**< Accumulated odometry for current individual. */
         target; /**< Accumulated request motion for current individual. */
  BBPopulation<BBInvKinIndividual>* population; /**< The population of individuals. */

  /**
  * The function optimizes the gait for the current motion request.
<<<<<<< BB2004InvKinWalkingEngine.h
  * @param walkRequest The current walk request.
=======
  * @param walkRequest The current motion request.
>>>>>>> 1.7
  */
  void learn(const WalkRequest& walkRequest);

  /**
  * The function sets a new parameter set using the individual specified.
  * @param p The individual that is used to set the walking parameters.
  */
  void setParameters(const BBIndividual& p);

  /**
  * The function loads a set of walking parameters.
  * @param parameters The variable that receives the loaded parameters.
  * @param name The name of the parameters. It will automatically be preceeded by the 
  *             current robot design.
  */
  void loadParameters(InvKinWalkingParameters& parameters, const char* name);

  /**
  * The function calculates the standard deviation of the last "useFrames" measurements of a certain sensor.
  * @param s The sensor.
  * @return The standard deviation of the measurements of s.
  */
  double stdV(SensorData::sensors s) const;

  /**
  * The function updates the odometry.
  */
  void updateOdometry();

public:
  /**
  * Constructor.
  * @param pEngine The actual walking engine used.
  */
  BB2004InvKinWalkingEngine(InvKinWalkingEngine* pEngine);

  /**
  * Destructor.
  */
  ~BB2004InvKinWalkingEngine() {delete population;}

  /**
  * The function executes the walking engine.
  * @param jointData The joints already set by other modules.
  * @param walkRequest The motion request to execute.
  * @param positionInWalkingCycle The position in walking cycle requested
  * @return Is it forbidden to change to another kind of motion (e.g. special action)?
  */
  virtual bool executeParameterized(JointData& jointData, const WalkRequest& walkRequest, double positionInWalkingCycle);

  /**
  * The function handles a message sent by the PC.
  * @param message The message sent.
  * @return Was the message handled by this function?
  */
  bool handleMessage(InMessage& message);
};

#endif // __BB2004InvKinWalkingEngine_h_

/*
* Change log :
* 
* $Log: BB2004InvKinWalkingEngine.h,v $
* Revision 1.8  2004/09/09 11:37:39  wachter
* - Fixed some more doxygen-errors
*
* Revision 1.7  2004/09/09 10:15:54  spranger
* fixed doxygen-errors
*
* Revision 1.6  2004/06/14 16:55:19  juengel
* Removed some WalkingEngineParameterSets.
*
* Revision 1.5  2004/06/07 18:47:50  spranger
* extended interface of executeParametrized by positionInWalkCycle
*
* Revision 1.4  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.3  2004/05/27 12:51:14  roefer
* Minor error fixed
*
* Revision 1.2  2004/05/26 09:29:30  roefer
* Remove redundant code
*
* Revision 1.2  2004/03/22 21:58:13  roefer
* True odometry
*
* Revision 1.1  2004/03/03 21:17:09  roefer
* For the sake of consistency, renamed BB* to BB2004*
*
* Revision 1.6  2004/02/16 17:56:32  dueffert
* InvKin engine and parameters separated
*
* Revision 1.5  2004/02/06 20:23:00  roefer
* Further improvements
*
* Revision 1.4  2004/01/28 21:55:49  roefer
* RobotDimensions revised
*
* Revision 1.3  2004/01/05 22:59:53  roefer
* Different parameters for ERS-210 and ERS-7
*
* Revision 1.2  2004/01/02 13:37:27  roefer
* Handle if a single individual was drawn too often
*
* Revision 1.1  2003/12/29 15:48:54  roefer
* Bremen Byters evo walking added
*
*/

---