Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/WalkingEngine/InvKinWalkingEngine.h

Go to the documentation of this file.

    00001 /**
    00002 * @file InvKinWalkingEngine.h
    00003 *
    00004 * Definition of class InvKinWalkingEngine
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #ifndef __InvKinWalkingEngine_h_
    00010 #define __InvKinWalkingEngine_h_
    00011
    00012 #include "WalkingEngine.h"
    00013 #include "InvKinWalkingParameterSets.h"
    00014
    00015 /**
    00016 * @class InvKinWalkingEngine
    00017 *
    00018 * Walking engine based on
    00019 * calculation of rectangular foot movement and
    00020 * inverse kinematics
    00021 *
    00022 * @author Max Risler
    00023 */
    00024 class InvKinWalkingEngine : public WalkingEngine
    00025 {
    00026 public:
    00027 /**
    00028 * Constructor.
    00029 * @param interfaces The paramters of the WalkingEngine module.
    00030   */
    00031   InvKinWalkingEngine(const WalkingEngineInterfaces& interfaces);
    00032
    00033   /**
    00034   * Destructor
    00035   */
    00036   ~InvKinWalkingEngine();
    00037
    00038   /** Executes the engine */
    00039   virtual bool executeParameterized(JointData& jointData, const WalkRequest& walkRequest, double positionInWalkingCycle);
    00040
    00041   /**
    00042   * Called from a MessageQueue to distribute messages
    00043   * @param message The message that can be read.
    00044   * @return true if the message was read (handled).
    00045   */
    00046   virtual bool handleMessage(InMessage& message);
    00047
    00048   /**
    00049   * Sets the engine's parameters
    00050   * @param p The new parameter set
    00051   * @param changeSteps Number of interpolation steps from old to new parameters
    00052   */
    00053   void setParameters(InvKinWalkingParameters* p, int changeSteps=32);
    00054
    00055   /**
    00056   * Gets the engine's parameters
    00057   */
    00058   const InvKinWalkingParameters& getParameters() const {return currentParameters;}
    00059
    00060 private:
    00061
    00062   //!@name parameter set interpolation
    00063   //!@{
    00064   /** Current parameters of this walk */
    00065   InvKinWalkingParameters currentParameters;
    00066
    00067   /** Pointer to the parameter set requested in setParameters */
    00068   InvKinWalkingParameters *requestedParameters;
    00069
    00070   /** Next parameters of this walk, target of current interpolation */
    00071   InvKinWalkingParameters nextParameters;
    00072
    00073   /** Last parameters of this walk, origin of current interpolation */
    00074   InvKinWalkingParameters lastParameters;
    00075
    00076   /** Counts parameter set interpolation steps */
    00077   int paramInterpolCount;
    00078
    00079   /** Stores the length of the current parameter set interpolation */
    00080   int paramInterpolLength;
    00081
    00082   /** Stores precise version of currentStep during interpolation */
    00083   double positionInWalkCycle;
    00084
    00085   /** Initialize interpolation of WalkingParameterSets */
    00086   void initParametersInterpolation(int changeSteps);
    00087
    00088   /** Calculate next step in parameterset interpolation and increase currentStep if walk is true*/
    00089   void nextParametersInterpolation(bool walk);
    00090   //!@}
    00091
    00092   unsigned long lastParametersFromPackageTimeStamp;
    00093
    00094   //!@name current walk values
    00095   //!@{
    00096   double legSpeedX[4]; ///< speed of leg in x direction (step size in mm)
    00097   double legSpeedY[4]; ///< speed of leg in y direction (step size in mm)
    00098   // int currentStep;  ///< current step
    00099   bool footOnGround[4]; ///< foot is on ground
    00100   double x[4]; ///< foot x positions
    00101   double y[4]; ///< foot y positions
    00102   double z[4]; ///< foot z positions
    00103   //!@}
    00104
    00105   /** currently executed motion request
    00106   * speeds in mm/s
    00107   */
    00108   Pose2D currentRequest;
    00109
    00110   /** odometry resulting from current request
    00111   * speed in mm/tick
    00112   */
    00113   Pose2D odometry;
    00114
    00115   /** calculates new leg speeds according to current motion request */
    00116   void calculateLegSpeeds();
    00117
    00118   /** calculate relative foot position for one leg
    00119   * rx is relative to current step size (range -1.0..1.0)
    00120   * ry is an absolute offset to y foot position
    00121   * rz is relative to step lift parameter (range 0..1.0)
    00122   */
    00123   void calculateRelativeFootPosition(int step, int leg, double &rx, double &ry, double &rz);
    00124
    00125   /** calculate current joint data values */
    00126   void calculateData(JointData &j);
    00127
    00128   /** calculate current foot positions */
    00129   void calculateFootPositions();
    00130
    00131   /** calculate angles for one leg in current step */
    00132   int calculateLegJoints(Kinematics::LegIndex leg,
    00133     double &j1, double &j2, double &j3,
    00134     double bodyTilt=0);
    00135
    00136   /** smooth motion request
    00137   * current request is adjusted according to motion request
    00138   * eliminating quick changes */
    00139   void smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest);
    00140
    00141   /**
    00142   * limit step to maximum step size
    00143   * corrects odometry accordingly
    00144   */
    00145   void limitToMaxSpeed(double& stepSizeX, double& stepSizeY, double& stepSizeR);
    00146
    00147   /** gets (hopefully optimized) relative foot position from a lookup table with interpolation
    00148   * @param index exact table index 0.0..0.25
    00149   * @param rz the returned relative z coordinate of the foot
    00150   * @return the returned relative x coordinate (signless) of the foot
    00151   */
    00152   double getLegPositionCurve(double& rz,double index);
    00153 };
    00154
    00155
    00156 /**
    00157 * @class ParamInvKinWalkingEngine
    00158 *
    00159 * This class is a wrapper for the InvKinWalkingEngine
    00160 * calling a given instance of the engine with a
    00161 * specific set of parameters.
    00162 *
    00163 * @author Max Risler
    00164 */
    00165 class ParamInvKinWalkingEngine : public WalkingEngine
    00166 {
    00167 public:
    00168   ParamInvKinWalkingEngine(InvKinWalkingParameters* pParams, InvKinWalkingEngine* pEngine) :
    00169     WalkingEngine(*pEngine),
    00170     pEngine(pEngine),
    00171     pParams(pParams)
    00172   {}
    00173
    00174   ~ParamInvKinWalkingEngine()
    00175   {
    00176     delete pParams;
    00177   }
    00178
    00179   InvKinWalkingEngine* pEngine;
    00180   InvKinWalkingParameters* pParams;
    00181
    00182   virtual bool executeParameterized(JointData& jointData,
    00183         const WalkRequest& walkRequest, double positionInWalkingCycle)
    00184   {
    00185     pEngine->setParameters(pParams);
    00186     return pEngine->executeParameterized(jointData, walkRequest, positionInWalkingCycle);
    00187   }
    00188
    00189   bool handleMessage(InMessage& message)
    00190   {
    00191     if (message.getMessageID() == idInvKinWalkingParameters)
    00192     {
    00193       InvKinWalkingParameters* newParams = new InvKinWalkingParameters;
    00194       delete pParams;
    00195       pParams = newParams;
    00196       message.bin >> *pParams;
    00197       pParams->readValues();
    00198       return true;
    00199     }
    00200     return pEngine->handleMessage(message);
    00201   }
    00202 };
    00203
    00204
    00205 /**
    00206  * @class ParamRearOnlyInvKinWalkingEngine
    00207  *
    00208  * This class is a wrapper for the InvKinWalkingEngine
    00209  * calling a given instance of the engine with a specific
    00210  * set of parameters. Only the hind legs are moved, every
    00211  * other joints are left in their current positions.
    00212  * \note  this is useful for turning with the ball.
    00213  *
    00214  * @author Thomas Kindler
    00215  */
    00216 class ParamRearOnlyInvKinWalkingEngine : public WalkingEngine
    00217 {
    00218 public:
    00219   ParamRearOnlyInvKinWalkingEngine(
    00220     InvKinWalkingParameters  *pParams,
    00221     InvKinWalkingEngine      *pEngine
    00222   ) :
    00223     WalkingEngine(*pEngine),
    00224     pEngine(pEngine),
    00225     pParams(pParams)
    00226   {}
    00227
    00228   ~ParamRearOnlyInvKinWalkingEngine()
    00229   {
    00230     delete pParams;
    00231   }
    00232
    00233   InvKinWalkingEngine     *pEngine;
    00234   InvKinWalkingParameters *pParams;
    00235
    00236   virtual bool executeParameterized(
    00237     JointData            &jointData,
    00238     const WalkRequest  &walkRequest,
    00239     double positionInWalkingCycle
    00240   ) {
    00241     pEngine->setParameters(pParams);
    00242
    00243     JointData jd = jointData;
    00244     bool result = pEngine->executeParameterized(jd, walkRequest, positionInWalkingCycle);
    00245
    00246     // Copy hind leg joint angles only
    00247     //
    00248     jointData.data[JointData::legHL1] = jd.data[JointData::legHL1];
    00249     jointData.data[JointData::legHL2] = jd.data[JointData::legHL2];
    00250     jointData.data[JointData::legHL3] = jd.data[JointData::legHL3];
    00251     jointData.data[JointData::legHR1] = jd.data[JointData::legHR1];
    00252     jointData.data[JointData::legHR2] = jd.data[JointData::legHR2];
    00253     jointData.data[JointData::legHR3] = jd.data[JointData::legHR3];
    00254     return  result;
    00255   }
    00256
    00257   bool handleMessage(InMessage &message)
    00258   {
    00259     if (message.getMessageID() == idInvKinWalkingParameters)
    00260     {
    00261       InvKinWalkingParameters* newParams = new InvKinWalkingParameters;
    00262       delete pParams;
    00263       pParams = newParams;
    00264       message.bin >> *pParams;
    00265       return true;
    00266     }
    00267     return pEngine->handleMessage(message);
    00268   }
    00269 };
    00270
    00271 #endif// __InvKinWalkingEngine_h_