ZMPPreviewControlWithMultiBodyZMP.cpp 26 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2006, 2007, 2008, 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
 *
 * Andrei  Herdt
5
 * Fumio   Kanehiro
Thomas Moulard's avatar
Thomas Moulard committed
6
 * Florent Lamiraux
7
 * Alireza Nakhaei
Thomas Moulard's avatar
Thomas Moulard committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
 * Nicolas Perrin
 * Mathieu Poirier
 * 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/>.
 *
27
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
28
29
30
31
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file ZMPPreviewControlWithMultiBodyZMP.cpp
  \brief This object generate all the values for the foot trajectories,
32
  and the desired ZMP based on a sequence of steps.
Thomas Moulard's avatar
Thomas Moulard committed
33
34
*/

35
#include <fstream>
Thomas Moulard's avatar
Thomas Moulard committed
36
37
38
#include <iostream>
#include <sstream>

39
#include <Debug.hh>
Thomas Moulard's avatar
Thomas Moulard committed
40

41
#include <PreviewControl/ZMPPreviewControlWithMultiBodyZMP.hh>
Thomas Moulard's avatar
Thomas Moulard committed
42
43
44

using namespace PatternGeneratorJRL;

45
46
47
ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP(
    SimplePluginManager *lSPM)
    : SimplePlugin(lSPM) {
Thomas Moulard's avatar
Thomas Moulard committed
48
49

  m_ComAndFootRealization = 0;
50
  m_PinocchioRobot = 0;
Thomas Moulard's avatar
Thomas Moulard committed
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

  m_StageStrategy = ZMPCOM_TRAJECTORY_FULL;

  RESETDEBUG4("DebugData.txt");
  RESETDEBUG4("DebugDataqrql.txt");
  RESETDEBUG4("DebugDataDiffZMP.txt");
  RESETDEBUG4("DebugDataCOMPC1.txt");
  RESETDEBUG4("DebugDataWaistZMP.txt");
  RESETDEBUG4("DebugDataZMPMB1.txt");
  RESETDEBUG4("DebugDataDeltaCOM.txt");
  RESETDEBUG4("DebugDataStartingCOM.dat");
  RESETDEBUG4("DebugDataUB.txt");
  RESETDEBUG4("DebugDatadUB.txt");
  RESETDEBUG4("DebugDataIKL.dat");
  RESETDEBUG4("DebugDataIKR.dat");
  RESETDEBUG4("DebugDataWP.txt");
  RESETDEBUG4("Dump.dat");
  RESETDEBUG4("DebugConfSO.dat");
  RESETDEBUG4("DebugConfSV.dat");
  RESETDEBUG4("DebugConfSA.dat");
71
  RESETDEBUG5("DebugDataCheckZMP1.txt");
Thomas Moulard's avatar
Thomas Moulard committed
72
  RESETDEBUG4("2ndStage.dat");
73
  RESETDEBUG4("ZMPPCWMZOGSOC.dat");
74
  // Sampling period.DMB
Thomas Moulard's avatar
Thomas Moulard committed
75
  m_SamplingPeriod = -1;
76

Thomas Moulard's avatar
Thomas Moulard committed
77
78
79
  RegisterMethods();

  // Initialization of the first and second preview controls.
80
81
82
83
  m_PC1x.resize(3, 1);
  m_PC1y.resize(3, 1);
  m_Deltax.resize(3, 1);
  m_Deltay.resize(3, 1);
Thomas Moulard's avatar
Thomas Moulard committed
84

85
86
87
  m_PC = new PreviewControl(
      lSPM, OptimalControllerSolver::MODE_WITHOUT_INITIALPOS, true);
  m_StartingNewSequence = true;
Thomas Moulard's avatar
Thomas Moulard committed
88

89
90
91
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++)
      m_FinalDesiredCOMPose(i, j) = 0.0;
Thomas Moulard's avatar
Thomas Moulard committed
92
93
94
95

  m_NumberOfIterations = 0;
}

96
ZMPPreviewControlWithMultiBodyZMP::~ZMPPreviewControlWithMultiBodyZMP() {}
Thomas Moulard's avatar
Thomas Moulard committed
97

98
void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControl(PreviewControl *aPC) {
Thomas Moulard's avatar
Thomas Moulard committed
99
100
101
  m_PC = aPC;
  m_SamplingPeriod = m_PC->SamplingPeriod();
  m_PreviewControlTime = m_PC->PreviewControlTime();
102
  m_NL = (unsigned int)(m_PreviewControlTime / m_SamplingPeriod);
103
104
}

