ZMPDiscretization.cpp 44.7 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
student's avatar
update    
student committed
2
 * Copyright 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
 *
 * Andrei Herdt
 * Francois Keith
 * Florent Lamiraux
 * Evrard Paul
 * Nicolas Perrin
 * Olivier Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
student's avatar
update    
student committed
26
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
27
28
29
30
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/* This object generate all the values for the foot trajectories,
   and the desired ZMP based on a sequence of steps. */
Thomas Moulard's avatar
Thomas Moulard committed
31
32
33
34

#include "portability/gettimeofday.hh"

#ifdef WIN32
35
#include <Windows.h>
Thomas Moulard's avatar
Thomas Moulard committed
36
#endif /* WIN32 */
Thomas Moulard's avatar
Thomas Moulard committed
37
38

#include <fstream>
39
40
#include <iostream>
#include <time.h>
Thomas Moulard's avatar
Thomas Moulard committed
41

42
#include <Debug.hh>
Thomas Moulard's avatar
Thomas Moulard committed
43

44
#include <Mathematics/ConvexHull.hh>
45
#include <ZMPRefTrajectoryGeneration/ZMPDiscretization.hh>
Thomas Moulard's avatar
Thomas Moulard committed
46
47

#ifdef WIN32
48
inline double round(double d) { return floor(d + 0.5); }
Thomas Moulard's avatar
Thomas Moulard committed
49
50
#endif /* WIN32 */

51
using namespace ::PatternGeneratorJRL;
Thomas Moulard's avatar
Thomas Moulard committed
52

53
OnLineState::OnLineState() { m_CurrentState = IDLE_STATE; }
Thomas Moulard's avatar
Thomas Moulard committed
54

55
OnLineState::~OnLineState() {}
Thomas Moulard's avatar
Thomas Moulard committed
56

57
unsigned int OnLineState::operator()() const { return m_CurrentState; }
Thomas Moulard's avatar
Thomas Moulard committed
58

59
60
OnLineState &OnLineState::operator=(unsigned int NewState) {
  if (NewState < DOUBLE_SUPPORT_PHASE)
Thomas Moulard's avatar
Thomas Moulard committed
61
62
63
64
    m_CurrentState = NewState;
  return *this;
}

65
ZMPDiscretization::ZMPDiscretization(SimplePluginManager *lSPM, string DataFile,
66
                                     PinocchioRobot *aPR)
67
    : ZMPRefTrajectoryGeneration(lSPM) {
Thomas Moulard's avatar
Thomas Moulard committed
68
69
70

  m_InitializationProfile = PREV_ZMP_INIT_PROFIL;

71
  m_PR = aPR;
72
  PRFoot *lLeftFoot = m_PR->leftFoot();
73
  m_FootTrajectoryGenerationStandard =
74
      new FootTrajectoryGenerationStandard(lSPM, lLeftFoot);
Thomas Moulard's avatar
Thomas Moulard committed
75
76
  m_FootTrajectoryGenerationStandard->InitializeInternalDataStructures();

77
  m_PolynomeZMPTheta = new Polynome3(0, 0);
Thomas Moulard's avatar
Thomas Moulard committed
78

79
80
81
  m_A.resize(6, 6);
  m_B.resize(6, 1);
  m_C.resize(2, 6);
Thomas Moulard's avatar
Thomas Moulard committed
82
83
84

  m_RelativeFootPositions.clear();

85
  m_ModulationSupportCoefficient = 0.9;
Thomas Moulard's avatar
Thomas Moulard committed
86
87
88
89

  ResetADataFile(DataFile);

  m_ZMPShift.resize(4);
90
91
  for (unsigned int i = 0; i < 4; i++)
    m_ZMPShift[i] = 0.0;
student's avatar
update    
student committed
92

Thomas Moulard's avatar
Thomas Moulard committed
93
94
95
  m_ZMPNeutralPosition[0] = 0.0;
  m_ZMPNeutralPosition[1] = 0.0;

96
97
98
99
100
  m_CurrentSupportFootPosition.resize(3, 3);
  for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
      if (i != j)
        m_CurrentSupportFootPosition(i, j) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
101
      else
102
        m_CurrentSupportFootPosition(i, j) = 1.0;
Thomas Moulard's avatar
Thomas Moulard committed
103
104

  m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;
student's avatar
update    
student committed
105

Thomas Moulard's avatar
Thomas Moulard committed
106
  // Prepare size of the matrix used in on-line walking
107
108
109
  m_vdiffsupppre.resize(2, 1);
  for (unsigned int i = 0; i < m_vdiffsupppre.rows(); i++)
    m_vdiffsupppre(i, 0) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
110
111

  RESETDEBUG4("DebugDataRFPos.txt");
112
  RESETDEBUG5("DebugZMPRefPos.dat");
Thomas Moulard's avatar
Thomas Moulard committed
113
114
115
116
117
118
  RESETDEBUG4("DebugFinalZMPRefPos.dat");

  // Add internal methods specific to this class.
  RegisterMethodsForScripting();
}

119
120
ZMPDiscretization::~ZMPDiscretization() {
  if (m_FootTrajectoryGenerationStandard != 0)
Thomas Moulard's avatar
Thomas Moulard committed
121
    delete m_FootTrajectoryGenerationStandard;
student's avatar
update    
student committed
122

123
  if (m_PolynomeZMPTheta != 0)
Thomas Moulard's avatar
Thomas Moulard committed
124
125
126
    delete m_PolynomeZMPTheta;
}

127
128
129
130
131
132
133
134
135
void ZMPDiscretization::GetZMPDiscretization(
    deque<ZMPPosition> &FinalZMPPositions, deque<COMState> &FinalCOMStates,
    deque<RelativeFootPosition> &RelativeFootPositions,
    deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &RightFootAbsolutePositions,
    double, // Xmax,
    COMState &lStartingCOMState, Eigen::Vector3d &lStartingZMPPosition,
    FootAbsolutePosition &InitLeftFootAbsolutePosition,
    FootAbsolutePosition &InitRightFootAbsolutePosition) {
Thomas Moulard's avatar
Thomas Moulard committed
136

137
138
139
140
  InitOnLine(FinalZMPPositions, FinalCOMStates, LeftFootAbsolutePositions,
             RightFootAbsolutePositions, InitLeftFootAbsolutePosition,
             InitRightFootAbsolutePosition, RelativeFootPositions,
             lStartingCOMState, lStartingZMPPosition);
Thomas Moulard's avatar
Thomas Moulard committed
141

142
143
  EndPhaseOfTheWalking(FinalZMPPositions, FinalCOMStates,
                       LeftFootAbsolutePositions, RightFootAbsolutePositions);
144

Thomas Moulard's avatar
Thomas Moulard committed
145
146
147
  FinalCOMStates.resize(FinalZMPPositions.size());
}

