Modules/WalkingEngine/GT2005WalkingEngine.h

Go to the documentation of this file.

/***************************************************************************
* @file GT2005WalkingEngine.h
* Definition of class GT2005WalkingEngine
* @author Denis Fisseler
****************************************************************************/

#ifndef GT2005WalkingEngine_h
#define GT2005WalkingEngine_h

#include "WalkingEngine.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Math/Pose2D.h"
#include "Tools/Math/Vector3.h"
#include "GT2005Polygon.h"
#include "GT2005Parameters.h"
#include "Tools/Streams/OutStreams.h"
#include "Tools/Streams/InStreams.h"

/***************************************************************************
* @class GT2005WalkingEngine
*
* Walking engine based on calculation of polygonal shape foot movement
* @author Denis Fisseler
***************************************************************************/
class GT2005WalkingEngine : public WalkingEngine
{
public:
  // Constructor.
  // @param interfaces The paramters of the WalkingEngine module.
  GT2005WalkingEngine(const WalkingEngineInterfaces& interfaces);

  // Destructor
  ~GT2005WalkingEngine();

  // 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);

public:

  bool initWithOldStyleShape; // calculate 4 edged polygon from old rectangel data

  // constants for the legs
  static const int Leg_FR;
  static const int Leg_FL;
  static const int Leg_HR;
  static const int Leg_HL;

  int activePoints; // points: [0 ... n-1]

  double footPhase[4]; // relative foot phase offsets
  Vector3<double> footZeroPosition[4]; // foot zero positions
  GT2005Polygon* currentPoly[4]; // a different polygon for each leg is possible.

  GT2005Parameters* currentFIPSParameters;

  int currentStep; //the current step from 0 to StepLength

  bool standing;

  bool loadingFailed;

private:
  // Zeug damit das funzt
  double neckHeight;
  double bodyTilt;

  Pose2D odometry;
  Pose2D currentRequest;
  double currentStepPercentage;
  GT2004Parameters* currentParameters;
  GT2004ParametersSet paramSet;

  void calculateData(JointData &j);
  void smoothMotionRequest(const Pose2D& request, Pose2D& currentRequest);

  bool debugging;
  bool debugRequest;
  bool debugLoop;
  void readDebugData(double ssf[4], double a[4], double c_offset[4], JointData &j);
  Vector3<double> debugDataReal[4][128];
  Vector3<double> debugDataIdeal[4][128];
  long debugDataBuffer[4][12];
  void streamDebugData();

  bool resampleRequest;
  bool resampling;
  void readResampleData(double c_scale[4], double c_angle[4], double c_offset[4], JointData &j, Vector3<double> fp[4]);

  void loadPolys(int polyID, InMessage& message);
  void sendPolys(int polyID);

  int polygonsRecieved;

  void readOdometryCalibrationData();
  Pose2D calculateOdometryCalibrationData(Pose2D request, WalkRequest::WalkType walkType);
  typedef struct
  {
    Pose2D controlled;
    Pose2D measured;
  } odometryPoint;
  odometryPoint odometryCalibrationData[15][13][7]; //[x][y][rot]
  odometryPoint odometryCalibrationData2[2][21]; // calibration data for pure rotation

  void loadOdometryPlane(int planeNum, char* filename, int alternateDir);
  void loadOdometryRotation(int rotNum, char* filename, int alternateDir);
};

#endif // GT2005WalkingEngine_h

---