TestNaveau2015.cpp 24.7 KB
Newer Older
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
/*
 * Copyright 2009, 2010, 2014
 *
 * Maximilien Naveau
 * 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/>.
 *
 *  Research carried out within the scope of the
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file TestNaveau2015.cpp
  \brief This Example shows you how to use the nmpc_genereator.cpp */
27
#include <cmath>
28
#include <fstream>
29
#include <iomanip>
30
#include <iostream>
31
#include <map>
32
33
#include <stdlib.h>
#include <string>
34

35
#include "CommonTools.hh"
36
#include "Debug.hh"
37
#include "MotionGeneration/ComAndFootRealizationByGeometry.hh"
38
39
#include "TestObject.hh"
#include <jrl/walkgen/pgtypes.hh>
40
41

using namespace std;
42
using namespace PatternGeneratorJRL;
43
using namespace ::PatternGeneratorJRL::TestSuite;
44
45

enum Profiles_t {
46
47
  PROFIL_NAVEAU,       // 0
  PROFIL_SIMPLE_NAVEAU // 1
48
49
};

50
typedef void (TestObject::*localeventHandler_t)(PatternGeneratorInterface &);
51

52
struct localEvent {
53
  unsigned time;
54
  localeventHandler_t Handler;
55
56
};

57
struct setOfLocalEvents {
58
59
  std::vector<localEvent> m_vecOfLocalEvents;
  PatternGeneratorInterface *m_PGI;
Olivier Stasse's avatar
Olivier Stasse committed
60

61
  setOfLocalEvents(PatternGeneratorInterface *aPGI) { m_PGI = aPGI; }
Olivier Stasse's avatar
Olivier Stasse committed
62

63
64
  void initVecOfLocalEvents(struct localEvent *events,
                            unsigned int sizeOfEvents) {
65
66
67
    for (unsigned int i = 0; i < sizeOfEvents; i++) {
      m_vecOfLocalEvents.push_back(events[i]);
    }
68
  }
Olivier Stasse's avatar
Olivier Stasse committed
69

70
  void evaluateEvents(struct OneStep &oneStep, TestObject &aTestObject) {
71
    // Test when triggering event.
72
73
74
75
    for (unsigned int i = 0; i < m_vecOfLocalEvents.size(); i++) {
      if (oneStep.m_NbOfIt == m_vecOfLocalEvents[i].time) {
        ODEBUG3("********* GENERATE EVENT EMS ***********");
        (aTestObject.*(m_vecOfLocalEvents[i].Handler))(*m_PGI);
76
      }
77
    }
78
  }
79
80
};