148
149
void ZMPDiscretization::DumpFootAbsolutePosition(
    string aFileName, deque<FootAbsolutePosition> &aFootAbsolutePositions) {
Thomas Moulard's avatar
Thomas Moulard committed
150
  ofstream aof;
151
152
153
154
155
156
157
158
159
  aof.open(aFileName.c_str(), ofstream::out);
  if (aof.is_open()) {
    for (unsigned int i = 0; i < aFootAbsolutePositions.size(); i++) {
      aof << aFootAbsolutePositions[i].time << " "
          << aFootAbsolutePositions[i].x << " " << aFootAbsolutePositions[i].y
          << " " << aFootAbsolutePositions[i].z << " "
          << aFootAbsolutePositions[i].omega << " "
          << aFootAbsolutePositions[i].theta << " "
          << aFootAbsolutePositions[i].stepType << " " << endl;
Thomas Moulard's avatar
Thomas Moulard committed
160
    }
161
162
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
163
164
}

165
166
167
168
169
void ZMPDiscretization::ResetADataFile(string &DataFile) {
  if (DataFile.length() != 0) {
    std::ifstream a_iof;
    a_iof.open(DataFile.c_str(), std::ifstream::in);
    if (a_iof.is_open()) {
student's avatar
update    
student committed
170

171
      a_iof.close();
Thomas Moulard's avatar
Thomas Moulard committed
172
    }
173
  }
Thomas Moulard's avatar
Thomas Moulard committed
174
}
175
176
177
void ZMPDiscretization::DumpDataFiles(
    string ZMPFileName, string FootFileName, deque<ZMPPosition> &ZMPPositions,
    deque<FootAbsolutePosition> &SupportFootAbsolutePositions) {
Thomas Moulard's avatar
Thomas Moulard committed
178
  ofstream aof;
179
180
181
182
183
184
  aof.open(ZMPFileName.c_str(), ofstream::out);
  if (aof.is_open()) {
    for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
      aof << ZMPPositions[i].time << " " << ZMPPositions[i].px << " "
          << ZMPPositions[i].py << " " << ZMPPositions[i].stepType << " 0.0"
          << endl;
Thomas Moulard's avatar
Thomas Moulard committed
185
    }
186
187
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
188

189
190
191
192
193
194
195
  aof.open(FootFileName.c_str(), ofstream::out);
  if (aof.is_open()) {
    for (unsigned int i = 0; i < SupportFootAbsolutePositions.size(); i++) {
      aof << SupportFootAbsolutePositions[i].x << " "
          << SupportFootAbsolutePositions[i].y << " "
          << SupportFootAbsolutePositions[i].z << " "
          << SupportFootAbsolutePositions[i].stepType << " 0.0" << endl;
Thomas Moulard's avatar
Thomas Moulard committed
196
    }
197
198
    aof.close();
  }
Thomas Moulard's avatar
Thomas Moulard committed
199
200
}

201
void ZMPDiscretization::InitializeFilter() {
Thomas Moulard's avatar
Thomas Moulard committed
202
  // Create the window for the filter.
203
204
205
  double T = 0.05; // Arbritrary from Kajita's San Matlab files.
  int n = 0;
  double sum = 0, tmp = 0;
student's avatar
update    
student committed
206

Thomas Moulard's avatar
Thomas Moulard committed
207
  assert(m_SamplingPeriod > 0);
208
209
210
211
212
213
  n = (int)floor(T / m_SamplingPeriod);
  m_ZMPFilterWindow.resize(n + 1);
  for (int i = 0; i < n + 1; i++) {
    tmp = sin((M_PI * i) / n);
    m_ZMPFilterWindow[i] = tmp * tmp;
  }
Thomas Moulard's avatar
Thomas Moulard committed
214

215
216
  for (int i = 0; i < n + 1; i++)
    sum += m_ZMPFilterWindow[i];
Thomas Moulard's avatar
Thomas Moulard committed
217

218
219
  for (int i = 0; i < n + 1; i++)
    m_ZMPFilterWindow[i] /= sum;
Thomas Moulard's avatar
Thomas Moulard committed
220
221
222
}

void ZMPDiscretization::FilterZMPRef(deque<ZMPPosition> &ZMPPositionsX,
223
224
225
                                     deque<ZMPPosition> &ZMPPositionsY) {
  int n = 0;
  double T = 0.050; // Arbritraty fixed from Kajita's San matlab files.
Thomas Moulard's avatar
Thomas Moulard committed
226
227
228
229
230
  deque<double> window;

  ZMPPositionsY.resize(ZMPPositionsX.size());
  // Creates window.
  assert(m_SamplingPeriod > 0);
231
  n = (int)floor(T / m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
232
233
234
235

  // Filter ZMPref.

  // First part of the filter.
236
237
238
  for (int i = 0; i < n + 1; i++) {
    ZMPPositionsY[i] = ZMPPositionsX[i];
  }
student's avatar
update    
student committed
239

240
241
242
243
244
  for (unsigned int i = n + 1; i < ZMPPositionsX.size(); i++) {
    double ltmp[2] = {0, 0};
    for (unsigned int j = 0; j < m_ZMPFilterWindow.size(); j++) {
      ltmp[0] += m_ZMPFilterWindow[j] * ZMPPositionsX[i - j].px;
      ltmp[1] += m_ZMPFilterWindow[j] * ZMPPositionsX[i - j].py;
Thomas Moulard's avatar
Thomas Moulard committed
245
    }
student's avatar
update    
student committed
246

247
248
249
250
251
252
    ZMPPositionsY[i].px = ltmp[0];
    ZMPPositionsY[i].py = ltmp[1];
    ZMPPositionsY[i].theta = ZMPPositionsX[i].theta;
    ZMPPositionsY[i].time = ZMPPositionsX[i].time;
    ZMPPositionsY[i].stepType = ZMPPositionsX[i].stepType;
  }
Thomas Moulard's avatar
Thomas Moulard committed
253
254
}

255
void ZMPDiscretization::SetZMPShift(vector<double> &ZMPShift) {
Thomas Moulard's avatar
Thomas Moulard committed
256

257
258
259
  for (unsigned int i = 0; i < ZMPShift.size(); i++) {
    m_ZMPShift[i] = ZMPShift[i];
  }
Thomas Moulard's avatar
Thomas Moulard committed
260
261
262
263
264
}

/* Start the online part of ZMP discretization. */

/* Initialiazation of the on-line stacks. */
265
266
267
268
269
270
271
272
std::size_t ZMPDiscretization::InitOnLine(
    deque<ZMPPosition> &FinalZMPPositions, deque<COMState> &FinalCoMStates,
    deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &RightFootAbsolutePositions,
    FootAbsolutePosition &InitLeftFootAbsolutePosition,
    FootAbsolutePosition &InitRightFootAbsolutePosition,
    deque<RelativeFootPosition> &RelativeFootPositions,
    COMState &lStartingCOMState, Eigen::Vector3d &lStartingZMPPosition) {
Thomas Moulard's avatar
Thomas Moulard committed
273
274
275
  m_RelativeFootPositions.clear();
  FootAbsolutePosition CurrentLeftFootAbsPos, CurrentRightFootAbsPos;

276
  double CurrentAbsTheta = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
277
  RESETDEBUG4("ZMDInitOnLine.txt");
278
  ODEBUG4("ZMP::InitOnLine - Step 1 ", "ZMDInitOnLine.txt");
Thomas Moulard's avatar
Thomas Moulard committed
279
280

  // Initialize position of the current support foot.
281
282
283
284
  for (unsigned int i = 0; i < 3; i++)
    for (unsigned int j = 0; j < 3; j++)
      if (i == j)
        m_CurrentSupportFootPosition(i, j) = 1.0;
Thomas Moulard's avatar
Thomas Moulard committed
285
      else
286
        m_CurrentSupportFootPosition(i, j) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
287
288
289

  m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;

290
  ODEBUG4("ZMP::InitOnLine - Step 2 ", "ZMDInitOnLine.txt");
student's avatar
update    
student committed
291
  // Initialize position of the feet.
Thomas Moulard's avatar
Thomas Moulard committed
292
293
294
  CurrentLeftFootAbsPos = InitLeftFootAbsolutePosition;
  CurrentLeftFootAbsPos.z = 0.0;
  CurrentLeftFootAbsPos.time = 0.0;
student's avatar
update    
student committed
295

296
297
  ODEBUG4("CurrentLeftFootAbsPos.y: " << CurrentLeftFootAbsPos.y,
          "ZMDInitOnLine.txt");
Thomas Moulard's avatar
Thomas Moulard committed
298
299
300
301
  CurrentRightFootAbsPos = InitRightFootAbsolutePosition;
  CurrentRightFootAbsPos.z = 0.0;
  CurrentRightFootAbsPos.time = 0.0;

student's avatar
update    
student committed
302
  // V pre is the difference between
Thomas Moulard's avatar
Thomas Moulard committed
303
  // the current support position and the precedent.
304
  ODEBUG4("ZMP::InitOnLine - Step 2.5 ", "ZMDInitOnLine.txt");
Thomas Moulard's avatar
Thomas Moulard committed
305

olivier-stasse's avatar
olivier-stasse committed
306
307
  // The current heading direction is the center of mass
  // between the direction of the left foot and the direction of the right foot.
308
309
310
311
312
  double CurrentAbsZMPTheta = 0;
  CurrentAbsZMPTheta =
      (CurrentRightFootAbsPos.theta + CurrentLeftFootAbsPos.theta) / 2.0;
  ODEBUG("CurrentZMPTheta at start: "
         << " " << CurrentRightFootAbsPos.theta << " "
313
         << CurrentLeftFootAbsPos.theta);
olivier-stasse's avatar
olivier-stasse committed
314
315

  // Initialize who is support foot.
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
  if (RelativeFootPositions[0].sy < 0) {
    m_vdiffsupppre(0, 0) = CurrentRightFootAbsPos.x - CurrentLeftFootAbsPos.x;
    m_vdiffsupppre(1, 0) = CurrentRightFootAbsPos.y - CurrentLeftFootAbsPos.y;
    m_AngleDiffToSupportFootTheta =
        CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
    m_AngleDiffFromZMPThetaToSupportFootTheta =
        CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
  } else {
    m_vdiffsupppre(0, 0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
    m_vdiffsupppre(1, 0) = -CurrentRightFootAbsPos.y + CurrentLeftFootAbsPos.y;
    m_AngleDiffToSupportFootTheta =
        CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
    m_AngleDiffFromZMPThetaToSupportFootTheta =
        CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
  }
Thomas Moulard's avatar
Thomas Moulard committed
331

332
  ODEBUG4("ZMP::InitOnLine - Step 3 ", "ZMDInitOnLine.txt");
333
334
  // Initialization of the ZMP position
  // (stable values during the Preview control time window).
Thomas Moulard's avatar
Thomas Moulard committed
335
336
337
  int AddArraySize;
  {
    assert(m_SamplingPeriod > 0);
338
339
    double ldAddArraySize = 2 * m_PreviewControlTime / m_SamplingPeriod;
    AddArraySize = (int)ldAddArraySize;
Thomas Moulard's avatar
Thomas Moulard committed
340
341
342
343
344
345
346
347
  }

  ODEBUG(AddArraySize);
  deque<ZMPPosition> ZMPPositions;
  ZMPPositions.resize(AddArraySize);
  FinalCoMStates.resize(AddArraySize);
  LeftFootAbsolutePositions.resize(AddArraySize);
  RightFootAbsolutePositions.resize(AddArraySize);
348
  int CurrentZMPindex = 0;
Thomas Moulard's avatar
Thomas Moulard committed
349

350
351
  // Also very important for the initialization: reshape
  // the ZMP reference for a smooth starting.
352
353
  double startingZMPREF[3] = {lStartingCOMState.x[0], lStartingCOMState.y[0],
                              lStartingZMPPosition(2)};
olivier-stasse's avatar
olivier-stasse committed
354

355
356
  // Reset the current ZMP because of the formulation of
  // Kajita implying that the CoM starts at (0,0).
357
358
359
  startingZMPREF[0] = 0.0;
  startingZMPREF[1] = 0.0;
  startingZMPREF[2] = lStartingZMPPosition(2);
student's avatar
update    
student committed
360

Thomas Moulard's avatar
Thomas Moulard committed
361
  // Make sure that the robot thinks it is at the position it thinks it is.
362
363
  // double startingZMPREF[3] =  { 0.00949035, 0.00142561,
  // lStartingZMPPosition(2)};
364
365
366
367
368
  double finalZMPREF[2] = {m_ZMPNeutralPosition[0], m_ZMPNeutralPosition[1]};
  ODEBUG("ZMPNeutralPosition: "
         << m_ZMPNeutralPosition[0] << " " << m_ZMPNeutralPosition[1] << endl
         << "StartingZMPPosition(toto):" << lStartingZMPPosition(0) << " "
         << lStartingZMPPosition(1) << " " << lStartingZMPPosition(2) << endl
369
         << "lStartingCOMState: " << lStartingCOMState.x[0] << " "
370
         << lStartingCOMState.y[0] << " " << lStartingCOMState.z[0] << endl
371
         << "CurrentAbsTheta : " << CurrentAbsTheta << endl
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
         << "AddArraySize:" << AddArraySize << " " << m_PreviewControlTime
         << " " << m_SamplingPeriod << endl
         << "FinalZMPref :( " << finalZMPREF[0] << " , " << finalZMPREF[1]
         << " ) " << ZMPPositions.size() << endl
         << "InitRightFootAbsPos.z " << InitRightFootAbsolutePosition.z);
  ODEBUG("lStartingCOMState: " << lStartingCOMState.x[0] << " "
                               << lStartingCOMState.y[0] << " "
                               << lStartingCOMState.z[0]);

  ODEBUG4("ZMP::InitOnLine - Step 4 ", "ZMDInitOnLine.txt");
  for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
    double coef = (double)i / (double)ZMPPositions.size();
    double icoef =
        (double)(ZMPPositions.size() - i) / (double)ZMPPositions.size();
    // Set ZMP positions.

    // Smooth ramp
    ZMPPositions[CurrentZMPindex].px =
        startingZMPREF[0] + (finalZMPREF[0] - startingZMPREF[0]) * coef;
    ZMPPositions[CurrentZMPindex].py =
        startingZMPREF[1] + (finalZMPREF[1] - startingZMPREF[1]) * coef;
    ZMPPositions[CurrentZMPindex].pz =
        (-startingZMPREF[2] + InitRightFootAbsolutePosition.z) * icoef;
    ZMPPositions[CurrentZMPindex].theta = CurrentAbsTheta;
    ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
    ZMPPositions[CurrentZMPindex].stepType = 0;

    // Set CoM positions.
    FinalCoMStates[CurrentZMPindex].z[0] = m_ComHeight;
    FinalCoMStates[CurrentZMPindex].z[1] = 0.0;
    FinalCoMStates[CurrentZMPindex].z[2] = 0.0;

    FinalCoMStates[CurrentZMPindex].pitch[0] =
405
        FinalCoMStates[CurrentZMPindex].pitch[1] =
406
            FinalCoMStates[CurrentZMPindex].pitch[2] = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
407

408
    FinalCoMStates[CurrentZMPindex].roll[0] =
409
        FinalCoMStates[CurrentZMPindex].roll[1] =
410
            FinalCoMStates[CurrentZMPindex].roll[2] = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
411

412
    FinalCoMStates[CurrentZMPindex].yaw[0] =
413
        ZMPPositions[CurrentZMPindex].theta;
414
    FinalCoMStates[CurrentZMPindex].yaw[1] =
415
        FinalCoMStates[CurrentZMPindex].yaw[2] = 0.0;
student's avatar
update    
student committed
416

417
418
419
    // Set Left and Right Foot positions.
    LeftFootAbsolutePositions[CurrentZMPindex] = CurrentLeftFootAbsPos;
    RightFootAbsolutePositions[CurrentZMPindex] = CurrentRightFootAbsPos;
Thomas Moulard's avatar
Thomas Moulard committed
420

421
422
    LeftFootAbsolutePositions[CurrentZMPindex].time =
        RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
Thomas Moulard's avatar
Thomas Moulard committed
423

424
    LeftFootAbsolutePositions[CurrentZMPindex].stepType =
425
        RightFootAbsolutePositions[CurrentZMPindex].stepType = 10;
student's avatar
update    
student committed
426

427
428
429
    m_CurrentTime += m_SamplingPeriod;
    CurrentZMPindex++;
  }
student's avatar
update    
student committed
430
431

  // The first foot when walking dynamically
Thomas Moulard's avatar
Thomas Moulard committed
432
433
  // does not leave the soil, but needs to be treated for the first phase.
  m_RelativeFootPositions.push_back(RelativeFootPositions[0]);
434
  FilterOutValues(ZMPPositions, FinalZMPPositions, true);
Thomas Moulard's avatar
Thomas Moulard committed
435

436
437
438
439
440
441
  ODEBUG5("InitOnLine", "DebugDataRFPos.txt");
  for (unsigned int i = 1; i < RelativeFootPositions.size(); i++) {
    OnLineAddFoot(RelativeFootPositions[i], FinalZMPPositions, FinalCoMStates,
                  LeftFootAbsolutePositions, RightFootAbsolutePositions, false);
  }
  ODEBUG5("ZMP::InitOnLine: End ", "ZMDInitOnLine.txt");
Thomas Moulard's avatar
Thomas Moulard committed
442
443
444
445

  return RelativeFootPositions.size();
}

446
447
void ZMPDiscretization::UpdateCurrentSupportFootPosition(
    RelativeFootPosition aRFP) {
Thomas Moulard's avatar
Thomas Moulard committed
448
449
450
  m_PrevCurrentSupportFootPosition = m_CurrentSupportFootPosition;

  // First orientation
451
452
453
454
455
456
457
458
459
460
461
462
463
464
  double c = cos(aRFP.theta * M_PI / 180.0);
  double s = sin(aRFP.theta * M_PI / 180.0);
  Eigen::Matrix<double, 2, 2> MM;
  ;
  Eigen::Matrix<double, 2, 2> Orientation;
  ;

  MM(0, 0) = c;
  MM(0, 1) = -s;
  MM(1, 0) = s;
  MM(1, 1) = c;
  for (int k = 0; k < 2; k++)
    for (int l = 0; l < 2; l++)
      Orientation(k, l) = m_CurrentSupportFootPosition(k, l);
student's avatar
update    
student committed
465

Thomas Moulard's avatar
Thomas Moulard committed
466
  // second position.
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
  Eigen::Matrix<double, 2, 1> v;
  ;
  Eigen::Matrix<double, 2, 1> v2;
  ;

  v(0, 0) = aRFP.sx;
  v(1, 0) = aRFP.sy;

  Orientation = MM * Orientation;
  v2 = Orientation * v;
  ODEBUG("v :" << v
               << " "
                  "v2 : "
               << v2
               << " "
                  "Orientation : "
               << Orientation
               << " "
                  "CurrentSupportFootPosition: "
               << m_CurrentSupportFootPosition);

  for (int k = 0; k < 2; k++)
    for (int l = 0; l < 2; l++)
      m_CurrentSupportFootPosition(k, l) = Orientation(k, l);

  for (int k = 0; k < 2; k++)
    m_CurrentSupportFootPosition(k, 2) += v2(k, 0);

  ODEBUG("v :" << v
               << " "
                  "v2 : "
               << v2
               << " "
                  "Orientation : "
               << Orientation
               << " "
                  "CurrentSupportFootPosition: "
               << m_CurrentSupportFootPosition);
Thomas Moulard's avatar
Thomas Moulard committed
505
506
}

507
508
509
510
511
512
void ZMPDiscretization::OnLine(
    double,                        // time,
    deque<ZMPPosition> &,          // FinalZMPPositions,
    deque<COMState> &,             // FinalCOMStates,
    deque<FootAbsolutePosition> &, // FinalLeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &) // FinalRightFootAbsolutePositions)
Thomas Moulard's avatar
Thomas Moulard committed
513
514
515
516
517
{
  /* Does nothing... */
}

/* The interface method which returns an appropriate update of the
student's avatar
update    
student committed
518
   appropriate stacks (ZMPRef, FootPosition) depending on the
Thomas Moulard's avatar
Thomas Moulard committed
519
   state of the relative steps stack. */
520
521
522
523
524
525
void ZMPDiscretization::OnLineAddFoot(
    RelativeFootPosition &NewRelativeFootPosition,
    deque<ZMPPosition> &FinalZMPPositions, deque<COMState> &FinalCOMStates,
    deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions,
    bool EndSequence) {
Thomas Moulard's avatar
Thomas Moulard committed
526
527
528
529
  deque<ZMPPosition> ZMPPositions;
  deque<FootAbsolutePosition> LeftFootAbsolutePositions;
  deque<FootAbsolutePosition> RightFootAbsolutePositions;
  FootAbsolutePosition CurrentLeftFootAbsPos, CurrentRightFootAbsPos;
530
  double CurrentAbsZMPTheta = 0;
Thomas Moulard's avatar
Thomas Moulard committed
531
532
533
534
535
536

  CurrentLeftFootAbsPos = FinalLeftFootAbsolutePositions.back();
  CurrentRightFootAbsPos = FinalRightFootAbsolutePositions.back();
  CurrentAbsZMPTheta = FinalZMPPositions.back().theta;

  m_RelativeFootPositions.push_back(NewRelativeFootPosition);
537
538
539
540
541
542
543
544
545
546
547
548
549
550
  int WhoIsSupportFoot = 1;
  double TimeFirstPhase = 0.0;
  int CurrentZMPindex = 0;
  Eigen::Matrix<double, 2, 1> vdiffsupp;
  ;
  Eigen::Matrix<double, 2, 1> vrel;
  ;

  double lTdble = m_Tdble, lTsingle = m_Tsingle;

  ODEBUG5(m_RelativeFootPositions[0].sx << " " << m_RelativeFootPositions[0].sy
                                        << " "
                                        << m_RelativeFootPositions[0].theta,
          "DebugDataRFPos.txt");
551
  ODEBUG(" OnLineAddFoot: m_RelativeFootPositions.size: "
552
553
554
555
556
557
558
559
560
561
562
         << m_RelativeFootPositions.size());
  ODEBUG(" OnLineAddFoot: "
         << endl
         << " NewRelativeFootPositions.x: " << NewRelativeFootPosition.sx
         << " NewRelativeFootPositions.y: " << NewRelativeFootPosition.sy
         << " NewRelativeFootPositions.theta: "
         << NewRelativeFootPosition.theta);
  if (m_RelativeFootPositions[1].DStime != 0.0) {
    lTdble = m_RelativeFootPositions[1].DStime;
    lTsingle = m_RelativeFootPositions[1].SStime;
  }
Thomas Moulard's avatar
Thomas Moulard committed
563
  // Compute on the direction of the support foot.
student's avatar
update    
student committed
564
  //  double stheta = sin(m_RelativeFootPositions[1].theta*M_PI/180.0);
565
  ODEBUG("Time of double support phase in OnLineFootAdd: " << lTdble);
Thomas Moulard's avatar
Thomas Moulard committed
566
567
568
  TimeFirstPhase = lTdble;

  // Initialize who is support foot.
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
  if (m_RelativeFootPositions[0].sy < 0) {
    WhoIsSupportFoot = -1; // Right
    m_vdiffsupppre(0, 0) = CurrentRightFootAbsPos.x - CurrentLeftFootAbsPos.x;
    m_vdiffsupppre(1, 0) = CurrentRightFootAbsPos.y - CurrentLeftFootAbsPos.y;
    m_AngleDiffToSupportFootTheta =
        CurrentRightFootAbsPos.theta - CurrentLeftFootAbsPos.theta;
    m_AngleDiffFromZMPThetaToSupportFootTheta =
        CurrentRightFootAbsPos.theta - CurrentAbsZMPTheta;
  } else {
    WhoIsSupportFoot = 1; // Left
    m_vdiffsupppre(0, 0) = -CurrentRightFootAbsPos.x + CurrentLeftFootAbsPos.x;
    m_vdiffsupppre(1, 0) = -CurrentRightFootAbsPos.y + CurrentLeftFootAbsPos.y;
    m_AngleDiffToSupportFootTheta =
        CurrentLeftFootAbsPos.theta - CurrentRightFootAbsPos.theta;
    m_AngleDiffFromZMPThetaToSupportFootTheta =
        CurrentLeftFootAbsPos.theta - CurrentAbsZMPTheta;
  }
student's avatar
student committed
586

587
  double TimeForThisFootPosition = TimeFirstPhase + lTsingle;
588
589
  ODEBUG5("TimeFirstPhase: " << TimeFirstPhase << " lTsingle: " << lTsingle,
          "DebugData.txt");
Thomas Moulard's avatar
Thomas Moulard committed
590
591
  // Compute the size of cells to add inside the array.
  assert(m_SamplingPeriod > 0);
592
  double l2AddArraySize = TimeForThisFootPosition / m_SamplingPeriod;
Thomas Moulard's avatar
Thomas Moulard committed
593
  int AddArraySize = (unsigned int)round(l2AddArraySize);
594
595
596
597
  ODEBUG("Added part: " << AddArraySize << " " << l2AddArraySize
                        << " TimeForThisFootPosition "
                        << TimeForThisFootPosition << " SamplingPeriod"
                        << m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
598
599
  ZMPPositions.resize(AddArraySize);
  LeftFootAbsolutePositions.resize(AddArraySize);
student's avatar
update    
student committed
600
601
  RightFootAbsolutePositions.resize(AddArraySize);

602
  m_CurrentAbsTheta += m_RelativeFootPositions[0].theta;
Thomas Moulard's avatar
Thomas Moulard committed
603
604
605
606
  //  m_CurrentAbsTheta = fmod(m_CurrentAbsTheta,180);

  // Computes the new ABSOLUTE position of the supporting foot .
  UpdateCurrentSupportFootPosition(m_RelativeFootPositions[0]);
student's avatar
update    
student committed
607

Thomas Moulard's avatar
Thomas Moulard committed
608
609
  // First Phase of the step cycle.
  assert(m_SamplingPeriod > 0);
610
  double dSizeOf1stPhase = TimeFirstPhase / m_SamplingPeriod;
Thomas Moulard's avatar
Thomas Moulard committed
611
  unsigned int SizeOf1stPhase = (unsigned int)round(dSizeOf1stPhase);
612
  ODEBUG("SizeOf1stPhase:" << SizeOf1stPhase);
student's avatar
update    
student committed
613
  ODEBUG("m_vdiffsupppre : " << m_vdiffsupppre);
614
  double px0, py0, theta0, delta_x, delta_y;
Thomas Moulard's avatar
Thomas Moulard committed
615

616
  ODEBUG("FinalZMPPositions.size() = " << FinalZMPPositions.size());
Thomas Moulard's avatar
Thomas Moulard committed
617
618
619
620
  // Initial value to start the new ZMP profile.
  px0 = FinalZMPPositions.back().px;
  py0 = FinalZMPPositions.back().py;
  theta0 = FinalZMPPositions.back().theta;
student's avatar
update    
student committed
621

622
  Eigen::Matrix<double, 3, 1> ZMPInFootCoordinates;
student's avatar
update    
student committed
623

Thomas Moulard's avatar
Thomas Moulard committed
624
625
626
  ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
  ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
  ZMPInFootCoordinates[2] = 1.0;
student's avatar
update    
student committed
627

628
  Eigen::Matrix<double, 3, 1> ZMPInWorldCoordinates;
student's avatar
update    
student committed
629

630
  ZMPInWorldCoordinates = m_CurrentSupportFootPosition * ZMPInFootCoordinates;
student's avatar
update    
student committed
631

632
633
  delta_x = (ZMPInWorldCoordinates(0) - px0) / SizeOf1stPhase;
  delta_y = (ZMPInWorldCoordinates(1) - py0) / SizeOf1stPhase;
student's avatar
update    
student committed
634

635
636
637
638
639
640
641
  ODEBUG("delta_x :" << delta_x << " delta_y : " << delta_y
                     << " m_CurrentSFP: " << m_CurrentSupportFootPosition
                     << " ZMPInFC : " << ZMPInFootCoordinates
                     << " ZMPinWC : " << ZMPInWorldCoordinates
                     << " px0: " << px0 << " py0:" << py0);
  ODEBUG5("Step 4 TimeForThisFootPosition " << TimeForThisFootPosition,
          "DebugData.txt");
student's avatar
update    
student committed
642
643

  // ZMP profile is changed if the stepping over is on, and then
Thomas Moulard's avatar
Thomas Moulard committed
644
  // depends on the phase during stepping over.
student's avatar
update    
student committed
645
  bool DoIt = 1;
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
  if (DoIt) {
    if (m_RelativeFootPositions[1].stepType == 3) {
      // delta_x = (m_CurrentSupportFootPosition(0,2)+
      // m_ZMPShift3Begin - px0)/SizeOf1stPhase;
      delta_x = (m_CurrentSupportFootPosition(0, 2) + m_ZMPShift[0] - px0) /
                SizeOf1stPhase;
      // delta_y = (m_CurrentSupportFootPosition(1,2)+
      // (WhoIsSupportFoot)*m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
      delta_y = (m_CurrentSupportFootPosition(1, 2) - py0) / SizeOf1stPhase;
    }
    if (m_RelativeFootPositions[1].stepType == 4) {
      delta_x = (m_CurrentSupportFootPosition(0, 2) + m_ZMPShift[2] - px0) /
                SizeOf1stPhase;
      delta_y = (m_CurrentSupportFootPosition(1, 2) - py0) / SizeOf1stPhase;
      // delta_x = (CurrentSupportFootPosition(0,2)
      // +m_ZMPShift4Begin - px0)/SizeOf1stPhase;
      // delta_y = (CurrentSupportFootPosition(1,2)+(WhoIsSupportFoot)
      // *m_ZMPShift3BeginY - py0)/SizeOf1stPhase;
    }
student's avatar
update    
student committed
665

666
667
668
669
670
671
672
673
674
675
676
677
678
679
    if (m_RelativeFootPositions[1].stepType == 5) {
      delta_x =
          (m_CurrentSupportFootPosition(0, 2) -
           (m_ZMPShift[0] + m_ZMPShift[2] + m_ZMPShift[1] + m_ZMPShift[3]) -
           px0) /
          SizeOf1stPhase;
      delta_y = (m_CurrentSupportFootPosition(1, 2) - py0) / SizeOf1stPhase;
      // delta_x = (CurrentSupportFootPosition(0,2)-(m_ZMPShift3Begin +
      // m_ZMPShift4Begin+m_ZMPShift3End + m_ZMPShift4End) -
      // px0)/SizeOf1stPhase;
      // delta_y = (CurrentSupportFootPosition(1,2)-
      // (WhoIsSupportFoot)*(m_ZMPShift3BeginY +
      // m_ZMPShift4BeginY+m_ZMPShift3EndY + m_ZMPShift4EndY) -
      // py0)/SizeOf1stPhase;
student's avatar
update    
student committed
680
    }
681
  }
student's avatar
update    
student committed
682

683
684
  ODEBUG5(" GetZMPDiscretization: Step 5 " << AddArraySize << " ",
          "DebugData.txt");
685
  ODEBUG("SizeOf1stPhase: " << SizeOf1stPhase << "dx: " << delta_x
686
                            << " dy: " << delta_y);
Thomas Moulard's avatar
Thomas Moulard committed
687

student's avatar
update    
student committed
688
  // First phase of the cycle aka Double support phase.
689
  for (unsigned int k = 0; k < SizeOf1stPhase; k++) {
student's avatar
update    
student committed
690

691
692
693
694
    ZMPPositions[CurrentZMPindex].px = px0 + k * delta_x;
    ZMPPositions[CurrentZMPindex].py = py0 + k * delta_y;
    ZMPPositions[CurrentZMPindex].pz = 0;
    ZMPPositions[CurrentZMPindex].theta = theta0;
student's avatar
update    
student committed
695

696
    ZMPPositions[CurrentZMPindex].time = m_CurrentTime;
student's avatar
update    
student committed
697

698
699
    ZMPPositions[CurrentZMPindex].stepType =
        m_RelativeFootPositions[1].stepType + 10;
student's avatar
update    
student committed
700

701
702
703
    // Right now the foot is not moving during the double support
    // TO DO: whatever you need to do ramzi....
    LeftFootAbsolutePositions[CurrentZMPindex] =
704
        FinalLeftFootAbsolutePositions.back();
student's avatar
update    
student committed
705

706
707
    // WARNING : This assume that you are walking on a plane.
    LeftFootAbsolutePositions[CurrentZMPindex].z = 0.0;
student's avatar
update    
student committed
708

709
    RightFootAbsolutePositions[CurrentZMPindex] =
710
        FinalRightFootAbsolutePositions.back();
Thomas Moulard's avatar
Thomas Moulard committed
711

712
713
    // WARNING : This assume that you are walking on a plane.
    RightFootAbsolutePositions[CurrentZMPindex].z = 0.0;
student's avatar
update    
student committed
714

715
    LeftFootAbsolutePositions[CurrentZMPindex].time =
716
        RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
Thomas Moulard's avatar
Thomas Moulard committed
717

718
    LeftFootAbsolutePositions[CurrentZMPindex].stepType =
719
        RightFootAbsolutePositions[CurrentZMPindex].stepType =
720
721
722
723
724
725
726
727
728
729
            m_RelativeFootPositions[1].stepType + 10;
    /*
      ofstream aoflocal;
      aoflocal.open("Corrections.dat",ofstream::app);
      aoflocal << "0.0 0.0 0.0 "<<endl;
      aoflocal.close();
    */
    m_CurrentTime += m_SamplingPeriod;
    CurrentZMPindex++;
  }
Thomas Moulard's avatar
Thomas Moulard committed
730
  //-- End Of First phase.
student's avatar
update    
student committed
731

Thomas Moulard's avatar
Thomas Moulard committed
732
  // Second Phase of the step cycle aka Single Support Phase.
student's avatar
update    
student committed
733

Thomas Moulard's avatar
Thomas Moulard committed
734
735
  // Next Theta : next relative angle between the current support foot angle
  // and the next support foot angle.
736
  double NextTheta = 0;
Thomas Moulard's avatar
Thomas Moulard committed
737
738
739
740
741

  // RelTheta : relative angle between the current angle of the
  // flying foot and the next angle of the flying foot.
  // RelZMPTheta : relative angle between the current angle of the
  // zmp and the next angle of the zmp.
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
  double RelTheta = 0, RelZMPTheta = 0;
  if (m_RelativeFootPositions.size() > 1) {
    NextTheta = m_RelativeFootPositions[1].theta;
    RelTheta = NextTheta + m_AngleDiffToSupportFootTheta;
    RelZMPTheta = NextTheta + m_AngleDiffFromZMPThetaToSupportFootTheta;

    double c, s;
    c = cos(NextTheta * M_PI / 180.0);
    s = sin(NextTheta * M_PI / 180.0);
    Eigen::Matrix<double, 2, 2> Orientation;
    ;
    Eigen::Matrix<double, 2, 1> v;
    ;
    Orientation(0, 0) = c;
    Orientation(0, 1) = -s;
    Orientation(1, 0) = s;
    Orientation(1, 1) = c;

    Eigen::Matrix<double, 2, 2> SubOrientation;
    ;
    SubOrientation = m_CurrentSupportFootPosition.block(0, 0, 2, 2);
    Orientation = Orientation * SubOrientation;

    v(0, 0) = m_RelativeFootPositions[1].sx;
    v(1, 0) = m_RelativeFootPositions[1].sy;
    vdiffsupp = Orientation * v;

    vrel = vdiffsupp + m_vdiffsupppre;

    // Compute relative feet orientation for the next step
  } else {
    vrel(0, 0) = 0.0;
    vrel(1, 0) = 0.0;
    RelTheta = 0.0;
    NextTheta = 0.0;
  }
Thomas Moulard's avatar
Thomas Moulard committed
778

779
780
781
782
  ODEBUG(cout << "vrel: " << vrel(0, 0) << " " << vrel(1, 0));
  ODEBUG(cout << "vdiffsupp: " << vdiffsupp(0, 0) << " " << vdiffsupp(1, 0));
  ODEBUG("vdiffsupppre: " << m_vdiffsupppre(0, 0) << " "
                          << m_vdiffsupppre(1, 0));
Thomas Moulard's avatar
Thomas Moulard committed
783

784
785
  ODEBUG5(" GetZMPDiscretization: Step 6 " << ZMPPositions.size() << " ",
          "DebugData.txt");
Thomas Moulard's avatar
Thomas Moulard committed
786
787

  m_vdiffsupppre = vdiffsupp;
student's avatar
update    
student committed
788

Thomas Moulard's avatar
Thomas Moulard committed
789
  // m_AngleDiffToSupportFootTheta = NextTheta;
student's avatar
update    
student committed
790

Thomas Moulard's avatar
Thomas Moulard committed
791
792
793
794
795
  // Create the polynomes for the none-support foot.
  // Change 08/12/2005: Speed up the modification of X and Y
  // for vertical landing of the foot (Kajita-San's trick n 1)
  //   double ModulationSupportCoefficient = 0.9;
  double ModulatedSingleSupportTime = lTsingle * m_ModulationSupportCoefficient;
796
  double EndOfLiftOff = (lTsingle - ModulatedSingleSupportTime) * 0.5;
student's avatar
update    
student committed
797
  ODEBUG("ModulatedSingleSupportTime:" << ModulatedSingleSupportTime << " "
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
                                       << vrel(0, 0) << " " << vrel(1, 0));
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::X_AXIS, ModulatedSingleSupportTime,
      vrel(0, 0));
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::Y_AXIS, ModulatedSingleSupportTime,
      vrel(1, 0));
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::Z_AXIS, m_Tsingle, 0);
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::THETA_AXIS, ModulatedSingleSupportTime,
      RelTheta);
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::OMEGA_AXIS, EndOfLiftOff, m_Omega);
  m_FootTrajectoryGenerationStandard->SetParameters(
      FootTrajectoryGenerationStandard::OMEGA2_AXIS, ModulatedSingleSupportTime,
      2 * m_Omega);
