TestNaveau2015.cpp 23.5 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
27
28
29
/*
 * 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 */
#include <stdlib.h>
#include <iostream>
#include <fstream>
30
31
32
33
#include <iomanip>
#include <string>
#include <map>
#include <cmath>
34

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

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

enum Profiles_t {
47
  PROFIL_NAVEAU // 1
48
49
};

50
// Class TestNaveau2015
51
52
53
54
55
class TestNaveau2015: public TestObject
{

private:
  int resetfiles ;
56
  bool m_DebugFGPIFull ;
57
58
59
  ComAndFootRealizationByGeometry * ComAndFootRealization_ ;
  SimplePluginManager * SPM_ ;
  vector<double> m_err_zmp_x, m_err_zmp_y;
Olivier Stasse's avatar
Olivier Stasse committed
60
61
62
  Eigen::VectorXd m_conf;
  Eigen::VectorXd m_vel ;
  Eigen::VectorXd m_acc ;
63
  int iteration;
64
65
66
67
  std::vector<se3::JointIndex> m_leftLeg  ;
  std::vector<se3::JointIndex> m_rightLeg ;
  std::vector<se3::JointIndex> m_leftArm  ;
  std::vector<se3::JointIndex> m_rightArm ;
68
69
  se3::JointIndex m_leftGripper  ;
  se3::JointIndex m_rightGripper ;
70
71
72
73
74
75
76

public:
  TestNaveau2015(int argc, char *argv[], string &aString, int TestProfile):
    TestObject(argc,argv,aString)
  {
    m_TestProfile = TestProfile;
    resetfiles=0;
77
    m_DebugFGPIFull=true;
78
    m_DebugFGPI=true;
79
80
81
82
83
    ComAndFootRealization_ = NULL;
    SPM_ = NULL ;
    m_err_zmp_x.clear();
    m_err_zmp_y.clear();
    iteration=0;
84
85
86
87
    m_leftLeg .clear();
    m_rightLeg.clear();
    m_leftArm .clear();
    m_rightArm.clear();
88
89
    m_leftGripper  = 0 ;
    m_rightGripper = 0 ;
90
91
92
93
  }

  ~TestNaveau2015()
  {
94
    if(ComAndFootRealization_!=NULL)
95
96
    {
      delete ComAndFootRealization_ ;
97
      ComAndFootRealization_ = NULL;
98
    }
99
    if(SPM_!=NULL)
100
    {
101
102
      delete SPM_ ;
      SPM_ = NULL;
103
104
105
    }
  }

106
  typedef void (TestObject:: * localeventHandler_t)(PatternGeneratorInterface &);
107
108
109
110
111
112
113

  struct localEvent
  {
    unsigned time;
    localeventHandler_t Handler ;
  };

114
  bool doTest(std::ostream &os)
115
  {
116
    bool ret = TestObject::doTest(os);
117
118
    if(m_DebugFGPIFull)
      ComputeAndDisplayZMPStatistic();
119
    return ret ;
120
  }
121

122
123
124
125
126
127
128
129
130
131
132
133
  bool init()
  {
    if(!TestObject::init())
      return false;
    SPM_ = new SimplePluginManager();
    ComAndFootRealization_ = new ComAndFootRealizationByGeometry(
          (PatternGeneratorInterfacePrivate*)SPM_ );
    ComAndFootRealization_->setPinocchioRobot(m_PR);
    ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM_));
    ComAndFootRealization_->SetHeightOfTheCoM(0.814);
    ComAndFootRealization_->setSamplingPeriod(0.005);
    ComAndFootRealization_->Initialization();
134

Olivier Stasse's avatar
Olivier Stasse committed
135
    Eigen::Matrix<double,6,1> waist;
136
    for (int i = 0 ; i < 6 ; ++i )
137
    {
138
      waist(i) = 0;
139
    }
Olivier Stasse's avatar
Olivier Stasse committed
140
    Eigen::Vector3d lStartingCOMState;
141