81
// Class TestNaveau2015
82
class TestNaveau2015 : public TestObject {
83
private:
84
85
  int resetfiles;
  bool m_DebugFGPIFull;
86
  vector<double> m_err_zmp_x, m_err_zmp_y;
87

88
  int iteration;
89
90
91
92
93
94
  std::vector<pinocchio::JointIndex> m_leftLeg;
  std::vector<pinocchio::JointIndex> m_rightLeg;
  std::vector<pinocchio::JointIndex> m_leftArm;
  std::vector<pinocchio::JointIndex> m_rightArm;
  pinocchio::JointIndex m_leftGripper;
  pinocchio::JointIndex m_rightGripper;
95

96
  /// Object to generate events according to profile.
97
  setOfLocalEvents *m_setOfLocalEvents;
Olivier Stasse's avatar
Olivier Stasse committed
98

99
100
101
public:
  TestNaveau2015(int argc, char *argv[], string &aString, int TestProfile)
      : TestObject(argc, argv, aString) {
102
    m_TestProfile = TestProfile;
103
104
105
    resetfiles = 0;
    m_DebugFGPIFull = true;
    m_DebugFGPI = true;
106
107
    m_err_zmp_x.clear();
    m_err_zmp_y.clear();
108
109
    iteration = 0;
    m_leftLeg.clear();
110
    m_rightLeg.clear();
111
    m_leftArm.clear();
112
    m_rightArm.clear();
113
114
    m_leftGripper = 0;
    m_rightGripper = 0;
115
    m_setOfLocalEvents = 0;
116
117
  }

118
  bool doTest(std::ostream &os) {
119
    bool ret = TestObject::doTest(os);
120
121
    if (m_DebugFGPIFull)
      ComputeAndDisplayZMPStatistic();
122
    return ret;
123
  }
124

125
  bool init() {
126
127
    if (!TestObject::init())
      return false;
128
129
130
131
132
133

    /// Initialize Configuration Velocity and Acceleration
    m_CurrentVelocity.setZero();
    m_CurrentAcceleration.setZero();
    m_setOfLocalEvents = new setOfLocalEvents(m_PGI);

134
    if (m_PR->getName() == "hrp2_14_reduced")
135
      m_ComAndFootRealization->SetHeightOfTheCoM(0.814);
136
    else if (m_PR->getName() == "talos")
137
      m_ComAndFootRealization->SetHeightOfTheCoM(0.876681);
Olivier Stasse's avatar
Olivier Stasse committed
138

139
140
141
142
    m_leftLeg =
        m_PR->jointsBetween(m_PR->waist(), m_PR->leftFoot()->associatedAnkle);
    m_rightLeg =
        m_PR->jointsBetween(m_PR->waist(), m_PR->rightFoot()->associatedAnkle);
143
144
145
146
147
148
149
    m_leftArm = m_PR->jointsBetween(m_PR->chest(), m_PR->leftWrist());
    m_rightArm = m_PR->jointsBetween(m_PR->chest(), m_PR->rightWrist());

    m_leftLeg.erase(m_leftLeg.begin());
    m_rightLeg.erase(m_rightLeg.begin());

    pinocchio::JointModelVector &ActuatedJoints = m_PR->getActuatedJoints();
150
151
    for (unsigned i = 0; i < m_leftLeg.size(); ++i)
      m_leftLeg[i] = pinocchio::idx_q(ActuatedJoints[m_leftLeg[i]]) - 1;
152
153
    for (unsigned i = 0; i < m_rightLeg.size(); ++i)
      m_rightLeg[i] = pinocchio::idx_q(ActuatedJoints[m_rightLeg[i]]) - 1;
154
155
    for (unsigned i = 0; i < m_leftArm.size(); ++i)
      m_leftArm[i] = pinocchio::idx_q(ActuatedJoints[m_leftArm[i]]) - 1;
156
157
158
159
160
161
    for (unsigned i = 0; i < m_rightArm.size(); ++i)
      m_rightArm[i] = pinocchio::idx_q(ActuatedJoints[m_rightArm[i]]) - 1;

    if ((m_robotModel.parents.size() >= m_rightArm.back() + 1) &&
        m_robotModel.parents[m_rightArm.back() + 1] == m_rightArm.back())
      m_rightGripper = m_rightArm.back() + 1;
162
    else
163
      m_rightGripper = 0;
164

165
166
167
    if ((m_robotModel.parents.size() >= m_leftArm.back() + 1) &&
        m_robotModel.parents[m_leftArm.back() + 1] == m_leftArm.back())
      m_leftGripper = m_leftArm.back() + 1;
168
    else
169
      m_leftGripper = 0;
170

171
    return true;
172
173
  }

174
protected:
175
176
177
178
179
180
  void ComputeStat(vector<double> vec, double &avg, double &max_abs) {
    double total = 0.0;
    avg = 0.0;
    max_abs = 0.0;
    for (unsigned int i = 0; i < vec.size(); ++i) {
      double abs_value = sqrt(vec[i] * vec[i]);
181
182
      if (abs_value > max_abs)
        max_abs = abs_value;
183
184
185
186
187

      total += abs_value;
    }
    avg = total / (double)(vec.size());
    return;
188
189
  }

190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
  void ComputeAndDisplayZMPStatistic() {
    cout << "Statistic for Dzmp in x : " << endl;
    double moy_delta_zmp_x = 0.0;
    double max_abs_err_x = 0.0;
    ComputeStat(m_err_zmp_x, moy_delta_zmp_x, max_abs_err_x);
    cout << "average : " << moy_delta_zmp_x << endl;
    cout << "maxx error : " << max_abs_err_x << endl;

    cout << "Statistic for Dzmp in y : " << endl;
    double moy_delta_zmp_y = 0.0;
    double max_abs_err_y = 0.0;
    ComputeStat(m_err_zmp_y, moy_delta_zmp_y, max_abs_err_y);
    cout << "average : " << moy_delta_zmp_y << endl;
    cout << "maxx error : " << max_abs_err_y << endl;
    return;
205
206
  }

207
208
  void analyticalInverseKinematics(Eigen::VectorXd &conf, Eigen::VectorXd &vel,
                                   Eigen::VectorXd &acc) {
209
210
211
    /// \brief calculate, from the CoM of computed by the preview control,
    ///    the corresponding articular position, velocity and acceleration
    /// ------------------------------------------------------------------
Olivier Stasse's avatar
Olivier Stasse committed
212
213
214
215
216
    Eigen::VectorXd aCOMState(6);
    Eigen::VectorXd aCOMSpeed(6);
    Eigen::VectorXd aCOMAcc(6);
    Eigen::VectorXd aLeftFootPosition(5);
    Eigen::VectorXd aRightFootPosition(5);
217

218
219
220
    aCOMState(0) = m_OneStep.m_finalCOMPosition.x[0];
    aCOMState(1) = m_OneStep.m_finalCOMPosition.y[0];
    aCOMState(2) = m_OneStep.m_finalCOMPosition.z[0];
221
222
223
    aCOMState(3) = m_OneStep.m_finalCOMPosition.roll[0] * 180 / M_PI;
    aCOMState(4) = m_OneStep.m_finalCOMPosition.pitch[0] * 180 / M_PI;
    aCOMState(5) = m_OneStep.m_finalCOMPosition.yaw[0] * 180 / M_PI;
224
225
226
227

    aCOMSpeed(0) = m_OneStep.m_finalCOMPosition.x[1];
    aCOMSpeed(1) = m_OneStep.m_finalCOMPosition.y[1];
    aCOMSpeed(2) = m_OneStep.m_finalCOMPosition.z[1];
228
229
230
    aCOMSpeed(3) = m_OneStep.m_finalCOMPosition.roll[1] /** * 180/M_PI  */;
    aCOMSpeed(4) = m_OneStep.m_finalCOMPosition.pitch[1] /** * 180/M_PI  */;
    aCOMSpeed(5) = m_OneStep.m_finalCOMPosition.yaw[1] /** * 180/M_PI  */;
231
232
233
234

    aCOMAcc(0) = m_OneStep.m_finalCOMPosition.x[2];
    aCOMAcc(1) = m_OneStep.m_finalCOMPosition.y[2];
    aCOMAcc(2) = m_OneStep.m_finalCOMPosition.z[2];
235
236
    aCOMAcc(3) = m_OneStep.m_finalCOMPosition.roll[2] /** * 180/M_PI  */;
    aCOMAcc(4) = m_OneStep.m_finalCOMPosition.pitch[2] /** * 180/M_PI  */;
237
238
239
240
241
242
243
244
245
246
247
248
249
    aCOMAcc(5) = m_OneStep.m_finalCOMPosition.yaw[2] /** * 180/M_PI  */;

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

    aRightFootPosition(0) = m_OneStep.m_RightFootPosition.x;
    aRightFootPosition(1) = m_OneStep.m_RightFootPosition.y;
    aRightFootPosition(2) = m_OneStep.m_RightFootPosition.z;
    aRightFootPosition(3) = m_OneStep.m_RightFootPosition.theta;
    aRightFootPosition(4) = m_OneStep.m_RightFootPosition.omega;
250
    m_ComAndFootRealization->setSamplingPeriod(0.005);
251
252
253
    m_ComAndFootRealization->ComputePostureForGivenCoMAndFeetPosture(
        aCOMState, aCOMSpeed, aCOMAcc, aLeftFootPosition, aRightFootPosition,
        conf, vel, acc, iteration, 1);
254
255
256
257
258

    if (m_leftGripper != 0 && m_rightGripper != 0) {
      conf(m_leftGripper) = 10.0 * M_PI / 180.0;
      conf(m_rightGripper) = 10.0 * M_PI / 180.0;
    }
259
  }
260

261
  Eigen::VectorXd parseFromURDFtoOpenHRPIndex() {
Olivier Stasse's avatar
Olivier Stasse committed
262
    Eigen::VectorXd conf = m_conf;
263
264
265
266
    {
      for (unsigned int i = 0; i < conf.size(); conf[i++] = 0.0)
        ;
    };
267

268
269
    for (unsigned int i = 0; i < 6; i++)
      conf(i) = m_conf(i);
270
271
    std::size_t index = 6;
    // RLEG
272
273
    for (unsigned int i = 0; i < m_rightLeg.size(); i++)
      conf(index + i) = m_conf(m_rightLeg[i]);
274
275
    index += (unsigned)m_rightLeg.size();
    // LLEG
276
277
    for (unsigned int i = 0; i < m_leftLeg.size(); i++)
      conf(index + i) = m_conf(m_leftLeg[i]);
278
279
    index += (unsigned)m_leftLeg.size();
    // CHEST
280
281
    for (unsigned int i = 0; i < 2; i++)
      conf(index + i) = 0.0;
282
283
    index += 2;
    // HEAD
284
285
    for (unsigned int i = 0; i < 2; i++)
      conf(index + i) = 0.0;
286
287
    index += 2;
    // RARM
288
289
    for (unsigned int i = 0; i < m_rightArm.size(); i++)
      conf(index + i) = m_HalfSitting(m_rightArm[i] - 6);
290
291
    index += (unsigned)m_rightArm.size();
    conf(index) = 10 * M_PI / 180;
292
    ++index;
293
    // LARM
294
295
    for (unsigned int i = 0; i < m_leftArm.size(); i++)
      conf(index + i) = m_HalfSitting(m_leftArm[i] - 6);
296
297
    index += m_leftArm.size();
    conf(index) = 10 * M_PI / 180;
298

299
    return conf;
300
301
  }

302
  void createOpenHRPFiles() {
303
304
    /// \brief Create file .hip .pos .zmp
    /// ---------------------------------
Olivier Stasse's avatar
Olivier Stasse committed
305
    Eigen::VectorXd conf = parseFromURDFtoOpenHRPIndex();
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
    ofstream aof;
    string aPosFileName = /*"/tmp/" +*/ m_TestName + ".pos";
    string aZMPFileName = /*"/tmp/" +*/ m_TestName + ".zmp";
    string aWaistFileName = /*"/tmp/" +*/ m_TestName + ".waist";
    string aHipFileName = /*"/tmp/" +*/ m_TestName + ".hip";
    if (iteration == 0) {
      aof.open(aPosFileName.c_str(), ofstream::out);
      aof.close();
      aof.open(aZMPFileName.c_str(), ofstream::out);
      aof.close();
      aof.open(aWaistFileName.c_str(), ofstream::out);
      aof.close();
      aof.open(aHipFileName.c_str(), ofstream::out);
      aof.close();
    }
321

322
    aof.open(aPosFileName.c_str(), ofstream::app);
323
324
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
325
326
327
328
329
330
    aof << filterprecision((double)iteration * 0.005) << " "; // 1
    for (unsigned i = 6; i < conf.size(); ++i)
      aof << filterprecision(conf(i)) << " "; // 2-30
    for (unsigned i = 0; i < 9; ++i)
      aof << filterprecision(0.0) << " "; // 31-40
    aof << 0.0 << endl;                   // 41
331
332
    aof.close();

333
    aof.open(aWaistFileName.c_str(), ofstream::app);
334
335
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
336
337
338
339
    aof << filterprecision((double)iteration * 0.005) << " ";             // 1
    aof << filterprecision(m_OneStep.m_finalCOMPosition.roll[0]) << " ";  // 2
    aof << filterprecision(m_OneStep.m_finalCOMPosition.pitch[0]) << " "; // 3
    aof << filterprecision(m_OneStep.m_finalCOMPosition.yaw[0]);          // 4
340
    aof << endl;
341
342
    aof.close();

343
    aof.open(aHipFileName.c_str(), ofstream::app);
344
345
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
346
347
348
349
    aof << filterprecision((double)iteration * 0.005) << " ";             // 1
    aof << filterprecision(m_OneStep.m_finalCOMPosition.roll[0]) << " ";  // 2
    aof << filterprecision(m_OneStep.m_finalCOMPosition.pitch[0]) << " "; // 3
    aof << filterprecision(m_OneStep.m_finalCOMPosition.yaw[0]);          // 4
350
    aof << endl;
351
352
353
    aof.close();

    FootAbsolutePosition aSupportState;
354
355
    if (m_OneStep.m_LeftFootPosition.stepType < 0)
      aSupportState = m_OneStep.m_LeftFootPosition;
356
    else
357
      aSupportState = m_OneStep.m_RightFootPosition;
358

359
    aof.open(aZMPFileName.c_str(), ofstream::app);
360
361
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
362
363
364
365
    aof << filterprecision((double)iteration * 0.005) << " ";          // 1
    aof << filterprecision(m_OneStep.m_ZMPTarget(0) - conf(0)) << " "; // 2
    aof << filterprecision(m_OneStep.m_ZMPTarget(1) - conf(1)) << " "; // 3
    aof << filterprecision(aSupportState.z - conf(2));                 // 4
366
    aof << endl;
367
    aof.close();
368
369
  }

370
371
372
373
374
375
376
377
378
379
380
  void prepareDebugFiles() {
    TestObject::prepareDebugFiles();
    if (m_DebugFGPIFull) {
      ofstream aof;
      string aFileName;

      aFileName = m_TestName;
      aFileName += "TestFGPIFull_description.dat";

      aof.open(aFileName.c_str(), ofstream::out);

381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
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
      string Titles[51] = {"Time",               // 1
                           "Com X",              // 2
                           "Com Y",              // 3
                           "Com Z",              // 4
                           "Com Yaw",            // 5
                           "Com dX",             // 6
                           "Com dY",             // 7
                           "Com dZ",             // 8
                           "Com dYaw",           // 9
                           "Com ddX",            // 10
                           "Com ddY",            // 11
                           "Com ddZ",            // 12
                           "Com ddYaw",          // 13
                           "ZMP X (world ref.)", // 14
                           "ZMP Y (world ref.)", // 15
                           "ZMP Z (world ref.)", // 16
                           "Left Foot X",        // 17
                           "Left Foot Y",        // 18
                           "Left Foot Z",        // 19
                           "Left Foot dX",       // 20
                           "Left Foot dY",       // 21
                           "Left Foot dZ",       // 22
                           "Left Foot ddX",      // 23
                           "Left Foot ddY",      // 24
                           "Left Foot ddZ",      // 25
                           "Left Foot Theta",    // 26
                           "Left Foot dTheta",   // 27
                           "Left Foot ddTheta",  // 28
                           "Left Foot Omega",    // 29
                           "Left Foot Omega2",   // 30
                           "Right Foot X",       // 31
                           "Right Foot Y",       // 32
                           "Right Foot Z",       // 33
                           "Right Foot dX",      // 34
                           "Right Foot dY",      // 35
                           "Right Foot dZ",      // 36
                           "Right Foot ddX",     // 37
                           "Right Foot ddY",     // 38
                           "Right Foot ddZ",     // 39
                           "Right Foot Theta",   // 40
                           "Right Foot dTheta",  // 41
                           "Right Foot ddTheta", // 42
                           "Right Foot Omega",   // 43
                           "Right Foot Omega2",  // 44
                           "ZMP MB X ",          // 45
                           "ZMP MB Y ",          // 46
                           "ZMP MB Z ",          // 47
                           "q ",                 // 48
                           "dq",                 // 48 + nq
                           "ddq "};              // 48 + nq + nv
      for (unsigned int i = 0; i < 51; i++)
        aof << i + 1 << ". " << Titles[i] << std::endl;
433
434
435
436
437
438
439

      aof.close();

      aFileName = m_TestName;
      aFileName += "TestFGPIFull.dat";
      if (m_OneStep.m_NbOfIt == 1) {
        aof.open(aFileName.c_str(), ofstream::out);
440
      }
441
    }
442
443
  }

444
  void fillInDebugFiles() {
445
    TestObject::fillInDebugFiles();
446

447
    Eigen::VectorXd &currentConfiguration = m_PR->currentRPYConfiguration();
448

449
    if (iteration == 0) {
450
451
      m_DumpReferencesObjects.setAnklePositions(
          m_PR->rightFoot()->anklePosition, m_PR->leftFoot()->anklePosition);
452
453
    }

454
455
    m_DumpReferencesObjects.fillInTests(m_TestName, m_OneStep,
                                        currentConfiguration);
456
    if (m_DebugFGPIFull) {
457
458
      analyticalInverseKinematics(currentConfiguration, m_CurrentVelocity,
                                  m_CurrentAcceleration);
459
460
461
462
463
464
      if (iteration == 0) {
        //        cout << endl ;
        //        cout << m_conf << endl ;
        //        cout << m_HalfSitting << endl ;
        //        cout << std::boolalpha << isHalfsitting << endl ;
        //        assert(isHalfsitting);
465
      }
466

467
468
      m_DebugPR->computeInverseDynamics(currentConfiguration, m_CurrentVelocity,
                                        m_CurrentAcceleration);
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
      Eigen::Vector3d com, dcom, ddcom;
      m_DebugPR->CenterOfMass(com, dcom, ddcom);
      createOpenHRPFiles();
      Eigen::Vector3d zmpmb;
      m_DebugPR->zeroMomentumPoint(zmpmb);
      m_err_zmp_x.push_back(zmpmb[0] - m_OneStep.m_ZMPTarget(0));
      m_err_zmp_y.push_back(zmpmb[1] - m_OneStep.m_ZMPTarget(1));

      ++iteration;

      ofstream aof;
      string aFileName;
      aFileName = m_TestName;
      aFileName += "TestFGPIFull.dat";
      if (m_OneStep.m_NbOfIt == 1) {
        aof.open(aFileName.c_str(), ofstream::out);
      }
      resetfiles = 1;
      aof.open(aFileName.c_str(), ofstream::app);
      aof.precision(8);
      aof.setf(ios::scientific, ios::floatfield);
      m_OneStep.fillInDebugFileContent(aof);
491
492
493
494
      aof << filterprecision(zmpmb[0]) << " "            // 45
          << filterprecision(zmpmb[1]) << " "            // 46
          << filterprecision(zmpmb[2]) << " ";           // 47
      for (unsigned int k = 0; k < m_conf.size(); k++) { // 48-53 -> 54-83
495
496
        aof << filterprecision(m_conf(k)) << " ";
      }
497
      for (unsigned int k = 0; k < m_vel.size(); k++) { // 84-89 -> 90-118
498
499
        aof << filterprecision(m_vel(k)) << " ";
      }
500
      for (unsigned int k = 0; k < m_acc.size(); k++) { // 119-125 -> 125-155
501
502
503
504
505
        aof << filterprecision(m_acc(k)) << " ";
      }
      aof << endl;
      aof.close();
    }
506
  }
507

508
  void createFullEventsForHRP2() {
509
    ODEBUG("createFullEventsForHRP2");
510
511
512
513
514
515
516
517
518
    localEvent events[8] = {{1 * 200, &TestObject::walkForwardSlow},
                            {2 * 200, &TestObject::startTurningRight2},
                            {10 * 200, &TestObject::walkForward2m_s},
                            {20 * 200, &TestObject::walkSidewards2m_s},
                            {30 * 200, &TestObject::walkX05Y04},
                            {50 * 200, &TestObject::walkOnSpot},
                            {66 * 200, &TestObject::stop},
                            {76 * 200, &TestObject::stopOnLineWalking}};

519
520
    if (m_setOfLocalEvents != 0)
      m_setOfLocalEvents->initVecOfLocalEvents(events, 8);
521
522
  }

523
524
525
526
527
  void createSimpleEventsForHRP2() {
    localEvent events[2] = {
        {1 * 200, &TestObject::walkOnSpot},
        {5 * 200, &TestObject::stop},
    };
Olivier Stasse's avatar
Olivier Stasse committed
528

529
530
    if (m_setOfLocalEvents != 0)
      m_setOfLocalEvents->initVecOfLocalEvents(events, 2);
531
532
  }

533
  void startHRP2OnLineWalking(PatternGeneratorInterface &aPGI) {
534
535
536
537
538
539
540
    CommonInitialization(aPGI);

    {
      istringstream strm2(":SetAlgoForZmpTrajectory Naveau");
      aPGI.ParseCmd(strm2);
    }
    {
541
      // istringstream strm2(":singlesupporttime 1.4");
542
      istringstream strm2(":singlesupporttime 0.7");
543
544
545
      aPGI.ParseCmd(strm2);
    }
    {
546
      // istringstream strm2(":doublesupporttime 0.2");
547
      istringstream strm2(":doublesupporttime 0.1");
548
549
550
551
552
553
554
555
556
557
558
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":NaveauOnline");
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":numberstepsbeforestop 2");
      aPGI.ParseCmd(strm2);
    }
    {
559
      istringstream strm2(":setfeetconstraint XY 0.095 0.055");
560
561
      m_PGI->ParseCmd(strm2);
    }
