Skip to content
Snippets Groups Projects
Commit cf47dd6d authored by mnaveau's avatar mnaveau
Browse files

Developping the herdt test to compute the difference between the multi body... 

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