105
106
107
108
109
void ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization(
    COMState &acomp, FootAbsolutePosition &aLeftFAP,
    FootAbsolutePosition &aRightFAP, Eigen::VectorXd &CurrentConfiguration,
    Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration,
    unsigned long int IterationNumber, int StageOfTheAlgorithm) {
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
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164

  // New scheme for WPG v3.0
  // We call the object in charge of generating the whole body
  // motion  ( for a given CoM and Feet points)
  // before applying the second filter.

  Eigen::VectorXd aCOMState(6);
  Eigen::VectorXd aCOMSpeed(6);
  Eigen::VectorXd aCOMAcc(6);

  aCOMState(0) = acomp.x[0];
  aCOMState(1) = acomp.y[0];
  aCOMState(2) = acomp.z[0];
  aCOMState(3) = acomp.roll[0];
  aCOMState(4) = acomp.pitch[0];
  aCOMState(5) = acomp.yaw[0];

  aCOMSpeed(0) = acomp.x[1];
  aCOMSpeed(1) = acomp.y[1];
  aCOMSpeed(2) = acomp.z[1];
  aCOMSpeed(3) = acomp.roll[1];
  aCOMSpeed(4) = acomp.roll[1];
  aCOMSpeed(5) = acomp.roll[1];

  aCOMAcc(0) = acomp.x[2];
  aCOMAcc(1) = acomp.y[2];
  aCOMAcc(2) = acomp.z[2];
  aCOMAcc(3) = acomp.roll[2];
  aCOMAcc(4) = acomp.roll[2];
  aCOMAcc(5) = acomp.roll[2];

  Eigen::VectorXd aLeftFootPosition(5);
  Eigen::VectorXd aRightFootPosition(5);

  aLeftFootPosition(0) = aLeftFAP.x;
  aLeftFootPosition(1) = aLeftFAP.y;
  aLeftFootPosition(2) = aLeftFAP.z;
  aLeftFootPosition(3) = aLeftFAP.theta;
  aLeftFootPosition(4) = aLeftFAP.omega;

  aRightFootPosition(0) = aRightFAP.x;
  aRightFootPosition(1) = aRightFAP.y;
  aRightFootPosition(2) = aRightFAP.z;
  aRightFootPosition(3) = aRightFAP.theta;
  aRightFootPosition(4) = aRightFAP.omega;

  /* Get the current configuration vector */
  CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();

  /* Get the current velocity vector */
  CurrentVelocity = m_PinocchioRobot->currentRPYVelocity();

  /* Get the current acceleration vector */
  CurrentAcceleration = m_PinocchioRobot->currentRPYAcceleration();

165
166
167
168
169
170
171
172
  m_ComAndFootRealization->ComputePostureForGivenCoMAndFeetPosture(
      aCOMState, aCOMSpeed, aCOMAcc, aLeftFootPosition, aRightFootPosition,
      CurrentConfiguration, CurrentVelocity, CurrentAcceleration,
      IterationNumber, StageOfTheAlgorithm);

  if (StageOfTheAlgorithm == 0) {
    /* Update the current configuration vector */
    m_PinocchioRobot->currentRPYConfiguration(CurrentConfiguration);
Guilhem Saurel's avatar
Guilhem Saurel committed
173

174
175
    /* Update the current velocity vector */
    m_PinocchioRobot->currentRPYVelocity(CurrentVelocity);
Guilhem Saurel's avatar
Guilhem Saurel committed
176

177
178
179
    /* Update the current acceleration vector */
    m_PinocchioRobot->currentRPYAcceleration(CurrentAcceleration);
  }
180
181
182
183
}

/* Removed lqr and lql, now they should be set automatically by
   m_ComAndFootRealization */
184
185
186
187
188
189
int ZMPPreviewControlWithMultiBodyZMP::OneGlobalStepOfControl(
    FootAbsolutePosition &LeftFootPosition,
    FootAbsolutePosition &RightFootPosition, ZMPPosition &,
    COMState &refandfinalCOMState, Eigen::VectorXd &CurrentConfiguration,
    Eigen::VectorXd &CurrentVelocity, Eigen::VectorXd &CurrentAcceleration) {
  FirstStageOfControl(LeftFootPosition, RightFootPosition, refandfinalCOMState);
190
191
192
193
194
195
  // This call is suppose to initialize
  // correctly the current configuration, speed and acceleration.
  COMState acompos = m_FIFOCOMStates[m_NL];
  FootAbsolutePosition aLeftFAP = m_FIFOLeftFootPosition[m_NL];
  FootAbsolutePosition aRightFAP = m_FIFORightFootPosition[m_NL];

196
197
198
199
200
201
  ODEBUG4SIMPLE(m_FIFOZMPRefPositions[0].px
                    << " " << m_FIFOZMPRefPositions[0].py << " "
                    << m_FIFOZMPRefPositions[0].pz << " " << acompos.x[0] << " "
                    << acompos.y[0] << " " << acompos.z[0] << " " << aLeftFAP.x
                    << " " << aLeftFAP.y << " " << aLeftFAP.z << " "
                    << aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
202
203
                "1ststage.dat");

Guilhem Saurel's avatar
Guilhem Saurel committed
204
205
206
207
  int StageOfTheAlgorithm = 0;
  CallToComAndFootRealization(
      acompos, aLeftFAP, aRightFAP, CurrentConfiguration, CurrentVelocity,
      CurrentAcceleration, m_NumberOfIterations, StageOfTheAlgorithm);
208

209
  if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY)
