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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  PROFIL_NAVEAU,       // 0
  PROFIL_SIMPLE_NAVEAU // 1
};

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

struct localEvent
{
  unsigned time;
  localeventHandler_t Handler ;
};

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

64
  setOfLocalEvents(PatternGeneratorInterface *aPGI)
Olivier Stasse's avatar
Olivier Stasse committed
65
66
  { m_PGI = aPGI;}

67
68
69
70
71
72
73
74
  void initVecOfLocalEvents( struct localEvent * events,
			     unsigned int sizeOfEvents)
  {
    for(unsigned int i=0;i<sizeOfEvents;i++)
      {
	m_vecOfLocalEvents.push_back(events[i]);
      }
  }
Olivier Stasse's avatar
Olivier Stasse committed
75

76
77
78
79
80
81
  void evaluateEvents(struct OneStep &oneStep,
		      TestObject &aTestObject)
  {
    // Test when triggering event.
    for(unsigned int i=0;i<m_vecOfLocalEvents.size();i++)
      {
82
	if ( oneStep.m_NbOfIt==m_vecOfLocalEvents[i].time)
83
84
85
86
87
88
	  {
	    ODEBUG3("********* GENERATE EVENT EMS ***********");
	    (aTestObject.*(m_vecOfLocalEvents[i].Handler))(*m_PGI);
	  }
      }
  }
89
90
};

91
// Class TestNaveau2015
92
93
94
95
96
class TestNaveau2015: public TestObject
{

private:
  int resetfiles ;
97
  bool m_DebugFGPIFull ;
98
  vector<double> m_err_zmp_x, m_err_zmp_y;
99

100
  int iteration;
Olivier Stasse's avatar
Olivier Stasse committed
101
102
103
104
105
106
  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 ;
107

108
109
  /// Object to generate events according to profile.
  setOfLocalEvents * m_setOfLocalEvents;
Olivier Stasse's avatar
Olivier Stasse committed
110

111
112
113
114
115
116
public:
  TestNaveau2015(int argc, char *argv[], string &aString, int TestProfile):
    TestObject(argc,argv,aString)
  {
    m_TestProfile = TestProfile;
    resetfiles=0;
117
    m_DebugFGPIFull=true;
118
    m_DebugFGPI=true;
119
120
121
    m_err_zmp_x.clear();
    m_err_zmp_y.clear();
    iteration=0;
122
123
124
125
    m_leftLeg .clear();
    m_rightLeg.clear();
    m_leftArm .clear();
    m_rightArm.clear();
126
127
    m_leftGripper  = 0 ;
    m_rightGripper = 0 ;
128
    m_setOfLocalEvents = 0;
129
130
131

  }

132
  bool doTest(std::ostream &os)
133
  {
134
    bool ret = TestObject::doTest(os);
135
136
    if(m_DebugFGPIFull)
      ComputeAndDisplayZMPStatistic();
137
    return ret ;
138
  }
139

140
141
142
143
  bool init()
  {
    if(!TestObject::init())
      return false;
144
145
146
147
148
149
150
151
152
153

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

    if (m_PR->getName()=="hrp2_14_reduced")
      m_ComAndFootRealization->SetHeightOfTheCoM(0.814);
    else if (m_PR->getName()=="talos")
      m_ComAndFootRealization->SetHeightOfTheCoM(0.876681);
Olivier Stasse's avatar
Olivier Stasse committed
154

155

156
    m_leftLeg =
157
      m_PR->jointsBetween(m_PR->waist(),m_PR->leftFoot()->associatedAnkle);
158
    m_rightLeg =
159
      m_PR->jointsBetween(m_PR->waist(),m_PR->rightFoot()->associatedAnkle);
160
    m_leftArm =
161
      m_PR->jointsBetween(m_PR->chest(),m_PR->leftWrist());
162
    m_rightArm =
163
      m_PR->jointsBetween(m_PR->chest(),m_PR->rightWrist());
164
165
166
167

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

Olivier Stasse's avatar
Olivier Stasse committed
168
    pinocchio::JointModelVector & ActuatedJoints = m_PR->getActuatedJoints();
169
    for(unsigned i=0 ; i <m_leftLeg.size() ; ++i)
Olivier Stasse's avatar
Olivier Stasse committed
170
      m_leftLeg[i] = pinocchio::idx_q(ActuatedJoints[m_leftLeg[i]])-1;
171
    for(unsigned i=0 ; i <m_rightLeg.size() ; ++i)
Olivier Stasse's avatar
Olivier Stasse committed
172
      m_rightLeg[i] = pinocchio::idx_q(ActuatedJoints[m_rightLeg[i]])-1;
173
    for(unsigned i=0 ; i <m_leftArm.size() ; ++i)
Olivier Stasse's avatar
Olivier Stasse committed
174
      m_leftArm[i] = pinocchio::idx_q(ActuatedJoints[m_leftArm[i]])-1;
175
    for(unsigned i=0 ; i <m_rightArm.size() ; ++i)
Olivier Stasse's avatar
Olivier Stasse committed
176
      m_rightArm[i] = pinocchio::idx_q(ActuatedJoints[m_rightArm[i]])-1;
177

178
179
180
181
182
183
184
185
186
187
188
189
    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 ;

190
    return true ;
191
192
  }

193
protected:
194
195
196
197
198
199
200

  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)
