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
/**
* @file FootStepPlanner.cpp
*
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* plan the foot step according to motion request
*/

#include "ZMPPreviewController.h"
#include <PlatformInterface/Platform.h>

using namespace std;

void ZMPPreviewController::execute(){
    if( !ready() ) {
      return;
    }

    // dummy, not used
    Vector3d com;
    Vector2d dcom, ddcom;
    control(com, dcom, ddcom);
    getZMPReferenceBuffer().pop();

    getTargetCoMFeetPose().pose.com.translation = com;

    PLOT("ZMPPreviewController:is_stationary", is_stationary());
    PLOT("ZMPPreviewController:com:x", com.x);
    PLOT("ZMPPreviewController:com:y", com.y);
    PLOT("ZMPPreviewController:dcom:x", dcom.x);
    PLOT("ZMPPreviewController:dcom:y", dcom.y);
    PLOT("ZMPPreviewController:dcom:abs", dcom.abs());
    PLOT("ZMPPreviewController:ddcom:x", ddcom.x);
    PLOT("ZMPPreviewController:ddcom:y", ddcom.y);
    PLOT("ZMPPreviewController:ddcom:abs", ddcom.abs());
}

ZMPPreviewController::ZMPPreviewController():<--- Member variable 'ZMPPreviewController::theZ' is not initialized in the constructor.
    parameters(getWalk2018Parameters().zmpPreviewControllerParams)
{
  double dt = parameters.parameterTimeStep; // 10ms

  A.c[0] = Vector3d(1      , 0 , 0);
  A.c[1] = Vector3d(dt     , 1 , 0);
  A.c[2] = Vector3d(dt*dt/2, dt, 1);

  b = Vector3d(dt*dt*dt/6, dt*dt/2, dt);

  parameters.update(200); // some initial height
}

void ZMPPreviewController::setHeight(double height) {
  int newHeight = static_cast<int>(Math::round(height));
  // todo: it's bad if this happens
  newHeight = Math::clamp(newHeight, 200, 300);

  if ( parameters.current->height != newHeight )
  {
    // std::cout << "[PreviewController] change height from " << parameters.current->height << " to " << newHeight << std::endl;
    parameters.update(newHeight);

    double z = newHeight * 0.001; // height in m
    c = Vector3d(1,0,-z/Math::g);
  }
}

void ZMPPreviewController::update(const std::list<double>& ref, Vector3d&x, double& err) const {
  // u = -Ki*err - Ks*X - F*P;

  double u = -parameters.current->Ki * err - parameters.current->K*x;

  // the preview length is limited by the nomber of parameters and
  // by the length of the reference zmp (ref)
  std::list<double>::const_iterator refi = ref.begin();
  std::vector<double>::const_iterator fi = parameters.current->f.begin();
  for (; refi != ref.end() && fi != parameters.current->f.end(); ++fi, ++refi)
  {
    u -= (*fi) * (*refi);
  }

  x = A * x + b*u;

  double zmp = c*x;
  err += (zmp - ref.front());
}

void ZMPPreviewController::control(Vector3d& com, Vector2d& dcom, Vector2d& ddcom) {
  ASSERT(ready());

  theZ = getZMPReferenceBuffer().front().z;
  setHeight(theZ);

  update(getZMPReferenceBuffer().getXRef(), theX, theErr.x);
  update(getZMPReferenceBuffer().getYRef(), theY, theErr.y);

  //refZMPx.pop_front();
  //refZMPy.pop_front();
  //refZMPz.pop_front();

  // copy results
  com.x = theX[0];
  dcom.x = theX[1];
  ddcom.x = theX[2];

  com.y = theY[0];
  dcom.y = theY[1];
  ddcom.y = theY[2];

  com.z = theZ;
}

bool ZMPPreviewController::ready() const {
  if (parameters.current == NULL) {
    return false;
  }

  Vector3<size_t> refZMPSize = getZMPReferenceBuffer().size();
  return refZMPSize.x >= previewSteps() && refZMPSize.y >= previewSteps();
}