142
143
144
145
146
147
148
149
150
151
152
153
    lStartingCOMState(0) = m_OneStep.finalCOMPosition.x[0] ;
    lStartingCOMState(1) = m_OneStep.finalCOMPosition.y[0] ;
    lStartingCOMState(2) = m_OneStep.finalCOMPosition.z[0] ;
    ComAndFootRealization_->setSamplingPeriod(0.005);
    ComAndFootRealization_->Initialization();
    ComAndFootRealization_->InitializationCoM(m_HalfSitting,lStartingCOMState,
                                              waist,
                                              m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
    m_conf = m_CurrentConfiguration ;
    m_vel  = m_CurrentVelocity ;
    m_acc  = m_CurrentAcceleration ;
    {
154
      istringstream strm2(":setfeetconstraint XY 0.09 0.06");
155
156
      m_PGI->ParseCmd(strm2);
    }
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
    m_leftLeg =
        m_PR->jointsBetween(m_PR->waist(),m_PR->leftFoot()->associatedAnkle);
    m_rightLeg =
        m_PR->jointsBetween(m_PR->waist(),m_PR->rightFoot()->associatedAnkle);
    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() );

    se3::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
    for(unsigned i=0 ; i <m_leftLeg.size() ; ++i)
      m_leftLeg[i] = se3::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
    for(unsigned i=0 ; i <m_rightLeg.size() ; ++i)
      m_rightLeg[i] = se3::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
    for(unsigned i=0 ; i <m_leftArm.size() ; ++i)
      m_leftArm[i] = se3::idx_q(ActuatedJoints[m_leftArm[i]])-1;
    for(unsigned i=0 ; i <m_rightArm.size() ; ++i)
      m_rightArm[i] = se3::idx_q(ActuatedJoints[m_rightArm[i]])-1;

179
180
181
182
183
184
185
186
187
188
189
190
    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 ;
    else
      m_rightGripper = 0 ;

    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 ;
    else
      m_leftGripper = 0 ;

191
    return true ;
192
193
  }

194
protected:
195
196
197
198
199
200
201
202
203
204
205
206
207
208

  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]) ;
      if( abs_value > max_abs)
        max_abs = abs_value ;

      total += abs_value ;
    }
Olivier Stasse's avatar
Olivier Stasse committed
209
    avg = total/(double)(vec.size());
210
211
212
213
214
215
216
217
    return ;
  }

  void ComputeAndDisplayZMPStatistic()
  {
    cout << "Statistic for Dzmp in x : " << endl ;
    double moy_delta_zmp_x = 0.0 ;
    double max_abs_err_x = 0.0 ;
218
    ComputeStat(m_err_zmp_x,moy_delta_zmp_x,max_abs_err_x);
219
220
221
222
223
224
    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 ;
225
    ComputeStat(m_err_zmp_y,moy_delta_zmp_y,max_abs_err_y);
226
227
228
229
230
    cout << "average : " << moy_delta_zmp_y << endl ;
    cout << "maxx error : " << max_abs_err_y << endl ;
    return ;
  }

Olivier Stasse's avatar
Olivier Stasse committed
231
232
233
  void analyticalInverseKinematics(Eigen::VectorXd & conf,
                                   Eigen::VectorXd & vel,
                                   Eigen::VectorXd & acc)
234
235
236
237
  {
    /// \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
238
239
240
241
242
    Eigen::VectorXd aCOMState(6);
    Eigen::VectorXd aCOMSpeed(6);
    Eigen::VectorXd aCOMAcc(6);
    Eigen::VectorXd aLeftFootPosition(5);
    Eigen::VectorXd aRightFootPosition(5);
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
270
271
272
273
274
275

    aCOMState(0) = m_OneStep.finalCOMPosition.x[0];
    aCOMState(1) = m_OneStep.finalCOMPosition.y[0];
    aCOMState(2) = m_OneStep.finalCOMPosition.z[0];
    aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]  * 180/M_PI ;
    aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0] * 180/M_PI ;
    aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]   * 180/M_PI ;

    aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1];
    aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1];
    aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1];
    aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1] /** * 180/M_PI  */ ;
    aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]/** * 180/M_PI  */ ;
    aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]/** * 180/M_PI  */ ;

    aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
    aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
    aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
    aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2]/** * 180/M_PI  */ ;
    aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2]/** * 180/M_PI  */ ;
    aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2] /** * 180/M_PI  */;

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

    aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
    aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
    aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
    aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
    aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
276
    ComAndFootRealization_->setSamplingPeriod(0.005);
277
278
279
    ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(
          aCOMState, aCOMSpeed, aCOMAcc, aLeftFootPosition, aRightFootPosition,
          conf, vel, acc, iteration, 1);
280

281
282
283
284
285
    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;
    }
286
  }
287

Olivier Stasse's avatar
Olivier Stasse committed
288
  Eigen::VectorXd parseFromURDFtoOpenHRPIndex()
