Modules/WalkingEngine/InvKinWalkingEngine.h

Go to the documentation of this file.

/**
* @file InvKinWalkingEngine.h
* 
* Definition of class InvKinWalkingEngine
*
* @author Max Risler
*/

#ifndef __InvKinWalkingEngine_h_
#define __InvKinWalkingEngine_h_

#include "WalkingEngine.h"
#include "InvKinWalkingParameterSets.h"

/**
* @class InvKinWalkingEngine
*
* Walking engine based on 
* calculation of rectangular foot movement and
* inverse kinematics
*
* @author Max Risler
*/
class InvKinWalkingEngine : public WalkingEngine
{
public:
/**
* Constructor.
* @param interfaces The paramters of the WalkingEngine module.
  */
  InvKinWalkingEngine(const WalkingEngineInterfaces& interfaces);
  
  /**
  * Destructor
  */
  ~InvKinWalkingEngine();
  
  /** 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);
  
  /**
  * Sets the engine's parameters
  * @param p The new parameter set
  * @param changeSteps Number of interpolation steps from old to new parameters
  */
  void setParameters(InvKinWalkingParameters* p, int changeSteps=32);
  
  /**
  * Gets the engine's parameters
  */
  const InvKinWalkingParameters& getParameters() const {return currentParameters;}
  
private:
  
  //!@name parameter set interpolation
  //!@{
  /** Current parameters of this walk */
  InvKinWalkingParameters currentParameters;

  /** Pointer to the parameter set requested in setParameters */
  InvKinWalkingParameters *requestedParameters;
  
  /** Next parameters of this walk, target of current interpolation */
  InvKinWalkingParameters nextParameters;
  
  /** Last parameters of this walk, origin of current interpolation */
  InvKinWalkingParameters lastParameters;
    
  /** Counts parameter set interpolation steps */
  int paramInterpolCount;
  
  /** Stores the length of the current parameter set interpolation */
  int paramInterpolLength;
  
  /** Stores precise version of currentStep during interpolation */
  double positionInWalkCycle;
  
  /** 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);
  //!@}
  
  unsigned long lastParametersFromPackageTimeStamp;
 
  //!@name current walk values
  //!@{
  double legSpeedX[4]; ///< speed of leg in x direction (step size in mm)
  double legSpeedY[4]; ///< speed of leg in y direction (step size in mm)
  // int currentStep;  ///< current step
  bool footOnGround[4]; ///< foot is on ground
  double x[4]; ///< foot x positions
  double y[4]; ///< foot y positions
  double z[4]; ///< foot z positions
  //!@}
  
  /** currently executed motion request
  * speeds in mm/s
  */
  Pose2D currentRequest;
  
  /** odometry resulting from current request
  * speed in mm/tick
  */
  Pose2D odometry;
  
  /** 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 current joint data values */
  void calculateData(JointData &j);
  
  /** calculate current foot positions */
  void calculateFootPositions();
  
  /** calculate angles for one leg in current step */
  int calculateLegJoints(Kinematics::LegIndex leg, 
    double &j1, double &j2, double &j3,
    double bodyTilt=0);
  
  /** smooth motion request 
  * current request is adjusted according to motion request
  * eliminating quick changes */
  void smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest);
  
  /** 
  * limit step to maximum step size
  * corrects odometry accordingly
  */
  void limitToMaxSpeed(double& stepSizeX, double& stepSizeY, double& stepSizeR);

  /** gets (hopefully optimized) relative foot position from a lookup table with interpolation
  * @param index exact table index 0.0..0.25
  * @param rz the returned relative z coordinate of the foot 
  * @return the returned relative x coordinate (signless) of the foot
  */
  double getLegPositionCurve(double& rz,double index);
};


/**
* @class ParamInvKinWalkingEngine
*
* This class is a wrapper for the InvKinWalkingEngine
* calling a given instance of the engine with a
* specific set of parameters.
*
* @author Max Risler
*/
class ParamInvKinWalkingEngine : public WalkingEngine
{
public:
  ParamInvKinWalkingEngine(InvKinWalkingParameters* pParams, InvKinWalkingEngine* pEngine) :
    WalkingEngine(*pEngine),
    pEngine(pEngine),
    pParams(pParams)
  {}

  ~ParamInvKinWalkingEngine()
  {
    delete pParams;
  }

  InvKinWalkingEngine* pEngine;
  InvKinWalkingParameters* pParams;