562
563
564
565
566
    {
      istringstream strm2(":stepheight 0.05");
      m_PGI->ParseCmd(strm2);
    }

567
568
569
570
571
    {
      istringstream strm2(":deleteallobstacles");
      m_PGI->ParseCmd(strm2);
    }

572
    {
573
      // istringstream strm2(":feedBackControl true");
574
      istringstream strm2(":feedBackControl false");
575
576
577
      m_PGI->ParseCmd(strm2);
    }

mnaveau's avatar
mnaveau committed
578
    {
579
      // istringstream strm2(":useDynamicFilter true");
580
      istringstream strm2(":useDynamicFilter false");
mnaveau's avatar
mnaveau committed
581
582
      m_PGI->ParseCmd(strm2);
    }
583
  }
584

585
  void startTalosOnLineWalking(PatternGeneratorInterface &aPGI) {
586
587
588
589
590
591
    CommonInitialization(aPGI);

    {
      istringstream strm2(":setDSFeetDistance 0.162");
      m_PGI->ParseCmd(strm2);
    }
Olivier Stasse's avatar
Olivier Stasse committed
592

593
594
595
596
597
    {
      istringstream strm2(":SetAlgoForZmpTrajectory Naveau");
      aPGI.ParseCmd(strm2);
    }
    {
598
      istringstream strm2(":singlesupporttime 1.0");
599
      // istringstream strm2(":singlesupporttime 0.7");
600
601
602
      aPGI.ParseCmd(strm2);
    }
    {
603
      istringstream strm2(":doublesupporttime 0.2");
604
      // istringstream strm2(":doublesupporttime 0.1");
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":NaveauOnline");
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":numberstepsbeforestop 2");
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":setfeetconstraint XY 0.091 0.0489");
      m_PGI->ParseCmd(strm2);
    }
    {
      istringstream strm2(":stepheight 0.05");
mnaveau's avatar
mnaveau committed
621
622
      m_PGI->ParseCmd(strm2);
    }