289
  {
Olivier Stasse's avatar
Olivier Stasse committed
290
291
    Eigen::VectorXd conf = m_conf;
    { for(unsigned int i=0;i<conf.size();conf[i++]=0.0);};
292
293
294

    for(unsigned int i = 0 ; i < 6 ; i++)
      conf(i) = m_conf(i);
295
296
297
298
    unsigned index=6;
    //RLEG
    for(unsigned int i = 0 ; i < m_rightLeg.size() ; i++)
      conf(index+i) = m_conf(m_rightLeg[i]);
Olivier Stasse's avatar
Olivier Stasse committed
299
    index+=(unsigned)m_rightLeg.size();
300
301
302
    //LLEG
    for(unsigned int i = 0 ; i < m_leftLeg.size() ; i++)
      conf(index+i) = m_conf(m_leftLeg[i]);
Olivier Stasse's avatar
Olivier Stasse committed
303
    index+=(unsigned)m_leftLeg.size();
304
305
306
307
308
309
310
311
312
313
    //CHEST
    for(unsigned int i = 0 ; i < 2 ; i++)
      conf(index+i) = 0.0 ;
    index+= 2 ;
    //HEAD
    for(unsigned int i = 0 ; i < 2 ; i++)
      conf(index+i) = 0.0 ;
    index+= 2 ;
    //RARM
    for(unsigned int i = 0 ; i < m_rightArm.size() ; i++)
314
      conf(index+i) = m_HalfSitting(m_rightArm[i]-6);
Olivier Stasse's avatar
Olivier Stasse committed
315
    index+=(unsigned)m_rightArm.size();
316
317
    conf(index) = 10*M_PI/180;
    ++index;
318
319
    //LARM
    for(unsigned int i = 0 ; i < m_leftArm.size() ; i++)
320
321
322
      conf(index+i) = m_HalfSitting(m_leftArm[i]-6);
    index+=m_leftArm.size();
    conf(index) = 10*M_PI/180;
323
324
325
326

    return conf ;
  }

327
  void createOpenHRPFiles()
328
  {
329
330
    /// \brief Create file .hip .pos .zmp
    /// ---------------------------------
Olivier Stasse's avatar
Olivier Stasse committed
331
    Eigen::VectorXd conf = parseFromURDFtoOpenHRPIndex();
332
    ofstream aof ;
333
334
335
336
    string aPosFileName = /*"/tmp/" +*/ m_TestName + ".pos" ;
    string aZMPFileName = /*"/tmp/" +*/ m_TestName + ".zmp" ;
    string aWaistFileName = /*"/tmp/" +*/ m_TestName + ".waist" ;
    string aHipFileName = /*"/tmp/" +*/ m_TestName + ".hip" ;
337
    if ( iteration == 0 )
338
    {
339
340
341
342
343
344
345
      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);
346
      aof.close();
347
348
    }

349
    aof.open(aPosFileName.c_str(),ofstream::app);
350
351
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
352
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;// 1
Olivier Stasse's avatar
Olivier Stasse committed
353
    for(unsigned i=6 ; i<conf.size() ; ++i)
354
      aof << filterprecision( conf(i) ) << " "  ;                    // 2-30
355
    for(unsigned i=0 ; i<9 ; ++i)
356
357
      aof << filterprecision( 0.0 ) << " "  ;                        // 31-40
    aof << 0.0  << endl ;                                            // 41
358
359
    aof.close();

360
     aof.open(aWaistFileName.c_str(),ofstream::app);
361
362
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
363
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;     // 1
364
365
366
367
368
369
    aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " "  ; // 2
    aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " "  ;// 3
    aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ;          // 4
    aof << endl ;
    aof.close();

370
371

    aof.open(aHipFileName.c_str(),ofstream::app);
372
373
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
374
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;     // 1
375
    aof << filterprecision( m_OneStep.finalCOMPosition.roll[0]) << " "  ; // 2
376
377
378
379
380
    aof << filterprecision( m_OneStep.finalCOMPosition.pitch[0]) << " "  ;// 3
    aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0]) ;          // 4
    aof << endl ;
    aof.close();

381

382
383
384
385
386
387
    FootAbsolutePosition aSupportState;
    if (m_OneStep.LeftFootPosition.stepType < 0 )
      aSupportState = m_OneStep.LeftFootPosition ;
    else
      aSupportState = m_OneStep.RightFootPosition ;