  virtual bool executeParameterized(JointData& jointData,
        const WalkRequest& walkRequest, double positionInWalkingCycle)
  {
    pEngine->setParameters(pParams);
    return pEngine->executeParameterized(jointData, walkRequest, positionInWalkingCycle);
  }

  bool handleMessage(InMessage& message)
  {
    if (message.getMessageID() == idInvKinWalkingParameters)
    {
      InvKinWalkingParameters* newParams = new InvKinWalkingParameters;
      delete pParams;
      pParams = newParams;
      message.bin >> *pParams;
      pParams->readValues();
      return true;
    }
    return pEngine->handleMessage(message);
  }
};


/**
 * @class ParamRearOnlyInvKinWalkingEngine
 *
 * This class is a wrapper for the InvKinWalkingEngine
 * calling a given instance of the engine with a specific
 * set of parameters. Only the hind legs are moved, every
 * other joints are left in their current positions.
 * \note  this is useful for turning with the ball.
 *
 * @author Thomas Kindler
 */
class ParamRearOnlyInvKinWalkingEngine : public WalkingEngine
{
public:
  ParamRearOnlyInvKinWalkingEngine(
    InvKinWalkingParameters  *pParams, 
    InvKinWalkingEngine      *pEngine
  ) :
    WalkingEngine(*pEngine),
    pEngine(pEngine),
    pParams(pParams)
  {}

  ~ParamRearOnlyInvKinWalkingEngine()
  {
    delete pParams;
  }

  InvKinWalkingEngine     *pEngine;
  InvKinWalkingParameters *pParams;

  virtual bool executeParameterized(
    JointData            &jointData,
    const WalkRequest  &walkRequest,
    double positionInWalkingCycle
  ) {
    pEngine->setParameters(pParams);

    JointData jd = jointData;
    bool result = pEngine->executeParameterized(jd, walkRequest, positionInWalkingCycle);
    
    // Copy hind leg joint angles only
    //
    jointData.data[JointData::legHL1] = jd.data[JointData::legHL1];
    jointData.data[JointData::legHL2] = jd.data[JointData::legHL2];
    jointData.data[JointData::legHL3] = jd.data[JointData::legHL3];
    jointData.data[JointData::legHR1] = jd.data[JointData::legHR1];
    jointData.data[JointData::legHR2] = jd.data[JointData::legHR2];
    jointData.data[JointData::legHR3] = jd.data[JointData::legHR3];
    return  result;
  }

  bool handleMessage(InMessage &message)
  {
    if (message.getMessageID() == idInvKinWalkingParameters)
    {
      InvKinWalkingParameters* newParams = new InvKinWalkingParameters;
      delete pParams;
      pParams = newParams;
      message.bin >> *pParams;
      return true;
    }
    return pEngine->handleMessage(message);
  }
};

#endif// __InvKinWalkingEngine_h_

