1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
/**
* @file FootStep.h
*
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* Declaration of class foot step for walking
*/

#ifndef _FOOTSTEP_H
#define _FOOTSTEP_H

#include <Tools/Math/Pose2D.h>
#include "../IKPose.h"
#include "Tools/Debug/DebugDrawings.h"

class FootStep
{
public:
  enum Foot {
    LEFT = 1,
    NONE = 0,
    RIGHT = -LEFT
  };

  FootStep(){}<--- Member variable 'FootStep::theLiftingFoot' is not initialized in the constructor.

  FootStep(const InverseKinematic::FeetPose& feet, Foot liftingFoot):
    theLiftingFoot(liftingFoot)
  {
    switch(theLiftingFoot)
    {
      case LEFT:
      {
        theFootBegin = feet.left;
        theFootEnd = feet.left;
        theSupFoot = feet.right;
        break;
      }
      case RIGHT:
      case NONE:
      {
        theFootBegin = feet.right;
        theFootEnd = feet.right;
        theSupFoot = feet.left;
        break;
      }
    }
  }

  InverseKinematic::FeetPose begin() const
  {
    InverseKinematic::FeetPose feet;

    switch(theLiftingFoot)
    {
      case LEFT:
      {
        feet.left = theFootBegin;
        feet.right = theSupFoot;
        break;
      }
      case RIGHT:
      case NONE:
      {
        feet.left = theSupFoot;
        feet.right = theFootBegin;
      }
    }
    return feet;
  }//end begin

  InverseKinematic::FeetPose end() const
  {
    InverseKinematic::FeetPose feet;

    switch(theLiftingFoot)
    {
      case LEFT:
      {
        feet.left = theFootEnd;
        feet.right = theSupFoot;
        break;
      }
      case RIGHT:
      case NONE:
      {
        feet.left = theSupFoot;
        feet.right = theFootEnd;
      }
    }
    return feet;
  }//end end

  // TODO
  void draw(DrawingCanvas2D& canvas) const
  {
    // foot begin
    canvas.translate(theFootBegin.translation.x, theFootBegin.translation.y);
    canvas.rotate(theFootBegin.rotation.getZAngle());
    canvas.pen(Color::BLUE, 1.0);
    canvas.drawBox(-50, -30, 50, 30);
    canvas.rotate(-theFootBegin.rotation.getZAngle());
    canvas.translate(-theFootBegin.translation.x, -theFootBegin.translation.y);

    // foot end
    canvas.translate(theFootEnd.translation.x, theFootEnd.translation.y);
    canvas.rotate(theFootEnd.rotation.getZAngle());
    canvas.pen(Color::RED, 10.0);
    canvas.drawBox(-50, -30, 50, 30);
    canvas.rotate(-theFootEnd.rotation.getZAngle());
    canvas.translate(-theFootEnd.translation.x, -theFootEnd.translation.y);
  }//end draw

   Foot liftingFoot() const { return theLiftingFoot; }

   const Pose3D& footBegin() const { return theFootBegin; }
   const Pose3D& footEnd() const { return theFootEnd; }
   Pose3D& footEnd() { return theFootEnd; }
   const Pose3D& supFoot() const { return theSupFoot; }
   // hack...
   Pose3D& supFoot() { return theSupFoot; }

   // calculated and used by the FootStepPlanner
   const Pose2D& stepRequest() const { return theStepRequest; }
   Pose2D& stepRequest() { return theStepRequest; }

   const Pose2D& offset() const { return theOffset; }
   Pose2D& offset() { return theOffset; }

private:
   Foot theLiftingFoot;
   Pose3D theFootBegin, theFootEnd, theSupFoot;

   Pose2D theStepRequest;
   Pose2D theOffset;
};

#endif  /* _FOOTSTEP_H */