diff --git a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
index fc8959dc0a067a7a31fef53c3473cc3d5829ab22..40aeea2c0301c888b0a77017a49d49a89b2a14b7 100644
--- a/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
+++ b/src/FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.cpp
@@ -336,17 +336,6 @@ void
FinalLeftFootTraj_deq[CurrentIndex+k].time =
FinalRightFootTraj_deq[CurrentIndex+k].time = Time+k*m_SamplingPeriod;
}
-
- if(CurrentSupport.Foot == LEFT)
- {
- LastSFP = &(FinalRightFootTraj_deq[(unsigned int)(CurrentIndex+QP_T_/m_SamplingPeriod)]);
- }
- else
- {
- LastSFP = &(FinalLeftFootTraj_deq[(unsigned int)(CurrentIndex+QP_T_/m_SamplingPeriod)]);
- }
- cout << "NewSFP->ddtheta = " << LastSFP->ddtheta << endl ;
-
}
else if (CurrentSupport.Phase == DS || Time+3.0/2.0*QP_T_ > CurrentSupport.TimeLimit)
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index c5a3455a399e32423547dcd222a10eeaa609ce99..8c2d83c1d5763bec19920f0199c88d12d40599c7 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -203,9 +203,6 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0)
PreviousConfiguration_ = aHS->currentConfiguration() ;
PreviousVelocity_ = aHS->currentVelocity();
PreviousAcceleration_ = aHS->currentAcceleration();
- PreviousConfigurationControl_ = aHS->currentConfiguration() ;
- PreviousVelocityControl_ = aHS->currentVelocity();
- PreviousAccelerationControl_ = aHS->currentAcceleration();
ComAndFootRealizationByGeometry_->SetPreviousConfigurationStage0(PreviousConfiguration_);
ComAndFootRealizationByGeometry_->SetPreviousVelocityStage0(PreviousVelocity_);
@@ -243,9 +240,9 @@ Solution_(),OFTG_DF_(0),OFTG_control_(0)
VelocityTraj_.resize( QP_N_ * NbSampleInterpolation_, aHS->currentVelocity() );
AccelerationTraj_.resize( QP_N_ * NbSampleInterpolation_, aHS->currentAcceleration() );
- ConfigurationTrajControl_.resize( NbSampleControl_, aHS->currentConfiguration() );
- VelocityTrajControl_.resize( NbSampleControl_, aHS->currentConfiguration() );
- AccelerationTrajControl_.resize( NbSampleControl_, aHS->currentConfiguration() );
+ tmpConfigurationTraj_.resize(QP_N_ * NbSampleInterpolation_) ;
+ tmpVelocityTraj_.resize(QP_N_ * NbSampleInterpolation_) ;
+ tmpAccelerationTraj_.resize(QP_N_ * NbSampleInterpolation_) ;
DeltaZMPMBPositions_.resize ( QP_N_ * NbSampleInterpolation_ );
ZMPMBTraj_deq_.resize( QP_N_ * NbSampleInterpolation_, vector<double>(2) );
@@ -595,7 +592,7 @@ void
ControlInterpolation( FinalCOMTraj_deq, FinalZMPTraj_deq, FinalLeftFootTraj_deq,
FinalRightFootTraj_deq, time) ;
- DynamicFilterInterpolation( time) ;
+ DynamicFilterInterpolation(time) ;
CallToComAndFootRealization(time);
@@ -1198,11 +1195,31 @@ void ZMPVelocityReferencedQP::CallToComAndFootRealization(double time)
it, stage0);
}
+ tmpConfigurationTraj_[0] = ( ConfigurationTraj_[1]+ConfigurationTraj_[0]+PreviousConfiguration_ )/3;
+ tmpVelocityTraj_[0] = ( VelocityTraj_[1]+VelocityTraj_[0]+PreviousVelocity_ )/3;
+ tmpAccelerationTraj_[0] = ( AccelerationTraj_[1]+AccelerationTraj_[0]+PreviousAcceleration_ )/3;
+
// saving the precedent state of the next QP_ computation
PreviousConfiguration_ = ConfigurationTraj_[NbSampleInterpolation_-1] ;
PreviousVelocity_ = VelocityTraj_[NbSampleInterpolation_-1] ;
PreviousAcceleration_ = AccelerationTraj_[NbSampleInterpolation_-1] ;
+ for (unsigned int i = 1 ; i < N-1 ; ++i )
+ {
+ tmpConfigurationTraj_[i] = ( ConfigurationTraj_[i+1] + ConfigurationTraj_[i] + ConfigurationTraj_[i-1] )/3;
+ tmpVelocityTraj_[i] = ( VelocityTraj_[i+1] + VelocityTraj_[i] + VelocityTraj_[i-1] )/3;
+ tmpAccelerationTraj_[i] = ( AccelerationTraj_[i+1] + AccelerationTraj_[i] + AccelerationTraj_[i-1] )/3;
+ }
+
+ tmpConfigurationTraj_[N-1] = ( ConfigurationTraj_[N-1]+ConfigurationTraj_[N-2] )*0.5;
+ tmpVelocityTraj_[N-1] = ( VelocityTraj_[N-1]+VelocityTraj_[N-2] )*0.5;
+ tmpAccelerationTraj_[N-1] = ( AccelerationTraj_[N-1]+AccelerationTraj_[N-2] )*0.5;
+
+
+ ConfigurationTraj_ = tmpConfigurationTraj_ ;
+ VelocityTraj_ = tmpVelocityTraj_ ;
+ AccelerationTraj_ = tmpAccelerationTraj_ ;
+
/// \brief Debug Purpose
/// --------------------
ofstream aof;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index d67ab3f99551326906ebcdcb135ef8605c7aa448..fb16682d1664f1074d4de4e1e77165809a761594 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -287,18 +287,18 @@ namespace PatternGeneratorJRL
unsigned NbSampleInterpolation_ ;
/// \brief Buffers for the CoM an Feet computation, i.e. the simplify inverse kinematics.
- vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
- vector <MAL_VECTOR_TYPE(double)> VelocityTraj_ ;
- vector <MAL_VECTOR_TYPE(double)> AccelerationTraj_ ;
- vector <MAL_VECTOR_TYPE(double)> ConfigurationTrajControl_ ;
- vector <MAL_VECTOR_TYPE(double)> VelocityTrajControl_ ;
- vector <MAL_VECTOR_TYPE(double)> AccelerationTrajControl_ ;
+ std::vector <MAL_VECTOR_TYPE(double)> ConfigurationTraj_ ;
+ std::vector <MAL_VECTOR_TYPE(double)> VelocityTraj_ ;
+ std::vector <MAL_VECTOR_TYPE(double)> AccelerationTraj_ ;
MAL_VECTOR_TYPE(double) PreviousConfiguration_ ;
MAL_VECTOR_TYPE(double) PreviousVelocity_ ;
MAL_VECTOR_TYPE(double) PreviousAcceleration_ ;
- MAL_VECTOR_TYPE(double) PreviousConfigurationControl_ ;
- MAL_VECTOR_TYPE(double) PreviousVelocityControl_ ;
- MAL_VECTOR_TYPE(double) PreviousAccelerationControl_ ;
+
+ // 2 point filter
+ vector <MAL_VECTOR_TYPE(double)> tmpConfigurationTraj_ ;
+ vector <MAL_VECTOR_TYPE(double)> tmpVelocityTraj_ ;
+ vector <MAL_VECTOR_TYPE(double)> tmpAccelerationTraj_ ;
+
/// \brief Buffers for the uotput of the Kajita preview control algorithm.
std::deque<COMState> ComStateBuffer_ ;
diff --git a/tests/TestHerdt2010DynamicFilter.cpp b/tests/TestHerdt2010DynamicFilter.cpp
index eeaa3fe304a9518d2148981d26186248f7904f80..7e0c82a8d8d59b3ba353147fcd54d417b44a5136 100644
--- a/tests/TestHerdt2010DynamicFilter.cpp
+++ b/tests/TestHerdt2010DynamicFilter.cpp
@@ -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[0] ) << " " // 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
@@ -297,7 +297,7 @@ protected:
<< filterprecision(m_OneStep.LeftFootPosition.ddx ) << " " // 17
<< filterprecision(m_OneStep.LeftFootPosition.ddy ) << " " // 18
<< filterprecision(m_OneStep.LeftFootPosition.ddz ) << " " // 19
- << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
+ << filterprecision(m_OneStep.LeftFootPosition.theta*M_PI/180 ) << " " // 20
<< filterprecision(m_OneStep.LeftFootPosition.omega ) << " " // 21
<< filterprecision(m_OneStep.LeftFootPosition.omega2 ) << " " // 22
<< filterprecision(m_OneStep.RightFootPosition.x ) << " " // 23