diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index dd4c0bbb1ca029ab409199a6e32582cffeed1fc0..57fbc53a4a5535871ec5b6376b633eaa64aa55c4 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -231,7 +231,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
AccelerationTraj_.resize( QP_N_ * NbSampleInterpolation_ );
DeltaZMPMBPositions_.resize ( QP_N_ * NbSampleInterpolation_ );
-
+ ZMPMBTraj_deq_.resize( QP_N_ * NbSampleInterpolation_, vector<double>(2) );
// Initialization of the configuration vectors
PreviousConfiguration_ = aHS->currentConfiguration() ;
PreviousVelocity_ = aHS->currentVelocity();
@@ -581,6 +581,89 @@ ZMPVelocityReferencedQP::OnLine(double time,
// --------------
DynamicFilter( time, FinalCOMTraj_deq );
+
+
+
+
+
+
+
+
+
+
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0 ;
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010footcom");
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int i = 0 ; i < NbSampleInterpolation_ * QP_N_ ; ++i )
+ {
+ aof << filterprecision( i * InterpolationPeriod_ ) << " " // 0
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].x[0] ) << " " // 1
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].y[0] ) << " " // 2
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].z[0] ) << " " // 3
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].x[1] ) << " " // 4
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].y[1] ) << " " // 5
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].z[1] ) << " " // 6
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].x[2] ) << " " // 7
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].y[2] ) << " " // 8
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].z[2] ) << " " // 9
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].yaw[0] ) << " " // 10
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].yaw[1] ) << " " // 11
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].yaw[2] ) << " " // 12
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].x ) << " " // 13
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].y ) << " " // 14
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].z ) << " " // 15
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].theta * M_PI / 180 ) << " " // 16
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].omega * M_PI / 180 ) << " " // 17
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].x ) << " " //18
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].y ) << " " //19
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].z ) << " " //20
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].theta * M_PI / 180 ) << " " //21
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].omega * M_PI / 180 ) << " " //22
+ << filterprecision( solution_.Solution_vec[i/NbSampleInterpolation_] ) << " " //23
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[0] ) << " " // 24
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[1] ) << " " // 25
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].roll[2] ) << " " // 26
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[0] ) << " " // 27
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[1] ) << " " // 28
+ << filterprecision( COMTraj_deq_[CurrentIndex_+i].pitch[2] ) << " " // 29
+ << endl ;
+ }
+ aof.close() ;
+
+ iteration++;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
// Specify that we are in the ending phase.
if (EndingPhase_ == false)
{
@@ -848,8 +931,12 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
DInitY_ = ZMPTraj_deq_[CurrentIndex_].py - ( m_force.n()[0] / m_force.f()[2] ) ;
Once_ = false ;
}
- DeltaZMPMBPositions_[i].px = ZMPTraj_deq_[CurrentIndex_+i].px - ( - m_force.n()[1] / m_force.f()[2] ) - DInitX_ ;
- DeltaZMPMBPositions_[i].py = ZMPTraj_deq_[CurrentIndex_+i].py - ( m_force.n()[0] / m_force.f()[2] ) - DInitY_ ;
+
+ ZMPMBTraj_deq_[i][0] = - m_force.n()[1] / m_force.f()[2] + DInitX_ ;
+ ZMPMBTraj_deq_[i][1] = m_force.n()[0] / m_force.f()[2] + DInitY_ ;
+
+ DeltaZMPMBPositions_[i].px = ZMPTraj_deq_[CurrentIndex_+i].px - ZMPMBTraj_deq_[i][0] ;
+ DeltaZMPMBPositions_[i].py = ZMPTraj_deq_[CurrentIndex_+i].py - ZMPMBTraj_deq_[i][1] ;
DeltaZMPMBPositions_[i].pz = 0.0 ;
DeltaZMPMBPositions_[i].theta = 0.0 ;
DeltaZMPMBPositions_[i].time = time + i * m_SamplingPeriod ;
@@ -915,6 +1002,116 @@ void ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> &
}
}
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ static int iteration = 0;
+ int iteration100 = (int)iteration/100;
+ int iteration10 = (int)(iteration - iteration100*100)/10;
+ int iteration1 = (int)(iteration - iteration100*100 - iteration10*10 );
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010DynamicDZMP");
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int i = 0 ; i < ZMPMBTraj_deq_.size() ; ++i )
+ {
+ aof << filterprecision( ZMPMBTraj_deq_[i][0] ) << " " // 1
+ << filterprecision( ZMPMBTraj_deq_[i][1] ) << " " // 2
+ << filterprecision( ZMPTraj_deq_[CurrentIndex_+i].px ) << " " // 3
+ << filterprecision( ZMPTraj_deq_[CurrentIndex_+i].py ) << " " // 4
+ << endl ;
+ }
+ aof.close();
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010DynamicDCOM");
+ oss << "_" << iteration100 << iteration10 << iteration1 << ".dat";
+ aFileName = oss.str();
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int i = 0 ; i < ComStateBuffer_.size() ; ++i )
+ {
+ aof << filterprecision( i ) << " " // 1
+ << filterprecision( ComStateBuffer_[i].x[0] ) << " " // 2
+ << filterprecision( ComStateBuffer_[i].y[0] ) << " " // 3
+ << filterprecision( ComStateBuffer_[i].x[1] ) << " " // 4
+ << filterprecision( ComStateBuffer_[i].y[1] ) << " " // 5
+ << filterprecision( ComStateBuffer_[i].x[2] ) << " " // 6
+ << filterprecision( ComStateBuffer_[i].y[2] ) << " " // 7
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[0]-ComStateBuffer_[i].x[0] ) << " " // 8
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[0]-ComStateBuffer_[i].y[0] ) << " " // 9
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[1]-ComStateBuffer_[i].x[1] ) << " " // 10
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[1]-ComStateBuffer_[i].y[1] ) << " " // 11
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[2]-ComStateBuffer_[i].x[2] ) << " " // 12
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[2]-ComStateBuffer_[i].y[2] ) << " " // 13
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[0] ) << " " // 14
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[0] ) << " " // 15
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[1] ) << " " // 16
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[1] ) << " " // 17
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[2] ) << " " // 18
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[2] ) << " " // 19
+ << endl ;
+ }
+ aof.close();
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010DynamicCOMXY.dat");
+ aFileName = oss.str();
+ if(iteration == 0)
+ {
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int i = 0 ; i < NbSampleControl_ ; ++i )
+ {
+ aof << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].x[0] ) << " " // 1
+ << filterprecision( FinalCOMTraj_deq[CurrentIndex_+i].y[0] ) << " " // 2
+ << endl ;
+ }
+ aof.close();
+
+ /// \brief Debug Purpose
+ /// --------------------
+ oss.str("TestHerdt2010DynamicZMPMB.dat");
+ aFileName = oss.str();
+ if(iteration == 0)
+ {
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int i = 0 ; i < NbSampleInterpolation_ ; ++i )
+ {
+ aof << filterprecision( ZMPMBTraj_deq_[i][0] ) << " " // 1
+ << filterprecision( ZMPMBTraj_deq_[i][1] ) << " " // 2
+ << endl ;
+ }
+ aof.close();
+
+ ++iteration;
return ;
}
@@ -985,19 +1182,19 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
std::deque<COMState> & COMTraj_deq , // OUTPUT
const std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
const std::deque<FootAbsolutePosition> & RightFootTraj_deq,// INPUT
- const solution_t * Solution, // INPUT
+ const solution_t * aSolutionReference, // INPUT
LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
const unsigned numberOfSample, // INPUT
const int IterationNumber) // INPUT
{
- if(Solution->SupportStates_deq.size() && Solution->SupportStates_deq[IterationNumber].NbStepsLeft == 0)
+ if(aSolutionReference->SupportStates_deq.size() && aSolutionReference->SupportStates_deq[IterationNumber].NbStepsLeft == 0)
{
double jx = (LeftFootTraj_deq[0].x + RightFootTraj_deq[0].x)/2 - LIPM->GetState().x[0];
double jy = (LeftFootTraj_deq[0].y + RightFootTraj_deq[0].y)/2 - LIPM->GetState().y[0];
if(fabs(jx) < 1e-3 && fabs(jy) < 1e-3) { Running_ = false; }
const double tf = 0.75;
- jx = 6/(tf*tf*tf)*(jx - tf*LIPM->GetState().x[1] - (tf*tf/2)*LIPM->GetState().x[2]);
- jy = 6/(tf*tf*tf)*(jy - tf*LIPM->GetState().y[1] - (tf*tf/2)*LIPM->GetState().y[2]);
+ jx = 6/(tf*tf*tf)*(jx - tf*COMTraj_deq[0].x[1] - (tf*tf/2)*COMTraj_deq[0].x[2]);
+ jy = 6/(tf*tf*tf)*(jy - tf*COMTraj_deq[0].y[1] - (tf*tf/2)*COMTraj_deq[0].y[2]);
LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
jx, jy);
LIPM->OneIteration( jx, jy );
@@ -1006,8 +1203,8 @@ void ZMPVelocityReferencedQP::CoMZMPInterpolation(
{
Running_ = true;
LIPM->Interpolation( COMTraj_deq, ZMPPositions, CurrentIndex_ + IterationNumber * numberOfSample,
- Solution->Solution_vec[IterationNumber], Solution->Solution_vec[IterationNumber+QP_N_] );
- LIPM->OneIteration( Solution->Solution_vec[IterationNumber],Solution->Solution_vec[IterationNumber+QP_N_] );
+ aSolutionReference->Solution_vec[IterationNumber], aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
+ LIPM->OneIteration( aSolutionReference->Solution_vec[IterationNumber],aSolutionReference->Solution_vec[IterationNumber+QP_N_] );
}
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 35604a77e5e672feb8a20b3e721c223235ac85f4..a90c17b58a7642fb3d479afd60a0c3b8acf8764c 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -244,6 +244,7 @@ namespace PatternGeneratorJRL
/// \brief Buffers for the Kajita's dynamic filter
deque<ZMPPosition> ZMPTraj_deq_ ;
+ deque< vector<double> >ZMPMBTraj_deq_ ;
deque<COMState> COMTraj_deq_ ;
deque<FootAbsolutePosition> LeftFootTraj_deq_ ;
deque<FootAbsolutePosition> RightFootTraj_deq_ ;
diff --git a/tests/CommonTools.hh b/tests/CommonTools.hh
index aa6479f8f3d28fb32379f38aed8fb3049b31daab..91d7120e85b8ec6d9192b0740a563477d104df71 100644
--- a/tests/CommonTools.hh
+++ b/tests/CommonTools.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* 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
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
#ifdef UNIX
@@ -56,25 +56,25 @@ namespace PatternGeneratorJRL
{
namespace TestSuite
{
- void getOptions(int , char *[],
+ void getOptions(int , char *[],
std::string &VRMLPath,
std::string &VRMLFileName,
std::string &SpecificitiesFileName,
std::string &LinkJointRank,
std::string &InitConfig,
unsigned int &TestProfil);
-
+
void CommonInitialization(PatternGeneratorJRL::PatternGeneratorInterface &aPGI);
-
+
/*! \brief Structure to handle information related to one step of each algorithm */
struct OneStep
{
- COMPosition finalCOMPosition;
+ COMState finalCOMPosition;
FootAbsolutePosition LeftFootPosition;
FootAbsolutePosition RightFootPosition;
MAL_VECTOR(ZMPTarget,double);
unsigned long int NbOfIt;
-
+
OneStep()
{
MAL_VECTOR_RESIZE(ZMPTarget,3);
@@ -83,7 +83,7 @@ namespace PatternGeneratorJRL
memset(&RightFootPosition,0,sizeof(RightFootPosition));
memset(&finalCOMPosition,0,sizeof(finalCOMPosition));
}
- };
+ };
} /* end of TestSuite namespace */
} /* end of PatternGeneratorJRL namespace */
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index 81204657b28f60864ad23b42c4d91ee05097f3d4..4b1ca78ee922fcc24125f13933ed0b0192b29f96 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -242,15 +242,15 @@ protected:
void initIK()
{
- MAL_VECTOR_DIM(BodyAngles,double,(m_HDR->numberDof()-6));
+ MAL_VECTOR_DIM(BodyAngles,double,MAL_VECTOR_SIZE(InitialPosition));
MAL_VECTOR_DIM(waist,double,6);
for (int i = 0 ; i < 6 ; ++i )
{
- waist(i) = m_PreviousConfiguration(i);
+ waist(i) = 0;
}
for (unsigned int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
{
- BodyAngles(i) = m_PreviousConfiguration(i+6);
+ BodyAngles(i) = InitialPosition(i);
}
MAL_S3_VECTOR(lStartingCOMState,double);
@@ -282,7 +282,7 @@ protected:
<< 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 ) << " " // 5
+ << 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
@@ -373,7 +373,7 @@ protected:
aof << filterprecision( iteration * 0.1 ) << " " ; // 1
aof << filterprecision( 0.0 ) << " " ; // 1
aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
aof << endl ;
}
aof.close();
@@ -401,6 +401,7 @@ protected:
void ComparingZMPs()
{
+ const int stage0 = 0 ;
/// \brief calculate, from the CoM of computed by the preview control,
/// the corresponding articular position, velocity and acceleration
/// ------------------------------------------------------------------
@@ -421,9 +422,9 @@ protected:
aCOMState(0) = m_OneStep.finalCOMPosition.x[0]; aCOMSpeed(0) = m_OneStep.finalCOMPosition.x[1]; aCOMAcc(0) = m_OneStep.finalCOMPosition.x[2];
aCOMState(1) = m_OneStep.finalCOMPosition.y[0]; aCOMSpeed(1) = m_OneStep.finalCOMPosition.y[1]; aCOMAcc(1) = m_OneStep.finalCOMPosition.y[2];
aCOMState(2) = m_OneStep.finalCOMPosition.z[0]; aCOMSpeed(2) = m_OneStep.finalCOMPosition.z[1]; aCOMAcc(2) = m_OneStep.finalCOMPosition.z[2];
- aCOMState(3) = m_OneStep.finalCOMPosition.roll; aCOMSpeed(3) = aCOMAcc(3) = 0 ;
- aCOMState(4) = m_OneStep.finalCOMPosition.pitch; aCOMSpeed(4) = aCOMAcc(4) = 0 ;
- aCOMState(5) = m_OneStep.finalCOMPosition.yaw; aCOMSpeed(5) = aCOMAcc(5) = 0 ;
+ aCOMState(3) = m_OneStep.finalCOMPosition.roll[0]; aCOMSpeed(3) = m_OneStep.finalCOMPosition.roll[1]; aCOMAcc(3) = m_OneStep.finalCOMPosition.roll[2];
+ aCOMState(4) = m_OneStep.finalCOMPosition.pitch[0]; aCOMSpeed(4) = m_OneStep.finalCOMPosition.pitch[1]; aCOMAcc(4) = m_OneStep.finalCOMPosition.pitch[2];
+ aCOMState(5) = m_OneStep.finalCOMPosition.yaw[0]; aCOMSpeed(5) = m_OneStep.finalCOMPosition.yaw[1]; aCOMAcc(5) = m_OneStep.finalCOMPosition.yaw[2];
aLeftFootPosition(0) = m_OneStep.LeftFootPosition.x; aRightFootPosition(0) = m_OneStep.RightFootPosition.x;
aLeftFootPosition(1) = m_OneStep.LeftFootPosition.y; aRightFootPosition(1) = m_OneStep.RightFootPosition.y;
@@ -438,7 +439,38 @@ protected:
CurrentVelocity,
CurrentAcceleration,
m_OneStep.NbOfIt,
- 0);
+ stage0);
+
+ /// \brief Debug Purpose
+ /// --------------------
+ ofstream aof;
+ string aFileName;
+ ostringstream oss(std::ostringstream::ate);
+ oss.str("TestHerdt2010DynamicART2.dat");
+ aFileName = oss.str();
+ if ( iteration_zmp == 0 )
+ {
+ aof.open(aFileName.c_str(),ofstream::out);
+ aof.close();
+ }
+ ///----
+ aof.open(aFileName.c_str(),ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ for (unsigned int j = 0 ; j < CurrentConfiguration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentConfiguration(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentVelocity.size() ; ++j)
+ {
+ aof << filterprecision(CurrentVelocity(j)) << " " ;
+ }
+ for (unsigned int j = 0 ; j < CurrentAcceleration.size() ; ++j)
+ {
+ aof << filterprecision(CurrentAcceleration(j)) << " " ;
+ }
+ aof << endl ;
+
/// \brief rnea, calculation of the multi body ZMP
/// ----------------------------------------------
@@ -495,7 +527,6 @@ protected:
// cout << "ecartmaxY :" << ecartmaxY << endl ;
// Writing of the two zmps and the error.
- ofstream aof;
if (ONCE)
{
aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
diff --git a/tests/TestKajita2003.cpp b/tests/TestKajita2003.cpp
index d21b5c4da5fcdbc35cd9ddfc38f96f36286a6261..6081a90b02c63ea6e7e9940d6523a74b5357782d 100644
--- a/tests/TestKajita2003.cpp
+++ b/tests/TestKajita2003.cpp
@@ -93,7 +93,7 @@ protected:
<< 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 ) << " " // 5
+ << 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
@@ -184,7 +184,7 @@ protected:
aof << filterprecision( iteration * 0.5 ) << " " ; // 1
aof << filterprecision( 0.0 ) << " " ; // 1
aof << filterprecision( 0.0 ) << " " ; // 1
- aof << filterprecision( m_OneStep.finalCOMPosition.yaw ) << " " ; // 1
+ aof << filterprecision( m_OneStep.finalCOMPosition.yaw[0] ) << " " ; // 1
aof << endl ;
}
aof.close();
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 21f440e4f4735103802c73664b151382232c42a5..fb8197b3108b4c878dbcc9359873531f9bac5f66 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -208,7 +208,7 @@ namespace PatternGeneratorJRL
// This is a vector corresponding to the DOFs actuated of the robot.
- MAL_VECTOR_DIM(InitialPosition,double,lNbActuatedJoints);
+ MAL_VECTOR_RESIZE(InitialPosition,lNbActuatedJoints);
//MAL_VECTOR_DIM(CurrentPosition,double,40);
if (conversiontoradneeded)
for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
@@ -345,7 +345,7 @@ namespace PatternGeneratorJRL
<< 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 ) << " " // 5
+ << 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
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index 04d89e05cd59713bb67c2397154b21eef45f219b..a0c55240cf4a3ec74d62c8a2960d81cc0e4d8782 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -1,5 +1,5 @@
/*
- * Copyright 2010,
+ * Copyright 2010,
*
* Olivier Stasse
*
@@ -18,7 +18,7 @@
* 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
+ * Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file Abstract Object test aim at testing various walking algorithms */
@@ -60,13 +60,13 @@ namespace PatternGeneratorJRL
public:
/*! \brief Constructor for the test named TestName.
All generated files will have their names prefixed by TestName*/
- TestObject(int argc, char *argv[],
+ TestObject(int argc, char *argv[],
std::string &TestName,
int lPGIInterface=0);
/*! \name Destructor */
~TestObject();
-
+
/*! \brief Initialize the test object. */
void init();
@@ -76,19 +76,19 @@ namespace PatternGeneratorJRL
/*! \brief Decide from which object the robot is build from. */
virtual void SpecializedRobotConstructor(CjrlHumanoidDynamicRobot *& aHDR,
CjrlHumanoidDynamicRobot *& aDebugHDR );
-
+
protected:
/*! \brief Choose which test to perform. */
virtual void chooseTestProfile()=0;
-
+
/*! \brief Generate events. */
virtual void generateEvent()=0;
/*! \brief Profile of the test to perform. */
unsigned int m_TestProfile;
- /*! \brief Useful methods to create the robot model.
+ /*! \brief Useful methods to create the robot model.
@{
*/
/*! */
@@ -99,9 +99,9 @@ namespace PatternGeneratorJRL
CjrlHumanoidDynamicRobot *& aHDR,
CjrlHumanoidDynamicRobot *& aDebugHDR,
PatternGeneratorJRL::PatternGeneratorInterface *&aPGI);
-
+
/*! @} */
-
+
/*! \name Vectors storing the robot's state.
@{
*/
@@ -119,10 +119,13 @@ namespace PatternGeneratorJRL
/*! \brief Previous velocity */
MAL_VECTOR(m_PreviousVelocity,double);
-
+
/*! \brief Previous acceleration */
MAL_VECTOR(m_PreviousAcceleration,double);
+ /*! \brief Initial Configuration */
+ MAL_VECTOR(InitialPosition,double);
+
/*! @} */
/* !\name Handle on the Humanoids models
@@ -139,22 +142,22 @@ namespace PatternGeneratorJRL
/*! \brief Pointer towards the Pattern Generator Interface */
PatternGeneratorInterface * m_PGI;
- /*! \name Debugging information
+ /*! \name Debugging information
@{
*/
- /*! \brief ZMP of the multibody robot.
+ /*! \brief ZMP of the multibody robot.
This flag makes sense only for algorithm allowing to compute
the whole robot articular trajectories.
*/
bool m_DebugZMP2;
-
+
/*! \brief Output Com, ZMP and feet trajectories
for a single mass robot model. */
bool m_DebugFGPI;
/*! \brief Reset debug files according to flags. */
void prepareDebugFiles();
-
+
/*! \brief Fill in the debug files with the appropriate
information */
void fillInDebugFiles();
@@ -170,13 +173,13 @@ namespace PatternGeneratorJRL
/*! \brief Name of the test */
std::string m_TestName;
- /*! \brief Clock CPU timing
+ /*! \brief Clock CPU timing
This object measure three parts of the algorithm:
off-line, on-line, and during modification.
*/
ClockCPUTime m_clock;
- /*! \brief Number of maximum iterations for outer loop.
+ /*! \brief Number of maximum iterations for outer loop.
Default value is set to 1.
*/
unsigned int m_OuterLoopNbItMax;
@@ -184,7 +187,7 @@ namespace PatternGeneratorJRL
/*! \brief Patten Generator Interface. */
int m_PGIInterface;
- /*! \brief Store options
+ /*! \brief Store options
@{*/
/*! \brief Path to the VRML. */
std::string m_VRMLPath;
@@ -201,7 +204,7 @@ namespace PatternGeneratorJRL
/*! @} */
};
-
+
} /* end of TestSuite namespace */
} /* end of PatternGeneratorJRL namespace */
#endif /* _TEST_OBJECT_PATTERN_GENERATOR_UTESTING_H_*/