623

624
625
626
627
    {
      istringstream strm2(":deleteallobstacles");
      m_PGI->ParseCmd(strm2);
    }
628

629
    {
630
      // istringstream strm2(":feedBackControl true");
631
632
633
      istringstream strm2(":feedBackControl false");
      m_PGI->ParseCmd(strm2);
    }
634

635
    {
636
      istringstream strm2(":useDynamicFilter true");
637
      // istringstream strm2(":useDynamicFilter false");
638
639
      m_PGI->ParseCmd(strm2);
    }
640
  }
641

642
643
644
  void chooseTestProfile() {
    ODEBUG("ROBOT:" << m_PR->getName() << " Profile: " << m_TestProfile);
    switch (m_TestProfile) {
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
      case PROFIL_NAVEAU:
        createFullEventsForHRP2();
        if (m_PR->getName() == "hrp2_14_reduced")
          startHRP2OnLineWalking(*m_PGI);
        else if (m_PR->getName() == "talos")
          startTalosOnLineWalking(*m_PGI);
        else
          throw("No valid robot "+m_PR->getName());
        break;

      case PROFIL_SIMPLE_NAVEAU:
        createSimpleEventsForHRP2();
        if (m_PR->getName() == "hrp2_14_reduced")
          startHRP2OnLineWalking(*m_PGI);
        else if (m_PR->getName() == "talos")
          startTalosOnLineWalking(*m_PGI);
        else
          throw("No valid robot "+m_PR->getName());
        break;

      default:
        throw("No correct test profile");
        break;
668
    }
669
670
  }