student's avatar
update    
student committed
815

816
  //  m_FootTrajectoryGenerationStandard->print();
Thomas Moulard's avatar
Thomas Moulard committed
817

818
819
  // m_PolynomeZMPTheta->SetParameters(lTsingle,NextZMPTheta);
  m_PolynomeZMPTheta->SetParameters(lTsingle, RelZMPTheta);
Thomas Moulard's avatar
Thomas Moulard committed
820
  assert(m_SamplingPeriod > 0);
821
  double dSizeOfSndPhase = lTsingle / m_SamplingPeriod;
Thomas Moulard's avatar
Thomas Moulard committed
822
  unsigned int SizeOfSndPhase = (unsigned int)round(dSizeOfSndPhase);
823
  int indexinitial = CurrentZMPindex - 1;
Thomas Moulard's avatar
Thomas Moulard committed
824

student's avatar
update    
student committed
825
826
  /*//polynomial planning for the stepover

827
828
829
830
    if (m_RelativeFootPositions[0].stepType==3)
    {
    StepOverPolyPlanner(m_RelativeFootPositions[0].stepType);
    };
student's avatar
update    
student committed
831
  */
832
833
834
  double px02, py02;
  px02 = ZMPPositions[CurrentZMPindex - 1].px;
  py02 = ZMPPositions[CurrentZMPindex - 1].py;
