modify the test to get another offline trajectory of the HerdtPG,

minor modification off output debug in the rest of the files

src/MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc

@@ -24,7 +24,6 @@ int MultiContactHirukawa::InverseKinematicsOnLimbs(std::vector< FootAbsolutePosi
                                                    std::vector< HandAbsolutePosition > & lh,
                                                    unsigned int currentIndex)
 {
-    cout << "q = \n" << q_ << endl ;
     //metapod::jac< Robot_Model>::run(*robot_, jacobian_);
     Jac_LF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_LF);
     Jac_RF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_RF);
@@ -136,7 +135,7 @@ int MultiContactHirukawa::online(vector<COMState> & comPos_,
                                  vector<HandAbsolutePosition> & lh_)
 {
     DetermineContact(rf_,lf_,rh_,lh_);
-//    InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
+    InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
 
     return 0 ;
 }
\ No newline at end of file

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp

@@ -597,7 +597,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
                                    RightFootTraj_deq_,
                                    deltaCOMTraj_deq_);
 
-//      #define DEBUG
+      #define DEBUG
       #ifdef DEBUG
         dynamicFilter_->Debug(FinalCOMTraj_deq,
                               FinalLeftFootTraj_deq,

tests/TestHerdt2010DynamicFilter.cpp

@@ -590,17 +590,17 @@ protected:
       aPGI.ParseCmd(strm2);
     }
   }
-  void walkForward(PatternGeneratorInterface &aPGI)
+  void walkForward1m_s(PatternGeneratorInterface &aPGI)
   {
     {
-      istringstream strm2(":setVelReference  0.2 0.0 0.0");
+      istringstream strm2(":setVelReference  0.1 0.0 0.0");
       aPGI.ParseCmd(strm2);
     }
   }
-  void walkForward1m_s(PatternGeneratorInterface &aPGI)
+  void walkForward2m_s(PatternGeneratorInterface &aPGI)
   {
     {
-      istringstream strm2(":setVelReference  0.1 0.0 0.0");
+      istringstream strm2(":setVelReference  0.2 0.0 0.0");
       aPGI.ParseCmd(strm2);
     }
   }
@@ -611,10 +611,10 @@ protected:
       aPGI.ParseCmd(strm2);
     }
   }
-  void walkSidewards3m_s(PatternGeneratorInterface &aPGI)
+  void walkSidewards1m_s(PatternGeneratorInterface &aPGI)
   {
     {
-      istringstream strm2(":setVelReference  0.0 -0.3 0.0");
+      istringstream strm2(":setVelReference  0.0 -0.1 0.0");
       aPGI.ParseCmd(strm2);
     }
   }
@@ -625,17 +625,10 @@ protected:
       aPGI.ParseCmd(strm2);
     }
   }
-  void walkSidewards1m_s(PatternGeneratorInterface &aPGI)
-  {
-    {
-      istringstream strm2(":setVelReference  0.0 -0.1 0.0");
-      aPGI.ParseCmd(strm2);
-    }
-  }
-  void walkSidewards(PatternGeneratorInterface &aPGI)
+  void walkSidewards3m_s(PatternGeneratorInterface &aPGI)
   {
     {
-      istringstream strm2(":setVelReference  0.0 -0.2 0.0");
+      istringstream strm2(":setVelReference  0.0 -0.3 0.0");
       aPGI.ParseCmd(strm2);
     }
   }
@@ -700,12 +693,15 @@ protected:
     };
 #define localNbOfEvents 12
     struct localEvent events [localNbOfEvents] =
-    { { 5*200,&TestHerdt2010::startWalkInDiagonal3m_s},
-      {10*200,&TestHerdt2010::startWalkInDiagonal1m_s},
-      {15*200,&TestHerdt2010::walkSidewards1m_s},
-      {20*200,&TestHerdt2010::walkSidewards2m_s},
-      {25*200,&TestHerdt2010::stop},
-      {30*200,&TestHerdt2010::stopOnLineWalking}};
+    { { 5*200,&TestHerdt2010::walkForward3m_s},
+      {10*200,&TestHerdt2010::walkForward2m_s},
+      {15*200,&TestHerdt2010::walkForward1m_s},
+      {20*200,&TestHerdt2010::walkSidewards1m_s},
+      {25*200,&TestHerdt2010::walkSidewards2m_s},
+      {30*200,&TestHerdt2010::walkSidewards3m_s},
+      {35*200,&TestHerdt2010::startTurningRightOnSpot},
+      {40*200,&TestHerdt2010::stop},
+      {45*200,&TestHerdt2010::stopOnLineWalking}};
     // Test when triggering event.
     for(unsigned int i=0;i<localNbOfEvents;i++)
       {

tests/TestHirukawa2007.cpp

@@ -127,8 +127,8 @@ public:
 
     bool doTest(ostream &os)
     {
-        metapod::bcalc<Robot_Model>::run(robot_,q_init_);
         metapod::jcalc<Robot_Model>::run(robot_,q_init_,Robot_Model::confVector::Zero());
+        metapod::bcalc<Robot_Model>::run(robot_,q_init_);
 
         ofstream aof;
         string aFileName;