210
211
212
213
214
215
216
    EvaluateMultiBodyZMP(-1);

  aLeftFAP = m_FIFOLeftFootPosition[0];
  aRightFAP = m_FIFORightFootPosition[0];

  SecondStageOfControl(refandfinalCOMState);

217
218
219
220
221
222
223
224
225
  ODEBUG4SIMPLE(
      refandfinalCOMState.x[0]
          << " " << refandfinalCOMState.y[0] << " " << refandfinalCOMState.z[0]
          << " " << aLeftFAP.x << " " << aLeftFAP.y << " " << aLeftFAP.z << " "
          << aLeftFAP.stepType << " " << aRightFAP.x << " " << aRightFAP.y
          << " " << aRightFAP.z << " " << aRightFAP.stepType,
      "2ndStage.dat");

  if (m_StageStrategy != ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY) {
Guilhem Saurel's avatar
Guilhem Saurel committed
226
    int StageOfTheAlgorithm = 1;
227
228
    CallToComAndFootRealization(
        refandfinalCOMState, aLeftFAP, aRightFAP, CurrentConfiguration,
229
        CurrentVelocity, CurrentAcceleration, m_NumberOfIterations - m_NL,
Guilhem Saurel's avatar
Guilhem Saurel committed
230
        StageOfTheAlgorithm);
231
  }
232
233
234
235

  // Here it is assumed that the 4x4 CoM matrix
  // is the orientation of the free flyer and
  // its position.
236
  double c, co, s, so;
237
238
239
240
241
242
  c = cos(CurrentConfiguration(5));
  s = sin(CurrentConfiguration(5));

  co = cos(CurrentConfiguration(4));
  so = sin(CurrentConfiguration(4));

243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
  m_FinalDesiredCOMPose(0, 0) = c * co;
  m_FinalDesiredCOMPose(0, 1) = -s;
  m_FinalDesiredCOMPose(0, 2) = c * so;

  m_FinalDesiredCOMPose(1, 0) = s * co;
  m_FinalDesiredCOMPose(1, 1) = c;
  m_FinalDesiredCOMPose(1, 2) = s * so;

  m_FinalDesiredCOMPose(2, 0) = -so;
  m_FinalDesiredCOMPose(2, 1) = 0;
  m_FinalDesiredCOMPose(2, 2) = co;

  m_FinalDesiredCOMPose(0, 3) = refandfinalCOMState.x[0];
  m_FinalDesiredCOMPose(1, 3) = refandfinalCOMState.y[0];
  m_FinalDesiredCOMPose(2, 3) = refandfinalCOMState.z[0];
  m_FinalDesiredCOMPose(3, 3) = 1.0;

  ODEBUG4SIMPLE(
      CurrentConfiguration[6]
          << " " << CurrentConfiguration[7] << " " << CurrentConfiguration[8]
          << " " << CurrentConfiguration[9] << " " << CurrentConfiguration[10]
          << " " << CurrentConfiguration[11] << " " << CurrentConfiguration[12]
          << " " << CurrentConfiguration[13] << " " << CurrentConfiguration[14]
          << " " << CurrentConfiguration[15] << " " << CurrentConfiguration[16]
          << " " << CurrentConfiguration[17] << " " << CurrentConfiguration[18]
          << " ",
      "DebugDataqrql.txt");
270
271
272
273
274
  m_NumberOfIterations++;

  return 1;
}

275
COMState ZMPPreviewControlWithMultiBodyZMP::GetLastCOMFromFirstStage() {
276
277
278
279
280
  COMState aCOM;
  aCOM = m_FIFOCOMStates.back();
  return aCOM;
}