Thomas Moulard's avatar
Thomas Moulard committed
835

836
  ODEBUG("SizeOfSndPhase: " << SizeOfSndPhase);
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
  for (unsigned int k = 0; k < SizeOfSndPhase; k++) {

    Eigen::Matrix<double, 3, 1> ZMPInFootCoordinates;

    ZMPInFootCoordinates[0] = m_ZMPNeutralPosition[0];
    ZMPInFootCoordinates[1] = m_ZMPNeutralPosition[1];
    ZMPInFootCoordinates[2] = 1.0;

    Eigen::Matrix<double, 3, 1> ZMPInWorldCoordinates;

    ZMPInWorldCoordinates = m_CurrentSupportFootPosition * ZMPInFootCoordinates;

    ODEBUG5("CSFP: " << m_CurrentSupportFootPosition << endl
                     << "ZMPiWC" << ZMPInWorldCoordinates << endl,
            "DebugData.txt");

    ZMPPositions[CurrentZMPindex].px = ZMPInWorldCoordinates(0);
    ZMPPositions[CurrentZMPindex].py = ZMPInWorldCoordinates(1);
    ZMPPositions[CurrentZMPindex].pz = 0;
    ZMPPositions[CurrentZMPindex].time = m_CurrentTime;

    if (DoIt) {
      if ((m_RelativeFootPositions[1].stepType == 3) ||
          (m_RelativeFootPositions[1].stepType == 4)) {

        if (m_RelativeFootPositions[1].stepType == 3) {
          delta_x =
              (m_CurrentSupportFootPosition(0, 2) + m_ZMPShift[1] - px02) /
              SizeOfSndPhase;
          delta_y =
              (m_CurrentSupportFootPosition(1, 2) - py02) / SizeOfSndPhase;
          // delta_x = (CurrentSupportFootPosition(0,2)+
          //  m_ZMPShift3End - px02)/SizeOfSndPhase;
          // delta_y = (CurrentSupportFootPosition(1,2)+
          //  (WhoIsSupportFoot)*m_ZMPShift3EndY - py02)/SizeOfSndPhase;
        } else {
          delta_x =
              (m_CurrentSupportFootPosition(0, 2) + m_ZMPShift[3] - px02) /
              SizeOfSndPhase;
          delta_y =
              (m_CurrentSupportFootPosition(1, 2) - py02) / SizeOfSndPhase;
          // delta_x = (CurrentSupportFootPosition(0,2)+
          //  m_ZMPShift4End - px02)/SizeOfSndPhase;
          // delta_y = (CurrentSupportFootPosition(1,2)+
          //  (WhoIsSupportFoot)*m_ZMPShift4EndY - py02)/SizeOfSndPhase;
882
        }
Thomas Moulard's avatar
Thomas Moulard committed
883

884
885
886
887
888
889
890
891
892
893
        ZMPPositions[CurrentZMPindex].px =
            ZMPPositions[CurrentZMPindex - 1].px + delta_x;
        ZMPPositions[CurrentZMPindex].py =
            ZMPPositions[CurrentZMPindex - 1].py + delta_y;
        ZMPPositions[CurrentZMPindex].pz = 0;
      }
    }

    ZMPPositions[CurrentZMPindex].theta =
        m_PolynomeZMPTheta->Compute(k * m_SamplingPeriod) +
894
        ZMPPositions[indexinitial].theta;
Thomas Moulard's avatar
Thomas Moulard committed
895

896
897
898
899
900
901
902
903
904
905
906
907
908
    ZMPPositions[CurrentZMPindex].stepType =
        WhoIsSupportFoot * m_RelativeFootPositions[0].stepType;
    if (WhoIsSupportFoot == 1) {
      m_FootTrajectoryGenerationStandard->UpdateFootPosition(
          LeftFootAbsolutePositions, RightFootAbsolutePositions,
          CurrentZMPindex, indexinitial, ModulatedSingleSupportTime,
          m_RelativeFootPositions[1].stepType, -1);
    } else {
      m_FootTrajectoryGenerationStandard->UpdateFootPosition(
          RightFootAbsolutePositions, LeftFootAbsolutePositions,
          CurrentZMPindex, indexinitial, ModulatedSingleSupportTime,
          m_RelativeFootPositions[1].stepType, 1);
    }
Thomas Moulard's avatar
Thomas Moulard committed
909

910
    LeftFootAbsolutePositions[CurrentZMPindex].time =
911
        RightFootAbsolutePositions[CurrentZMPindex].time = m_CurrentTime;
student's avatar
update    
student committed
912

913
914
915
    m_CurrentTime += m_SamplingPeriod;
    CurrentZMPindex++;
  }