201
202
203
204
      {
	double abs_value = sqrt(vec[i]*vec[i]) ;
	if( abs_value > max_abs)
	  max_abs = abs_value ;
205

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

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

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

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
    aCOMState(0) = m_OneStep.m_finalCOMPosition.x[0];
    aCOMState(1) = m_OneStep.m_finalCOMPosition.y[0];
    aCOMState(2) = m_OneStep.m_finalCOMPosition.z[0];
    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 ;

    aCOMSpeed(0) = m_OneStep.m_finalCOMPosition.x[1];
    aCOMSpeed(1) = m_OneStep.m_finalCOMPosition.y[1];
    aCOMSpeed(2) = m_OneStep.m_finalCOMPosition.z[1];
    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  */ ;

    aCOMAcc(0) = m_OneStep.m_finalCOMPosition.x[2];
    aCOMAcc(1) = m_OneStep.m_finalCOMPosition.y[2];
    aCOMAcc(2) = m_OneStep.m_finalCOMPosition.z[2];
    aCOMAcc(3) = m_OneStep.m_finalCOMPosition.roll[2]/** * 180/M_PI  */ ;
    aCOMAcc(4) = m_OneStep.m_finalCOMPosition.pitch[2]/** * 180/M_PI  */ ;
    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;
275
    m_ComAndFootRealization->setSamplingPeriod(0.005);
276
277
278
279
280
    m_ComAndFootRealization->
      ComputePostureForGivenCoMAndFeetPosture
      (aCOMState, aCOMSpeed, aCOMAcc,
       aLeftFootPosition, aRightFootPosition,
       conf, vel, acc, iteration, 1);
281

282
    if(m_leftGripper!=0 && m_rightGripper!=0)
283
284
285
286
      {
	conf(m_leftGripper) = 10.0*M_PI/180.0;
	conf(m_rightGripper) = 10.0*M_PI/180.0;
      }
287
  }
288

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

    for(unsigned int i = 0 ; i < 6 ; i++)
      conf(i) = m_conf(i);
296
    std::size_t index=6;
297
298
299
    //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
300
    index+=(unsigned)m_rightLeg.size();
301
302
303
    //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
304
    index+=(unsigned)m_leftLeg.size();
305
306
307
308
309
310
311
312
313
314
    //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++)
315
      conf(index+i) = m_HalfSitting(m_rightArm[i]-6);
Olivier Stasse's avatar
Olivier Stasse committed
316
    index+=(unsigned)m_rightArm.size();
317
318
    conf(index) = 10*M_PI/180;
    ++index;
319
320
    //LARM
    for(unsigned int i = 0 ; i < m_leftArm.size() ; i++)
321
322
323
      conf(index+i) = m_HalfSitting(m_leftArm[i]-6);
    index+=m_leftArm.size();
    conf(index) = 10*M_PI/180;
324
325
326
327

    return conf ;
  }

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

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

361
    aof.open(aWaistFileName.c_str(),ofstream::app);