281
282
int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
    COMState &finalCOMState) {
283
284
285
286
287
288
289
290
  // Inverse Kinematics variables.

  COMState aCOMState = m_FIFOCOMStates[0];
  FootAbsolutePosition LeftFootPosition, RightFootPosition;

  LeftFootPosition = m_FIFOLeftFootPosition[0];
  RightFootPosition = m_FIFORightFootPosition[0];

Olivier Stasse's avatar
Olivier Stasse committed
291
  double Deltazmpx2, Deltazmpy2;
Guilhem Saurel's avatar
Guilhem Saurel committed
292

293
  // Preview control on delta ZMP.
294
295
296
  if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) ||
      (m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) {
    ODEBUG2(m_FIFODeltaZMPPositions[0].px << " "
Guilhem Saurel's avatar
Guilhem Saurel committed
297
                                          << m_FIFODeltaZMPPositions[0].py);
298

299
    ODEBUG("Second Stage Size of FIFODeltaZMPPositions: "
Guilhem Saurel's avatar
Guilhem Saurel committed
300
301
302
           << m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " "
           << m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " "
           << Deltazmpx2 << " " << Deltazmpy2);
303
304
305
306
307
308
309
310
311
312

    m_PC->OneIterationOfPreview(m_Deltax, m_Deltay, m_sxDeltazmp, m_syDeltazmp,
                                m_FIFODeltaZMPPositions, 0, Deltazmpx2,
                                Deltazmpy2, true);

    // Correct COM position
    // but be carefull this is the COM for NL steps behind.
    for (int i = 0; i < 3; i++) {
      aCOMState.x[i] += m_Deltax(i, 0);
      aCOMState.y[i] += m_Deltay(i, 0);
313
    }
314
  }
315

316
317
  ODEBUG2("Delta :" << m_Deltax(0, 0) << " " << m_Deltay(0, 0) << " "
                    << aCOMState.x[0] << " " << aCOMState.y[0]);
318
319
320
  // Update finalCOMState
  finalCOMState = aCOMState;

321
322
  if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) ||
      (m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) {
323

324
325
    m_FIFODeltaZMPPositions.pop_front();
  }
326
327
328
329
330
331
332
333
  m_FIFOCOMStates.pop_front();
  m_FIFOLeftFootPosition.pop_front();
  m_FIFORightFootPosition.pop_front();

  ODEBUG2("End");
  return 1;
}

334
335
336
int ZMPPreviewControlWithMultiBodyZMP::FirstStageOfControl(
    FootAbsolutePosition &LeftFootPosition,
    FootAbsolutePosition &RightFootPosition, COMState &afCOMState)
337
338
339
340
341
342
343

{

  double zmpx2, zmpy2;
  COMState acomp;
  acomp.yaw[0] = 0.0;
  acomp.pitch[0] = 0.0;
344
345
  if ((m_StageStrategy == ZMPCOM_TRAJECTORY_FULL) ||
      (m_StageStrategy == ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY)) {
346

347
348
349
350
    m_PC->OneIterationOfPreview(m_PC1x, m_PC1y, m_sxzmp, m_syzmp,
                                m_FIFOZMPRefPositions, 0, zmpx2, zmpy2, true);
    for (unsigned j = 0; j < 3; j++)
      acomp.x[j] = m_PC1x(j, 0);
351

352
353
    for (unsigned j = 0; j < 3; j++)
      acomp.y[j] = m_PC1y(j, 0);
354

355
356
    for (unsigned j = 0; j < 3; j++)
      acomp.z[j] = afCOMState.z[j];
357

358
359
    for (unsigned j = 0; j < 3; j++)
      acomp.yaw[j] = afCOMState.yaw[j];
360

361
362
    for (unsigned j = 0; j < 3; j++)
      acomp.pitch[j] = afCOMState.pitch[j];
363

364
365
    for (unsigned j = 0; j < 3; j++)
      acomp.roll[j] = afCOMState.roll[j];
366

367
368
369
  } else if (m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) {
    for (unsigned j = 0; j < 3; j++)
      acomp.x[j] = m_PC1x(j, 0) = afCOMState.x[j];
370

371
372
    for (unsigned j = 0; j < 3; j++)
      acomp.y[j] = m_PC1y(j, 0) = afCOMState.y[j];
373

374
375
    for (unsigned j = 0; j < 3; j++)
      acomp.z[j] = afCOMState.z[j];
376

377
378
    for (unsigned j = 0; j < 3; j++)
      acomp.yaw[j] = afCOMState.yaw[j];
379

380
381
382
    for (unsigned j = 0; j < 3; j++)
      acomp.pitch[j] = afCOMState.pitch[j];
  }
383
384
385
386
387
388

  // Update of the FIFOs
  m_FIFOCOMStates.push_back(acomp);
  m_FIFORightFootPosition.push_back(RightFootPosition);
  m_FIFOLeftFootPosition.push_back(LeftFootPosition);

389
390
391
  ODEBUG("FIFOs COM:" << m_FIFOCOMStates.size()
                      << " RF: " << m_FIFORightFootPosition.size()
                      << " LF: " << m_FIFOLeftFootPosition.size());
392
393
394
395
  m_FIFOZMPRefPositions.pop_front();
  return 1;
}