Thomas Moulard's avatar
Thomas Moulard committed
916

917
918
  if (WhoIsSupportFoot == 1)
    WhoIsSupportFoot = -1; // Right
student's avatar
update    
student committed
919
  else
920
    WhoIsSupportFoot = 1; // Left
Thomas Moulard's avatar
Thomas Moulard committed
921
922
923

  m_RelativeFootPositions.pop_front();

924
925
926
  for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
    COMState aCOMState;
    aCOMState.x[0] = aCOMState.x[1] = aCOMState.x[2] = 0.0;
student's avatar
update    
student committed
927

928
    aCOMState.y[0] = aCOMState.y[1] = aCOMState.y[2] = 0.0;
student's avatar
update    
student committed
929

930
    aCOMState.z[0] = m_ComHeight;
Thomas Moulard's avatar
Thomas Moulard committed
931

932
933
934
    aCOMState.pitch[0] = aCOMState.pitch[1] = aCOMState.pitch[2] =
        aCOMState.roll[0] = aCOMState.roll[1] = aCOMState.roll[2] =
            aCOMState.yaw[1] = aCOMState.yaw[2] = 0.0;
student's avatar
update    
student committed
935

936
    aCOMState.z[1] = aCOMState.z[2] = 0.0;
student's avatar
update    
student committed
937