362
363
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
364
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;     // 1
365
366
367
    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
368
369
370
    aof << endl ;
    aof.close();

371
372

    aof.open(aHipFileName.c_str(),ofstream::app);
373
374
    aof.precision(8);
    aof.setf(ios::scientific, ios::floatfield);
375
    aof << filterprecision( (double)iteration * 0.005 ) << " "  ;     // 1
376
377
378
    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
379
380
381
    aof << endl ;
    aof.close();

382

383
    FootAbsolutePosition aSupportState;
384
385
    if (m_OneStep.m_LeftFootPosition.stepType < 0 )
      aSupportState = m_OneStep.m_LeftFootPosition ;
386
    else
387
      aSupportState = m_OneStep.m_RightFootPosition ;
388

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

  void prepareDebugFiles()
  {
    TestObject::prepareDebugFiles() ;
403
    if (m_DebugFGPIFull)
404
      {
405
406
407
	ofstream aof;
	string aFileName;

Olivier Stasse's avatar
Olivier Stasse committed
408
	aFileName = m_TestName;
409
410
        aFileName += "TestFGPIFull_description.dat";

411
        aof.open(aFileName.c_str(),ofstream::out);
412
413
414
415
416
417
418
419
420
421
422
423
424
425

        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
Olivier Stasse's avatar
Olivier Stasse committed
426
	    "Com ddYaw",                     // 13
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
	    "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
Olivier Stasse's avatar
Olivier Stasse committed
444
	    "Right Foot X" ,                 // 31
445
446
447
448
449
	    "Right Foot Y" ,                 // 32
	    "Right Foot Z" ,                 // 33
	    "Right Foot dX" ,                // 34
	    "Right Foot dY" ,                // 35
	    "Right Foot dZ" ,                // 36
Olivier Stasse's avatar
Olivier Stasse committed
450
	    "Right Foot ddX" ,               // 37
451
452
453
454
455
456
457
458
459
460
	    "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
Olivier Stasse's avatar
Olivier Stasse committed
461
	    "q " ,                           // 48
462
463
464
465
	    "dq" ,                           // 48 + nq
	    "ddq "};                         // 48 + nq + nv
        for(unsigned int i=0;i<51;i++)
          aof << i+1 << ". " <<Titles[i] <<std::endl;
Olivier Stasse's avatar
Olivier Stasse committed
466

467
468
469
470
        aof.close();

	aFileName = m_TestName;
	aFileName += "TestFGPIFull.dat";
471
	if (m_OneStep.m_NbOfIt==1)
472
473
474
475
	  {
	    aof.open(aFileName.c_str(),ofstream::out);
	  }

476
477
478
      }
  }

479

Olivier Stasse's avatar
Olivier Stasse committed
480

481
482
483
  void fillInDebugFiles()
  {
    TestObject::fillInDebugFiles();
484

485
486
487
    m_DumpReferencesObjects.fillInTests(m_TestName,
					m_OneStep,
					m_CurrentConfiguration);
488
    if(m_DebugFGPIFull)
489
      {
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
	analyticalInverseKinematics(m_CurrentConfiguration,
				    m_CurrentVelocity,
				    m_CurrentAcceleration);
	if(iteration==0)
	  {

	    //        cout << endl ;
	    //        cout << m_conf << endl ;
	    //        cout << m_HalfSitting << endl ;
	    //        cout << std::boolalpha << isHalfsitting << endl ;
	    //        assert(isHalfsitting);
	  }

	m_DebugPR->computeInverseDynamics(m_CurrentConfiguration,
					  m_CurrentVelocity,
					  m_CurrentAcceleration);
	Eigen::Vector3d com, dcom, ddcom;
	m_DebugPR->CenterOfMass(com, dcom, ddcom);
	createOpenHRPFiles();
	Eigen::Vector3d zmpmb;
	m_DebugPR->zeroMomentumPoint(zmpmb);
511
512
	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)) ;
513
514
515
516
517
518
519

	++iteration;

	ofstream aof;
	string aFileName;
	aFileName = m_TestName;
	aFileName += "TestFGPIFull.dat";
520
	if (m_OneStep.m_NbOfIt==1)