396
397
int ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP(
    int /* StartingIteration */) {
398
399
400
401
402
403
404
  ODEBUG("Start EvaluateMultiBodyZMP");
  m_PinocchioRobot->computeInverseDynamics();

  // Call the Humanoid Dynamic Multi Body robot model to
  // compute the ZMP related to the motion found by CoMAndZMPRealization.
  Eigen::Vector3d ZMPmultibody;
  m_PinocchioRobot->zeroMomentumPoint(ZMPmultibody);
405
  ODEBUG5(ZMPmultibody[0] << " " << ZMPmultibody[1] << " "
406
407
                          << m_FIFOZMPRefPositions[0].px << " "
                          << m_FIFOZMPRefPositions[0].py,
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
          "DebugDataCheckZMP1.txt");

  Eigen::Vector3d CoMmultibody;
  m_PinocchioRobot->positionCenterOfMass(CoMmultibody);
  ODEBUG("Stage 2");
  // Fill the delta ZMP FIFO for the second stage of the control.
  ZMPPosition aZMPpos;
  aZMPpos.px = m_FIFOZMPRefPositions[0].px - ZMPmultibody[0];
  aZMPpos.py = m_FIFOZMPRefPositions[0].py - ZMPmultibody[1];
  aZMPpos.pz = 0.0;
  aZMPpos.theta = 0.0;
  aZMPpos.stepType = 1;
  aZMPpos.time = m_FIFOZMPRefPositions[0].time;
  ODEBUG("Stage 3");
  Eigen::VectorXd CurrentConfiguration;
  /* Get the current configuration vector */
  CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();

  ODEBUG("Stage 4");
  m_FIFODeltaZMPPositions.push_back(aZMPpos);
  m_StartingNewSequence = false;
  ODEBUG("Final");
  return 1;
}

433
434
435
436
int ZMPPreviewControlWithMultiBodyZMP::Setup(
    deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
    deque<FootAbsolutePosition> &LeftFootPositions,
    deque<FootAbsolutePosition> &RightFootPositions) {
437
438
  m_NumberOfIterations = 0;
  Eigen::VectorXd CurrentConfiguration =
439
440
      m_PinocchioRobot->currentRPYConfiguration();
  Eigen::VectorXd CurrentVelocity = m_PinocchioRobot->currentRPYVelocity();
441
  Eigen::VectorXd CurrentAcceleration =
442
      m_PinocchioRobot->currentRPYAcceleration();
443
444
445

  m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);

446
  SetupFirstPhase(ZMPRefPositions, COMStates, LeftFootPositions,
447
                  RightFootPositions);
448
449
450
451
452
  for (unsigned int i = 0; i < m_NL; i++)
    SetupIterativePhase(ZMPRefPositions, COMStates, LeftFootPositions,
                        RightFootPositions, CurrentConfiguration,
                        CurrentVelocity, CurrentAcceleration, i);
  ODEBUG4("<========================================>", "ZMPPCWMZOGSOC.dat");
453
454
455
  return 0;
}

456
int ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase(
Olivier Stasse's avatar
Olivier Stasse committed
457
    deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &,
458
459
460
    deque<FootAbsolutePosition> &LeftFootPositions,
    deque<FootAbsolutePosition> &RightFootPositions) {
  ODEBUG6("Beginning of Setup 0 ", "DebugData.txt");
461
  ODEBUG("Setup");
462
  // double zmpx2, zmpy2;
463

464
465
  m_sxzmp = 0.0;
  m_syzmp = 0.0;
466
467
468
469
470
471
472
473
474
  m_sxDeltazmp = 0.0;
  m_syDeltazmp = 0.0;

  m_StartingNewSequence = true;

  // Fill the Fifo
  m_FIFOZMPRefPositions.resize(m_NL);
  m_FIFOLeftFootPosition.resize(m_NL);
  m_FIFORightFootPosition.resize(m_NL);
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
  for (unsigned int i = 0; i < m_NL; i++) {
    m_FIFOZMPRefPositions[i] = ZMPRefPositions[i];
    m_FIFOLeftFootPosition[i] = LeftFootPositions[i];
    m_FIFORightFootPosition[i] = RightFootPositions[i];
  }
  ODEBUG6("After EvaluateCOM", "DebugData.txt");

  ODEBUG6("Beginning of Setup 1 ", "DebugData.txt");
  m_PC1x(0, 0) = m_StartingCOMState[0];
  ODEBUG("COMPC1 init X: " << m_PC1x(0, 0));
  // m_PC1x(0,0) = 0.0;
  m_PC1x(1, 0) = 0.0;
  m_PC1x(2, 0) = 0.0;

  m_PC1y(0, 0) = m_StartingCOMState[1];
  ODEBUG("COMPC1 init Y: " << m_PC1y(0, 0));
  // m_PC1y(0,0) = 0.0;
  m_PC1y(1, 0) = 0;
  m_PC1y(2, 0) = 0;

  m_Deltax(0, 0) = 0.0; //-StartingCOMState[0];
  m_Deltax(1, 0) = 0;
  m_Deltax(2, 0) = 0;
  m_Deltay(0, 0) = 0.0; //-StartingCOMState[1];
  m_Deltay(1, 0) = 0;
  m_Deltay(2, 0) = 0;
501
502
503

  //  m_sxzmp=-StartingCOMState[0];m_syzmp=-StartingCOMState[1];
  //  zmpx2=StartingCOMState[0];zmpy2=StartingCOMState[1];
504
  m_sxzmp = 0.0;
505
506
  m_syzmp = 0.0;
  // zmpx2 = 0.0; zmpy2 = 0.0;
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530

  m_FIFODeltaZMPPositions.clear();
  m_FIFOCOMStates.clear();

  Eigen::VectorXd CurrentConfiguration;
  Eigen::VectorXd CurrentVelocity;
  Eigen::VectorXd CurrentAcceleration;
  /* Get the current configuration vector */
  CurrentConfiguration = m_PinocchioRobot->currentRPYConfiguration();
  CurrentVelocity = m_PinocchioRobot->currentRPYVelocity();
  CurrentAcceleration = m_PinocchioRobot->currentRPYAcceleration();
  CurrentVelocity.setZero();
  CurrentAcceleration.setZero();

  m_PinocchioRobot->currentRPYVelocity(CurrentVelocity);
  m_PinocchioRobot->currentRPYAcceleration(CurrentAcceleration);

#ifdef _DEBUG_MODE_ON_
  m_FIFOTmpZMPPosition.clear();
#endif

  return 0;
}