388
    aof.open(aZMPFileName.c_str(),ofstream::app);
389
390
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
391
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;   // 1
392
393
394
    aof << filterprecision( m_OneStep.ZMPTarget(0) - conf(0)) << " "  ; // 2
    aof << filterprecision( m_OneStep.ZMPTarget(1) - conf(1) ) << " "  ;// 3
    aof << filterprecision( aSupportState.z  - conf(2))  ;              // 4
395
396
    aof << endl ;
    aof.close();
397
398
399
400
401
  }

  void prepareDebugFiles()
  {
    TestObject::prepareDebugFiles() ;
402
    if (m_DebugFGPIFull)
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
    {
      ofstream aof;
      string aFileName;
      aFileName = m_TestName;
      aFileName += "TestFGPIFull.dat";
      if (m_OneStep.NbOfIt==1)
      {
        aof.open(aFileName.c_str(),ofstream::out);
      }
    }
  }

  void fillInDebugFiles()
  {
    TestObject::fillInDebugFiles();
418

419
    if(m_DebugFGPIFull)
420
    {
421
422

      analyticalInverseKinematics(m_conf,m_vel,m_acc);
423
      if(iteration==0)
424
      {
425

426
427
428
429
430
//        cout << endl ;
//        cout << m_conf << endl ;
//        cout << m_HalfSitting << endl ;
//        cout << std::boolalpha << isHalfsitting << endl ;
//        assert(isHalfsitting);
431
      }
432

433
      m_DebugPR->computeInverseDynamics(m_conf,m_vel,m_acc);
Olivier Stasse's avatar
Olivier Stasse committed
434
      Eigen::Vector3d com, dcom, ddcom;
435
436
437
438
439
440
441
442
443
444
445
      m_DebugPR->CenterOfMass(com, dcom, ddcom);
//      cout << m_OneStep.finalCOMPosition.x[0] - com(0) << " "
//           << m_OneStep.finalCOMPosition.y[0] - com(1) << " "
//           << m_OneStep.finalCOMPosition.z[0] - com(2) << " ; "
//           << m_OneStep.finalCOMPosition.x[1] - dcom(0) << " "
//           << m_OneStep.finalCOMPosition.y[1] - dcom(1) << " "
//           << m_OneStep.finalCOMPosition.z[1] - dcom(2) << " ; "
//           << m_OneStep.finalCOMPosition.x[2] - ddcom(0) << " "
//           << m_OneStep.finalCOMPosition.y[2] - ddcom(1) << " "
//           << m_OneStep.finalCOMPosition.z[2] - ddcom(2) << endl ;

446
      createOpenHRPFiles();
Olivier Stasse's avatar
Olivier Stasse committed
447
      Eigen::Vector3d zmpmb;
448
449
450
      m_DebugPR->zeroMomentumPoint(zmpmb);
      m_err_zmp_x.push_back(zmpmb[0]-m_OneStep.ZMPTarget(0)) ;
      m_err_zmp_y.push_back(zmpmb[1]-m_OneStep.ZMPTarget(1)) ;
451

452
      ++iteration;
453
454
455
456
457
458
459
460
461
462
463
464
465

      ofstream aof;
      string aFileName;
      aFileName = m_TestName;
      aFileName += "TestFGPIFull.dat";
      if (m_OneStep.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);
466
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
505
506
507
508
509
510
511
512
      aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " "             // 1
          << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " "    // 2
          << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " "    // 3
          << filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " "    // 4
          << filterprecision(m_OneStep.finalCOMPosition.yaw[0] ) << " "  // 5
          << filterprecision(m_OneStep.finalCOMPosition.x[1] ) << " "    // 6
          << filterprecision(m_OneStep.finalCOMPosition.y[1] ) << " "    // 7
          << filterprecision(m_OneStep.finalCOMPosition.z[1] ) << " "    // 8
          << filterprecision(m_OneStep.finalCOMPosition.yaw[1] ) << " "  // 9
          << filterprecision(m_OneStep.finalCOMPosition.x[2] ) << " "    // 10
          << filterprecision(m_OneStep.finalCOMPosition.y[2] ) << " "    // 11
          << filterprecision(m_OneStep.finalCOMPosition.z[2] ) << " "    // 12
          << filterprecision(m_OneStep.finalCOMPosition.yaw[2] ) << " "  // 13
          << filterprecision(m_OneStep.ZMPTarget(0) ) << " "             // 14
          << filterprecision(m_OneStep.ZMPTarget(1) ) << " "             // 15
          << filterprecision(m_OneStep.ZMPTarget(2) ) << " "             // 16
          << filterprecision(m_OneStep.LeftFootPosition.x  ) << " "      // 17
          << filterprecision(m_OneStep.LeftFootPosition.y  ) << " "      // 18
          << filterprecision(m_OneStep.LeftFootPosition.z  ) << " "      // 19
          << filterprecision(m_OneStep.LeftFootPosition.dx  ) << " "     // 20
          << filterprecision(m_OneStep.LeftFootPosition.dy  ) << " "     // 21
          << filterprecision(m_OneStep.LeftFootPosition.dz  ) << " "     // 22
          << filterprecision(m_OneStep.LeftFootPosition.ddx  ) << " "    // 23
          << filterprecision(m_OneStep.LeftFootPosition.ddy  ) << " "    // 24
          << filterprecision(m_OneStep.LeftFootPosition.ddz  ) << " "    // 25
          << filterprecision(m_OneStep.LeftFootPosition.theta ) << " "   // 26
          << filterprecision(m_OneStep.LeftFootPosition.dtheta ) << " "  // 27
          << filterprecision(m_OneStep.LeftFootPosition.ddtheta ) << " " // 28
          << filterprecision(m_OneStep.LeftFootPosition.omega  ) << " "  // 29
          << filterprecision(m_OneStep.LeftFootPosition.omega2  ) << " " // 30
          << filterprecision(m_OneStep.RightFootPosition.x ) << " "      // 31
          << filterprecision(m_OneStep.RightFootPosition.y ) << " "      // 32
          << filterprecision(m_OneStep.RightFootPosition.z ) << " "      // 33
          << filterprecision(m_OneStep.RightFootPosition.dx ) << " "     // 34
          << filterprecision(m_OneStep.RightFootPosition.dy ) << " "     // 35
          << filterprecision(m_OneStep.RightFootPosition.dz ) << " "     // 36
          << filterprecision(m_OneStep.RightFootPosition.ddx ) << " "    // 37
          << filterprecision(m_OneStep.RightFootPosition.ddy ) << " "    // 38
          << filterprecision(m_OneStep.RightFootPosition.ddz ) << " "    // 39
          << filterprecision(m_OneStep.RightFootPosition.theta ) << " "  // 40
          << filterprecision(m_OneStep.RightFootPosition.dtheta ) << " " // 41
          << filterprecision(m_OneStep.RightFootPosition.ddtheta ) << " "// 42
          << filterprecision(m_OneStep.RightFootPosition.omega  ) << " " // 43
          << filterprecision(m_OneStep.RightFootPosition.omega2  ) << " "// 44
          << filterprecision(zmpmb[0]) << " "                            // 45
          << filterprecision(zmpmb[1]) << " "                            // 46
          << filterprecision(zmpmb[2]) << " "                           ;// 47
513
514
      for(unsigned int k = 0 ; k < m_conf.size() ; k++){ // 48-53 -> 54-83
        aof << filterprecision( m_conf(k) ) << " "  ;
515
      }
516
517
518
519
520
521
      for(unsigned int k = 0 ; k < m_vel.size() ; k++){ // 84-89 -> 90-118
        aof << filterprecision( m_vel(k) ) << " "  ;
      }
      for(unsigned int k = 0 ; k < m_acc.size() ; k++){ // 119-125 -> 125-155
        aof << filterprecision( m_acc(k) ) << " "  ;
      }
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
      aof << endl;
      aof.close();
    }
  }

  void startOnLineWalking(PatternGeneratorInterface &aPGI)
  {
    CommonInitialization(aPGI);

    {
      istringstream strm2(":SetAlgoForZmpTrajectory Naveau");
      aPGI.ParseCmd(strm2);

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

563
564
565
566
567
    {
      istringstream strm2(":deleteallobstacles");
      m_PGI->ParseCmd(strm2);
    }

568
    {
569
570
      //istringstream strm2(":feedBackControl true");
      istringstream strm2(":feedBackControl false");
571
572
573
      m_PGI->ParseCmd(strm2);
    }

mnaveau's avatar
mnaveau committed
574
    {
575
      istringstream strm2(":useDynamicFilter true");
mnaveau's avatar
mnaveau committed
576
      //istringstream strm2(":useDynamicFilter false");
mnaveau's avatar
mnaveau committed
577
578
      m_PGI->ParseCmd(strm2);
    }
579
//    {
580
581
582
583
584
585
//      istringstream strm2(":addoneobstacle 9.0 0.5 0.23");
//      m_PGI->ParseCmd(strm2);
//    }


//    {
586
//      istringstream strm2(":addoneobstacle 1.0 0.5 0.23");
587
588
589
//      m_PGI->ParseCmd(strm2);
//    }

590
591
592
593
//    {
//      istringstream strm2(":addoneobstacle 1.5 -0.0 0.23");
//      m_PGI->ParseCmd(strm2);
//    }
594

595

596
597
598
599
//    {
//      istringstream strm2(":updateoneobstacle 1 1.5 -1.5 0.23");
//      m_PGI->ParseCmd(strm2);
//    }
600
601
602
603
604
605
606
607
  }

  void chooseTestProfile()
  {

    switch(m_TestProfile)
    {

608
      case PROFIL_NAVEAU:
609
610
611
612
613
614
615
616
617
618
        startOnLineWalking(*m_PGI);
        break;
      default:
        throw("No correct test profile");
        break;
    }
  }

  void generateEventOnLineWalking()
  {
619
620
621
    #define localNbOfEvents 20
    struct localEvent events [localNbOfEvents] =
    {
622
      {1*200,&TestObject::walkForwardSlow},
623
624
625
626
627
628
629
      {10*200,&TestObject::walkForward2m_s},
      {20*200,&TestObject::walkSidewards2m_s},
      {30*200,&TestObject::walkX05Y04},
      {40*200,&TestObject::startTurningRight2},
      {50*200,&TestObject::walkOnSpot},
      {66*200,&TestObject::stop},
      {76*200,&TestObject::stopOnLineWalking}
630
631
632
    };
    // Test when triggering event.
    for(unsigned int i=0;i<localNbOfEvents;i++)
633
    {
634
635
636
637
638
      if ( m_OneStep.NbOfIt==events[i].time)
      {
        ODEBUG3("********* GENERATE EVENT EMS ***********");
        (this->*(events[i].Handler))(*m_PGI);
      }
639
    }
640
//    if(m_OneStep.NbOfIt>=0*200)
mnaveau's avatar
mnaveau committed
641
642
643
//    {
//      ostringstream oss ;
//      oss << ":perturbationforce "
644
645
//          << 0.0 << " "
//          << 0.0 << " "
mnaveau's avatar
mnaveau committed
646
647
648
649
//          << " 0.0";
//      istringstream strm (oss.str()) ;
//      m_PGI->ParseCmd(strm);
//    }
650
  }
651

652
653
654
  void generateEvent()
  {
    switch(m_TestProfile){
655
      case PROFIL_NAVEAU:
656
657
658
659
660
661
662
663
664
665
666
        generateEventOnLineWalking();
        break;
      default:
        break;
    }
  }

};