671
  void generateEventOnLineWalking() {
672
673
    if (m_setOfLocalEvents != 0)
      m_setOfLocalEvents->evaluateEvents(m_OneStep, *this);
674
  }
675

676
677
  void generateEvent() {
    switch (m_TestProfile) {
678
679
680
681
682
683
684
    case PROFIL_NAVEAU:
      generateEventOnLineWalking();
      break;
    case PROFIL_SIMPLE_NAVEAU:
      generateEventOnLineWalking();
    default:
      break;
685
    }
686
687
688
  }
};

689
int PerformTests(int argc, char *argv[]) {
690
#define NB_PROFILES 2
mnaveau's avatar
mnaveau committed
691
  std::string CompleteName = string(argv[0]);
692
  std::size_t found = CompleteName.find_last_of("/\\");
693
  std::string TestName = CompleteName.substr(found + 1);
694

695
  int TestProfiles[NB_PROFILES] = {PROFIL_NAVEAU, PROFIL_SIMPLE_NAVEAU};
696

697
  int indexProfile = -1;
Olivier Stasse's avatar
Olivier Stasse committed
698

699
700
701
702
  if (TestName.compare(14, 6, "Online") == 0)
    indexProfile = PROFIL_NAVEAU;
  if (TestName.compare(14, 12, "OnlineSimple") == 0)
    indexProfile = PROFIL_SIMPLE_NAVEAU;
703

704
705
706
  if (indexProfile == -1) {
    std::cerr << "CompleteName: " << CompleteName << std::endl;
    std::cerr << " TestName: " << TestName << std::endl;
707
708
    std::cerr << "Failure to find the proper indexFile:"
              << TestName.substr(14, 6) << endl;
709
710
711
712
    exit(-1);
  } else {
    ODEBUG("Index detected: " << indexProfile);
  }
713

714
715
716
717
718
719
720
721
  TestNaveau2015 aTN2015(argc, argv, TestName, TestProfiles[indexProfile]);
  if (!aTN2015.init()) {
    cout << "pb on init" << endl;
    return -1;
  }
  try {
    if (!aTN2015.doTest(std::cout)) {
      cout << "Failed test " << indexProfile << endl;
mnaveau's avatar
mnaveau committed
722
      return -1;
723
724
725
    } else
      cout << "Passed test " << indexProfile << endl;
  } catch (const char *astr) {
726
    cerr << "Failed on following error " << astr << std::endl;
727
728
    return -1;
  }
729

730
731
732
  return 0;
}

733
734
735
736
737
738
int main(int argc, char *argv[]) {
  try {
    return PerformTests(argc, argv);
  } catch (const std::string &msg) {
    std::cerr << msg << std::endl;
  }
mnaveau's avatar
mnaveau committed
739
  return 0;
740
}