531
532
533
534
535
536
int ZMPPreviewControlWithMultiBodyZMP::SetupIterativePhase(
    deque<ZMPPosition> &ZMPRefPositions, deque<COMState> &COMStates,
    deque<FootAbsolutePosition> &LeftFootPositions,
    deque<FootAbsolutePosition> &RightFootPositions,
    Eigen::VectorXd &CurrentConfiguration, Eigen::VectorXd &CurrentVelocity,
    Eigen::VectorXd &CurrentAcceleration, int localindex) {
537

538
  ODEBUG("SetupIterativePhase " << localindex << " " << CurrentConfiguration);
539
  ODEBUG("m_FIFOZMPRefPositions.size():" << m_FIFOZMPRefPositions.size());
540
541
542
543
  ODEBUG("COMState[" << localindex << "]=" << COMStates[localindex].x[0] << " "
                     << COMStates[localindex].y[0] << " "
                     << COMStates[localindex].z[0]
                     << " COMStates.size()=" << COMStates.size());
544
  FirstStageOfControl(LeftFootPositions[localindex],
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
                      RightFootPositions[localindex], COMStates[localindex]);
  ODEBUG("m_FIFOCOMStates["
         << localindex << "]=" << m_FIFOCOMStates[localindex].x[0] << " "
         << m_FIFOCOMStates[localindex].y[0] << " "
         << m_FIFOCOMStates[localindex].z[0]
         << " m_FIFOCOMStates.size()=" << m_FIFOCOMStates.size());
  // COMState acompos = m_FIFOCOMStates[localindex];
  // FootAbsolutePosition aLeftFAP = m_FIFOLeftFootPosition[localindex];
  // FootAbsolutePosition aRightFAP = m_FIFORightFootPosition[localindex];

  ODEBUG4SIMPLE(m_FIFOZMPRefPositions[0].px
                    << " " << m_FIFOZMPRefPositions[0].py << " "
                    << m_FIFOZMPRefPositions[0].pz << " " << acompos.x[0] << " "
                    << acompos.y[0] << " " << acompos.z[0] << " " << aLeftFAP.x
                    << " " << aLeftFAP.y << " " << aLeftFAP.z << " "
                    << aRightFAP.x << " " << aRightFAP.y << " " << aRightFAP.z,
561
562
                "ZMPPCWMZOGSOC.dat");

Guilhem Saurel's avatar
Guilhem Saurel committed
563
  int StageOfTheAlgorithm = 0;
564
  CallToComAndFootRealization(
565
      m_FIFOCOMStates[localindex], m_FIFOLeftFootPosition[localindex],
Guilhem Saurel's avatar
Guilhem Saurel committed
566
567
      m_FIFORightFootPosition[localindex], CurrentConfiguration,
      CurrentVelocity, CurrentAcceleration, m_NumberOfIterations,
568
      StageOfTheAlgorithm);
569
570
571

  EvaluateMultiBodyZMP(localindex);

572
  m_FIFOZMPRefPositions.push_back(ZMPRefPositions[localindex + 1 + m_NL]);
573
574
575
576

  m_NumberOfIterations++;
  return 0;
}
577
578
579
void ZMPPreviewControlWithMultiBodyZMP::CreateExtraCOMBuffer(
    deque<COMState> &m_ExtraCOMBuffer, deque<ZMPPosition> &m_ExtraZMPBuffer,
    deque<ZMPPosition> &m_ExtraZMPRefBuffer)