521
522
523
524
525
526
527
	  {
	    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);
528
529
530
	m_OneStep.fillInDebugFileContent(aof);
	aof    << filterprecision(zmpmb[0]) << " "                            // 45
	       << filterprecision(zmpmb[1]) << " "                            // 46
531
532
533
534
535
536
537
538
539
540
541
542
	    << filterprecision(zmpmb[2]) << " "                           ;// 47
	for(unsigned int k = 0 ; k < m_conf.size() ; k++){ // 48-53 -> 54-83
	  aof << filterprecision( m_conf(k) ) << " "  ;
	}
	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) ) << " "  ;
	}
	aof << endl;
	aof.close();
543
      }
544
  }
545

546
547
548
549
  void createFullEventsForHRP2()
  {
    ODEBUG3("createFullEventsForHRP2");
    localEvent events[8] =
550
      {
551
552
553
554
555
556
557
558
559
560
561
562
	{1*200,&TestObject::walkForwardSlow},
	{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}
      };

    if (m_setOfLocalEvents!=0)
      m_setOfLocalEvents->initVecOfLocalEvents(events,8);
563
564
  }

565
566
567
568
  void createSimpleEventsForHRP2()
  {
    localEvent events[2] =
      {
Olivier Stasse's avatar
Olivier Stasse committed
569
	{1*200,&TestObject::walkOnSpot},
570
571
	{10*200,&TestObject::stop},
      };
Olivier Stasse's avatar
Olivier Stasse committed
572

573
574
    if (m_setOfLocalEvents!=0)
      m_setOfLocalEvents->initVecOfLocalEvents(events,2);
575
576
  }

Olivier Stasse's avatar
Olivier Stasse committed
577
578


579
  void startHRP2OnLineWalking(PatternGeneratorInterface &aPGI)
580
581
582
583
584
585
586
587
588
  {
    CommonInitialization(aPGI);

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

    }
    {
589
590
      //istringstream strm2(":singlesupporttime 1.4");
      istringstream strm2(":singlesupporttime 0.7");
591
592
593
      aPGI.ParseCmd(strm2);
    }
    {
594
595
      //istringstream strm2(":doublesupporttime 0.2");
      istringstream strm2(":doublesupporttime 0.1");
596
597
598
599
600
601
602
603
604
605
606
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":NaveauOnline");
      aPGI.ParseCmd(strm2);
    }
    {
      istringstream strm2(":numberstepsbeforestop 2");
      aPGI.ParseCmd(strm2);
    }
    {
607
      istringstream strm2(":setfeetconstraint XY 0.095 0.055");
608
609
      m_PGI->ParseCmd(strm2);
    }
610
611
612
613
614
    {
      istringstream strm2(":stepheight 0.05");
      m_PGI->ParseCmd(strm2);
    }

615
616
617
618
619
    {
      istringstream strm2(":deleteallobstacles");
      m_PGI->ParseCmd(strm2);
    }

620
    {
621
622
      //istringstream strm2(":feedBackControl true");
      istringstream strm2(":feedBackControl false");
623
624
625
      m_PGI->ParseCmd(strm2);
    }

mnaveau's avatar
mnaveau committed
626
    {
627
628
      //istringstream strm2(":useDynamicFilter true");
      istringstream strm2(":useDynamicFilter false");
mnaveau's avatar
mnaveau committed
629
630
      m_PGI->ParseCmd(strm2);
    }
631

632
  }
633