938
    aCOMState.yaw[0] = ZMPPositions[i].theta;
Thomas Moulard's avatar
Thomas Moulard committed
939

940
941
942
943
944
    FinalCOMStates.push_back(aCOMState);
    FinalLeftFootAbsolutePositions.push_back(LeftFootAbsolutePositions[i]);
    FinalRightFootAbsolutePositions.push_back(RightFootAbsolutePositions[i]);
  }
  FilterOutValues(ZMPPositions, FinalZMPPositions, false);
student's avatar
update    
student committed
945

946
  ODEBUG_CODE(DumpReferences(FinalZMPPositions, ZMPPositions));
student's avatar
update    
student committed
947

948
  if (EndSequence) {
Thomas Moulard's avatar
Thomas Moulard committed
949

950
951
952
953
954
    // End Phase of the walking includes the filtering.
    EndPhaseOfTheWalking(FinalZMPPositions, FinalCOMStates,
                         FinalLeftFootAbsolutePositions,
                         FinalRightFootAbsolutePositions);
  }
Thomas Moulard's avatar
Thomas Moulard committed
955
956
}

957
958
void ZMPDiscretization::DumpReferences(deque<ZMPPosition> &FinalZMPPositions,
                                       deque<ZMPPosition> &ZMPPositions) {
Thomas Moulard's avatar
Thomas Moulard committed
959

960
961
962
963
  ofstream dbg_aof("DebugZMPRefPos.dat", ofstream::app);
  for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
    dbg_aof << ZMPPositions[i].px << " " << ZMPPositions[i].py << endl;
  }