580
581
582
583
584
585
586
587

{
  deque<ZMPPosition> aFIFOZMPRefPositions;
  Eigen::MatrixXd aPC1x;
  Eigen::MatrixXd aPC1y;
  double aSxzmp, aSyzmp;
  double aZmpx2, aZmpy2;

588
589
  // initialize ZMP FIFO
  for (unsigned int i = 0; i < m_NL; i++)
590
591
    aFIFOZMPRefPositions.push_back(m_ExtraZMPRefBuffer[i]);

592
  // use accumulated zmp error  of preview control so far
593
594
595
596
597
598
  aSxzmp = m_sxzmp;
  aSyzmp = m_syzmp;

  aPC1x = m_PC1x;
  aPC1y = m_PC1y;

599
  // create the extra COMbuffer
600
601
602

#ifdef _DEBUG_MODE_ON_
  ofstream aof_ExtraCOM;
603
604
605
606
607
608
  static unsigned char FirstCall = 1;
  if (FirstCall) {
    aof_ExtraCOM.open("CartExtraCOM_1.dat", ofstream::out);
  } else {
    aof_ExtraCOM.open("CartExtraCOM_1.dat", ofstream::app);
  }
609
610
611
612
613

  if (FirstCall)
    FirstCall = 0;
#endif

614
615
616
617
  for (unsigned int i = 0; i < m_ExtraCOMBuffer.size(); i++) {
    aFIFOZMPRefPositions.push_back(m_ExtraZMPRefBuffer[i]);
    m_PC->OneIterationOfPreview(aPC1x, aPC1y, aSxzmp, aSyzmp,
                                aFIFOZMPRefPositions, 0, aZmpx2, aZmpy2, true);
618

619
620
621
622
    for (unsigned j = 0; j < 3; j++) {
      m_ExtraCOMBuffer[i].x[j] = aPC1x(j, 0);
      m_ExtraCOMBuffer[i].y[j] = aPC1y(j, 0);
    }
623

624
625
    m_ExtraZMPBuffer[i].px = aZmpx2;
    m_ExtraZMPBuffer[i].py = aZmpy2;
626

627
    m_ExtraCOMBuffer[i].yaw[0] = m_ExtraZMPRefBuffer[i].theta;
628

629
    aFIFOZMPRefPositions.pop_front();
630
631

#ifdef _DEBUG_MODE_ON_
632
633
634
635
    if (aof_ExtraCOM.is_open()) {
      aof_ExtraCOM << m_ExtraZMPRefBuffer[i].time << " "
                   << m_ExtraZMPRefBuffer[i].px << " " << aPC1x(0, 0) << " "
                   << aPC1y(0, 0) << endl;
636
    }
637
638
639
#endif
  }
  ODEBUG("ik ben hier b" << aFIFOZMPRefPositions.size());
640
  ODEBUG("ik ben hier c" << m_ExtraZMPRefBuffer.size());
641

642
#ifdef _DEBUG_MODE_ON_
643
644
645
  if (aof_ExtraCOM.is_open()) {
    aof_ExtraCOM.close();
  }
646
#endif
647
}
648

649
650
void ZMPPreviewControlWithMultiBodyZMP::UpdateTheZMPRefQueue(
    ZMPPosition NewZMPRefPos) {
651
652
  m_FIFOZMPRefPositions.push_back(NewZMPRefPos);
}
653

654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
void ZMPPreviewControlWithMultiBodyZMP::SetStrategyForStageActivation(
    int aZMPComTraj) {
  switch (aZMPComTraj) {
  case ZMPCOM_TRAJECTORY_FULL:
    m_StageStrategy = ZMPCOM_TRAJECTORY_FULL;
    break;
  case ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY:
    m_StageStrategy = ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY;
    break;
  case ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY:
    m_StageStrategy = ZMPCOM_TRAJECTORY_FIRST_STAGE_ONLY;
    break;

  default:
    break;
  }
670
671
}

672
int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForStageActivation() {
673
674
675
676
  return m_StageStrategy;
}

// TODO : Compute the position of the waist inside the COM Frame.
677
678
Eigen::Matrix4d
ZMPPreviewControlWithMultiBodyZMP::GetCurrentPositionofWaistInCOMFrame() {
679
680
  Eigen::Matrix4d PosOfWaistInCoMFrame;
  PosOfWaistInCoMFrame =
681
      m_ComAndFootRealization->GetCurrentPositionofWaistInCOMFrame();
682

Olivier Stasse's avatar
Olivier Stasse committed
683
684
685
  //  cerr << " Should implement:
  // ZMPPreviewControlWithMultiBodyZMP::
  // GetCurrentPositionOfWaistInCOMFrame()" << endl;
686
687
688
  return PosOfWaistInCoMFrame;
}

