From 2d13d6fef92770aec6bb0bd766d30ffa45d208ca Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 18 Jun 2014 13:50:27 +0200
Subject: [PATCH] preparing the PG of Morisawa for the dynamic filter (2)
---
.../AnalyticalZMPCOGTrajectory.cpp | 18 +-
src/Mathematics/AnalyticalZMPCOGTrajectory.hh | 11 +
.../AnalyticalMorisawaCompact.cpp | 151 +++++++------
.../DynamicFilter.cpp | 204 ++++++++++++++++--
.../DynamicFilter.hh | 30 ++-
5 files changed, 326 insertions(+), 88 deletions(-)
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
index 2568bae9..99c63b31 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.cpp
@@ -178,7 +178,7 @@ namespace PatternGeneratorJRL
deltaj = t- m_AbsoluteTimeReference - m_RefTime[j];
r = cosh(m_omegaj[j]*deltaj) * m_V[j] +
- sinh(m_omegaj[j]*deltaj) * m_W[j];
+ sinh(m_omegaj[j]*deltaj) * m_W[j];
r += m_ListOfCOGPolynomials[j]->Compute(deltaj);
return true;
}
@@ -189,9 +189,21 @@ namespace PatternGeneratorJRL
deltaj = t- m_AbsoluteTimeReference - m_RefTime[j];
r = m_omegaj[j] * sinh(m_omegaj[j]*deltaj) * m_V[j] +
- m_omegaj[j] * cosh(m_omegaj[j]*deltaj) * m_W[j];
+ m_omegaj[j] * cosh(m_omegaj[j]*deltaj) * m_W[j];
r += m_ListOfCOGPolynomials[j]->ComputeDerivative(deltaj);
- ODEBUG("ComputeCOMPSeed: " << r);
+ ODEBUG("ComputeCOMSpeed: " << r);
+ return true;
+ }
+
+ bool AnalyticalZMPCOGTrajectory::ComputeCOMAcceleration(double t, double &r, int j)
+ {
+ double deltaj=0.0;
+ deltaj = t- m_AbsoluteTimeReference - m_RefTime[j];
+
+ r = m_omegaj[j] * m_omegaj[j] * cosh(m_omegaj[j]*deltaj) * m_V[j] +
+ m_omegaj[j] * m_omegaj[j] * sinh(m_omegaj[j]*deltaj) * m_W[j];
+ r += m_ListOfCOGPolynomials[j]->ComputeSecDerivative(deltaj);
+ ODEBUG("ComputeCOMAcceleration: " << r);
return true;
}
diff --git a/src/Mathematics/AnalyticalZMPCOGTrajectory.hh b/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
index 0f6c97b6..716ee6e7 100644
--- a/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
+++ b/src/Mathematics/AnalyticalZMPCOGTrajectory.hh
@@ -95,6 +95,17 @@ namespace PatternGeneratorJRL
*/
bool ComputeCOMSpeed(double t,double &r, int i);
+ /*! Compute the current acceleration according
+ to time and the index of the interval.
+ To be efficient this method does not have
+ any boundary check.
+ @param t: the time,
+ @param i: the numero of the interval
+ @param r: the result,
+ @return Returns true if the function has been
+ computed, false otherwise.
+ */
+ bool ComputeCOMAcceleration(double t, double &r, int j);
/*! Compute the current value according
to time.
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 25c00c43..087f111c 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -645,9 +645,9 @@ namespace PatternGeneratorJRL
m_kajitaDynamicFilter->init(m_CurrentTime,
m_SamplingPeriod,
+ 0.050,
m_SamplingPeriod,
- m_SamplingPeriod,
- m_PreviewControlTime,
+ 1.6,
lStartingCOMState.z[0]
);
@@ -666,71 +666,92 @@ namespace PatternGeneratorJRL
{
if (m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,lIndexInterval))
{
-
- ZMPPosition aZMPPos;
- memset(&aZMPPos,0,sizeof(aZMPPos));
- COMState aCOMPos;
- memset(&aCOMPos,0,sizeof(aCOMPos));
-
- if (m_FilteringActivate)
- {
- double FZmpX=0, FComX=0,FComdX=0;
-
- // Should we filter ?
- bool r = m_FilterXaxisByPC->UpdateOneStep(time,FZmpX, FComX, FComdX);
- if (r)
- {
- double FZmpY=0, FComY=0,FComdY=0;
- // Yes we should.
- m_FilterYaxisByPC->UpdateOneStep(time,FZmpY, FComY, FComdY);
-
- /*! Feed the ZMPPositions. */
- aZMPPos.px = FZmpX;
- aZMPPos.py = FZmpY;
-
- /*! Feed the COMStates. */
- aCOMPos.x[0] = FComX; aCOMPos.x[1] = FComdX;
- aCOMPos.y[0] = FComY; aCOMPos.y[1] = FComdY;
- }
- }
+ // prepare the buffers for the dynamic filter
+ unsigned N = (unsigned)(m_kajitaDynamicFilter->getPreviewWindowSize_()/
+ m_kajitaDynamicFilter->getInterpolationPeriod());
+
+ std::deque<ZMPPosition> ZMPPos_deq (N);
+ std::deque<COMState> COMPos_deq(N);
+ std::deque<FootAbsolutePosition> LeftFootAbsPos_deq (N);
+ std::deque<FootAbsolutePosition> RightFootAbsPos_deq (N);
+ for (unsigned i = 0 ; i < N ; ++i)
+ {
+ memset(&ZMPPos_deq[i],0,sizeof(ZMPPos_deq[i]));
+ memset(&COMPos_deq[i],0,sizeof(COMPos_deq[i]));
+ memset(&LeftFootAbsPos_deq[i],0,sizeof(LeftFootAbsPos_deq[i]));
+ memset(&RightFootAbsPos_deq[i],0,sizeof(RightFootAbsPos_deq[i]));
+ }
+
+ /*! Feed the buffer for the dynamic filter */
+ double current_time = time ;
+ for (unsigned i = 0 ; i < N ; ++i)
+ {
+ current_time += i*0.050 ;
+
+ if (0/*m_FilteringActivate*/)
+ {
+ double FZmpX=0, FComX=0,FComdX=0;
+
+ // Should we filter ?
+ bool r = m_FilterXaxisByPC->UpdateOneStep(current_time,FZmpX, FComX, FComdX);
+ if (r)
+ {
+ double FZmpY=0, FComY=0,FComdY=0;
+ // Yes we should.
+ m_FilterYaxisByPC->UpdateOneStep(current_time,FZmpY, FComY, FComdY);
+
+ /*! Feed the ZMPPositions. */
+ ZMPPos_deq[i].px = FZmpX;
+ ZMPPos_deq[i].py = FZmpY;
+
+ /*! Feed the COMStates. */
+ COMPos_deq[i].x[0] = FComX; COMPos_deq[i].x[1] = FComdX;
+ COMPos_deq[i].y[0] = FComY; COMPos_deq[i].y[1] = FComdY;
+ }
+ }
+
+ /*! Feed the ZMPPositions. */
+ double lZMPPosx=0.0,lZMPPosy=0.0;
+ m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(current_time,lZMPPosx,lIndexInterval);
+ ZMPPos_deq[i].px = lZMPPosx;
+ m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(current_time,lZMPPosy,lIndexInterval);
+ ZMPPos_deq[i].py = lZMPPosy;
+
+ /*! Feed the COMStates. */
+ double lCOMPosx=0.0, lCOMPosdx=0.0, lCOMPosddx=0.0;
+ double lCOMPosy=0.0, lCOMPosdy=0.0, lCOMPosddy=0.0;
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(current_time,lCOMPosx,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(current_time,lCOMPosdx,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryX->ComputeCOMAcceleration(current_time,lCOMPosddx,lIndexInterval);
+
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(current_time,lCOMPosy,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(current_time,lCOMPosdy,lIndexInterval);
+ m_AnalyticalZMPCoGTrajectoryY->ComputeCOMAcceleration(current_time,lCOMPosddy,lIndexInterval);
+ COMPos_deq[i].x[0] = lCOMPosx ; COMPos_deq[i].x[1] = lCOMPosdx ; COMPos_deq[i].x[2] = lCOMPosddx ;
+ COMPos_deq[i].y[0] = lCOMPosy ; COMPos_deq[i].y[1] = lCOMPosdy ; COMPos_deq[i].y[2] = lCOMPosddy ;
+ COMPos_deq[i].z[0] = m_InitialPoseCoMHeight;
+
+ /*! Feed the FootPositions. */
+ /*! Left */
+ m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,current_time,LeftFootAbsPos_deq[i],lIndexInterval);
+ m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,current_time,RightFootAbsPos_deq[i],lIndexInterval);
+ }
std::deque<COMState> deltaCoMTraj_deq (1);
- //m_kajitaDynamicFilter->filter(,,,,deltaCoMTraj_deq);
-
- /*! Feed the ZMPPositions. */
- double lZMPPosx=0.0,lZMPPosy=0.0;
- m_AnalyticalZMPCoGTrajectoryX->ComputeZMP(time,lZMPPosx,lIndexInterval);
- aZMPPos.px += lZMPPosx;
- m_AnalyticalZMPCoGTrajectoryY->ComputeZMP(time,lZMPPosy,lIndexInterval);
- aZMPPos.py += lZMPPosy;
- FinalZMPPositions.push_back(aZMPPos);
-
- /*! Feed the COMStates. */
- double lCOMPosx=0.0, lCOMPosdx=0.0;
- double lCOMPosy=0.0, lCOMPosdy=0.0;
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOM(time,lCOMPosx,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryX->ComputeCOMSpeed(time,lCOMPosdx,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOM(time,lCOMPosy,lIndexInterval);
- m_AnalyticalZMPCoGTrajectoryY->ComputeCOMSpeed(time,lCOMPosdy,lIndexInterval);
- aCOMPos.x[0] += lCOMPosx + deltaCoMTraj_deq[0].x[0] ; aCOMPos.x[1] += lCOMPosdx + deltaCoMTraj_deq[0].x[1];
- aCOMPos.y[0] += lCOMPosy + deltaCoMTraj_deq[0].y[0] ; aCOMPos.y[1] += lCOMPosdy + deltaCoMTraj_deq[0].y[1];
- aCOMPos.z[0] = m_InitialPoseCoMHeight;
- FinalCOMStates.push_back(aCOMPos);
- /*! Feed the FootPositions. */
-
-
- /*! Left */
- FootAbsolutePosition LeftFootAbsPos;
- memset(&LeftFootAbsPos,0,sizeof(LeftFootAbsPos));
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(1,time,LeftFootAbsPos,lIndexInterval);
- FinalLeftFootAbsolutePositions.push_back(LeftFootAbsPos);
-
- /*! Right */
- FootAbsolutePosition RightFootAbsPos;
- memset(&RightFootAbsPos,0,sizeof(RightFootAbsPos));
- m_FeetTrajectoryGenerator->ComputeAnAbsoluteFootPosition(-1,time,RightFootAbsPos,lIndexInterval);
- FinalRightFootAbsolutePositions.push_back(RightFootAbsPos);
-
+ memset(&deltaCoMTraj_deq[0],0,sizeof(deltaCoMTraj_deq[0]));
+ m_kajitaDynamicFilter->filter(COMPos_deq,ZMPPos_deq,LeftFootAbsPos_deq,RightFootAbsPos_deq,deltaCoMTraj_deq);
+
+
+ COMState aCOMState (COMPos_deq[0]) ;
+ for(unsigned i = 0 ; i < 3 ; ++i)
+ {
+ aCOMState.x[i] += deltaCoMTraj_deq[0].x[i] ;
+ aCOMState.y[i] += deltaCoMTraj_deq[0].y[i] ;
+ }
+ FinalZMPPositions.push_back(ZMPPos_deq[0]);
+ FinalCOMStates.push_back(aCOMState);
+ FinalLeftFootAbsolutePositions.push_back(LeftFootAbsPos_deq[0]);
+ FinalRightFootAbsolutePositions.push_back(RightFootAbsPos_deq[0]);
}
}
else
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 293a5b25..5f6cfa6b 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -129,17 +129,22 @@ int DynamicFilter::filter(
deque<COMState> & outputDeltaCOMTraj_deq_
)
{
- InverseKinematics(
- inputCOMTraj_deq_,
- inputLeftFootTraj_deq_,
- inputRightFootTraj_deq_);
+// InverseKinematics(
+// inputCOMTraj_deq_,
+// inputLeftFootTraj_deq_,
+// inputRightFootTraj_deq_);
- printDebug();
- InverseDynamics(inputZMPTraj_deq_);
+ //InverseDynamics(inputZMPTraj_deq_);
- int error = OptimalControl(outputDeltaCOMTraj_deq_);
+ //int error = OptimalControl(outputDeltaCOMTraj_deq_);
+ printBuffers(inputCOMTraj_deq_,
+ inputZMPTraj_deq_,
+ inputLeftFootTraj_deq_,
+ inputRightFootTraj_deq_,
+ outputDeltaCOMTraj_deq_);
+ int error = 0 ;
return error ;
}
@@ -321,21 +326,25 @@ double DynamicFilter::filterprecision(double adb)
return lintadb2/1e7;
}
-void DynamicFilter::printDebug()
+void DynamicFilter::printAlongTime(deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ )
{
- /// \brief Debug Purpose
- /// --------------------
+ // 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 );
+// 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("TestHerdt2010DynamicArtDF.dat");
+ // --------------------
+ oss.str("DynamicFilterAllVariablesAlongInTime.dat");
aFileName = oss.str();
if(iteration == 0)
{
@@ -348,8 +357,64 @@ void DynamicFilter::printDebug()
aof.setf(ios::scientific, ios::floatfield);
for (unsigned int i = 0 ; i < NbI_ ; ++i )
{
- aof << filterprecision( 0.0 ) << " " // 1
- << filterprecision( 0.0 ) << " " ; // 2
+ aof << filterprecision( iteration*controlPeriod_ + i*interpolationPeriod_ ) << " " // 0
+ << filterprecision( inputCOMTraj_deq_[i].x[0] ) << " " // 1
+ << filterprecision( inputCOMTraj_deq_[i].x[1] ) << " " // 2
+ << filterprecision( inputCOMTraj_deq_[i].x[2] ) << " " // 3
+ << filterprecision( inputCOMTraj_deq_[i].y[0] ) << " " // 4
+ << filterprecision( inputCOMTraj_deq_[i].y[1] ) << " " // 5
+ << filterprecision( inputCOMTraj_deq_[i].y[2] ) << " " // 6
+ << filterprecision( inputCOMTraj_deq_[i].z[0] ) << " " // 7
+ << filterprecision( inputCOMTraj_deq_[i].z[1] ) << " " // 8
+ << filterprecision( inputCOMTraj_deq_[i].z[2] ) << " " // 9
+ << filterprecision( inputCOMTraj_deq_[i].roll[0] ) << " " // 10
+ << filterprecision( inputCOMTraj_deq_[i].roll[1] ) << " " // 11
+ << filterprecision( inputCOMTraj_deq_[i].roll[2] ) << " " // 12
+ << filterprecision( inputCOMTraj_deq_[i].pitch[0] ) << " "// 13
+ << filterprecision( inputCOMTraj_deq_[i].pitch[1] ) << " "// 14
+ << filterprecision( inputCOMTraj_deq_[i].pitch[2] ) << " "// 15
+ << filterprecision( inputCOMTraj_deq_[i].yaw[0] ) << " " // 16
+ << filterprecision( inputCOMTraj_deq_[i].yaw[1] ) << " " // 17
+ << filterprecision( inputCOMTraj_deq_[i].yaw[2] ) << " " // 18
+
+ << filterprecision( inputZMPTraj_deq_[i].px ) << " " // 19
+ << filterprecision( inputZMPTraj_deq_[i].py ) << " " // 20
+
+ << filterprecision( ZMPMB_vec_[i][0] ) << " " // 21
+ << filterprecision( ZMPMB_vec_[i][1] ) << " " // 22
+
+ << filterprecision( inputLeftFootTraj_deq_[i].x ) << " " // 23
+ << filterprecision( inputLeftFootTraj_deq_[i].y ) << " " // 24
+ << filterprecision( inputLeftFootTraj_deq_[i].z ) << " " // 25
+ << filterprecision( inputLeftFootTraj_deq_[i].theta ) << " " // 26
+ << filterprecision( inputLeftFootTraj_deq_[i].omega ) << " " // 27
+ << filterprecision( inputLeftFootTraj_deq_[i].dx ) << " " // 28
+ << filterprecision( inputLeftFootTraj_deq_[i].dy ) << " " // 29
+ << filterprecision( inputLeftFootTraj_deq_[i].dz ) << " " // 30
+ << filterprecision( inputLeftFootTraj_deq_[i].dtheta ) << " " // 31
+ << filterprecision( inputLeftFootTraj_deq_[i].domega ) << " " // 32
+ << filterprecision( inputLeftFootTraj_deq_[i].ddx ) << " " // 33
+ << filterprecision( inputLeftFootTraj_deq_[i].ddy ) << " " // 34
+ << filterprecision( inputLeftFootTraj_deq_[i].ddz ) << " " // 35
+ << filterprecision( inputLeftFootTraj_deq_[i].ddtheta ) << " " // 36
+ << filterprecision( inputLeftFootTraj_deq_[i].ddomega ) << " " // 37
+
+ << filterprecision( inputRightFootTraj_deq_[i].x ) << " " // 38
+ << filterprecision( inputRightFootTraj_deq_[i].y ) << " " // 39
+ << filterprecision( inputRightFootTraj_deq_[i].z ) << " " // 40
+ << filterprecision( inputRightFootTraj_deq_[i].theta ) << " " // 41
+ << filterprecision( inputRightFootTraj_deq_[i].omega ) << " " // 42
+ << filterprecision( inputRightFootTraj_deq_[i].dx ) << " " // 43
+ << filterprecision( inputRightFootTraj_deq_[i].dy ) << " " // 44
+ << filterprecision( inputRightFootTraj_deq_[i].dz ) << " " // 45
+ << filterprecision( inputRightFootTraj_deq_[i].dtheta ) << " " // 46
+ << filterprecision( inputRightFootTraj_deq_[i].domega ) << " " // 47
+ << filterprecision( inputRightFootTraj_deq_[i].ddx ) << " " // 48
+ << filterprecision( inputRightFootTraj_deq_[i].ddy ) << " " // 49
+ << filterprecision( inputRightFootTraj_deq_[i].ddz ) << " " // 50
+ << filterprecision( inputRightFootTraj_deq_[i].ddtheta ) << " "// 51
+ << filterprecision( inputRightFootTraj_deq_[i].ddomega ) << " ";// 52
+
for(unsigned int j = 0 ; j < configurationTraj_[i].size() ; j++ )
aof << filterprecision( configurationTraj_[i](j) ) << " " ;
for(unsigned int j = 0 ; j < velocityTraj_[i].size() ; j++ )
@@ -364,7 +429,6 @@ void DynamicFilter::printDebug()
static double ecartmaxX = 0 ;
static double ecartmaxY = 0 ;
-
for (unsigned int i = 0 ; i < deltaZMP_deq_.size() ; i++ )
{
if ( abs(deltaZMP_deq_[i].px) > ecartmaxX )
@@ -372,8 +436,110 @@ void DynamicFilter::printDebug()
if ( abs(deltaZMP_deq_[i].py) > ecartmaxY )
ecartmaxY = abs(deltaZMP_deq_[i].py) ;
}
+
return ;
}
+void DynamicFilter::printBuffers(deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ )
+{
+ // 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 );
+
+ // --------------------
+ oss.str("DumpingData/DynamicFilterAllVariablesAlongInTime_");
+ 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 < inputCOMTraj_deq_.size() ; ++i )
+ {
+ aof << filterprecision( iteration*controlPeriod_ + i*interpolationPeriod_ ) << " " // 0
+ << filterprecision( inputCOMTraj_deq_[i].x[0] ) << " " // 1
+ << filterprecision( inputCOMTraj_deq_[i].x[1] ) << " " // 2
+ << filterprecision( inputCOMTraj_deq_[i].x[2] ) << " " // 3
+ << filterprecision( inputCOMTraj_deq_[i].y[0] ) << " " // 4
+ << filterprecision( inputCOMTraj_deq_[i].y[1] ) << " " // 5
+ << filterprecision( inputCOMTraj_deq_[i].y[2] ) << " " // 6
+ << filterprecision( inputCOMTraj_deq_[i].z[0] ) << " " // 7
+ << filterprecision( inputCOMTraj_deq_[i].z[1] ) << " " // 8
+ << filterprecision( inputCOMTraj_deq_[i].z[2] ) << " " // 9
+ << filterprecision( inputCOMTraj_deq_[i].roll[0] ) << " " // 10
+ << filterprecision( inputCOMTraj_deq_[i].roll[1] ) << " " // 11
+ << filterprecision( inputCOMTraj_deq_[i].roll[2] ) << " " // 12
+ << filterprecision( inputCOMTraj_deq_[i].pitch[0] ) << " "// 13
+ << filterprecision( inputCOMTraj_deq_[i].pitch[1] ) << " "// 14
+ << filterprecision( inputCOMTraj_deq_[i].pitch[2] ) << " "// 15
+ << filterprecision( inputCOMTraj_deq_[i].yaw[0] ) << " " // 16
+ << filterprecision( inputCOMTraj_deq_[i].yaw[1] ) << " " // 17
+ << filterprecision( inputCOMTraj_deq_[i].yaw[2] ) << " " // 18
+
+ << filterprecision( inputZMPTraj_deq_[i].px ) << " " // 19
+ << filterprecision( inputZMPTraj_deq_[i].py ) << " " // 20
+
+// << filterprecision( ZMPMB_vec_[i][0] ) << " " // 21
+// << filterprecision( ZMPMB_vec_[i][1] ) << " " // 22
+
+ << filterprecision( inputLeftFootTraj_deq_[i].x ) << " " // 23
+ << filterprecision( inputLeftFootTraj_deq_[i].y ) << " " // 24
+ << filterprecision( inputLeftFootTraj_deq_[i].z ) << " " // 25
+ << filterprecision( inputLeftFootTraj_deq_[i].theta ) << " " // 26
+ << filterprecision( inputLeftFootTraj_deq_[i].omega ) << " " // 27
+ << filterprecision( inputLeftFootTraj_deq_[i].dx ) << " " // 28
+ << filterprecision( inputLeftFootTraj_deq_[i].dy ) << " " // 29
+ << filterprecision( inputLeftFootTraj_deq_[i].dz ) << " " // 30
+ << filterprecision( inputLeftFootTraj_deq_[i].dtheta ) << " " // 31
+ << filterprecision( inputLeftFootTraj_deq_[i].domega ) << " " // 32
+ << filterprecision( inputLeftFootTraj_deq_[i].ddx ) << " " // 33
+ << filterprecision( inputLeftFootTraj_deq_[i].ddy ) << " " // 34
+ << filterprecision( inputLeftFootTraj_deq_[i].ddz ) << " " // 35
+ << filterprecision( inputLeftFootTraj_deq_[i].ddtheta ) << " " // 36
+ << filterprecision( inputLeftFootTraj_deq_[i].ddomega ) << " " // 37
+
+ << filterprecision( inputRightFootTraj_deq_[i].x ) << " " // 38
+ << filterprecision( inputRightFootTraj_deq_[i].y ) << " " // 39
+ << filterprecision( inputRightFootTraj_deq_[i].z ) << " " // 40
+ << filterprecision( inputRightFootTraj_deq_[i].theta ) << " " // 41
+ << filterprecision( inputRightFootTraj_deq_[i].omega ) << " " // 42
+ << filterprecision( inputRightFootTraj_deq_[i].dx ) << " " // 43
+ << filterprecision( inputRightFootTraj_deq_[i].dy ) << " " // 44
+ << filterprecision( inputRightFootTraj_deq_[i].dz ) << " " // 45
+ << filterprecision( inputRightFootTraj_deq_[i].dtheta ) << " " // 46
+ << filterprecision( inputRightFootTraj_deq_[i].domega ) << " " // 47
+ << filterprecision( inputRightFootTraj_deq_[i].ddx ) << " " // 48
+ << filterprecision( inputRightFootTraj_deq_[i].ddy ) << " " // 49
+ << filterprecision( inputRightFootTraj_deq_[i].ddz ) << " " // 50
+ << filterprecision( inputRightFootTraj_deq_[i].ddtheta ) << " "// 51
+ << filterprecision( inputRightFootTraj_deq_[i].ddomega ) << " ";// 52
+
+// for(unsigned int j = 0 ; j < configurationTraj_[i].size() ; j++ )
+// aof << filterprecision( configurationTraj_[i](j) ) << " " ;
+// for(unsigned int j = 0 ; j < velocityTraj_[i].size() ; j++ )
+// aof << filterprecision( velocityTraj_[i](j) ) << " " ;
+// for(unsigned int j = 0 ; j < accelerationTraj_[i].size() ; j++ )
+// aof << filterprecision( accelerationTraj_[i](j) ) << " " ;
+ aof << endl ;
+ }
+ aof.close();
+
+
+ ++iteration;
+ return ;
+}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 46ca4830..0df47c46 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -62,7 +62,20 @@ namespace PatternGeneratorJRL
/// --------------------------------------
int OptimalControl(std::deque<COMState> & outputDeltaCOMTraj_deq_);
- void printDebug();
+ void printAlongTime(deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ );
+
+ void printBuffers(deque<COMState> & inputCOMTraj_deq_,
+ deque<ZMPPosition> inputZMPTraj_deq_,
+ deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
+ deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
+ deque<COMState> & outputDeltaCOMTraj_deq_
+ );
+
double filterprecision(double adb);
@@ -81,10 +94,25 @@ namespace PatternGeneratorJRL
inline void setPGPeriod(double PG_T)
{PG_T_ = PG_T ;}
+ inline void setPreviewWindowSize_(double previewWindowSize)
+ { previewWindowSize_ = previewWindowSize; }
+
/// \brief getter :
inline ComAndFootRealization * getComAndFootRealization()
{ return comAndFootRealization_;};
+ inline double getCurrentTime()
+ {return currentTime_ ;}
+
+ inline double getControlPeriod()
+ {return controlPeriod_ ;}
+
+ inline double getInterpolationPeriod()
+ {return interpolationPeriod_ ;}
+
+ inline double getPreviewWindowSize_()
+ {return previewWindowSize_ ;}
+
private: // Private members
/// \brief Time variables
--
GitLab