/*
* Change log :
* 
* $Log: InvKinWalkingEngine.h,v $
* Revision 1.5  2004/09/09 10:15:55  spranger
* fixed doxygen-errors
*
* Revision 1.4  2004/06/14 16:54:11  juengel
* Removed some WalkingEngineParameterSets.
*
* Revision 1.3  2004/06/07 18:49:37  spranger
* -extended interface of executeParametrized by positionInWalkCycle,
* -removed currentStep and doubleStep
* -all cycle determination relies on positionInWalkCycle
*
* Revision 1.2  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.1.1.1  2004/05/22 17:22:31  cvsadm
* created new repository GT2004_WM
*
* Revision 1.10  2004/04/08 15:33:06  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.9  2004/03/26 10:57:04  thomas
* added name of parameters to streaming operator
* call readValues after setting new parameters
*
* Revision 1.8  2004/03/22 21:58:13  roefer
* True odometry
*
* Revision 1.7  2004/03/18 16:59:40  risler
* new pointer when receving parameter set
*
* Revision 1.6  2004/03/17 15:38:51  thomas
* added walkType debug for debugging WalkingEngineParameterSets
* paramSets-messages send by debug-message are stored in the new debug-walkType only
*
* Revision 1.5  2004/03/09 22:31:49  cesarz
* added functionality to change invKinWalkingParameters through the behavior
*
* Revision 1.4  2004/03/08 02:11:54  roefer
* Interfaces should be const
*
* Revision 1.3  2004/02/16 17:56:32  dueffert
* InvKin engine and parameters separated
*
* Revision 1.2  2003/12/02 18:19:23  dueffert
* comment bug fixed
*
* Revision 1.1  2003/10/06 14:10:15  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.6  2003/09/26 11:38:52  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.5  2003/09/01 15:57:42  dueffert
* Genom and Individual merged
*
* Revision 1.4  2003/08/08 15:43:25  dueffert
* some evolution implementation added
*
* Revision 1.3  2003/07/24 12:44:19  risler
* new walking engine Parameters loweringPhase, Phase parameters for fore and hind legs now, leaveAnytime, new footmode freeFormQuad
*
* Revision 1.2  2003/07/05 09:49:05  roefer
* Generic debug message for bodyOffsets improved
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.22  2003/06/12 21:42:26  roefer
* GT2003 walking parameters finished
*
* Revision 1.21  2003/05/12 00:03:30  dueffert
* doxygen bugs fixed
*
* Revision 1.20  2003/05/03 16:20:01  roefer
* robot.cfg added
*
* Revision 1.19  2003/04/16 07:00:16  roefer
* Bremen GO checkin
*
* Revision 1.19  2003/04/08 18:28:28  roefer
* bodyTiltOffset added
*
* Revision 1.18  2003/04/04 17:37:38  dueffert
* a bunch of fine tuning
*
* Revision 1.17  2003/03/19 09:22:17  dueffert
* circle mode added
*
* Revision 1.16  2003/03/06 12:08:14  dueffert
* execute with parameters renamed to avoid inheritance warnings
*
* Revision 1.15  2003/03/05 10:31:46  risler
* interpolation bug fixed
*
* Revision 1.14  2003/03/03 17:32:57  risler
* walking engine now writes calculated bodyTilt to headState
* minor cleanup to new parameter interpolation stuff
* parameter interpolation when standing
*
* Revision 1.13  2003/03/03 08:44:32  dueffert
* switching InvKinWalkingParameters is smoothed now
*
* Revision 1.12  2003/02/03 19:32:25  risler
* sending walking parameters now works properly again
*
* Revision 1.11  2003/01/23 16:44:10  risler
* only one instance of InvKinWalkingEngine
* parameter sets can now be switched while walking
* added UNSWFastTurn, combining two parameter sets
*
* Revision 1.10  2003/01/18 17:40:16  risler
* changed walking engine parameter coordinate system
*
* Revision 1.9  2002/12/11 14:58:45  risler
* InvKinWalkingEngine:
* added body shift parameters,
* and rotation center correction value
*
* Revision 1.8  2002/11/28 16:57:34  risler
* added leg phase parameters for variable gaits
*
* Revision 1.7  2002/11/26 12:28:57  dueffert
* doxygen docu corrected
*
* Revision 1.6  2002/11/25 12:35:33  dueffert
* bodyTilt should be used to calculate leg positions from joints and vice versa
*
* Revision 1.5  2002/11/19 17:10:54  risler
* DebugMessageGenerator uses InConfigMemory
* Can handle comments now
*
* Revision 1.4  2002/10/14 13:14:24  dueffert
* doxygen comments corrected
*
* Revision 1.3  2002/09/22 18:40:53  risler
* added new math functions, removed GTMath library
*
* Revision 1.2  2002/09/11 00:06:58  loetzsch
* continued change of module/solution mechanisms
*
* Revision 1.1  2002/09/10 15:36:16  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.4  2002/08/22 14:41:03  risler
* added some doxygen comments
*
* Revision 1.3  2002/07/23 13:33:43  loetzsch
* new streaming classes
*
* removed many #include statements
*
* Revision 1.2  2002/05/11 08:07:43  risler
* added head and mouth joints to InvKinWalkingParameters
*
* Revision 1.1.1.1  2002/05/10 12:40:19  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.5  2002/05/02 18:17:03  risler
* added InvKin Parameters fore/hindFootTilt
*
* Revision 1.4  2002/05/02 09:45:33  risler
* no message
*
* Revision 1.3  2002/04/30 16:22:34  risler
* added InvKin Parameters footMode, footTilt
*
* Revision 1.2  2002/04/29 13:48:21  risler
* added lastWalkType to WalkingEngine execute
*
* Revision 1.1  2002/04/26 13:35:32  risler
* DarmstadtGOWalkingEngine renamed to InvKinWalkingEngine
* added InvKinParameterSets
*
* Revision 1.3  2002/04/17 17:04:41  risler
* Darmstadt GO
*
* Revision 1.2  2002/04/09 16:46:02  risler
* added DarmstadtGOWalkingEngine
*
* Revision 1.1  2002/03/29 19:36:53  risler
* added DarmstadtGOWalkingEngine
*
*
*/

---