689
Eigen::Matrix4d ZMPPreviewControlWithMultiBodyZMP::GetFinalDesiredCOMPose() {
690
691
692
  return m_FinalDesiredCOMPose;
}

693
694
695
696
697
698
699
700
701
702
int ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingState(
    Eigen::VectorXd &BodyAnglesInit, Eigen::Vector3d &aStartingCOMState,
    Eigen::Vector3d &aStartingZMPPosition,
    Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
    FootAbsolutePosition &InitLeftFootPosition,
    FootAbsolutePosition &InitRightFootPosition) {
  int r = EvaluateStartingCoM(BodyAnglesInit, aStartingCOMState,
                              aStartingWaistPosition, InitLeftFootPosition,
                              InitRightFootPosition);
  aStartingZMPPosition = m_ComAndFootRealization->GetCOGInitialAnkles();
703
704
  return r;
}
705
706
707
708
709
int ZMPPreviewControlWithMultiBodyZMP::EvaluateStartingCoM(
    Eigen::VectorXd &BodyAnglesInit, Eigen::Vector3d &aStartingCOMState,
    Eigen::Matrix<double, 6, 1> &aStartingWaistPosition,
    FootAbsolutePosition &InitLeftFootPosition,
    FootAbsolutePosition &InitRightFootPosition) {
710
711
  ODEBUG("EvaluateStartingCOM: BodyAnglesInit :" << BodyAnglesInit);

712
713
714
  m_ComAndFootRealization->InitializationCoM(
      BodyAnglesInit, m_StartingCOMState, aStartingWaistPosition,
      InitLeftFootPosition, InitRightFootPosition);
715
716
717
718
719
720
721
722
723

  ODEBUG("EvaluateStartingCOM: m_StartingCOMState: " << m_StartingCOMState);
  aStartingCOMState[0] = m_StartingCOMState[0];
  aStartingCOMState[1] = m_StartingCOMState[1];
  aStartingCOMState[2] = m_StartingCOMState[2];

  return 0;
}

724
void ZMPPreviewControlWithMultiBodyZMP::SetStrategyForPCStages(int Strategy) {
725
726
727
  m_StageStrategy = Strategy;
}

728
int ZMPPreviewControlWithMultiBodyZMP::GetStrategyForPCStages() {
729
730
731
  return m_StageStrategy;
}

732
733
734
void ZMPPreviewControlWithMultiBodyZMP::RegisterMethods() {
  std::string aMethodName[3] = {":samplingperiod", ":previewcontroltime",
                                ":comheight"};
735

736
737
738
739
740
  for (int i = 0; i < 3; i++) {
    if (!RegisterMethod(aMethodName[i])) {
      std::cerr << "Unable to register " << aMethodName << std::endl;
    } else {
      ODEBUG("Succeed in registering " << aMethodName[i]);
741
    }
742
  }
743
744
}

745
746
void ZMPPreviewControlWithMultiBodyZMP::SetSamplingPeriod(
    double lSamplingPeriod) {
747
748
  m_SamplingPeriod = lSamplingPeriod;

749
750
751
  m_NL = 0;
  if (m_SamplingPeriod != 0.0)
    m_NL = (unsigned int)(m_PreviewControlTime / m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
752
753
}

754
755
void ZMPPreviewControlWithMultiBodyZMP::SetPreviewControlTime(
    double lPreviewControlTime) {
Thomas Moulard's avatar
Thomas Moulard committed
756
757
  m_PreviewControlTime = lPreviewControlTime;

758
759
760
  m_NL = 0;
  if (m_SamplingPeriod != 0.0)
    m_NL = (unsigned int)(m_PreviewControlTime / m_SamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
761
762
}

763
764
765
766
767
768
769
770
void ZMPPreviewControlWithMultiBodyZMP::CallMethod(std::string &Method,
                                                   std::istringstream &strm) {
  if (Method == ":samplingperiod") {
    std::string aws;
    if (strm.good()) {
      double lSamplingPeriod;
      strm >> lSamplingPeriod;
      SetSamplingPeriod(lSamplingPeriod);
Thomas Moulard's avatar
Thomas Moulard committed
771
    }
772
773
774
775
776
777
  } else if (Method == ":previewcontroltime") {
    std::string aws;
    if (strm.good()) {
      double lpreviewcontroltime;
      strm >> lpreviewcontroltime;
      SetPreviewControlTime(lpreviewcontroltime);
Thomas Moulard's avatar
Thomas Moulard committed
778
    }
779
  }
Thomas Moulard's avatar
Thomas Moulard committed
780
}