Thomas Moulard's avatar
Thomas Moulard committed
964
  dbg_aof.close();
student's avatar
update    
student committed
965

966
967
968
969
970
  dbg_aof.open("DebugFinalZMPRefPos.dat", ofstream::app);
  for (unsigned int i = 0; i < FinalZMPPositions.size(); i++) {
    dbg_aof << FinalZMPPositions[i].px << " " << FinalZMPPositions[i].py
            << endl;
  }
Thomas Moulard's avatar
Thomas Moulard committed
971
972
973
  dbg_aof.close();
}

974
975
976
977
void ZMPDiscretization::FilterOutValues(deque<ZMPPosition> &ZMPPositions,
                                        deque<ZMPPosition> &FinalZMPPositions,
                                        bool InitStep) {
  unsigned int lshift = 2;
Thomas Moulard's avatar
Thomas Moulard committed
978
  // Filter out the ZMP values.
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
  for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
    double ltmp[3] = {0, 0, 0};

    std::size_t o = FinalZMPPositions.size() - 1 - lshift;
    for (unsigned int j = 0; j < m_ZMPFilterWindow.size(); j++) {
      int r;
      r = i - j + lshift;
      if (r < 0) {

        if (InitStep) {
          ltmp[0] += m_ZMPFilterWindow[j] * ZMPPositions[lshift].px;
          ltmp[1] += m_ZMPFilterWindow[j] * ZMPPositions[lshift].py;
          ltmp[2] += m_ZMPFilterWindow[j] * ZMPPositions[lshift].pz;
        } else {
          if (-r < (int)o) {
            ltmp[0] += m_ZMPFilterWindow[j] * FinalZMPPositions[o + r].px;
            ltmp[1] += m_ZMPFilterWindow[j] * FinalZMPPositions[o + r].py;
            ltmp[2] += m_ZMPFilterWindow[j] * FinalZMPPositions[o + r].pz;
          } else {
            ltmp[0] += m_ZMPFilterWindow[j] * FinalZMPPositions[0].px;
            ltmp[1] += m_ZMPFilterWindow[j] * FinalZMPPositions[0].py;
            ltmp[2] += m_ZMPFilterWindow[j] * FinalZMPPositions[0].pz;
          }
1002
        }
1003
1004
1005
1006
1007
1008
1009
1010
1011
      } else {
        if (r >= (int)ZMPPositions.size())
          r = (int)ZMPPositions.size() - 1;

        ltmp[0] += m_ZMPFilterWindow[j] * ZMPPositions[r].px;
        ltmp[1] += m_ZMPFilterWindow[j] * ZMPPositions[r].py;
        ltmp[2] += m_ZMPFilterWindow[j] * ZMPPositions[r].pz;
      }
    }
student's avatar
update    
student committed
1012

1013
1014
1015
1016
1017
1018
1019
    ZMPPosition aZMPPos;
    aZMPPos.px = ltmp[0];
    aZMPPos.py = ltmp[1];
    aZMPPos.pz = ltmp[2];
    aZMPPos.theta = ZMPPositions[i].theta;
    aZMPPos.time = ZMPPositions[i].time;
    aZMPPos.stepType = ZMPPositions[i].stepType;
student's avatar
update    
student committed
1020

1021
1022
1023
1024
1025
1026
1027
    FinalZMPPositions.push_back(aZMPPos);
  }
  ODEBUG("ZMPPosition.back=( " << ZMPPositions.back().px << " , "
                               << ZMPPositions.back().py << " )");
  ODEBUG("FinalZMPPosition.back=( " << FinalZMPPositions.back().px << " , "
                                    << FinalZMPPositions.back().py << " )");
  ODEBUG("FinalZMPPositions.size()=" << FinalZMPPositions.size());
