diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 8b47ef8d2f576a756314ab13897991b2659c4c69..8d456fc004a08c5bde5b988b5802460d67294c45 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -84,7 +84,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
QP_T_ = 0.1 ;
QP_N_ = 16 ;
m_SamplingPeriod = 0.005 ;
- InterpolationPeriod_ = QP_T_/20 ;
+ InterpolationPeriod_ = QP_T_/20;
StepPeriod_ = 0.8 ;
SSPeriod = 0.7 ;
DSPeriod = 0.1 ;
@@ -119,24 +119,14 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
OrientPrw_->CurrentTrunkState( CurrentTrunkState );
// Initialize the 2D LIPM
- LIPM_control_.SetSimulationControlPeriod( QP_T_ );
- LIPM_control_.SetRobotControlPeriod( m_SamplingPeriod );
- LIPM_control_.InitializeSystem();
+ LIPM_.SetSimulationControlPeriod( QP_T_ );
+ LIPM_.SetRobotControlPeriod( m_SamplingPeriod );
+ LIPM_.InitializeSystem();
// Initialize the 2D LIPM
- LIPM_control_subsampled_.SetSimulationControlPeriod( QP_T_ );
- LIPM_control_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
- LIPM_control_subsampled_.InitializeSystem();
-
- // Initialize the 2D LIPM
- LIPM_DF_.SetSimulationControlPeriod( QP_T_ );
- LIPM_DF_.SetRobotControlPeriod( m_SamplingPeriod );
- LIPM_DF_.InitializeSystem();
-
- // Initialize the 2D LIPM
- LIPM_DF_subsampled_.SetSimulationControlPeriod( QP_T_ );
- LIPM_DF_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
- LIPM_DF_subsampled_.InitializeSystem();
+ LIPM_subsampled_.SetSimulationControlPeriod( QP_T_ );
+ LIPM_subsampled_.SetRobotControlPeriod( InterpolationPeriod_ );
+ LIPM_subsampled_.InitializeSystem();
// Create and initialize simplified robot model
Robot_ = new RigidBodySystem( SPM, aHS, SupportFSM_ );
@@ -188,7 +178,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// ------------------------------------------------------------------------------
ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
ComAndFootRealization_->setHumanoidDynamicRobot(aHS);
- ComAndFootRealization_->SetHeightOfTheCoM(0.0);// seems weird...
+ ComAndFootRealization_->SetHeightOfTheCoM(CoMHeight_);// seems weird...
ComAndFootRealization_->setSamplingPeriod(InterpolationPeriod_);
ComAndFootRealization_->Initialization();
@@ -223,8 +213,8 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
// size = numberOfIterationOfThePreviewControl * NumberOfSample + Margin
ZMPTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
COMTraj_deq_.resize( QP_N_ * NbSampleInterpolation_+20);
- tmpCoM_.resize(QP_N_ * NbSampleInterpolation_ + 20);
- tmpZMP_.resize(QP_N_ * NbSampleInterpolation_ + 20);
+ tmpCoM_.resize(QP_N_ * NbSampleControl_ + 20);
+ tmpZMP_.resize(QP_N_ * NbSampleControl_ + 20);
ConfigurationTraj_.resize( QP_N_ * NbSampleInterpolation_ );
VelocityTraj_.resize( QP_N_ * NbSampleInterpolation_ );
@@ -433,23 +423,15 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
CoM.z[0] = lStartingCOMState.z[0];
CoM.z[1] = lStartingCOMState.z[1];
CoM.z[2] = lStartingCOMState.z[2];
- LIPM_control_.SetComHeight(lStartingCOMState.z[0]);
- LIPM_control_.InitializeSystem();
- LIPM_control_(CoM);
- //--
- LIPM_control_subsampled_.SetComHeight(lStartingCOMState.z[0]);
- LIPM_control_subsampled_.InitializeSystem();
- LIPM_control_subsampled_(CoM);
- //--
- LIPM_DF_.SetComHeight(lStartingCOMState.z[0]);
- LIPM_DF_.InitializeSystem();
- LIPM_DF_(CoM);
+ LIPM_.SetComHeight(lStartingCOMState.z[0]);
+ LIPM_.InitializeSystem();
+ LIPM_(CoM);
//--
- LIPM_DF_subsampled_.SetComHeight(lStartingCOMState.z[0]);
- LIPM_DF_subsampled_.InitializeSystem();
- LIPM_DF_subsampled_(CoM);
+ LIPM_subsampled_.SetComHeight(lStartingCOMState.z[0]);
+ LIPM_subsampled_.InitializeSystem();
+ LIPM_subsampled_(CoM);
//--
- IntermedData_->CoM(LIPM_control_());
+ IntermedData_->CoM(LIPM_());
// Initialize preview of orientations
OrientPrw_->CurrentTrunkState( lStartingCOMState );
@@ -462,7 +444,7 @@ ZMPVelocityReferencedQP::InitOnLine(deque<ZMPPosition> & FinalZMPTraj_deq,
VRQPGenerator_->build_invariant_part( Problem_ );
// initialize intermed data needed during the interpolation
- InitStateLIPMcontrol_ = LIPM_control_.GetState() ;
+ InitStateLIPM_ = LIPM_.GetState() ;
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
FinalStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
@@ -505,7 +487,7 @@ ZMPVelocityReferencedQP::OnLine(double time,
VelRef_=NewVelRef_;
SupportFSM_->update_vel_reference(VelRef_, IntermedData_->SupportState());
IntermedData_->Reference( VelRef_ );
- IntermedData_->CoM( LIPM_control_() );
+ IntermedData_->CoM( LIPM_() );
// PREVIEW SUPPORT STATES FOR THE WHOLE PREVIEW WINDOW:
// ----------------------------------------------------
@@ -560,27 +542,80 @@ ZMPVelocityReferencedQP::OnLine(double time,
for (unsigned i = 0 ; i < CurrentIndex_ ; i++)
{
ZMPTraj_deq_[i] = FinalZMPTraj_deq[i] ;
- COMTraj_deq_[i] = tmpCoM_[i] = FinalCOMTraj_deq[i] ;
+ COMTraj_deq_[i] = FinalCOMTraj_deq[i] ;
}
LeftFootTraj_deq_ = FinalLeftFootTraj_deq ;
RightFootTraj_deq_ = FinalRightFootTraj_deq ;
FinalZMPTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
FinalCOMTraj_deq.resize( NbSampleControl_ + CurrentIndex_ );
+ static int iteration = 0;
+ if (iteration == 11){
+ iteration = 11;
+ }
+
// INTERPOLATION
- ControlInterpolation( FinalZMPTraj_deq, FinalLeftFootTraj_deq,
+ ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
FinalRightFootTraj_deq, time) ;
- DynamicFilterInterpolation( FinalCOMTraj_deq, time) ;
+ DynamicFilterInterpolation( time) ;
// DYNAMIC FILTER
// --------------
- //DynamicFilter( time, FinalCOMTraj_deq );
+ DynamicFilter( time, FinalCOMTraj_deq );
+
+ /// \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("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( 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( LeftFootTraj_deq_[CurrentIndex_+i].x ) << " " // 11
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].y ) << " " // 12
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].z ) << " " // 13
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].theta * M_PI / 180 ) << " " // 14
+ << filterprecision( LeftFootTraj_deq_[CurrentIndex_+i].omega * M_PI / 180 ) << " " // 15
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].x ) << " " //16
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].y ) << " " //17
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].z ) << " " //18
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].theta * M_PI / 180 ) << " " //19
+ << filterprecision( RightFootTraj_deq_[CurrentIndex_+i].omega * M_PI / 180 ) << " " //20
+ << filterprecision( solution_.Solution_vec[i/NbSampleInterpolation_] ) << " " //20
+ << endl ;
+ }
+ aof.close() ;
+
+
+ iteration++;
- // PREPARATION OF THE FOLLOWING STEP
- // ---------------------------------
- LIPM_DF_.setState(FinalCOMTraj_deq[NbSampleControl_ + CurrentIndex_ - 1]);
- LIPM_DF_subsampled_.setState(FinalCOMTraj_deq[NbSampleControl_ + CurrentIndex_ - 1]) ;
- OrientPrw_->CurrentTrunkState(FinalStateOrientPrw_);
// Specify that we are in the ending phase.
if (EndingPhase_ == false)
@@ -650,24 +685,27 @@ int ZMPVelocityReferencedQP::ReturnOptimalTimeToRegenerateAStep()
}
void ZMPVelocityReferencedQP::ControlInterpolation(
- std::deque<ZMPPosition> & FinalZMPTraj_deq,
- std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- double time)
+ std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
+ std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
+ double time) // INPUT
{
- InitStateLIPMcontrol_ = LIPM_control_.GetState() ;
+ InitStateLIPM_ = LIPM_.GetState() ;
+ InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
+
// INTERPOLATE CoM AND ZMP TRAJECTORIES:
// -------------------------------------
- CoMZMPInterpolation(FinalZMPTraj_deq, tmpCoM_,
+ CoMZMPInterpolation(FinalZMPTraj_deq, FinalCOMTraj_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq,
- &solution_, &LIPM_control_, NbSampleControl_, 0);
+ &solution_, &LIPM_, NbSampleControl_, 0);
// INTERPOLATE TRUNK ORIENTATION:
// ------------------------------
InitStateOrientPrw_ = OrientPrw_->CurrentTrunkState() ;
OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
m_SamplingPeriod, solution_.SupportStates_deq,
- tmpCoM_ );
+ FinalCOMTraj_deq );
FinalStateOrientPrw_ = OrientPrw_->CurrentTrunkState();
// INTERPOLATE THE COMPUTED FOOT POSITIONS:
@@ -676,49 +714,29 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
solution_.SupportStates_deq, solution_,
solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
-
return ;
}
-void ZMPVelocityReferencedQP::DynamicFilterInterpolation(
- std::deque<COMState> & FinalCOMTraj_deq,
- double time)
+void ZMPVelocityReferencedQP::DynamicFilterInterpolation(double time)
{
- LIPM_control_subsampled_.setState(InitStateLIPMcontrol_) ;
- for ( int i = 0 ; i < QP_N_ ; i++ )
- {
- // INTERPOLATE ZMP TRAJECTORIES:
- // -----------------------------
- CoMZMPInterpolation(ZMPTraj_deq_, tmpCoM_,
- LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &LIPM_control_subsampled_,
- NbSampleInterpolation_, i);
- }
+ LIPM_subsampled_.setState(InitStateLIPM_) ;
+ OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
- // INTERPOLATE COM TRAJECTORIES:
- // -----------------------------
for ( int i = 0 ; i < QP_N_ ; i++ )
{
- CoMZMPInterpolation(tmpZMP_, COMTraj_deq_,
+ CoMZMPInterpolation(ZMPTraj_deq_, COMTraj_deq_,
LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &LIPM_DF_subsampled_,
+ &solution_, &LIPM_subsampled_,
NbSampleInterpolation_, i);
}
- CoMZMPInterpolation(tmpZMP_, FinalCOMTraj_deq,
- LeftFootTraj_deq_, RightFootTraj_deq_,
- &solution_, &LIPM_DF_, NbSampleControl_, 0);
-
- // INTERPOLATE TRUNK ORIENTATION and FEET POSITIONS :
- // --------------------------------------------------
- OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
- OrientPrw_->interpolate_trunk_orientation( time, CurrentIndex_,
- m_SamplingPeriod, solution_.SupportStates_deq,
- FinalCOMTraj_deq );
-
- OrientPrw_->CurrentTrunkState(InitStateOrientPrw_) ;
+ OFTG_DF_->SetSamplingPeriod( InterpolationPeriod_ );
for ( int i = 0 ; i < QP_N_ ; i++ )
{
+ if (i > QP_N_*0.5 && solution_.SupportStates_deq.front().StateChanged )
+ {
+ solution_.SupportOrientations_deq[0] = solution_.SupportOrientations_deq[2] ;
+ }
OrientPrw_->interpolate_trunk_orientation( time + i * QP_T_,
CurrentIndex_ + i * NbSampleInterpolation_, InterpolationPeriod_,
solution_.SupportStates_deq, COMTraj_deq_ );
@@ -729,6 +747,7 @@ void ZMPVelocityReferencedQP::DynamicFilterInterpolation(
LeftFootTraj_deq_, RightFootTraj_deq_);
solution_.SupportStates_deq.pop_front();
}
+ OrientPrw_->CurrentTrunkState(FinalStateOrientPrw_);
return ;
}
@@ -758,35 +777,6 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
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 < N ; ++i )
- {
- aof << filterprecision( COMTraj_deq_[i].x[0] ) << " " // 1
- << filterprecision( COMTraj_deq_[i].y[0] ) << " " // 2
- << filterprecision( COMTraj_deq_[i].x[1] ) << " " // 3
- << filterprecision( COMTraj_deq_[i].y[1] ) << " " // 4
- << filterprecision( COMTraj_deq_[i].x[2] ) << " " // 5
- << filterprecision( COMTraj_deq_[i].y[2] ) << " " // 6
- << filterprecision( LeftFootTraj_deq_[i].x ) << " " // 7
- << filterprecision( LeftFootTraj_deq_[i].y ) << " " // 8
- << filterprecision( LeftFootTraj_deq_[i].theta * M_PI / 180 ) << " " // 9
- << filterprecision( RightFootTraj_deq_[i].x ) << " " //10
- << filterprecision( RightFootTraj_deq_[i].y ) << " " //11
- << filterprecision( RightFootTraj_deq_[i].theta * M_PI / 180 ) << " " //12
- << endl ;
- }
- aof.close() ;
-
/// \brief Debug Purpose
/// --------------------
oss.str("TestHerdt2010DynamicZMPMB.dat");
@@ -835,6 +825,16 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
}
aof.close();
+ static double ecartmaxX = 0 ;
+ static double ecartmaxY = 0 ;
+ if ( abs(DeltaZMPMBPositions_[0].px) > ecartmaxX )
+ ecartmaxX = abs(DeltaZMPMBPositions_[0].px) ;
+ if ( abs(DeltaZMPMBPositions_[0].py) > ecartmaxY )
+ ecartmaxY = abs(DeltaZMPMBPositions_[0].py) ;
+
+// cout << "ecartmaxX :" << ecartmaxX << endl ;
+// cout << "ecartmaxY :" << ecartmaxY << endl ;
+
/// Preview control on the ZMPMBs computed
/// --------------------------------------
//init of the Kajita preview control
@@ -842,17 +842,18 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
PC_->SetSamplingPeriod (InterpolationPeriod_);
PC_->SetHeightOfCoM(CoMHeight_);
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
- for(int j=0;j<3;j++)
- {
- m_deltax(j,0) = 0 ;
- m_deltay(j,0) = 0 ;
- }
+
double aSxzmp (0) , aSyzmp(0);
double deltaZMPx (0) , deltaZMPy (0) ;
// calcul of the preview control along the "ZMPTraj_deq_"
for (unsigned i = 0 ; i < NbSampleControl_ ; i++ )
{
+ for(int j=0;j<3;j++)
+ {
+ m_deltax(j,0) = 0 ;
+ m_deltay(j,0) = 0 ;
+ }
PC_->OneIterationOfPreview(m_deltax,m_deltay,
aSxzmp,aSyzmp,
DeltaZMPMBPositions_,i,
@@ -876,7 +877,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
}
else
{
- //cout << "kajita2003 preview control diverged\n" ;
+ cout << "kajita2003 preview control diverged\n" ;
}
}
}
@@ -926,13 +927,14 @@ int ZMPVelocityReferencedQP::DynamicFilter(double time, std::deque<COMState> & F
}
-void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp,
- FootAbsolutePosition & aLeftFAP,
- FootAbsolutePosition & aRightFAP,
- MAL_VECTOR_TYPE(double) & CurrentConfiguration,
- MAL_VECTOR_TYPE(double) & CurrentVelocity,
- MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- unsigned IterationNumber)
+void ZMPVelocityReferencedQP::CallToComAndFootRealization(
+ const COMState & acomp,
+ const FootAbsolutePosition & aLeftFAP,
+ const FootAbsolutePosition & aRightFAP,
+ MAL_VECTOR_TYPE(double) & CurrentConfiguration,
+ MAL_VECTOR_TYPE(double) & CurrentVelocity,
+ MAL_VECTOR_TYPE(double) & CurrentAcceleration,
+ const unsigned IterationNumber)
{
// New scheme for WPG v3.0
@@ -987,6 +989,7 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp,
CurrentVelocity = PreviousVelocity_ ;
CurrentAcceleration = PreviousAcceleration_ ;
}
+ ComAndFootRealization_->setSamplingPeriod(InterpolationPeriod_);
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
@@ -1009,16 +1012,16 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(COMState & acomp,
}
void ZMPVelocityReferencedQP::CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions,
- std::deque<COMState> & COMTraj_deq ,
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & RightFootTraj_deq,
- solution_t * Solution,
- LinearizedInvertedPendulum2D * LIPM,
- unsigned numberOfSample,
- int IterationNumber)
+ std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
+ 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
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber) // INPUT
{
- if(Solution->SupportStates_deq.size() && Solution->SupportStates_deq[0].NbStepsLeft == 0)
+ if(Solution->SupportStates_deq.size() && Solution->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];
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 6f0d5e615ef8990a5c0df666d8ffa97d1e5a6fe8..26c8aa32e0819a985e1dca02a55bc973c0455ed7 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -194,10 +194,12 @@ namespace PatternGeneratorJRL
int QP_N_;
/// \brief 2D LIPM to simulate the evolution of the robot's CoM.
- LinearizedInvertedPendulum2D LIPM_control_ ;
- LinearizedInvertedPendulum2D LIPM_control_subsampled_ ;
- LinearizedInvertedPendulum2D LIPM_DF_ ;
- LinearizedInvertedPendulum2D LIPM_DF_subsampled_ ;
+// LinearizedInvertedPendulum2D LIPM_control_ ;
+// LinearizedInvertedPendulum2D LIPM_control_subsampled_ ;
+// LinearizedInvertedPendulum2D LIPM_DF_ ;
+// LinearizedInvertedPendulum2D LIPM_DF_subsampled_ ;
+ LinearizedInvertedPendulum2D LIPM_ ;
+ LinearizedInvertedPendulum2D LIPM_subsampled_ ;
/// \brief Simplified robot model
RigidBodySystem * Robot_ ;
@@ -247,6 +249,8 @@ namespace PatternGeneratorJRL
deque<COMState> tmpCoM_ ;
deque<ZMPPosition> tmpZMP_ ;
+ deque<FootAbsolutePosition> tmpRF_ ;
+ deque<FootAbsolutePosition> tmpLF_ ;
/// \brief Index where to begin the interpolation
unsigned CurrentIndex_ ;
@@ -295,7 +299,7 @@ namespace PatternGeneratorJRL
/// \brief Used to compute once, the initial difference between the ZMP and the ZMPMB
bool Once_ ;
double DInitX_, DInitY_ ;
- COMState InitStateLIPMcontrol_ ;
+ COMState InitStateLIPM_ ;
COMState InitStateOrientPrw_ ;
COMState FinalStateOrientPrw_ ;
@@ -349,36 +353,38 @@ namespace PatternGeneratorJRL
int ReturnOptimalTimeToRegenerateAStep();
- int DynamicFilter(double time, std::deque<COMState> & FinalCOMTraj_deq);
+ int DynamicFilter(double time,
+ std::deque<COMState> & FinalCOMTraj_deq
+ );
- void CallToComAndFootRealization(COMState & acomp,
- FootAbsolutePosition & aLeftFAP,
- FootAbsolutePosition & aRightFAP,
+ void CallToComAndFootRealization(
+ const COMState & acomp,
+ const FootAbsolutePosition & aLeftFAP,
+ const FootAbsolutePosition & aRightFAP,
MAL_VECTOR_TYPE(double) & CurrentConfiguration,
MAL_VECTOR_TYPE(double) & CurrentVelocity,
MAL_VECTOR_TYPE(double) & CurrentAcceleration,
- unsigned IterationNumber
+ const unsigned IterationNumber
);
void CoMZMPInterpolation(
- std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
- std::deque<COMState> & COMTraj_deq , // OUTPUT
- std::deque<FootAbsolutePosition> & LeftFootTraj_deq, // INPUT
- std::deque<FootAbsolutePosition> & RightFootTraj_deq, // INPUT
- solution_t * Solution, // INPUT
- LinearizedInvertedPendulum2D * LIPM, // INPUT
- unsigned numberOfSample, // INPUT
- int IterationNumber); // INPUT
+ std::deque<ZMPPosition> & ZMPPositions, // OUTPUT
+ 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
+ LinearizedInvertedPendulum2D * LIPM, // INPUT/OUTPUT
+ const unsigned numberOfSample, // INPUT
+ const int IterationNumber); // INPUT
void ControlInterpolation(
- std::deque<ZMPPosition> & FinalZMPTraj_deq,
- std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq,
- std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq,
- double time);
-
- void DynamicFilterInterpolation(
- std::deque<COMState> & FinalCOMTraj_deq,
- double time);
+ std::deque<COMState> & FinalCOMTraj_deq, // OUTPUT
+ std::deque<ZMPPosition> & FinalZMPTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalLeftFootTraj_deq, // OUTPUT
+ std::deque<FootAbsolutePosition> & FinalRightFootTraj_deq, // OUTPUT
+ double time); // INPUT
+
+ void DynamicFilterInterpolation(double time);
};
}
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index d2263d3f5abb09b0c75e45e6b355797bb7dc6aca..f865e5f16196b3f47866fbd7bb1a3751e4126d4b 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -67,24 +67,29 @@ class TestHerdt2010: public TestObject
private:
// buffer to save all the zmps positions
- double errZMP [2] ;
+ vector< vector<double> > errZMP_ ;
Robot_Model2 robot_ ;
ComAndFootRealization * ComAndFootRealization_;
SimplePluginManager * SPM ;
double dInitX, dInitY;
- int iteration ;
+ int iteration,iteration_zmp ;
public:
TestHerdt2010(int argc, char *argv[], string &aString, int TestProfile):
TestObject(argc,argv,aString)
{
m_TestProfile = TestProfile;
- errZMP[0]=0.0;
- errZMP[1]=0.0;
+ {
+ vector<double> tmp_zmp(2) ;
+ tmp_zmp[0] =0.0 ;
+ tmp_zmp[1] =0.0 ;
+ errZMP_.push_back(tmp_zmp);
+ }
+
ComAndFootRealization_ = 0 ;
- SimplePluginManager * SPM = 0 ;
dInitX = 0 ;
dInitY = 0 ;
+ iteration_zmp = 0 ;
iteration = 0 ;
};
@@ -109,6 +114,7 @@ private:
/*! Open and reset appropriatly the debug files. */
prepareDebugFiles();
+ initIK();
for (unsigned int lNbIt=0;lNbIt<m_OuterLoopNbItMax;lNbIt++)
{
os << "<===============================================================>"<<endl;
@@ -168,13 +174,9 @@ private:
m_clock.fillInStatistics();
/*! Fill the debug files with appropriate information. */
- if ( m_OneStep.NbOfIt == 1 )
- {
- initIK();
- }
ComparingZMPs();
+ ComputeAndDisplayAverageError(false);
fillInDebugFiles();
- iteration++;
}
else
{
@@ -190,7 +192,7 @@ private:
m_clock.writeBuffer(lProfileOutput);
m_clock.displayStatistics(os,m_OneStep);
// Compare debugging files
- ComputeAndDisplayAverageError();
+ ComputeAndDisplayAverageError(true);
return compareDebugFiles();
}
@@ -229,9 +231,9 @@ private:
ComAndFootRealization_ = new ComAndFootRealizationByGeometry( (PatternGeneratorInterfacePrivate*) SPM );
ComAndFootRealization_->setHumanoidDynamicRobot(m_HDR);
- ComAndFootRealization_->SetHeightOfTheCoM(0.814);
- ComAndFootRealization_->setSamplingPeriod(0.1);
ComAndFootRealization_->SetStepStackHandler(new StepStackHandler(SPM));
+ ComAndFootRealization_->SetHeightOfTheCoM(0.814);
+ ComAndFootRealization_->setSamplingPeriod(0.005);
ComAndFootRealization_->Initialization();
}
@@ -245,7 +247,7 @@ protected:
{
waist(i) = m_PreviousConfiguration(i);
}
- for (int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
+ for (unsigned int i = 0 ; i < (m_HDR->numberDof()-6) ; ++i )
{
BodyAngles(i) = m_PreviousConfiguration(i+6);
}
@@ -254,9 +256,13 @@ protected:
lStartingCOMState(0) = m_OneStep.finalCOMPosition.x[0] ;
lStartingCOMState(1) = m_OneStep.finalCOMPosition.y[0] ;
lStartingCOMState(2) = m_OneStep.finalCOMPosition.z[0] ;
+ ComAndFootRealization_->SetHeightOfTheCoM(0.814);
+ ComAndFootRealization_->setSamplingPeriod(0.005);
+ ComAndFootRealization_->Initialization();
ComAndFootRealization_->InitializationCoM(BodyAngles,lStartingCOMState,
waist,
m_OneStep.LeftFootPosition, m_OneStep.RightFootPosition);
+ ComAndFootRealization_->Initialization();
}
void fillInDebugFiles( )
@@ -369,6 +375,8 @@ protected:
aof << endl ;
}
aof.close();
+
+ iteration++;
}
void SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR, CjrlHumanoidDynamicRobot *& aDebugHDR )
@@ -420,14 +428,13 @@ protected:
aLeftFootPosition(2) = m_OneStep.LeftFootPosition.z; aRightFootPosition(2) = m_OneStep.RightFootPosition.z;
aLeftFootPosition(3) = m_OneStep.LeftFootPosition.theta; aRightFootPosition(3) = m_OneStep.RightFootPosition.theta;
aLeftFootPosition(4) = m_OneStep.LeftFootPosition.omega; aRightFootPosition(4) = m_OneStep.RightFootPosition.omega;
-
ComAndFootRealization_->ComputePostureForGivenCoMAndFeetPosture(aCOMState, aCOMSpeed, aCOMAcc,
aLeftFootPosition,
aRightFootPosition,
CurrentConfiguration,
CurrentVelocity,
CurrentAcceleration,
- iteration,
+ iteration_zmp,
1);
/// \brief rnea, calculation of the multi body ZMP
@@ -450,56 +457,87 @@ protected:
ZMPMB[1] = aforce.n()[0] / aforce.f()[2] ;
- if (m_OneStep.NbOfIt==10){
+ 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 ;
+ {
+ vector<double> tmp_zmp(2) ;
+ tmp_zmp[0] =0.0 ;
+ tmp_zmp[1] =0.0 ;
+ errZMP_.push_back(tmp_zmp);
+ }
}
+ 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 ) ) ;
+ {
+ vector<double> tmp_zmp(2) ;
+ tmp_zmp[0] =errx ;
+ tmp_zmp[1] =erry ;
+ errZMP_.push_back(tmp_zmp);
+ }
+ }
+
+
+ static double ecartmaxX = 0 ;
+ static double ecartmaxY = 0 ;
+ if ( abs(errZMP_.back()[0]) > ecartmaxX )
+ ecartmaxX = abs(errZMP_.back()[0]) ;
+ if ( abs(errZMP_.back()[1]) > ecartmaxY )
+ ecartmaxY = abs(errZMP_.back()[1]) ;
+
+ cout << "ecartmaxX :" << ecartmaxX << endl ;
+ cout << "ecartmaxY :" << ecartmaxY << endl ;
+
// Writing of the two zmps and the error.
ofstream aof;
if (ONCE)
{
- ofstream aof;
aof.open("TestHerdt2010ErrorZMP.dat",ofstream::out);
aof.close();
ONCE = false ;
}
+ aof.open("TestHerdt2010ErrorZMP.dat",ofstream::app);
+ aof.precision(8);
+ aof.setf(ios::scientific, ios::floatfield);
+ aof << filterprecision( iteration_zmp ) << " " // 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();
- 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();
- }
+ iteration_zmp++;
+ return ;
}
- void ComputeAndDisplayAverageError(){
- double moyErrX = errZMP[0] / m_OneStep.NbOfIt ;
- double moyErrY = errZMP[1] / m_OneStep.NbOfIt ;
- cout << "moyErrX = " << moyErrX << endl
- << "moyErrY = " << moyErrY << endl ;
-
- // Writing of the two zmps and the error.
+ void ComputeAndDisplayAverageError(bool display){
+ static int plot_it = 0 ;
+ double moyErrX = 0 ;
+ double moyErrY = 0 ;
+ for (unsigned int i = 0 ; i < errZMP_.size(); ++i)
+ {
+ moyErrX += errZMP_[i][0] ;
+ moyErrY += errZMP_[i][1] ;
+ }
+ moyErrX = moyErrX / errZMP_.size() ;
+ moyErrY = moyErrY / errZMP_.size() ;
+ if ( display )
+ {
+ cout << "moyErrX = " << moyErrX << endl
+ << "moyErrY = " << moyErrY << endl ;
+ }
ofstream aof;
string aFileName;
aFileName = m_TestName;
aFileName += "MoyZMP.dat";
+ if(plot_it==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);
@@ -507,10 +545,11 @@ protected:
<< filterprecision(moyErrY ) << " " // 2
<< endl ;
aof.close();
+ plot_it++;
}
- void startOnLineWalking(PatternGeneratorInterface &aPGI)
- {
+void startOnLineWalking(PatternGeneratorInterface &aPGI)
+{
CommonInitialization(aPGI);
{
@@ -735,7 +774,7 @@ protected:
}
}
- };
+};
int PerformTests(int argc, char *argv[])
{