diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 150b9c33bd9705f7dc8914fcc394a9c76b8ef45c..4f451380ac235ddb842a47878d903ee979d4aa61 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -83,6 +83,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
QP_T_ = 0.1;
QP_N_ = 16;
m_SamplingPeriod = 0.005;
+ m_ControlPeriod = 0.005 ;
PerturbationOccured_ = false;
UpperTimeLimitToUpdate_ = 0.0;
RobotMass_ = aHS->mass();
@@ -171,8 +172,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
MAL_MATRIX_RESIZE(m_deltax,3,1); MAL_MATRIX_RESIZE(m_deltay,3,1);
m_PC = new PreviewControl(SPM,
OptimalControllerSolver::MODE_WITH_INITIALPOS,
- true);
- m_PC->SetPreviewControlTime (QP_T_*(QP_N_-1));
+ false);
+ m_PC->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/m_SamplingPeriod*m_ControlPeriod);
m_PC->SetSamplingPeriod (m_SamplingPeriod);
m_PC->SetHeightOfCoM(0.814);
@@ -200,6 +201,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
m_QP_T_previousVelocity = aHS->currentVelocity();
m_QP_T_previousAcceleration = aHS->currentAcceleration();
+ m_comStateBuffer.resize(m_numberOfSample);
+
m_once = true ;
m_dInitX = 0 ;
m_dInitY = 0 ;
@@ -501,9 +504,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
Problem_.dump( time );
}
- static int iteration = 0 ;
- Solution_.print(cout);
-
// INTERPOLATE THE NEXT COMPUTED COM STATE:
// ----------------------------------------
m_currentIndex = FinalCOMTraj_deq.size();
@@ -556,7 +556,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
Robot_->generate_trajectories( time, m_solution,
m_solution.SupportStates_deq, m_solution.SupportOrientations_deq,
m_LeftFootTraj_deq, m_RightFootTraj_deq );
- cout << "support X , Y = " << m_solution.SupportStates_deq.front().X << ", " << m_solution.SupportStates_deq.front().Y << endl ;
m_solution.SupportStates_deq.pop_front();
for ( int i = 1 ; i < QP_N_ ; i++ ){
OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_, m_currentIndex + i * m_numberOfSample,
@@ -565,52 +564,9 @@ ZMPVelocityReferencedQP::OnLine(double time,
Robot_->generate_trajectories( time + i * QP_T_, m_solution,
m_solution.SupportStates_deq, m_solution.SupportOrientations_deq,
m_LeftFootTraj_deq, m_RightFootTraj_deq );
- cout << "support X , Y = " << m_solution.SupportStates_deq.front().X << ", " << m_solution.SupportStates_deq.front().Y << endl ;
m_solution.SupportStates_deq.pop_front();
}
-
- /// \brief Debug Purpose
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
-
- 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("TestHerdt2010DynamicBuffers");
- 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 < m_currentIndex + QP_N_ * m_numberOfSample ; i++){
- aof << m_COMTraj_deq[i].roll[0] << " " // 1
- << m_COMTraj_deq[i].pitch[0] << " " // 2
- << m_COMTraj_deq[i].yaw[0] << " " // 3
- << m_COMTraj_deq[i].x[0] << " " // 4
- << m_COMTraj_deq[i].y[0] << " " // 5
- << m_ZMPTraj_deq[i].px << " " // 6
- << m_ZMPTraj_deq[i].py << " " // 7
- << m_LeftFootTraj_deq[i].theta *M_PI/180 << " " // 8
- << m_RightFootTraj_deq[i].theta *M_PI/180 << " " // 9
- << m_LeftFootTraj_deq[i].x << " " // 10
- << m_RightFootTraj_deq[i].x << " " // 11
- << m_LeftFootTraj_deq[i].y << " " // 12
- << m_RightFootTraj_deq[i].y << " " // 13
- << m_LeftFootTraj_deq[i].z << " " // 14
- << m_RightFootTraj_deq[i].z << " " // 15
- << endl ;
- }
- aof.close();
-
FinalZMPTraj_deq.resize( m_numberOfSample + m_currentIndex );
FinalLeftFootTraj_deq.resize( m_numberOfSample + m_currentIndex );
FinalRightFootTraj_deq.resize( m_numberOfSample + m_currentIndex );
@@ -629,7 +585,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
CoM_.setState(m_COMTraj_deq[m_numberOfSample + m_currentIndex - 1]);
OrientPrw_->CurrentTrunkState(m_COMTraj_deq[m_numberOfSample + m_currentIndex - 1]);
-
// RECOPIE DU BUFFER
// -----------------
FinalCOMTraj_deq.resize( m_numberOfSample + m_currentIndex );
@@ -638,72 +593,6 @@ ZMPVelocityReferencedQP::OnLine(double time,
FinalCOMTraj_deq[i] = m_COMTraj_deq[i] ;
}
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010DynamicFinalBuffers");
- 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 < FinalZMPTraj_deq.size() ; i++){
- aof << FinalCOMTraj_deq[i].roll[0] << " " // 1
- << FinalCOMTraj_deq[i].pitch[0] << " " // 2
- << FinalCOMTraj_deq[i].yaw[0] << " " // 3
- << FinalCOMTraj_deq[i].x[0] << " " // 4
- << FinalCOMTraj_deq[i].y[0] << " " // 5
- << FinalZMPTraj_deq[i].px << " " // 6
- << FinalZMPTraj_deq[i].py << " " // 7
- << FinalLeftFootTraj_deq[i].theta *M_PI/180 << " " // 8
- << FinalRightFootTraj_deq[i].theta *M_PI/180 << " " // 9
- << FinalLeftFootTraj_deq[i].x << " " // 10
- << FinalRightFootTraj_deq[i].x << " " // 11
- << FinalLeftFootTraj_deq[i].y << " " // 12
- << FinalRightFootTraj_deq[i].y << " " // 13
- << FinalLeftFootTraj_deq[i].z << " " // 14
- << FinalRightFootTraj_deq[i].z << " " // 15
- << endl ;
- }
- aof.close();
-
- /// \brief Debug Purpose
- /// --------------------
- if ( iteration == 0 )
- {
- oss.str("TestHerdt2010Orientation.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("TestHerdt2010Orientation.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << iteration*0.1 << " " // 1
- << FinalLeftFootTraj_deq[0].theta *M_PI/180 << " " // 2
- << FinalRightFootTraj_deq[0].theta *M_PI/180 << " " // 3
- << FinalCOMTraj_deq[0].roll[0] << " " // 4
- << FinalCOMTraj_deq[0].pitch[0] << " " // 5
- << FinalCOMTraj_deq[0].yaw[0] << " " // 6
- << FinalCOMTraj_deq[0].x[0] << " " // 7
- << FinalCOMTraj_deq[0].y[0] << " " // 8
- << FinalZMPTraj_deq[0].px << " " // 9
- << FinalZMPTraj_deq[0].py << " " // 10
- << filterprecision( m_LeftFootTraj_deq[0].x ) << " " // 11
- << filterprecision( m_LeftFootTraj_deq[0].y ) << " " // 12
- << filterprecision( m_RightFootTraj_deq[0].x ) << " " // 13
- << filterprecision( m_RightFootTraj_deq[0].y ) << " " // 14
-
- << endl ;
- aof.close();
-
- iteration++;
-
// Specify that we are in the ending phase.
if (EndingPhase_ == false)
{
@@ -779,17 +668,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
double time
)
{
-
- /// \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 );
-
const unsigned int N = m_numberOfSample * QP_N_ ;
// \brief calculate, from the CoM computed by the preview control,
// the corresponding articular position, velocity and acceleration
@@ -805,22 +683,8 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
i);
}
- // \brief rnea, calculation of the multi body ZMP
- // ----------------------------------------------
- double zmpmbX, zmpmbY;
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010ZMPMB");
- 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 < N ; i++ ){
+ for (unsigned int i = 0 ; i < N ; i++ )
+ {
// Apply the RNEA to the metapod multibody and print the result in a log file.
for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
{
@@ -843,27 +707,12 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
m_deltaZMPMBPositions[i].theta = 0.0 ;
m_deltaZMPMBPositions[i].time = time + i * m_SamplingPeriod ;
m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
-
- if ( i == 0 ){
- zmpmbX = - m_force.n()[1] / m_force.f()[2] ;
- zmpmbY = m_force.n()[0] / m_force.f()[2] ;
- }
- aof << filterprecision( - m_force.n()[1] / m_force.f()[2] ) << " " // 1
- << filterprecision( m_force.n()[0] / m_force.f()[2] ) << " " // 2
- << filterprecision( ZMPPositions[currentIndex+i].px ) << " " // 3
- << filterprecision( ZMPPositions[currentIndex+i].py ) << " " // 4
- << filterprecision( - m_force.n()[1] / m_force.f()[2] + m_dInitX) << " " // 5
- << filterprecision( m_force.n()[0] / m_force.f()[2] + m_dInitY) << " " // 6
- << endl ;
-
-
}
- aof.close();
/// Preview control on the ZMPMBs computed
/// --------------------------------------
//init of the Kajita preview control
- m_PC->SetPreviewControlTime (QP_T_*QP_N_ - 20*m_SamplingPeriod);
+ m_PC->SetPreviewControlTime (QP_T_*QP_N_ - QP_T_/m_SamplingPeriod*m_ControlPeriod);
m_PC->SetSamplingPeriod (m_SamplingPeriod);
m_PC->SetHeightOfCoM(0.814);
m_PC->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
@@ -874,7 +723,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
}
double aSxzmp (0) , aSyzmp(0);
double deltaZMPx (0) , deltaZMPy (0) ;
- std::deque<COMState> COMStateBuffer (m_numberOfSample);
// calcul of the preview control along the "ZMPPositions"
for (unsigned i = 0 ; i < m_numberOfSample ; i++ )
{
@@ -884,80 +732,21 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> &ZMPPositions
deltaZMPx, deltaZMPy, false);
for(int j=0;j<3;j++)
{
- COMStateBuffer[i].x[j] = m_deltax(j,0);
- COMStateBuffer[i].y[j] = m_deltay(j,0);
+ m_comStateBuffer[i].x[j] = m_deltax(j,0);
+ m_comStateBuffer[i].y[j] = m_deltay(j,0);
}
}
-
-
for (unsigned int i = 0 ; i < m_numberOfSample ; i++)
{
for(int j=0;j<3;j++)
{
- if ( COMStateBuffer[i].x[j] == COMStateBuffer[i].x[j] )
- COMTraj_deq[currentIndex+i].x[j] += COMStateBuffer[i].x[j] ;
- else
- cout << "PC problem nan in : x[" << j << "] and index : " << i << endl ;
- if ( COMStateBuffer[i].y[j] == COMStateBuffer[i].y[j] )
- COMTraj_deq[currentIndex+i].y[j] += COMStateBuffer[i].y[j] ;
- else
- cout << "PC problem nan in : y[" << j << "] and index : " << i << endl ;
+ if ( m_comStateBuffer[i].x[j] == m_comStateBuffer[i].x[j] )
+ COMTraj_deq[currentIndex+i].x[j] += m_comStateBuffer[i].x[j] ;
+ if ( m_comStateBuffer[i].y[j] == m_comStateBuffer[i].y[j] )
+ COMTraj_deq[currentIndex+i].y[j] += m_comStateBuffer[i].y[j] ;
}
}
-
- /// \brief Debug Purpose
- /// --------------------
- oss.str("TestHerdt2010blabla");
- 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( COMStateBuffer[i].x[0] ) << " " // 1
- << filterprecision( COMStateBuffer[i].x[1] ) << " " // 1
- << filterprecision( COMStateBuffer[i].x[2] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[0] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[1] ) << " " // 1
- << filterprecision( COMStateBuffer[i].y[2] ) << " " // 1
- << endl ;
- }
- aof.close();
-
- /// \brief Debug Purpose
- /// --------------------
- if ( iteration == 0 )
- {
- oss.str("TestHerdt2010ZMPMB.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- }
- ///----
- oss.str("TestHerdt2010ZMPMB.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision( zmpmbX ) << " " // 1
- << filterprecision( zmpmbY ) << " " // 2
- << filterprecision( ZMPPositions[currentIndex].px ) << " " // 3
- << filterprecision( ZMPPositions[currentIndex].py ) << " " // 4
- << filterprecision( zmpmbX + m_dInitX ) << " " // 5
- << filterprecision( zmpmbY + m_dInitY ) << " " // 6
- << filterprecision( COMStateBuffer[currentIndex].x[1] ) << " " // 7
- << filterprecision( COMStateBuffer[currentIndex].y[1] ) << " " // 8
- << filterprecision( m_deltaZMPMBPositions[0].px ) << " " // 9
- << filterprecision( m_deltaZMPMBPositions[0].py ) << " " // 10
- << endl ;
- aof.close();
-
- iteration++;
-
return 0;
}
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 4ede3a567c4bbdc30cbe4498224b87c220a63d76..764b9bebca279f5f1da4d65c0a1960aefc6d2e01 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -145,9 +145,12 @@ namespace PatternGeneratorJRL
{ ComAndFootRealization_ = aCFR; return true;};
inline ComAndFootRealization * getComAndFootRealization()
{ return ComAndFootRealization_;};
-
/// \}
+ inline double ControlPeriod ()
+ { return m_ControlPeriod ; }
+ inline void ControlPeriod ( double period )
+ { m_ControlPeriod = period ; }
//
@@ -237,10 +240,15 @@ namespace PatternGeneratorJRL
deque<FootAbsolutePosition> m_LeftFootTraj_deq ;
deque<FootAbsolutePosition> m_RightFootTraj_deq ;
-
+ /// \brief Index where to begin the interpolation
unsigned m_currentIndex ;
+
+ /// \brief Copy of the QP_ solution
solution_t m_solution ;
+ /// \brief Control Period of the robot
+ double m_ControlPeriod ;
+
/// \brief Number of interpolated point computed during QP_T_ (27/02/2014 :0.1)
unsigned m_numberOfSample ;
@@ -254,6 +262,7 @@ namespace PatternGeneratorJRL
MAL_VECTOR_TYPE(double) m_QP_T_Configuration ;
MAL_VECTOR_TYPE(double) m_QP_T_previousVelocity ;
MAL_VECTOR_TYPE(double) m_QP_T_previousAcceleration ;
+ std::deque<COMState> m_comStateBuffer ;
/// \brief force acting the CoM of the robot expressed in the Euclidean Frame
Force_HRP2_14 m_force ;
@@ -272,9 +281,6 @@ namespace PatternGeneratorJRL
/// \brief Initialize the robot with the autogenerated files by MetapodFromUrdf
Robot_Model m_robot;
- /// \brief Standard polynomial trajectories for the feet.
- OnLineFootTrajectoryGeneration * OFTG_;
-
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // to use the vector of eigen used by metapod
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index cd65be18af40fab6c5450671803c6d5e1ca3c486..bbdb868952d49da02bc22be1d854b355e0f441ee 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -60,6 +60,8 @@ enum Profiles_t {
PROFIL_HERDT_EMERGENCY_STOP // 2
};
+bool ONCE = true ;
+
class TestHerdt2010: public TestObject
{
@@ -69,6 +71,8 @@ private:
Robot_Model2 robot_ ;
ComAndFootRealization * ComAndFootRealization_;
SimplePluginManager * SPM ;
+ double dInitX, dInitY;
+ int iteration ;
public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
@@ -79,6 +83,9 @@ private:
errZMP[1]=0.0;
ComAndFootRealization_ = 0 ;
SimplePluginManager * SPM = 0 ;
+ dInitX = 0 ;
+ dInitY = 0 ;
+ iteration = 0 ;
};
~TestHerdt2010()
@@ -102,7 +109,6 @@ private:
/*! Open and reset appropriatly the debug files. */
prepareDebugFiles();
-
for (unsigned int lNbIt=0;lNbIt<m_OuterLoopNbItMax;lNbIt++)
{
os << "<===============================================================>"<<endl;
@@ -162,15 +168,18 @@ private:
m_clock.fillInStatistics();
/*! Fill the debug files with appropriate information. */
- //ComparingZMPs();
+ if ( m_OneStep.NbOfIt == 1 )
+ {
+ initIK();
+ }
+ ComparingZMPs();
fillInDebugFiles();
-
+ iteration++;
}
else
{
cerr << "Nothing to dump after " << m_OneStep.NbOfIt << endl;
}
-
}
os << endl << "End of iteration " << lNbIt << endl;
@@ -181,7 +190,7 @@ private:
m_clock.writeBuffer(lProfileOutput);
m_clock.displayStatistics(os,m_OneStep);
// Compare debugging files
- //ComputeAndDisplayAverageError();
+ ComputeAndDisplayAverageError();
return compareDebugFiles();
}
@@ -216,7 +225,6 @@ private:
MAL_VECTOR_RESIZE(m_PreviousVelocity, m_HDR->numberDof());
MAL_VECTOR_RESIZE(m_PreviousAcceleration, m_HDR->numberDof());
-
SPM = new SimplePluginManager();
ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
@@ -225,11 +233,32 @@ private:
ComAndFootRealization_->setSamplingPeriod(0.1);
ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM));
ComAndFootRealization_->Initialization();
-
}
protected:
+ void initIK()
+ {
+ MAL_VECTOR_DIM(BodyAngles,double,(m_HDR->numberDof()-6));
+ MAL_VECTOR_DIM(waist,double,6);
+ for (int i = 0 ; i < 6 ; ++i )
+ {
+ waist(i) = m_PreviousConfiguration(i);
+ }
+ for (int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
+ {
+ BodyAngles(i) = m_PreviousConfiguration(i+6);
+ }
+ MAL_S3_VECTOR(lStartingCOMState,double);
+
+ lStartingCOMState(0) = m_OneStep.finalCOMPosition.x[0] ;
+ lStartingCOMState(1) = m_OneStep.finalCOMPosition.y[0] ;
+ lStartingCOMState(2) = m_OneStep.finalCOMPosition.z[0] ;
+ ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
+ waist,
+ m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
+ }
+
void fillInDebugFiles( )
{
if (m_DebugFGPI)
@@ -297,7 +326,6 @@ protected:
ofstream aof;
string aFileName;
ostringstream oss(std::ostringstream::ate);
- static int iteration = 0;
if ( iteration == 0 ){
oss.str("/tmp/walk_mnaveau.pos");
@@ -340,9 +368,6 @@ protected:
aof << endl ;
}
aof.close();
-
-
- iteration++;
}
void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
@@ -365,8 +390,7 @@ protected:
void ComparingZMPs()
{
- static int iteration = 0 ;
- /// \brief claculate, from the CoM of computed by the preview control,
+ /// \brief calculate, from the CoM of computed by the preview control,
/// the corresponding articular position, velocity and acceleration
/// ------------------------------------------------------------------
MAL_VECTOR(CurrentConfiguration,double);
@@ -408,7 +432,7 @@ protected:
/// \brief rnea, calculation of the multi body ZMP
/// ----------------------------------------------
Robot_Model2::confVector q, dq, ddq;
- for(unsigned int j = 0 ; j < m_CurrentConfiguration.size() ; j++ )
+ for(unsigned int j = 0 ; j < CurrentConfiguration.size() ; j++ )
{
q(j,0) = CurrentConfiguration[j] ;
dq(j,0) = CurrentVelocity[j] ;
@@ -416,39 +440,52 @@ protected:
}
metapod::rnea< Robot_Model2, true >::run(robot_, q, dq, ddq);
- Node2 & node = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
- Force2 & aforce = node.joint.f ;
+ Node2 node = boost::fusion::at_c<Robot_Model2::BODY>(robot_.nodes);
+ Force2 aforce = node.body.iX0.applyInv (node.joint.f) ;
double ZMPMB[2];
ZMPMB[0] = - aforce.n()[1] / aforce.f()[2] ;
ZMPMB[1] = aforce.n()[0] / aforce.f()[2] ;
- double errx = fabs ( m_OneStep.ZMPTarget(0) - ZMPMB[0] ) ;
- double erry = fabs ( m_OneStep.ZMPTarget(1) - ZMPMB[1] ) ;
- errZMP[0] += errx ;
- errZMP[1] += erry ;
+ if (m_OneStep.NbOfIt==10){
+ dInitX = m_OneStep.ZMPTarget(0) - ZMPMB[0] ;
+ dInitY = m_OneStep.ZMPTarget(1) - ZMPMB[1] ;
+ errZMP[0] = 0 ;
+ errZMP[1] = 0 ;
+ }
// Writing of the two zmps and the error.
ofstream aof;
- if (iteration == 0)
+ if (ONCE)
{
ofstream aof;
- aof.open("TestHerdt2010DynamicFilterDeltaZMP.dat",ofstream::out);
+ aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
+ aof.close();
+ ONCE = false ;
+ }
+
+ if (m_OneStep.NbOfIt >= 10)
+ {
+ double errx = sqrt ( ( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX )*( m_OneStep.ZMPTarget(0) - ZMPMB[0] - dInitX ) ) ;
+ double erry = sqrt ( ( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY )*( m_OneStep.ZMPTarget(1) - ZMPMB[1] - dInitY ) ) ;
+
+ errZMP[0] += errx ;
+ errZMP[1] += erry ;
+
+
+ aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision(m_OneStep.NbOfIt*0.1 ) << " " // 1
+ << filterprecision( ZMPMB[0] + dInitX ) << " " // 2
+ << filterprecision( ZMPMB[1] + dInitY ) << " " // 3
+ << filterprecision(m_OneStep.ZMPTarget(0) ) << " " // 4
+ << filterprecision(m_OneStep.ZMPTarget(1) ) << " " // 5
+ << endl ;
aof.close();
}
- aof.open("TestHerdt2010DynamicFilterDeltaZMP.dat",ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*0.005 ) << " " // 1
- << filterprecision( ZMPMB[0] ) << " " // 2
- << filterprecision( ZMPMB[1] ) << " " // 3
- << filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 4
- << filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 5
- << endl ;
- aof.close();
- iteration++;
}
void ComputeAndDisplayAverageError(){
@@ -639,20 +676,18 @@ protected:
#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
- { {1*200,&TestHerdt2010::walkForward},
- // {10*200,&TestHerdt2010::walkSidewards},
- // {15*200,&TestHerdt2010::startTurningRightOnSpot},
-// {35*200,&TestHerdt2010::walkForward},
-// {45*200,&TestHerdt2010::startTurningLeftOnSpot},
-// {55*200,&TestHerdt2010::walkForward},
-// {65*200,&TestHerdt2010::startTurningRightOnSpot},
-// {75*200,&TestHerdt2010::walkForward},
-// {85*200,&TestHerdt2010::startTurningLeft},
-// {95*200,&TestHerdt2010::startTurningRight},
- {3*200,&TestHerdt2010::startTurningLeft2},
- {6*200,&TestHerdt2010::startTurningRight2},
- {9*200,&TestHerdt2010::stop},
- {15*200,&TestHerdt2010::stopOnLineWalking}
+ { {5*200,&TestHerdt2010::walkForward},
+ {10*200,&TestHerdt2010::walkSidewards},
+ {25*200,&TestHerdt2010::startTurningRightOnSpot},
+ {35*200,&TestHerdt2010::walkForward},
+ {45*200,&TestHerdt2010::startTurningLeftOnSpot},
+ {55*200,&TestHerdt2010::walkForward},
+ {65*200,&TestHerdt2010::startTurningRightOnSpot},
+ {75*200,&TestHerdt2010::walkForward},
+ {85*200,&TestHerdt2010::startTurningLeft},
+ {95*200,&TestHerdt2010::startTurningRight},
+ {105*200,&TestHerdt2010::stop},
+ {110*200,&TestHerdt2010::stopOnLineWalking}
};
// Test when triggering event.
@@ -732,14 +767,12 @@ int main(int argc, char *argv[])
try
{
int ret = PerformTests(argc,argv);
- system("pause");
return ret ;
}
catch (const std::string& msg)
{
std::cerr << msg << std::endl;
}
- system("pause");
return 1;
}
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index bc21ee7e47f85ab7ffbfdfd8833d1fbaf8608ea2..21f440e4f4735103802c73664b151382232c42a5 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -147,14 +147,14 @@ namespace PatternGeneratorJRL
SpecializedRobotConstructor(aHDR,aDebugHDR);
if ((aHDR==0) || (aDebugHDR==0))
- {
- if (aHDR!=0) delete aHDR;
- if (aDebugHDR!=0) delete aDebugHDR;
+ {
+ if (aHDR!=0) delete aHDR;
+ if (aDebugHDR!=0) delete aDebugHDR;
- dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
- aHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
- aDebugHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
- }
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+ aHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ aDebugHDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+ }
// Parsing the file.
@@ -184,38 +184,38 @@ namespace PatternGeneratorJRL
ifstream aif;
aif.open(InitConfig.c_str(),ifstream::in);
if (aif.is_open())
- {
- for(unsigned int i=0;i<lNbActuatedJoints;i++)
- aif >> dInitPos[i];
- }
+ {
+ for(unsigned int i=0;i<lNbActuatedJoints;i++)
+ aif >> dInitPos[i];
+ }
aif.close();
bool DebugConfiguration = true;
ofstream aofq;
if (DebugConfiguration)
- {
- aofq.open("TestConfiguration.dat",ofstream::out);
- if (aofq.is_open())
- {
- for(unsigned int k=0;k<30;k++)
- {
- aofq << dInitPos[k] << " ";
- }
- aofq << endl;
- }
+ {
+ aofq.open("TestConfiguration.dat",ofstream::out);
+ if (aofq.is_open())
+ {
+ for(unsigned int k=0;k<30;k++)
+ {
+ aofq << dInitPos[k] << " ";
+ }
+ aofq << endl;
+ }
- }
+ }
// This is a vector corresponding to the DOFs actuated of the robot.
MAL_VECTOR_DIM(InitialPosition,double,lNbActuatedJoints);
//MAL_VECTOR_DIM(CurrentPosition,double,40);
if (conversiontoradneeded)
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
- InitialPosition(i) = dInitPos[i]*M_PI/180.0;
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i]*M_PI/180.0;
else
- for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
- InitialPosition(i) = dInitPos[i];
+ for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
+ InitialPosition(i) = dInitPos[i];
aPGI->SetCurrentJointValues(InitialPosition);
// Specify the walking mode: here the default one.
@@ -231,18 +231,18 @@ namespace PatternGeneratorJRL
MAL_VECTOR_DIM(PreviousVelocity,double,lNbDofs);
MAL_VECTOR_DIM(PreviousAcceleration,double,lNbDofs);
for(int i=0;i<6;i++)
- {
- PreviousConfiguration[i] =
- PreviousVelocity[i] =
- PreviousAcceleration[i] = 0.0;
- }
+ {
+ PreviousConfiguration[i] =
+ PreviousVelocity[i] =
+ PreviousAcceleration[i] = 0.0;
+ }
for(unsigned int i=6;i<lNbDofs;i++)
- {
- PreviousConfiguration[i] = InitialPosition[i-6];
- PreviousVelocity[i] =
- PreviousAcceleration[i] = 0.0;
- }
+ {
+ PreviousConfiguration[i] = InitialPosition[i-6];
+ PreviousVelocity[i] =
+ PreviousAcceleration[i] = 0.0;
+ }
MAL_VECTOR_DIM(ZMPTarget,double,3);
@@ -253,7 +253,7 @@ namespace PatternGeneratorJRL
string inValue[5]={"0.005","false","false","true","true"};
for(unsigned int i=0;i<5;i++)
- aDebugHDR->setProperty(inProperty[i],
+ aDebugHDR->setProperty(inProperty[i],
inValue[i]);
delete [] dInitPos;