Modules/WalkingEngine/GT2004WalkingEngine.h

Go to the documentation of this file.

/**
* @file GT2004WalkingEngine.h
* 
* Definition of class GT2004WalkingEngine
*
* @author Uwe Düffert
*/

#ifndef __GT2004WalkingEngine_h_
#define __GT2004WalkingEngine_h_

#include "WalkingEngine.h"
#include "InvKinWalkingEngine.h"
#include "GT2004ParameterSet.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Evolution/Individual.h"


/**
* @class GT2004WalkingEngine
*
* Walking engine based on 
* calculation of rectangular foot movement and
* inverse kinematics
*
* @author Uwe Dueffert
*/
class GT2004WalkingEngine : public WalkingEngine
{
public:
/**
* Constructor.
* @param interfaces The paramters of the WalkingEngine module.
  */
  GT2004WalkingEngine(const WalkingEngineInterfaces& interfaces);
  
  /**
  * Destructor
  */
  ~GT2004WalkingEngine();
  
  /** Executes the engine */
  virtual bool executeParameterized(JointData& jointData, const WalkRequest& walkRequest, double positionInWalkingCycle);
  
  /** 
  * Called from a MessageQueue to distribute messages 
  * @param message The message that can be read.
  * @return true if the message was read (handled).
  */
  virtual bool handleMessage(InMessage& message);
  
  /**
  * find correct table entry to update (in turnOnly and withWalk):
  * find the entry most similar to nextParameters, copy the old
  * value to lastParameters, set parametersToChange and start
  * interpolation
  * @param steps Number of interpolation steps from old to new parameters
  */
  void setNextParameters(int steps);
  
private:
  /** a set of GT2004Parameters optimized for different requests */
  GT2004ParametersSet paramSet;
  
  //!@name parameter set interpolation
  //!@{
  /** Points to parameter set currently in use */
  GT2004Parameters* currentParameters;
  
  /** Fixed parameter set for InvKin compatibility */
  GT2004Parameters fixedParameters;
  
  /** Pointer to the parameter set that shall be changed by interpolation */
  GT2004Parameters *parametersToChange;
  
  /** Next parameters of this walk, target of current interpolation */
  GT2004Parameters nextParameters;
  
  /** Last parameters of this walk, origin of current interpolation */
  GT2004Parameters lastParameters;
  
  /** Counts parameter set interpolation steps */
  int paramInterpolCount;
  
  /** Stores the length of the current parameter set interpolation */
  int paramInterpolLength;
  
  /** Stores precise version of currentStep for all kinds of interpolation */
  double currentStepPercentage;
  
  /** Initialize interpolation of WalkingParameterSets */
  void initParametersInterpolation(int changeSteps);
  
  /** Calculate next step in parameterset interpolation and increase currentStep if walk is true*/
  void nextParametersInterpolation(bool walk);
  
  //!@}
  //!@name fixed gait parameters resulting from merged parameter set
  //!@{
  int groundTime[2];      ///< number of ticks foot is on ground
  int liftTime[2];        ///< number of ticks for lifting foot
  int loweringTime[2];    ///< number of ticks for lowering foot
  int airTime[2];         ///< number of ticks foot is in air
  int stepLift[2];        ///< time index for lifting foot
  int stepAir[2];         ///< time index when foot is fully lifted
  int stepLower[2];       ///< time index for lowering foot
  int legPhaseIndex[4];   ///< leg phases, time indexes for lifting each leg
  int firstStep;          ///< time index for first time all feet are on ground
  //!@}
  
  /** decides whether we use merging of rotationOnly and withWalk or fixedParameters */
  bool useFixedParameters;

  /** true if stuff like legSpeed has to be recalculated because of changed request or parameterset */
  bool recalcEngineParameters;

  unsigned long lastParametersFromPackageTimeStamp;
  
  /** neck height in mm while walking */
  double neckHeight;
  
  /** body tilt angle */
  double bodyTilt;
  
  //!@name current walk values
  //!@{
  Vector2<double> legSpeed[4]; ///< speed of leg on ground (step size in mm)
  // int currentStep;             ///< current step
  Vector3<double> footPos[4];  ///< foot positions
  //!@}
  
  /** currently executed motion request
  * speeds in mm/s
  */
  Pose2D currentRequest;
  
  /** odometry resulting from current request
  * speed in mm/tick
  */
  Pose2D odometry;
  
  /** initialise fixed parameters */
  void init();
  
  /** calculate all engine wide parameters like neckHeight from currentParameters
  * this is essential after every change in currentParameters!
  */
  void calculateParams();
  
  /** smooth motion request 
  * current request is adjusted according to motion request
  * eliminating quick changes */
  void smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest);
  
  /** calculates new leg speeds according to current motion request */
  void calculateLegSpeeds();
  
  /** calculate relative foot position for one leg 
  * rx is relative to current step size (range -1.0..1.0)
  * ry is an absolute offset to y foot position
  * rz is relative to step lift parameter (range 0..1.0)
  */
  void calculateRelativeFootPosition(int step, int leg, double &rx, double &ry, double &rz);
  
  /** calculate angles for one leg in current step */
  int calculateLegJoints(Kinematics::LegIndex leg, 
    double &j1, double &j2, double &j3,
    double bodyTilt=0);
  
  /** calculate current joint data values */
  void calculateData(JointData &j);
  
  /** calculate current foot positions */
  void calculateFootPositions();
};

#endif// __InvKinWalkingEngine_h_

/*
* Change log :
* 
* $Log: GT2004WalkingEngine.h,v $
* Revision 1.1  2004/07/10 00:18:31  spranger
* renamed (readded) for coderelease
*
* Revision 1.4  2004/06/16 14:40:45  spranger
* - usage of positionInWalkCycle integrated
* - removed currentStep and doublestep
*
* Revision 1.3  2004/06/07 18:47:50  spranger
* extended interface of executeParametrized by positionInWalkCycle
*
* Revision 1.2  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.1.1.1  2004/05/22 17:23:06  cvsadm
* created new repository GT2004_WM
*
* Revision 1.5  2004/04/21 14:07:43  dueffert
* handling of incomming/changing parameters improved; Motion can load parameterset itself on request now
*
* Revision 1.4  2004/03/08 02:11:55  roefer
* Interfaces should be const
*
* Revision 1.3  2004/02/18 14:51:37  dueffert
* engine can now use new parameters provided via packageCognitionMotion and calculates StepSizeR correct
*
* 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
*
*
*/

---