diff --git a/src/Mathematics/Bsplines.cpp b/src/Mathematics/Bsplines.cpp
index b91e9689ab83ee4197086b172593e05f2ae97a7e..a018298d2e47f71a328086b8133cb8dd4a6735fb 100644
--- a/src/Mathematics/Bsplines.cpp
+++ b/src/Mathematics/Bsplines.cpp
@@ -405,35 +405,35 @@ void ZBsplines::ZGenerateControlPoints(double IP, double FT, double FP, double T
std::ofstream myfile1;
myfile1.open("control_point.txt");
- Point A = {0.0,IP};
+ Point A; A.x = 0.0 ; A.y = IP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT*0.05,IP};
+ A.x = m_FT*0.05 ; A.y = IP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT*0.1,IP};
+ A.x = m_FT*0.1 ; A.y = IP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.85*m_ToMP,m_MP};
+ A.x = 0.85*m_ToMP ; A.y = m_MP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {1.15*m_ToMP,m_MP};
+ A.x = 1.15*m_ToMP ; A.y = m_MP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.90*m_FT,m_FP};
+ A.x = 0.90*m_FT ; A.y = m_FP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {0.95*m_FT,m_FP};
+ A.x = 0.95*m_FT ; A.y = m_FP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
- A = {m_FT,m_FP};
+ A.x = m_FT ; A.y = m_FP ;
control_points.push_back(A);
myfile1 << A.x <<" "<< A.y<< endl;
diff --git a/src/Mathematics/PolynomeFoot.hh b/src/Mathematics/PolynomeFoot.hh
index f53f8738b7fdf42a37a375c4c31a74b03d59a878..327929ccc2770cd412b00d9ae6b55f19c266f673 100644
--- a/src/Mathematics/PolynomeFoot.hh
+++ b/src/Mathematics/PolynomeFoot.hh
@@ -161,7 +161,6 @@ namespace PatternGeneratorJRL
void SetParameters(double FT, double FP);
- double Compute(double t);
/*! Set the parameters such that
the initial position, and initial speed
are different from zero.
diff --git a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
index 9d20273ca884194102139ea7c84e41b552f50946..9afd45a514c2dc88033164513e6168146fef807e 100644
--- a/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
+++ b/src/ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
@@ -76,7 +76,7 @@ namespace PatternGeneratorJRL
AnalyticalMorisawaCompact::AnalyticalMorisawaCompact(SimplePluginManager *lSPM , CjrlHumanoidDynamicRobot *aHS)
- : AnalyticalMorisawaAbstract(lSPM)
+ : AnalyticalMorisawaAbstract(lSPM)
{
RegisterMethods();
@@ -101,7 +101,7 @@ namespace PatternGeneratorJRL
/*! Dynamic allocation of the analytical trajectories for the ZMP and the COG */
m_AnalyticalZMPCoGTrajectoryX = new AnalyticalZMPCOGTrajectory(7);
m_AnalyticalZMPCoGTrajectoryY = new AnalyticalZMPCOGTrajectory(7);
- // m_AnalyticalZMPCoGTrajectoryZ = new AnalyticalZMPCOGTrajectory(7);
+ // m_AnalyticalZMPCoGTrajectoryZ = new AnalyticalZMPCOGTrajectory(7);
/*! Dynamic allocation of the filters. */
m_FilterXaxisByPC = new FilteringAnalyticalTrajectoryByPreviewControl(lSPM,
@@ -813,37 +813,37 @@ When the limit is reached, and the stack exhausted this method is called again.
{
if (m_AnalyticalZMPCoGTrajectoryX->GetIntervalIndexFromTime(time,lIndexInterval))
{
-// ZMPPosition aZMPPos0;
-// memset(&aZMPPos0,0,sizeof(aZMPPos0));
-// COMState aCOMPos0;
-// memset(&aCOMPos0,0,sizeof(aCOMPos0));
-// unsigned int n = m_kajitaDynamicFilter->getPreviewWindowSize_()/m_SamplingPeriod ;
-// std::deque<ZMPPosition> deltaZMPPos_deq(n,aZMPPos0) ;
-// std::deque<COMState> deltaCOMPos_deq(n,aCOMPos0) ;
-// if (m_FilteringActivate)
-// {
-// for (unsigned int i = 0 ; i < n ; ++i)
-// {
-// 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. */
-// deltaZMPPos_deq[i].px = FZmpX;
-// deltaZMPPos_deq[i].py = FZmpY;
-//
-// /*! Feed the COMStates. */
-// deltaCOMPos_deq[i].x[0] = FComX; deltaCOMPos_deq[i].x[1] = FComdX;
-// deltaCOMPos_deq[i].y[0] = FComY; deltaCOMPos_deq[i].y[1] = FComdY;
-// }
-// }
-// }
+ // ZMPPosition aZMPPos0;
+ // memset(&aZMPPos0,0,sizeof(aZMPPos0));
+ // COMState aCOMPos0;
+ // memset(&aCOMPos0,0,sizeof(aCOMPos0));
+ // unsigned int n = m_kajitaDynamicFilter->getPreviewWindowSize_()/m_SamplingPeriod ;
+ // std::deque<ZMPPosition> deltaZMPPos_deq(n,aZMPPos0) ;
+ // std::deque<COMState> deltaCOMPos_deq(n,aCOMPos0) ;
+ // if (m_FilteringActivate)
+ // {
+ // for (unsigned int i = 0 ; i < n ; ++i)
+ // {
+ // 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. */
+ // deltaZMPPos_deq[i].px = FZmpX;
+ // deltaZMPPos_deq[i].py = FZmpY;
+ //
+ // /*! Feed the COMStates. */
+ // deltaCOMPos_deq[i].x[0] = FComX; deltaCOMPos_deq[i].x[1] = FComdX;
+ // deltaCOMPos_deq[i].y[0] = FComY; deltaCOMPos_deq[i].y[1] = FComdY;
+ // }
+ // }
+ // }
std::deque<ZMPPosition> ZMPPos_deq ;
std::deque<COMState> COMPos_deq ;
std::deque<FootAbsolutePosition> LeftFootAbsPos ;
@@ -2639,17 +2639,14 @@ new step has to be generate.
aCOMPos.yaw[1] = 0.5*(LeftFootAbsPos.dtheta + RightFootAbsPos.dtheta);
aCOMPos.yaw[2] = 0.5*(LeftFootAbsPos.ddtheta + RightFootAbsPos.ddtheta);
- FinalCoMPositions.push_back(aCOMPos);
-
- ComputeCoMz(t,aCOMPos.z[0]);
-
- aCOMPos.z[1] = (aCOMPos.z[0] - preCoMz) / m_SamplingPeriod;
- preCoMz = aCOMPos.z[0];
+ ComputeCoMz(t,aCOMPos.z[0]);
+ aCOMPos.z[1] = (aCOMPos.z[0] - preCoMz) / m_SamplingPeriod;
+ preCoMz = aCOMPos.z[0];
- FinalCoMPositions.push_back(aCOMPos);
+ FinalCoMPositions.push_back(aCOMPos);
ODEBUG4(aZMPPos.px << " " << aZMPPos.py << " " <<
- aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0] <<" "<<
+ aCOMPos.x[0] << " " << aCOMPos.y[0] << " " << aCOMPos.z[0] <<" "<<
LeftFootAbsPos.x << " " << LeftFootAbsPos.y << " " << LeftFootAbsPos.z << " " <<
RightFootAbsPos.x << " " << RightFootAbsPos.y << " " << RightFootAbsPos.z << " " <<
samplingPeriod,"Test.dat");
@@ -2657,13 +2654,13 @@ new step has to be generate.
}
- void AnalyticalMorisawaCompact::ComputeCoMz(double t, double &CoMz)
- {
+ void AnalyticalMorisawaCompact::ComputeCoMz(double t, double &CoMz)
+ {
- int Index,Index2;
+ unsigned int Index;
double moving_time = m_RelativeFootPositions[0].SStime + m_RelativeFootPositions[0].DStime;
double deltaZ;
- // double static CoMzpre = CoMz;
+ // double static CoMzpre = CoMz;
double up=0.1,upRight = 0.9 ,upLeft = 0.0;
double upRight1 = 0.9 ,upLeft1 = 0.0;
@@ -2672,69 +2669,69 @@ new step has to be generate.
if (t >= moving_time){ // we start analyze since 2nd step
- Index2 = int(t/moving_time);
+ Index = int(t/moving_time);
+
+ if (Index < m_AbsoluteSupportFootPositions.size())
+ {
+ // climbing
- if (Index2 < m_AbsoluteSupportFootPositions.size())
+ // put first leg on the stairs with decrease of CoM //up// of stair height
+ // the CoM line will decrease between an //upLeft to upRight// interval of SStime.
+ // the CoM line will go up between an //upLeft1 to upRight1// interval of SStime while 2nd leg moving up on the stairs.
+ if (m_AbsoluteSupportFootPositions[Index].z > m_AbsoluteSupportFootPositions[Index-1].z) // first leg
+ {
+ deltaZ = (-m_AbsoluteSupportFootPositions[Index].z + m_AbsoluteSupportFootPositions[Index-1].z );
+ if (t <= Index*moving_time + upRight*m_RelativeFootPositions[Index].SStime && t >= Index*moving_time + upLeft*m_RelativeFootPositions[Index].SStime)
+ CoMz = (t-Index*moving_time - upLeft*m_RelativeFootPositions[Index].SStime)*up*deltaZ/((upRight-upLeft)*m_RelativeFootPositions[Index].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z ;
+ else if (t < Index*moving_time + upLeft*m_RelativeFootPositions[Index].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z + up*deltaZ;
+ }
+ else if (m_AbsoluteSupportFootPositions[Index].z == m_AbsoluteSupportFootPositions[Index-1].z && m_RelativeFootPositions[Index-1].sz > 0) // 2nd leg
{
- // climbing
-
- // put first leg on the stairs with decrease of CoM //up// of stair height
- // the CoM line will decrease between an //upLeft to upRight// interval of SStime.
- // the CoM line will go up between an //upLeft1 to upRight1// interval of SStime while 2nd leg moving up on the stairs.
- if (m_AbsoluteSupportFootPositions[Index2].z > m_AbsoluteSupportFootPositions[Index2-1].z) // first leg
- {
- deltaZ = (-m_AbsoluteSupportFootPositions[Index2].z + m_AbsoluteSupportFootPositions[Index2-1].z );
- if (t <= Index2*moving_time + upRight*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + upLeft*m_RelativeFootPositions[Index2].SStime)
- CoMz = (t-Index2*moving_time - upLeft*m_RelativeFootPositions[Index2].SStime)*up*deltaZ/((upRight-upLeft)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
- else if (t < Index2*moving_time + upLeft*m_RelativeFootPositions[Index2].SStime)
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
- else
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z + up*deltaZ;
- }
- else if (m_AbsoluteSupportFootPositions[Index2].z == m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2-1].sz > 0) // 2nd leg
- {
- deltaZ = (m_AbsoluteSupportFootPositions[Index2].z - m_AbsoluteSupportFootPositions[Index2-2].z );
- if (t <= Index2*moving_time + upRight1*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + upLeft1*m_RelativeFootPositions[Index2].SStime)
- CoMz = (t-Index2*moving_time - upLeft1*m_RelativeFootPositions[Index2].SStime)*(1+up)*deltaZ/((upRight1-upLeft1)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-2].z - up*deltaZ ;
- else if (t < Index2*moving_time + upLeft1*m_RelativeFootPositions[Index2].SStime)
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-2].z - up*deltaZ;
- else
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
- }
-
- // going down
- // the CoM line will decrease an //1+down// stair height between an //downLeft to downRight// interval of SStime while moving first leg down
- // put the 2nd leg down while standing up the CoM.
- else if (m_AbsoluteSupportFootPositions[Index2].z < m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2].sz < 0) // first leg
- {
- deltaZ = (m_AbsoluteSupportFootPositions[Index2].z - m_AbsoluteSupportFootPositions[Index2-1].z );
- if (t <= Index2*moving_time + downRight*m_RelativeFootPositions[Index2].SStime && t >= Index2*moving_time + downLeft*m_RelativeFootPositions[Index2].SStime)
- CoMz = (t-Index2*moving_time- downLeft*m_RelativeFootPositions[Index2].SStime)*(1+down)*deltaZ/((downRight - downLeft)*m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
- else if (t < Index2*moving_time + downLeft*m_RelativeFootPositions[Index2].SStime)
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z ;
- else
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z + down*deltaZ;
- }
- else if (m_AbsoluteSupportFootPositions[Index2].z == m_AbsoluteSupportFootPositions[Index2-1].z && m_RelativeFootPositions[Index2-1].sz < 0) //second leg
- {
- deltaZ = (m_AbsoluteSupportFootPositions[Index2-2].z - m_AbsoluteSupportFootPositions[Index2].z );
- if (t <= Index2*moving_time + m_RelativeFootPositions[Index2].SStime )
- CoMz = (t-Index2*moving_time)*down*deltaZ/(m_RelativeFootPositions[Index2].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2-1].z - down*deltaZ ;
- else
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z ;
- }
- else // normal walking
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index2].z;
+ deltaZ = (m_AbsoluteSupportFootPositions[Index].z - m_AbsoluteSupportFootPositions[Index-2].z );
+ if (t <= Index*moving_time + upRight1*m_RelativeFootPositions[Index].SStime && t >= Index*moving_time + upLeft1*m_RelativeFootPositions[Index].SStime)
+ CoMz = (t-Index*moving_time - upLeft1*m_RelativeFootPositions[Index].SStime)*(1+up)*deltaZ/((upRight1-upLeft1)*m_RelativeFootPositions[Index].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-2].z - up*deltaZ ;
+ else if (t < Index*moving_time + upLeft1*m_RelativeFootPositions[Index].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-2].z - up*deltaZ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index].z;
}
- else //after final step
- CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions.back().z;
+ // going down
+ // the CoM line will decrease an //1+down// stair height between an //downLeft to downRight// interval of SStime while moving first leg down
+ // put the 2nd leg down while standing up the CoM.
+ else if (m_AbsoluteSupportFootPositions[Index].z < m_AbsoluteSupportFootPositions[Index-1].z && m_RelativeFootPositions[Index].sz < 0) // first leg
+ {
+ deltaZ = (m_AbsoluteSupportFootPositions[Index].z - m_AbsoluteSupportFootPositions[Index-1].z );
+ if (t <= Index*moving_time + downRight*m_RelativeFootPositions[Index].SStime && t >= Index*moving_time + downLeft*m_RelativeFootPositions[Index].SStime)
+ CoMz = (t-Index*moving_time- downLeft*m_RelativeFootPositions[Index].SStime)*(1+down)*deltaZ/((downRight - downLeft)*m_RelativeFootPositions[Index].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z ;
+ else if (t < Index*moving_time + downLeft*m_RelativeFootPositions[Index].SStime)
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index].z + down*deltaZ;
}
+ else if (m_AbsoluteSupportFootPositions[Index].z == m_AbsoluteSupportFootPositions[Index-1].z && m_RelativeFootPositions[Index-1].sz < 0) //second leg
+ {
+ deltaZ = (m_AbsoluteSupportFootPositions[Index-2].z - m_AbsoluteSupportFootPositions[Index].z );
+ if (t <= Index*moving_time + m_RelativeFootPositions[Index].SStime )
+ CoMz = (t-Index*moving_time)*down*deltaZ/(m_RelativeFootPositions[Index].SStime) + m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index-1].z - down*deltaZ ;
+ else
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index].z ;
+ }
+ else // normal walking
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions[Index].z;
+ }
+
+ else //after final step
+ CoMz = m_InitialPoseCoMHeight + m_AbsoluteSupportFootPositions.back().z;
+ }
else //first step
- CoMz = m_InitialPoseCoMHeight ;
+ CoMz = m_InitialPoseCoMHeight ;
-}
+ }
void AnalyticalMorisawaCompact::FillQueues(double StartingTime,
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 318b650123338af1a13b6e5728f6eaa5f10a8682..e3085531df24a78f0fd72b8d247cc755b81802ad 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -193,19 +193,10 @@ int DynamicFilter::filter(
inputRightFootTraj_deq_);
InverseDynamics(inputZMPTraj_deq_);
-
+ outputDeltaCOMTraj_deq_;
//int error = OptimalControl(outputDeltaCOMTraj_deq_);
int error = 0;
- printBuffers(inputCOMTraj_deq_,
- inputZMPTraj_deq_,
- inputLeftFootTraj_deq_,
- inputRightFootTraj_deq_,
- outputDeltaCOMTraj_deq_);
- printAlongTime(inputCOMTraj_deq_,
- inputZMPTraj_deq_,
- inputLeftFootTraj_deq_,
- inputRightFootTraj_deq_,
- outputDeltaCOMTraj_deq_);
+
return error ;
}
@@ -598,272 +589,3 @@ void DynamicFilter::computeWaist(const FootAbsolutePosition & inputLeftFoot)
return ;
}
-
-double DynamicFilter::filterprecision(double adb)
-{
- if (fabs(adb)<1e-7)
- return 0.0;
-
- if (fabs(adb)>1e7)
- return 1e7 ;
-
- double ladb2 = adb * 1e7;
- double lintadb2 = trunc(ladb2);
- return lintadb2/1e7;
-}
-
-void DynamicFilter::printAlongTime(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;
-
- // --------------------
- oss.str("DynamicFilterAllVariablesNoisyAlongInTime.dat");
- aFileName = oss.str();
- if(iteration == 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);
- aof << filterprecision( iteration*controlPeriod_) << " " // 0
- << filterprecision( inputCOMTraj_deq_[0].x[0] ) << " " // 1
- << filterprecision( inputCOMTraj_deq_[0].x[1] ) << " " // 2
- << filterprecision( inputCOMTraj_deq_[0].x[2] ) << " " // 3
- << filterprecision( inputCOMTraj_deq_[0].y[0] ) << " " // 4
- << filterprecision( inputCOMTraj_deq_[0].y[1] ) << " " // 5
- << filterprecision( inputCOMTraj_deq_[0].y[2] ) << " " // 6
- << filterprecision( inputCOMTraj_deq_[0].z[0] ) << " " // 7
- << filterprecision( inputCOMTraj_deq_[0].z[1] ) << " " // 8
- << filterprecision( inputCOMTraj_deq_[0].z[2] ) << " " // 9
- << filterprecision( inputCOMTraj_deq_[0].roll[0] ) << " " // 10
- << filterprecision( inputCOMTraj_deq_[0].roll[1] ) << " " // 11
- << filterprecision( inputCOMTraj_deq_[0].roll[2] ) << " " // 12
- << filterprecision( inputCOMTraj_deq_[0].pitch[0] ) << " "// 13
- << filterprecision( inputCOMTraj_deq_[0].pitch[1] ) << " "// 14
- << filterprecision( inputCOMTraj_deq_[0].pitch[2] ) << " "// 15
- << filterprecision( inputCOMTraj_deq_[0].yaw[0] ) << " " // 16
- << filterprecision( inputCOMTraj_deq_[0].yaw[1] ) << " " // 17
- << filterprecision( inputCOMTraj_deq_[0].yaw[2] ) << " " // 18
-
- << filterprecision( inputZMPTraj_deq_[0].px ) << " " // 19
- << filterprecision( inputZMPTraj_deq_[0].py ) << " " // 20
-
- << filterprecision( ZMPMB_vec_[0][0] ) << " " // 21
- << filterprecision( ZMPMB_vec_[0][1] ) << " " // 22
-
- << filterprecision( inputLeftFootTraj_deq_[0].x ) << " " // 23
- << filterprecision( inputLeftFootTraj_deq_[0].y ) << " " // 24
- << filterprecision( inputLeftFootTraj_deq_[0].z ) << " " // 25
- << filterprecision( inputLeftFootTraj_deq_[0].theta ) << " " // 26
- << filterprecision( inputLeftFootTraj_deq_[0].omega ) << " " // 27
- << filterprecision( inputLeftFootTraj_deq_[0].dx ) << " " // 28
- << filterprecision( inputLeftFootTraj_deq_[0].dy ) << " " // 29
- << filterprecision( inputLeftFootTraj_deq_[0].dz ) << " " // 30
- << filterprecision( inputLeftFootTraj_deq_[0].dtheta ) << " " // 31
- << filterprecision( inputLeftFootTraj_deq_[0].domega ) << " " // 32
- << filterprecision( inputLeftFootTraj_deq_[0].ddx ) << " " // 33
- << filterprecision( inputLeftFootTraj_deq_[0].ddy ) << " " // 34
- << filterprecision( inputLeftFootTraj_deq_[0].ddz ) << " " // 35
- << filterprecision( inputLeftFootTraj_deq_[0].ddtheta ) << " " // 36
- << filterprecision( inputLeftFootTraj_deq_[0].ddomega ) << " " // 37
-
- << filterprecision( inputRightFootTraj_deq_[0].x ) << " " // 38
- << filterprecision( inputRightFootTraj_deq_[0].y ) << " " // 39
- << filterprecision( inputRightFootTraj_deq_[0].z ) << " " // 40
- << filterprecision( inputRightFootTraj_deq_[0].theta ) << " " // 41
- << filterprecision( inputRightFootTraj_deq_[0].omega ) << " " // 42
- << filterprecision( inputRightFootTraj_deq_[0].dx ) << " " // 43
- << filterprecision( inputRightFootTraj_deq_[0].dy ) << " " // 44
- << filterprecision( inputRightFootTraj_deq_[0].dz ) << " " // 45
- << filterprecision( inputRightFootTraj_deq_[0].dtheta ) << " " // 46
- << filterprecision( inputRightFootTraj_deq_[0].domega ) << " " // 47
- << filterprecision( inputRightFootTraj_deq_[0].ddx ) << " " // 48
- << filterprecision( inputRightFootTraj_deq_[0].ddy ) << " " // 49
- << filterprecision( inputRightFootTraj_deq_[0].ddz ) << " " // 50
- << filterprecision( inputRightFootTraj_deq_[0].ddtheta ) << " "// 51
- << filterprecision( inputRightFootTraj_deq_[0].ddomega ) << " ";// 52
-
- for(unsigned int j = 0 ; j < previousConfiguration_.size() ; j++ )
- aof << filterprecision( previousConfiguration_(j) ) << " " ;
- for(unsigned int j = 0 ; j < previousVelocity_.size() ; j++ )
- aof << filterprecision( previousVelocity_(j) ) << " " ;
- for(unsigned int j = 0 ; j < previousAcceleration_.size() ; j++ )
- aof << filterprecision( accelerationTraj_[0](j) ) << " " ;
-
- aof << filterprecision( outputDeltaCOMTraj_deq_[0].x[0] ) << " "
- << filterprecision( outputDeltaCOMTraj_deq_[0].x[1] ) << " "
- << filterprecision( outputDeltaCOMTraj_deq_[0].x[2] ) << " "
- << filterprecision( outputDeltaCOMTraj_deq_[0].y[0] ) << " "
- << filterprecision( outputDeltaCOMTraj_deq_[0].y[1] ) << " "
- << filterprecision( outputDeltaCOMTraj_deq_[0].y[2] ) << " ";
-
- aof << endl ;
- aof.close();
-
- ++iteration;
-
- 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( inputLeftFootTraj_deq_[i].x ) << " " // 21
- << filterprecision( inputLeftFootTraj_deq_[i].y ) << " " // 22
- << filterprecision( inputLeftFootTraj_deq_[i].z ) << " " // 23
- << filterprecision( inputLeftFootTraj_deq_[i].theta ) << " " // 24
- << filterprecision( inputLeftFootTraj_deq_[i].omega ) << " " // 25
- << filterprecision( inputLeftFootTraj_deq_[i].dx ) << " " // 26
- << filterprecision( inputLeftFootTraj_deq_[i].dy ) << " " // 27
- << filterprecision( inputLeftFootTraj_deq_[i].dz ) << " " // 28
- << filterprecision( inputLeftFootTraj_deq_[i].dtheta ) << " " // 29
- << filterprecision( inputLeftFootTraj_deq_[i].domega ) << " " // 30
- << filterprecision( inputLeftFootTraj_deq_[i].ddx ) << " " // 31
- << filterprecision( inputLeftFootTraj_deq_[i].ddy ) << " " // 32
- << filterprecision( inputLeftFootTraj_deq_[i].ddz ) << " " // 33
- << filterprecision( inputLeftFootTraj_deq_[i].ddtheta ) << " " // 34
- << filterprecision( inputLeftFootTraj_deq_[i].ddomega ) << " " // 35
-
- << filterprecision( inputRightFootTraj_deq_[i].x ) << " " // 36
- << filterprecision( inputRightFootTraj_deq_[i].y ) << " " // 37
- << filterprecision( inputRightFootTraj_deq_[i].z ) << " " // 38
- << filterprecision( inputRightFootTraj_deq_[i].theta ) << " " // 39
- << filterprecision( inputRightFootTraj_deq_[i].omega ) << " " // 40
- << filterprecision( inputRightFootTraj_deq_[i].dx ) << " " // 41
- << filterprecision( inputRightFootTraj_deq_[i].dy ) << " " // 42
- << filterprecision( inputRightFootTraj_deq_[i].dz ) << " " // 43
- << filterprecision( inputRightFootTraj_deq_[i].dtheta ) << " " // 44
- << filterprecision( inputRightFootTraj_deq_[i].domega ) << " " // 45
- << filterprecision( inputRightFootTraj_deq_[i].ddx ) << " " // 46
- << filterprecision( inputRightFootTraj_deq_[i].ddy ) << " " // 47
- << filterprecision( inputRightFootTraj_deq_[i].ddz ) << " " // 48
- << filterprecision( inputRightFootTraj_deq_[i].ddtheta ) << " " // 49
- << filterprecision( inputRightFootTraj_deq_[i].ddomega ) << " ";// 50
-
- 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 << filterprecision( ZMPMB_vec_[i][0] ) << " " // 159
- << filterprecision( ZMPMB_vec_[i][1] ) << " "; // 160
-
- aof << filterprecision( deltaZMP_deq_[i].px ) << " " // 161
- << filterprecision( deltaZMP_deq_[i].py ) << " "; // 162
-
-
- aof << endl ;
- }
- aof.close();
-
-
- static double maxErrX = 0 ;
- static double maxErrY = 0 ;
- for (unsigned int i = 0 ; i < deltaZMP_deq_.size() ; ++i )
- {
- if ( deltaZMP_deq_[i].px > maxErrX )
- {
- maxErrX = deltaZMP_deq_[i].px ;
- }
- if ( deltaZMP_deq_[i].py > maxErrY )
- {
- maxErrY = deltaZMP_deq_[i].py ;
- }
- }
-
- static double moyErrX = 0 ;
- static double moyErrY = 0 ;
- static double sumErrX = 0 ;
- static double sumErrY = 0 ;
- static int nbRNEAcomputed = 0 ;
- for (unsigned int i = 0 ; i < deltaZMP_deq_.size(); ++i)
- {
- sumErrX += deltaZMP_deq_[i].px ;
- sumErrY += deltaZMP_deq_[i].py ;
- }
- nbRNEAcomputed += deltaZMP_deq_.size() ;
- moyErrX = sumErrX / nbRNEAcomputed ;
- moyErrY = sumErrY / nbRNEAcomputed ;
-
- aFileName = "TestMorisawa2007OnLine32MoyNoisyZMP.dat" ;
- if(iteration==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);
- aof << filterprecision(moyErrX ) << " " // 1
- << filterprecision(moyErrY ) << " " // 2
- << filterprecision(maxErrX ) << " " // 3
- << filterprecision(maxErrY ) << " " // 4
- << endl ;
- aof.close();
-
-
- ++iteration;
- return ;
-}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index d069fb039634bce16e7bdc3a1363d875200f2eed..c76d249be8b5a382ccc9404238e7e02bd375dc9f 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -116,24 +116,6 @@ namespace PatternGeneratorJRL
// -------------------------------------------------------------------
- /// \brief Debug function
- void printAlongTime(deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_
- );
- /// \brief Debug function
- void printBuffers(deque<COMState> & inputCOMTraj_deq_,
- deque<ZMPPosition> inputZMPTraj_deq_,
- deque<FootAbsolutePosition> & inputLeftFootTraj_deq_,
- deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
- deque<COMState> & outputDeltaCOMTraj_deq_
- );
- /// \brief Debug function
- double filterprecision(double adb);
-
-
public: // The accessors
/// \brief setter :
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 92f4df4ba2b6a948a6dab8459a9191d8c6ad02cf..5e17edf6f8bec168a6c3c1324977e9175195cc6b 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -571,7 +571,7 @@ void
FinalLeftFootTraj_deq[i],
FinalRightFootTraj_deq[i],
ZMPMB_deq[i-CurrentIndex_],1,
- (int)time/QP_T_ + i*m_SamplingPeriod
+ (int)(time/QP_T_ + i*m_SamplingPeriod)
);
diff --git a/tests/TestBsplines.cpp b/tests/TestBsplines.cpp
index 55851f8b6f25a3beca999ccda253ab7793d1781f..ea4713340870fbaed659d40f21761e4f6c964b2f 100644
--- a/tests/TestBsplines.cpp
+++ b/tests/TestBsplines.cpp
@@ -35,8 +35,6 @@ int main()
{
PatternGeneratorJRL::ZBsplines *Z;
double t=0.0;
- int m_degree;
- int i , j;
ofstream myfile;
myfile.open("TestBsplines.txt");
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 343713728e83fe6a1afc7f146a8390502c6d3add..56875016bfa769e0565c95c6722f3a022470360f 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -38,7 +38,7 @@ enum Profiles_t {
PROFIL_ANALYTICAL_ONLINE_WALKING, // 1
PROFIL_ANALYTICAL_SHORT_STRAIGHT_WALKING, // 2
PROFIL_ANALYTICAL_CLIMBING_STAIRS, // 3
- PROFIL_ANALYTICAL_GOING_DOWN_STAIRS, // 4
+ PROFIL_ANALYTICAL_GOING_DOWN_STAIRS // 4
};
#define NBOFPREDEFONLINEFOOTSTEPS 11