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_

---