Thomas Moulard's avatar
Thomas Moulard committed
1028
1029
}

1030
1031
1032
1033
1034
1035
1036
1037
int ZMPDiscretization::OnLineFootChange(
    double,                        // time,
    FootAbsolutePosition &,        // aFootAbsolutePosition,
    deque<ZMPPosition> &,          // FinalZMPPositions,
    deque<COMState> &,             // CoMStates,
    deque<FootAbsolutePosition> &, // FinalLeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &, // FinalRightFootAbsolutePositions,
    StepStackHandler *)            // aStepStackHandler)
Thomas Moulard's avatar
Thomas Moulard committed
1038
1039
1040
1041
{
  return -1;
}

1042
int ZMPDiscretization::ReturnOptimalTimeToRegenerateAStep() {
Thomas Moulard's avatar
Thomas Moulard committed
1043
  assert(m_SamplingPeriod > 0);
1044
1045
  int r = (int)(m_PreviewControlTime / m_SamplingPeriod);
  return 2 * r;
Thomas Moulard's avatar
Thomas Moulard committed
1046
1047
}

1048
1049
1050
1051
1052
void ZMPDiscretization::EndPhaseOfTheWalking(
    deque<ZMPPosition> &FinalZMPPositions, deque<COMState> &FinalCOMStates,
    deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
    deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions)

Thomas Moulard's avatar
Thomas Moulard committed
1053
1054
1055
1056
1057
1058
{
  deque<ZMPPosition> ZMPPositions;
  FootAbsolutePosition LeftFootAbsolutePosition;
  FootAbsolutePosition RightFootAbsolutePosition;

  ODEBUG("m_RelativeFootPositions.size(): " << m_RelativeFootPositions.size());
1059
  if (m_RelativeFootPositions.size() > 0)
Thomas Moulard's avatar
Thomas Moulard committed
1060
1061
1062
1063
    UpdateCurrentSupportFootPosition(m_RelativeFootPositions[0]);

  // Deal with the end phase of the walking.
  assert(m_SamplingPeriod > 0);
1064
  double dlAddArraySize = m_Tdble / (2 * m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
1065
  unsigned int AddArraySize = (unsigned int)round(dlAddArraySize);
student's avatar
update    
student committed
1066

1067
  std::size_t currentsize = 0;
Thomas Moulard's avatar
Thomas Moulard committed
1068
  unsigned int CurrentZMPindex = 0;
1069
1070
1071
  ZMPPositions.resize(currentsize + AddArraySize);
  ODEBUG5(" GetZMPDiscretization: Step 7 " << currentsize << " "
                                           << AddArraySize,
1072
          "DebugData.txt");
Thomas Moulard's avatar
Thomas Moulard committed
1073

1074
  double dSizeOfEndPhase = m_Tdble / (2 * m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
1075
  unsigned int SizeOfEndPhase = (unsigned int)round(dSizeOfEndPhase);
1076
1077
  double px0, py0, delta_x, delta_y;
  double pxf = 0, pyf = 0;
Thomas Moulard's avatar
Thomas Moulard committed
1078
1079
1080
1081
1082
1083
1084
  px0 = FinalZMPPositions.back().px;
  py0 = FinalZMPPositions.back().py;
  //  int lindex = SupportFootAbsolutePositions.size()-1;

  // We assume that the last positon of the ZMP
  // will the middle of the two last position
  // of the support foot.
1085
1086
1087
1088
1089
1090
  ODEBUG("Cur/Prev SuppFootPos (X) : "
         << m_CurrentSupportFootPosition(0, 2) << " "
         << m_PrevCurrentSupportFootPosition(0, 2));
  ODEBUG("Cur/Prev SuppFootPos (Y) : "
         << m_CurrentSupportFootPosition(1, 2) << " "
         << m_PrevCurrentSupportFootPosition(1, 2));
Thomas Moulard's avatar
Thomas Moulard committed
1091

1092
1093
1094
1095
  pxf = 0.5 * (m_CurrentSupportFootPosition(0, 2) +
               m_PrevCurrentSupportFootPosition(0, 2));
  pyf = 0.5 * (m_CurrentSupportFootPosition(1, 2) +
               m_PrevCurrentSupportFootPosition(1, 2));
student's avatar
update    
student committed
1096

1097
1098
  delta_x = (pxf - px0) / (double)SizeOfEndPhase;
  delta_y = (pyf - py0) / (double)SizeOfEndPhase;
student's avatar
update    
student committed
1099

Thomas Moulard's avatar
Thomas Moulard committed
1100
1101
1102
  ZMPPositions[0].px = px0 + delta_x;
  ZMPPositions[0].py = py0 + delta_y;
  ZMPPositions[0].time = m_CurrentTime;
student's avatar
update    
student committed
1103

Thomas Moulard's avatar
Thomas Moulard committed
1104
  ZMPPositions[0].theta = FinalZMPPositions.back().theta;
1105
  ZMPPositions[0].stepType = 0;
Thomas Moulard's avatar
Thomas Moulard committed
1106
1107
  CurrentZMPindex++;

1108
1109
  LeftFootAbsolutePosition = FinalLeftFootAbsolutePositions.back();
  RightFootAbsolutePosition = FinalRightFootAbsolutePositions.back();
student's avatar
update    
student committed
1110

1111
1112
  LeftFootAbsolutePosition.time = RightFootAbsolutePosition.time =
      m_CurrentTime;
student's avatar
update    
student committed
1113

1114