int PerformTests(int argc, char *argv[])
{
667
#define NB_PROFILES 1
mnaveau's avatar
mnaveau committed
668
669
670
671
  std::string CompleteName = string(argv[0]);
  unsigned found = CompleteName.find_last_of("/\\");
  std::string TestName =  CompleteName.substr(found+1);
  std::string TestNames[NB_PROFILES] = {TestName};
672
  int TestProfiles[NB_PROFILES] = {PROFIL_NAVEAU};
673

674

675
676
  for (unsigned int i=0;i<NB_PROFILES;i++){
    TestNaveau2015 aTN2015(argc,argv,TestNames[i],TestProfiles[i]);
mnaveau's avatar
mnaveau committed
677
678
679
680
681
682
683
    if(!aTN2015.init())
    {
      cout << "pb on init" << endl;
      return -1;
    }
    try
    {
684
685
686
687
688
689
690
691
692
693
694
      if (!aTN2015.doTest(std::cout)){
        cout << "Failed test " << i << endl;
        return -1;
      }
      else
        cout << "Passed test " << i << endl;
    }
    catch (const char * astr){
      cerr << "Failed on following error " << astr << std::endl;
      return -1; }
  }
695

696
697
698
699
700
701
702
  return 0;
}

int main(int argc, char *argv[])
{
  try
  {
703
    return PerformTests(argc,argv);
704
705
706
707
708
  }
  catch (const std::string& msg)
  {
    std::cerr << msg << std::endl;
  }
mnaveau's avatar
mnaveau committed
709
  return 0;
710
}