634
635
636
637
638
639
640
641
  void startTalosOnLineWalking(PatternGeneratorInterface &aPGI)
  {
    CommonInitialization(aPGI);

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

643
644
645
646
647
648
    {
      istringstream strm2(":SetAlgoForZmpTrajectory Naveau");
      aPGI.ParseCmd(strm2);

    }
    {
649
      istringstream strm2(":singlesupporttime 1.0");
650
      //istringstream strm2(":singlesupporttime 0.7");
651
652
653
      aPGI.ParseCmd(strm2);
    }
    {
654
655
      istringstream strm2(":doublesupporttime 0.2");
      //istringstream strm2(":doublesupporttime 0.1");
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
      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
672
673
      m_PGI->ParseCmd(strm2);
    }
674

675
676
677
678
    {
      istringstream strm2(":deleteallobstacles");
      m_PGI->ParseCmd(strm2);
    }
679

680
681
682
683
684
    {
      //istringstream strm2(":feedBackControl true");
      istringstream strm2(":feedBackControl false");
      m_PGI->ParseCmd(strm2);
    }
685

686
    {
687
688
      istringstream strm2(":useDynamicFilter true");
      //istringstream strm2(":useDynamicFilter false");
689
690
      m_PGI->ParseCmd(strm2);
    }
691

692

693
694
695
696
  }

  void chooseTestProfile()
  {
697
698
    ODEBUG3("ROBOT:" << m_PR->getName() <<
	    " Profile: " << m_TestProfile);
699
    switch(m_TestProfile)
700
      {
701

702
      case PROFIL_NAVEAU:
703
704
705
706
707
708
709
710
	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");
        break;
Olivier Stasse's avatar
Olivier Stasse committed
711

712
713
714
715
716
717
718
719
      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");
720
        break;
Olivier Stasse's avatar
Olivier Stasse committed
721

722
723
724
      default:
        throw("No correct test profile");
        break;
725
      }
726
727
728
729
  }

  void generateEventOnLineWalking()
  {
730
731
    if (m_setOfLocalEvents!=0)
      m_setOfLocalEvents->evaluateEvents(m_OneStep,*this);
732
  }
733

734
735
  void generateEvent()
  {
736
737
    switch(m_TestProfile)
      {
738
      case PROFIL_NAVEAU:
739
740
        generateEventOnLineWalking();
        break;
741
742
      case PROFIL_SIMPLE_NAVEAU:
	generateEventOnLineWalking();
743
744
      default:
        break;
745
      }
746
747
748
749
750
751
  }

};

int PerformTests(int argc, char *argv[])
{
752
#define NB_PROFILES 2
mnaveau's avatar
mnaveau committed
753
  std::string CompleteName = string(argv[0]);
754
  std::size_t found = CompleteName.find_last_of("/\\");
mnaveau's avatar
mnaveau committed
755
  std::string TestName =  CompleteName.substr(found+1);
756

757

758
759
760
761
762
763
764
  int TestProfiles[NB_PROFILES] =
    {
      PROFIL_NAVEAU,
      PROFIL_SIMPLE_NAVEAU
    };

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

766
767
768
769
770
771
772
773
774
775
776
777
778
  if (TestName.compare(14,6,"Online")==0)
    indexProfile=PROFIL_NAVEAU;
  if (TestName.compare(14,12,"OnlineSimple")==0)
    indexProfile=PROFIL_SIMPLE_NAVEAU;

  if (indexProfile==-1)
    {
      std::cerr << "CompleteName: " << CompleteName << std::endl;
      std::cerr<< " TestName: " << TestName <<std::endl;
      std::cerr<< "Failure to find the proper indexFile:" << TestName.substr(14,6) << endl;
      exit(-1);
    }
  else
779
    { ODEBUG3("Index detected: " << indexProfile);}
780

781
782
  TestNaveau2015 aTN2015(argc,argv,TestName,TestProfiles[indexProfile]);
  if(!aTN2015.init())
mnaveau's avatar
mnaveau committed
783
784
785
786
    {
      cout << "pb on init" << endl;
      return -1;
    }
787
  try
mnaveau's avatar
mnaveau committed
788
    {
789
      if (!aTN2015.doTest(std::cout)){
790
791
	cout << "Failed test " << indexProfile << endl;
	return -1;
792
793
      }
      else
794
	cout << "Passed test " << indexProfile << endl;
795
    }
796
797
798
  catch (const char * astr){
    cerr << "Failed on following error " << astr << std::endl;
    return -1; }
799

800
801
802
803
804
805
  return 0;
}

int main(int argc, char *argv[])
{
  try
806
807
808
    {
      return PerformTests(argc,argv);
    }
809
  catch (const std::string& msg)
810
811
812
    {
      std::cerr << msg << std::endl;
    }
mnaveau's avatar
mnaveau committed
813
  return 0;
814
}